机器人运动学（精品教程）(1)

第2章 机器人位置运动学

2.1 引言

本章将研究机器人正逆运动学。当已知所有的关节变量时，可用正运动学来确定机器人末端手的位姿。如果要使机器人末端手放在特定的点上并且具有特定的姿态，可用逆运动学来计算出每一关节变量的值。首先利用矩阵建立物体、位置、姿态以及运动的表示方法，然后研究直角坐标型、圆柱坐标型以及球坐标型等不同构型机器人的正逆运动学，最后利用Denavit-Hartenberg(D-H)表示法来推导机器人所有可能构型的正逆运动学方程。

实际上，机器手型的机器人没有末端执行器，多数情况下，机器人上附有一个抓持器。根据实际应用，用户可为机器人附加不同的末端执行器。显然，末端执行器的大小和长度决定了机器人的末端位置，即如果末端执行器的长短不同，那么机器人的末端位置也不同。在这一章中，假设机器人的末端是一个平板面，如有必要可在其上附加末端执行器，以后便称该平板面为机器人的“手”或“端面”。如有必要，还可以将末端执行器的长度加到机器人的末端来确定末端执行器的位姿。

2.2 机器人机构

机器手型的机器人具有多个自由度（DOF），并有三维开环链式机构。

在具有单自由度的系统中，当变量设定为特定值时，机器人机构就完全确定了，所有其他变量也就随之而定。如图2.1所示的四杆机构，当曲柄转角设定为120°时，则连杆与摇杆的角度也就确定了。然而在一个多自由度机构中，必须独立设定所有的输入变量才能知道其余的参数。机器人就是这样的多自由度机构，必须知道每一关节变量才能知道机器人的手处在什么位置。

图2.1 具有单自由度闭环的四杆机构

如果机器人要在空间运动，那么机器人就需要具有三维的结构。虽然也可能有二维多自由度的机

器人，但它们并不常见。

机器人是开环机构，它与闭环机构不同（例如四杆机构），即使设定所有的关节变量，也不能确保机器人的手准确地处于给定的位置。这是因为如果关节或连杆有丝毫的偏差，该关节之后的所有关节的位置都会改变且没有反馈。例如，在图2.2所示的四杆机构中，如果连杆AB偏移，它将影响O2B杆。而在开环系统中（例如机器人），由于没有反馈，之后的所有构件都会发生偏移。于是，在开环系统中，必须不断测量所有关节和连杆的参数，或者监控系统的末端，以便知道机器的运动位置。通过比较如下的两个连杆机构的向量方程，可以表示出这种差别，该向量方程表示了不同连杆之间的关系。

O1A?AB?OO12?O2B （2.1） O1A?AB?BC?OC1 （2.2）

可见，如果连杆AB偏移，连杆O2B也会相应地移动，式（2.1）的两边随连杆的变化而改变。而另一方面，如果机器人的连杆AB偏移，所有的后续连杆也会移动，除非O1C有其他方法测量，否

则这种变化是未知的。

为了弥补开环机器人的这一缺陷，机器人手的位置可由类似摄像机的装置来进行不断测量，于是机器人需借助外部手段（比如辅助手臂或激光束）来构成闭环系统。或者按照常规做法，也可通过增加机器人连杆和关节强度来减少偏移，采用这种方法将导致机器人重量重、体积大、动作慢，而且它的额定负载与实际负载相比非常小。

图2.2 (a)闭环机构；(b)开环机构

2.3 机器人运动学的矩阵表示

矩阵可用来表示点、向量、坐标系、平移、旋转以及变换，还可以表示坐标系中的物体和其他运

动元件。

2.3.1 空间点的表示

空间点P（如图2.3所示）可以用它的相对于参考坐标系的三个坐标来表示：

P?axi?byj?czk (2.3)

其中，ax,by,cz 是参考坐标系中表示该点的坐标。显然，也可以用其他坐标来表示空间点的 位置。

图2.3 空间点的表示

2.3.2 空间向量的表示

向量可以由三个起始和终止的坐标来表示。如果一个向量起始于点A，终止于点B，那么它可以表示为PAB?(Bx?Ax)i?(By?Ay)j?(Bz?Az)k。特殊情况下，如果一个向量起始于原点（如图2.4所示），则有：

P?axi?byj?czk （2.4）

其中ax,by,cz是该向量在参考坐标系中的三个分量。实际上，前一节的点P就是用连接到该点的向量来表示的，具体地说，也就是用该向量的三个坐标来表示。

图2.4 空间向量的表示

向量的三个分量也可以写成矩阵的形式，如式（2.5）所示。在本书中将用这种形式来表示运动分量：

?ax??? P?by （2.5） ????cz?? 这种表示法也可以稍做变化：加入一个比例因子w,如果x, y, z各除以w,则得到

ax,by,cz。于是，

这时向量可以写为：

?x??y?xy P???，其中ax?,by?,等等 （2.6）

?z?ww???w?

变量w可以为任意数，而且随着它的变化，向量的大小也会发生变化，这与在计算机图形学中缩放一张图片十分类似。随着w值的改变，向量的大小也相应地变化。如果w大于1，向量的所有分量都变大；如果w小于1，向量的所有分量都变小。这种方法也用于计算机图形学中改变图形与画片的大小。

如果w是1，各分量的大小保持不变。但是，如果w=0，ax,by,cz则为无穷大。在这种情况下，x，y和z（以及ax,by,cz）表示一个长度为无穷大的向量，它的方向即为该向量所表示的方向。这就意味着方向向量可以由比例因子w=0的向量来表示，这里向量的长度并不重要，而其方向由该向量的三个分量来表示。

例2.1 有一个向量P=3i+5j+2k,按如下要求将其表示成矩阵形式：

（1）比例因子为2

（2）将它表示为方向的单位向量 解：

该向量可以表示为比例因子为2的矩阵形式，当比例因子为0时，则可以表示为方向向量，结果如下：

?6??3??10??5?P??? 和 P???

?4??2?????2???0?然而，为了将方向向量变为单位向量，须将该向量归一化使之长度等于1。这样，向量的每一个分量都要除以三个分量平方和的开方：

??22PX2?P?6.16,其中Px?Y?PZ35，等等和

?0.487,Py?6.166.16?0.487??0.811?? Punit???0.324???0??2.3.3 坐标系在固定参考坐标系原点的表示

一个中心位于参考坐标系原点的坐标系由三个向量表示，通常着三个向量相互垂直，称为单位向量n,o,a，分别表示法线（normal）、指向（orientation）和接近（approach）向量（如图2.5所示）。正如2.3.3节所述，每一个单位向量都由它们所在参考坐标系着的三个分量表示。这样，坐标系F可以由三个向量以矩阵的形式表示为：

?nx? F?ny???nzoxoyozax?ay?? （2.7） az??

图2.5 坐标系在参考坐标系原点的表示

2.3.4 坐标系在固定参考坐标系中的表示

如果一个坐标系不再固定参考坐标系的原点（实际上也可包括在原点的情况），那么该坐标系的原点相对于参考坐标系的位置也必须表示出来。为此，在该坐标系原点与参考坐标系原点之间做一个向量来表示该坐标系的位置（如图2.6所示）。这个向量由相对于参考坐标系的三个向量来表示。这样，这个坐标系就可以由三个表示方向的单位向量以及第四个位置向量来表示。

?nx?ny F???nz??0oxoyoz0axayaz0px?py?? （2.8） pz??1?

图2.6 一个坐标系在另一个坐标系中的表示

如式（2.8）所示，前三个向量是w=0的方向向量，表示该坐标系的三个单位向量n,o,a的方向，而第四个w=1的向量表示该坐标系原点相对于参考坐标系的位置。与单位向量不同，向量P的长度十分重要，因而使用比例因子为1。坐标系也可以由一个没有比例因子的3?4矩阵表示，但不常用。 例2.2 如图2.7所示的F坐标系位于参考坐标系中3，5，7的位置，它的n轴与x轴平行，o轴相对

于y轴的角度为45°，a轴相对于z轴的角度为45°。该坐标系可以表示为：

00?1?00.707?0.707 F???00.7070.707?00?03?5?? 7??1?

图2.7 坐标系在空间的表示举例

2.3.5 刚体的表示

一个物体在空间的表示可以这样实现：通过在它上面固连一个坐标系，再将该固连的坐标系在空间表示出来。由于这个坐标系一直固连在该物体上，所以该物体相对于坐标系的位姿是已知的。因此，只要这个坐标系可以在空间表示出来，那么这个物体相对于固定坐标系的位姿也就已知了（如图2.8所示）。如前所述，空间坐标系可以用矩阵表示，其中坐标原点以及相对于参考坐标系的表示该坐标系姿态的三个向量也可以由该矩阵表示出来。于是有：

Fobject?nx?n??y?nz??0oxoyoz0axayaz0px?py?? （2.9） pz??1?如第1章所述，空间中的一个点只有三个自由度，它只能沿三条参考坐标轴移动。但在空间的一个钢体有六个自由度，也就是说，它不仅可以沿着X,Y,Z三轴移动，而且还可绕这三个轴转动。因此，要全面地定义空间以物体，需要用6条独立的信息来描述物体原点在参考坐标系中相对于三个参考坐标轴的位置，以及物体关于这三个坐标轴的姿态。而式（2.9）给出了12条信息，其中9条为姿态信息，三条为位置信息（排除矩阵中最后一行的比例因子，因为它们没有附加信息）。显然，在该表达式中必定存在一定的约束条件将上述信息数限制为6。因此，需要用6个约束方程将12条信息减少到6条信息。这些约束条件来自于目前尚未利用的已知的坐标系特性，即：

?三个向量n,o,a相互垂直 ?每个单位向量的长度必须为1

图2.8 空间物体的表示

我们可以将其转换为以下六个约束方程：

（1）n?o?0

（2）n?a?0 （3）a?o?0

（4）n?1（向量的长度必须为1） （2.10） （5）o?1

（6）a?1

因此，只有前述方程成立时，坐标系的值才能用矩阵表示。否则，坐标系将不正确。式（2.10）中前三个方程可以换用如下的三个向量的叉积来代替：

n?o?a （2.11）

例2.3 对于下列坐标系，求解所缺元素的值，并用矩阵来表示这个坐标系。

0??5???0.707???3?F?????0?2??0001 ??

解：

