运动正解与逆解 matlab的编程

%矩阵中参数定义：
syms theta1 theta2 theta3 theta4 theta5 theta6
syms 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(thet

a1)*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=0；
x= 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 =

0


theta2 =

0


theta3 =

0


theta4 =

0


theta5 =

0


theta6 =

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(theta3)*sin(theta2)*sin(theta4)) + nx*(cos(theta1)*(cos(theta4)*sin(theta2) + cos(theta2)*cos(theta3)*sin(theta4)) - sin(theta1)*sin(theta3)*sin(theta4)) + ny*(sin(theta1)*(cos(theta4)*sin(theta2) + cos(theta2)*cos(theta3)*sin(theta4)) + cos(theta1)*sin(theta3)*sin(theta4)), oz*(cos(theta2)*cos(theta4) - cos(theta3)*sin(theta2)*sin(theta4)) + ox*(cos(theta1)*(cos(theta4)*sin(theta2) + cos(theta2)*cos(theta3)*sin(theta4)) - sin(theta1)*sin(theta3)*sin(theta4)) + oy*(sin(theta1)*(cos(theta4)*sin(theta2) + cos(theta2)*cos(theta3)*sin(theta4)) + cos(theta1)*sin(theta3)*sin(theta4)), az*(cos(theta2)*cos(theta4) - cos(theta3)*sin(theta2)*sin(theta4)) + ax*(cos(theta1)*(cos(theta4)*sin(theta2) + cos(theta2)*cos(theta3)*sin(theta4)) - sin(theta1)*sin(theta3)*sin(theta4)) + ay*(sin(theta1)*(cos(theta4)*sin(theta2) + cos(theta2)*cos(theta3)*sin(theta4)) + cos(theta1)*sin(theta3)*sin(theta4)), pz*(cos(theta2)*cos(theta4) - cos(theta3)*sin(theta2)*sin(theta4)) - h*(cos(theta2)*cos(theta4) - cos(theta3)*sin(theta2)*sin(theta4)) - l3*sin(theta4) + px*(cos(theta1)*(cos(theta4)*sin(theta2) + cos(theta2)*cos(theta3)*sin(theta4)) - sin(theta1)*sin(theta3)*sin(theta4)) + py*(sin(theta1)*(cos(theta4)*sin(theta2) + cos(theta2)*cos(theta3)*sin(theta4)) + cos(theta1)*sin(theta3)*sin(theta4)) - l2*cos(theta3)*sin(theta4)]
[ 0, 0, 0, 1]

>> k5*k4*k3*k2*k1*t6

ans =

