空间机器手臂的计算力矩控制法仿真程序

博大广阔

function JU_ohe_jisuanliju_joint()
%仿真耦合了动量和动量矩的单臂机器手臂，控制方法为计算力矩方法>>仅是话关节1和关节2的曲线
clc
clear all

%%%%%%%%%%%%%%%%%%%%%%%{模块一：difinition the systems parameters%%%%%%%%%%%%%%%%
global M L I A J   h                            %definited the global variabal
M=[100 10 10];L=[2 1 1];A=[0.5 0.5];J=[25 1.5 1.5];             %symstem parameters
                                                     %the care number of robot arm one pose  and two joint
h=0.005;                                                     %the size of simulation

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%模块二：givened the innitial value%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t=0; tmax=10;                                                   %simulation from 0 to 10 /s
q=[0.1 1.3].';                                                  %innitial angle of pose and joint
dq=zeros(2,1);                                             %innitial angular velocity
qb=[0.1];
dq0=[0];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
TT=0;
qq=q;

RRq=[0;0];                                              %初始角度
RRdq=[0.12;0.12];
RRddq=[0;0];

ddqq=zeros(2,1);
qqb=[0.1];
TTor=[0;0];
dx0=[0];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%模块三：begin intergration by time%%%%%%%%%
while(t<tmax)
      Rq=[1.2*(t/10-(1/pi)*sin(2*pi*t/5));       % calculated every moment angular of arm 1
          1.2*(1-t/10+(1/pi)*sin(2*pi*t/5));];
      Rdq=[1.2*(1/10-(2/5)*cos(2*pi*t/5));       %moment speed
             1.2*(-1/10+(2/5)*cos(2*pi*t/5)); ];
      Rddq=[1.2*0.4*sin(2*pi*t/5)*(2*pi/5);
           -1.2*0.4*sin(2*pi*t/5)*(2*pi/5);];
  %%%%%%%%%%%%%%%%%%%the last value give the new variable as a nenw initial%%%%%%%%%%%%%%%%%%
  q0=q;          %2*1
  dq0=dq;        %2*1
  qb0=qb;        %1*1
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
     [Tor,dx0,dx1 ,dx2]=derives(q,dq,Rddq,Rdq,Rq,dx0);
       k11=dx1;      q=q0+(h/2)*dx1;               %以便于下次的更新
       k12=dx2;      dq=dq0+(h/2)*dx2;
       k13=dx0;      qb=qb0+(h/2)*dx0;  
      
      t=t+h/2;
      [Tor,dx0,dx1 ,dx2]=derives(q,dq,Rddq,Rdq,Rq,dx0);
       k21=dx1;      q=q0+(h/2)*dx1;
       k22=dx2;      dq=dq0+(h/2)*dx2;
       k23=dx0;      qb=qb0+(h/2)*dx0;
      
      
       [Tor,dx0,dx1 ,dx2]=derives(q,dq,Rddq,Rdq,Rq,dx0);
         k31=dx1;      q=q0+(h)*dx1;
         k32=dx2;      dq=dq0+(h)*dx2;
         k33=dx0;      qb=qb0+(h)*dx0;
         
         t=t+h/2;
         [Tor,dx0,dx1 ,dx2]=derives(q,dq,Rddq,Rdq,Rq,dx0);
          k41=dx1;      q=q0+(h/6)*(k11+2*k21+2*k31+k41);
          k42=dx2;      dq=dq0+(h/6)*(k12+2*k22+2*k32+k42);
          k43=dx0;      qb=qb0+(h/6)*(k13+2*k23+2*k33+k43);
      
       %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%save%%%%%%%%%%%%%%%   
       TT=[TT,t];
        qqb=[qqb,qb];
       qq=[qq,q];      
       RRq=[RRq,Rq];
       TTor=[TTor,Tor];
      
      
       %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
end

plot(TT,qq(1,1:end),'m',TT,RRq(1,1:end),'-',TT,qq(2,1:end),'m',TT,RRq(2,1:end),'b');grid on
legend('实际轨迹1', '期望轨迹1' ,'实际轨迹2' ,'期望轨迹');
%figure
%plot(TT,qqb,'b');grid on
figure
plot(TT,TTor(1,1:end),'m',TT,TTor(2,1:end),'b');grid on
end

function [Tor,dx0,dx1 ,dx2]=derives(q,dq,Rddq,Rdq,Rq,dq0)
% solution the stale variable derives ，求解力矩，角度的倒数（角速度），角速度的导数（叫加速度），姿态角的导数（姿态角角速度）
global M L I A J
Kv=[3.2 3.2];Kv=diag(Kv);
Kp=[5  5];Kp=diag(Kp);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%动量力矩守恒的dq0=K1*dq1+K2*dq2计算结果%%%%%%%%%%%%%%%%%%
m0=M(1); m1=M(2); m2=M(3); I0=J(1); I1=J(2); I2=J(3); l0=L(1); l1=L(2); l2=L(3); a1=A(1); a2=A(2);
q1=q(1); q2=q(2); dq1=dq(1);dq2=dq(2);dq0=dq0;

K1=-(l1^2*m0*m2 - I1*m1 - I2*m0 - I1*m2 - I2*m1 - I2*m2 - I1*m0 + l1^2*m1*m2 + a1^2*m0*m1 + a1^2*m1*m2 + a2^2*m0*m2 + ...       %结果不包含q0
      a2^2*m1*m2 - 2*a1*l1*m1*m2 + a2*l0*m0*m2*cos(q1 + q2) - 2*a1*a2*m1*m2*cos(q2) + a1*l0*m0*m1*cos(q1) + ...
         2*a2*l1*m0*m2*cos(q2) + 2*a2*l1*m1*m2*cos(q2) + l0*l1*m0*m2*cos(q1));
K3= l0^2*m0*m1 - I0*m1 - I1*m0 - I0*m2 - I1*m1 - I2*m0 - I1*m2 - I2*m1 - I2*m2 - I0*m0 + l0^2*m0*m2 + l1^2*m0*m2 + l1^2*m1*m2 + ...
      a1^2*m0*m1 + a1^2*m1*m2 + a2^2*m0*m2 + a2^2*m1*m2 - 2*a1*l1*m1*m2 + 2*a2*l0*m0*m2*cos(q1 + q2) - 2*a1*a2*m1*m2*cos(q2) +...
        2*a1*l0*m0*m1*cos(q1) + 2*a2*l1*m0*m2*cos(q2) + 2*a2*l1*m1*m2*cos(q2) + 2*l0*l1*m0*m2*cos(q1);
K2=-(a2^2*m0*m2 - I2*m1 - I2*m2 - I2*m0 + a2^2*m1*m2 + a2*l0*m0*m2*cos(q1 + q2) - a1*a2*m1*m2*cos(q2) + a2*l1*m0*m2*cos(q2) + a2*l1*m1*m2*cos(q2));
k1=K1/K3;
k2=K2/K3;
dx0=k1*dq(1)+k2*dq(2); %姿态角的导数（姿态角角速度）
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dx1=dq;             %速度的倒数既是角速度
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%计算角加速度%%%%%%%%%%%%%%%%%%%%%%%%%%%%
q1=q(1); q2=q(2); dq1=dq(1);dq2=dq(2); dq0=dq0;

Mh =[  25/9*(65+39*cos(q2)-18*cos(q1)^2-12*cos(q1)*cos(q1+q2)-2*cos(q1+q2)^2)/(109+24*cos(q1)+8*cos(q1+q2)+3*cos(q2)), 25/36*(78*cos(q2)+9*cos(q2)*cos(q1)-3*cos(q2)*cos(q1+q2)+26+3*cos(q1)-9*cos(q1+q2)-24*cos(q1)*cos(q1+q2)-8*cos(q1+q2)^2)/(109+24*cos(q1)+8*cos(q1+q2)+3*cos(q2));
   25/36*(78*cos(q2)+9*cos(q2)*cos(q1)-3*cos(q2)*cos(q1+q2)+26+3*cos(q1)-9*cos(q1+q2)-24*cos(q1)*cos(q1+q2)-8*cos(q1+q2)^2)/(109+24*cos(q1)+8*cos(q1+q2)+3*cos(q2)),                                              -25/288*(-217-48*cos(q1)+64*cos(q1+q2)^2+48*cos(q2)*cos(q1+q2)+9*cos(q2)^2)/(109+24*cos(q1)+8*cos(q1+q2)+3*cos(q2))];



Nh =[    -25/9*(-11250*dq1*sin(q1)*cos(q1)-1250*dq1*sin(q1+q2)*cos(q1+q2)-750*dq1*sin(q1+q2)*cos(q2)-3750*dq1*sin(q1+q2)*cos(q1)-1250*dq2*sin(q1+q2)*cos(q1+q2)-750*dq2*sin(q1+q2)*cos(q2)-3750*dq2*sin(q1+q2)*cos(q1)-5400*dq1*sin(q1)*cos(q1)^2-200*dq2*sin(q1+q2)*cos(q1+q2)^2-200*dq1*sin(q1+q2)*cos(q1+q2)^2-600*dq1*sin(q1)*cos(q1+q2)^2+75*sin(q2)*dq2*cos(q1+q2)^2+675*sin(q2)*dq2*cos(q1)^2-1800*dq2*sin(q1+q2)*cos(q1)^2-1800*dq1*sin(q1+q2)*cos(q1)^2+2325*sin(q2)*dq2*cos(q1+q2)+6975*sin(q2)*dq2*cos(q1)+12564*dq0*sin(q1)*cos(q1+q2)+14130*dq0*sin(q1)*cos(q2)+37692*dq0*sin(q1)*cos(q1)+4188*dq0*sin(q1+q2)*cos(q1+q2)+4710*dq0*sin(q1+q2)*cos(q2)+12564*dq0*sin(q1+q2)*cos(q1)-3750*dq1*sin(q1)*cos(q1+q2)-2250*dq1*sin(q1)*cos(q2)-3750*dq1*sin(q1)-1250*dq1*sin(q1+q2)+9750*sin(q2)*dq2+14814*dq0*sin(q1)+180*dq0*sin(q1)*cos(q1+q2)*cos(q2)+4938*dq0*sin(q1+q2)+540*dq0*sin(q1)*cos(q2)*cos(q1)+180*dq0*sin(q1+q2)*cos(q2)*cos(q1)+450*sin(q2)*dq2*cos(q1+q2)*cos(q1)+60*dq0*sin(q1+q2)*cos(q1+q2)*cos(q2)-1200*cos(q1)*dq1*sin(q1+q2)*cos(q1+q2)-1200*cos(q1)*dq2*sin(q1+q2)*cos(q1+q2)-3600*cos(q1)*dq1*sin(q1)*cos(q1+q2)-450*cos(q2)*dq2*sin(q1+q2)*cos(q1)-150*cos(q2)*dq2*sin(q1+q2)*cos(q1+q2)-1250*dq2*sin(q1+q2)-450*cos(q2)*dq1*sin(q1+q2)*cos(q1)-150*cos(q2)*dq1*sin(q1+q2)*cos(q1+q2)-1350*cos(q2)*dq1*sin(q1)*cos(q1)-450*cos(q2)*dq1*sin(q1)*cos(q1+q2))/(67471+26752*cos(q1+q2)+13302*cos(q2)+80256*cos(q1)+9600*cos(q1)*cos(q1+q2)+4320*cos(q2)*cos(q1)+14400*cos(q1)^2+1600*cos(q1+q2)^2+1440*cos(q2)*cos(q1+q2)+315*cos(q2)^2), -25/72*(1368*dq1*sin(q1)*cos(q1)-11752*dq1*sin(q1+q2)*cos(q1+q2)-8364*dq1*sin(q1+q2)*cos(q2)-41256*dq1*sin(q1+q2)*cos(q1)-11752*dq2*sin(q1+q2)*cos(q1+q2)-8364*dq2*sin(q1+q2)*cos(q2)-41256*dq2*sin(q1+q2)*cos(q1)-1600*dq2*sin(q1+q2)*cos(q1+q2)^2-1600*dq1*sin(q1+q2)*cos(q1+q2)^2+4800*dq1*sin(q1)*cos(q1+q2)^2+600*sin(q2)*dq2*cos(q1+q2)^2+10800*sin(q2)*dq2*cos(q1)^2-28800*dq2*sin(q1+q2)*cos(q1)^2-28800*dq1*sin(q1+q2)*cos(q1)^2+19257*sin(q2)*dq2*cos(q1+q2)+104571*sin(q2)*dq2*cos(q1)+131712*dq0*sin(q1)*cos(q1+q2)+69012*dq0*sin(q1)*cos(q2)+2736*dq0*sin(q1)*cos(q1)+30000*dq0*sin(q1+q2)*cos(q1+q2)+13476*dq0*sin(q1+q2)*cos(q2)-40800*dq0*sin(q1+q2)*cos(q1)+6456*dq1*sin(q1)*cos(q1+q2)+3492*dq1*sin(q1)*cos(q2)-630*dq1*sin(q1+q2)*cos(q2)^2+1890*dq0*sin(q1)*cos(q2)^2+945*sin(q2)*dq2*cos(q2)*cos(q1)+10800*dq1*sin(q2)*cos(q1)^2+1890*dq1*sin(q1)*cos(q2)^2+1890*dq1*sin(q2)*cos(q1)*cos(q2)+1890*dq0*sin(q2)*cos(q1)*cos(q2)+570*dq1*sin(q1)+10800*dq0*sin(q2)*cos(q1)^2+104742*dq1*sin(q2)*cos(q1)+104742*dq0*sin(q2)*cos(q1)-12190*dq1*sin(q1+q2)-630*dq2*sin(q1+q2)*cos(q2)^2+7200*dq1*sin(q2)*cos(q1+q2)*cos(q1)+630*dq1*sin(q2)*cos(q1+q2)*cos(q2)-630*dq0*sin(q1+q2)*cos(q2)^2+34914*dq1*sin(q2)*cos(q1+q2)+16380*dq1*sin(q2)*cos(q2)+34914*dq0*sin(q2)*cos(q1+q2)+16380*dq0*sin(q2)*cos(q2)+1200*dq0*sin(q2)*cos(q1+q2)^2+1200*dq1*sin(q2)*cos(q1+q2)^2-28800*dq0*sin(q1+q2)*cos(q1)^2+28800*dq0*sin(q1)*cos(q1+q2)*cos(q1)+315*sin(q2)*dq2*cos(q1+q2)*cos(q2)-9600*dq0*sin(q1+q2)*cos(q1+q2)*cos(q1)+7200*dq0*sin(q2)*cos(q1+q2)*cos(q1)+630*dq0*sin(q2)*cos(q1+q2)*cos(q2)+95082*sin(q2)*dq2+12426*dq0*sin(q1)+8640*dq0*sin(q1)*cos(q1+q2)*cos(q2)+9600*dq0*sin(q1)*cos(q1+q2)^2+8190*sin(q2)*dq2*cos(q2)+96564*sin(q2)*dq1-8238*dq0*sin(q1+q2)+15120*dq0*sin(q1)*cos(q2)*cos(q1)-7200*dq0*sin(q1+q2)*cos(q2)*cos(q1)+5400*sin(q2)*dq2*cos(q1+q2)*cos(q1)-1200*dq0*sin(q1+q2)*cos(q1+q2)*cos(q2)-14400*cos(q1)*dq1*sin(q1+q2)*cos(q1+q2)-14400*cos(q1)*dq2*sin(q1+q2)*cos(q1+q2)+14400*cos(q1)*dq1*sin(q1)*cos(q1+q2)-9720*cos(q2)*dq2*sin(q1+q2)*cos(q1)-2040*cos(q2)*dq2*sin(q1+q2)*cos(q1+q2)+96564*sin(q2)*dq0-12190*dq2*sin(q1+q2)-9720*cos(q2)*dq1*sin(q1+q2)*cos(q1)-2040*cos(q2)*dq1*sin(q1+q2)*cos(q1+q2)+7560*cos(q2)*dq1*sin(q1)*cos(q1)+6120*cos(q2)*dq1*sin(q1)*cos(q1+q2))/(67471+26752*cos(q1+q2)+13302*cos(q2)+80256*cos(q1)+9600*cos(q1)*cos(q1+q2)+4320*cos(q2)*cos(q1)+14400*cos(q1)^2+1600*cos(q1+q2)^2+1440*cos(q2)*cos(q1+q2)+315*cos(q2)^2);
        25/72*(1800*dq1*sin(q1)*cos(q1)+8200*dq1*sin(q1+q2)*cos(q1+q2)+3000*dq1*sin(q1+q2)*cos(q2)+600*dq1*sin(q1+q2)*cos(q1)+8200*dq2*sin(q1+q2)*cos(q1+q2)+3000*dq2*sin(q1+q2)*cos(q2)+600*dq2*sin(q1+q2)*cos(q1)+1600*dq2*sin(q1+q2)*cos(q1+q2)^2+1600*dq1*sin(q1+q2)*cos(q1+q2)^2+4800*dq1*sin(q1)*cos(q1+q2)^2+600*sin(q2)*dq2*cos(q1+q2)^2+3075*sin(q2)*dq2*cos(q1+q2)+225*sin(q2)*dq2*cos(q1)+30912*dq0*sin(q1)*cos(q1+q2)+11772*dq0*sin(q1)*cos(q2)+3600*dq0*sin(q1)*cos(q1)-37104*dq0*sin(q1+q2)*cos(q1+q2)-43284*dq0*sin(q1+q2)*cos(q2)-141024*dq0*sin(q1+q2)*cos(q1)+24600*dq1*sin(q1)*cos(q1+q2)+9000*dq1*sin(q1)*cos(q2)+1890*dq0*sin(q1)*cos(q2)^2+675*sin(q2)*dq2*cos(q2)*cos(q1)+10800*dq1*sin(q2)*cos(q1)^2+1350*dq1*sin(q2)*cos(q1)*cos(q2)+1350*dq0*sin(q2)*cos(q1)*cos(q2)+3000*dq1*sin(q1)+10800*dq0*sin(q2)*cos(q1)^2+67050*dq1*sin(q2)*cos(q1)+67050*dq0*sin(q2)*cos(q1)+1000*dq1*sin(q1+q2)+7200*dq1*sin(q2)*cos(q1+q2)*cos(q1)+450*dq1*sin(q2)*cos(q1+q2)*cos(q2)-630*dq0*sin(q1+q2)*cos(q2)^2+22350*dq1*sin(q2)*cos(q1+q2)+2250*dq1*sin(q2)*cos(q2)+22350*dq0*sin(q2)*cos(q1+q2)+2250*dq0*sin(q2)*cos(q2)+1200*dq0*sin(q2)*cos(q1+q2)^2+1200*dq1*sin(q2)*cos(q1+q2)^2-28800*dq0*sin(q1+q2)*cos(q1)^2+28800*dq0*sin(q1)*cos(q1+q2)*cos(q1)+225*sin(q2)*dq2*cos(q1+q2)*cos(q2)-9600*dq0*sin(q1+q2)*cos(q1+q2)*cos(q1)+7200*dq0*sin(q2)*cos(q1+q2)*cos(q1)+450*dq0*sin(q2)*cos(q1+q2)*cos(q2)+375*sin(q2)*dq2+3714*dq0*sin(q1)+8640*dq0*sin(q1)*cos(q1+q2)*cos(q2)+9600*dq0*sin(q1)*cos(q1+q2)^2+1125*sin(q2)*dq2*cos(q2)+81750*sin(q2)*dq1-50646*dq0*sin(q1+q2)+10800*dq0*sin(q1)*cos(q2)*cos(q1)-10080*dq0*sin(q1+q2)*cos(q2)*cos(q1)+1800*sin(q2)*dq2*cos(q1+q2)*cos(q1)-1680*dq0*sin(q1+q2)*cos(q1+q2)*cos(q2)+4800*cos(q1)*dq1*sin(q1+q2)*cos(q1+q2)+4800*cos(q1)*dq2*sin(q1+q2)*cos(q1+q2)+14400*cos(q1)*dq1*sin(q1)*cos(q1+q2)+1800*cos(q2)*dq2*sin(q1+q2)*cos(q1)+600*cos(q2)*dq2*sin(q1+q2)*cos(q1+q2)+81750*sin(q2)*dq0+1000*dq2*sin(q1+q2)+1800*cos(q2)*dq1*sin(q1+q2)*cos(q1)+600*cos(q2)*dq1*sin(q1+q2)*cos(q1+q2)+5400*cos(q2)*dq1*sin(q1)*cos(q1)+1800*cos(q2)*dq1*sin(q1)*cos(q1+q2))/(67471+26752*cos(q1+q2)+13302*cos(q2)+80256*cos(q1)+9600*cos(q1)*cos(q1+q2)+4320*cos(q2)*cos(q1)+14400*cos(q1)^2+1600*cos(q1+q2)^2+1440*cos(q2)*cos(q1+q2)+315*cos(q2)^2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                         -25/576*(-79616*dq1*sin(q1+q2)*cos(q1+q2)-30096*dq1*sin(q1+q2)*cos(q2)-9600*dq1*sin(q1+q2)*cos(q1)-79616*dq2*sin(q1+q2)*cos(q1+q2)-30096*dq2*sin(q1+q2)*cos(q2)-9600*dq2*sin(q1+q2)*cos(q1)-12800*dq2*sin(q1+q2)*cos(q1+q2)^2-12800*dq1*sin(q1+q2)*cos(q1+q2)^2+38400*dq1*sin(q1)*cos(q1+q2)^2-4800*sin(q2)*dq2*cos(q1+q2)^2-29856*sin(q2)*dq2*cos(q1+q2)-3600*sin(q2)*dq2*cos(q1)+268800*dq0*sin(q1+q2)*cos(q1+q2)+152640*dq0*sin(q1+q2)*cos(q2)-2304*dq0*sin(q1+q2)*cos(q1)+8448*dq1*sin(q1)*cos(q1+q2)+3888*dq1*sin(q1)*cos(q2)-2520*dq1*sin(q1+q2)*cos(q2)^2-10800*sin(q2)*dq2*cos(q2)*cos(q1)+7560*dq1*sin(q1)*cos(q2)^2+4320*dq1*sin(q2)*cos(q1)*cos(q2)+4320*dq0*sin(q2)*cos(q1)*cos(q2)+456*dq1*sin(q1)-864*dq1*sin(q2)*cos(q1)-864*dq0*sin(q2)*cos(q1)-9752*dq1*sin(q1+q2)-2520*dq2*sin(q1+q2)*cos(q2)^2-945*sin(q2)*dq2*cos(q2)^2+100800*dq1*sin(q2)*cos(q1+q2)+57240*dq1*sin(q2)*cos(q2)+100800*dq0*sin(q2)*cos(q1+q2)+57240*dq0*sin(q2)*cos(q2)-4320*sin(q2)*dq2*cos(q1+q2)*cos(q2)-3657*sin(q2)*dq2-11286*sin(q2)*dq2*cos(q2)+8712*sin(q2)*dq1+23232*dq0*sin(q1+q2)+11520*dq0*sin(q1+q2)*cos(q2)*cos(q1)-28800*sin(q2)*dq2*cos(q1+q2)*cos(q1)-76800*cos(q1)*dq1*sin(q1+q2)*cos(q1+q2)-76800*cos(q1)*dq2*sin(q1+q2)*cos(q1+q2)-28800*cos(q2)*dq2*sin(q1+q2)*cos(q1)-11520*cos(q2)*dq2*sin(q1+q2)*cos(q1+q2)+8712*sin(q2)*dq0-9752*dq2*sin(q1+q2)-28800*cos(q2)*dq1*sin(q1+q2)*cos(q1)-11520*cos(q2)*dq1*sin(q1+q2)*cos(q1+q2)+34560*cos(q2)*dq1*sin(q1)*cos(q1+q2))/(67471+26752*cos(q1+q2)+13302*cos(q2)+80256*cos(q1)+9600*cos(q1)*cos(q1+q2)+4320*cos(q2)*cos(q1)+14400*cos(q1)^2+1600*cos(q1+q2)^2+1440*cos(q2)*cos(q1+q2)+315*cos(q2)^2)];

Tor=Mh*(Rddq-Kv*(dq-Rdq)-Kp*(q-Rq))+Nh*dq;

dx2=inv(Mh)*(Tor-Nh*dq);
%dx2=Rddq-Kv*(dq-Rdq)-Kp*(q-Rq);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


end

ChaChing

原创 or 转贴 !? 个人专业有限, 看似像分享!? 是吗?
若是, 若能稍作简易说明下相关输入/输出/算法, 甚至举例, 或许更适合学习! 个人浅见

kyorri

楼主这个程序应该没有考虑卫星基座吧？空间机械手臂和卫星基座是动力学耦合的，那这个程序是解决什么问题的？能不能举例说下。感谢楼主分享！

博大广阔

这个是姿态和位置不受控制情况下，也就是耦合了动量守恒和动量矩守恒。 系统最好的方程直接放在子函数里面。 。。。程序不能，，但背后的理由很难几句话说清楚。也做机器手臂控制，及机器人末端轨迹跟踪的人。。跟我联系。。我们一起探讨

zmda

谢谢楼主分享

earth123654

貌似很复杂啊