显然，表示坐标系原点位置的值5，3，2对约束方程无影响。注意在三个方向向量中只有三个值是给定的，但这也已足够了。根据式（2.10），得： nxox?nyoy?nzo??0.70oy7(?n)( )z0 或 nx(0)zoz? nxax?nyay?nzaz?0 或 nx(ax)?0.70a7y(?n)z axox?ayoy?azoz?0 或 ax(0?)ay(oy?)?( 0)0o(z?)

nx2?ny2?nz2?1 或 nx2?0.7027?nz2? 1 ox2?oy2?oz2?1 或 02?oy2?oz2?1 ax2?ay2?az2?1 或 ax2?ay2?02?1

将这些方程化简得：

0.707oy?nzoz?0nxax?0.707ay?0

ayoy?0

nx2?nz2?0.5

oy2?oz2?1

ax2?ay2?1

解这六个方程得：nx??0.707,nz?0,oy?0,oz?1,ax??0.707和ay??0.707。应注意，nx和ax必须同号。非唯一解的原因是由于给出的参数可能得到两组在相反方向上相互垂直的向量。最终得到的矩阵如下：

?0.70?0.70700.70?7??0.7070?7?0.7070?73?0.7070?73?0.70?0.70??F??F???0?2?0?21010????00010001?? 或 ??

由此可见，两个矩阵都满足约束方程的要求。但应注意三个方向向量所表述的值不是任意的，而是受这些约束方程的约束，因此不可任意给矩阵赋值。

同样，可通过n与o叉乘并令其等于a，即n?o?a来求解，表示如下：

inxoxjnyoyknz?axi?ayj?azkoz

或表示为：i(nyoz?nzoy)?j(nxoz?nzox)?k(nxoy?nyox)?axi?ayj?azk 将值代入方程得：

i(0.70o?n(o(nyo?)x?aiy?a jz7zoy?)jnxz?)kx 同时解下面这三个方程：

0.70o7oy?az?nz ?nxoz?anxoy?0ykx

以此来代替三个点乘方程。联立三个单位向量长度的约束方程，便得到六个方程，求解这六个方程，

可得到相同的未知参数的解。

2.4 齐次变换矩阵

由于各种原因，变换矩阵应写成方型形式，3?3或4?4均可。首先，如后面看到的，计算方形矩

阵的逆要比计算长方形矩阵的逆容易得多。其次，为使两矩阵相乘，它们的维数必须匹配，即第一矩阵的列数必须与第二矩阵的行数相同。如果两矩阵是方阵则无上述要求。由于要以不同顺序将许多矩阵乘在一起得到机器人运动方程，因此，应采用方阵进行计算。 为保证所表示的矩阵为方阵，如果在同一矩阵中既表示姿态又表示位置，那么可在矩阵中加入比例因子使之成为4?4矩阵。如果只表示姿态，则可去掉比例因子得到3?3矩阵，或加入第四列全为0的位置数据以保持矩阵为方阵。这种形式的矩阵称为齐次矩阵，它们写为：

?nx?n F??y?nz??0oxoyoz0axayaz0p?x?p?y （2.12） ?zp?1?2.5 变换的表示

变换定义为空间的一个运动。当空间的一个坐标系（一个向量、一个物体或一个运动坐标系）相

对于固定参考坐标系运动时，这一运动可以用类似于表示坐标系的方式来表示。这是因为变换本身就

是坐标系状态的变化（表示坐标系位姿的变化），因此变换可以用坐标系来表示。变换可为如下几种形式中的一种： ?纯平移

?绕一个轴的纯旋转 ?平移与旋转的结合

为了解它们的表示方法，我们将逐一进行探讨。 2.5.1 纯平移变换的表示

如果一坐标系（它也可能表示一个物体）在空间以不变的姿态运动，那么该坐标就是纯平移。在这种情况下，它的方向单位向量保持同一方向不变。所有的改变只是坐标系原点相对于参考坐标系的变化，如图2.9所示。相对于固定参考坐标系的新的坐标系的位置可以用原来坐标系的原点位置向量加上表示位移的向量求得。若用矩阵形式，新坐标系的表示可以通过坐标系左乘变换矩阵得到。由于在纯平移中方向向量不改变，变换矩阵T可以简单地表示为：

?1?0 T???0??001000dx?0dy?? （2.13） 1dz??01?

图2.9 空间纯平移变换的表示

其中dx,dy,dz是纯平移向量d相对于参考坐标系x,y,z轴的三个分量。可以看到，矩阵的前三列表示没有旋转运动（等同于单位阵），而最后一列表示平移运动。新的坐标系位置为：

?1?0 Fnew???0??00dx??nxoxaxpx??nxoxax?pxd?x???n?0dy?noapoa?pdyyyyyyyy?????? ? y （2.14）

?z1dz??nzozazpz??nzozaz?pzd?????01001?0001??0??这个方程也可用符号写为：

Fnew?Trans(dx,dy,dz)?Fold （2.15） 首先，如前面所看到的，新坐标系的位置可通过在坐标系矩阵前面左乘变换矩阵得到，后面将看到，无论以何种形式，这种方法对于所有的变换都成立。其次可以注意到，方向向量经过纯平移后保持不变。但是，新的坐标系的位置是d和P向量相加的结果。最后应该注意到，齐次变换矩阵与矩阵乘法的关系使得到的新矩阵的维数和变换前相同。

例2.4 坐标系F沿参考坐标系的x轴移动9个单位，沿z轴移动5个单位。求新的坐标系位置。

0100?0.527?0.5740.6285??0.3690.8190.4393??F????0.76600.6438???001? ?0解：由式（2.14）或式（2.15）得：

Fnew?Trans(dx,dy,dz)?FoldTrans(9,0,5)?Fold= 和 ?10?01F???00??00?0.527?0.369????0.766??009??0.527?0.5270.6285??0.3690.8190.4393?00?????15???0.76600.6438????01??0001??0.5740.62814?0.8190.4393??00.64313??001?

2.5.2 绕轴纯旋转变换的表示

为简化绕轴旋转的推导，首先假设该坐标系位于参考坐标系的原点并且与之平行，之后将结果推广到其他的旋转以及旋转的组合。

假设坐标系（n,o,a）位于参考坐标系（x,y,z）的原点，坐标系（n,o,a）绕参考坐标系的x轴旋转一个角度?，再假设旋转坐标系（n,o,a）上有一点P相对于参考坐标系的坐标为Px,Py,Pz，相对于运动坐标系的坐标为Pn,Po,Pa。当坐标系绕x轴旋转时，坐标系上的点P也随坐标系一起旋转。在旋转之前，P点在两个坐标系中的坐标是相同的（这时两个坐标系位置相同，并且相互平行）。旋转后，该点坐标Pn,Po,Pa在旋转坐标系（x ,y ,z）中保持不变，但在参考坐标系中Px,Py,Pz却改变了（如图2.10

所示）。现在要求找到运动坐标系旋转后P相对于固定参考坐标系的新坐标。

让我们从x轴来观察在二维平面上的同一点的坐标，图2.10显示了点P在坐标系旋转前后的坐标。点P相对于参考坐标系的坐标是Px,Py,Pz，而相对于旋转坐标系（点P所固连的坐标系）的坐标仍为Pn,Po,Pa。

由图2.11可以看出，Px不随坐标系统x轴的转动而改变，而Py和Pz却改变了，可以证明：

Px?PnPy?l1?l2?P0cos??Pasin? （2.16） Pz?l3?l4?P0sin??Pacos?写成矩阵形式为：

0?Px??1??? Py?0cos??????Pz????0sin???Pn??P? （2.17）

?sin????o?cos?????Pa??0

图2.10 在坐标系旋转前后的点的坐标

图2.11 相对于参考坐标系的点的坐标和从x轴上观察旋转坐标系

可见，为了得到在参考坐标系中的坐标，旋转坐标系中的点P（或向量P）的坐标必须左乘旋转矩阵。这个旋转矩阵只适用于绕参考坐标系的x轴做纯旋转变换的情况，它可表示为：

Pxyz?Rot(x,?)?Pnoa （2.18）

注意在式（2.17）中，旋转矩阵的第一列表示相对于x轴的位置，其值为1，0，0，它表示沿x轴的坐标没有改变。

为简化书写，习惯用符号C?表示cos?以及用S?表示sin?。因此，旋转矩阵也可写为：

?10 Rot(x,?)???0C???0S??10? Rot(x,?)?0C????0S?0??S??? （2.19） C???0??S??? （2.20） C???u可用同样的方法来分析坐标系绕参考坐标系y轴和z轴旋转的情况，可以证明其结果为：

0??10??S?? 和 Rot(x,?)?0C????C????0S?式（2.18）也可写为习惯的形式，以便于理解不同坐标系间的关系，为此，可将该变换表示为TR

RP

(读作坐标系R相对于坐标系U（Universe）的变换)，将Pnoa表示为P（P相对于坐标系R），将xyz表

u示为P（P相对于坐标系U），式（2.18）可简化为：

uP?uTR?RP （2.21） 由上式可见，去掉R便得到了P相对于坐标系U的坐标。

例2.5 旋转坐标系中有一点P?234?，此坐标系绕参考坐标系x轴旋转90度。求旋转后该点相对于参考坐标系的坐标，并且用图解法检验结果。

解：

由于点P固连在旋转坐标系中，因此点P相对于旋转坐标系的坐标在旋转前后保持不变。该点相对于参考坐标系的坐标为：

0?Px??1?P???0cos??y????Pz????0sin???Pn??100??2??2??P???00?1??3????4?

?sin????o???????cos?????Pa????010????4????3??0T如图2.12所示，根据前面的变换，得到旋转后P点相对于参考坐标系的坐标为2，-4，3.

图2.12 相对于参考坐标系的坐标系旋转

2.5.3 复合变换的表示

复合变换是由固定参考坐标系或当前运动坐标系的一系列沿轴平移和绕轴旋转变换所组成的。任何变换都可以分解为按一定顺序的一组平移和旋转变换。例如，为了完成所要求的变换，可以先绕x轴旋转，再沿x,y,z轴平移，最后绕y轴旋转。在后面将会看到，这个变换顺序很重要，如果颠倒两个依次变换的顺序，结果将会完全不同。

为了探讨如何处理复合变换，假定坐标系（n,o,a）相对于参考坐标系（x,y,z）依次进行了下面三个变换：

（1）绕x轴旋转?度；

（2）接着平移?l1l2l3?（分别相对于x,y,z轴）； （3）最后绕y轴旋转?度。

