二关节机械臂计算力矩控制simulink程序.7z

clear all;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Model of three-link robot （“Inverse Control of a two-link manipulator“ 2001）
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
l   = [0.20.3]; %length of link i （each colum corresponds to one robot）
lc  = [0.10.15]; %distances from previous joint to CM of link i （each colum corresponds to one robot）
m   = [11.5]; %mass of inertia of link i
I   = [0.0130.045]; %moment of inertia of link i
%constant part of parameters
Theta1 = m（1）*lc（1）^2+m（2）*（l（1）^2+lc（2）^2）+I（1）+I（2）; % ;
Theta2 = m（2）*l（1）*lc（2）;                              % ;
Theta3 = m（2）*lc（2）^2+I（2）;                            % ;
Theta4 = m（1）*lc（1）+m（2）*l（1）;
Theta5 = m（2）*l（2）;
Theta  = [Theta1; Theta2; Theta3; Theta4; Theta5];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%paremeters of the graph with 4 leaders and 6 followers
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%control paremeter
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k_p=3;
k_v=2;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%initial data of six two-link robots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%qd=[0.3;0.1];                        %%the reference position
%vd0=[0.1;0.1]*0;                     %%initial value of the velocity estimtor
q0=[0;0]; %intitial positions of the followers    
% for i=1:6
% dq0（:i） = [0.2*i-0.5; -0.1*i+0.5]; %intitial velocities of the followers
% end
dq0=[0 ; 0];
%s0=zeros（36）;
%zd=0;
g0=9.8;                               %%g is the gravitational acceleration