空间机器手臂的计算力矩控制法仿真程序
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=;L=;A=;J=; %symstem parameters
%the care number of robot arm one poseand two joint
h=0.005; %the size of simulation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%模块二：givened the innitial value%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t=0; tmax=10; %simulation from 0 to 10 /s
q=.'; %innitial angle of pose and joint
dq=zeros(2,1); %innitial angular velocity
qb=;
dq0=;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
TT=0;
qq=q;
RRq=; %初始角度
RRdq=;
RRddq=;
ddqq=zeros(2,1);
qqb=;
TTor=;
dx0=;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%模块三：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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
=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;
=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;
=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;
=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=;
qqb=;
qq=;
RRq=;
TTor=;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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 =derives(q,dq,Rddq,Rdq,Rq,dq0)
% solution the stale variable derives ，求解力矩，角度的倒数（角速度），角速度的导数（叫加速度），姿态角的导数（姿态角角速度）
global M L I A J
Kv=;Kv=diag(Kv);
Kp=;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 于 2011-10-23 16:27 编辑
原创 or 转贴 !? 个人专业有限, 看似像分享!? 是吗?
若是, 若能稍作简易说明下相关输入/输出/算法, 甚至举例, 或许更适合学习! 个人浅见 楼主这个程序应该没有考虑卫星基座吧？空间机械手臂和卫星基座是动力学耦合的，那这个程序是解决什么问题的？能不能举例说下。感谢楼主分享！ 这个是姿态和位置不受控制情况下，也就是耦合了动量守恒和动量矩守恒。 系统最好的方程直接放在子函数里面。 。。。程序不能，，但背后的理由很难几句话说清楚。也做机器手臂控制，及机器人末端轨迹跟踪的人。。跟我联系。。我们一起探讨
谢谢楼主分享 {:{10}:}貌似很复杂啊
页:
[1]