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

Θ1-t图Θ2-t图

Θ3-t图Θ4-t图

Θ5-t图Θ6-t图

理想轨迹仿真轨迹

使用matlab计算motoman机器人的正、反解；雅克比并实现仿真（程序如下）

%正解反解雅可比

clear all;

syms thet;

syms thet1thet2thet3thet4thet5thet6;

syms a1a2a3d4;

%正解

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) -p1(2,4);-p1(3,4) 0 p1(1,4);p1(2,4) -p1(1,4) 0]; j32=p1(1:3,1:3);

p4=inv(j32);

j31=(inv(j32))*sp;

ja3=j31(:,3);

jb3=p4(:,3);

j3=[ja3;jb3]

p1=t642;

sp=[0 p1(3,4) -p1(2,4);-p1(3,4) 0 p1(1,4);p1(2,4) -p1(1,4) 0]; j42=p1(1:3,1:3);

p5=inv(j42);

j41=(inv(j42))*sp;

ja4=j41(:,3);

jb4=p5(:,3);

j4=[ja4;jb4]

p1=t65;

sp=[0 p1(3,4) -p1(2,4);-p1(3,4) 0 p1(1,4);p1(2,4) -p1(1,4) 0]; j52=p1(1:3,1:3);

p6=inv(j52);

j51=(inv(j52))*sp;

ja5=j51(:,3);

jb5=p6(:,3);

j5=[ja5;jb5]

【注释】同样用simple命令简化各雅克比矩阵。