比如点Pnoa固定在旋转坐标系，开始时旋转坐标系的原点与参考坐标系的原点重合。随着坐标系

（n,o,a）相对于参考坐标系旋转或者平移时，坐标系中的P点相对于参考坐标系也跟着改变。如前

面所看到的，第一次变换后，P点相对于参考坐标系的坐标可用下列方程进行计算。

P1,xyz?Rot(x,?)?Pnoa （2.22）

其中，P1,xyz是第一次变换后该点相对于参考坐标系的坐标。第二次变换后，该点相对于参考坐标

系的坐标是：

P?Tra(n1,sl2l,?3l)1,xyP?2x,yzzTra(n1,sl2?,l3)l?R?(o,tx) noaP同样，第三次变换后，该点相对于参考坐标系的坐标为：

Pxyz?P,l2,l3)?Rot(x,?)?P3,xyz?Rot(y,?)?P2,xyz?Rot(y,?)?Trans(l1noa

可见，每次变换后该点相对于参考坐标系的坐标都是通过用每个变换矩阵左乘该点的坐标得到

的。当然，矩阵的顺序不能改变。同时还应注意，对于相对于参考坐标系的每次变换，矩阵都是左乘的。因此，矩阵书写的顺序和进行变换的顺序正好相反。

例2.6 固连在坐标系（n,o,a）上的点P?732?经历如下变换，求出变换后该点相对于参考坐标系的坐标。

(1)绕z轴旋转90度；

(2)接着绕y轴旋转90度； (3)接着再平移[4，-3，7]。 解：

表示该变换的矩阵方程为：

Pxyz?Trans(4,?3,7)Rot(y,90)Pnoa??1?0??0??04??0?010?3????017???1??001??000010??0?1?10100????000??00??001??0000??7??6??3??4?00????????10??2??10??????01??1??1?

T

从图2.13可以看到，（n,o,a）坐标系首先绕z轴旋转90度，接着绕y轴旋转，最后相对于参考坐标系的x,y,z轴平移。坐标系中的P点相对于n,o,a轴的位置如图所示，最后该点在x,y,z轴上的坐标分别为4+2=6，-3+7=4，7+3=10。请确认也能从图中理解上述结果。

图2.13 三次顺序变换的结果

例2.7 根据上例，假定(n,o,a)坐标系上的点P?732?经历相同变换，但变换按如下不同顺序进行，求出变换后该点相对于参考坐标系的坐标。

（1）绕z轴旋转90度； （2）接着平移[4，-3，7]； （3）接着再绕y轴旋转90度。 解：

表示该变换的矩阵方程为：

Pxyz?Rot(y,90)Trans(4,?3,7)Rot(z,90)Pnoa??0?0???1??0010??1?0100????000??0??001??04??0?1?1010?3????017??00??001??000000??7??9??3??4?00????????10??2???1??????01??1??1?T

不难发现，尽管所有的变换与例2.6完全相同，但由于变换的顺序变了，该点最终坐标与前例完全不同。用图2.14可以清楚地说明这点，这时可以看出，尽管第一次变换后坐标系的变化与前例完全相同，但第二次变换后结果就完全不同，这是由于相对于参考坐标系轴的平移使得旋转坐标系

?n,o,a?向外移动了。经第三次变换，该坐标系将绕参考坐标系y轴旋转，因此向下旋转了，坐标系

上点P的位置也显示在图中。

请证明该点相对于参考坐标系的坐标为7+2=9，-3+7=4和-4+3=-1，它与解析的结果相同。

图2.14 变换的顺序改变将改变最终结果

2.5.4 相对于旋转坐标系的变换

到目前为止，本书所讨论的所有变换都是相对于固定参考坐标系。也就是说，所有平移、旋转和距离（除了相对于运动坐标系的点的位置）都是相对参考坐标系轴来测量的。然而事实上，也有可能做相对于运动坐标系或当前坐标系的轴的变换。例如，可以相对于运动坐标系（也就是当前坐标系）的n轴而不是参考坐标系的x轴旋转90度。为计算当前坐标系中的点的坐标相对于参考坐标系的变化，这时需要右乘变换矩阵而不是左乘。由于运动坐标系中的点或物体的位置总是相对于运动坐标系

测量的，所以总是右乘描述该点或物体的位置矩阵。

例2.8 假设与例2.7中相同的点现在进行相同的变换，但所有变换都是相对于当前的运动坐标系，具体变换出如下。求出变换完成后该点相对于参考坐标系的坐标。 （1）绕a轴旋转90度；

（2）然后沿n,o,a轴平移[4，-3，7]； （3）接着绕o轴旋转90度。

解：

在本例中，因为所作变换是相对于当前坐标系的，因此右乘每个变换矩阵，可得表示该坐标的方程为：

Pxyz?Rot(a,90)Trans(4,?3,7)Rot(o,90)Pnoa??0?1?10??00??0000100??1?00????0??0??1??0010004??0?00?3????17???1??01??0010010000??7??0??3??6?0????????0??2??0??????1??1??1?

如所期望的，结果与其他各例完全不同，不仅因为所作变换是相对于当前坐标系的，而且也因为矩阵顺序的改变。下面的图展示了这一结果，应注意它是怎样相对于当前坐标来完成这个变换的。

同时应注意，在当前坐标系中p点的坐标7，3，2是变换后得到相对于参考坐标系的坐标0，6，0的。

图2.15 相对于当前坐标系的变换

例2.9 坐标系B绕x轴旋转90°，然后沿当前坐标系a轴做了3英寸的平移，然后在绕z轴旋转90，最后沿当前坐标系o轴做5英寸的平移。

(a)写出描述该运动的方程。

(b)求坐标系中的点p（1，5，4）相对于参考坐标系的最终位置。 解：

在本例中，相对于参考坐标系以及当前坐标系的运动是交替进行的。

(a)相应地左乘或右乘每个运动矩阵，得到：

uTB=Rot（z,90)Rot(x,90)Trans(0,0,3)Trans(0,5,0)

?0?1?10UUBP?TB?P???00??0000??1?000???10??0??01??00??1?00?10???100??0??001??000000??1?0100???013??0??001??0000?105??010??001?

(b)带入具体的矩阵并将它们相乘，得到：

?0?1???0? ?0001010007??1?1?0?1?

2.6 变换矩阵的逆

正如前面所提到的，在机器人分通过析中有很多地方要用到矩阵的逆，在下面的例子中可以看到

一种涉及变换矩阵的情况。在图2.16中，假设机器人要在零件p上钻孔而须向零件p处移动。机器

人基座相对于参考坐标系u的位置用坐标系R来描述，机器人手用坐标系H来描述，末端执行器（即用来钻孔的钻头的末端）用坐标系E来描述，零件的位置用坐标系P来描述。钻孔的点的位置于参考坐标系U可以通过两个独立的路径发生联系：一个是通过该零件的路径，另一个是通过机器人的路径。因此，可以写出下面的方程：

URHT?TTT?ERHE

UUTPP T E （2.24）

这就是说，该零件中点E的位置可以通过从U变换到P，并从P变换到E来完成，或者从U变换到

R，从R变换到H，再从H变换到E。

图2.16 全局坐标系、机器人坐标系、手坐标系、零件坐标系及末端执行器坐标系

事实上，由于在任何情况下机器人的基座位置在安装时就是已知的，因此变换UTR(坐标系R相对于坐标系U的变换)时已知的。比如，一个机器人安装在一个工作台上，由于它被紧固在工作台上，所以它的基座的位置时已知的。即使机器人时可移的或放在传送带上，因为控制器始终掌控着机器人基座的运动，因此它在任意时刻的位置也是已知的。由于用于末端执行器的任何器械都是已知的，而且其尺寸和结构也是已知的，所以HTE（机器人末端执行器相对于机器人手的变换）也是已知的。此外，

UTP（零件相对于全局坐标系的变换）也是已知的，还必须要知道将在其上面钻孔的零件的位置，该

位置可以通过将该零件放在钻模上，让后用照相机，视觉系统，传送带，传感器或其他类似仪器来确

定。最后需要知道零件上钻孔的位置，所以PTE也是已知的。此时，唯一未知的变换就是RTH（机器人手相对于机器人基座的变换）。因此，必须找出机器人的关节变量（机器人旋转关节的角度以及滑动关节的连杆长度），以便将末端执行器定位在要钻孔的位置上。可见，必须要计算出这个变换，它指出机器人需要完成的工作。后面将用所求出的变换来求解机器人关节的角度和连杆的长度。

不能像在代数方程中那样来计算这个矩阵，即不能简单的用方程的右边除以方程的左边，而应该用合适的矩阵的逆并通过左乘或右乘来将它们从左边去调。因此有：

(UTR)?1(UTRRTHHTE)(HTE)?1?(UTR)?1(UTPPTE)(HTE)?1 （2.25）

由于(UTR)?1(UTR)?1和(HTE)(HTE)?1?1，式（2.25）的左边可简化为RTH,于是得：

RTH?UTR?1UTPPTEHTE?1 （2.26）

EH?1T(T)HE该方程的正确性可以通过认为与相同来加以检验。因此，该方程可重写为：

RTH?UTR?1UTPPTEHTE?1?RTUUTPPTEETH?RTH （2.27）

显然为了对机器人运动学进行分析，需要能够计算变换矩阵的逆。

我们来看看关于x轴的简单旋转矩阵的求逆计算情况。关于x轴的旋转矩阵是：

