1、第三章第三章 运动学方程运动学方程 Chapter Kinematic Equations3.1 3.1 引言引言 3.8 T3.8 T6 6的说明的说明 3.23.2 姿态描述姿态描述 3.9 3.9 各种各种A A矩阵的说明矩阵的说明 3.33.3 欧拉角欧拉角 3.10 3.10 根据根据A A矩阵来确定矩阵来确定T T6 6 3.4 3.4 摇摆、俯仰和偏转摇摆、俯仰和偏转 3.11 3.11 斯坦福机械手的运动方程斯坦福机械手的运动方程3.5 3.5 位置的确定位置的确定 3.12 3.12 肘机械手的运动方程肘机械手的运动方程3.6 3.6 圆柱坐标圆柱坐标 3.13 3.13 小
2、结小结 3.7 3.7 球坐标球坐标3.1 引言引言(Introduction)本章，我们采用齐次变换来描述在各种坐标系中机械手的位置与方向。首先介绍各种正交坐标系的齐次变换。然后介绍在非正交关节坐标系中描述机械手末端的齐次变换。注意，对任何数目关节的各种机械手均可以这样进行。描述一个连杆与下一个连杆之间关系的齐次变换称A矩阵。A矩阵是描述连杆坐标系之间的相对平移和旋转的齐次变换。连续变换的若干A矩阵的积称为T矩阵，对于一个六连杆（六自由度）机械手有 T6=A1 A2 A3 A4 A5 A6 （3.1）六连杆的机械手有六个自由度，其中三个自由度用来确定位置，三个自由度用来确定方向。表示机械手在
3、基坐标中的位置与方向。则变换矩阵有下列元素 nx ox ax px ny oy ay py T6 =nz oz az pz （3.2）0 0 0 1 如图3.1所示，机器人的末端执行器（手爪）的姿态（方向）由 n、o、a 三个旋转矢量描述，其坐标位置由平移矢量 p 描述，这就构成了式（3.2）中的变换矩阵 T。由于 n、o、a 三个旋转矢量是正交矢量，所以有n=oa图3.1 末端执行器的描述 3.2 姿态描述姿态描述(Specification of Orientation)对式（3.2）中16个元素一一赋值就可确定T6。假定机械手可以到达要求的位置，而单位旋转矢量o和a正交，即 oo 1 （
4、3.3）aa 1 （3.4）oa 0 （3.5）a形成单位向量 a a （3.6）|a|构成与o和a正交的n n oa （3.7）在o和a形成的平面上旋转o，使得o与n和a正交 o an （3.8）单位向量o是 o o （3.9）|o|根据第二章给出的一般性的旋转矩阵ot(k,)，它把机械手末端的姿态规定为绕k轴旋转角。3.欧拉角欧拉角(Euler Angles)姿态变更常用绕x,y或z轴的一系列旋转来确定。欧拉角描述方法是：先绕z轴旋转，然后绕新的y(即y/)轴旋转，最后绕更新的z(z/)轴旋转（见图3.2）欧拉变换Euler(,)可以通过连乘三个旋转矩阵来求得Euler(,)ot(z,)o
5、t(y,)ot(z,)（3.10）在一系列旋转中，旋转的次序是重要的。应注意，旋转序列如果按相反的顺序进行，则是绕基坐标中的轴旋转：绕z轴旋转，接着绕y轴旋转，最后再一次绕z轴旋转，结果如图3.3所示，它与图3.2是一致的。xxxxyyyzzzzy图3.2 欧拉角0 xxxxyyyzzzzy图3.3 基于基坐标的欧拉角03.4 摇摆、俯仰和偏转摇摆、俯仰和偏转(Roll,Pitch and Yaw)摇摆、俯仰和偏转为另一种旋转。如图3.4所示，就像水中航行的一条小船一样，绕着它前进的方向（z轴）旋转 称为摇摆，绕着它的横向中轴（y轴）旋转 称为俯仰，绕着它甲板的垂直向上的方向（x轴）旋转 称为
6、偏转。借助于这种旋转来描述机械手的末端执行器如图3.5所示。规定旋转的次序为 RPY(,)ot(z,)ot(y,)ot(x,)（3.12）即，绕x轴旋转，接着绕y轴旋转，最后绕z轴旋转，这个变换如下 cos 0 sin 0 1 0 0 0 0 1 0 0 0 cos sin 0 RPY(,)=ot(z,)sin 0 cos 0 0 sin cos 0 （3.13）0 0 0 1 0 0 0 1 cos sin 0 0 cos sinsin sincos 0 sin cos 0 0 0 cos sin 0 RPY(,)=0 0 1 0 -sin cossin coscos 0 （3.14）0 0
7、 0 1 0 0 0 1 图3.4 摇摆、俯仰和偏 转角图3.5 机械手的末端执行器 的摇摆、俯仰和偏 转RPY(，)=cos cos cos sinsin sin cos cos sincos+sin sin 0sin cos sin sinsin+cos cos sin sincoscos sin 0 -sin cossin coscos 0 0 0 0 1 （3.15）3.5 位置的确定位置的确定(Specification of Position)一旦方向被确定之后，用一个相应的p向量的位移变换可得到机器人末端执行器在基坐标中的位置：1 0 0 px 0 1 0 py T6=0 0 1
8、 pz （3.16）0 0 0 1 旋转变换矩阵3.6 圆柱坐标圆柱坐标(Cylindrical Coordinates)如图3.6所示，在圆柱坐标中确定机械手的位置是沿x轴平移r，接着绕z轴旋转，最后沿着z轴平移z。Cyl(z,r)=Trans(0,0,z)Rot(z,)Trans(r,0,0)cos-sin 0 0 1 0 0 r sin cos 0 0 0 1 0 0Cyl(z,r)=Trans(0,0,z)0 0 1 0 0 0 1 0 0 0 0 1 0 0 0 1 （3.17）1 0 0 0 cos -sin 0 rcos 0 1 0 0 sin cos 0 rsinCyl(z,r
9、)=0 0 1 z 0 0 1 0 0 0 0 1 0 0 0 1 （3.18）zaCzyxBAorn图3.6 圆柱坐标注意：圆柱坐标只能绕 z 轴旋转 cos-sin 0 rcos sin cos 0 rsin Cyl(z,r)=0 0 1 z （3.19）0 0 0 1 如用一个绕z轴旋转-的变换矩阵右乘式（3.19）,结果如下 cos-sin 0 rcos cos(-)-sin(-)0 0 sin cos 0 rsin sin(-)cos(-)0 0Cyl(z,r)=0 0 1 z 0 0 0 0 （3.20）0 0 1 1 0 0 0 1 cos -sin 0 rcos cos sin
10、 0 0 sin cos 0 rsin -sin cos 0 0Cyl(z,r)=0 0 1 z 0 0 0 0 （3.21）0 0 0 1 0 0 0 1 1 0 0 r cos 0 1 0 r sin Cyl(z,r)=0 0 1 z （3.22）0 0 0 1 上式表明平移矢量未变，旋转矩阵为单位阵，此时末端坐标的姿态未变，而只是改变了它的空间位置。3.7 球坐标球坐标(Spherical Coordinates)如图3.7所示，用球坐标来确定位置向量的方法是：沿着z轴平移，然后绕y轴旋转，最后绕z轴旋转。Sph(,)=Rot(z,)Rot(y,)Trans(0,0,)（3.23）cos
11、 0 sin 0 1 0 0 0 0 1 0 0 1 1 0 0Sph(,)=Rot(z,)-sin 0 cos 0 0 0 1 0 0 0 1 0 0 0 1 （3.24）aonzyx图3.7 球坐标 cos-sin 0 0 cos 0 sin rsin sin cos 0 0 0 1 0 0 Sph(,)=0 0 1 0 -sin 0 cos rcos （3.25）0 0 0 1 0 0 0 1 coscos -sin cossin cossin sincos cos sinsin sinsin Sph(,)=-sin 0 cos cos （3.26）0 0 0 1 同样，如果不希望改变末
12、端坐标的姿态，而只是改变其空间位置，我们可以用Rot(y,-)和Rot(z,-)右乘式（3.26）Sph(,)=Rot(z,)Rot(y,)Trans(0,0,)Rot(y,-)Rot(z,-)（3.27）1 0 0 cossin 0 1 0 sinsin Sph(,)=0 0 1 cos （3.28）0 0 0 1 3.7 T6的确定的确定(Specification of T6)T6可以用旋转和平移的方法来确定。T6=平移旋转 （3.29）表3.1 各种平移与旋转的表达式 Translation Eqn Rotation Eqn px,py,pz ox o y oz ax a y a z
13、Rot(k,)2.32 Cyl(z,r)3.22 Euler(,)3.11 Sph(,)3.26 RPY(,)3.12 我们已经研究过的各种平移与旋转的式子，总结在表3.1中。如果我们使用Cyl和Sph的非旋转的形式，那么矩阵积（3.29）仅仅是一个平移变换。3.9 各种各种A矩阵的确定矩阵的确定(Specification of matrices A)现在考虑方程(3.1)右边各A矩阵的确定。串联杆型机械手是由一系列通过连杆与其活动关节连接在一起所组成。如图3.8所示，任何一个连杆都可以用两个量来描述：一个是公共垂线距离an，另一个是与an垂直的平面上两个轴的夹角n，习惯上称an为连杆长度，
14、n称为连杆的扭转角。图3.8 连杆的长度与扭转角 如图3.9所示，在每个关节轴上有两个连杆与之相连，即关节轴有两个公垂线与之垂直，每一个连杆一个。两个相连的连杆的相对位置用dn和n确定，dn是沿着n关节轴两个垂线的距离，n是在垂直这个关节轴的平面上两个被测垂线之间的夹角，dn和n分别称作连杆之间的距离及夹角。图3.9 连杆参数xn-1 为了描述连杆之间的关系，我们对每个连杆赋一个坐标系。转动关节转动关节：关节变量为n。连杆n的坐标原点设在关节n和关节n+1轴之间的公共垂线与关节n+1轴的交点上。在关节轴相交的情况下（无公垂线），这个原点就在两个关节轴的相交点上（an0）。如果两个关节轴平行（有
15、无数条公垂线），则原点的选择要使下一个连杆的关节距离为0（dn0），连杆n的z轴与n+1关节轴在一条直线上。x轴与任何存在的公共垂线成一条直线，并且沿着这条垂线从n关节指向n+1关节。在相交关节的情况下，x轴的方向平行或者逆平行zn-1zn的向量叉积，应该注意，这个条件对于沿着关节n和n+1之间垂线的x轴同样满足。当xn-1和xn平行，且有相同的指向时，则对于第n个转动关节n0。连杆本身的参数连杆长度an连杆两个轴的公垂线距离（x方向）连杆扭转角n连杆两个轴的夹角（x轴的扭转角）连杆之间的参数连杆之间的距离dn相连两连杆公垂线距离（z方向平移距）连杆之间的夹角n相连两连杆公垂线的夹角（z轴旋转
16、角）表3.2 连杆参数棱形关节棱形关节：关节变量为dn。关节轴的方向就是关节的运动方向。与转动关节不同，轴的运动方向被确定了，但在空间的位置并没有确定（见图2.10）。对于棱形关节，连杆长度an没有意义，所以被设置为0。棱形关节坐标的z轴（zn）与下一个连杆的轴在一条直线上，x轴（xn）平行或逆平行棱形关节轴的方向（zn-1）与zn的叉积。对于棱形关节，当dn=0时，定义为0位置（即坐标原点）。因此棱形关节坐标原点与上一个关节（n-1）坐标原点重合，上一个关节的z轴（zn-1）与棱形关节的轴向相同，其关节长度an-1为上一个关节的轴线与zn-1的公垂线长度，xn-1轴向为公垂线向下一个关节延伸
17、的方向。图3.10 棱型关节的连杆参数an-1 根据上述模式用下列旋转和位移我们可以建立相邻的n-1和n坐标系之间的关系：绕 zn-1 旋转一个角度n 沿 zn-1 位移一个距离 dn 沿着被旋转的 xn-1 即 xn 位移 an 绕 xn 旋转的扭转角为n 这四个齐次变换的积为A矩阵，即 An=Rot(z,)Trans(0,0,d)Trans(a,0,0)Rot(x,)（3.30）cos -sin 0 0 1 0 0 a 1 0 0 0 sin cos 0 0 0 1 0 0 0 cos -sin 0 An=0 0 1 0 0 0 1 d 0 sin cos 0 （3.31）0 0 0 1
18、0 0 0 1 0 0 0 1 cos -sincos sinsin acos sin coscos -cossin asin An=0 sin cos d （3.32）0 0 0 1 对于棱形关节，an=0，则式（3.32）A矩阵简化为 cos -sincos sinsin 0 sin coscos -cossin 0 An=0 sin cos d （3.33）0 0 0 1 一旦给机械手各连杆坐标系都赋了值，各种固定的连杆参数可以确定：对于后面是旋转关节的连杆参数为d,a和，对于后面是棱形关节的连杆参数为和。根据这些参数，的正弦和余弦也可以求出。这样，对于旋转关节，A矩阵变成了关节变量的函
19、数。或在棱形关节的情况下，变成了d的函数。一旦这些值给出，对于六个Ai变换矩阵的值就可以确定。3.10 根据根据A矩阵来确定矩阵来确定T6(Specification of T6 in Terms of the A matrices)机械手的坐标变换图如图3.11所示，机械手的末端（即连杆坐标系6）相对于连杆坐标系n-1的描述用n-1T6表示，即：n-1T6=An An+1 A6 （3.34）0zA1A2A3A4A5A60EX0T61T62T63T64T65T6图3.11 机械手的坐标变换图 机械手的末端相对于基坐标系（用T6表示）用下式给出 T6=A1 A2 A3 A4 A5 A6 （3.3
20、5）如果机械手用变换矩阵Z与参考坐标系相联系，机械手末端执 行器用E来描述，末端执行器的位置和方向相对参考坐标系用X来 描述，如图3.11所示有 X=Z T6 E （3.36）由此可以得到T6的表达式 T6 =Z-1 X E-1 （3.37）3.11 斯坦福机械手的运动方程斯坦福机械手的运动方程(Kinematic Equations for the Stanford Manipulator)斯坦福机械手及其各关节坐标的设置如图3.12所示。将角的正弦和余弦简化 sini =Si cosi=Ci sin(i+j)=Sij cos(i+j)=Cij注：将所有关节x轴的方向设置 一致，可简化坐标变
21、换。图3.12 斯坦福机械手坐标系表3.2 斯坦福机械手连杆参数 Link Variable a d cos sin 1 1 -90 0 0 0 -1 2 2 90 0 d2 0 1 3 d3 0 0 d3 1 0 4 4 -90 0 0 0 -1 5 5 90 0 0 0 1 6 6 0 0 0 1 0斯坦福机械手的A变换如下：C1 0 -S1 0 S1 0 C1 0 A1=0 -1 0 0 （3.38）0 0 0 1 C2 0 S2 0 S2 0 -C2 0 A2=0 1 0 d2 （3.39）0 0 0 1 1 0 0 0 0 1 0 0 A3=0 0 1 d3 （3.40）0 0 0
22、1 C4 0 -S4 0 S4 0 C4 0 A4=0 -1 0 0 （3.41）0 0 0 1 C5 0 S5 0 S5 0 -C5 0 A5=0 1 0 0 （3.42）0 0 0 1 C6 -S6 0 0 S6 C6 0 0 A6 =0 0 1 0 （3.43）0 0 0 1斯坦福机械手A变换的积如下所示，这些是从连杆6开始，然后逐步回到基坐标。C6 -S6 0 0 S6 C6 0 0 5T6 =0 0 1 0 （3.44）0 0 0 1 C5C6 -C5S6 S5 0 S5C6 -S5S6 -C5 0 4T6 =S6 C6 0 0 （3.45）0 0 0 1 C4C5C6-S4S6 -
23、C4C5S6-S4C6 C4S5 0 S4C5C6+C4S6 -S4C5S6+C4C6 S4S5 0 3T6 =-S5C6 S5S6 C5 0 （3.46）0 0 0 1 C4C5C6-S4S6 -C4C5S6-S4C6 C4S5 0 S4C5C6+C4S6 -S4C5S6+C4C6 S4S5 0 2T6 =-S5C6 S5S6 C5 d3 （3.47）0 0 0 1 C2(C4C5C6-S4S6)-S2S5C6 -C2(C4C5S6+S4C6)+S2S5S6 S2(C4C5C6-S4S6)+C2S5C6 -S2(C4C5 S6+S4C6)-C2S5S6 1T6 =S4C5C6+C4C6 -S
24、4C5S6+C4C6 0 0 C2C4S5+S2C5 S2d3 S2C4S5-C2C5 -C2d3 S4S5 d2 （3.48）0 1 nx ox ax px ny oy ay py T6 =nz oz az pz （3.49）0 0 0 1其中 nx=C1 C2(C4C5C6-S4S6)-S2S5C6 -S1(S4C5S6+C4S6)ny=S1 C2(C4C5C6-S4S6)-S2S5C6 +C1(S4C5S6+C4S6)nz=-S2(C4C5C6-S4S6)-C2S5C6 ox=C1 -C2(C4C5S6+S4C6)+S2S5C6 -S1(-S4C5S6+C4S6)oy=S1 -C2(C4
25、C5C6+S4C6)+S2S5S6 +C1(-S4C5S6+C4S6)oz=S2(C4C5C6+S4C6)+C2S5S6 ax=C1(C2C4S5+S2C5)S1S4C5 ay=S1(C2C4S5+S2C5)+C1S4S5 az=S2C4S5+C2C5 px=C1S2d3 S1d2 py=S1S2d3+C1d2 pz=C2d33.12 肘机械手的运动方程肘机械手的运动方程(Kineamtic Equations for an Elbow Manipulator)肘机械手及其各关节坐标的设置如图3.13所示。z1z0z2z3z4z5、z6xa3a4a2图3.13 肘机械手的坐标系x0y0 表3.
26、3 肘机械手的连杆参数 Link Variable a d cos sin 1 1 90 0 0 0 1 2 2 0 a2 0 1 0 3 3 0 a3 0 1 0 4 4 -90 a4 0 0 -1 5 5 90 0 0 0 1 6 6 0 0 0 1 0注：在以下的T矩阵中用变量23=2+3和234=23+4来进 行简化。肘机械手的A变换如下：C1 0 S1 0 S1 0 -C1 0 A1=0 1 0 0 （3.50）0 0 0 1 C2 -S2 0 C2S2 S2 C2 0 S2a2 A2=0 1 1 0 （3.51）0 0 0 1 C3 -S3 0 C3a3 S3 C3 0 S3a3
27、A3=0 0 1 0 （3.52）0 0 0 1 C4 0 -S4 C4a4 S4 0 C4 S4a4 A4=0 -1 0 0 （3.53）0 0 0 1 C5 0 S5 0 S5 0 -C5 0 A5=0 -1 0 0 （3.54）0 0 0 1 C6 -S6 0 0 S6 C6 0 0 A6 =0 0 1 0 （3.55）0 0 0 1 为了得到T6，我们从连杆6开始来算A矩阵的积，逐步往回计算到基坐标。C6 -S6 0 0 S6 C6 0 0 5T6=0 0 1 0 （3.55）0 0 0 1 C5C6 -C5S6 S5 0 S5C6 -S5S6 -C5 0 4T6=S6 C6 0 0
28、（3.56）0 0 0 1 C4C5C6-S4S6 -C4C5S6-S4C6 C4S5 C4a4 S4C5C6+C4S6 -S4C5S6+C4C6 S4S5 S4a4 3T6=-S5C6 S5S6 C5 0 （3.57）0 0 0 1 C34C5C6-S34S5 -C34C5S6-S34C6 C34S5 C34a4+C3a3 S34C5C6+C34S6 -S34C5S6+C34C6 S34S5 S34a4+S3a3 2T6=-S5C6 S5S6 C5 0 （3.58）0 0 0 1 C234C5C6-S234S6 -C234C5C6-S234C6 C234S5 C234a4+C23a3+C2a
29、2 S234C5C6+C234S6 -S234C5S6+C234C6 S234S5 S234a4+S23a3+S2a2 1T6=-S5C6 S5S6 C5 0 （3.59）0 0 0 1 nx ox ax px ny oy ay py T6=nz oz az pz （3.60）0 0 0 1 其中 ox=-C1 C234C5S6+S234C6 +S1S5S6 oy=-S1 C234C5S6+S234C6 -C1S5S6 oz=-S234C5C6+C234C6 ax=C1C234S5+S1C5 ay=S1C234S5-C1C5 az=S234S5 px=C1 C234a4+C23a3+C2a2
30、py=S1 C234a4+C23a3+C2a2 pz=S234a4+S23a3+S2a23.13 小结小结(Summary)本章，主要介绍了用齐次变换来描述在各种坐标系中机械手的位置与方向。首先介绍了各种正交坐标系的齐次变换。然后介绍了非正交关节坐标系描述机械手末端的齐次变换。上述变换关系对任何数目关节的各种机械手均适用。第四章 逆运动学方程Chapter Inverse Kinematic Equations4.1 引言4.2 逆运动学方程的解4.3 斯坦福机械手的逆运动学解4.4 欧拉变换的逆运动学解4.5 RPY变换的逆运动学解4.6 球坐标变换的逆运动学解4.7 本章小结 4.1 引言
31、(Introduction)所谓逆运动学方程的解，就是已知机械手直角坐标空间的位姿（pose）T6，求出各节变量n or dn 。T6=A1 A2 A3 A4 A5 A6 （4.1）逆运动学方程解的步骤如下：（1）根据机械手关节坐标设置确定An An为关节坐标的齐次坐标变换，由关节变量和参数确定。关节变量和参数有：an连杆长度;n连杆扭转角;dn相邻两连杆的距离;n相邻两连杆的夹角。对于旋转关节n为关节变量，而对于滑动关节dn为关节变量。其余为连杆参数，由机械手的几何尺寸和组合形态决定。（2）根据任务确定机械手的位姿T6 T6为机械手末端在直角坐标系（参考坐标或基坐标）中的位姿，由任务确定，即
32、式（3.37）给出的表达式T6=Z-1 X E-1确定。它是由三个平移分量构成的平移矢量P（确定空间位置）和三个旋转矢量n，o，a（确定姿态）组成的齐次变换矩阵描述。（3）由T6和An（n1，2，6）和式（4.1）求出相应的关节变量n 或 dn。10006zzzzyyyyxxxxpaonpaonpaonT4.2 逆运动学方程的解（Solving inverse kinematic equations）根据式（4.1）T6=A1 A2 A3 A4 A5 A6分别用An（n1，2，5）的逆左乘式（4.1）有 A1-1 T6=1T6 (1T6=A2 A3 A4 A5 A6)（4.2）A2-1 A1-
33、1 T6=2T6 (2T6 =A3 A4 A5 A6)（4.3）A3-1A2-1 A1-1 T6=3T6 (3T6 =A4 A5 A6 )（4.4）A4-1 A3-1A2-1 A1-1 T6=4T6 (4T6 =A5 A6)（4.5）A5-1 A4-1 A3-1A2-1 A1-1 T6=5T6 (5T6=A6 )（4.6）根据上述五个矩阵方程对应元素相等，可得到若干个可解的代数方程，便可求出关节变量n或 dn。4.3 斯坦福机械手的逆运动学解斯坦福机械手的逆运动学解 （Inverse solution of Stanford manipulator）在第三章我们推导出 Stanford Man
34、ipulator 的运动方程和各关节齐次变换式。下面应用式（4.2）（4.6）进行求解：这里 f11=C1 xS1 y (4.10)f12=-z (4.11)f13=-S1 xC1 y (4.12)其中 x=nx ox ax px T，y=ny oy ay py T，z=nz oz az pz T由第三章得到的斯坦福机械手运动学方程式（3.48）为 C2(C4C5C6-S4S6)-S2S5C6 -C2(C4C5S6+S4C6)+S2S5S6 S2(C4C5C6 -S4S6)+C2S5C6 -S2(C4C5 S6+S4C6)-C2S5S6 1T6 =S4C5C6+C4C6 -S4C5S6+C4C
35、6 0 0 C2C4S5+S2C5 S2d3 S2C4S5-C2C5 -C2d3 S4S5 d2 （4.13）0 1比较式（4.9）和式（4.13）矩阵中的第三行第四列元素相等得到 f13（p）=d2 （4.14）或 -S1 pxC1 py=d2 （4.15）令 px=r cos （4.16）py=r sin （4.17）其中 （4.18）（4.19）将式（4.16）和式（4.17）代入式（4.15）有 sincon1consin1 d2/r （0 d2/r 1）（4.20）由式（4.20）可得 sin(1)d2/r （0 1 ）（4.21）con(1)（4.22）这里号表示机械手是右肩结构（
36、）还是左肩结构（）。22yxpprxypp1tan221rd由式（4.21）、（4.22）和（4.18）可得到第一个关节变量1的值 （4.23）根据同样的方法，利用式（4.9）和式（4.13）矩阵元素相等建立的相关的方程组，可得到其它各关节变量如下：2222111tantandrdppxyzyxppSpC1112tan（4.24）zyxpCpSpCSd21123（4.25）zyxyxaSaSaCCaCaS21121114tan（4.26）zyxyxzyxaCaSaCSaCaSSaSaSaCCC21121142112415tan（4.27）yxzyxzyxyxzyxoCoSCoSoSoCCSoC
37、oSoCSSoCoSSoSoSoCCCC114211242112511421124516tan（4.28）注意：l在求解关节变量过程中如出现反正切函数的分子和分母太小，则计算结果误差会很大，此时应重新选择矩阵元素建立新的方程组再进行计算，直到获得满意的结果为止。同样，如果计算结果超出了机械手关节的运动范围，也要重新计算，直到符合机械手关节的运动范围。l由于机械手各关节变量的相互耦合，后面计算的关节变量与前面的关节变量有关，因此当前面关节变量的计算结果发生变化时，后面关节变量计算的结果也会发生变化，所以逆运动方程的解不是唯一的，我们应该根据机械手的组合形态和各关节的运动范围，经过多次反覆计算，从
38、中选择一组合理解。由此可见，求解机械手的逆运动方程是一个十分复杂的过程。4.4 欧拉变换的逆运动学解 （Inverse solution of Euler Angles）由第三章知欧拉变换为Euler(,)Rot(z,)Rot(y,)Rot(z,)（4.29）我们用T来表示欧拉变换的结果，即T Euler(,)（4.30）或T Rot(z,)Rot(y,)Rot(z,)（4.31）其中 （4.32）1000zzzzyyyyxxxxpaonpaonpaonT（4.33）10000sinsincossin0sinsincoscossincossinsincoscoscossin0sincoscos
39、sinsincoscossinsincoscoscos1000010000cossin00sincos10000cossin00100sin0cos1000010000cossin00sincos),(),(),(conzRotyRotzRot比较式（4.32）和式（4.33）有 （4.34）（4.35）（4.36）（4.37）（4.38）（4.39）（4.40）（4.41）（4.42）sinsincoscoscosxnsincoscoscossinyncossinzncossinsincoscosxocoscossincossinyosinsinzosincosxasinsinyacosza
40、由式（4.42）可解出角 （4.43）由式（4.40）和式（4.43）可解出角 （4.44）由式（4.36）和式（4.43）可解出角 （4.45）)(cos1zasincos1xasincos1zn 这里需要指出的是，在我们采用式（4.43）式（4.45）来计算、时都是采用反余弦函数，而且式（4.43）和式（4.45）的分母为sin，这会带来如下问题：1）由于绝对值相同的正负角度的余弦相等，如coscos（-），因此不能确定反余弦的结果是在那个象限；2）当sin接近于0时，由式（4.43）和式（4.45）所求出的角度和是不精确的；3）当0或180时，式（4.43）和式（4.45）无数值解。为此
41、，我们必须寻求更为合理的求解方法。由三角函数的知识我们知道，反正切函数tan1（x/y）所在的象限空间可由自变量的分子和分母的符号确定（如图4.1所示），因此如果我们得到欧拉角的正切表达式，就不难确定欧拉角所在的象限。为此，我们采用本章第二节的方法，用Rot(z,)1左乘式（4.31）有Rot1(z,)T Rot(y,)Rot(z,)（4.46）yxyyxyxxyx图4.1 正切函数所在象限即（4.47）将上式写成如下形式（4.48）式中 （4.49）（4.50）（4.51）同样，上面三个式子中的x、y、z分别表示n、o、a、p矢量的各个分量，如 （4.52）10000cossinsincos
42、sin00cossin0sinsincoscoscos10001000010000cossin00sincoszzzzyyyyxxxxpaonpaonpaon10000cossinsincossin00cossin0sinsincoscoscos1000)()()()()()()()()()()()(131313131212121211111111pfafofnfpfafofnfpfafofnfyxfsincos11yxfcossin12zf13yxaaafcossin)(12比较式（4.48）等号两边矩阵的第2行第3列元素可知 （4.63）即 （4.54）由此可得到 （4.55）或 （4.5
43、6）结果得到 （4.57）或 （4.58）0)(12af0cossinyxaaxyaacossintanxyaacossintanxyaa1tanxyaa1tan 上述结果相差180，可根据实际系统的组合形态从中选择一个合理解。如果ay和ax都为0，则式（4.57）和式（4.58）无定义，这是一种退化现象，此时值可任意设置，如0。由于角已求出，比较式（4.48）等号两边矩阵第1行第3列和第3行第3列元素相等有 （4.59）（4.60）或 （4.61）（4.62）由此可得 （4.63）)(sin11af)(cos13afyxaasincossinzacoszyxaaasincostan1同样比较
44、式（4.48）等号两边矩阵的第2行第1列和第2行第2列元素可知 （4.64）（4.65）或 （4.66）（4.67）由此可得 （4.68）至此，我们求出了欧拉变换的逆运动学解。)(sin12nf)(cos12ofyxnncossinsinyxoocossincosyxyxoonncossincossintan14.5 RPY变换的逆运动学解（Inverse solution of RPY）第三章介绍的摇摆、俯仰和偏转(RPY)变换的表达式如下T=RPY(,)Rot(z,)Rot(y,)Rot(x,)（4.69）用Rot1(z,)左乘上式得到Rot1(z,)T Rot(y,)Rot(x,)（4.
45、70）将上式写成式（4.48）的形式 （4.71）式中 （4.72）（4.73）（4.74）10000coscossincossin0sincos00cossinsinsincos10000)()()(0)()()(0)()()(131313121212111111afofnfafofnfafofnfyxfsincos11yxfcossin12zf13由式（4.71）等号两边矩阵的第2行第1列元素相等有 （4.75）由此得到 （4.76）或 （4.77）角已求出，根据式（4.71）等号两边矩阵的第3行第1列和第1行第1列元素相等有 （4.78）（4.79）由此可得 （4.80）0cossiny
46、xnnxynn1tan0180znsinyxnnsincoscosyxznnnsincostan1 进一步比较式（4.71）等号两边矩阵元素，由第2行第3列和第2行第2列元素相等有 （4.81）（4.82）由此可得 （4.83）至此，我们求出了RPY的逆运动学解。yxaacossinsinyxoocossincosyxyxooaacossincossintan14.6 球坐标变换的逆运动学解 （Inverse solution of Spherical Coordinates）第三章介绍的球坐标变换的表达式如下T=Sph(,)=Rot(z,)Rot(y,)Trans(0,0,)（4.84）用R
47、ot1(z,)左乘上式得到Rot1(z,)T=Rot(y,)Trans(0,0,)（4.85）将上列矩阵方程的第4列元素写出有 （4.86）由上式第2行元素相等有 （4.87）1cos0sin1cossinsincoszyxyxppppp0cossinyxpp由式（4.87）可得到 （4.88）或 （4.89）由式（4.86）第1行和第3行元素相等有 （4.90）（4.91）由此可得 （4.92）xypp1tan0180yxppsincossinzpcoszyxpppsincostan1为了获得平移量，我们用Rot1(y,)左乘式（4.85）Rot1(y,)Rot1(z,)T=Trans(0,
48、0,)（4.93）上式第4列元素是 （4.94）由上式第3行元素相等得到 （4.95）至此，我们求出了球坐标变换的逆运动学解。1001cos)sincos(sincossinsin)sincos(coszyxyxzyxppppppppzyxpppcos)sin(cossin4.7 本章小结（Summary）l解逆运动方程是应用齐次坐标变换原理，从机器人末端执行器的直角坐标空间到关节坐标的变换（T6 n、dn），它是求解正运动方程的逆过程（n、dn T6），是机器人运动学的重要内容，是机器人控制的依据。l要注意的是正运动方程的解是唯一解，而逆运动方程的解不是唯一解，因此选择合理解是解逆运动方程的一项重要内容。