[ nz*(sin(theta2)*(sin(theta3)*sin(theta5) - cos(theta3)*cos(theta4)*cos(theta5)) - cos(theta2)*cos(theta5)*sin(theta4)) - ny*(sin(theta1)*(cos(theta2)*(sin(theta3)*sin(theta5) - cos(theta3)*cos(theta4)*cos(theta5)) + cos(theta5)*sin(theta2)*sin(

theta4)) - cos(theta1)*(cos(theta3)*sin(theta5) + cos(theta4)*cos(theta5)*sin(theta3))) - nx*(cos(theta1)*(cos(theta2)*(sin(theta3)*sin(theta5) - cos(theta3)*cos(theta4)*cos(theta5)) + cos(theta5)*sin(theta2)*sin(theta4)) + sin(theta1)*(cos(theta3)*sin(theta5) + cos(theta4)*cos(theta5)*sin(theta3))), oz*(sin(theta2)*(sin(theta3)*sin(theta5) - cos(theta3)*cos(theta4)*cos(theta5)) - cos(theta2)*cos(theta5)*sin(theta4)) - oy*(sin(theta1)*(cos(theta2)*(sin(theta3)*sin(theta5) - cos(theta3)*cos(theta4)*cos(theta5)) + cos(theta5)*sin(theta2)*sin(theta4)) - cos(theta1)*(cos(theta3)*sin(theta5) + cos(theta4)*cos(theta5)*sin(theta3))) - ox*(cos(theta1)*(cos(theta2)*(sin(theta3)*sin(theta5) - cos(theta3)*cos(theta4)*cos(theta5)) + cos(theta5)*sin(theta2)*sin(theta4)) + sin(theta1)*(cos(theta3)*sin(theta5) + cos(theta4)*cos(theta5)*sin(theta3))), az*(sin(theta2)*(sin(theta3)*sin(theta5) - cos(theta3)*cos(theta4)*cos(theta5)) - cos(theta2)*cos(theta5)*sin(theta4)) - ay*(sin(theta1)*(cos(theta2)*(sin(theta3)*sin(theta5) - cos(theta3)*cos(theta4)*cos(theta5)) + cos(theta5)*sin(theta2)*sin(theta4)) - cos(theta1)*(cos(theta3)*sin(theta5) + cos(theta4)*cos(theta5)*sin(theta3))) - ax*(cos(theta1)*(cos(theta2)*(sin(theta3)*sin(theta5) - cos(theta3)*cos(theta4)*cos(theta5)) + cos(theta5)*sin(theta2)*sin(theta4)) + sin(theta1)*(cos(theta3)*sin(theta5) + cos(theta4)*cos(theta5)*sin(theta3))), l2*(sin(theta3)*sin(theta5) - cos(theta3)*cos(theta4)*cos(theta5)) - py*(sin(theta1)*(cos(theta2)*(sin(theta3)*sin(theta5) - cos(theta3)*cos(theta4)*cos(theta5)) + cos(theta5)*sin(theta2)*sin(theta4)) - cos(theta1)*(cos(theta3)*sin(theta5) + cos(theta4)*cos(theta5)*sin(theta3))) - px*(cos(theta1)*(cos(theta2)*(sin(theta3)*sin(theta5) - cos(theta3)*cos(theta4)*cos(theta5)) + cos(theta5)*sin(theta2)*sin(theta4)) + sin(theta1)*(cos(theta3)*sin(theta5) + cos(theta4)*cos(theta5)*sin(theta3))) - l4*cos(theta5) - h*(sin(theta2)*(sin(theta3)*sin(theta5) - cos(theta3)*cos(theta4)*cos(theta5)) - cos(theta2)*cos(theta5)*sin(theta4)) + pz*(sin(theta2)*(sin(theta3)*sin(theta5) - cos(theta3)*cos(theta4)*cos(theta5)) - cos(theta2)*cos(theta5)*sin(theta4)) - l3*cos(theta4)*cos(theta5)]
[ - nz*(cos(theta2)*cos(theta4) - cos(theta3)*sin(theta2)*sin(theta4)) - nx*(cos(theta1)*(cos(theta4)*sin(theta2) + cos(theta2)*cos(theta3)*sin(theta4)) - sin(theta1)*sin(theta3)*sin(theta4)) - ny*(sin(theta1)*(cos(theta4)*sin(theta2) + cos(theta2)*cos(theta3)*sin(theta4)) + cos(theta1)*sin(theta3)*sin(theta4)), - oz*(cos(theta2)*cos(theta4) - cos(theta3

)*sin(theta2)*sin(theta4)) - ox*(cos(theta1)*(cos(theta4)*sin(theta2) + cos(theta2)*cos(theta3)*sin(theta4)) - sin(theta1)*sin(theta3)*sin(theta4)) - oy*(sin(theta1)*(cos(theta4)*sin(theta2) + cos(theta2)*cos(theta3)*sin(theta4)) + cos(theta1)*sin(theta3)*sin(theta4)), - az*(cos(theta2)*cos(theta4) - cos(theta3)*sin(theta2)*sin(theta4)) - ax*(cos(theta1)*(cos(theta4)*sin(theta2) + cos(theta2)*cos(theta3)*sin(theta4)) - sin(theta1)*sin(theta3)*sin(theta4)) - ay*(sin(theta1)*(cos(theta4)*sin(theta2) + cos(theta2)*cos(theta3)*sin(theta4)) + cos(theta1)*sin(theta3)*sin(theta4)), h*(cos(theta2)*cos(theta4) - cos(theta3)*sin(theta2)*sin(theta4)) - pz*(cos(theta2)*cos(theta4) - cos(theta3)*sin(theta2)*sin(theta4)) + l3*sin(theta4) - px*(cos(theta1)*(cos(theta4)*sin(theta2) + cos(theta2)*cos(theta3)*sin(theta4)) - sin(theta1)*sin(theta3)*sin(theta4)) - py*(sin(theta1)*(cos(theta4)*sin(theta2) + cos(theta2)*cos(theta3)*sin(theta4)) + cos(theta1)*sin(theta3)*sin(theta4)) + l2*cos(theta3)*sin(theta4)]
[ nz*(sin(theta2)*(cos(theta5)*sin(theta3) + cos(theta3)*cos(theta4)*sin(theta5)) + cos(theta2)*sin(theta4)*sin(theta5)) - ny*(sin(theta1)*(cos(theta2)*(cos(theta5)*sin(theta3) + cos(theta3)*cos(theta4)*sin(theta5)) - sin(theta2)*sin(theta4)*sin(theta5)) - cos(theta1)*(cos(theta3)*cos(theta5) - cos(theta4)*sin(theta3)*sin(theta5))) - nx*(cos(theta1)*(cos(theta2)*(cos(theta5)*sin(theta3) + cos(theta3)*cos(theta4)*sin(theta5)) - sin(theta2)*sin(theta4)*sin(theta5)) + sin(theta1)*(cos(theta3)*cos(theta5) - cos(theta4)*sin(theta3)*sin(theta5))), oz*(sin(theta2)*(cos(theta5)*sin(theta3) + cos(theta3)*cos(theta4)*sin(theta5)) + cos(theta2)*sin(theta4)*sin(theta5)) - oy*(sin(theta1)*(cos(theta2)*(cos(theta5)*sin(theta3) + cos(theta3)*cos(theta4)*sin(theta5)) - sin(theta2)*sin(theta4)*sin(theta5)) - cos(theta1)*(cos(theta3)*cos(theta5) - cos(theta4)*sin(theta3)*sin(theta5))) - ox*(cos(theta1)*(cos(theta2)*(cos(theta5)*sin(theta3) + cos(theta3)*cos(theta4)*sin(theta5)) - sin(theta2)*sin(theta4)*sin(theta5)) + sin(theta1)*(cos(theta3)*cos(theta5) - cos(theta4)*sin(theta3)*sin(theta5))), az*(sin(theta2)*(cos(theta5)*sin(theta3) + cos(theta3)*cos(theta4)*sin(theta5)) + cos(theta2)*sin(theta4)*sin(theta5)) - ay*(sin(theta1)*(cos(theta2)*(cos(theta5)*sin(theta3) + cos(theta3)*cos(theta4)*sin(theta5)) - sin(theta2)*sin(theta4)*sin(theta5)) - cos(theta1)*(cos(theta3)*cos(theta5) - cos(theta4)*si

n(theta3)*sin(theta5))) - ax*(cos(theta1)*(cos(theta2)*(cos(theta5)*sin(theta3) + cos(theta3)*cos(theta4)*sin(theta5)) - sin(theta2)*sin(theta4)*sin(theta5)) + sin(theta1)*(cos(theta3)*cos(theta5) - cos(theta4)*sin(theta3)*sin(theta5))), l2*(cos(theta5)*sin(theta3) + cos(theta3)*cos(theta4)*sin(theta5)) - px*(cos(theta1)*(cos(theta2)*(cos(theta5)*sin(theta3) + cos(theta3)*cos(theta4)*sin(theta5)) - sin(theta2)*sin(theta4)*sin(theta5)) + sin(theta1)*(cos(theta3)*cos(theta5) - cos(theta4)*sin(theta3)*sin(theta5))) - py*(sin(theta1)*(cos(theta2)*(cos(theta5)*sin(theta3) + cos(theta3)*cos(theta4)*sin(theta5)) - sin(theta2)*sin(theta4)*sin(theta5)) - cos(theta1)*(cos(theta3)*cos(theta5) - cos(theta4)*sin(theta3)*sin(theta5))) + l4*sin(theta5) - h*(sin(theta2)*(cos(theta5)*sin(theta3) + cos(theta3)*cos(theta4)*sin(theta5)) + cos(theta2)*sin(theta4)*sin(theta5)) + pz*(sin(theta2)*(cos(theta5)*sin(theta3) + cos(theta3)*cos(theta4)*sin(theta5)) + cos(theta2)*sin(theta4)*sin(theta5)) + l3*cos(theta4)*sin(theta5)]
[ 0, 0, 0,

1]







