motoman机器人正反解及其仿真分析.doc

Θ1-t图 Θ2-t图 Θ3-t图 Θ4-t图 Θ5-t图 Θ6-t图 理想轨迹 仿真轨迹 使用matlab计算motoman机器人的正、反解；雅克比并实现仿真（程序如下） %正解反解雅可比 clear all; syms thet; syms thet1 thet2 thet3 thet4 thet5 thet6; syms a1 a2 a3 d4; %正解 rotz=[cos(thet) -sin(thet) 0 0;sin(thet) cos(thet) 0 0;0 0 1 0;0 0 0 1]; t100=eye(4,4); t210=[1 0 0 a1;0 0 -1 0;0 1 0 0;0 0 0 1]; t320=[1 0 0 a2;0 1 0 0;0 0 1 0;0 0 0 1]; t430=[1 0 0 a3;0 0 -1 0;0 1 0 0;0 0 0 1]; t540=[1 0 0 0 ;0 0 1 0;0 -1 0 0;0 0 0 1]; t650=[1 0 0 0;0 0 -1 0;0 1 0 0 ;0 0 0 1]; tg0=[1 0 0 0;0 1 0 0 ;0 0 1 d4;0 0 0 1]; t10=subs(t100*rotz,thet,thet1); t21=subs(t210*rotz,thet,thet2); t32=subs(t320*rotz,thet,thet3); tg=t430*rotz; t43=subs(tg*tg0,thet,thet4); t54=subs(t540*rotz,thet,thet5); t65=subs(t650*rotz,thet,thet6); t61=t21*t32*t43*t54*t65; t62=t32*t43*t54*t65; t610=t10*t61 %反解 syms px py pz nx ny nz ox oy oz ax ay az; t60=[nx ox ax px;ny oy ay py;nz oz az pz;0 0 0 1]; %求theta1 t01=inv(t10);t610=t01*t60;f11=t610(2,4);f12=t61(2,4); %求theta3 f311=t610(1,4);f312=t61(1,4);f321=t610(3,4);f322=t61(3,4); %求theta2 t30=t10*t21*t32;t03=inv(t30);t631=t03*t60;t632=t43*t54*t65; f211=t631(1,4);f212=t632(1,4);f221=t631(2,4);f222=t632(2,4); %求theta4 f411=t631(1,3);f412=t632(1,3);f421=t631(3,3);f422=t632(3,3); %求theta5 t40=t10*t21*t32*t43;t04=inv(t40);t641=t04*t60;t642=t54*t65 f511=t641(1,3);f512=t642(1,3);f521=t641(3,3);f522=t642(3,3); %求theta6 t45=inv(t54);t651=t45*t641;t652=t45*t642; f611=t651(3,1);f612=t652(3,1);f621=t651(1,1);f622=t652(1,1); 【注释】此处可用simple命令简化各矩阵。 %雅可比 %j1 p1=t61; sp=[0 p1(3,4) -p1(2,4);-p1(3,4) 0 p1(1,4);p1(2,4) -p1(1,4) 0]; j12=p1(1:3,1:3); p2=inv(j12) j11=p2*sp; ja1=j11(:,3); jb1=p2(:,3); j1=[ja1;jb1] p1=t62; sp=[0 p1(3,4) -p1(2,4);-p1(3,4) 0 p1(1,4);p1(2,4) -p1(1,4) 0]; j22=p1(1:3,1:3); p3=inv(j22); j21=(inv(j22))*sp; ja2=j21(:,3); jb2=p3(:,3); j2=[ja2;jb2] p1=t632; sp=[0 p1(3,4