matlab驗證斯坦福機械手雅可比矩陣

1、引Stanford Arm Forword Kinematics 的基本結論：一計算的Jacobian結果二各級矩陣三矢量叉積法J=J1 J2 J3 J4 J5 J6四驗證直接微分法和矢量叉積法的結果一致：1.J1與直接求導比較：.2.J2matlab的計算驗證過程：>> syms c6 s6 c5 s5 c4 s4>> T65=c6 -1*s6 0 0;0 0 -1 0;s6 c6 0 0;0 0 0 1>> T54=c5 -1*s5 0 0;0 0 1 0;-1*s5 -1*c5 0 0;0 0 0 1>> T43=c4 -1*s4 0 0;

2、s4 c4 0 0 ;0 0 1 0;0 0 0 1>> T63=T43*T54*T65T63 = c4*c5*c6-s4*s6, -c4*c5*s6-s4*c6, c4*s5, 0 s4*c5*c6+c4*s6, -s4*c5*s6+c4*c6, s4*s5, 0 -s5*c6, s5*s6, c5, 0 0, 0, 0, 1>> syms d3>> T32=1 0 0 0; 0 0 -1 -1*d3;0 1 0 0;0 0 0 1>> T62=T32*T63T62 = c4*c5*c6-s4*s6, -c4*c5*s6-s4*c6, c4*s

3、5, 0 s5*c6, -s5*s6, -c5, -d3 s4*c5*c6+c4*s6, -s4*c5*s6+c4*c6, s4*s5, 0 0, 0, 0, 1>> syms c1 c2 s1 s2>> R20=c1*c2 -1*c1*s2 -1*s1;s1*c2 -1*s1*s2 c1;-1*s2 -1*c2 0>> p622=0;-1*d3;0p622 = 0 -d3 0>> p620=R20*p622p620 = c1*s2*d3 s1*s2*d3c2*d3>> z2=0 0 c1;0 0 s1;-1*c1 -1*s1 0z2

4、 = 0, 0, c1 0, 0, s1 -c1, -s1, 0>> v2=z2*p620v2 = c1*c2*d3 s1*c2*d3 -c12*s2*d3-s12*s2*d3與直接求導比較：.在matlab中用B=jacobian(f,v)方法直接求導獲取雅可比矩陣>> clear>> syms theta1 d3 d2 theta2>> F=cos(theta1)*d3*sin(theta2)-sin(theta1)*d2;sin(theta1)*d3*sin(theta2)+cos(theta1)*d2;d3*cos(theta2)F =

5、cos(theta1)*d3*sin(theta2)-sin(theta1)*d2 sin(theta1)*d3*sin(theta2)+cos(theta1)*d2 d3*cos(theta2) >> syms theta4 theta5 theta6>> v=theta1;theta2;d3;theta4;theta5;theta6v = theta1 theta2 d3 theta4 theta5 theta6>> jacob=jacobian(F,v) 直接求偏導：>> syms theta1 d3 d2 theta2 theta4 th

6、eta5 theta6>>F1=cos(theta1)*d3*sin(theta2)-sin(theta1)*d2>> dif(F1,theta1)補充對教材雅可比矩陣逆矩陣的求解：>> syms l1 theta1 l2 theta2>>J=-l1*sin(theta1)-l2*sin(theta1+theta2)-l2*sin(theta1+theta2);l1*cos(theta1)+l2*cos(theta1+theta2) l2*cos(theta1+theta2)J = -l1*sin(theta1)-l2*sin(theta1+th

7、eta2), -l2*sin(theta1+theta2) l1*cos(theta1)+l2*cos(theta1+theta2), l2*cos(theta1+theta2)>> inv(J) ans = -cos(theta1+theta2)/l1/(cos(theta1+theta2)*sin(theta1)-sin(theta1+theta2)*cos(theta1), -sin(theta1+theta2)/l1/(cos(theta1+theta2)*sin(theta1)-sin(theta1+theta2)*cos(theta1) (l1*cos(theta1)+l2*cos(theta1+theta2)/l2/l1/(cos(theta1+theta2)*sin(theta1)-sin(theta1+theta2)*cos(theta1), (l1*sin(theta1)+l2*sin(theta1+theta2)/l2/l1/(cos(theta1+theta2)*sin(theta1)-sin(theta1+theta2)*cos(theta1) >> J11=simple(-co