00??1??x)??0?C??S （2.28） Ro(t,????0S?C???必须采用一下的步骤来计算矩阵的逆：

?计算矩阵的行列式； ?将矩阵转置；

?将转置矩阵的每个元素用它的子行列式（伴随矩阵）代替； ?用转换后的矩阵除以行列式。 将上面的步骤用到该旋转，得到：

??1(C2??S2?)?0?1

00??1??xT)??0?C?S Ro(t,????0?S?C???现在计算每一个子行列式（伴随矩阵）。例如，元素2，2的子行列式是C??0?C?，元素1，1的子行列式C2??S2??1。可以注意到，这里的每一个元素的子行列式与其本身相同，因此有：

Rot(x,?T)mior(?,T )n?Rotx由于原旋转矩阵的行列式为1，因此用Rot(x,?)Tminor矩阵除以行列式仍得出相同的结果。因此，关于x轴的旋转矩阵的逆的行列式与它的转置矩阵相同，即：

Rot(x,?)?1?Rot(x,?)T （2.29）

当然，如果采用附录A中提到的第二种方法也能得到同样的结果。具有这种特征的矩阵称为酉

矩阵，也就是说所有的旋转矩阵都是酉矩阵。因此，计算旋转矩阵的逆就是将该矩阵转置。可以证明，关于y轴和z轴的旋转矩阵同样也是酉矩阵。

应注意，只有旋转矩阵才是酉矩阵。如果一个矩阵不是一个简单的旋转矩阵，那么它也许就不是

酉矩阵。

以上结论只对简单的不表示位置的3×3旋转矩阵成立。对一个齐次的4×4变换矩阵而言，它的求逆可以将矩阵分为两部分。矩阵的旋转部分仍是酉矩阵，只需简单的转置；矩阵的位置部分是向量

P分别与n,o,a向量点积的负值，其结果为：

?nx?nF??y?nz??0?nx??oF??x?ax??0oxoyoz0nyoyay0axayaz0nzozaz0px?py?? （2.30） pz??1??P?n???P?o?? ?P?a?1??如上所示，矩阵的旋转部分是简单的转置，位置的部分由点乘的负值代替，而最后一行（比例因

子）则不受影响。这样做对于计算变换矩阵的逆是很有帮助的，而直接计算4×4矩阵的逆是一个很冗长的过程。

例2.10 计算表示Rot(x,40?)?1的矩阵

解：绕x轴旋转40°的矩阵为：

00?1?00.766?0.643Rot(x,40?)???00.6430.766?00?0其矩阵的逆是：

0?0?? 0??1?000??1?00.7660.6430?? Rot(x,40?)?1???0?0.6430.7660???0001?? 需注意的是，由于矩阵的位置向量为0，它与n,o,a向量的点积也为零。 例2.11 计算如下变换矩阵的逆。

?0.5?0.866T???0??0解：

根据先前的计算，变换矩阵的逆是：

00.8663?0?52??105??001?

?0.50.8660?(3?0.5?2?0.866?5?0)??0?01?(3?0?2?0?5?1)?1?T???0.866?0.50?(3?0.866?2??0.5?5?0)???0001??

0.866?0?0.5?3.2?0?01?5????0.866?0.5?0?5981.??0001? ?可以证明TT是单位阵。

例2.12 一个具有六个自由度的机器人的第五个连杆上装有照相机，照相机观察物体并测定它相对于

照相机坐标系的位置，然后根据以下数据来确定末端执行器要到达物体所必须完成的运动。

?15Tcam?00?1?0?10????100??000?0?1???0??0001010003??0?1?100?? 5TH???005???1??00010000100010?0?0? ?4??1?0?0? ?3??1camTobj2??1?02?? HTE???04???1??0解：

参照式（2.24），可以写出一个与它类似的方程，它将不同的变换和坐标系联系在一起。

RT5?5TH?HTE?ETobj?RT5?5Tcam?camTobj

由于方程两边都有RT5，所以可以将它消去。除了ETobj之外所有其他矩阵都是已知的，所以：

ETobj?HTE?1?5TH?1?5Tcam?camTobj?ETH?HT5?5Tcam?camTobj0100100000?00??1?4??01? 3??0?10???5??0??1??0001010002?2??4??1?

00??1???1005?TH?1???01??3??01? ?0将矩阵及矩阵的逆代入前面的方程，得：

?1?0???0??0??1?0???0??0010000??0??100???1?3??0??01??01000?1?0HTE?1???0??0ETobj00??00?1?0?1000???1?4???100??01??000ETobj00?2?101??0?1?4??001?

2.7 机器人的正逆运动学

假设有一个构型已知的机器人，即它的所有连杆长度和关节角度都是已知的，那么计算机器人手的位姿就称为正运动学分析。换言之，如果已知所有机器人关节变量，用正运动学方程就能计算任一瞬间机器人的位姿。然而，如果想要将机器人的手放在一个期望的位姿，就必须知道机器人的每一个连杆的长度和关节的角度，才能将手定位在所期望的位姿，这就叫做逆运动学分析，也就是说，这里

不是把已知的机器人变量代入正向运动学方程中，而是要设法找到这些方程的逆，从而求得所需的关节变量，使机器人放置在期望的位姿。事实上，逆运动学方程更为重要，机器人的控制器将用这些方程来计算关节值，并以此来运行机器人到达期望的位姿。下面首先推导机器人的正运动学方程，然后利用这些方程来计算逆运动学方程。

对正运动学，必须推导出一组与机器人特定构型（将构件组合在一起构成机器人的方法）有关的方程，以使得将有关的关节和连杆变量代入这些方程就能计算出机器人的位姿，然后可用这些方程推出逆运动学方程。

根据第一章中的相关内容，要确定一个刚体在空间的位姿，须在物体上固连一个坐标系，然后描述该坐标系的原点位置和它三个轴的姿态，总共需要六个自由度或六条信息来完整地定义该物体的位姿。同理，如果要确定或找到机器人手在空间的位姿，也必须在机器人手上固连一个坐标系并确定机器人手坐标系的位姿，这正是机器人正运动学方程所要完成的任务。换言之，根据机器人连杆和关节的构型配置，可用一组特定的方程来建立机器人手的坐标系和参考坐标系之间的联系。图2.17所示为机器人手的坐标系、参考坐标系以及它们的相对位姿，两个坐标系之间的关系与机器人的构型有关。当然，机器人可能有许多不同的构型，后面将会看到将如何根据机器人的构型来推导出与这两个坐标系相关的方程。

为使过程简化，可分别分析位置和姿态问题，首先推导出位置方程，然后再推导出姿态方程，再将两者结合在一起而形成一组完整的方程。最后，将看到关于Denavit-Hartenberg表示法的应用，该方法可用于对任何机器人构型建模。

图2.17 机器人的手坐标系相对于参考坐标系

2.7.1 位置的正逆运动学方程

对于机器人的定位，可以通过相对于任何惯用坐标系的运动来实现。比如，基于直角坐标系对空间的一个点定位，这意味着有三个关于x,y,z轴的线性运动，此外，如果用球坐标来实现，就意味着需要有一个线性运动和两个旋转运动。常见的情况有：

(a)笛卡尔（台架，直角）坐标 (b)圆柱坐标 (c)球坐标

(d)链式(拟人或全旋转)坐标 2.7.1.1 笛卡尔(台架，直角)坐标

这种情况下有三个沿x,y,z轴的线性运动，这一类型的机器人所有的驱动机构都是线性的（比如液压活塞或线性动力丝杠），这时机器人手的定位是通过三个线性关节分别沿三个轴的运动来完成的（如图2.18所示）。台架式机器人基本上就是一个直角坐标机器人，只不过是将机器人固连在一个朝下的直角架上。IBM7565机器人就是一个台架式直角坐标机器人。

图2.18 直角坐标

当然，如果没有旋转运动，表示向P点运动的变换矩阵是一种简单的平移变换矩阵，下面将可以看到这一点。注意这里只涉及坐标系原点的定位，而不涉及姿态。在直角坐标系中，表示机器人手位置的正运动学变换矩阵为：

RTP?Tcart?1?0???0??001000Px?0Py?? （2.31） 1Pz??01?其中RTP是参考坐标系与手坐标系原点P的变换矩阵，而Tcart表示直角坐标变换矩阵。对于逆运动学的求解，只需简单地设定期望的位置等于P.

例2.13 要求笛卡尔坐标机器人手坐标系原点定位在点P=?347?，计算所需要的笛卡儿坐标运动。

解：

设定正运动学方程用式（2.31）中的RTP矩阵表示，根据期望的位置可得知如下结果：

T?1?0RTP???0??001000Px??1?00Py????1Pz??0??01??0010000103?4??或Px?3,Py?4,Pz?77??1?

2.7.1.2 圆柱坐标

圆柱型坐标系统包括两个线性平移运动和一个旋转运动。其顺序为：先沿x轴移动r,再绕z轴旋转?角，最后沿z轴移动l，如图2.19所示。这三个变换建立了手坐标系与参考坐标系之间的联系。由于这些变换都是相对于全局参考坐标系的坐标轴的，因此由这三个变换所产生的总变换可以通过依次左乘每一个矩阵而求得：

RTP?Tcyl(r,?,l)?Trans(0,0,l)Rot(z,?)Trans(r,0,0) （2.32）

010000100??C??S?0????l??0??1??0?S?C?0000100??1?00????0??0??1??001000010r?0?? （2.33） 0??1??1?0RTP???0??0

0rC??0rS???1l??01?

经过一系列变换后，前三列表示了坐标系的姿态，然而我们只对坐标系的原点位置即最后一列感兴趣。显然，在圆柱型坐标运动中，由于绕z轴旋转了?角，运动坐标系的姿态也将改变，这一改变将在后面讨论。

实际上，可以通过绕n,o,a坐标系中的a轴旋转??角度使坐标系回转到和初始参考坐标系平行的状态，它等效于圆柱坐标矩阵右乘旋转矩阵(a,??)，其结果是，该坐标系的位置仍在同一地方，但其姿态再次平行于参考坐标系，如下所示：

?C??S?RTP?Tcyl???0??0?S?C?00

图2.19 圆柱坐标

?C??S?Tcyl?Rot(z,??)???0??0?S?C?000rC???C(??)?S(??)?S(??)C(??)0rS?????1l??00??01??0000??1?000????10??0??01??000rC??10rS???01l?

?001?由此可见，运动坐标系的原点位置没有改变，但它转回到了与参考坐标系平行的状态。需注意的是，

最后的旋转是绕本地坐标系的a轴的，其目的是为了不引起坐标系位置的任何改变，而只改变姿态。

例2.14 假设要将圆柱坐标机器人手坐标系的原点放在?347?,计算该机器人的关节变量。

解：

根据式（2.33）的Tcyl矩阵，将手坐标系原点的位置分量设置为期望值，可以得到：

l?7T rC??3

rS??44于是有： tan??和??53.1?

3将?代入其中任何一个方程，可得r?5，最终结果是：

??53.?l1?, r?5,应注意：必须确保在机器人运动学中计算的角度位于正确的象限。在这个例子中，请注意rC?和rS?都是正的，并且r也是正的，这样角度?便在第一象限，且为53.1°。

2.7.1.3 球坐标

如图2.20所示，球坐标系统由一个线性运动和两个旋转运动组成，运动顺序为：先沿z轴平移r,再绕y轴旋转?并绕z轴旋转?。这三个变换建立了手坐标系与参考坐标系之间的联系。由于这些变换都是相对于全局参考坐标系的坐标轴的，因此有这三个变换所产生的总变换可以通过一次左乘每一个矩阵而求得：

RTP?Tsph?r,?,???Rot?z,??Rot?y,??Trans?0,0,?? (2.34)

?S?C?0000100??C??00????0???S???1??0?C??C??C??S?????S???00S?100C?00?S?C?000??1?00????0??0??1??0S??C?S??S?C?0010000100?0?? r??1??C??S?RTP???0??0RTP?TsphrS??C??rS??S??? (2.35) rC???1?

图2.20 球坐标

前三列表是了经过一系列变换后的坐标系的姿态，而最后一列则表示了坐标系原点的位置。以后还要进一步讨论该矩阵的姿态部分。 这里也可回转最后一个坐标系，使它与参考坐标系平行。这一问题将作为练习留给读者，要求找出正确的运动顺序来获得正确的答案。

球坐标的逆运动学方程比简单的直角坐标和圆柱坐标更复杂，因为两个角度?和?是耦合的。让

我们通过下例来说明如何求解球坐标的逆运动学方程。

例2.15 假设要将球坐标机器人手坐标系原点放在?3,4,7?，计算机器人的关节变量。

T解：

根据式（2.35）的Tsph矩阵，将手坐标系原点的位置分量设置为期望值，可以得到：

rS?C?=3

rS?S?=4

rC?=7

由第三个方程，我们得出C?是正数，但没有关于S?是正或负的信息。将前两个方程彼此相除，

因为不知道S?的实际符号是什么，因此可能会有两个解。下面的方法给出了两个可能的解，后面还必须对这最后的结果进行检验以确保它们是正确的。

4?=53.1? 或 233.1? tan?=?

3

S?=0.8 或 -0.8 C?=0.6 或 -0.6

3=5 0.6

?S?= 或 -5

?C?=7 ?=35.5? 或 -35.5? ?=8.6

可以对这两组解进行检验并正是这两组解都能满足所有的位置方程。如果沿给定的三维坐标轴旋转这些角度，物理上的确能到达同一点。然而必须注意，其中只有一组解能满足姿态方程。换句话说，前两种解将产生同样的位置，但处于不同的姿态。由于目前并不关心手坐标系在这点的姿态，因此两个位置解都是正确的。实际上，由于不能对三自由度的机器人指定姿态，所以无法确定两个解中哪一个和特定的姿态有关。 2.7.1.4 链式坐标 如图2.21所示，链式坐标由三个旋转组成。后面在讨论Denavit-Hartenberg表示法时，将推导链式坐标的矩阵表示法。

图2.21 链式坐标

2.7.2 姿态的正逆运动学方程

假设固连在机器人手上的运动坐标系已经运动到期望的位置上，但它仍然平行于参考坐标系，或者假设其姿态并不是所期望的，下一步是要在不改变位置的情况下，适当地旋转坐标系而使其达到所期望的姿态。合适的旋转顺序取决于机器人手腕的设计以及关节装配在一起的方式。考虑以下三中常见的构型配置：

（a） 滚动角、俯仰角、偏航（RPY）角 （b） 欧拉角

（c） 链式关节

2.7.2.1 滚动角、俯仰角和偏航角

这是分别绕当前a,o,n轴的三个旋转顺序，能够把机器人的手调整到所期望的姿态。此时，假定当前的坐标系平行于参考坐标系，于是机器人手的姿态在RPY（滚动角、俯仰角、偏航角）运动前与参考坐标系相同。如果当前坐标系不平行于参考坐标系，那么机器人手最终的姿态将会是先前的姿态与RPY右乘的结果。

因为不希望运动坐标系原点的位置有任何改变（它已被放在一个期望的位置上，所以只需要旋转她到所期望的姿态），所以RPY的旋转运动都是相对于当前的运动轴的。否则。如前面所看到的，运动坐标系的位置将会改变。于是，右乘所有由RPY和其他旋转所产生的与姿态改变有关的矩阵。

图2.22 绕当前坐标轴的RPY旋转

参考图2.22，可看到RPY旋转包括以下几种： ? 绕a轴（运动坐标系的z轴）旋转?a叫滚动； ? 绕o轴（运动坐标系的y轴）旋转?o叫俯仰； ? 绕n轴（运动坐标系的x轴）旋转?n叫偏航。 表示RPY姿态变化的矩阵为：

RPY(?a,?o,?n)?Rot?a,?a?Rot?o,?o?Rot?n,?n??

0?0?? （2.36） 0??1? 这个矩阵表示了仅由RPY引起的姿态变化。该坐标系相对于参考坐标系的位置和最终姿态是表示位置变化和RPY的两个矩阵的乘积。例如，假设一个机器人是根据球坐标和RPY来设计的，那么这个机器人就可以表示为：

?C?aC?o?S?C??ao??S?o??0C?aS?oS?n?S?aC?nS?aS?oS?n?C?aC?nC?oS?n0C?aS?oC?n?S?aS?nS?aS?oC?n?C?aS?nC?oC?n0RTH?Tsph?r,?,???RPY??a,?o,?n?

关于RPY的你运动学方程的解比球坐标更复杂，因为这里有三个耦合角，所以需要所有三个角各自的正弦和余弦值的信息才能解出这个角。为解出这三个角的正弦值和余弦值，必须将这些角解耦。因此，用Rot?a,?a?的逆左乘方程两边，得：

Rot?a,?a?RPY??a,?o,?n??Ro?to,?o?,Ro?tn,?n? （2.37）

?1假设用RPY得到的最后所期望的姿态是用n,o,a矩阵来表示的，则有：

???nx??1?nyRot?a,?a??nz??0进行矩阵相乘后得：

oxoyoz0axayaz00?0???Rot?o,?o?,Rot?n,?n? （2.38） 0??1?axC?a?ayS?aayC?a?axS?aaz0S?oS?nC?nC?oS?n00?0??? 0??1?0?0?? （2.39） 0??1??nxC?a?nyS?a?nC??nS?axa?y?nz?0?oxC?a?oyS?aoyC?a?oxS?aoz0?C?o?0???S?o??0S?oC?n?S?nC?oC?n0在式（2.38）中的n,o,a分量表示了最终的期望值，它们通常是给定或已知的，而RPY角的值是未知的变量。

让式（2.39）左右两边对应的元素相等，将产生如下结果（关于ATAN2函数的解释请参考附录A）： 根据2，1元素得：

??ny,?nx? （2.40） nyC?a?nxS?a?0??a?ATAN2?ny,nx? ?a?ATA2N

根据3，1元素和1，1元素得：

C?o?nxC?a?nyS?a??o?ATAN2??nz,?nxC?a?nyS?a??

根据2，2元素和2，3元素得：

S?o??nz （2.41）

C?n?oyC?a?oxS?aS?n??ayC?a?axS?a??n?ATAN2??ayC?a?axS?a?,?oyC?a?oxS?a??? （2.42）

例2.16 下面给出了一个笛卡尔坐标-RPY型机器人手所期望的最终位姿，求滚动角，俯仰角，偏航角

和位移。

?nx?nRTp??y?nz??0

解：

oxoyoz0axayaz0px??0.354?0.6740.6494.33??0.505?py?0.7220.4752.50???? pz???0.7880.1600.5958????1??0001?

根据上述方程，得到两组解：

?a?TAN2?ny,nx??ATAN?0.505,0.354??55?或235?

?o?ATAN2?nz,nxC?a?nyS?a?ATAN2?0.788,0.616??52?或128?

?????n?ATAN2???ayC?a?axS?a?,?oyC?a?oxS?a??

?ATAN2?0.259,0.966??15?或195?

px?4.33 py?2.5 pz?8

例2.17 与例2.16中的位姿一样，如果机器人是圆柱坐标-RPY型，求所有关节变量。 解： 在这种情况下，可用：

?0.354?0.6740.6494.33??0.505?0.7220.4752.50R??Tcy1(r,?,l)?RPY(?a,?o,?n) TP????0.7880.1600.5958???0001??这个方程右边有4个角，它们是耦合的，因此必须像前面那样将它们解耦。但是，因为对于圆柱

坐标系z轴旋转?a角并不影响a轴，所以它仍平行于z轴。其结果是，对于RPY绕a轴旋转的?角可简单地加到?a上。这意味着，求出的?a实际上是?a??（参见图2.23）。根据给定的位置数据，可求得例2.16的解。参考式（2.33），得到：

rC??4.33 rS??2.5???30?

?a???55? ??a?25?

S??0.5 ?r?5

pz?8 ?l?8

同例2.16一样 ??o?52? ?n?15?

当然，可以用类似的解法求出第二组解。

图2.23 例2.17的圆柱和RPY坐标

2.7.2.2 欧拉角

除了最后的旋转使然当前的a轴外，欧拉角的其他方面均与RPY相似（参见图2.24）。我们仍需要使所有旋转都是绕当前的轴转动以防止机器人的位置有任何改变。表示欧拉角的转动如下：

? 绕a轴（运动坐标系的z轴）旋转?度； ? 接着绕o轴（运动坐标系的y轴）旋转?度； ? 最后再绕a轴（运动坐标系的x轴）旋转?度。 表示欧拉角姿态变化的矩阵是：

Euler??,?,???Rot?a,??Rot?o,??,Rot?a,???

?C?C?C??S?C??S?C?C??C?S????S?C??0??C?C?S??S?C??S?C?S??C?C?S?S?0C?S?S?S?C?00?0?? （2.43） 0??1?

图2.24 绕当前坐标轴的欧拉旋转

再次强调，该矩阵只是表示了由欧拉角所引起的姿态变化。相对于参考坐标系，这个坐标系的最终位姿是表示位置变化的矩阵和表示欧拉角的矩阵的乘积。

欧拉角的逆运动学求解与RPY非常相似。可以使欧拉方程的两边左乘Rot?1?a,??来消去其中一边的?。让两边的对应元素相等，就可得到以下方程（假设由欧拉角得到的最终所期望的姿态是由n,o,a矩阵表示）：

???nx?n?1Rot?a,????y?nz??0或

oxoyoz0axayaz00??C?C??S?0????0???S?C???1??0?C?S?C?S?S?0S?0C?00?0?? （2.44） 0??1??nxC??nyS???nS??nC?y?x?nz?0?oxC??oyS??oxS??oyC?oz0?C?C??S????S?C???0axC??ayS??axS??ayC?az0S?0C?00?0??? 0??1??C?S?C?S?S?00?0?? （2.45） 0??1?记住，式（2.44）中的n,o,a表示了最终的期望值，它们通常是给定或已知的。欧拉角的值是未知变量。

让式（2.45）左右两边对应的元素相等，可得到如下结果： 根据2，3元素，可得：

?axS??ayC??0???ATAN2?ay,ax?或??ATAN2??ay,?ax? （2.46）

由于求得了值，因此式（2.45）左边所有的元素都是已知的。根据2，1元素和2，2元素得：

S???nxS??nyC?

C???oxS??oyC????ATAN2??nxS??nyC?,?oxS??oyC?? （2.47）

最后根据1，3元素和3，3元素，得：

C??az???ATAN2?axC??ayS?,az??nx?nRTH??y?nz??0oxoyoz0axayaz0px??0.579?0.548?0.604?0.5400.813?0.220py????pz??0.611?0.1990.766??1??000S??axC??ayS? （2.48）

例2.18 给定一个直角坐标系—欧拉角型机器人手的最终期望状态，求相应的欧拉角。

5?7?? 3??1?解：根据前面的方程，得到：

??ATAN2?ay,ax??ATAN2??0,220,?0.604??20?或200?

将20?和200?的正弦值和余弦值应用于其他部分，可得：

??ATAN2??nxS??nyC?,?oxS??oyC????0.31,0.952??18?或198? ??ATAN2?axC??ayS?,az??ATAN2??0.643,0.766???40?或40?

2.7.2.3 链式关节

链式关节由3个旋转组成，而不是上面刚提出来的旋转模型，这些旋转取决于关节的设计。就像在2.7.1（d）中所做的那样，我们将在讨论D-H表示法时来推导表示链式关节的矩阵。 2.7.3 位姿的正逆运动学方程

表示机器人最终位姿的矩阵是前面方程的组合，该矩阵取决于所用的坐标。假设机器人的运动是由直角坐标和RPY的组合关节组成，那么该坐标系相对于参考坐标系的最终位姿是表示直角坐标位置变化的矩阵和RPY矩阵的乘积。它可表示为：

RTH?Tcart?Px,Py,Pz??RPY??a,?o,?n? （2.49）

如果机器人是采用球坐标定位、欧拉角定姿的方式所设计的，那么将得到下列方程。其中位置由球坐标决定，而最终姿态既受球坐标角度的影响也受欧拉角的影响。

RTH?Tsph?r,?,???Euler??,?,?? （2.50）

由于有多种不同的组合，所以这种情况下的正逆运动学解不在这里讨论。对于复杂的设计，推荐用D_H表示法来求解，并将在下面对此进行讨论。

2.8 机器人正运动学方程的D-H表示法

在1955年，Denavit和Hartenberg在“ASME Journal of Applied Mechanics”发表了一篇论文，后来利用那个这篇论文来对机器人进行表示和建模，并导出了它们的运动方程，这已成为表示机器人和对机器人运动进行建模的标准方法，所以必须学习这部分内容。Denavit-Hartenberg(D_H)模型表示了对机器人连杆和关节进行建模的一种非常简单的方法，可用于任何机器人构型，而不管机器人的结构顺序和复杂程度如何。它也可用于表示已经讨论过的在任何坐标中的变换，例如直角坐标、圆柱坐标、球坐标、欧拉角坐标及RPY坐标等。另外，它也可以用于表示全旋转的链式机器人、SCARA机器人或任何可能的关节和连杆组合。尽管采用前面的方法对机器人直接建模会更快、更直接，但D-H表示法有其附加的好处，使用它已经开发了许多技术，例如，雅克比矩阵的计算和力分析等。

假设机器人由一系列关节和连杆组成。这些关节可能是滑动（线性）的或旋转（转动）的，它们可以按任意的顺序放置并处于任意的平面。连杆也可以是任意的长度（包括零），它可能被弯曲或扭曲，也可能位于任意平面上。所以任何一组关节和连杆都可以构成一个我们想要建模和表示的机器人。

为此，需要给每个关节指定一个参考坐标系，然后，确定从一个关节到下一个关节（一个坐标系到下一个坐标系）来进行变换的步骤。如果将从基座到第一个关节，再从第一个关节到第二个关节直至到最后一个关节的所有变换结合起来，就得到了机器人的总变换矩阵。在下一节，将根据D-H表示法确定一个一般步骤来为每个关节指定参考坐标系，然后确定如何实现任意两个相邻坐标系之间的变换，最后写出机器人的总变换矩阵。

图2.25 通用关节—连杆组合的D-H表示

假设一个机器人由任意多的连杆和关节以任意形式构成。图2.25表示了三个顺序的关节和两个连杆。虽然这些关节和连杆并不一定与任何实际机器人的关节或连杆相似，但是他们非常常见，且能很容易地表示实际机器人的任何关节。这些关节可能是旋转的、滑动的、或两者都有。尽管在实际情况下，机器人的关节通常只有一个自由度，但图2.25中的关节可以表示一个或两个自由度。

图2.25（a）表示了三个关节，每个关节都是可以转动或平移的。第一个关节指定为关节n，第二个关节为关节n+1，第三个关节为关节n+2。在这些关节的前后可能还有其他关节。连杆也是如此表示，连杆n位于关节n与n+1之间，连杆n+1位于关节n+1与n+2之间。

为了用D-H表示法对机器人建模，所要做的第一件事是为每个关节指定一个本地的参考坐标系。因此，对于每个关节，都必须指定一个z轴和x轴，通常并不需要指定y轴，因为y轴总是垂直于x轴和z轴的。此外，D-H表示法根本就不用y轴。以下是给每个关节指定本地参考坐标系的步骤：

? 所有关节，无一例外的用z轴表示。如果关节是旋转的，z轴位于按右手规则旋转的方向。

如果关节是滑动的，z轴为沿直线运动的方向。在每一种情况下，关节n处的z轴（以及该

关节的本地参考坐标系）的下表为n-1。例如，表示关节数n+1的z轴是zn。这些简单规则可使我们很快地定义出所有关节的z轴。对于旋转关节，绕z轴的旋转（?角）是关节变量。对于滑动关节，沿z轴的连杆长度d是关节变量。

? 如图2.25（a）所示，通常关节不一定平行或相交。因此，通常z轴是斜线，但总有一条距离

最短的公垂线，它正交于任意两条斜线。通常在公垂线方向上定义本地参考坐标系的x轴。

所以如果an表示zn?1与zn之间的公垂线，则xn的方向将沿an。同样，在zn与zn?1之间的公垂线为an?1，xn?1的方向将沿an?1。注意相邻关节之间的公垂线不一定相交或共线，因此，两个

相邻坐标系原点的位置也可能不在同一个位置。根据上面介绍的知识并考虑下面例外的特殊情况，可以为所有的关节定义坐标系。

? 如果两个关节的z轴平行，那么它们之间就有无数条公垂线。这时可挑选与前一关节的公垂

线共线的一条公垂线，这样做就可以简化模型。

? 入股两个相邻关节的z轴是相交的，那么它们之间就没有公垂线（或者说公垂线距离为零）。

这时可将垂直于两条轴线构成的平面的直线定义为x轴。也就是说，其公垂线是垂直于包含了两条z轴的平面的直线，它也相当于选取两条z轴的叉积方向作为x轴。这也会使模型得以简化。

在图2.25（a）中，?角表示绕z轴的旋转角，d表示在z轴上两条相邻的公垂线之间的距离，a表示每一条公垂线的长度（也叫关节偏移量），角?表示两个相邻的z轴之间的角度 （也叫关节扭转）。通常，只有?和d是关节变量。 下一步来完成几个必要的运动，即将一个参考坐标系变换到下一个参考坐标系。假设现在位于本地坐标系xn?zn，那么通过以下四步标准运动即可到达下一个本地坐标系xn?1?zn?1。

（1）绕zn轴旋转?n?1（如图2.25（a）与（b）所示），它使得xn和xn?1互相平行，因为an和an??都

是垂直于zn轴的，因此绕zn轴旋转?n?1使它们平行（并且共面）。

（2）沿zn轴平移dn?1距离，使得xn和xn?1共线（如图2.25（c）所示）。因为xn和xn?1已经平行并

且垂直于zn，沿着zn移动则可使它们互相重叠在一起。

（3）沿xn轴平移an?1的距离，使得xn和xn?1的原点重合（如图2.25（d）和（e）所示）。这是两

个参考坐标系的原点处在同一位置。

（4）将zn轴绕xn?1轴旋转?n?1，使得zn轴与zn?1轴对准（如图2.25（f）所示）。这时坐标系n和n+1完全相同（如图2.25（g）所示）。至此，我们成功地从一个坐标系变换到了下一个坐标

系。

在n+1和n+2坐标系间严格地按照同样的四个运动顺序可以将一个坐标变换到下一个坐标系。如有必要，可以重复以上步骤，就可以实现一系列相邻坐标系之间的变换。从参考坐标系开始，我们可以将其转换到机器人的基座，然后到第一个关节，第二个关节??，直至末端执行器。这里比较好的一点是，在任何两个坐标系之间的变换均可采用与前面相同的运动步骤。 通过右乘表示四个运动的四个矩阵就可以得到变换矩阵A，矩阵A表示了四个依次的运动。由于所有的变换都是相对于当前坐标系的（即他们都是相对于当前的本地坐标系来测量与执行），因此所有的矩阵都是右乘。从而得到结果如下：

nTn?1?An?1?Rot?z,?n?1??Tran?0,0,dn?1??Tran?an?1,0,0??Rot?x,an?1?

?C?n?1?S?n?1?S?C?n?1??n?1?00?0?0?1?0???0??0010000100??1?00????0??0??1??0010000?00?? 1dn?1??01?0?0?? （2.51） 0??1?0an?1??100?0C?00??S?n?1n?1???10??0S?n?1C?n?1??01??000?C?n?1?S?n?1C?n?1S?n?1S?n?1an?1C?n?1??S??C?C??C?S?aS?n?1n?1n?1n?1n?1n?1n?1? （2.52） An?1???0S?n?1C?n?1dn?1???0001??比如，一般机器人的关节2与关节3之间的变换可以简化为：

a3C?3?a3S?3?? （2.53） d3??1? 在机器人的基座上，可以从第一个关节开始变换到第二个关节，然后到第三个??，再到机器人的手，最终到末端执行器。若把每个变换定义为，则可以得到许多表示变换的矩阵。在机器人的基座与手之间的总变换则为：

RTH?RT11T22T3?n?1Tn?A1A2A3?An （2.54）

其中n是关节数。对于一个具有六个自由度的机器人而言，有6个A矩阵。 为了简化A矩阵的计算，可以制作一张关节和连杆参数的表格，其中每个连杆和关节的参数值可从机器人的原理示意图上确定，并且可将这些参数代入A矩阵。表2.1可用于这个目的。 在以下几个例子中，我们将建立必要的坐标系，填写参数表，并将这些数值代入A矩阵。首先从简单的机器人开始，以后再考虑复杂的机器人。

表2.1 D-H参数表 ?C?3?S? 2T3?A3??3?0??0?S?3C?3C?3C?3S?30S?3S?3?C?3S?3C?30# 1 2 3 ? d a ? 例2.19 对于如图2.26所示的简单机器人，根据D-H表示法，建立必要的坐标系，并填写相应的参数表。

解：

为方便起见，在此例中，假设关节2，3和4在同一平面内，即它们的dn值为0。为建立机器人的坐标系，首先寻找关节（如图2.26所示）。该机器人有六个自由度，在这个简单机器人中，所有的关节都是旋转的。第一个关节（关节1）在连杆0（固定基座）和连杆1之间，关节2在连杆1和连杆2之间，等等。首先，如前面已经讨论过的那样，对每个关节建立z轴，接着建立z轴。观察图2.27和图2.28所示的坐标可以发现，图2.28是图2.27的简化线图。应注意每个坐标系原点3在它所在位置的原因。

4 5 6

图2.26 具有六个自由度的简单链式机器人

图2.27 简单六个自由度链式机器人的参考坐标系

图2.28 简单六个自由度链式机器人的参考坐标系线图

从关节1开始，z0表示第一个关节，它是一个旋转关节。选择x0与参考坐标系的x轴平行，这样做仅仅是为了方便，x0是一个固定的坐标轴，表示机器人的基座，它是不动的。第一个关节的运动

是围绕着z0-x0轴进行的，但这两个轴并不运动。接下来，在关节2处设定z1，因为坐标轴z0和z1是相交的，所以x1垂直于z0和z1。x2在z1和z2之间的公垂线方向上，x3在z2和z3之间的公垂线方向上，类似地，x4在z3和z4之间的公垂线方向上。最后，z5和z6是平行且共线的。z5表示关节6的运动，而z6表示末端执行的运动。通常在运动方程中不包含末端执行器，但应包含末端执行器的坐标系，这是因为它可以容许进行从坐标系z5?x5出发的变换。同时也要注意第一个和最后一个坐标系的原点的位置，它们将决定机器人的总编换方程。可以在第一个和最后的坐标系之间建立其他的（或不同的）中间坐标系，但只要第一个和最后的坐标系没有改变，机器人的总变换便是不变的。应注意的是，第一个关节的原点并不在关节的实际位置，但证明这样做是没有问题的，因为无论实际关节是高一点还是低一点，机器人的运动并不会有任何差异。因此，考虑原点位置时可不用考虑基座上关节的实际位置。

接下来，我们将根据已建立的坐标系来填写表2.2中的参数。参考前一节中任意两个坐标系之间的四个运动的顺序。从z0?x0开始，有一个旋转运动将x0转到了x1，为使得x0与x1轴重合，需要沿z1和沿x1的平移均为零，还需要一个旋转将z0转到z1，注意旋转是根据右手规则进行的，即将右手手指按旋转的方向弯曲，大拇指的方向则为旋转坐标轴的方向。到了这时，z0?x0就变换到了z1?x1。

接下来，绕z1旋转?2，将x1转到了x2，然后沿x2轴移动距离a2，使坐标系原点重合。由于前后两个z轴是平行的，所以没有必要绕x轴旋转。按照这样的步骤继续做下去，就能得到所需要的结果。 必须要认识到，与其他机械类似，机器人也不会保持原理图中所示的一种构型不变。尽管机器人的原理图是二维的，但必须要想象出机器人的运动，也就是说，机器人的不同连杆和关节在运动时，与之相连的坐标系也随之运动。如果这时原理图所示机器人构型的坐标轴处于特殊的位姿状态，当机器人移动时它们又会处于其他的点和姿态上。比如，x3总是沿着关节3与关节4之间连线a3的方向。当机器人的下臂绕关节2旋转而运动。在确定参数时，必须记住这一点。

表2.2 例2.19机器人的参数 # 1 2 3 4 5 6 ? ?1 d 0 0 0 0 0 0 a 0 ? 90 0 0 -90 90 0 ?2 a2 ?3 ?4 a3 a4 0 0 ?5 ?6 ?表示旋转关节的关节变量，d表示滑动关节的关节变量。因为这个机器人的关节全是旋转的，因此所有关节变量都是角度。

通过简单地从参数表中选取参数代入A矩阵，便可写出每两个相邻关节之间的变换。例如，在坐

标系0和1之间的变换矩阵A1可通过将?（sin90?=1,cos90?=0, ?=90?）以及指定C1为?1等代入A矩阵得到，对其他关节的A2~A6矩阵也是这样，最后得：

?C1?SA1??1?0??00S10?C11000?S3C3000??C2?S0?? A2??2?00???1??0?S2C2000C2a2?0S2a2?? 10??01?C4a4?S4a4?? （2.55） 0??1?0?0?? 0??1??C3?SA3??3?0??0?C5?SA5??5?0??00C3a3??C4?S0S3a3?? A4??4?010???01??00?S40C4?1000?S6C60000100??C6?S0?? A6??6?00???1??0特别注意：为简化最后的解，将用到下列三角函数关系式：

0S50?C51000S?1C?2?C?1S?2?S(?1??2)?S12C?1C?2?S?1S?2?C(?1??2)?C12在机器人的基座和手之间的总变换为：

R （2.56）

TH?A1A2A3A4A5A6 （2.57）

C1(C234a4??C1(C234C5C6?S234S6)C1(?C234C5C6?S234C6)C1(C234S5)???SSC??SSS?SCCa?Ca)1561523322?156?S1(C234C5C6?S234S6)S1(?C234C5C6?S234C6)S1(C234S5)S1(C234a4?? ????CSS?C1S5S6?C1C5C23a3?C2a2)?156??SCC?SCC?CCSSSa?Sa?Sa234562345623462345234423322????0001??例2.20 斯坦福机械手臂。在斯坦福机械手臂上指定坐标系（如图2.29所示），并填写参数表。斯坦福机械手臂是一个球坐标手臂，即开始的两个关节是旋转的，第三个关节是滑动的，最后三个腕关节全是旋转关节。

图2.29 斯坦福机械手臂示意图

解：

在看本题解答之前，现根据自己的理解来做，问题的答案在本章的最后。建议在看解答中建立的坐标系和机械手臂的解之前，先试着自己做。

机器手臂最后的正运动学解是相邻关节之间的6个变换矩阵的乘积：

RTHSTANGORD?nx?n?0T6??y?nz??0oxoyoz0axayaz0px?py?? pz??1?其中

nx?C1?C2?C4C5C6?S4S6??S2S5C6??S1?S4C5C6?C4S6?nz??S2?C4C5C6?S4S6??C2S5C6ny?S1?C2?C4C5C6?S4S6??S2S5C6??C1?S4C5C6?C4S6? ox?C1??C2?C4C5C6?S4S6??S2S5C6??S1??S4C5C6?C4S6?oz?S2?C4C5C6?S4S6??C2S5C6ax?C1?C2C4S5?S2C6??S1S4S5ay?S1?C2C4S5?S2C6??C1S4S5 （2.58） az??S2C4S5?C2C5px?C1S2d3?S1d2py?S1S2d3?C1d2 pz?C2d3 oy?S1??C2?C4C5C6?S4S6??S2S5C6??C1??S4C5C6?C4S6?

2.9 机器人的你运动学解

如前所述，这里真正重要的是你运动学解。为了使机器人手臂处于期望的位姿，如果有了逆运动学解就能确定每个关节的值。前面已对特定坐标系统的逆运动学解作了介绍。在这一部分，将研究求解逆运动方程的一般步骤。

现在你可能已经注意到，前面的运动方程中有许多角度的耦合，比如C234，这就使得无法从矩阵中提取足够的元素来求解单个的正弦和余弦项以计算角度。为使角度解耦，可例行地用单个RTH矩阵

?1左乘An矩阵，使得方程右边不再包括这个角度，于是可以找到产生角度的正弦值和余弦值的元素，

并进而求得相应的角度。

这里概要地给出了这个方法，并将其用于例2.19中的简单机械手臂。虽然所给出的解决方法只针对这一给定构型的机器人，但也可以类似地用于其它机器人。正如在例2.19中看到的，表示机器人的最后方程为：

RTH?A1A2A3A4A5A6C1(?C234C5C6?S234C6)C1(C234S5)?S1S5S6?S1C5S1(?C234C5C6?S234C6)S1(C234S5)?C1S5S6?S234C5C6?C234C60?C1C5S234S50?C23a3?C2a2)?? S1(C234a4??C23a3?C2a2)??S234a4?S23a3?S2a2??1?C1(C234a4??C1(C234C5C6?S234S6)??SSC?156S1(C234C5C6?S234S6)????CSS?156?S234C5C6?C234C6?0?为了书写方便，将上面的矩阵表示为[RHS](Right-Hand Side)。这里再次将机器人的期望位姿表示

为：

?nx?nRTH??y?nz??0oxoyoz0axayaz0px?py?? （2.59） pz??1??1为了求解角度，从An开始，依次用A1?1左乘上述两个矩阵，得到：

?nx?nA1?1??yoxoyaxaypx?py???A1?1[RHS]?A2A3A4A5A6 （2.60） ??nzozazpz??0001????C1S100??nxoxaxpx??0010?????nyo?yaypy??S1?C100?ozazp??A2A3A4A5A6 z??0001????nz?0001????nxC1?nyS1oxC1?oyS1axC1?ayS1PxC1?PyS1??n?zoZazpz??nn??xS1?yC1oxS1?oyC1axS1?ayC1PxS1?PyC1??0001????C?S 234C5C6?S234S6?C234C5C6234C6C234S5C234a4?C23a3?C2a2??S234C5C6?C234S6?S234C5C6?C234C6S234S5S234a4?S23a3?S2a?2???SC?5C6S5S650??0001??根据方程的3，4元素，有：

pSpyx1?pyC1?0 ? ?1?arctapn)(和?1???1?180 x根据1，4元素和2，4元素，可得：

pxC1?pyS1?C234a4?C23a3?C2a2p z?S234a4?S23a3?S2a2整理上面两个方程并对两边平方，然后将平方值相加，得：

(p2xC1?pyS1?C234a4)?(C23a3?C2a2)2(p2z?S234a4)?(S23a3?S2a2)2

(p?p)2?(p222xC1yS1?C234a4z?S234a4)?a2?a3?2a2a3?S2S23?C2C23?

根据式（2.56）的三角函数方程，可得：

S2S23?C2C23?cos???2??3???2??cos?3

于是：

CpxC1?p2yS1?C234a4)?(pz?S234a4)2?a222?a33?(2a 2a32.61）

2.62）

2.63）

2.64）

（ （ （ （

在这个方程中，除S234和C234外，每个变量都是已知的，S234和C234将在后面求出。已知：

S3??1?C3

2于是可得：

?3?arctan

S3 （2.65） C3因为关节2，3和4都是平行的，左乘A2和A3的逆不会产生有用的结果。下一步左乘A1~A4的逆，

结果为：

?nx?n?1?1?1?1A4A3A2A1??y?nz??0乘开后可得：

oxoyoz0axayaz0px?py??1?1?1?1??A4A3A2A1[RHS]?A5A6 （2.66） ?pz?1?C234(C1ox?S1oy)?C234(C1ax?C234(C1px?S1py)?S234pz??C234(C1nx?S1ny)???S1ay)?S234axS234oz?C34a2?C4a3?a4?S234nz?C1ny?S1nxC1oy?S1oxC1ay?S1ax0?? ??S234(C1nx?S1ny)??S234(C1ox?S1oy)??S234(C1ax?S1ay)??S234(C1Px?S1Py)?C234pz???C234ozC234az?S34a2?S4a3?C234nz???0001???C5C6?SC??56?S6??0

?C5S6?S5S6C60S5?C5000?0?? （2.67） 0??1?根据式（2.67）矩阵的3，3元素，

-S234（C1ax?S1ay)?C234az?0 ?

