机器人雅可比矩阵

1、机器人雅可比矩阵第1页，共29页，2022年，5月20日，2点20分，星期四5.1 引言（Introduction） 微分变换在机器人视觉、动力学和机器人控制（如力控、刚度控制、阻抗控制、顺应控制等）中十分重要。例如当摄像机或其它传感装置检测到机器人末端执行器的位置和方向的微小变化时，需要将该微小变化从摄像机或其它传感装置坐标转换到基坐标或参考坐标系。在机器人刚度控制中，需要获得在控制坐标系中力与位置的微分变换。又如将直角坐标的微分变换转化为关节坐标的微分变换，还有在下一章介绍的机器人动力学问题时，也会用到微分变换。本章将介绍微分变换的基本原理和方法，包括微分平移、微分旋转、坐标系之间的微分变

2、换、雅可比矩阵和逆雅可比矩阵及其应用。 2022/9/202第2页，共29页，2022年，5月20日，2点20分，星期四5.2 微分矩阵（Derivative Matrixes） 给出一个44的矩阵A （5.1）矩阵A的微分就是对矩阵A中的每一个元素对自变量x的微分，结果如下 （5.2）2022/9/203第3页，共29页，2022年，5月20日，2点20分，星期四5.3 微分平移和旋转变换 ( Differential Translation and Rotation ) 微分平移和旋转变换可以是针对基坐标或参考坐标系，也可以是针对某个指定的坐标系进行。例如对于一个变换矩阵T，它对基坐标的微

3、分变换可表示为 （5.3） 式中是在基坐标的x，y，z轴向上分别平移dx，dy，dz；和绕基坐标的向量k旋转d角。由此可得到 （5.4） 如果上述微分变换不是针对基坐标而是针对坐标系T，那么微分变换的结果可表示为 （5.5） 此时，式中 是在T坐标的x，y，z轴向上分别平移dx，dy，dz； 是绕T坐标的向量k旋转d角。由此可得到 （5.6） 2022/9/204第4页，共29页，2022年，5月20日，2点20分，星期四 我们用符号 来表示式（5.4）和式（5.6）中的 并将它称为微分变换算子（5.6） 这样式（5.4）和式（5.6）就可写成如下形式 （5.7） 和 （5.8） 式（5.7）

4、中的微分变换算子 是针对基坐标的，而式（5.8）中的微分变换算子 则是针对T坐标的。 在第二章我们给出了平移和一般性旋转变换的齐次变换矩阵表达式，平移变换矩阵是 1 0 0 a 0 1 0 bTrans( a, b, c ) = 0 0 1 c （5.9） 0 0 0 12022/9/205第5页，共29页，2022年，5月20日，2点20分，星期四当平移向量是微分向量ddxi+dyj+dzk时，微分平移矩阵为 1 0 0 dx 0 1 0 dyTrans( d ) = 0 0 1 dz （5.10） 0 0 0 1一般性旋转变换的变换矩阵是 kxkxvers + cos kykxvers -

5、 kzsin kzkxvers + kysin 0 kxkyvers + kzsin kykyvers + cos kzkyvers - kxsin 0Rot( k,) = kxkzvers - kysin kykzvers + kxsin kzkzvers + cos 0 (5.11) 0 0 0 1当进行微分旋转变换时，旋转角d极小，此时有如下关系2022/9/206第6页，共29页，2022年，5月20日，2点20分，星期四将上述关系代入式（5.11）可得 1 - kzd kyd 0 kzd 1 - kxd 0 Rot( k, d) = - kyd kxd 1 0 (5.12) 0 0

6、0 1由式（5.6）可得 (5.13)2022/9/207第7页，共29页，2022年，5月20日，2点20分，星期四5.4 微分旋转 (Differential Rotations) 式（5.13）给出的微分变换算子 是基于微分旋转角d的微分平移和旋转变换表达式，下面讨论绕坐标轴x、y、z旋转x、y、z的微分变换。 第二章给出的绕坐标轴x、y、z旋转的变换矩阵分别为 （5.14） （5.15） （5.16）2022/9/208第8页，共29页，2022年，5月20日，2点20分，星期四在微分变换的情况下，sind，con1，上面三个式子变为 （5.17） （5.18） （5.19）由此可得到

7、 （5.20）2022/9/209第9页，共29页，2022年，5月20日，2点20分，星期四 比较式（5.12）和式（5.20）可知，绕任意向量k旋转d的微分旋转与绕x、y、z轴分别旋转 的结果相同，即 （5.21） 由此可得到绕坐标轴x、y、z旋转x、y、z的微分变换算子为 （5.22） 微分变换算子中的元素由微分平移向量d和微分旋转向量的各个分量组成，即 （5.23） （5.24） 将上述二个向量组合构成一个微分运动矢量D （5.25） 这样，我们就可根据式（5.25）给出的微分运动矢量D直接得到微分变换算子 ，或基于T坐标的微分运动矢量 的微分变换算子 。2022/9/2010第10页

