matlab验证斯坦福机械手雅可比矩阵.doc

引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;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*s5, 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 = 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 = 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 theta5 theta6F1=cos(theta1)*d3*sin(theta2)-sin(theta1)*d2 dif(F1,theta1)补充对教材雅可比矩阵逆矩阵的求解： syms l1 theta1 l2 theta2J=-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+theta2), -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(-cos(theta1+th