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

1、引Stanford Arm Forword Kinematics的基本结论:1aH%T1d.010-9090 0 -909000000CM fO o p p O O OStanford ScheinmanArm一.i|算的Jacobian结果0引Stanford Arm Forword Kinematics的基本结论:Stanford Scheinman Arm Jacobian二.各级矩阵(7XpJ=內2刖3z。乙107Y2- $卢23300-也 +勺也*卢2000一$23c2000一$10C】$2一g$4-怕0C10一*2*十C1C100吋4SiC2C4S5十eSAS5+ SiS2CsH

2、+今2+ClS2C5-1X0X CXC2CAS5-SSj$3+cs2s5XC/Vs十+ 气X-S?CjS$ -r c5c2q$100-J4(5(00 ()O)00)1q0-s、0000 0_1 00 00 10L0ClD Og一C*2 -s sdiD 00np、I =$心%25001 0SQ0000D 1.0001s3仃小 7沁$4Sy +C一$20_ 0001qcg一钠FS -帕中2件4+佔-伯+g$宀 佃2+也*5,54q0001 L XX-cgyqd-泌0q00 t -S0j000 q0!r =001T =00-10010e20001000001000d_0001一$60-1X00 0C

3、.Syd yd、星&3十qd、C3GCQ(CC$C6 SjSg) S?S$CG S、(S.C$CG+C4S6)S.C2(C.C5C.- $)- 宀2 C(S.d + C)5J(C4C5C$C2S5CCi(-G(C.CS6S.C6)+ 555S 1 - 5 (-S4C556- C4C6)$X(gS6+ 孔q)+ s?sa+ egg g)s：(cdQ)4Ss5|(C*2C 45 + 6C)十CjSSjS：C&Ss + c?c$三.矢咼叉积法对于移动关节,有: = Q,d=gM对于转动关节，有：X是中，在在基坐标系o中的表示。J=J1 J2J3 J4J5 J6四.验证直接微分法和矢量叉积法的结果一致

4、:1.J10-Iocld3s2-sld2-cd2-ss2d3JVi=ZxP =100-shl3s2 + chl2=一 sld2 + cls23000(I3c200An = 01_与直接求导比较：.2.J20JV2=Z2X2Z=0-clc4c5c6-s3s6-c4c5s6一 s4c6c4s50s5c6-s5s6c5 一3s4c5c6 + c4s6-s4c5s6 + c4c6s4$500001clC2-clS2-SI0cs2d3sc2-sls2-cld3ss2d3-s2-cl00c2d3 00cfmatlab的计算验证过程： syms c6 s6 c5 s5 c4 s4 T65=c6-l*s6 0

5、 0:0 0-1 0;s6 c600;000 1 T54=c5-l*s5 0 0;0 0 1 0:-l *s5 -l*c500;000 1 T43=c4-l*s4 0 0;s4 c40 0 ;0 0 1 0;000 1T63=T43*T54*T650 cl0 si 児-510带入:V2=Z2X2P =0-cl0一T63 =c4叱5*c6 s4*s6. -c4*c5*s6-s4*c6.c4*s5.0s4*c5*c6+c4*s6, -s4*c5*s6+c4*c6,s4*s5,0-s5*c6,s5*s6,c5, 0. 0, 0, syms d3T32=1 0 0 0:00-1 -l*d3:0 1 0

6、 0;0 0 0 1T62=T32*T6301T62 =c4*c5*c6-s4*s6, -c4*c5*s6-s4*c6,s5*c6,-s5*s6,c4*s5,Y5,0-d3s4*c5*c6+c4*s6, -s4*c5*s6+c4*c6,s4*s5,0 0. 0, 0, 1 syms cl c2 si s2 R20=cI*c2-l*cl*s2-l*sl;sl*c2-l*sl*s2cl;-l*s2 -1 *c2 0 p622=(0;-l*d3;0p622 =0-d30 p620=R20*p622p620 =cl*s2*d3sl*s2*d3c2*d3z2=0 0cl;0 0sl;-l*cl -l*s

7、l 0z2 =o,0,cl0,0,si-cl, -si,0v2=z2*p620v2 =cl*c2*d3sl*c2*d3-clA2*s2*d3-slA2*s2*d3与直接求导比较：.在matlab中用B=jacobian(f.v)方法直接求导获取雅可比矩阵 clear syms theta 1 d3 d2 theta2F=cos(thetal)*d3*sin(theta2)-sin(thetal)*d2;sin(thetal)*d3*sin(theta2)+cos(thetal)*d2;d3*cos(t hcta2)F =cos(theta 1 )*d3 *sin(theta2)-sin(the

8、ta 1 )*d2sin(theta 1 )*d3*sin(theta2)+cos(theta 1 )*d2d3*cos(theta2) syms theta4 theta5 theta6 v=theta 1 ;theta2;d3;theta4;theta5;theta6theta 1theta2d3theta4theta5theta6jacob=jacobian(Ev)jacob =I -sin(lhela 1 )*d3*sin(theta2-cos(thela 1 )*d2. cos(thctal )*d3*cos(lheta2), |cos(thclal)*d3*sin(lhcta2)-

9、sinthetal)*d2. sin(thetal )3|d3*cos(theta2h |0.-d3*sin(lhela2).直接求偏导： syms theta 1 d3 d2 theta2 theta4 theta5 theta6F l=cos(thcta 1 )*d3*sin(theta2 )-sin( theta l)*d2 dif(FLthetal)补充对教材雅可比矩阵逆矩阵的求解： syms 11 theta 1 12 theta2J=-11 *sin(thcta 1 )-12*sin(theta 1 +theta2)-12*sin(theta 1 +theta2);ll *cos(

10、thcta 1 )+12*cos(theta 1 +th eta2)12 *cos( theta 1 +theta2)J =-11 *sin(theta l)-12*sin(theta 1 +theta2),-12*sin(theta 1 +theta2)11 *cos( theta 1 )+12*cos(theta 1 +theta2),12*cos(theta l+theta2)inv(J)ans = cos(theta I +theta2)/ll /(cos(theta l+theta2)*sin(theta 1 )-sin(theta 1 +theta2)*cos(thetal)X-s

11、in(theta 1 +theta2)/l l/(cos(theta 1 +theta2)*sin(theta 1 )-sin(theta 1 +theta2)*cos(theta 1)co5(lhc【al严sin(lhcla2).sindhctal )*sin(lheta2),cos(heta2).0. 0, 00. 0. 00, 0. 0(Il*cos(theta 1 )+12*cos(theta 1 +theta2)/12/ll/(cos(theta 1 +theta2)*sin(thetal)-sin(theta 1 +theta2)*cos(thetal),(11 *sin(theta l)+12*sin(theta l+theta2)/12/l l/(cos(theta 1 +theta2)*sin(theta l)-sin(theta 1 +theta2)*cos(thetal)J11 =simplc(-cos(theta 1 +theta2)/l l/(cos(theta 1 +theta2)*sin(theta l)-sin