?234?arctan(az)和?234??234?180? （2.68）

C1ax?S1ay由此可计算S234和C234，如前面所讨论过的，它们可用来计算?3。

现在再参照式（2.63），并在这里重复使用它就可计算角?2的正弦和余弦值。具体步骤如下：

?pxC1?pyS1?C234a4?C23a3?C2a2 ?pz?S234a4?S23a3?S2a2?由于C12?C1C2?S1S2以及S12?S1C2?C1S2，可得：

?pxC1?pyS1?C234a4??C2C3?S2S3?a3?C2a2 （2.69） ?pz?S234a4?(S2C3?C2S3)a3?S2a2?上面两个方程中包含两个未知数，求解C2和S2，可得：

(C3a3?a2)(pz?S234a4)?S3a3(pxC1?pyS1?C234a4)??S2?2(C3a3?a2)2?S32a3? （2.70） ?(C3a3?a2)(pxC1?pyS1?C234a4)?S3a3(pz?S234a4)?C2?222?(Ca?a)?S3323a3?尽管这个方程较复杂，但它的所有元素都是已知的，因此可以计算得到：

?2?arctan既然?2和?3已知，进而可得：

(C3a3?a2)(pz?S234a4)?S3a3(pxC1?pyS1?C234a4)(C3a3?a2)(pxC1?pyS1?C234a4)?S3a3(pz?S234a4) （2.71）

