第2章 工业机器人运动学-old

1、注：1）2008年春季讲课用；2）带下划线的黑体字为板书内容；3）公式及带波浪线的部分为必讲内容第2章 工业机器人运动学2.1 引言 通过上一章的学习我们知道，从机构学的角度看，工业机器人可以认为是用一系列关节连接起来的连杆所组成的开链机构。工业机器人运动学研究的是各连杆之间的位移关系、速度关系和加速度关系。本章仅研究位移关系，重点是研究手部相对于机座的位姿与各连杆之间的相互关系。“位姿”是“位置和姿态”的简称。 工业机器人手部相对于机座的位姿与工业机器人各连杆之间的相互关系直接相关。为了便于数学上的分析，一般选定一个与机座固联的坐标系，称为固定坐标系，并为每一个连杆（包括手部）选定一个与之固

2、联的坐标系，称为连杆坐标系。一般把机座也视为一个连杆，即零号连杆。这样，连杆之间的相互关系可以用连杆坐标系之间的相互关系来描述。工业机器人手部相对机座的位姿就是固联在手部的坐标系相对固定坐标系的位姿。 工业机器人运动学主要包括正向运动学和反向运动学两类问题。正向运动学是在已知各个关节变量的前提下，解决如何建立工业机器人运动学方程，以及如何求解手部相对固定坐标系位姿的问题。反向运动学则是在已知手部要到达目标位姿的前提下，解决如何求出关节变量的问题。反向运动学也称为求运动学逆解。 在工业机器人控制中，先根据工作任务的要求确定手部要到达的目标位姿，然后根据反向运动学求出关节变量，控制器以求出的关节变

3、量为目标值，对各关节的驱动元件发出控制命令，驱动关节运动，使手部到达并呈现目标位姿。可见，工业机器人反向运动学是工业机器人控制的基础。在后面的介绍中我们会发现，正向运动学又是反向运动学的基础。 工业机器人相邻连杆之间的相对运动不是旋转运动，就是平移运动，这种运动体现在连接两个连杆的关节上。物理上的旋转运动或平移运动在数学上可以用矩阵代数来表达，这种表达称之为坐标变换。与旋转运动对应的是旋转变换，与平移运动对应的是平移变换。坐标系之间的运动关系可以用矩阵之间的乘法运算来表达。用坐标变换来描述坐标系（刚体）之间的运动关系是工业机器人运动学分析的基础。 在工业机器人运动学分析中要注意下面四个问题：

4、1）工业机器人操作臂可以看成是一个开式运动链，它是由一系列连杆通过转动或移动关节串联起来的。开链的一端固定在机座上，另一端是自由的。自由端安装着手爪（或工具，统称手部或末端执行器），用以操作物体，完成各种作业。关节变量的改变导致连杆的运动，从而导致手爪位姿的变化。 2）在开链机构简图中，关节符号只表示了运动关系。在实际结构中，关节由驱动器驱动，驱动器一般要通过减速装置（如用电机或马达驱动）或机构（如用油缸驱动）来驱动操作臂运动，实现要求的关节变量。 3）为了研究操作臂各连杆之间的位移关系，可在每个连杆上固联一个坐标系，然后描述这些坐标系之间的关系。Denavit和Hartenberg提出一种通

5、用的方法，用一个4×4的齐次变换矩阵描述相邻两连杆的空间关系，从而推导出“手部坐标系”相对于“固定坐标系”的齐次变换矩阵，建立操作臂的运动方程。 4）在轨迹规划时，人们最感兴趣的是手部相对于固定坐标系的位姿。2.2 齐次坐标及对象物的描述 齐次变换具有较直观的几何意义，非常适合描述坐标系之间的变换关系。另外，齐次变换可以将旋转变换与平移变换用一个矩阵来表达，关系明确，表达简洁。所以常用于解决工业机器人运动学问题。下面我们先介绍有关齐次坐标和齐次变换的内容。2.2.1 点的位置描述 如图2-1所示，在选定的三维空间直角坐标系A中，空间任一点P的坐标可以用一个(3×1)列阵（或

6、称三维列向量）Ap表示，即：OYXZAP(x，y，z)图2-1 点的位置描述(2-1)式中：X，y，z是点P在坐标系A中的三个坐标分量； Ap的左上标A代表选定的参考坐标系。2.2.2 齐次坐标 如果用四个数组成的(4×1)列阵（或称四维列向量）表示三维空间直角坐标系A中的点P，即：(2-2)则定义列阵x y z 1T为三维空间点P的齐次坐标。 必须注意，齐次坐标的表示不是唯一的。如果将列阵p中的元素同乘一非零系数w后，仍然代表同一点P，即：(2-3)式中：xa/w，yb/w，zc/w。2.2.3 坐标轴的描述bOYXZvagijkh图2-2 坐标轴及矢量的描述 如图2-2所示，i、

