一、运动学正解程序及结果
1、程序:
syms x1 x2 x3 x4 x5 x6 d1 d2 d4 a2 a3 x d a
Rx=[1 0 0 0;0 cos(x) -sin(x) 0;0 sin(x) cos(x) 0;0 0 0 1];
Rz=[cos(x) -sin(x) 0 0;sin(x) cos(x) 0 0;0 0 1 0;0 0 0 1];
Tx=[1 0 0 a;0 1 0 0;0 0 1 0;0 0 0 1];
Tz=[1 0 0 0;0 1 0 0;0 0 1 d;0 0 0 1];
t=pi/180;
y1=90;y2=-90;y3=-90;
T01=subs(Rz,x,x1)*subs(Tz,d,d1)*subs(Rx,x,y1*t);
T12=subs(Rz,x,x2)*subs(Tz,d,d2)*subs(Tx,a,a2);
T23=subs(Rz,x,x3)*subs(Tx,a,a3)*subs(Rx,x,y3*t);
T34=subs(Rz,x,x4)*subs(Tz,d,d4)*subs(Rx,x,y4*t);
T45=subs(Rz,x,x5)*subs(Rx,x,90);
T=T01*T12*T23*T34*T45;
t=subs(T,{y1,y3,y4,y5},[pi/2,-pi/2,-pi/2,pi/2]);
t= simplify(t);
nx=t(1,1);ny=t(2,1);nz=t(3,1);
ox=t(1,2);oy=t(2,2);oz=t(3,2);
ax=t(1,3);ay=t(2,3);az=t(3,3);
px=t(1,4);py=t(2,4);pz=t(3,4);
结果:
Nx=sin(x2 + x3)*cos(x1)*sin(x5) -
cos(x5)*sin(x1)*sin(x4)+cos(x1)*cos(x2)*cos(x3)*cos(x4)*cos(x5) -
cos(x1)*cos(x4)*cos(x5)*sin(x2)*sin(x3)
Ny=cos(x1)*cos(x5)*sin(x4) + sin(x2 +
x3)*sin(x1)*sin(x5)+cos(x2)*cos(x3)*cos(x4)*cos(x5)*sin(x1) -
cos(x4)*cos(x5)*sin(x1)*sin(x2)*sin(x3)
Nz=sin(x2 + x3)*cos(x4)*cos(x5) - cos(x2 + x3)*sin(x5)
Ox=sin(x4)*(cos(x1)*sin(x2)*sin(x3) - cos(x1)*cos(x2)*cos(x3)) - cos(x4)*sin(x1)
Oy=cos(x1)*cos(x4) - sin(x4)*(cos(x2)*cos(x3)*sin(x1) - sin(x1)*sin(x2)*sin(x3))
Oz=-sin(x2 + x3)*sin(x4)
Ax=cos(x1)*cos(x2)*cos(x3)*cos(x4)*sin(x5) - sin(x2 + x3)*cos(x1)*cos(x5) -
sin(x1)*sin(x4)*sin(x5) - cos(x1)*cos(x4)*sin(x2)*sin(x3)*sin(x5)
Ay=cos(x1)*sin(x4)*sin(x5) - sin(x2 + x3)*cos(x5)*sin(x1) +
评论0