运动正解与逆解 matlab的编程.doc

%syms theta1 theta2 theta3 theta4 theta5 theta6syms d h l2 l3 l4 b1 b2 b3 b4 b5 b6%b1=cos(theta1) 0 -sin(theta1) 0;sin(theta1) 0 cos(theta1) 0;0 -1 0 h;0 0 0 1 b1 = cos(theta1), 0, -sin(theta1), 0 sin(theta1), 0, cos(theta1), 0 0, -1, 0, h 0, 0, 0, 1 b2=cos(theta2) 0 sin(theta2) l2*cos(theta2);sin(theta2) 0 -cos(theta2) l2*sin(theta2);0 1 0 0;0 0 0 1 b2 = cos(theta2), 0, sin(theta2), l2*cos(theta2) sin(theta2), 0, -cos(theta2), l2*sin(theta2) 0, 1, 0, 0 0, 0, 0, 1 b3=cos(theta3) 0 -sin(theta3) l3*cos(theta3);sin(theta3) 0 cos(theta3) l3*sin(theta3);0 -1 0 0;0 0 0 1 b3 = cos(theta3), 0, -sin(theta3), l3*cos(theta3) sin(theta3), 0, cos(theta3), l3*sin(theta3) 0, -1, 0, 0 0, 0, 0, 1 b4=cos(theta4) 0 sin(theta4) l4*cos(theta4);sin(theta4) 0 -cos(theta4) l4*sin(theta4);0 1 0 0; 0 0 0 1 b4 = cos(theta4), 0, sin(theta4), l4*cos(theta4) sin(theta4), 0, -cos(theta4), l4*sin(theta4) 0, 1, 0, 0 0, 0, 0, 1 b5=cos(theta5) 0 -sin(theta5) 0;sin(theta5) 0 cos(theta5) 0;0 -1 0 0;0 0 0 1 b5 = cos(theta5), 0, -sin(theta5), 0 sin(theta5), 0, cos(theta5), 0 0, -1, 0, 0 0, 0, 0, 1 b6=cos(theta6) -sin(theta6) 0 0; sin(theta6) cos(theta6) 0 0;0 0 1 d; 0 0 0 1 b6 = cos(theta6), -sin(theta6), 0, 0 sin(theta6), cos(theta6), 0, 0 0, 0, 1, d 0, 0, 0, 1% b1*b2*b3*b4*b5*b6 ans = sin(theta6)*(sin(theta4)*(sin(theta1)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3) - cos(theta1)*cos(theta4)*sin(theta2) - cos(theta6)*(cos(theta5)*(cos(theta4)*(sin(theta1)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3) + cos(theta1)*sin(theta2)*sin(theta4) + sin(theta5)*(cos(theta3)*sin(theta1) + cos(theta1)*cos(theta2)*sin(theta3), cos(theta6)*(sin(theta4)*(sin(theta1)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3) - cos(theta1)*cos(theta4)*sin(theta2) + sin(theta6)*(cos(theta5)*(cos(theta4)*(sin(theta1)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3) + cos(theta1)*sin(theta2)*sin(theta4) + sin(theta5)*(cos(theta3)*sin(theta1) + cos(theta1)*cos(theta2)*sin(theta3), sin(theta5)*(cos(theta4)*(sin(theta1)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3) + cos(theta1)*sin(theta2)*sin(theta4) - cos(theta5)*(cos(theta3)*sin(theta1) + cos(theta1)*cos(theta2)*sin(theta3), d*(sin(theta5)*(cos(theta4)*(sin(theta1)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3) + cos(theta1)*sin(theta2)*sin(theta4) - cos(theta5)*(cos(theta3)*sin(theta1) + cos(theta1)*cos(theta2)*sin(theta3) - l4*cos(theta4)*(sin(theta1)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3) + l2*cos(theta1)*cos(theta2) - l3*sin(theta1)*sin(theta3) + l3*cos(theta1)*cos(theta2)*cos(theta3) - l4*cos(theta1)*sin(theta2)*sin(theta4) cos(theta6)*(cos(theta5)*(cos(theta4)*(cos(theta1)*sin(theta3) + cos(theta2)*cos(theta3)*sin(theta1) - sin(theta1)*sin(theta2)*sin(theta4) + sin(theta5)*(cos(theta1)*cos(theta3) - cos(theta2)*sin(theta1)*sin(theta3) - sin(theta6)*(sin(theta4)*(cos(theta1)*sin(theta3) + cos(theta2)*cos(theta3)*sin(theta1) + cos(theta4)*sin(theta1)*sin(theta2), - cos(theta6)*(sin(theta4)*(cos(theta1)*sin(theta3) + cos(theta2)*cos(theta3)*sin(theta1) + cos(theta4)*sin(theta1)*sin(theta2) - sin(theta6)*(cos(theta5)*(cos(theta4)*(cos(theta1)*sin(theta3) + cos(theta2)*cos(theta3)*sin(theta1) - sin(theta1)*sin(theta2)*sin(theta4) + sin(theta5)*(cos(theta1)*cos(theta3) - cos(theta2)*sin(theta1)*sin(theta3), cos(theta5)*(cos(theta1)*cos(theta3) - cos(theta2)*sin(theta1)*sin(theta3) - sin(theta5)*(cos(theta4)*(cos(theta1)*sin(theta3) + cos(theta2)*cos(theta3)*sin(theta1) - sin(theta1)*sin(theta2)*sin(theta4), l4*cos(theta4)*(cos(theta1)*sin(theta3) + cos(theta2)*cos(theta3)*sin(theta1) - d*(sin(theta5)*(cos(theta4)*(cos(theta1)*sin(theta3) + cos(theta2)*cos(theta3)*sin(theta1) - sin(theta1)*sin(theta2)*sin(theta4) - cos(theta5)*(cos(theta1)*cos(theta3) - cos(theta2)*sin(theta1)*sin(theta3) + l2*cos(theta2)*sin(theta1) + l3*cos(theta1)*sin(theta3) + l3*cos(theta2)*cos(theta3)*sin(theta1) - l4*sin(theta1)*sin(theta2)*sin(theta4) - cos(theta6)*(cos(theta5)*(cos(theta2)*sin(theta4) + cos(theta3)*cos(theta4)*sin(theta2) - sin(theta2)*sin(theta3)*sin(theta5) - sin(theta6)*(cos(theta2)*cos(theta4) - cos(theta3)*sin(theta2)*sin(theta4), sin(theta6)*(cos(theta5)*(cos(theta2)*sin(theta4) + cos(theta3)*cos(theta4)*sin(theta2) - sin(theta2)*sin(theta3)*sin(theta5) - cos(theta6)*(cos(theta2)*cos(theta4) - cos(theta3)*sin(theta2)*sin(theta4), sin(theta5)*(cos(theta2)*sin(theta4) + cos(theta3)*cos(theta4)*sin(theta2) + cos(theta5)*sin(theta2)*sin(theta3), h - l2*sin(theta2) + d*(sin(theta5)*(cos(theta2)*sin(theta4) + cos(theta3)*cos(theta4)*sin(theta2) + cos(theta5)*sin(theta2)*sin(theta3) - l3*cos(theta3)*sin(theta2) - l4*cos(theta2)*sin(theta4) - l4*cos(theta3)*cos(theta4)*sin(theta2) 0, 0, 0, 1 %syms theta1 theta2 theta3 theta4 theta5 theta6 d l2 l3 l4 theta1=0,theta2=0,theta3=0,theta4=0,theta5=0,theta6=0x= d*(sin(theta5).*(cos(theta4).*(sin(theta1).*sin(theta3)-cos(theta1).*cos(theta2).*cos(theta3)+cos(theta1).*sin(theta2).*sin(theta4)-cos(theta5).*(cos(theta3).*sin(theta1) +cos(theta1).*cos(theta2).*sin(theta3)-l4*cos(theta4).*(sin(theta1).*sin(theta3)-cos(theta1).*cos(theta2).*cos(theta3)+l2*cos(theta1).*cos(theta2)-l3*sin(theta1).*sin(theta3) + l3*cos(theta1).*cos(theta2).*cos(theta3) - l4*cos(theta1).*sin(theta2).*sin(theta4)y= l4*cos(theta4).*(cos(theta1).*sin(theta3) + cos(theta2).*cos(theta3).*sin(theta1) - d*(sin(theta5).*(cos(theta4).*(cos(theta1).*sin(theta3) + cos(theta2).*cos(theta3).*sin(theta1) - sin(theta1).*sin(theta2).*sin(theta4) - cos(theta5).*(cos(theta1).*cos(theta3) - cos(theta2).*sin(theta1).*sin(theta3) + l2*cos(theta2).*sin(theta1) + l3*cos(theta1).*sin(theta3) + l3*cos(theta2).*cos(theta3).*sin(theta1) - l4*sin(theta1).*sin(theta2).*sin(theta4)z= h - l2*sin(theta2) + d*(sin(theta5).*(cos(theta2).*sin(theta4) + cos(theta3).*cos(theta4).*sin(theta2) + cos(theta5).*sin(theta2).*sin(theta3) - l3*cos(theta3).*sin(theta2) - l4*cos(theta2).*sin(theta4) - l4*cos(theta3).*cos(theta4).*sin(theta2)theta1 = 0theta2 = 0theta3 = 0theta4 = 0theta5 = 0theta6 = 0 x = l2 + l3 + l4 y = d z = h% k1=inv(b1); k2=inv(b2); k3=inv(b3); k4=inv(b4); k5=inv(b5); k6=inv(b6); k1 k1 = cos(theta1), sin(theta1), 0, 0 0, 0, -1, h -sin(theta1), cos(theta1), 0, 0 0, 0, 0, 1 k2 k2 = cos(theta2), sin(theta2), 0, -l2 0, 0, 1, 0 sin(theta2), -cos(theta2), 0, 0 0, 0, 0, 1 k3 k3 = cos(theta3), sin(theta3), 0, -l3 0, 0, -1, 0 -sin(theta3), cos(theta3), 0, 0 0, 0, 0, 1 k4 k4 = cos(theta4), sin(theta4), 0, -l4 0, 0, 1, 0 sin(theta4), -cos(theta4), 0, 0 0, 0, 0, 1 k5 k5 = cos(theta5), sin(theta5), 0, 0 0, 0, -1, 0 -sin(theta5), cos(theta5), 0, 0 0, 0, 0, 1 k6 k6 = cos(theta6), sin(theta6), 0, 0 -sin(theta6), cos(theta6), 0, 0 0, 0, 1, -d 0, 0, 0, 1% syms nx ox ax px ny oy ay py nz oz az pz t6=nx ox ax px;ny oy ay py;nz oz az pz;0 0 0 1 t6 = nx, ox, ax, px ny, oy, ay, py nz, oz, az, pz 0, 0, 0, 1 k1*t6 ans = nx*cos(theta1) + ny*sin(theta1), ox*cos(theta1) + oy*sin(theta1), ax*cos(theta1) + ay*sin(theta1), px*cos(theta1) + py*sin(theta1) -nz, -oz, -az, h - pz ny*cos(theta1) - nx*sin(theta1), oy*cos(theta1) - ox*sin(theta1), ay*cos(theta1) - ax*sin(theta1), py*cos(theta1) - px*sin(theta1) 0, 0, 0, 1 k2*k1*t6 ans = nx*cos(theta1)*cos(theta2) - nz*sin(theta2) + ny*cos(theta2)*sin(theta1), ox*cos(theta1)*cos(theta2) - oz*sin(theta2) + oy*cos(theta2)*sin(theta1), ax*cos(theta1)*cos(theta2) - az*sin(theta2) + ay*cos(theta2)*sin(theta1), h*sin(theta2) - l2 - pz*sin(theta2) + px*cos(theta1)*cos(theta2) + py*cos(theta2)*sin(theta1) ny*cos(theta1) - nx*sin(theta1), oy*cos(theta1) - ox*sin(theta1), ay*cos(theta1) - ax*sin(theta1), py*cos(theta1) - px*sin(theta1) nz*cos(theta2) + nx*cos(theta1)*sin(theta2) + ny*sin(theta1)*sin(theta2), oz*cos(theta2) + ox*cos(theta1)*sin(theta2) + oy*sin(theta1)*sin(theta2), az*cos(theta2) + ax*cos(theta1)*sin(theta2) + ay*sin(theta1)*sin(theta2), pz*cos(theta2) - h*cos(theta2) + px*cos(theta1)*sin(theta2) + py*sin(theta1)*sin(theta2) 0, 0, 0, 1 k3*k2*k1*t6 ans = ny*(cos(theta1)*sin(theta3) + cos(theta2)*cos(theta3)*sin(theta1) - nx*(sin(theta1)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3) - nz*cos(theta3)*sin(theta2), oy*(cos(theta1)*sin(theta3) + cos(theta2)*cos(theta3)*sin(theta1) - ox*(sin(theta1)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3) - oz*cos(theta3)*sin(theta2), ay*(cos(theta1)*sin(theta3) + cos(theta2)*cos(theta3)*sin(theta1) - ax*(sin(theta1)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3) - az*cos(theta3)*sin(theta2), py*(cos(theta1)*sin(theta3) + cos(theta2)*cos(theta3)*sin(theta1) - px*(sin(theta1)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3) - l3 - l2*cos(theta3) + h*cos(theta3)*sin(theta2) - pz*cos(theta3)*sin(theta2) - nz*cos(theta2) - nx*cos(theta1)*sin(theta2) - ny*sin(theta1)*sin(theta2), - oz*cos(theta2) - ox*cos(theta1)*sin(theta2) - oy*sin(theta1)*sin(theta2), - az*cos(theta2) - ax*cos(theta1)*sin(theta2) - ay*sin(theta1)*sin(theta2), h*cos(theta2) - pz*cos(theta2) - px*cos(theta1)*sin(theta2) - py*sin(theta1)*sin(theta2) ny*(cos(theta1)*cos(theta3) - cos(theta2)*sin(theta1)*sin(theta3) - nx*(cos(theta3)*sin(theta1) + cos(theta1)*cos(theta2)*sin(theta3) + nz*sin(theta2)*sin(theta3), oy*(cos(theta1)*cos(theta3) - cos(theta2)*sin(theta1)*sin(theta3) - ox*(cos(theta3)*sin(theta1) + cos(theta1)*cos(theta2)*sin(theta3) + oz*sin(theta2)*sin(theta3), ay*(cos(theta1)*cos(theta3) - cos(theta2)*sin(theta1)*sin(theta3) - ax*(cos(theta3)*sin(theta1) + cos(theta1)*cos(theta2)*sin(theta3) + az*sin(theta2)*sin(theta3), py*(cos(theta1)*cos(theta3) - cos(theta2)*sin(theta1)*sin(theta3) - px*(cos(theta3)*sin(theta1) + cos(theta1)*cos(theta2)*sin(theta3) + l2*sin(theta3) - h*sin(theta2)*sin(theta3) + pz*sin(theta2)*sin(theta3) 0, 0, 0, 1 k4*k3*k2*k1*t6 ans = - nz*(cos(theta2)*sin(theta4) + cos(theta3)*cos(theta4)*sin(theta2) - nx*(cos(theta1)*(sin(theta2)*sin(theta4) - cos(theta2)*cos(theta3)*cos(theta4) + cos(theta4)*sin(theta1)*sin(theta3) - ny*(sin(theta1)*(sin(theta2)*sin(theta4) - cos(theta2)*cos(theta3)*cos(theta4) - cos(theta1)*cos(theta4)*sin(theta3), - oz*(cos(theta2)*sin(theta4) + cos(theta3)*cos(theta4)*sin(theta2) - ox*(cos(theta1)*(sin(theta2)*sin(theta4) - cos(theta2)*cos(theta3)*cos(theta4) + cos(theta4)*sin(theta1)*sin(theta3) - oy*(sin(theta1)*(sin(theta2)*sin(theta4) - cos(theta2)*cos(theta3)*cos(theta4) - cos(theta1)*cos(theta4)*sin(theta3), - az*(cos(theta2)*sin(theta4) + cos(theta3)*cos(theta4)*sin(theta2) - ax*(cos(theta1)*(sin(theta2)*sin(theta4) - cos(theta2)*cos(theta3)*cos(theta4) + cos(theta4)*sin(theta1)*sin(theta3) - ay*(sin(theta1)*(sin(theta2)*sin(theta4) - cos(theta2)*cos(theta3)*cos(theta4) - cos(theta1)*cos(theta4)*sin(theta3), h*(cos(theta2)*sin(theta4) + cos(theta3)*cos(theta4)*sin(theta2) - l4 - pz*(cos(theta2)*sin(theta4) + cos(theta3)*cos(theta4)*sin(theta2) - l3*cos(theta4) - px*(cos(theta1)*(sin(theta2)*sin(theta4) - cos(theta2)*cos(theta3)*cos(theta4) + cos(theta4)*sin(theta1)*sin(theta3) - py*(sin(theta1)*(sin(theta2)*sin(theta4) - cos(theta2)*cos(theta3)*cos(theta4) - cos(theta1)*cos(theta4)*sin(theta3) - l2*cos(theta3)*cos(theta4) ny*(cos(theta1)*cos(theta3) - cos(theta2)*sin(theta1)*sin(theta3) - nx*(cos(theta3)*sin(theta1) + cos(theta1)*cos(theta2)*sin(theta3) + nz*sin(theta2)*sin(theta3), oy*(cos(theta1)*cos(theta3) - cos(theta2)*sin(theta1)*sin(theta3) - ox*(cos(theta3)*sin(theta1) + cos(theta1)*cos(theta2)*sin(theta3) + oz*sin(theta2)*sin(theta3), ay*(cos(theta1)*cos(theta3) - cos(theta2)*sin(theta1)*sin(theta3) - ax*(cos(theta3)*sin(theta1) + cos(theta1)*cos(theta2)*sin(theta3) + az*sin(theta2)*sin(theta3), py*(cos(theta1)*cos(theta3) - cos(theta2)*sin(theta1)*sin(theta3) - px*(cos(theta3)*sin(theta1) + cos(theta1)*cos(theta2)*sin(theta3) + l2*sin(theta3) - h*sin(theta2)*sin(theta3) + pz*sin(theta2)*sin(theta3) nz*(cos(theta2)*cos(theta4) - cos