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

一．已知计算各级T矩阵 由公式： 以及各连杆坐标系之间的参数表，可得： 与斯坦福大学开发课件比较发现其课件中T21计算有错： 二．计算出各连杆坐标系到基坐标系0的变换矩阵： 与斯坦福开发课程课件比较 Matlab计算过程如下： clear syms c1 s1 c2 s2 c3 s3 c4 s4 c5 s5 c6 s6 d1 d2 d3 d4 d5 d6 a1 a2 a3 a4 a5 a6 T10=[c1 -s1 0 0;s1 c1 0 0;0 0 1 0;0 0 0 1] T21=[c2 -s2 0 0;0 0 1 d2;-s2 -c2 0 0;0 0 0 1] T32=[1 0 0 0;0 0 -1 -d3;0 1 0 0;0 0 0 1] T43=[c4 -s4 0 0;s4 c4 0 0;0 0 1 0;0 0 0 1] T54=[c5 -s5 0 0;0 0 1 0; -s5 -c5 0 0;0 0 0 1] T65=[c6 -s6 0 0;0 0 -1 0;s6 c6 0 0;0 0 0 1] T20=T10*T21 T30=T20*T32 T40=T30*T43 T50=T40*T54 T60=T50*T65 %可以用simplify函数简化，如： T60=simplify(T60) 三．用速度矢量合成的方法计算雅可比矩阵Jv部分： 1.计算 为连杆1坐标系的z轴单位向量在基坐标系0中的描述； 为连杆1坐标系原点到连杆6坐标系原点连线矢量，在基坐标系0中的描述，计算过程为：计算矩阵T61，T61的第四列即为，由于坐标系1相对于坐标系0有绕Z轴的转动，故需要对其进行转换，转换方法为；，为T10中旋转部分 注：Matlab中向量叉积方法：e=cross(a,b) T61=T21*T32*T43*T54*T65 %计算出在坐标系1中的描述 P161=[s2*d3;d2;c2*d3] Rot10=[c1 -s1 0;s1 c1 0;0 0 1] %由T10知道旋转部分变换3*3矩阵 P160= Rot10* P161 % 与P60最后一列比较 z1=[0;0;1] e=cross(z1,P160) %可得到Jv第一列： e =[ -s1*s2*d3-c1*d2; c1*s2*d3-s1*d2;0] 2.计算 为连杆2坐标系的z轴单位向量在基坐标系0中的描述； 为连杆2坐标系原点到连杆6坐标系原点连线矢量，在基坐标系0中的描述，计算过程为：计算矩阵P62，P62的第四列即为，由于坐标系2相对于坐标系0有姿态变化，故需要对其进行转换，转换方法为；，为T20中旋转部分 注：Matlab中向量叉积方法：e=cross(a,b) T62= T32*T43*T54*T65 %计算出在坐标系2中的描述 P262=[0;-d3;0] Rot20=[c1*c2 -c1*s2 -s1;s1*c2 -s1*s2 c1;-s2 -c2 0] %由T20知旋转部分变换3*3矩阵 P260= Rot20* P262 z2=[-s1;c1;0] e=cross(z2, P260) %可得到Jv第一列： e =[c1*c2*d3; s1*c2*d3; -s1^2*s2*d3-c1^2*s2*d3] 3.由于连杆3坐标系为移动坐标系，故起对连杆6的速度贡献不能计算为，而应该为Z3的单位向量在基坐标系0中的表示；故由T30直接可得Jv第三列为： 4.由于坐标系4、5、6和坐标系6的坐标原点重合故对应的计算结果均为0 ，于是可得 5.用直接求导的方法验证上面Jv的计算的正确性： 在matlab中用B=jacobian(f,v)方法直接求导获取雅可比矩阵 1.举例： B=jacobian(f,v) f,v为两个列向量，分别为m维和n维 例题：已知求其Jacobian矩阵 syms tao T wcg K Gm f=[K*Gm*(cos(wcg*tao)-wcg*T*sin(wcg*tao))+1+wcg^2*T^2 ;sin(wcg*tao)+wcg*T*cos(wcg*tao)] v=[tao,T] jacob=jacobian(f,v) jacob = [K*Gm*(-sin(wcg*tao)*wcg-wcg^2*T*cos(wcg*tao)), -K*Gm*sin(wcg*tao)*wcg+2*wcg^2*T] [cos(wcg*tao)*wcg-wcg^2*T*sin(wcg*tao), cos(wcg*tao)*wcg] 2. jacobian方法计算斯坦福机械手雅可比矩阵