8、，共29页，2022年，5月20日，2点20分，星期四【例5.1】已知坐标A的变换矩阵为 当用微分平移矢量d = 1i + 0j + 0.5k和微分旋转矢量 0i + 0.1j + 0k对坐标A 进行变换时，求出微分变换的结果dA。解：首先，由式（5.22）求出微分变换算子 由式（5.7）可得 即微分变换结果如图5.1所示。xyzzAyA+dAx图5.1 坐标A的微分变换2022/9/2011第11页，共29页，2022年，5月20日，2点20分，星期四5.5 坐标系之间的微分变换 (Transforming Differential Changes between Coordinate Fr

9、ames) 上节讨论了基于基坐标或某个指定坐标的微分变换，本节继续讨论坐标系之间的微分变换，也就是已知微分变换算子 ，如何求出T坐标的微分变换算子 。由式（5.7）和（5.8）可知 （5.26） 则为 （5.27） 上式是一个重要的表达式，它描述了坐标系之间的微分变换关系。下面我们用微分平移矢量d和微分旋转矢量 来推导 的表达式。 已知变换矩阵T为2022/9/2012第12页，共29页，2022年，5月20日，2点20分，星期四我们用矢量的叉乘来得到式（5.27）等号右边二项的乘积 （5.29）式中d和 分别是微分平移和微分旋转矢量。用 左乘式（5.29）可得 （5.30）上式矩阵元素都具有

10、如下矢量三重积形式根据矢量三重积的性质有 （5.31）2022/9/2013第13页，共29页，2022年，5月20日，2点20分，星期四同时，三重积中只要有二个矢量是相同的，其结果为零。如 （5.32）根据上述性质，式（5.30）可写成 （5.33）对于正交矢量有 （5.34）这样，式（5.33）可重写成 （5.35）2022/9/2014第14页，共29页，2022年，5月20日，2点20分，星期四 上式可进一步简化为 （5.36） 比较式（5.35）和式（5.36）的矩阵元素可得 （5.37） （5.38） 在式（5.37）和式（5.38）中，n、o、a和p是微分坐标变换矩阵T的旋转和平

11、移矢量， 和 是对应坐标T的微分平移和旋转矢量。2022/9/2015第15页，共29页，2022年，5月20日，2点20分，星期四式（5.37）和式（5.38）也可用66的矩阵形式表示如下 （5.39）将上式写成式（5.36）和式（5.37）的形式如下 （5.40） （5.41）式（5.40）和式（5.41）是后续内容中要经常用到的重要结果。2022/9/2016第16页，共29页，2022年，5月20日，2点20分，星期四【例5.2】给出与例5.1相同的坐标的变换矩阵、微分平移矢量和微分旋转矢量如下： d = 1i + 0j + 0.5k 0i + 0.1j + 0k 试求出坐标A上的等效

12、微分变换dA。解：由坐标变换矩阵A可得到相应的旋转与平移矢量 由此可求出 根据式（5.40）和式（5.41）得到2022/9/2017第17页，共29页，2022年，5月20日，2点20分，星期四用上述结果来验证坐标A上的等效微分变换dA，由式（5.8）有由已求出的 、 和式（5.36）可得到则上述结果与例5.1相同。2022/9/2018第18页，共29页，2022年，5月20日，2点20分，星期四5. 6 机械手的微分变换方程雅可比方程 （The Manipulator Jacobian） 在第三章我们介绍过，机械手的运动学方程由它的末端相对于基坐标的齐次变换矩阵T6表示，即T6 = A1

13、 A2 A3 A4 A5 A6 （5.42） 其中每一个关节变换矩阵Ai描述了该关节坐标相对于前一个关节坐标的变换关系，关节变量用qi表示，如果是旋转关节，关节变量是i，它是绕前一个关节坐标z轴的旋转角度；如果是滑动关节，关节变量是di，它是沿前一个关节坐标z轴滑动的距离。同样，当我们讨论机械手的微分变换方程时，首先定义微分关节变量为dqi，如果是旋转关节，则为di，如果是滑动关节，则为ddi。2022/9/2019第19页，共29页，2022年，5月20日，2点20分，星期四 机械手第i个关节的微分变换引起第6个连杆末端（即机械手末端）的微分变换dT6可由下式表示： （5.43） 则 （5.