%逆解部分2

>> b2*b3*b4*b5*b6


ans =

[ - cos(theta6)*(cos(theta5)*(sin(theta2)*sin(theta4) - cos(theta2)*cos(theta3)*cos(theta4)) + cos(theta2)*sin(theta3)*sin(theta5)) - sin(theta6)*(cos(theta4)*sin(theta2) + cos(theta2)*cos(theta3)*sin(theta4)), sin(theta6)*(cos(theta5)*(sin(theta2)*sin(theta4) - cos(theta2)*cos(theta3)*cos(theta4)) + cos(theta2)*sin(theta3)*sin(theta5)) - cos(theta6)*(cos(theta4)*sin(theta2) + cos(theta2)*cos(theta3)*sin(theta4)), sin(theta5)*(sin(theta2)*sin(theta4) - cos(theta2)*cos(theta3)*cos(theta4)) - cos(theta2)*cos(theta5)*sin(theta3), l2*cos(theta2) + d*(sin(theta5)*(sin(theta2)*sin(theta4) - cos(theta2)*cos(theta3)*cos(theta4)) - cos(theta2)*cos(theta5)*sin(theta3)) + l3*cos(theta2)*cos(theta3) - l4*sin(theta2)*sin(theta4) + l4*cos(theta2)*cos(theta3)*cos(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)), cos(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)), - sin(theta5)*(cos(theta2)*sin(theta4) + cos(theta3)*cos(theta4)*sin(theta2)) - cos(theta5)*sin(theta2)*sin(theta3), 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)]
[ cos(theta6)*(cos(theta3)*sin(theta5) + cos(theta4)*cos(theta5)*sin(theta3)) - sin(theta3)*sin(theta4)*sin(theta6), - sin(theta6)*(cos(theta3)*sin(theta5) + cos(theta4)*cos(theta5)*sin(theta3)) - cos(theta6)*sin(theta3)*sin(theta4), cos(theta3)*cos(theta5) - cos(theta4)*sin(theta3)*sin(theta5), d*(cos(theta3)*cos(theta5) - cos(theta4)*sin(theta3)*sin(theta5)) + l3*sin(theta3) + l4*cos(theta4)*sin(theta3)]
[

0, 0, 0, 1]