?4??234??2??3 （2.72）

因为式（2.68）中的?234有两个解，所以?4也有两个解。 根据式（2.67）中的1，3元素和2，3元素，可以得到：

（C1ax?S1ay)?S234az?S5?C234 （2.73） ?C??Ca?Sa51y1x?C3(4C1ax?S1ay)?S23a4z和 ?5?arcta2 （2.74） nS1ax?C1ay也许已注意到，因为对于?6没有解耦方程，所以必须用A5矩阵的逆左乘式（2.67）来对它解耦。这样做后可得到：

?C5C234?C1nx?S1ny??S234nz???S5?S1nx?C1ny???S234?C1nx?S1ny??C234nz?0??0???C5C234?C1ox?S1oy??S234oz?S5?S1ox?C1oy??S234?C1ox?S1oy??C234oz00??0010?0??0?? ?0?1???C6?S?6?0??0

?S6C60000100?0?? (2.75) 0??1?根据式（2.75）中的2，1元素和2，2元素，得到：

?6?arctan?S234(C1nx?S1ny)?C234nz?S234(C1ox?S1oy)?C234oz （2.76）

至此找到了6个方程，它们合在一起给出了机器人置于任何期望位姿时所需的关节值。虽然这种方法仅适用于给定的机器人，但也可采取类似的方法来处理其他的机器人。 值得注意的是，仅仅因为机器人的最后三个关节交于一个公共点才使得这个方法有可能求解，否则就不能用这个方法来求解，而只能直接求解矩阵或通过计算矩阵的逆来求解未知的量。大多数工业机器人都有相交的腕关节。 2.10 机器人的逆运动学编程

