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

一 已知计算各级 T 矩阵 由公式 1 11111 1111 s0 0001 iii iiiiiiii i iiiiii ca s cc cssd T s sc sccd 以及各连杆坐标系之间的参数表 可得 665544 44543 654 6655 2211 32210 321 22 000000 0010001000 00000010 000100010001 10000000 001001 010000 00010001 cscscs sc TTT scsc cscs dd TTT sc 11 00 0010 0001 sc 与斯坦福大学开发课件比较发现其课件中 T21 计算有错 二 计算出各连杆坐标系到基坐标系 0 的变换矩阵 11 110 11 1 21 21121 1 21 211210 22 22 1 211 213 212 1 211 213 2120 3 222 000 000 00101 00010 000 00010 0 可知 可知 cs sc Tz c cc sss ds s cs scc dc Tz sc c csc sc d ss d s ccs ss d sc d T scc d 1 2 1 2 3 32 00010 可知 c s s s z c 1 241 41 2 41 41 21 2312 1 241 41 2 41 41 21 23120 4 242 4223 1 241 41 2 51 241 451 2 51 2 41 41 2312 1 241 450 5 0001 c c cs sc c ss cc sc s ds d s c cc ss c sc cs ss s dc d T s cs scc d c c cs sc s sc c cs s sc s cc c ss cc s ds d s c cc s cs T 1 2 51 241 451 2 51 2 41 41 2312 2 423 1 24 51 4 51 2 51 2312 1 24 51 4 51 2 51 23120 6 24 55 223 2 4 52 52 4 52 5 0001 12 34 56 00 s ss c cc s ss s cs c sc cs s dc d s c cc ss c sc cs sc d XXc c c ss s sc s cc s ds d XXs c c sc s ss s cs s dc d T XXs c sc cc d 01 与斯坦福开发课程课件比较 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 部分 356124 123456 1020405060 16263465666 12456 1020 16263 12456 0 000 0 vvvvvv JJJJJJ J JJJJJJ zpzpzzpzpzp zzzzz zpzpz zzzzz 1 计算 10 16 zp 为连杆 1 坐标系的 z 轴单位向量在基坐标系 0 中的描述 1 z 为连杆 1 坐标系原点到连杆 6 坐标系原点连线矢量 在基坐标系 0 中的描述 计 10 6 p 16 O O 算过程为 计算矩阵 T61 T61 的第四列即为 由于坐标系 1 相对于坐标系 0 有绕 Z 16 O O 轴的转动 故需要对其进行转换 转换方法为 为 T10 中旋转部分 0 116 O O R 0 1R 注 Matlab 中向量叉积方法 e cross a b T61 T21 T32 T43 T54 T65 计算出在坐标系 1 中的描述 16 O O 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 计算 20 26 zp 为连杆 2 坐标系的 z 轴单位向量在基坐标系 0 中的描述 2 z 为连杆 2 坐标系原点到连杆 6 坐标系原点连线矢量 在基坐标系 0 中的描述 20 6 p 26 O O 计算过程为 计算矩阵 P62 P62 的第四列即为 由于坐标系 2 相对于坐标系 0 有姿 26 O O 态变化 故需要对其进行转换 转换方法为 为 T20 中旋转部分 0 226 O O R 0 2R 注 Matlab 中向量叉积方法 e cross a b T62 T32 T43 T54 T65 计算出在坐标系 2 中的描述 26 O O 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 的速度贡献不能计算为 而应 30 36 zp 该为 Z3 的单位向量在基坐标系 0 中的表示 故由 T30 直接可得 Jv 第三列为 1 2 1 2 3 2 0 c s s s z c 4 由于坐标系 4 5 6 和坐标系 6 的坐标原点重合故对应 的计算结果均为 0 于是可得 6 0 66 ii q q ii Oiii vzOOzp 356124 123456 121 231 231 2 121 231 231 2 232 11 2 41 41 2 41 41 24 51 4 51 2 5 11 2 41 41 2 41 41 24 5 000 000 0000 00 00 vvvvvv JJJJJJ J JJJJJJ c ds s dc c dc s s dc s ds c ds s s dc sc c ss cc c ss cc c c ss s sc s c cs c sc cs c sc cs c c sc 1 4 51 2 5 2 42 424 55 2 100 s ss s c s ss ss c sc c 5 用直接求导的方法验证上面 Jv 的计算的正确性 在 matlab 中用 B jacobian f v 方法直接求导获取雅可比矩阵 1 举例 B jacobian f v f v 为两个列向量 分别为 m 维和 n 维 例题 已知求其 Jacobian 矩阵 22 1 cos sin 1 2 sin cos mcgcgcgcg cgcgcg fTKGww TwwT fTww Tw 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 方法方法计算斯坦福机械手雅可比矩阵 clear syms theta1 d3 d2 theta2 theta4 theta5 theta6 F cos theta1 d3 sin theta2 sin theta1 d2 s