>> b3*b4*b5*b6


ans =

[ - cos(theta6)*(sin(theta3)*sin(theta5) - cos(theta3)*cos(theta4)*cos(theta5)) - cos(theta3)*sin(theta4)*sin(theta6), sin(theta6)*(sin(theta3)*sin(theta5) - cos(theta3)*cos(theta4)*cos(theta5)) - cos(theta3)*cos(theta6)*sin(theta4), - cos(theta5)*sin(theta3) - cos(theta3)*cos(theta4)*sin(theta5), l3*cos(theta3) - d*(cos(theta5)*sin(theta3) + cos(theta3)*cos(theta4)*sin(theta5)) + l4*cos(theta3)*cos(theta4)]
[ cos(theta6)*(cos(theta3)*sin(theta5) + cos(theta4)*cos(theta5)*sin(theta3)) - sin(theta3)*sin(theta4)*sin(theta6), - sin(theta6)*(cos(theta3)*sin(theta5) + cos(theta4)*cos(theta5)*sin(theta3)) - cos(theta6)*sin(theta3)*sin(theta4), cos(theta3)*cos(theta5) - cos(theta4)*sin(theta3)*sin(theta5), d*(cos(theta3)*cos(theta5) - cos(theta4)*sin(theta3)*sin(theta5)) + l3*sin(theta3) + l4*cos(theta4)*sin(theta3)]
[ - cos(theta4)*sin(theta6) - cos(theta5)*cos(theta6)*sin(theta4), cos(theta5)*sin(theta4)*sin(theta6) - cos(theta4)*cos(theta6), sin(theta4)*sin(theta5), d*sin(theta4)*sin(theta5) - l4*sin(theta4)]
[ 0, 0, 0, 1]

>> b4*b5*b6


ans =

[ cos(theta4)*cos(theta5)*cos(theta6) - sin(theta4)*sin(theta6), - cos(theta6)*sin(theta4) - cos(theta4)*cos(theta5)*sin(theta6), -cos(theta4)*sin(theta5), l4*cos(theta4) - d*cos(theta4)*sin(theta5)]
[ cos(theta4)*sin(theta6) + cos(theta5)*cos(theta6)*sin(theta4), cos(theta4)*cos(theta6) - cos(theta5)*sin(theta4)*sin(theta6), -sin(theta4)*sin(theta5), l4*sin(theta4) - d*sin(theta4)*sin(theta5)]
[ cos(theta6)*sin(theta5), -sin(theta5)*sin(theta6), cos(theta5), d*cos(theta5)]
[ 0,

0, 0, 1]

>> b5*b6

ans =

[ cos(theta5)*cos(theta6), -cos(theta5)*sin(theta6), -sin(theta5), -d*sin(theta5)]
[ cos(theta6)*sin(theta5), -sin(theta5)*sin(theta6), cos(theta5), d*cos(theta5)]
[ -sin(theta6), -cos(theta6), 0, 0]
[ 0, 0, 0, 1]



%解逆解！！
[theta1 theta2 theta3 theta4 theta5 theta6]=solve('k1*t6=b2*b3*b4*b5*b6','k2*k1*t6=b3*b4*b5*b6','k3*k2*k1*t6=b4*b5*b6','k4*k3*k2*k1*t6=b5*b6','k5*k4*k3*k2*k1*t6=b6')

Warning: Explicit solution could not be found.
> In solve at 81

theta1 =

[ empty sym ]


theta2 =

[]


theta3 =

[]


theta4 =

[]


theta5 =

[]


theta6 =

[]


%空间轨迹
syms theta1 theta2 theta3 theta4 theta5 theta6 d l2 l3 l4 h
>> theta1=[-pi/2:pi/12:pi/2] ;theta2=[-pi/2:pi/12:pi/2]; theta3=0; theta4=0; theta5=0; theta6=0; d=150; l2=200; l3=200; l4=50; h=50;
>> x=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);