1、第三章 工业机器人的运动学-3,主要内容,数学基础齐次坐标变换机器人运动学方程的建立（正运动学）机器人逆运动学分析（逆运动学）,三、逆运动学方程 （ Inverse Kinematic Equations ）,3.1 引言 3.2 逆运动学方程的解 3.3 斯坦福机械手的逆运动学解 3.4 欧拉变换的逆运动学解 3.5 RPY变换的逆运动学解 3.6 球坐标变换的逆运动学解 3.7 本章小结,3.1 引言 (Introduction),所谓逆运动学方程的解，就是已知机械手直角坐标空间的位姿（pose）T6，求出各节变量n or dn 。 T6 = A1 A2 A3 A4 A5 A6 （3.1）
2、 逆运动学方程解的步骤如下： （1）根据机械手关节坐标设置确定AnAn为关节坐标的齐次坐标变换，由关节变量和参数确定。关节变量和参数有：an连杆长度; n连杆扭转角;dn相邻两连杆的距离; n相邻两连杆的夹角。 对于旋转关节n为关节变量，而对于滑动关节dn为关节变量。其余为连杆参数，由机械手的几何尺寸和组合形态决定。,（2） 根据任务确定机械手的位姿T6T6为机械手末端在直角坐标系（参考坐标或基坐标）中的位姿，由任务确定，即式（ 2.37 ）给出的表达式T6 = Z-1 X E-1确定。它是由三个平移分量构成的平移矢量P（确定空间位置）和三个旋转矢量n，o，a（确定姿态）组成的齐次变换矩阵描述
3、。（3）由T6和An（n1，2，6）和式（4.1）求出相应的关节变量n 或 dn。,3.2 逆运动学方程的解（Solving inverse kinematic equations）,根据式（3.1） T6 = A1 A2 A3 A4 A5 A6 分别用An（n1，2，5）的逆左乘式（3.1）有A1-1 T6 = 1T6 ( 1T6 = A2 A3 A4 A5 A6 ) （3.2）A2-1 A1-1 T6 = 2T6 ( 2T6 = A3 A4 A5 A6 ) （3.3）A3-1A2-1 A1-1 T6 = 3T6 ( 3T6 = A4 A5 A6 ) （3.4）A4-1 A3-1A2-1 A
4、1-1 T6 = 4T6 ( 4T6 = A5 A6 ) （3.5）A5-1 A4-1 A3-1A2-1 A1-1 T6 = 5T6 ( 5T6 = A6 ) （3.6）根据上述五个矩阵方程对应元素相等，可得到若干个可解的代数方程，便可求出关节变量n或 dn。,3.3 斯坦福机械手的逆运动学解（ Inverse solution of Stanford manipulator） 在第三章我们推导出 Stanford Manipulator 的运动方程和各关节齐次变换式。下面应用式（3.2）（3.6）进行求解：,这里f11 = C1 xS1 y (3.10)f12 = - z (3.11)f13
5、 = - S1 xC1 y (3.12) 其中x = nx ox ax px T， y = ny oy ay py T， z = nz oz az pz T 由前节得到的斯坦福机械手运动学方程式（2.48）为C2( C4C5C6 - S4S6 ) - S2S5C6 -C2( C4C5S6 + S4C6 )+ S2S5S6 S2( C4C5C6 - S4S6 ) + C2S5C6 -S2( C4C5 S6+ S4C6 )- C2S5S6 1T6 = S4C5C6 + C4C6 -S4C5S6 + C4C6 0 0C2C4S5 + S2C5 S2d3 S2C4S5 - C2C5 -C2d3S4S5
6、 d2 （3.13）0 1,比较式（3.9）和式（3.13）矩阵中的第三行第四列元素相等得到f13（p）= d2 （3.14） 或 - S1 pxC1 py = d2 （3.15） 令 px = r cos （3.16）py = r sin （3.17） 其中（3.18）（3.19）将式（3.16）和式（3.17）代入式（3.15）有sincon1consin1 d2/r （ 0 d2/r 1 ） （3.20） 由式（3.20）可得sin(1) d2/r （0 1 ） （3.21）con(1) （3.22）这里号表示机械手是右肩结构（）还是左肩结构（）。,由式（3.21）、（3.22）和（3.
7、18）可得到第一个关节变量1的值（3.23）根据同样的方法，利用式（3.9）和式（3.13）矩阵元素相等建立的相关的方程 组，可得到其它各关节变量如下：,（3.24）,（3.25）,（3.26）,（3.27）,（3.28）,注意： 在求解关节变量过程中如出现反正切函数的分子和分母太小，则计算结果误差会很大，此时应重新选择矩阵元素建立新的方程组再进行计算，直到获得满意的结果为止。同样，如果计算结果超出了机械手关节的运动范围，也要重新计算，直到符合机械手关节的运动范围。 由于机械手各关节变量的相互耦合，后面计算的关节变量与前面的关节变量有关，因此当前面关节变量的计算结果发生变化时，后面关节变量计算
8、的结果也会发生变化，所以逆运动方程的解不是唯一的，我们应该根据机械手的组合形态和各关节的运动范围，经过多次反覆计算，从中选择一组合理解。由此可见，求解机械手的逆运动方程是一个十分复杂的过程。,3.4 欧拉变换的逆运动学解 （Inverse solution of Euler Angles ）,由前节知欧拉变换为 Euler (, ,) Rot (z, ) Rot (y, ) Rot (z,) （3.29） 我们用T来表示欧拉变换的结果，即 T Euler (, ,) （3.30） 或 T Rot (z, ) Rot (y, ) Rot (z,) （3.31） 其中（3.32）,（3.33）,比
9、较式（3.32）和式（3.33）有（3.34）（3.35）（3.36）（3.37）（3.38）（3.39）（3.40）（3.41）（3.42）,由式（3.42）可解出角（3.43） 由式（3.40）和式（3.43）可解出角（3.44） 由式（3.36）和式（3.43）可解出角（3.45）,这里需要指出的是，在我们采用式（3.43）式（3.45）来计算、时都是采用反余弦函数，而且式（3.43）和式（3.45）的分母为sin，这会带来如下问题：1）由于绝对值相同的正负角度的余弦相等，如coscos（-），因此不能确定反余弦的结果是在那个象限；2）当sin接近于0时，由式（3.43）和式（3.45）
10、所求出的角度和是不精确的；3）当0或180时，式（3.43）和式（3.45）无数值解。为此，我们必须寻求更为合理的求解方法。由三角函数的知识我们知道，反正切函数tan1（x / y）所在的象限空间可由自变量的分子和分母的符号确定（如图3.1所示），因此如果我们得到欧拉角的正切表达式，就不难确定欧拉角所在的象限。为此，我们采用前节的方法，用Rot (z, )1左乘式（3.31）有 Rot1(z,) T Rot (y, ) Rot (z, ) （3.46）,即（3.47）将上式写成如下形式（3.48）式中（3.49）（3.50）（3.51） 同样，上面三个式子中的x、y、z分别表示n、o、a、p矢
11、量的各个分量，如（3.52）,比较式（3.48）等号两边矩阵的第2行第3列元素可知（3.63） 即（3.54） 由此可得到（3.55） 或（3.56） 结果得到（3.57） 或 （3.58）,上述结果相差180，可根据实际系统的组合形态从中选择一个合理解。如果ay和ax都为0，则式（3.57）和式（3.58）无定义，这是一种退化现象，此时值可任意设置，如0。由于角已求出，比较式（3.48）等号两边矩阵第1行第3列和第3行第3列元素相等有（3.59）（3.60）或 （3.61）（3.62）由此可得（3.63）,同样比较式（3.48）等号两边矩阵的第2行第1列和第2行第2列元素可知（3.64）（3
12、.65） 或 （3.66）（3.67） 由此可得（3.68）至此，我们求出了欧拉变换的逆运动学解。,3.5 RPY变换的逆运动学解（Inverse solution of RPY）,第三章介绍的摇摆、俯仰和偏转( RPY )变换的表达式如下 T = RPY ( , ,) Rot ( z, ) Rot ( y, ) Rot ( x, ) （3.69） 用Rot1( z, )左乘上式得到 Rot1( z, ) T Rot ( y, ) Rot ( x, ) （3.70） 将上式写成式（3.48）的形式（3.71）式中（3.72）（3.73）（3.74）,由式（3.71）等号两边矩阵的第2行第1列元
13、素相等有（3.75） 由此得到（3.76） 或 （3.77） 角已求出，根据式（3.71）等号两边矩阵的第3行第1列和第1行第1列元素相等有（3.78）（3.79） 由此可得（3.80）,进一步比较式（3.71）等号两边矩阵元素，由第2行第3列和第2行第2列元素相等有（3.81）（3.82）由此可得 （3.83）至此，我们求出了RPY的逆运动学解。,3.6 球坐标变换的逆运动学解 （Inverse solution of Spherical Coordinates ）,前节介绍的球坐标变换的表达式如下 T = Sph (, , ) = Rot ( z, ) Rot (y, ) Trans( 0
14、, 0, ) （3.84） 用Rot1(z,)左乘上式得到 Rot1( z, ) T = Rot ( y, ) Trans ( 0, 0, ) （3.85） 将上列矩阵方程的第4列元素写出有（3.86）由上式第2行元素相等有（3.87）,由式（3.87）可得到（3.88） 或（3.89） 由式（3.86）第1行和第3行元素相等有（3.90）（3.91） 由此可得 （3.92）,为了获得平移量，我们用Rot1( y, )左乘式（3.85） Rot1( y, ) Rot1( z, ) T = Trans ( 0, 0, ) （3.93） 上式第4列元素是（3.94）由上式第3行元素相等得到（3.95） 至此，我们求出了球坐标变换的逆运动学解。,3.7 本章小结（Summary）,解逆运动方程是应用齐次坐标变换原理，从机器人末端执行器的直角坐标空间到关节坐标的变换（T6 n、dn），它是求解正运动方程的逆过程（n、dn T6），是机器人运动学的重要内容，是机器人控制的依据。 要注意的是正运动方程的解是唯一解，而逆运动方程的解不是唯一解，因此选择合理解是解逆运动方程的一项重要内容。,