7、j、k分别是直角坐标系中X、Y、Z坐标轴的单位矢量，若用齐次坐标来描述X、Y、Z轴，则定义下面三个(4×1)列阵分别为单位矢量i、j、k（即X、Y、Z坐标轴）的方向列阵。i1000Tj0100Tk0010T 图2-2中所示矢量v的单位矢量h的方向列阵为：ha b c 0Tcosa cosb cosg 0T(2-4)式中，a、b、g分别是矢量v与坐标轴X、Y、Z的夹角，0°£a£180°，0°£b £180°，0°£g £180°。cosa、cosb、cosg称为矢量

8、v的方向余弦，且满足cos2a+cos2b+cos2g1。 综上所述，可得下面两点结论： 1）(4×1)列阵a b c wT中第四个元素不为零，则表示空间某点的位置； 2）(4×1)列阵a b c 0T中第四个元素为零，且a2+b2+c21，则表示某个坐标轴(或某个矢量)的方向，a b c 0T称为该矢量的方向列阵。 表示坐标原点的(4×1)列阵定义为：o0 0 0 aT a0 例2-l 用齐次坐标分别写出图2-3中矢量u、v、w的方向列阵。图2-3 用不同方向角描述的方向矢量u、wOYXZua90°b45°g45°abgOYXZva

9、45°b90°g45°abgOYXZwa60°b60°g45°abg 解：矢量u：ucosa cosb cosg 0T0.0 0.7071 0.7071 0T 矢量v：vcosa cosb cosg 0T0.7071 0.0 0.7071 0T 矢量w：wcosa cosb cosg 0T0.5 0.5 0.7071 0T2.2.4 动坐标系位姿的描述 对动坐标系位姿的描述就是相对固定坐标系对动坐标系原点位置的描述以及对动坐标系三个坐标轴方向的描述，现以两个实例说明。 1）刚体位姿的描述 组成工业机器人的每一个连杆都可以看作是一个刚体

10、。若给定了刚体上某一点的位置和该刚体在空间的姿态，则这个刚体在空间上是完全确定的。QOYXZO¢(x0，y0，z0)图2-4 刚体的位置和姿态X¢Y¢Z¢noap 设有一刚体Q，如图2-4所示，在刚体上选任一点O¢，建立与刚体固连的坐标系O¢X¢Y¢Z¢，称为动坐标系。O¢点在固定坐标系OXYZ中的位置可用齐次坐标形式的(4×1)列阵表示为：(2-5) 刚体姿态可用动坐标系三个坐标轴的方向来表示。令n、o、a分别为X¢、Y¢、Z¢坐标轴的单位方向矢量，每个

11、单位方向矢量在固定坐标系上的分量为动坐标系该坐标轴的方向余弦，用齐次坐标列阵分别表示为：nix iy iz 0TcosaX¢ cosbX¢ cosgX¢ 0Toox oy oz 0TcosaY¢ cosbY¢ cosgY¢ 0T(2-6)aax ay az 0TcosaZ¢ cosbZ¢ cosgZ¢ 0T式中：aX¢、aY¢、aZ¢分别为X¢、Y¢、Z¢坐标轴与X坐标轴的夹角； bX¢、bY¢、bZ¢分别为X

12、62;、Y¢、Z¢坐标轴与Y坐标轴的夹角； gX¢、gY¢、gZ¢分别为X¢、Y¢、Z¢坐标轴与Z坐标轴的夹角。 因此，图2-4中刚体的位姿可用下面的(4×4)矩阵来描述：(2-7) 很明显，对刚体Q位姿的描述就是对固连于刚体Q的坐标系O¢X¢Y¢Z¢位姿的描述。 OAXAYAAYBXBB30°OB(xB，yB，zB)图2-5 动坐标系B的描述例2-2 图2-5表示固连于刚体的坐标系B位于OB点，xb10，yb6，zb0。ZB轴和ZA轴与纸面垂直，坐标系B

13、相对固定坐标系A有一个30°的偏转，试写出表示刚体位姿的坐标系B的(4×4)矩阵表达式。 解：XB的方向列阵：ncos30° cos60° cos90° 0T0.866 0.5 0.0 0T YB的方向列阵：ocos120° cos30° cos90° 0T-0.5 0.866 0.0 0T ZB的方向列阵：acos90° cos90° cos0° 0T0.0 0.0 1.0 0T 坐标系B的位置列阵：p10.0 6.0 0.0 1T所以，坐标系B的(4×4)矩阵表达式为：