14、44） 由式（5.27）可得到机械手末端的微分变换算子 （5.45） 其中 （5.46） 如果关节i是旋转关节，则di = 0，式（5.40）和式（5.41）变为 （5.47）（5.48）2022/9/2020第20页，共29页，2022年，5月20日，2点20分，星期四 当 ，为单位微分旋转矢量时，式（5.47）和（5.48）可进一步简化为 （5.49） （5.50） 如果关节i是棱形滑动关节，则i0，di = 0i + 0j + 1k，式（5.40）和式（5.41）变为 （5.51） （5.52） 机械手末端坐标T6的微分变换是所有6个关节微分变量的函数，可用66的矩阵表示，矩阵元素由6个

15、关节的微分平移和微分旋转矢量构成，该矩阵称为雅可比矩阵。它的每一列元素为对应关节的微分平移和微分旋转矢量。应用雅可比矩阵的机械手微分变换方程雅可比方程如下： （5.53）2022/9/2021第21页，共29页，2022年，5月20日，2点20分，星期四5.7 雅可比逆矩阵（The Inverse Jacobian） 当微分变换是由直角坐标空间向关节坐标空间进行时，由式（5.53）可得到 （5.72） 上式等号右边矩阵是雅可比逆矩阵。显然，用符号运算来得到雅可比逆阵是很困难的，因为微分变换要进行大量算术运算，同时当机械手出现退化时，其结果会出错。 为此，我们采用第四章介绍的根据T6的值计算关节

16、坐标值的方法和步骤来计算微分关节坐标值。将关节坐标的微分变换表示为dT6中各元素的函数，然后求出各关节的微分变换值。该方法相对比较简单，而且在机械手出现退化时，将相应关节的微分变换值设置为零，这就不会影响后续关节的计算结果。在后面的讨论中，我们假设机械手的符号解存在，而且关节变量的正弦和余弦值已知。2022/9/2022第22页，共29页，2022年，5月20日，2点20分，星期四 为了计算dT6,我们首先根据式（5.37）和式（5.38）对T6进行微分变换得到微分平移矢量 和微分旋转矢量 ，然后根据式（5.22）求出 ，最后根据式（5.8）得到dT6。 下面通过对第四章介绍的斯坦福机械手逆运

17、动学解的微分变换来说明上述方法的具体步骤。 由第四章式（4.15）有S1 pxC1 py = d2 （5.73） 对式（5.73）求导可直接得到第一个关节变量1的微分 （5.74） 对于正切函数 （5.75） 其微分公式为 （5.76）2022/9/2023第23页，共29页，2022年，5月20日，2点20分，星期四由第四章式（4.24）和式（4.25）有 （5.77） （5.78）对式（5.77）和式（5.78）求微分得到 （5.79） （5.80）由公式（5.76）可得到第二个关节变量2的微分 （5.81）将式（5.77）代入第四章的式（4.31）有 （5.82）对式（5.82）进行微分

18、可直接得到第三个滑动关节变量d3的微分 （5.83）2022/9/2024第24页，共29页，2022年，5月20日，2点20分，星期四 由第四章式（4.38）和式（4.39）有NS4 =S1axC1ay （5.84） NC4 = C2D41S2az （5.85）其中 D41 = C1axS1ay （5.86）对式（5.84）式（5.86）进行微分得到 （5.87） （5.88） （5.89）由式（5.76）可得到第四个关节变量4的微分d4。在计算第五个关节变量微分时，为了简化计算，我们可将式（5.76）简化为 （5.90） 由第四章式（4.42）和式（4.43）有S5 = C4NC4+ NS

19、4 （5.91）C5 = S2D41 + C2az （5.92）2022/9/2025第25页，共29页，2022年，5月20日，2点20分，星期四对式（5.91）和式（5.92）进行微分得到 （5.93） （5.94）由式（5.90）可得到第五个关节变量5的微分d5。 最后，我们由第四章式（4.49）和式（4.50）有S6 =C5N61S5N612 （5.95）C6 =S4N611 + C4N6112 （5.96）其中N6111 = C1ox + S1oy （5.97）dN6111 = dC1ox + C1dox + dS1oy + S1doy （5.98）N6112 =S1ox + C1o

20、y （5.99）dN6112 =dS1oxS1dox + dC1oy + C1doy （5.100）2022/9/2026第26页，共29页，2022年，5月20日，2点20分，星期四N611 = C2N6111S2oz （5.101）dN611 = dC2N6111 + C2dN6111dS2ozS2doz （5.102）N612 =S2N6111C2oz （5.103）dN612 =dS2N6111 + S2dN6111dC2ozC2doz （5.104）N61 =C4N611 + S4N6112 （5.105）dN61 =dC4N611 + C4dN611 + dS4N6112S4dN6112 （5.106）对式（5.95）和式（5.96）进行微分得到