二关节机械臂计算力矩跟踪控制

clear all; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Model of three-link robot ("Inverse Control of a two-link manipulator" 2001) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% l = [0.2,0.3]; %length of link i (each colum corresponds to one robot) lc = [0.1,0.15]; %distances from previous joint to CM of link i (each colum corresponds to one robot) m = [1,1.5]; %mass of inertia of link i I = [0.013,0.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(3,6); %zd=0; g0=9.8; %%g is the gravitational acceleration