14、2）手部位姿的表示noBOBaZBYBXBpOZXY图2-6 手部位置及姿态的描述 工业机器人手部的位姿也可以用固连于手部的坐标系B的位姿来表示，如图2-6所示。坐标系B可以这样来确定：取手部的中心点OB为原点；关节轴为ZB轴，ZB轴的单位方向矢量a称为接近矢量，指向朝外；两个手指的连线为YB轴，指向可任意选定，YB轴的单位方向矢量o称为姿态矢量；XB轴与YB轴及ZB轴垂直，XB轴的单位方向矢量n称为法向矢量，且no×a，指向符合右手法则。 手部的位置矢量为固定坐标系原点指向手部坐标系B原点的矢量p，手部的方向矢量为n、o、a。于是，手部的位姿可用(4×4)矩阵表示为：(2

15、-8) 例2-3 图2-7表示手部抓握物体Q，物体为边长2个单位的正立方体，写出表达该手部位姿的矩阵式。 解：因为物体Q的形心与手部坐标系O¢X¢Y¢Z¢的坐标原点O¢重合，固定坐标系原点O为正立方体的后下方左侧顶点，所以手部位置的(4×1)列阵为：p1 1 1 1T 设n、o、a为手部坐标系三个坐标轴的单位方向矢量，由图2-7可知： 矢量n的方向角为：aX¢90°bX¢180°gX¢90°O¢n(X¢)oaQZY图2-7 握住物体Q的手部X(Z¢

16、;)(Y¢) 矢量o的方向角为：aY¢180°bY¢90°gY¢90° 矢量a的方向角为：aZ¢90°bZ¢90°gZ¢180°于是有： ncosaX¢ cosbX¢ cosgX¢ 0T0 -1 0 0T ocosaY¢ cosbY¢ cosgY¢ 0T-1 0 0 0T acosaZ¢ cosbZ¢ cosgZ¢ 0T0 0 -1 0T 根据式(2-8)可知，表达该手部位姿的

17、矩阵式为：2.2.5 目标物的齐次矩阵表示 设有一楔块Q如图2-8所示，坐标系OXYZ为固定坐标系，坐标系O¢X¢Y¢Z¢为与楔块Q固连的动坐标系。在图(a)情况下，动坐标系O¢X¢Y¢Z¢与固定坐标系OXYZ重合。楔块Q的位置和姿态可用6个点的齐次坐标来描述，在图(a)情况下，其矩阵表达式为：图2-8 物体的齐次矩表示O(O¢)Z(Z¢)QC(-1, 0, 2)F(-1, 4, 0)DB(-1, 0, 0)A(1, 0, 0)E(1, 4, 0)(a)X(X¢)Y(Y¢)(

18、1, 0, 2)QD(6, 1, 0)B(4, -1, 0)F(4, -1, 4)A(4, 1, 0)YZX(b)Z¢ Y¢O¢X¢OC(6, -1, 0)E(4, 1, 4) 若让楔块Q先绕Z轴旋转90°，再绕Y轴旋转90°，最后沿X轴方向平移4，则楔块成为图(b)之情况。此时楔块用新的6个点的齐次坐标来描述它的位置和姿态，其矩阵表达式为： 这个矩阵是根据图2-8(b)直接写出来的，后面讲完齐次变换以后将会知道，这个矩阵可以由图2-8(a)对应的矩阵计算得到，见例2-7。2.3 齐次变换及运算 刚体的运动是由转动和平移组成的。为了能

19、用同一矩阵表示转动和平移，有必要引入(4×4)的齐次坐标变换矩阵。2.3.1 平移的齐次变换 我们首先介绍点在空间直角坐标系中的平移。如图2-9所示，空间某一点A的坐标为(x，y，z)，当它平移至A¢点后，坐标为(x¢，y¢，z¢)，且有：A(x，y，z)XYODxDyDzA¢(x¢，y¢，z¢)图2-9 点的平移变换Zx¢x+Dxy¢y+Dy (2-9)z¢z+Dz或写成如下矩阵形式：也可以简写为：a¢Trans(Dx，Dy，Dz)×a(2-10)式中，

20、Trans(Dx，Dy，Dz)表示齐次坐标变换的平移算子，且：(2-11)其中，第四列元素Dx，Dy，Dz分别表示沿坐标轴X，Y，Z的移动量。 齐次坐标变换的运算规则：若算子左乘，表示坐标变换是相对固定坐标系进行的；假如相对动坐标系进行坐标变换，则算子应该右乘。 平移齐次变换公式(2-10)同样适用于坐标系、物体等的变换，这时最右端为一个(4×n)的矩阵。对于坐标系的变换，n=4；对于物体的变换，n=描述物体的顶点个数。 例2-4 图2-10中有下面三种情况：1）动坐标系A相对于固定坐标系作(-1，2，2)平移后到A¢；2）动坐标系A相对于自身坐标系(即动坐标系)作(-1，

21、2，2)平移后到A¢¢；3）物体Q相对于固定坐标系作(2，6，0)平移后到Q¢。已知：O¢O¢¢OO0Z¢X¢Y¢A¢X0Y0Z0ZXYAZ¢¢X¢¢Y¢¢A¢¢O0(1, 6, 0)(1, 6, 1)(3, 6, 1)(-1, 0, 1)X0Y0Z0Q(1, 0, 1)(1, 0, 0)(1, 3, 0)(-1, 0, 0)(-1, 3, 0)Q¢(3, 6, 0)(3, 9, 0)(1, 9, 0)图2