求解机器人逆运动问题所建立的方程可以直接用于驱动机器人到达一个位置。事实上，没有机器人真正用正运动方程求解这个问题，所用到的仅为计算关节值的6个方程，并反过来用它们驱动机器人到达期望位置。这样做是必须的，其实际原因是：计算机计算正运动方程的逆或将值代入正运动方

程，并用高斯消去法来求解未知量（关节变量）将花费大量时间。 为使机器人按预定的轨迹运动，譬如说直线，那么在一秒内必须多次反复计算关节变量。现假设机器人沿直线从起点A运动到终点B，如果期间不采取其他措施，那么机器人从A运动到B的轨迹难以预测。机器人将运动它的所有关节直到他们都到达终值，这是机器人便到达了终点B，然而，机器人手在两点间运行的路径是未知的，它取决于机器人每个关节的变化率。为了使机器人按直线运动，必须把这一路径分成如图2.30所示的许多小段，让机器人按照分好的小段路径在两点间依次运动。这就意味着对每一小段路径都必须计算新的逆运动学解。典型情况下，每秒钟要对位置反复计算50~200次。也就是说，如果计算逆解耗时5~20ms以上，那么机器人将丢失精度或不能按照指定路径运动。用来计算新解的时间越短，机器人的运动据越精确。因此，必须尽量减少不必要的计算，从而使计算机控制器能做更多的逆解计算。这也就是为什么设计者必须事先做好所有的数学处理，并仅需为计算机控制器编程来计算最终的解的原因。第5章将详细讨论这个问题。