22、-10 坐标系及物体的平移变换 试计算出坐标系A¢、A¢¢以及物体Q¢的矩阵表达式。 解：动坐标系A的两个平移坐标变换算子均为： A¢坐标系是动坐标系A相对于固定坐标系作平移变换得来的，变换算子应该左乘，因此，A¢的矩阵表达式为： 从这个(4×4)的矩阵可以看出，O¢在O0X0Y0Z0坐标系中的坐标为(0，3，3)。 A¢¢坐标系是动坐标系A相对于自身（动坐标系）作平移变换得来的，变换算子应该右乘，因此，A¢¢的矩阵表达式为： 从这个(4×4)的矩阵可以看出，O&#

23、162;¢在O0X0Y0Z0坐标系中的坐标为(-1，2，-1)。 物体Q的平移坐标变换算子为： Q相对于固定坐标系做平移变换，变换算子应该左乘，因此，Q¢的矩阵表达式为： 经过平移变换后，坐标系A¢、A¢¢以及物体Q¢的实际情况已图解在图2-10中了。 我们可以根据所作的移动，从图中分析出O¢在O0X0Y0Z0坐标系中的坐标。因为坐标系A的原点为(1，1，1)，当它沿X0轴反向移动1个单位后变为(1-1，1，1)，再沿Y0轴正向移动2个单位后变为(0，1+2，1)，最后再沿Z0轴正向移动2个单位后就变为(0，3，1+2)，即

24、(0，3，3)。可见，上面计算的结果与此相符。 我们可以根据所作的移动，从图中分析出O¢¢在O0X0Y0Z0坐标系中的坐标。因为坐标系A的原点为(1，1，1)，当它沿X轴反向（即沿Y0轴正向）移动1个单位后变为(1，1+1，1)，再沿Y轴正向（即沿X0轴反向）移动2个单位后变为(1-2，2，1)，最后再沿Z轴正向（即沿Z0轴反向）移动2个单位后就变为(-1，2，1-2)，即(-1，2，-1)。可见，上面计算的结果与此相符。2.3.2 旋转的齐次变换 首先我们介绍点在空间直角坐标系中的旋转。如图2-11所示，空间某一点A，坐标为(x，y，z)，当它绕Z轴旋转q角后至A

25、2;点，坐标为(x¢，y¢，z¢)。A¢点和A点的坐标关系为：qXYOx¢图2-11 点的旋转变换xZyy¢A(x，y，z)A¢(x¢，y¢，z¢)zz¢q(2-12)或用矩阵表示为： A¢点和A点的齐次坐标分别为x¢ y¢ z¢ 1T和x y z 1T，因此A点的旋转齐次变换过程为：(2-13)也可简写为：a¢Rot(z，q)×a(2-14) 式中，Rot(z，q)表示齐次坐标变换时绕Z轴的旋转算子，算子的内容为：(2-15

26、)式中：cqcosq；sqsinq。 同理，可写出绕X轴的旋转算子和绕Y轴的旋转算子，其内容为：XYOZqkAA¢图2-12 一般旋转变换(2-16)(2-17) 图2-12所示为点A绕任意过原点的单位矢量k旋转q角的情况。kx，ky，kz分别为单位矢量k在固定坐标系坐标轴X、Y、Z上的三个分量（方向余弦），且kx2+ky2+kz21。 可以证得，绕任意过原点的单位矢量k转q角的旋转齐次变换公式为：(2-18)式中；versq(1-cosq)。 式(2-18)称为一般旋转齐次变换的通式，绕X轴、Y轴、Z轴进行的旋转齐次变换是其特殊情况，例如： 当kx1，kykz0时，即绕X轴旋转，则

27、由式(2-18)可得到式(2-16)； 当ky1，kxkz0时，即绕Y轴旋转，则由式(2-18)可得到式(2-17)； 当kz1，kxky0时，即绕Z轴旋转，则由式(2-18)可得到式(2-15)。 反之，若给出某个旋转齐次变换矩阵：则可根据式(2-18)求出其等效转轴的单位矢量k及等效转角q，计算公式为：(2-19)式中：当q取0°到180°之间的值时，式中的符号取+号。VWUXYOZ图2-13 两次旋转变换 当转角q很小时，公式(2-19)很难确定转轴。当q接近0°或180°时，转轴完全不确定。 与平移变换一样，旋转变换算子公式(2-15)、(2-1

28、6)、(2-17)以及一般旋转变换算子公式(2-18)，不仅适用于点的旋转变换，而且也适用于矢量、坐标系、物体等的旋转变换。若相对固定坐标系进行变换，则算子左乘；若相对动坐标系进行变换，则算于右乘。 例2-5 已知坐标系中点U的位置矢量u7 3 2 1T，将此点绕Z轴旋转90°，再绕Y轴旋转90°，如图2-13所示，求旋转变换后所得的点W。 解：图2-14 手臂转动和手腕转动 例2-6 如图2-14所示单臂操作手，手腕也具有一个自由度。已知手部起始位姿矩阵为：若手臂绕Z0轴旋转+90°，则手部到达G2；若手臂不动，仅手部绕手腕Z1轴旋转+90°，则手部到

29、达G3。写出手部坐标系G2及G3的矩阵表达式。 解：手臂绕Z0轴转动是相对固定坐标系作旋转变换，所以，算子应该左乘，即： 手部绕手腕Z1轴旋转是相对动坐标系作旋转变换，所以，算子应该右乘，即： 讲课时对照图分析这两个矩阵。2.3.3 平移加旋转的齐次变换 平移变换算子和旋转变换算子可以组合在一个(4×4)的矩阵中。若例2-5中的点W还要作4i-3j+7k的平移，如图2-15所示，则只要再左乘上平移变换算子，即可得到最后E点的列阵表达式，即：XYZVWEOU图2-15 平移加旋转变换式中：为平移加旋转的复合变换矩阵。 讲课时分析一下其组成特点。 例2-7 图2-8所示的楔块Q，在图(a

30、)情况下描述它的齐次矩阵为：试证明楔块经过绕固定坐标系OXYZ的Z轴旋转90°，再绕Y轴旋转90°，最后沿X轴方向平移4后见图2-8(b)的齐次矩阵表达式为： 证明：因为楔块从图(a)至图(b)的所有变换都是相对于固定坐标系OXYZ进行的，所以各坐标变换算子应该依次左乘，即：式中：即为楔块平移加旋转的复合变换矩阵。 讲课时分析一下其组成特点。证毕。2.3.4 旋量的概念 旋量Screw(k，r，j)表示沿k轴移动r，并绕k轴转动j角的综合齐次变换。 旋量Screw(k，r，j)与移动和转动发生的先后次序无关，只要它们连续即可，即：Screw(k，r，j)Rot(k，j)Tr

31、ans(k，r)Trans(k，r)Rot(k，j)(2-19a) 例如，沿Z轴移动r，并绕Z轴转动j角的综合变换为：式中：c表示cos，s表示sin。2.4 工业机器人连杆参数及其齐次变换矩阵 工业机器人可以认为是一系列通过关节连接起来的连杆组成的开链机构（P5图1-4）。手部相对固定坐标系的位姿与各连杆之间的相互关系直接相关。因此，在研究手部相对于机座的几何关系时，首先要分析两个相邻连杆之间的关系，这种关系可以用固连于相邻连杆上的坐标系之间的关系来描述。为此，我们首先必须建立连杆坐标系。下面介绍一种由Denavit和Hartenberg提出的通用方法，即DH法。2.4.1 连杆参数及连杆坐

32、标系的建立 工业机器人相邻连杆之间的关系，与连杆自身的特征和连杆之间的连接方式有关。因此，我们首先应该清楚如何对连杆的特征和连接方式进行描述。 一、连杆特征的描述连杆i关节i关节i+1aiLi图2-16 连杆尺寸参数li及ai 如图2-16所示，连杆i两端有关节i和i+1。该连杆的特征可以用两个参数来描述：一个是两个关节轴线沿公垂线的距离li，称为连杆长度；另一个是在垂直于公垂线的平面内两个轴线的夹角ai，称为连杆扭角。这两个参数为表述连杆特征的尺寸参数。连杆长度li恒为非负数，但连杆扭角ai可正、可负。ai的正负是这样规定的：公垂线的正向规定为从关节i指向关节i+1，按右手法则从轴线i绕公垂

33、线转至轴线i+1，逆时针为正，瞬时针为负。两轴线平行时，ai0；两轴线相交时，li0，此时扭角ai为两轴线的夹角，正负与Xi轴选向有关。 二、连杆连接方式的描述li-1, -1, 图2-17 连杆关系参数di及qi连杆i-2关节i-1qi-1连杆i-1连杆i连杆i+1关节i+1qi+1qiqiXi-1Zi-1lidiaiXiZi关节iXi 如图2-17所示，连杆i与连杆i-1通过关节i相连，因此，关节i的轴线有两条公垂线与它垂直。两条公垂线的相对位置可用两个参数di和qi来确定，其中di是沿关节i轴线测量的两个公垂线与i轴线交点的距离，当关节轴线相交时，di为i轴线上两交点的距离；qi是在关节

34、i轴线的垂直平面内两个公垂线的夹角，当公垂线不存在时，对旋转关节qi仍然存在。di和qi是表达相邻连杆连接关系的参数。di和qi都可正、可负(详见表2-1)。 这样，相邻两个连杆之间的关系可以由四个参数所描述：其中两个参数（li和ai）描述连杆i的尺寸；另外两个参数（di和qi）描述连杆i和连杆i-1之间的连接关系。对于旋转关节，qi是关节变量，其它三个参数固定不变；对于移动关节，di是关节变量，其它三个参数固定不变。(对照图2-17解释，一个关节即为一个自由度) 三、连杆坐标系的建立 D-H法要求按下面规则建立连杆i的坐标系i（简称i系）：1） 坐标系i与连杆i固连。Zi轴与关节i+1的轴线