图2.30 直线的小段运动

对于早先讨论过的旋转机器人情况，给定最终的期望位姿为：

RTHDESIRED?nx?n??y?nz??0oxoyoz0axayaz0px?py?? pz??1?为了计算未知角度，控制器所需要的所有计算是如下的一组逆解：

py?1?arctan()和?1??1?180?px?234?arctan(C3?az)和?234??234?180?C1ax?S1ay22(pxC1?pyS1?C234a4)2?(pz?S234a4)2?a2?a32a2a32S3??1?C3?3?arctanS3C3(C3a3?a2)(pz?S234a4)?S3a3(pxC1?pyS1?C234a4)(C3a3?a2)(pxC1?pyS1?C234a4)?S3a3(pz?S234a4)?2?arctan?4??234??2??3C234(C1ax?S1ay)?S234az?5?arctanS1ax?C1ay?6?arctan?S234(C1nx?S1ny)?C234nz?S234(C1ox?S1oy)?C234oz （2.77）

虽然以上计算也并不简单，但用这些方程来计算角度要比对矩阵求逆或使用高斯消去法计算要快得多。这里所有的运算都是简单的算术运算和三角运算。

2.11 机器人的退化和灵巧特性

退化 当机器人失去一个自由度，并因此不按所期望的状态运动时即称机器人发生了退化。在两种条件下会发生退化：（1）机器人关节达到其物理极限而不能进一步运动，（2）如果两个相似关节的z轴共线时，机器人可能会在其工作空间中变为退化状态。这意味此时无论哪个关节运动都将产生同样的运动，结果是控制器将不知道是哪个关节在运动。无论哪一种情况，机器人的自由度总数都小

于6，因此机器人的方程无解。在关节共线时，位置矩阵的行列式也为零。图2.31显示了一个处于垂直构型的简单机器人，其中关节1和6共线。可以看到，无论关节1或关节6旋转，末端执行器的旋转结果都是一样的。实际上，这时指令控制器采取紧急行动是十分重要的，否则机器人将停止运行。应注意，这种情况只在两关节相似时才会发生，反之，如果一个关节是滑动型的，而另一个是旋转型的（例如斯坦福机械手臂的关节3和关节4），那么即使它们的z轴共线，机器人也不会出现退化的现象。

Paul指出：如果sin(?4),sin(?5),或sin(?5)为0，机器人就将退化。显然，可以适当设计?4和?5来防止机器人退化。此外，如果任何时候?5接近0?或者180?，机器人就将变成退化状态。

灵巧 一般认为只要确定了机器人手的位姿，就能为具有六个自由度的机器人在其工作范围内的任何位置定位和定姿。实际上，随着机器人越来越接近其工作空间的极限，虽然机器人仍可能定位在期望的点上，但却有可能不能定姿在期望的位姿上。能对机器人定位但不能对它定姿的点的区域成为不灵巧区域。

图2.31 处于退化状态的机器人

2.12 D-H表示法的基本问题

虽然D-H表示法已广泛用于机器人的运动建模和分析，并已成为解决该问题的标准方法，但它在技术上仍存在着根本的缺陷，很多研究者试图通过改进D-H表示法来解决这个问题。其根本问题是：由于所有的运动都是关于x和z轴的，而无法表示关于y轴的运动，因此只要有任何关于y轴的运动，此方法就不适用，而且这种情况十分普遍。例如，假设原本应该平行的两个关节轴在安装时有一点小的偏差，由于两轴之间存在小的夹角，因此需要沿y轴运动。由于实际的工业机器人在其制造过程中都存在一定的误差，所以该误差不能用D-H法来建模。

例2.20（续） 斯坦福机械手臂的参考坐标系。图2.32显示了例2.20中（如图2.29所示）斯坦福机械手臂的参考坐标系。为了改进可视性，进行了简化。表2.3所示为斯坦福机械手臂的参数表。

本文来源：https://www.bwwdw.com/article/r5g3.html