35、重合，指向任意；Xi轴与连杆i的两个关节轴线的公垂线重合，方向从关节i指向关节i+1。当li0时，取Xi±Zi-1×Zi，但Xi轴取向影响ai正负（图2-16）；2） 坐标系i的Yi轴按右手法则规定，即YiZi×Xi；3） 坐标系i的原点Oi取在Xi和Zi的交点上。当关节i的轴线与关节i+1的轴线相交时，原点Oi取在两轴线的交点上；当关节i的轴线与关节i+1的轴线平行时，原点Oi取在使di0的地方。（对照图2-17解释） 图2-17画出了坐标系i-1和i的设定位姿（为什么叫设定位姿）。 在建立连杆坐标系时，下面四点值得注意：1） 连杆坐标系的建立不是唯一的。例如，

36、虽然Zi轴与关节i+1的轴线重合，但Zi轴的指向有两种选择；当Zi轴与Zi-1轴相交时，Xi轴的指向也有两种选择；2） 坐标系i也可以建立在关节i的轴线上，并使Zi轴与关节i的轴线重合；3） 建立不同的连杆坐标系，相应的连杆参数将会不同。应使描述连杆i的四个参数中尽可能多地为零；4） 与机座固连的0系原则上可以任意规定，但是，为了方便计算，一般应将0系建立在连杆1的关节1的轴线上，并使0系与1系尽量靠近或重合（画极坐标型）。 现将连杆参数与坐标系的建立归纳为表2-1。表2-1 连杆参数及坐标系连杆i的参数名称含义正负号性 质qi转角 Xi-l轴绕Zi-l轴转至与Xi轴平行时的转角 按右手法则确

37、定转动关节为变量移动关节为常量di距离 Xi-l轴沿Zi-l方向移动至与Xi轴相交时发生的位移 与Zi-l正向一致为正转动关节为常量移动关节为变量li长度 Zi-l轴沿Xi方向移动至与Zi轴相交时移动的距离 恒为非负数常量ai扭角 Zi-l轴绕Xi轴转至与Zi轴平行时的转角 按右手法则确定常量连杆i的坐标系OiXiYiZi原点Oi坐标轴Zi坐标轴Xi坐标轴Yi位于连杆i两关节轴线之公垂线与关节i+1轴线的交点处与关节i+1的轴线重合，方向任意确定沿连杆i两关节轴线的公垂线，并指向i+1关节按右手法则确定2.4.2 连杆坐标系之间的变换矩阵 建立了各连杆的坐标系后，i-1系和i系之间的变换关系可

38、以用坐标系的平移、旋转来实现。从i-1系到i系的变换，可先令i-1系绕Zi-l轴旋转qi角（旋转后Xi-l轴与Xi轴平行），再沿Zi-l轴平移di（平移后Xi-l轴与Xi轴重合），然后沿Xi轴平移li（平移后Oi-l点与Oi点重合），最后绕Xi轴旋转ai角（旋转后Zi-l轴与Zi轴重合，Yi-l轴与Yi轴也自然重合），最后使得i-1系和i系重合。 建立了各连杆的坐标系后，i-1系和i系之间的变换关系可以用坐标系的平移、旋转来实现。假设i系和i-1系原来是重合的，先令i系绕Zi轴旋转qi角（旋转后Xi轴与实际位置平行），再沿Zi轴平移di（平移后Xi轴与实际位置重合），然后沿Xi轴平移li（平移

39、后Oi点与实际位置重合），最后绕Xi轴旋转ai角（旋转后Zi轴与实际位置重合，Yi轴也自然实际位置重合），最后i系到达了实际的位姿。 用一个变换矩阵Ai来综合表示上述四次变换时，应注意原来的i系在每次旋转或平移后发生了变动，后一次变换是相对动坐标系进行的，因此，在运算中变换算子应该右乘。于是，连杆i的齐次变换矩阵为： (2-20) 也可以利用前面介绍的旋量概念用Screw(z，di，qi)×Screw(x，li，ai)来计算Ai。 实际上，很多工业机器人在设计时，常常使某些连杆参数取特殊值，如使ai=0°或90°，也有使li=0或di=0，从而可以简化变换矩阵Ai

40、的计算，同时也可以简化控制。2.4.3 工业机器人正向运动学方程及实例 工业机器人正向运动学主要解决正向运动学方程的建立及手部位姿的求解问题。 当为机器人的每个关节建立了连杆坐标系之后，就可以获得相邻两个连杆坐标系之间的变换矩阵Ai（i=1，2，n。n为机器人的自由度数），则有下列矩阵Tn=A1×A2×¼×An(2-21) 矩阵Tn就是工业机器人手部坐标系相对于固定坐标系的位姿，我们称式(2-21)为机器人正向运动学方程，Tn是如下的(4×4)矩阵：(2-22)式中，前三列表示手部的姿态，第四列表示手部的位置。 事实上，很多工业机器人在设计时，

41、常常使某些连杆参数取特殊值，使得连杆坐标系比较容易建立，同时，相邻两个连杆坐标系之间的变换矩阵Ai也比容易获得，可以不必非得按照DH法来做。下面通过两个实例来介绍建立工业机器人运动学方程的方法。 1）平面关节型工业机器人的运动学方程 图2-19(a)所示为具有一个肩关节、一个肘关节和一个腕关节的3自由度SCARA型工业机器人。考虑到关节轴线相互平行，并且连杆都在一个平面内的特点，将固定坐标系0和连杆1、连杆2、连杆3的坐标系1、2、3分别建立在关节1、关节2、关节3和手部的中心，如图2-19(a)所示。坐标系3就是手部坐标系。连杆参数中q为变量，d、l、a均为常量。建立了连杆坐标系之后，即可列

42、出该工业机器人的连杆参数如表2-2所示。X12Y0X0Z0Y11Y2X2O3Y3X3l1l2l30Z1Z2O1Z3O0O2(a)3图2-19 SCARA型机器人的坐标系Y0X00Y1X1q31O0Y2X22O1O2O3Y3X33q2q1(b)l1l2l3表2-2 图2-19(a)所示3自由度SCARA工业机器人的连杆参数连杆转角q两连杆之间距离d连杆长度l连杆扭角a连杆1q1d10l1100a10连杆2q2d20l2100a20连杆3q3d30l320a30 该SCARA型工业机器人的运动学方程为：T3A1A2A3式中，Ai（i=1，2，3）表示坐标系i相对于坐标系i-1的齐次变换矩阵。 把表

43、2-2中每一行的参数代入公式(2-20)中，即可得出齐次变换矩阵A1、A2和A3。因为该SCARA型工业机器人的各连杆之间的关系比较简单，可以参考图2-19(b)直接写出矩阵A1、A2和A3。我们以A1为例说明其计算方法。1系的运动过程是：先沿X0移动l1，再绕Z0转动q1，因为转动是相对固定坐标系进行的，所以，Rot(z，q1)应该左乘Trans(l1，0，0)。因此，A1、A2和A3分别为：A1Rot(z，q1)×Trans(l1，0，0)A2Rot(z，q2)×Trans(l2，0，0)A3Rot(z，q3)×Trans(l3，0，0)即：因此，可以写出(2

44、-23)式中：c123cos(q1+q2+q3)；s123sin(q1+q2+q3)；c12cos(q1+q2)；s12sin(q1+q2)；c1cosq1；s1sinq1。(在以后的叙述中，cos可用c表示，sin可用s表示。) T3表示手部坐标系3（即手部）在固定坐标系中的位置和姿态。 式(2-23)即为图2-19(a)所示的平面关节型工业机器人的正向运动学方程。 当l1、l2、l3和转角变量q1、q2、q3给定时，可以根据式(2-23)算出T3的具体数值。如图2-19(b)所示，设l1l2100，l320；q130°，q2-60°，q3-30°，则可以根据式

45、(2-23)求出手部的位姿矩阵表达式为： 2）斯坦福工业机器人的运动学方程 图2-20为斯坦福工业机器人简图及研究人员赋给各连杆的坐标系。表2-3是研究人员根据设定的坐标系得出的斯坦福工业机器人各连杆的参数。把表2-3中每一行的参数代入公式(2-20)中，即可得出齐次变换矩阵A1A6。表2-3 斯坦福工业机器人连杆参数杆 号关节转角q扭角a杆长l距离d1q1-90°002q290°0d2300°0d34q4-90°005q590°006q60°0H杆2d2Z1Z4X0、1Z0X2Z2HZ3、5X3、4、5杆1杆2杆3d3X6Z6图2-

46、20a 斯坦福机器人及连杆坐标系杆5杆6q4q5q6q2q1关节4关节5关节6关节1关节2杆1杆2杆3杆4杆5杆6Hd200图2-20b 斯坦福机器人机构简图d3关节3Z0X0Y00Z1X1Y1(a)Z2X2Y2Z1X1Y1d2q1q2(b)Z2X2Y2X3Y3Z3d3(c)图2-21 斯坦福机器人手臂坐标系之间的关系 1系与0系是旋转关节连接，如图2-21(a)所示。1系相对于0系的变换过程是：1系绕0系的X0轴作a1-90°的旋转，然后1系绕0系的Z0作变量q1的旋转，所以：(2-24) 2系与1系是旋转关节连接，连杆距离为d2，如图2-21(b)所示。2系相对于1系的变换过程是

47、：2系绕1系的X1轴作a290°的旋转，然后2系沿着1系的Z1轴正向作d2距离的平移，再绕1系的Z1轴作变量q2的旋转，所以：(2-25)X4q5Z3X3d3q4Z4X5Z5q6Z6X6H图2-22 斯坦福机器人手腕关节 3系与2系是移动关节连接，如图2-21(c)所示。坐标系3沿着坐标系2的Z2轴正向作变量d3的平移。所以：(2-26) 图2-22是斯坦福工业机器人手腕三个关节的示意图，它们都是转动关节，关节变量为q4，q5及q6，并且三个关节的中心重合。下面根据图2-23所示手腕坐标系之间的关系写出齐次变换矩阵A4A6。Z3X3Y30Z4X4Y4(a)Z5X5Y5Z4X4Y4q4

48、q5(b)Z5X5Y5X6Y6Z6H(c)图2-23 斯坦福机器人手腕坐标系之间的关系q60 如图2-23(a)所示，4系相对于3系的变换过程是：4系绕3系的X3轴作a4-90°的旋转，然后绕3系的Z3轴作变量q4的旋转，所以：(2-27) 如图2-23(b)所示，5系相对于4系的变换过程是：5系绕4系的X4轴作a590°的旋转，然后绕4系的Z4轴作变量q5的旋转，所以：(2-28) 如图2-23(c)所示，6系沿着5系的Z5轴作距离H的平移，并绕5系的Z5轴作变量q6的旋转，所以：(2-29) 这样，所有杆的A矩阵已建立。如果要知道非相邻连杆间的关系，只要用相应的A矩阵连

49、乘即可。如：3T6A4A5A62T6A3A4A5A61T6A2A3A4A5A6 斯坦福工业机器人运动学方程为：0T6A1A2A3A4A5A6(2-30) 方程(2-30)右边的结果就是最后一个坐标系手部坐标系6相对于固定坐标系0的位置和姿态矩阵，各元素均为qi和di（i=1，2，6）的函数。当qi和di给出后，可以计算出斯坦福工业机器人手部坐标系6的位置p和姿态n、o、a。这就是斯坦福工业机器人手部位姿的解，这个求解过程叫做斯坦福工业机器人运动学正解。 例2-8 斯坦福工业机器人连杆参数如表2-3所示。现已知关节变量为：q190°，q290°，d3300mm，q490

50、76;，q590°，q690°，并且，已知工业机器人结构参数d2100mm，H50mm。根据斯坦福工业机器人运动学方程式进行正向运动学求解，写出手部位置及姿态（即6系相对0系的齐次变换矩阵）。 解：设图2-20是斯坦福工业机器人的零位（起始位置），按本例给出的关节变量进行图解，工业机器人手部及各连杆状态如图2-24所示。 利用公式(2-24)、(2-25)、(2-26)、(2-27)、(2-28)、(2-29)可求得矩阵A1、A2、A3、A4、A5及A6。所以，坐标系6的位姿矩阵可根据运动学方程式(2-30)求出：a5=90°图2-24 斯坦福机器人手部及各杆件状

51、态Z6(a)X0Z0q1=90°Y0X1Z1q2=90°a1=-90°a2=90°Z2X2d3=300mmH=50mmY6(o)X6(n)q6=90°Z5X5a4=-90°Z4X3Z3q4=90°q5=90°d2=100mmX4(2-31a)式中：(2-31b)式中：ci=cosqi，si=sinqi，（i=1，2，6） 将本例给出的已知数据（q190°，q290°，d3300mm，q490°，q590°，q690°以及d2100mm，H50mm）代入式(2-31b

52、)前面九个公式可得姿态矢量n、o、a的分量分别为（注意cos90°0）：ixc1c2(c4c5c6-s4s6)-s2s5c6)-s1(s4c5c6+c4s6)0-1(0+0)0iys1c2(c4c5c6-s4s6)-s2s5c6)+c1(s4c5c6+c4s6)1(0-0)+00iz-s2(c4c5c6-s4s6)-c2s5c6-1(0-1´1)+01oxc1-c2(c4c5s6+s4c6)+s2s5s6)-s1(-s4c5s6+c4c6)0-1(0+0)0oys1-c2(c4c5s6+s4c6)+s2s5s6)+c1(-s4c5s6+c4c6)10+1´1

53、80;1+01ozs2(c4c5s6+s4c6)+c2s5s61(0+0)+00axc1(c2c4s5+s2c5)-s1s4s50-1´1´1-1ays1(c2c4s5+s2c5)+c1s4s51(0+0)+00az-s2c4s5+c2c50+00 将本例给出的已知数据代入式(2-31b)后面三个公式可得位置矢量的分量px、py、pz分别为（注意cos90°0）：pxc1c2c4s5H-s2(c5H-d3)-s1(s4s5H+d2)0-1(1´1´50+100)-150pys1c2c4s5H-s2(c5H-d3)+c1(s4s5H+d2)10-1(0-300)+0300pz-s2c4s5H+c2(c5H-d3)-0+00 根据以上计算结果，可以写出手部位姿矩阵的数值解（即6系相对0系的齐次变换矩阵）为： 该(4×4)矩阵即为斯坦福工业机器人在题目给定情况下手部的