第3章机器人运动学与动力学
机器人运动学包括正向运动学和逆向运动学。正向运动学即给定机器人各关节变量，计算机器人末端的位置姿态； 逆向运动学即已知机器人末端的位置姿态，计算机器人对应位置时的每一关节变量的值。机器人动力学包括动力学正问题和动力学逆问题。动力学正问题是已知机器人各关节的作用力或力矩，计算各关节的位移、速度、加速度，求得运动轨迹； 机器人动力学逆问题是已知机器人末端的运动轨迹(即各关节的位移、速度、加速度)，求各关节所需的驱动力或力矩。
正向运动学容易求出它的唯一解，而逆向运动学分析比较复杂，有多个解无法建立通用的解析算法，需要考虑解的存在性、唯一性及求解的方法等问题。动力学问题一般需要建立6个非线性微分方程组，求解非线性微分方程组，得不出一般的通解，需要假设简化方程组进行处理。
本章主要介绍机器人运动学和动力学。首先介绍位姿的描述和齐次坐标的表示，它们是运动学分析的数学基础，然后讲述了运动学的正逆问题，重点介绍DH方法建立机器人运动学过程，简要介绍了机器人雅可比公式，讲述了动力学方程的推导以及简化计算，最后综合运动学和动力学介绍了机器人的静态特性与动态特性。
3.1位姿描述与齐次变换
机器人在进行运动学分析时，有许多矩阵公式需要运算，这都是建立在位姿的描述以及齐次坐标变换的基础上。以下讲述位姿描述与齐次坐标变换。
3.1.1位置描述

在二维平面内，某点P在坐标系中可用二维点(px，py)表示。类似的，在三维空间中，某点P在其内的表达


图3.1矢量OP及其分量

可用三维点(px，py，pz)表示。px，py，pz表示矢量OP在坐标系中的坐标分量，如图3.1所示。
在直角坐标系{A}中，把三个分量记为列矢量，空间任一点P的位置用3×1的列矢量AP表示，即


AP=px
py
pz(3.1)


3.1.2姿态描述
研究机器人的运动与操作，不仅需要知道刚体在空间中某个点的位置，而且需要知道刚体的姿态。刚体的姿态可由某个固连与此刚体的坐标系描述。为了描述空间某刚体P的姿态，可以设置一直角坐标系{B}与此刚体固连。用坐标系{B}的三个单位主矢量AxB，AyB，AzB相对于参考坐标系{A}的方向余弦组成的3×3矩阵： 


ABR=AxBAyBAzB(3.2)


或


ABR=r11r12r13

r21r22r23

r31r32r33


表示刚体B相对于坐标系{A}的姿态。 ABR称为旋转矩阵。在式（3.2）中，上标A代表参考坐标系{A}，下标B代表被描述的坐标系{B}。 ABR总共有9位元素，但只有3个是独立的，由于ABR的三个矢量AxB，AyB，AzB都是单位矢量，且双双相互垂直，因此它们满足6个正交条件： 


AxB·AxB= AyB·AyB= AzB·AzB=1(3.3)
AxB·AyB= AyB·AzB= AzB·AxB=0(3.4)


当然，上述正交条件式(3.4)也可用下面三个矢量的叉积代替： AxB×AyB=AzB。
可见，旋转矩阵ABR是正交的，并且满足： 


ABR－1= ABRT，ABR=1(3.5)


机器人运动过程中，经常用到绕x轴、y轴或z轴转动一角度θ，以下列出它们的姿态旋转矩阵： 


R(x，θ)=100
0cosθ－sinθ
0sinθcosθ(3.6)
R(y，θ)=cosθ0sinθ
010
－sinθ0cosθ(3.7)
R(z，θ)=cosθ－sinθ0
sinθcosθ0
001(3.8)


在本书中，用s表示sinθ，c表示cosθ。
3.1.3位姿描述
上面已经了解采用位置矢量描述点的位置，用旋转矩阵描述刚体的姿态。刚体B在空间的位置和姿态称为刚体的位姿，要完全描述刚体的位姿，通常将物体B与坐标系{B}相固连。坐标系{B}的坐标原点一般选取在物体B的质心处。相对参考系{A}，坐标系{B}的原点位置和坐标的姿态，分别由位置矢量APBO和旋转矩阵ABR描述。这样，我们就可以描述刚体在空间的位姿。


{B}=ABRAPBO(3.9)


当表示位置时，式(3.9)中的旋转矩阵ABR=I(单位矩阵)； 当表示姿态时，式(3.9)中的位置矢量APBO=0。
3.1.4运动坐标系描述

一般在笛卡儿坐标系中描述参考坐标系和运动坐标系。用x、y、z轴表示参考坐标系{A}，用n、o、a轴表示运动坐标系{B}。运动坐标系固连在末端执行器上，a(英文单词为approach的首字母)轴对应于参考坐标系的z轴，用来表示手爪接近物体的方向，o(英文单词为orientation的首字母)轴对应于参考坐标系的y轴，用来表示手爪抓紧物体的方向，n(英文单词为normal的首字母)轴对应于参考坐标系的x轴，用来表示垂直于a轴和o轴的方向，根据右手法则确定。图3.2是参考坐标系与运动坐标系。



图3.2参考坐标系{A}与运动坐标系{B}


根据式(3.9)，可知手爪的位姿矩阵为： 


{B}=noaP(3.10)


3.1.5齐次坐标变换

上面所述的为单一的固定坐标系的描述，但空间中任意一点P在不同的坐标系中的描述是各不相同的。为了阐述从一个坐标系到另一个坐标系的关系表示，需用讨论矩阵变换的相关问题，由此引出齐次坐标变换。
1. 平移坐标变换
首先，我们假设坐标系{B}，{A}具有相同的姿态，但是{B}与{A}的坐标系原点不重合。用位置矢量APBO描述它相对于{A}的位置，


图3.3平移坐标变换


如图3.3所示。称APBO为{B}相对于{A}的平移矢量。如果点P在坐标系{B}中的位置为BP，那么它相对于坐标系{A}的位置矢量AP可由矢量相加得出，即


AP= BP+ APBO(3.11)


式(3.11)为平移方程。平移坐标变换如图3.3所示。
2. 旋转坐标变换
P点在坐标系{B}和{A}有共同的坐标原点，只是姿态不同。可用旋转矩阵ABR描述{A}相对于{B}的姿态，如图3.4所示。得出同一点P在两个坐标系中的变换关系为： 


AP= ABRBP(3.12)


式(3.12)为旋转方程。
类似地用BAR描述坐标系{A}相对于{B}的姿态。BAR和ABR都是正交矩阵，两者互逆。根据正交矩阵的性质结合式(3.5)可得到如下公式： 


BAR= ABR－1= ABRT(3.13)


一般情况，坐标系{B}的原点和坐标系{A}的原点不重合，{B}的姿态与{A}的姿态也不相同。综合平移和旋转变换公式，可以得到任意一点P的变换关系如下： 


AP= ABRBP+ APBO(3.14)


式(3.14)是平移和旋转的复合变换方程，如图3.5所示。


图3.4旋转坐标变换




图3.5复合变换



复合变换方程可以这样理解，在变换过程中相当于有一个中间的坐标系{C}，此坐标系{C}的坐标原点与{B}重合，而姿态与{A}相同。根据式(3.12)，得到中间坐标系{C}的变换方程为： 


CP= CBRBPCBR= ABRCP= ABRBP


由式(3.11)，同样得到复合变换式(3.14)(见图3.5)。


AP= CP+ APCO= ABRBP+ APBO



例3.1已知坐标系{B}和{A}，它们的初始位姿重合，首先坐标系{B}相对于坐标系{A}的ZA轴转30°，又沿{A}的XA轴移动10个单位，再沿{A}的YA轴移动20个单位。求位置矢量APBO和旋转矩阵ABR。如果点P在{B}的描述为BP=4，8，0T，求它在坐标系{A}中的描述AP。
解： 根据式(3.8)和式(3.1)可得： 


ABR=R(Z，30°)=cos30°－sin30°0
sin30°cos30°0
001=0.866－0.50
0.50.8660
001，APBO=10
20
0


由式(3.14)可得： 


AP= ABRBP+ APBO=0.866－0.50
0.50.8660
0014
8
0+10
20
0=9.464
28.928
0


3. 齐次变换
在平面直角坐标系中，点A的坐标可以表示为A(x，y)或A′(x′，y′)。但在图3.6中，点F可表示为AD和BC的交点，变换后A′D′∥B′C′，其交点F′为一无穷远点，但是这一无穷远点不能用直角坐标来表示。为了解决此问题，可以把某点的x或y坐标用两个数的比来表示，如5可以表示为10/2或5/1等。因此，一点的直角坐标(x，y)可表示为(X/W，Y/W)。对同一点，随着W的值不同而会有不同的坐标。有序的三组数(X，Y，W)称为点的齐次坐标。这样一来就可以将N维空间的点在N+1维空间中表示。点的齐次坐标(X，Y，W)与直角坐标(x，y)的关系为： 


x=X/Wy=Y/W


平行线的交点示意图如图3.6所示。


图3.6平行线的交点示意图


可以得到某点P的直角坐标和齐次坐标分别为： 


P=x
y
zP=x
y
z
1


坐标原点的矢量，即零矢量表示为0，0，0，1，它是没有定义的。具有形如［a，b，c，0］的矢量表示无限远的矢量，用来表示方向，即用1，0，0，0，0，1，0，0，0，0，1，0分别表示x，y，z轴的方向。
在机器人矩阵变换中同理，三维坐标点可以用四维空间进行表示。
可得式(3.14)的齐次变换形式为： 


AP
1=ABRAPBO
01BP
1(3.15)


式（3.15）的矩阵形式为： 


AP= ABTBP(3.16)


式(3.16)中，齐次坐标AP和BP是一个4×1的列矢量，与式(3.14)中的维数不同，加入了第4个元素1。齐次变换矩阵ABT是4×4的方阵，具有如下形式： 


 ABT=ABRAPB0
01(3.17)


ABT综合地表示了平移变换和旋转变换。
可见，引入齐次坐标后，主要解决了以下问题： 
(1) 无穷远点表示问题。在几何当中，两条平行线表示为Ax+By+C=0，Ax+By+D=0的两个方程，它们之间没有交点(解)，但引入齐次坐标后两条直线可表示为AX/W+BY/W+C=0，AX/W+BY/W+D=0，它们之间有交点(解)，交点为无穷远点，即W=0，(X，Y，0)表示无穷远点。
(2) 矩阵运算。可以在矩阵运算中，把平移旋转等各种变换放在一个矩阵内进行，便于表达齐次变换矩阵ABT。
4. 平移齐次坐标变换
为了保证所表示的矩阵为方阵，如果同一矩阵既表示姿态又表示位置，那么可在矩阵中加入比例因子使之成为4×4矩阵，如果只表示姿态，则可去掉比例因子得到3×3矩阵，或加入第4列全为零的位置数据以保持矩阵为方阵。
平移齐次变换矩阵可以简单地表示为： 


T=Trans(dx，dy，dz)=100dx
010dy
001dz
0001(3.18)


其中dx，dy和dz是纯平移矢量d相对于参考坐标系x，y，z的三个分量。
5. 旋转齐次坐标变换
为了得到在参考坐标系中的坐标，旋转坐标系中的点P(或矢量P)的坐标必须左乘旋转矩阵。
对于绕轴x，y，z做转角为θ的旋转变换，其旋转齐次变换矩阵可以表示为： 


T=Rot(x，θ)=1000
0cθ－sθ0
0sθcθ0
0001(3.19)
T=Rot(y，θ)=cθ0sθ0
0100
－sθ0cθ0
0001(3.20)
T=Rot(z，θ)=cθ－sθ00
sθcθ00
0010
0001(3.21)


6. 复合变换
复合变换是由固定参考坐标系或者当前运动坐标系的一系列沿轴平移变换和旋转变换所组成的。任何变换都可以分解为按一定顺序的组合平移变换和旋转变换。
在实际应用中，点P相对于参考坐标系的坐标都是通过用相应的每个变换矩阵左乘该点的坐标得到的。如果相对于运动坐标系或当前坐标系的轴的变换，则为了计算当前坐标系中点的坐标相对于参考坐标系的变化，这时需要右乘变换矩阵。
例3.2在坐标系{A}中，点P的运动轨迹为： 首先绕Za轴转30°，又沿{A}的Xa轴移动10个单位，再沿{A}的Ya轴移动20个单位。如果点P在原来的位置为AP1=［4，8，0］T，用齐次坐标变换法求运动后的位置AP2。
解： 用齐次坐标变换来解答，可得实现旋转和平移的齐次复合变换矩阵为： 


T=0.866－0.5010
0.50.866020
0010
0001


已知： 


AP1=4
8
0
1


利用齐次坐标变换的复合变换矩阵，可得： 


AP2=TAP1=0.866－0.5010
0.50.866020
0010
00014
8
0
1=9.464
28.928
0
1


3.2正运动学与逆运动学
机器人运动学研究的是机器人的工作空间与关节空间之间的映射关系或机器人的运动学模型，包括正运动学问题和逆运动学问题两部分。
运动学问题是在不考虑引起运动的力和力矩的情况下，描述机械臂的运动。因此运动学描述的是一种几何方法。首先介绍正运动学问题，它是根据给定的机器人关节变量的取值确定末端执行器的位置和姿态。逆运动学问题是根据给定的末端执行器的位置和姿态确定机器人关节变量的取值。
3.2.1连杆参数与关节变量
机器人是由一组通过关节连接在一起的连杆组成。杆间的连接物称为关节。可以对杆件和关节进行编号，应用于矩阵中，表示机器人运动。编号的方法为： 固定的基座为连杆0，从基座起依次向上第一个可动连杆为连杆1，之后为连杆2、……、连杆n； 关节i连接连杆i-1和连杆i，即连杆i离基座近的一端(简称近端)有关节i，而离基座远的一端(简称远端)有关节i+1。
为了确定末端执行器在三维空间的位置和姿态，机器人至少需要6个关节(因为当描述一个物体在空间的位置和姿态时需要6个参数： 3个位置和3个姿态)。
1. 连杆参数
建立机器人运动学方程时，为了确定两个相邻关节轴的位置关系，可以把连杆看作刚体，用空间的直线表示关节轴，关节轴i可用空间的一条直线，也就是用一个矢量表示，连杆i绕关节轴i相对于连杆i-1转动。所以，在描述连杆运动时，一个连杆的运动用两个参数描述，这两个参数定义了空间两个关节轴之间的相对位置，具体如图3.7所示。
三维空间中，任意两轴之间的距离为确定值，两轴之间的距离就是轴心线之间的公垂线的长度。两轴的公垂线条数取决于轴心线是否平行。当轴心线平行时，有无数条长度相等的公垂线，而当轴心线不平行时，两轴之间的公垂线只有一条。
在图3.7中，关节轴i-1和关节轴i之间的公垂线长度记为ai－1，ai－1即为连杆长度。
两关节轴的相对位置关系除了连杆长度，还有另一个参数——连杆转角。它表示关节轴i-1和关节轴i投影在与两连杆公垂线垂直的平面内，测量的轴i-1依据右手法则绕关节长度ai－1转向轴i的角度。
在图3.7中，关节轴i-1和关节轴i之间的夹角角度记为αi－1，αi－1即为连杆转角。
2. 关节变量
相邻两连杆之间有一个共同的关节轴线。因此，每一关节轴线有两条公垂线与它垂直，每条公垂线相应于一个连杆。一般把这两条公垂线的距离称为连杆的偏距，记为di，它代表连杆i相对于连杆i-1的偏置。两公垂线之间的夹角称为关节角，记为θi，它代表连杆i相对于连杆i-1绕该轴线i的旋转角度。具体如图3.8所示。


图3.7连杆参数




图3.8关节变量



在图3.8中，公垂线ai－1和公垂线ai之间的长度记为di，di即为连杆偏距。
在图3.8中，公垂线ai－1的延长线和公垂线ai之间绕关节轴i旋转所形成的角度记为θi，θi即为关节角。
3.2.2DH方法与正运动学方程
DH方法是在1955年由Denavit和Hartenberg在ASME Journal of Applied Mechanics提出的。后来作者利用这篇论文对机器人进行表示和建模，并推导出了它们的运动方程，这已成为表示机器人和对机器人运动学建模的标准方法。
3.2.1节介绍的连杆参数和关节变量的四个参数： 连杆长度ai－1，连杆转角αi－1，连杆偏距di和关节角θi即为机器人的DenavitHartenberg参数，简称DH参数。它们是DH方法建立机器人运动学的基础。
DH法建立机器人运动学模型的步骤，首先需要指定单个关节的参考坐标系。参考图3.8，以下是给每个关节指定参考坐标系的步骤： 
(1) 指定z轴。坐标系{i}的z轴zi与关节轴i共线，指向任意规定。对于旋转关节，z轴位于按右手规则旋转方向，绕z轴的旋转θ是关节变量。对于滑动关节，z轴为沿直线运动的方向，沿z轴的连杆长度d是关节变量。关节i处的坐标原点位于轴i-1和i的公垂线与关节i轴线的交点上。若相邻连杆的轴线相交于一点，那原点就在交点上，若两轴线平行，则选择原点使对下一连杆的距离di+1=0。
(2) 指定x轴。通常在公垂线方向上定义本地参考坐标系的x轴。例如，如果ai－1表示zi－1与zi之间的公垂线，则定义xi－1的方向为沿ai－1的方向。同样，如果zi与zi+1之间的公垂线为ai，则xi的方向将沿ai的方向。如果ai=0，则取xi的方向为zi与zi+1的矢量积同轴方向上，可同向或反向。
(3) y轴。知道了x和z轴，y轴按右手法则确定。即为zi和xi的矢量积方向。
DH参数的正负取值。在建立机器人关节坐标系时，在关节轴i上，建立坐标系轴zi，zi正向在两个方向中选一个方向即可，但所有z轴应尽可能一致。DH法4个参数中，ai是大于等于0的，αi－1、θi的正负根据判定旋转矢量方向的右手法则确定，di的正负根据移动方向与zi一致时取正，否则取负号。
其次需要知道将一个参考坐标系变换到下一个参考坐标系的变换矩阵。
假设现在位于本地参考坐标系xi－1－zi－1，那么通过以下4步标准运动即可到达下一个本地参考坐标系xn－zn。
(1) 将zi－1轴绕xi－1轴旋转αi－1，使得zi－1与zi互相平行。
(2) 沿着xi－1轴平移ai－1的距离，使得zi－1与zi共线。这时两个坐标系的原点处在同一位置。
(3) 绕zi旋转θi。使得xi－1与xi互相平行。
(4) 沿zi轴平移di距离。这时坐标系i-1和i完全相同。
以上从一个坐标系变换到了下一个坐标系。表示前面4个运动的两个依次坐标之间的变换i-1iT是4个运动变换矩阵的乘积。由于所有的变换都是相对于当前的坐标系进行的，因此所有的变换矩阵都是右乘的。从而得到如下结果： 


i－1iT=Rot(x，αi－1)×Trans(x，ai－1，)×Rot(z，θi)×Trans(z，di)(3.22)


该公式把关节i-1变换到i的变换矩阵。
把式(3.18)~式（3.21），代入式(3.22)，可得连杆变换矩阵i－1iT的一般表达式： 


i－1iT=cθi－sθi0ai－1
sθicαi－1cθicαi－1－sαi－1－disαi－1
sθisαi－1cθisαi－1cαi－1dicαi－1
0001(3.23)



从机器人的参考坐标系开始，我们可以将其转换到机器人的第1个关节，再转换到第2个关节，以此类推，直至转换到末端执行器。注意，在任何坐标之间的变换均采用与前面相同的运动步骤。
在机器人的基座上，可以从第1个关节开始变换到第2个关节，然后到第3个关节，等等，直到机器人末端和最终的末端执行器。则机器人对基座的关系06T为： 


06T= 01T12T23T34T45T56T(3.24)


如果机器人6个关节中的变量分别是： θ1，θ2，θ3，θ4，θ5，θ6，则末端相对于基座的齐次矩阵也应该是包含这6个变量的4×4矩阵，即


06T= 01T(θ1)12T(θ2)23T(θ3)34T(θ4)45T(θ5)56T(θ6)(3.25)


此为机器人正向运动学的表达式，即已知机器人各关节值，计算出末端相对于基座的位姿。
一般为了简化T矩阵计算，可以制作一张关节和连杆参数的表格，其中每个关节和连杆的参数值可从机器人的结构示意图上确定，并且将这些参数代入T矩阵，如表3.1所示。


表3.1DH参数表



iαi－1ai－1diθi
1
…
n

例3.3PUMA560属于关节式机器人，6个关节都是转动关节，如图3.9所示。前3个关节确定手腕参考点的位置，后3个关节确定手腕的方位。与大多数工业机器人一样，后3个关节轴线交于一点。该点选作为手腕的参考点，也选作为连杆坐标系{4}，{5}和{6}的原点。关节1的轴线为铅直方向，关节2和关节3的轴线水平，且平行，距离为a2。关节1和关节2的轴线垂直相交，关节3和关节4的轴线垂直交错，距离为a3。各连杆坐标系如图3.9所示，相应的连杆参数列于表3.2。其中，a2=431.8mm，a3=20.32mm，d2=149.09mm，d4=433.07mm，d6=56.25mm。


图3.9PUMA560机器人DH坐标系


解： 


表3.2PUMA560 DH参数表



连杆iαi－1ai－1diθi变 量 范 围
10°00θ1(90°)-160°~160°
2-90°0d2θ2(0°)-225°~45°
30°a20θ3(－90°)-45°~225°
4-90°a3d4θ4(0°)-110°~170°
590°00θ5(0°)-100°~100°
6-90°00θ6(0°)-266°~266°

据式(3.23)和表3.2所示连杆参数，可求得各连杆变换矩阵如下： 



01T=cθ1－sθ100
sθ1cθ100
0010
0001，12T=cθ2－sθ200
001d2
－sθ2－cθ200
0001
23T=cθ3－sθ30a2
sθ3cθ300
0010
0001，34T=cθ4－sθ40a3
001d4
－sθ4－cθ400
0001
45T=cθ5－sθ500
00－10
sθ5cθ500
0001，56T=cθ6-sθ600
0010
-sθ6-cθ600
0001


各连杆变换矩阵相乘，得到PUMA560机器人的变换矩阵： 


06T= 01T(θ1)12T(θ2)23T(θ3)34T(θ4)45T(θ5)56T(θ6)


即为关节变量θ1，θ2，…，θ6的函数。要求解此运动方程，需先计算某些中间结果： 


46T= 45T56T=c5c6－c5s6－s50
s6c600
s5c6－s5s6c50
0001(3.26)
36T= 34T46T=c4c5c6－s4s6－c4c5c6－s4c6－c4s5a3
s5c6－s5s6c5d4
－s4c5c6－c4s6s4c5s6－c4c6s4s50
0001(3.27)


由于PUMA560的关节2和关节3相互平行，把12T(θ2)和23T(θ3)相乘


13T= 12T23T=c23－s230a2c2
001d2
－s23－c230－a2s2
0001(3.28)


其中，c23=cos(θ2+θ3)=c2c3－s2s3； s23=sin(θ2+θ3)=c2s3+s2c3。可见，两旋转关节平行时，利用角度之和的公式，可以得到比较简单的表达式。
再将式(3.27)与式(3.28)相乘，可得


16T= 13T36T=1nx1ox1ax1px
1ny1oy1ay1py
1nz1oz1az1pz
0001(3.29)


其中： 


1nx=c23(c4c5c6－s4s6)－s23s5c6，
1ny=－s4c5c6－c4s6，
1nz=－s23(c4c5c6－s4s6)－c23s5c6，
1ox=－c23(c4c5s6+s4c6)+s23s5s6，
1oy=s4c5s6－c4c6，
1oz=s23(c4c5s6+s4c6)+c23s5s6，
1ax=－c23c4s5－s23c5，
1ay=s4s5，
1az=s23c4s5－c23c5，
1px=a2c2+a3c23－d4s23，
1py=d2，
1pz=－a3s23－a2s2－d4c23，(3.30)


于是，可求得机器人的T变换矩阵： 


06T= 01T16T=nxoxaxpx
nyoyaypy
nzozazpz
0001(3.31)


其中： 


nx=c1c23(c4c5c6－s4s6)－s23s5c6+s1(s4c5c6+c4s6)，
ny=s1c23(c4c5c6－s4s6)－s23s5c6－c1(s4c5c6+c4s6)，
nz=－s23(c4c5c6－s4s6)－c23s5c6，
ox=c1c23(－c4c5s6－s4c6)+s23s5s6+s1(c4c6－s4c5s6)，
oy=s1c23(－c4c5s6－s4c6)+s23s5s6－c1(c4c6－s4c5c6)，
oz=－s23(－c4c5s6－s4c6)+c23s5s6，
ax=－c1(c23c4s5+s23c5)－s1s4s5，
ay=－s1(c23c4s5+s23c5)+c1s4s5，
az=s23c4s5－c23c5，
px=c1a2c2+a3c23－d4s23－d2s1，
py=s1a2c2+a3c23－d4s23+d2c1，
pz=－a3s23－a2s2－d4c23，(3.32)


式(3.31)表示的PUMA560手臂变换矩阵06T，描述了末端连杆坐标系{6}相对基坐标系{0}的位姿，是机器人运动分析的基础。
为校核所得06T的正确性，计算θ1=90°，θ2=0°，θ3=－90°，θ4=θ5=θ6=0°时，手臂变换矩阵06T的值。其计算结果为： 


06T=010－d2

001a2+d4

100a3

0001


与图3.9所示的情况完全一致。
3.2.3逆运动学方程
在3.2.2节讨论了机器人的正向运动学，此节将研究逆向运动学问题，即机器人运动方程的求解问题： 已知末端坐标系相对于基座坐标系的期望位置和姿态，求机器人能够达到预期位姿的关节变量。
以PUMA560机器人为例来阐述如何求解这些方程。
将PUMA560的运动方程(3.31)写为： 


06T=nxoxaxpx

nyoyaypy

nzozazpz

0001
= 01T(θ1)12T(θ2)23T(θ3)34T(θ4)45T(θ5)56T(θ6)(3.33)


若末端连杆的位姿已经给定，即n，o，a和p为已知，则求关节变量θ1，θ2，…，θ6的值称为运动反解。用未知的连杆逆变换同时左乘方程(3.33)两边，把关节变量分离出来，从而求解。具体步骤如下： 
1) 求θ1
可用逆变换01T－1(θ1)左乘方程(3.33)两边，


01T－1(θ1)06T= 12T(θ2)23T(θ3)34T(θ4)45T(θ5)56T(θ6)(3.34)


各式代入得


c1s100

－s1c100

0010

0001
nxoxaxpx
nyoyaypy
nzozazpz
0001= 16T(3.35)


令矩阵方程(3.35)两端的元素(2，4)对应相等，可得： 


－s1px+c1py=d2(3.36)


利用三角代换： 


px=ρcosΦ，py=ρsinΦ(3.37)


式中，ρ=p2x+p2y； Φ=atan2(py，px)。把代换式(3.37)代入式(3.36)，得到θ1的解： 


sin(Φ－θ1)=d2/ρ； cos(Φ－θ1)=±1－(d2/ρ)2

Φ－θ1=atan2d2ρ，±1－d2ρ2

θ1=atan2(py，px)－atan2d2，±p2x+p2y－d22(3.38)


式中，正负号对应于θ1的两个可能解。
2) 求θ3
在选定θ1的一个解之后，再令矩阵方程(3.35)两端的元素(1，4)和(3，4)分别对应相等，即得两方程： 


c1px+s1py=a3c23－d4s23+a2c2


－px=a3s23+d4c23+a2s2(3.39)


式(3.36)与式(3.39)的平方和为： 


a3c3－d4s3=k(3.40)


式中，k=p2x+p2y+p2z－a22－a23－d22－d242a2(3.41)
方程(3.40)中已经消去θ2，且方程(3.36)与方程(3.40)具有相同形式，因而可由三角代换求解θ3： 


θ3=atan2(a3，d4)－atan2k，±a23+d24－k2(3.42)


式中，正负号对应θ3的两种可能解。
3) 求 θ2
为求解θ2，在矩阵方程(3.33)两边左乘逆变换03T－1，有： 


03T－1(θ1，θ2，θ3)06T= 34T(θ4)45T(θ5)56T(θ6)(3.43)


即有


c1c23s1c23－s23－a2c3

－c1s23－s1s23－c23a2s3

－s1c10－d2

0001nxoxaxpx

nyoyaypy

nzozazpz

0001= 36T(3.44)


式中，变换36T由式(3.27)给出。令矩阵方程(3.44)两边的元素(1，4)和(2，4)分别对应相等，可得： 


c1c23px+s1c23py－s23pz－a2c3=a3


－c1s23px－s1s23py－c23px+a2s3=d4(3.45)


联立求解得s23和c23： 



s23=(－a3－a2c3)pz+(c1px+s1py)(a2s3－d4)p2z+(c1px+s1py)2

c23=(－d4+a2s3)pz－(c1px+s1py)(－a2c3－a3)p2z+(c1px+s1py)2


s23和c23表达式的分母相等，且为正。于是： 


θ23=θ2+θ3=atan2－(a3+a2c3)pz+(c1px+s1py)(a2s3－d4)，(－d4+a2s3)pz

+(c1px+s1py)(a2c3+a3)(3.46)


根据θ1和θ3解的四种可能组合，由式(3.46)可以得到相应的四种可能值θ23，于是可得到θ2的四种可能解： 


θ2=θ23－θ3(3.47)


式中，θ2取与θ3相对应的值。
4) 求θ4
因为式(3.44)的左边均为已知，令两边元素(1，3)和(3，3)分别对应相等，则可得： 


axc1c23+ays1c23－azs23=－c4s5


－axs1+ayc1=s4s5


只要s5≠0，便可求出θ4： 


θ4=atan2(－axs1+ayc1，－axc1c23－ays1c23+azs23)(3.48)


当s5=0时，机器人处于奇异形位。此时，关节轴4和6重合，只能解出θ4与θ6的和或差。奇异形位可以由式(3.48)中atan2的两个变量是否都接近零来判别。若都接近零，则为奇异形位； 否则，不是奇异形位。在奇异形位时，可任意选取θ4的值，再计算相应的θ6值。
5) 求θ5
据求出的θ4，可进一步解出θ5，将式(3.33)两端左乘逆变换04T－1(θ1，θ2，θ3，θ4)，可得： 


04T－1(θ1，θ2，θ3，θ4)06T= 45T(θ5)56T(θ6)(3.49)


式(3.49)的θ1，θ2，θ3，θ4前已求出，则逆变换04T－1(θ1，θ2，θ3，θ4)为： 


c1c23c4+s1s4s1c23c4－c1s4－s23c4－a2c3c4+d2s4－a3c4
－c1c23s4+s1c4－s1c23s4－c1c4s23s4a2c3s4+d2c4+a3s4
－c1s23－s1s23－c23a2s3－d4
0001


方程式(3.49)的右边46T(θ5，θ6)= 45T(θ5)56T(θ6)，由式(3.26)给出。据矩阵两边元素(1，3)和(3，3)分别对应相等，可得： 


ax(c1c23c4+s1s4)+ay(s1c23c4－c1s4)－az(s23c4)=－s5


ax(－c1s23)+ay(－s1s23)+az(－c23)=c5(3.50)


由此得到θ5的封闭解： 


θ5=atan2(s5，c5)(3.51)


6) 求θ6
将式(3.33)改写为： 


05T－1(θ1，θ2，…，θ5)06T= 56T(θ6)(3.52)


让矩阵方程(3.52)两边元素(3，1)和(1，1)分别对应相等，可得： 


－nx(c1c23s4－s1c4)－ny(s1c23s4+c1c4)+nz(s23s4)=s6

nx(c1c23c4+s1s4)c5－c1s23s5+ny(s1c23c4－c1s4)c5－s1s23s5

－nz(s23c4c5+c23s5)=c6


从而可求出θ6的封闭解： 


θ6=atan2(s6，c6)(3.53)


PUMA560的运动反解可能存在8种解。但是，由于结构的限制，例如各关节变量不能在全部360°范围内运动，有些解不能实现。在机器人存在多种解的情况下，应选取其中最满意的一组解，以满足机器人的工作要求。
3.3雅可比公式
雅可比矩阵表示机构部件随时间变化的几何关系，它可以将单个关节的微分运动或速度转换为末端执行器的微分运动或速度，也可将单个关节的运动与整个机构的运动联系起来。微分运动指机构(如机器人)的微小运动，可以用它来推导不同部件之间的速度关系。在对机器人进行操作与控制时，常常涉及机器人位置和姿态的微小变化。这些变化可由描述机器人位置的齐次变换矩阵的微小变化表示。由于关节角的值是随时间变化的，从而雅可比矩阵各元素的大小也随时间变化，因此雅可比矩阵是与时间相关的。
3.3.1雅可比矩阵
机器人的操作速度与关节速度的线性变换定义为机器人的雅可比矩阵，可视它为从关节空间向操作空间运动速度的传动比。
令六自由度机器人的运动方程为： 


xi=fi(q1，q2，q3，q4，q5，q6)(3.54)


代表操作空间x与关节空间q之间的位移关系。
由qi的微分变化引起的xi的微分变化为： 


δx1=f1q1δq1+f1q2δq2+f1q3δq3+f1q4δq4+f1q5δq5+f1q6δq6

δx2=f2q1δq1+f2q2δq2+f2q3δq3+f2q4δq4+f2q5δq5+f2q6δq6
δx3=f3q1δq1+f3q2δq2+f3q3δq3+f3q4δq4+f3q5δq5+f3q6δq6
δx4=f4q1δq1+f4q2δq2+f4q3δq3+f4q4δq4+f4q5δq5+f4q6δq6
δx5=f5q1δq1+f5q2δq2+f5q3δq3+f5q4δq4+f5q5δq5+f5q6δq6
δx6=f6q1δq1+f6q2δq2+f6q3δq3+f6q4δq4+f6q5δq5+f6q6δq6(3.55)


式（3.55）写为矩阵形式： 


δx1
δx2
δx3
δx4
δx5
δx6=f1q1f1q2f1q3f1q4f1q5f1q6
f2q1f2q2f2q3f2q4f2q5f2q6
f3q1f3q2f3q3f3q4f3q5f3q6
f4q1f4q2f4q3f4q4f4q5f4q6
f5q1f5q2f5q3f5q4f5q5f5q6
f6q1f6q2f6q3f6q4f6q5f6q6δq1
δq2
δq3
δq4
δq5
δq6或δxi=fiqjδqj
(3.56)


式（3.56）表示各单个变量和函数之间的微分关系。包含这一关系的矩阵称为雅可比矩阵。
同理，对六自由度机器人位置方程求微分，可写下列矩阵形式： 


dx
dy
dz
δx
δy
δz=机器人
雅可比
矩阵dθ1
dθ2
dθ3
dθ4
dθ5
dθ6或D=JDθ(3.57)


其中，D中的dx、dy、dz分别表示机器人手爪沿x、y、z轴的微分运动； D中的δx、δy、δz分别表示机器人手爪绕x、y、z轴的微分旋转； Dθ表示关节的微分运动。
将式(3.54)或式(3.57)两边对时间求导，即得出q与x之间的微分关系


x·=J(q)q·(3.58)


式中，x为末端在操作空间的广义速度，简称操作速度； q为关节速度； J(q)是6×6的偏导数矩阵，称为机器人的雅可比矩阵。
例3.4给定某时刻的机器人雅可比矩阵如下，计算在给定微分运动的情况下，机器人末端执行器坐标系的线位移微分运动和角位移微分运动。


J=100020
－101000
010000
000300
001000
000001，D=0.1
0
－0.1
0
0
0.1


解： 根据式(3.57)，可得： 



D=JDθ=100020
－101000
010000
000300
001000
0000010.1
0
－0.1
0
0
0.1=0.1
-0.2
0
0
-0.1
0.1=dx
dy
dz
δx
δy
δz


3.3.2机器人的微分运动
1. 微分平移

用式Trans(dx，dy，dz)表示坐标系中微分平移dx，dy，dz的变换，其含义为坐标系沿x，y，z轴做微小运动。


Trans(dx，dy，dz)=100dx
010dy
001dz
0001(3.59)


2. 绕坐标系轴微分旋转
用式Rot(x，δx)、Rot(y，δy)、Rot(z，δz)表示坐标系中微分绕x，y，z轴旋转的变换，其含义为坐标系绕x，y，z轴做微小旋转。
在旋转量很小的情况下，近似下列值： 
sinδx=δx(以弧度值计)，cosδx=1，则可得： 


Rot(x，δx)=1000
01－δx0
0δx10
0001(3.60)
Rot(y，δy)=10δy0
0100
－δy010
0001(3.61)
Rot(z，δz)=1－δz00
δz100
0010
0001(3.62)


3. 绕任意轴q轴微分旋转
在数学计算中，若略去高阶微分，即使δxδy=0，可证明： 


Rot(x，δx)Rot(y，δy)=Rot(y，δy)Rot(x，δx)(3.63)


由此可得，在微分旋转计算中，矩阵相乘的顺序对计算结果影响甚小，故而在微分旋转计算中可交换相乘顺序。可得： 


Rot(q，dθ)=Rot(x，δx)Rot(y，δy)Rot(z，δz)
=1000
01－δx0
0δx10
000110δy0
0100
－δy010
00011－δz00
δz100
0010
0001
=1－δzδy0
δxδy－δxδyδz+1－δx0
－δy+δxδzδx+δyδz10
0001(3.64)


忽略所有高阶微分，可得： 


Rot(q，dθ)=Rot(x，δx)Rot(y，δy)Rot(z，δz)=1－δzδy0
δz1－δx0
－δyδx10
0001(3.65)


例3.5求绕三个坐标系轴旋转小的旋转后，所得总微分变换。其中(δx=0.05rad、δy=0.1rad、δz=0.05rad)。
解： 由式(3.65)，可得： 


Rot(q，dθ)=1－δzδy0
δz1－δx0
－δyδx10
0001=1－0.050.10
0.051－0.050
－0.10.0510
0001


4. 微分算子
机器人实际操作和运行时，是微分平移和绕任意轴旋转的复杂运动。假如用T表示原始坐标系，dT表示变化量，若沿坐标系轴进行运动，则有下式表达： 


T+dT=Trans(dx，dy，dz)Rot(q，dθ)T(3.66)


移项得： dT=Trans(dx，dy，dz)Rot(q，dθ)－IT，I为单位矩阵。
令Δ=Trans(dx，dy，dz)Rot(q，dθ)－I，则上式可简化为： 


dT=ΔT(3.67)


其中Δ称为对于固定基坐标系的微分算子。


Δ=100dx
010dy
001dz
00011－δzδy0
δz1－δx0
－δyδx10
0001－1000
0100
0010
0001(3.68)
Δ=0－δzδydx
δz0－δxdy
－δyδx0dz
0000(3.69)


同理，若沿着当前坐标系进行运动，则可得到如下公式： 


TΔ=0－TδzTδyTdx
Tδz0－TδxTdy
－TδyTδx0Tdz
0000(3.70)


TΔ称为对于当前坐标系的微分算子。
例3.6已知坐标系{A}和对基坐标系的微分平移与微分旋转为： 


A=0018
1004
0100
0001
d=0.8，0，0.6，δ=0，0.2，0


试求微分变换结果。
解： 首先据式(3.69)可得下式： 


Δ=000.20.8
0000
－0.2000.6
0000


再按照dT=ΔT，有dA=ΔA，即


dA=000.20.8
0000
－0.2000.6
00000018
1004
0100
0001=00.200.8
0000
00－0.2－1
0000




图3.10坐标系{A}微分变化

坐标系{A}的这一微分变化如图3.10所示。
5. 微分运动的等价变换
要求得机器人的雅可比(Jacobian)矩阵，就需要把一个坐标系内的位置和姿态的微分变化，变换为另一坐标系内的等效表达式。


dT=ΔT=TTΔ


等式两边同时乘以T－1，得到T－1ΔT=T－1TTΔ


所有TΔ=T－1ΔT(3.71)


若坐标系T用noap矩阵表达，则： 


T=nxoxaxpx
nyoyaypy
nzozazpz
0001
可得： 

T－1=nxnynz－p·n
oxoyoz－p·o
axayaz－p·a
0001(3.72)
ΔT=0－δzδydx
δz0－δxdy
－δyδx0dz
0000nxoxaxpx
nyoyaypy
nzozazpz
0001(3.73)


利用式TΔ=T－1ΔT可得TΔ的表达式为： 


ΔT=－δzny+δynz－δzoy+δyoz－δzay+δyaz－δzpy+δypz+dx
δznx+δxnzδzox+δxozδzax+δxazδzpx+δxpz+dy
－δynx+δxny－δyox+δxoy－δyax+δxay－δypx+δxpy+dz
0000
(3.74)


它与下式等价： 


ΔT=(δ×n)x(δ×o)x(δ×a)x(δ×p+d)x
(δ×n)y(δ×o)y(δ×a)y(δ×p+d)y
(δ×n)z(δ×o)z(δ×a)z(δ×p+d)z
0000(3.75)


式中，下标表示只取某方向的一个量。
用T－1左乘式（3.75）得： 


T－1ΔT=nxnynz－p·n
oxoyoz－p·o
axayaz－p·a
0001(δ×n)x(δ×o)x(δ×a)x(δ×p+d)x
(δ×n)y(δ×o)y(δ×a)y(δ×p+d)y
(δ×n)z(δ×o)z(δ×a)z(δ×p+d)z
0000(3.76)
T－1ΔT=n·(δ×n)n·(δ×o)n·(δ×a)n·(δ×p+d)
o·(δ×n)o·(δ×o)o·(δ×a)o·(δ×p+d)
a·(δ×n)a·(δ×o)a·(δ×a)a·(δ×p+d)
0000(3.77)


应用三矢量相乘的两个性质a·(b×c)=b·(c×a)及a·(a×c)=0，并据式(3.71)可把式(3.77)变换为： 


TΔ=0－δ·(n×o)δ·(a×n)δ·(p×n)+d·n
δ·(n×o)0－δ·(o×a)δ·(p×o)+d·o
－δ·(a×n)δ·(o×a)0δ·(p×a)+d·a
0000(3.78)


化简得： 


TΔ=0－δ·aδ·oδ·(p×n)+d·n
δ·a0－δ·nδ·(p×o)+d·o
－δ·oδ·n0δ·(p×a)+d·a
0000(3.79)


由于TΔ已被式(3.70)所定义，所以令式(3.70)与式(3.79)各元分别相等，可求得下列各式： 


Tδx=δ·n，Tδy=δ·o，Tδz=δ·a(3.80)
Tdx=δ·(p×n)+d·n
Tdy=δ·(p×o)+d·o
Tdz=δ·(p×a)+d·a(3.81)


式中n，o，a和p，分别为微分坐标变换T的列矢量。从上列两式可得微分运动TD和D关系如下： 


Tdx
Tdy
Tdz
Tδx
Tδy
Tδz=nxnynz(p×n)x(p×n)y(p×n)z
oxoyoz(p×o)x(p×o)y(p×o)z
axayaz(p×a)x(p×a)y(p×a)z
000nxnynz
000oxoyoz
000axayazdx
dy
dz
δx
δy
δz(3.82)


应用三矢量相乘的性质a·(b×c)=c·(a×b)，我们可进一步将式(3.80)和式(3.81)写为： 


Tdx=n·((δ×p)+d)
Tdy=o·((δ×p)+d)
Tdz=a·((δ×p)+d)(3.83)
Tδx=n·δ
Tδy=o·δ
Tδz=a·δ(3.84)


应用上述二式，能够方便地把对基坐标系的微分变化变换为对坐标系T的微分变化。式(3.82)可简写为： 


Td
Tδ=RT－SRT(p)
0RTd
δ(3.85)


式中，R是旋转矩阵，


R=nxnxax
nynzay
nzozaz(3.86)


对于任何三维矢量P=px，py，pzT，其反对称矩阵S(p)定义为： 


S(p)=0－pxpy
pz0－px
－pypx0(3.87)


例3.7已知坐标系A和对基坐标系的微分平移与微分旋转为： 


A=0018
1004
0100
0001


d=0.8，0，0.6，δ=0，0.2，0。试求对坐标系A的等价微分平移和微分旋转。
解： 因为


n=0，1，0，o=0，0，1，a=1，0，0，p=8，4，0，

d=0.8，0，0.6，δ=0，0.2，0


以及


δ×p=ijk
00.20
840=0，0，-1.6


δ×p+d=［0，0，-1.6］+［0.8，0，0.6］=［0.8，0，－1］
又据式(3.83)和式(3.84)，可求得等价微分平移和微分旋转为： 


Ad=［0，－1，0.8］，Aδ=［0.2，0，0］



据式dT=TTΔ计算dA=AAΔ，以检验所得微分运动是否正确。据式(3.70)有： 


AΔ=0000
00－0.2-1
00.200.8
0000
dA=0018
1004
0100
00010000
00－0.2-1
00.200.8
0000


即


dA=00.200.8
0000
00－0.2－1
0000


所得结果与例3.6一致。可见所求得的对A的微分平移和微分旋转是正确无误的。
3.3.3雅可比矩阵的计算
雅可比矩阵中的每个元素是对应的运动学方程对其中一个变量的导数，参考式(3.57)。
可以看到，D中的第一个元素是dx，它表示第一个运动学方程且必须沿x轴的运动，即px。换句话说，px表示手的坐标系沿x轴的运动，它的微分为dx。同样，dy和dz也是如此。若考虑用n、o、a和p表示的矩阵，对相应的元素px、py和pz求微分就可得到dx、dy和dz。
1. 微分变换法
对于转动关节i，连杆i相对连杆i-1绕坐标系{i}的zi轴做微分转动dθi，其微分运动矢量为： 


d=0
0
0，δ=0
0
1dθi(3.88)


利用式(3.82)得出手爪相应的微分运动矢量为： 


Tdx
Tdy
Tdz
Tδx
Tδy
Tδy=(p×n)z
(p×o)z
(p×a)z
nz
oz
azdθi(3.89)


对于移动关节，连杆i沿zi轴相对于连杆i-1做微分移动ddi，其微分运动矢量为： 


d=0
0
1ddiδ=0
0
0(3.90)


而手爪的微分运动矢量为： 


Tdx
Tdy
Tdz
Tδx
Tδy
Tδy=nz
oz
az
0
0
0ddi(3.91)


于是，可得雅可比矩阵J(q)的第i列如下： 
对于转动关节i有： 


TJli=(p×n)z
(p×o)z
(p×a)z，TJai=nz
oz
az(3.92)


对于移动关节i有： 


TJli=nz
oz
az，TJai=0
0
0(3.93)


式中，n，o，a，p是inT的4个列向量。
上述求雅可比TJ(q)的方法是构造性的，只要知道各连杆变换i－1iT，就可自动生成雅可比矩阵，不需求解方程。其自动生成的步骤如下： 
(1) 计算各连杆变换01T，12T，L，n-1nT。TJi和inT之间的关系如图3.11所示。


图3.11TJi和inT之间的关系

(2) 计算各连杆至末端连杆的变换(见图3.11)： 


n－1nT=n－1nT，n－2nT= n－2n－1Tn－1nT，…，
i－1nT= i－1iTinT，…，0nT= 01T1nT


(3) 计算J(q)的各列元素，第i列TJi由inT决定。根据式(3.90)和式(3.91)计算TJli和TJai。
2. 雅可比计算
例3.8PUMA560的六个关节均为转动，所以它的雅可比矩阵有六列，以微分变换法求其雅可比矩阵。
解： TJ(q)第一列TJ1(q)对应变换矩阵记为16T，根据上节内容，可得： 


TJ1(q)=TJ1x
TJ1y
TJ1z
－s23(c4c5c6－s4s6)－c23s5c6
s23(c4c5s6+s4c6)+c23s5c6
－s23c4s5－c23c5(3.94)


其中


TJ1x=－d2c23(c4c5c6－s4s6)－s23s5c6－(a2c2+a3c23－d4s23)(s4c5c6+c4s6)
TJ1y=－d2－c23(c4c5s6+s4c6)+s23s5s6+(a2c2+a3c23－d4s23)(s4c5s6－c4c6)

TJ1z=d2(c23c5s5+s23c5)+(a2c2+a3c23－d4s23)s4s5


由变换矩阵26T，可得： 


TJ2(q)=TJ2x
TJ2y
TJ2z
－s4c5c6－c4s6
s4c5s6－c4c6
s4s5(3.95)


其中


TJ2x=a3s5c6－d4(c4c5c6－s4s6)+a2s3(c4c5c6－s4s6)+c3s5c6
TJ2y=－a3s5s6－d4(－c4c5s6－s4c6)+a2s3(－c4c5s6－s4c6)+c3s5s6
TJ2z=a3c6+d4c4s5+a2(－s3c4s5+c3c6)


同理可得： 


TJ3(q)=－d4(c4c5c6－s4s6)+a3(s5c6)
d4(c4c5s6+s4c6)－a3(s5s6)
d4c4s5+a3c6
－s4c5c6－c4s6
s4c5s6－c4c6
s4s5(3.96)
TJ4(q)=0
0
0
s5c6
－s5s6
c5(3.97)
TJ5(q)=0
0
0
－s6
－c6
0(3.98)
TJ6(q)=0
0
0
0
0
1(3.99)


3.4动力学方程
动力学方程描述力和运动之间的关系。当机器人手臂加速时，驱动器就要有足够的力和力矩驱动机器人的连杆与关节，以获得期望的速度和加速度。解决此类问题最常用的是拉格朗日方法和牛顿—欧拉方法。拉格朗日方法只需从能量的角度列写计算公式，计算相对比较简单； 而牛顿—欧拉方法需要计算内部作用力，多自由度机器人计算起来比较复杂，常用于数值计算中。本节主要介绍拉格朗日方程。
3.4.1拉格朗日法
拉格朗日函数L被定义为系统的动能K和势能P之差，即


L=K－P(3.100)


对直线运动，拉格朗日方程如下： 


Fi=ddtLx·i－Lxi，i=1，2，…，n(3.101)


对旋转运动，拉格朗日方程如下： 


Ti=ddtLθ·i－Lθi，i=1，2，…，n(3.102)


式中，xi和θi为系统变量； Fi是产生线性运动的外力之和； Ti是产生旋转运动的外力矩之和； n为连杆数目。


图3.12二自由度机器人

为了得到运动方程，需要推导能量方程，根据能量方程求得拉格朗日方程，然后根据以上两个公式求得动力学方程。
例3.9求图3.12的二自由度机器人的动力学方程，其两个关节均为旋转关节。图中，m1和m2为连杆1和连杆2的质量，且以连杆末端的点质量表示； d1和d2分别为两连杆的长度，θ1和θ2为广义坐标； g为重力加速度。
解： (1) 计算动能。
连杆1的动能为


K1=12m1v21


式中


v1=d1θ·1


则


K1=12m1d21θ·21


连杆2的动能为


K2=12m2v22


式中


v22=x·22+y·22

x2=d1sinθ1+d2sin(θ1+θ2)

y2=－d1cosθ1－d2cos(θ1+θ2)

x·2=d1cosθ1θ·1+d2cos(θ1+θ2)(θ·1+θ·2)

y·2=d1sinθ1θ·1+d2sin(θ1+θ2)(θ·1+θ·2)


求得： v22=d21θ·21+d22(θ·21+2θ·1θ·2+θ·22)+2d1d2cosθ2(θ·21+θ·1θ·2)

则K2=12m2d21θ·21+12m2d22(θ·1+θ·2)2+m2d1d2cosθ2(θ·21+θ·1θ·2)

总动能： K=K1+K2


K=12(m1+m2)d21θ·21+12m2d22(θ·1+θ·2)2+m2d1d2cosθ2(θ·21+θ·1θ·2)(3.103)


(2) 计算势能。
连杆1的势能为： 


P1=m1gh1


式中h1=－d1cosθ1
则P1=－m1gd1cosθ1
连杆2的势能为： 


P2=mgy2


式中y2=－d1cosθ1－d2cos(θ1+θ2)
则P2=－m2gd1cosθ1－m2gd2cos(θ1+θ2)
总势能： P=P1+P2


P=－(m1+m2)gd1cosθ1－m2gd2cos(θ1+θ2)(3.104)


(3) 计算拉格朗日方程。
由以上求得的总动能和总势能，可得拉格朗日方程


L=K－P

=12(m1+m2)d21θ·21+12m2d22(θ·21+2θ·1θ·2+θ·22)+m2d1d2cosθ2(θ·21+θ·1θ·2)

+(m1+m2)gd1cosθ1+m2gd2cos(θ1+θ2)(3.105)


(4) 二自由度机器人动力学方程。
对L求偏导数和导数： 


Lθ1=－(m1+m2)gd1sinθ1－m2gd2sin(θ1+θ2)

Lθ2=－m2d1d2sinθ2(θ·21+θ·1θ·2)－m2gd2sin(θ1+θ2)

Lθ·1=(m1+m2)d21θ·1+m2d22θ·1+m2d22θ·2+2m2d1d2cosθ2θ·1+m2d1d2cosθ2θ·2

Lθ·2=m2d22θ·1+m2d22θ·2+m2d1d2cosθ2θ·1


以及


ddtLθ·1=［(m1+m2)d21+m2d22+2m2d1d2cosθ2］θ¨1+(m2d22+m2d1d2cosθ2)θ¨2

－2m2d1d2sinθ2θ·1θ·2－m2d1d2sinθ2θ·22

ddtLθ·2=m2d22θ¨1+m2d22θ¨2+m2d1d2cosθ2θ¨1－m2d1d2sinθ2θ·1θ·2


把相应各导数和偏导数代入式(3.102)中，即可求得力矩T1和T2的动力学方程式： 


T1=ddtLθ·1－Lθ1

=［(m1+m2)d21+m2d22+2m2d1d2cosθ2］θ¨1+(m2d22+m2d1d2cosθ2)θ¨2
－2m2d1d2sinθ2θ·1θ·2－m2d1d2sinθ2θ·22+(m1+m2)gd1sinθ1+m2gd2sin(θ1+θ2)(3.106)

T2=ddtLθ·2－Lθ2

=(m2d22+m2d1d2cosθ2)θ¨1+m2d22θ¨2
+m2d1d2sinθ2θ·21+m2gd2sin(θ1+θ2)
(3.107)


式(3.106)和式(3.107)的一般形式和矩阵形式如下： 

T1=D11θ¨1+D12θ¨2+D111θ·21+D122θ·22+D112θ·1θ·2+D121θ·2θ·1+D1(3.108)
T2=D21θ¨1+D22θ¨2+D211θ·21+D222θ·22+D212θ·1θ·2+D221θ·2θ·1+D2(3.109)
T1
T2=D1D12
D21D22θ¨1
θ¨2+D111D122
D211D222θ·21
θ·22+D112D121
D212D221θ·1θ·2
θ·2θ·1+D1
D2(3.110)

式中，Dii称为关节i的有效惯量，因为关节i的加速度θ¨i将在关节i上产生一个等于Diiθ¨i的惯性力； Dij称为关节i和j间耦合惯量，因为关节i和j的加速度θ¨i和θ¨j将在关节j或i上分别产生一个等于Dijθ¨i或Dijθ¨j的惯性力； Dijkθ·21项是由关节j的速度θ·j在关节i上产生的向心力； (Dijkθ·jθ·k+Dijkθ·kθ·j)项是由关节j和k的速度θ·i和θ·k引起的作用于关节i的哥氏力； Di表示关节i处的重力。
比较式(3.106)、式(3.107)与式(3.108)、式(3.109)，可得本系统各系数如下： 
有效惯量


D11=(m1+m2)d21+m2d22+2m2d1d2cosθ2

D22=m2d22


耦合惯量


D12=m2d22+m2d1d2cosθ2=m2(d22+d1d2cosθ2)


向心加速度系数


D111=0

D122=－m2d1d2sinθ2

D211=m2d1d2sinθ2

D222=0


哥氏加速度系数


D112=D121=－m2d1d2sinθ2

D212=D221=0


重力项


D1=(m1+m2)gd1sinθ1+m2gd2sin(θ1+θ2)

D2=m2gd2sin(θ1+θ2)


3.4.2多自由度机器人拉格朗日动力学
从以上的二自由度机器人分析中，可以同理推导出多自由度机器人动力学方程。推导过程分5步进行： 
（1） 计算任一连杆上任一点的速度； 
（2） 计算各连杆的动能和机器人的总动能； 
（3） 计算各连杆的势能和机器人的总势能； 
（4） 建立机器人系统的拉格朗日函数； 
（5） 对拉格朗目函数求导，以得到动力学方程式。
图3.13表示一个多自由度机器人的结构(连杆2和连杆1之间是滑动连接，其他是旋转连接)，以它为例，求得此机器人某个连杆(如连杆3)上某一点(如点P)的速度，质点和机器人的动能与势能、拉格朗日方程，再求系统的动力学方程式。


图3.13多自由度机器人



1. 动能
1) 速度的计算
图3.13中连杆3上点P的位置为： 


0rp=T33rp


式中，0rp为总(基)坐标系中的位置矢量； 3rp为局部(相对关节O3)坐标系中的位置矢量； T3为变换矩阵，包括旋转变换和平移变换。
对于任一连杆i上的一点，其位置为： 


0r=Tiir(3.111)


P点的速度为： 


0vp=ddt(0rp)=ddt(T33rP)=T·33rp(3.112)


式中，T·3=dT3dt=∑3j=1T3qiq·j，所以有： 


0vp=∑3j=1T3qjq·i(3rp)


对于连杆i上任一点的速度为： 


v=drdt=∑ij=1Tiqjq·jir


P点的加速度为： 


0ap=ddt(0vp)=ddt(T·33rp)=T¨33rp=ddt∑3j=1T3qiq·i(3rp)

=∑3j=1T3qiddtq·(3rp)+∑3k=1∑3j=12T3qjqkq·kq·j(3rp)

=∑3j=1T3qiq¨i(3rp)+∑3k=1∑3j=12T3qjqkq·kq·j(3rp)


速度的平方为： 


(0vp)2=(0vp)·(0vp)=Trace(0vp)·(0vp)T

=Trace∑3j=1T3qjq·j(3rp)·∑3k=1T3qkq·k(3rp)T

=Trace∑3j=1∑3k=1T3qj(3rp)(3rp)TT3qkq·jq·k


对于任一机器人上一点的速度平方为： 


v2=drdt2=Trace∑ij=1Tiqiq·jir∑ik=1Tiqkq·kirT

=Trace∑ij=1∑ik=1TiqkirirTTiqkTq·kq·k(3.113)


式中，Trace表示矩阵的迹。对于n阶方阵来说，其迹即为它的主对角级上各元素之和。
2) 质点动能的计算
令连杆3上任一质点处的质量为dm，则其动能为： 


dK3=12v2pdm

=12Trace∑3j=1∑3k=1T3qi3rp(3rp)TT3qkTq·iq·kdm

=12Trace∑3j=1∑3k=1T3qi(3rpdm3rTp)TT3qkTq·iq·k(3.114)


任一机器人连杆上位置矢量r的质点，其动能如下式所示： 


dKK=12Trace∑ij=1∑ik=1TiqjjrirTTiqkq·jq·kdm

=12Trace∑ij=1∑ik=1Tiqj(irdmirT)TTTiqkq·jq·k(3.115)


对连杆3积分dK3，得连杆3的动能为： 


K3=∫连杆3dK3=12Trace∑3j=1∑3k=1T3qj∫连杆33rp3rTpdmT3qkTq·jq·k(3.116)


式中，积分∫3r3prTpdm称为连杆的伪惯量矩阵，并记为： 


I3=∫连杆33rp3rTpdm


这样，


K3=12Trace∑3j=1∑3k=1T3qkI3T3qkTq·jq·k(3.117)


任何机器人上任一连杆i动能为： 


Ki=∫连杆idKi=12Trace∑ij=1∑ik=1TiqkIiTiqkq·jq·k(3.118)


式中，Ii为伪惯量矩阵，其一般表达式为： 



Ii=∫连杆iirirTdm=∫iirirTdm

=∫iix2dm∫iixiydm∫iixizdm∫iixdm

∫iixiydm∫iiy2dm∫iiyizdm∫iiydm

∫iixizdm∫iiyizdm∫iiz2dm∫iizdm

∫iixdm∫iiydm∫iizdm∫idm(3.119)


根据理论力学或物理学可知，物体的转动惯量、矢量积以及一阶矩量为： 


Ixx=∫(y2+z2)dm，Iyy=∫(x2+z2)dm，Izz=∫(x2+y2)dm；
Ixy=Iyx=∫xydm，Ixz=Izx=∫xzdm，Iyz=Izy=∫yzdm；

mx=∫xdm，my=∫ydm，mz=∫zdm


如果令


∫x2dm=－12∫(y2+z2)dm+12∫(x2+z2)dm+12∫(x2+y2)dm

=(－Ixx+Iyy+Izz)2

∫y2dm=+12∫(y2+z2)dm－12∫(x2+z2)dm+12∫(x2+y2)dm

=(+Ixx－Iyy+Izz)2

∫z2dm=+12∫(y2+z2)dm+12∫(x2+z2)dm－12∫(x2+y2)dm

=(+Ixx+Iyy－Izz)2


于是可把Ii表示为： 


Ii=－Iixx+Iiyy+Iizz2IixyIixzmix-i

IixyIixx－Iiyy+Iizz2Iiyzmiy-i

IixzIiyzIixx+Iiyy－Iizz2miz-i

mix-imiy-imiz-imi(3.120)


具有n个连杆的机器人总的动能为： 


K=∑ni=1Ki=12∑ni=1Trace∑nj=1∑ik=1TiqiIiTTiqkq·iq·k(3.121)


此外，连杆i的传动装置动能为： 


Kai=12Iaiq·2i(3.122)


式中，Iai为传动装置的等效转动惯量，对于平动关节； Ia为等效质量； q·i为关节i的速度所有关节的传动装置总动能为： 


Ka=12∑ni=1Iaiq·2i(3.123)


于是得到机器人系统(包括传动装置)的总动能为： 


Kt=K+Ka

=12∑6i=1∑ij=1∑ik=1TraceTiqiIiTTiqkq·jq·k+12∑6i=1Iaiq·2i(3.124)


2. 势能
下面再来计算机器人的势能。众所周知，一个在高度h处质量为m的物体，其势能为： 


P=mgh(3.125)


连杆i上位置ir处的质点dm。其势能为： 


dPi=－dmgT0r+ －gTTiirdm(3.126)


式中，gT=gx，gy，gz，1。


Pi=∫连杆idPi=－∫连杆igTTiirdm=－gTTi∫连杆iirdm


其中，mi为连杆i的质量； iri为连杆i相对于其前端关节坐标系的重心位置。
由于传动装置的重力作用Pai一般是很小的，可以忽略不计，所以，机器人系统的总势能为： 


P=∑ni=1(Pi－Pai)≈∑ni=1Pi=－∑ni=1migTTiiri(3.127)


3. 多自由度机器人动力学方程
据式(3.100)求拉格朗日函数


L=Kt－P

=12∑ni=1∑nj=1∑nk=1TraceTiqkIiTTiqkq·jq·k+12∑ni=1Iaiq·2i+∑ni=1migTTiiri，n=1，2，…
(3.128)


根据式（3.128）可以求得动力学方程。由于篇幅现实，本书的求导过程省略，感兴趣的读者可查阅和参考相关书籍。
多自由度机器人动力学方程的最终形式为： 


Ti=∑nj=1Dijq¨j+Iaiq¨i+∑nj=1∑nk=1Dijkq·jq·k+Di(3.129)


其中


Dij=∑np=max(i，j)TraceTpqjIpTTpqi(3.130)
Dijk=∑np=max(i，j，k)Trace2TpqjqkIiTTpqi(3.131)
Di=∑np=i－mpgTTpqiprp(3.132)


上述各方程中，第一部分是角加速度惯量项，第二部分是传动装置惯量项，第三部分是科里奥利力和向心力项，第四部分是重力项。
4. 拉格朗日动力学方程的简化
3.4.2节中惯量项Dij和重力项Di等的计算必须化简，便于进行实际计算。
1) 惯量项Dij的简化
3.3节中讨论雅可比矩阵时，曾得到偏导数T6/qi=TT66Δi，这实际上是p=6时的特例。可以把它推广至一般形式： 


Tpqi=TTppΔi(3.133)


式中，TpΔi=(Ai，Ai+1，…，Ap)－1i+1Δi(Ai，Ai+1，…，AP)，而微分坐标变换为: 


i－1Tp=(Ai，Ai+1，…，Ap)


对于旋转关节，据式(3.74)可得如下微分平移矢量和微分旋转矢量： 


pdix=－i－1npxi－1ppy+i－1npyi－1ppx

pdiy=－i－1opxi－1ppy+i－1opyi－1ppx

pdiz=－i－1apxi－1ppy+i－1apyi－1ppx(3.134)

pδi=i－1npzi+i－1opzj+i－1apzk(3.135)


上式中采用了下列缩写： 把TPdi写为pdi，把Ti－1n写成i－1np，等等。
对于平移关节，据式(3.79)可得各矢量为: 


pdi=i－1ppzi+i－1opzj+i－1apzk
pδi=0i+0j+0k


以式(3.133)代入式(3.130)得： 


Dij=∑6p=max{i，j}Trace(TppΔjIppΔiTTTp)(3.136)


对式（3.136）中间三项展开得： 


Dij=∑6p=max(i，j)TraceTp0－pδjzpδjypdjx

pδjz0－pδjxpdjy

－pδjypδjx0pdjz

0000X

－Ixx+Iyy+Izz2IxyIxzmix-i

Ixy－Ixx－Iyy+Izz2Iyzmiy-i

IxzIyzIxx+Iyy－Izz2miz-i

mix-imiy-imiz-imi
X0pδix－pδiy0
－pδiz0pδix0

pδiy－pδix00

pδixpδiypdiz0TTp(3.137)


这中间三项是由式(3.69)、式(3.120)和式(3.69)的转置得到的。它们相乘所得矩阵的底行及右列各元均为零。当它们左乘Tp和右乘TTp时，只用到Tp变换的旋转部分。在这种运算下，矩阵的迹为不变式。因此，只要上述表达式中间三项的迹，它的简化矢量为： 


Dij=∑6p=max{i，y］mppδiTkppδj+pdipdj+pr-j(pdi×pδj+pdj×pδj)(3.138)


式中，


kp=k2pxx－kpxy－k2pxy

－k2pxyk2pyy－k2pyz

－k2pxz－k2pyzk2pzz


以及


mpk2pxx=Ipxx，mpk2pxz=Ipzz，mpk2pzz=Ipzz
mpk2pxy=Ipxy，mpk2pyz=Ipyz，mpk2pxz=Ipxz


如果设定上式中非对角线各惯量项为0，即为一个正态假设，那么式(3.134)进一步简化为： 


Dij=∑6p=max{i，j}mppδixk2pxxpδjx+pδjyk2pyypδjy+pδizk2pzzpδjz

+pdi·pdj+pr-p·(di×pδi+pdj×pδi(3.139)


由式(3.139)可见，Dij和式的每一元是由三组项组成的。其第一组项pδixk2pxxpδjx表示质量mp，在连杆p上的分布作用。第二组项表示连杆p质量的分布，记为有效力矩臂pdi·pdj。最后一组项是由连杆p的质心不在其坐标系原点而产生的。当各连杆的质心相距较大时，上述第二部分的项将起主要作用，而且可以忽略第一组项和第三组项的影响。
2) 惯量项Dij的进一步简化
在式(3.139)中，当i=j时，Dij可进一步简化为Dii： 


Dii=∑6p=imppδ2ixk2pxx+pδ2iyk2pyy+pδ2izk2pzz+pdi·di+2pr-p·(pdi·pδi)(3.140)


如果为旋转关节，那么把式(3.134)和式(3.135)代入式(3.140)可得： 


Dii=∑6p=impn2pxk2pxx+o2pyk2pyy+a2pxk2pzz+p-p·p-p

+2pr-p.(p-p·np)i+(p-p·op)j+(p-p·ap)k(3.141)


式中，np，op，ap和pp为i－1Tp的矢量，且


p-=pxi+pyj+0k


可使式(3.139)和式(3.140)中的有关对应项相等： 


pδ2ixk2pxx+pδ2iyk2pyy+pδ2izk2pzz=n2pxk2pxx+o2pyk2pyy+a2pxk2pzz
pdi·pdi=p-p·p-p
pdi·pδi=(p-p·np)i+(p-p·op)j+(p-p·ap)k


正如式(3.128)一样，Dii和式的每个元也是由三个项组成的。如果为平移关节，pδi=0，pdi·pdi=1，那么


Dii=∑6p=imp(3.142)


3) 重力项Di的化简
将式(3.133)代入式(3.132)得： 


Di=∑6p=i－mpgTTppΔipr-p(3.143)


把Tp分离为Ti－1i－1TP，并用i－1TP－1i－1Tp后乘pΔi，得： 


Di=∑6p=i－mpgTTi－1i－1TppΔii－1T－1pi－1Tppr-p(3.144)


当i－1Δi=i－1T－1P，irp=iTppr-p时，可进一步化简Di为： 


Di=－gTTi－1i－1Δi∑6p=impi－1r-p(3.145)


定义i－1g=－gTTi－1i－1Δi，则有： 


i－1g=－gxgygz0nxoxaxpx
nyoyaypy
nzozazpz
00000－δxδydx
δz0－δxdy
－δyδx0dz
0000


对应旋转关节i，i－1Δi对应于绕z轴的旋转。于是可把上式化简为： 


i－1g=－gxgygz0nxoxaxpx
nyoyaypy
nzozazpz
00000－100
1000
0000
0000

=－g·o，g·n，0，0(3.146)


对于平移关节，i－1Δi对应于沿z轴的平移，这时有下式： 


i－1g=－gxgygz0nxoxaxpx
nyoyaypy
nzozazpz
00000000
0000
0001
0000

0，0，0，－g·a(3.147)


于是，可把Di写成： 


Di=i－1g∑6p=impi－1r-p(3.148)


3.4.3牛顿欧拉方程
1. 一般形式

牛顿欧拉(NewtonEuler)方程的动力学一般形式为： 


Wqi=ddtKq·i－Kqi+Dq·i+Pqi，i=1，2，…，n(3.149)


式中，W，K，D，P和qi等的含义与拉格朗日法相同； i为连杆代号，n为连杆数目。
2. 二自由度机器人牛顿欧拉方程
质量m1和m2的位置矢量r1和r2(见图3.14)为： 


r1=r0+(d1cosθ1)i+(d1sinθ1)j

=(d1cosθ1)i+(d1sinθ1)j

r2=r1+d2cosθ(θ2+θ2)i+d2sin(θ1+θ2)j

=d1cosθ1+d2cos(θ1+θ2)i+d1sinθ1+d2sin(θ1+θ2)j




图3.14二自由度机器人


速度矢量v1和v2： 


v1=dr1dt=－θ·1d1sinθ1i+θ·1d1cosθ1j

v2=dr2dt=－θ·1d1sinθ1－(θ1+θ2)d2sin(θ1+θ2)i

+θ·1d1cosθ1－(θ1+θ2)d2cos(θ1+θ2)j


再求速度的平方，计算结果得： 


v21=d21θ·21
v22=d21θ·21+d22θ·21+2θ·1θ·2+θ·22+2d1d2θ·21+θ·1θ·2cosθ2


于是可得系统动能： 



K=12m1v21+12m2v22
=12(m1+m2)d21θ·21+12m2d22θ·21+2θ·1θ·2+θ·22+m2d1d2θ·21+θ·1θ·2cosθ2


系统的势能随r的增大(位置下降)而减少。我们以坐标原点为参考点进行计算： 


P=－m1gr1－m2gr2
=－(m1+m2)gd1cosθ1－m2gd2cos(θ1+θ2)


系统能耗： 


D=12C1θ·21+12C2θ·22


外力矩所做的功： 


W=T1θ1+T2θ2


至此，求得关于K，P，D和W的四个标量方程式。有了这四个方程式，就能够按式(3.149)求出系统的动力学方程式。为此，先求有关导数和偏导数。
当qi=θ1时，


Kθ·1=(m1+m2)d21θ·1+m2d22(θ1+θ2)+m2d1d2(2θ·1+θ·2)cosθ2
ddtKθ·1=(m1+m2)d21θ¨1+m2d22(θ¨1+θ¨2)+m2d1d2(2θ¨1+θ¨2)cosθ2
－m2d1d2(2θ·1+θ·2)θ·2sinθ2
Kθ1=0
Dθ·1=C1θ·1
Pθ1=(m1+m2)gd1sinθ1+m2d2gsin(θ1+θ2)
Wθ1=T1


把所求得的上列各导数代入式(3.149)，经合并整理可得： 


T1=(m1+m2)d21+m2d22+2m2d1d2cosθ2θ¨1
+m2d22+m2d1d2cosθ2θ¨1+c1θ·1－(2m2d1d2sinθ2)θ·1θ·2(3.150)
－(m2d1d2sinθ2)(m1+m2)gd1sinθ1+m2d2sin(θ1+θ2)


当qi=θ2时，


Kθ·2=m2d22(θ·1+θ·2)+m2d1d2θ·1cosθ2
ddtKθ·2=m2d22(θ¨1+θ¨2)+m2d1d2θ¨1cosθ2－m2d1d2θ¨1θ¨2sinθ2
Kθ·2=－m2d22(θ·21+θ·1θ·2)sinθ2
Dθ·2=C2θ·2
Pθ1=m2gd2sin(θ1+θ2)
Wθ2=T2


把上列各式代入式(3.149)，并化简得： 


T1=m2d22+m2d1d2cosθ2θ¨1+m2d22θ¨2+m2d1d2sinθ2θ·21(3.151)
+c2θ·2+m2gd2sin(θ1+θ2)


以上为二自由度机器人牛顿—欧拉动力学方程的形式。
3.5机器人静态特性与动态特性
机器人的静态特性主要是指稳定负载——力和力矩。关节力和力矩可以由末端执行器固连的坐标系的力和力矩决定。机器人的动态特性则包含稳定性、空间分辨度和精度、重复性和固有频率等。
3.5.1机器人的静态特性
1. 静力分析
机器人末端执行器上的力和力矩都是矢量，用固连坐标系描述。用矢量f标记力，用fx，fy和fz表达沿坐标系轴x，y和z的作用力。用矢量m标记力矩，用mx，my和mz表达各轴的分力矩。用矢量F定义： 


F=fx
fy
fz
mx
my
mz(3.152)


根据定义力和力矩的表示方法，定义坐标系轴x，y和z的位移和转角矩阵： 


D=dx
dy
dz
δx
δy
δz(3.153)


定义关节处的力(对滑动关节)和力矩(对转动关节)： 


T=T1
T2
T3
T4
T5
T6(3.154)


定义关节的微分运动： 


Dθ=dθ1
dθ2
dθ3
dθ4
dθ5
dθ6(3.155)


根据虚功法，关节的总虚功等于坐标系内的总虚功，结合雅可比矩阵，得到关节力和力矩与坐标系中期望的力和力矩关节，省略其推导过程，可得： 


T=JTF(3.156)


根据运动学分析和雅可比矩阵，由此控制器便可控制力和力矩。
2. 力和力矩的变换
不同坐标系间静力和静力矩需要进行等效变换。假设一个物体固连两个不同的坐标系，假设一个力和力矩作用在第一个坐标系的原点处，求出作用在另一个坐标系的等效力和力矩，使它们对物体作用效果一样。可以用虚功法解决等效变换这个问题。
设作用在物体固连坐标系A上的力和力矩为F，由它引起的A坐标系上的位移为D，同样设作用在物体固连坐标系B上的力和力矩为BF，由它引起的B坐标系上的位移为BD。虚功用δW表示，根据下式： 


FT=fx，fy，fz，mx，my，mz(3.157)
DT=dx，dy，dzz，δx，δy，δz(3.158)
BFT=Bfx，Bfy，Bfz，Bmx，Bmy，Bmz(3.159)
BDT=Bdx，Bdy，Bdz，Bδx，Bδy，Bδz(3.160)


用虚功原理，即作用于同一物体上的虚功相等，得： 


δW=FTD=BFTBD(3.161)


式中，坐标系B内的虚位移BD等价于坐标系A内的虚位移D，可得： 


BD=BJD(3.162)


把式（3.162）带入式(3.161)，得： 


FTD=BFTBJD(3.163)


式（3.163）可化简为： 


FT=BFTBJ(3.164)


式（3.164）变换为： 


F=BJTBF(3.165)


参照式(3.82)把式（3.165）写成矩阵方程形式： 


fx
fy
fz
mx
my
mz=nxoxax000
nyoyay000
nzozaz000
(p×n)x(p×o)x(p×a)xnxoxax
(p×n)y(p×o)y(p×a)ynyoyay
(p×n)z(p×o)z(p×a)znzozazBfx
Bfy
Bfz
Bmx
Bmy
Bmz(3.166)


对式（3.166）求逆： 


Bfx
Bfy
Bfz
Bmx
Bmy
Bmz=nxnynz000
oxoyoz000
axayaz000
(p×n)x(p×n)y(p×n)znxnynz
(p×o)x(p×o)y(p×o)zoxoyoz
(p×a)x(p×a)y(p×a)zaxayazfx
fy
fz
mx
my
mz(3.167)


再对式（3.167）左边和右边的前三行与后三行进行交换： 


Bmx
Bmy
Bmz
Bfx
Bfy
Bfz=nxnynz(p×n)x(p×n)y(p×n)z
oxoyoz(p×o)x(p×o)y(p×o)z
axayaz(p×a)x(p×a)y(p×a)z
000nxnynz
000oxoyoz
000axayazmx
my
mz
fx
fy
fz(3.168)


比较式(3.167)和式(3.161)可见，两式的右边第一个矩阵，即雅可比矩阵是相同的。因此，不同坐标系间的力和力矩变换可用与微分平移变换及微分旋转变换一样的方法进行。因此，由式(3.82)~式(3.84)进行推导，可以得到： 


Bfx=n·f

Bfy=o·f(3.169)

Bfz=a·f

Bmx=n·(f×p)+m

Bmy=o·(f×p)+m(3.170)

Bmz=a·(f×p)+m


式中n，o，a和p为3.3节所定义的微分坐标变换的列矢量。用与微分平移一样的方法进行力变换，而用与微分旋转一样的方法进行力矩变换。
例3.10一个物体固连于坐标系B，它受到坐标系A中力和力矩F的作用，求它在坐标系B内的等效力和力矩。


FT=5，0，0，0，10，0
B=0102
0014
1006
0001


解： 根据已知两式，可得


f=5，0，0m=0，10，0p=2，4，6
n=0，0，1o=1，0，0a=0，1，0
f×p=ijk
500
246=(0)i－(30)j+(20)k
(f×p)+m=-20j+20k


根据式(3.165)，可得


Bfx=n·f=0

Bfy=o·f=5

Bfz=a·f=0

Bmx=n·(f×p)+m=20

Bmy=o·(f×p)+m=0

Bmz=a·(f×p)+m=－20
则BFT=0，5，0，20，0，－20


3.5.2机器人的动态特性
机器人的动态特性描述下列能力： 它能够移动多快，能以怎样准确性快速地停在给定点，以及它对停止位置超调了多少距离等。如果机器人移动太慢，虽然容易控制，但耗费比较多的时间，损失了效率； 如果移动太快，任何超调都可能造成损失或事故。从伺服控制角度看，惯性负载不仅是由物体的惯量决定的，而且也取决于这些关节的瞬时位置及运动情况。在快速运动时，机器人上各刚性连杆的质量和转动惯量(即惯量矩)给这些关节的伺服系统的总负载强加上一个很大的摩擦负载。
本节将简要介绍机器人的稳定性、空间分辨度和精度、重复性以及固有频率。
1. 稳定性
稳定性主要涉及系统运行过程中的振动。
伺服系统的设计者确信，机器人决不会突然引起振荡。当手臂的姿态改变时，单独关节伺服装置上的惯性负载和重力负载也随之变动，这就使振荡难以形成。此外，伺服系统必须在一个宽大的位置误差(在某些情况下还有速度误差)动态范围内运行，而且必须在所有情况下可靠地工作，而不管所做传动装置强加的速度和加速度限制如何。
有一种机器人控制器，当它的每个关节第一次到达其设定点时，能够独立地锁定该关节。当工具进入离设定位置一定距离时，它也能使关节减速。这种锁定，可按任何次序进行。当所有关节都锁定时，机械臂处于稳态，并可开始向下一位置运动。如果维持在一个位置的时间达几秒以上，那么工具将从编程位置缓慢移开。当位置误差积累达到显著值时，关节伺服系统能够使工具返回初始位置。工具位置的这一变化，是一种技术上的不稳定形式，但不影响机器人的正常运行。另一种控制器允许各关节伺服系统连续运行。从建造数控工具的经验中得到的复杂的伺服系统设计技术，能够防止启动时产生振荡，而不管负载情况如何。一些特殊的条件可能使关节伺服系统处于极不稳定的状态。当负载突然从工具末端滑脱出去时所发生的情况，就是一个典型的例子。这会使一个或多个关节上的重力负载产生阶跃变化，并会使设计不好的机器人引起振荡。关节的运动也能产生有效惯性力、向心力和对其他关节的耦合向心力(或力矩)的各种组合。其他关节对这些力矩的作用也会对原关节产生各种作用力。这是另一个潜在的振荡根源。
2. 空间分辨度和精度
空间分辨度是设计机器人控制系统的特性指标，它指明系统能够区别工作空间所需要的最小运动增量。分辨度可以是控制系统能够控制的最小位置增量的函数，或者是控制测量系统能够辨别的最小位置增量。空间分辨度与机械偏差一起构成控制分辨度。为了确定空间分辨度，机器人上每个关节的工作范围是由控制增量数进行区分的。
机器人精度主要包含三个方面因素： ①各控制部件的分辨度； ②各机械部件的偏差； ③某个任意的从未接近的固定位置(目标)。


图3.15考虑机械偏差时精度与

空间分辨度的关系

当包括机械部件偏差时，精度将变差。图3.15给出考虑机械偏差时精度与空间分辨度的关系。产生最大位置偏差的机械偏差确定了最恶劣的条件，这个偏差用来决定实际的空间分辨度，并据这一分辨度来求出精度。产生这些偏差的因素有齿轮啮合间隙、连杆松动和负载的影响等。在转轴情况下，反馈元件被装在旋转关节上，而且负载离轴伸出一定距离； 这时，齿轮啮合间隙的影响更大。对于大负载重量，横梁偏转开始发生作用，并降低精度，在动态条件下，梁偏转作用存在于所有轴上。如果出现驱动啮合间隙，那么横梁偏转还可能引起严重的谐振。

当机器人只在示教复演模式下运行时，谈论其精度是没有意义的。在这种模式下，控制系统在机器人训练(示教)期间，只记录关节的位置，然后在作业期间复现这些位置。这时，重复性和分辨度是重要的技术性能。分辨度这一技术要求确定机器人是否能足够接近地到达训练(示教)时第一次作业的位置。重复性这一技术要求确定机器人在生产中第二次和以后各次作业时能否足够接近地到达目标位置。
当机器人控制系统中的计算机必须计算一系列关节位置，而且这些位置使工具顶端放置到以机器人独立坐标系描述的位置时，精度对于描述这样的机器人才有意义。
3. 重复性
重复性又称重复定位精度，指的是机器人自身重复到达原先被命令或训练位置的能力。


图3.16重复性示例


图3.16绘出重复性的简单例子。开始时，机器人被定位在由控制分辨度所限制的尽可能接近于任意目标的位置上，对应于位置T。接着，移开机器人，并命令它返回位置T。当它力图返回预先示教过的位置时，由于控制系统和机械部件的偏差，使此机器人停止在位置R。位置T和R之间的差距就是此机器人重复性的一种量度。图中的位置变化是被夸大了的。
4. 固有频率
固有频率是系统在受到外界激励产生运动时发生自然振动的频率，这些特定的频率被称为系统的固有频率。
由振动理论可知在某一振型下机器人的固有频率越高，则相应的刚度越大，抵抗相应变形的能力越强，振动衰减的速度越快，机器人跟踪预期运动轨迹的振动衰减的速度越快，机器人跟踪预期运动轨迹的精度也越高。由于机器人在运动时其拓扑结构发生改变，所以机器人系统的固有频率也会发生相应的变化，这直接导致了机器人的力学特性和动态性能的改变。机器人的固有频率不仅与机器人的结构参数有关，而且还与标识机器人位形和运动状态的运动参数有关，机器人在运动的过程中固有频率可能会明显下降，这不仅会降低机器人抵抗变形的能力，而且一旦固有频率下降到与激振力的频率接近或相等时，机器人会发生动力奇异，导致出现振幅急剧增大甚至使系统结构发生破坏的现象。既然机器人的动态性能与其固有频率有关，而固有频率又与机器人的运动参数有关，因此可以通过适当调整运动参数来提高机器人在运动过程中的固有频率，以改善机器人的动态性能。
本 章 小 结
本章描述了机器人运动学的相关数学描述，用改进DH法推导了机器人正逆运动学，用正运动学确定末端执行器的位姿，当某个位姿确定后，实际应用中，是以位姿为源头，计算每个关节处的运动值。为了能够精确地控制机器人，需要知道机器人的微分运动，推导了微分算子。接着，简要介绍了机器人关节运动与末端运动的控制关系。
机器人动力学问题的研究目的在于控制和保证机器人保持优良的动态特性和静态特性。文中推导了机器人的动力学方程，用来估计以一定速度和加速度驱动机器人各关节所需的力和力矩，并以此为基础选择机器人驱动器。
参 考 文 献


［1］蔡自兴，谢斌.机器人学［M］.3版.北京： 清华大学出版社，2015.

［2］Mark W.Spong、Seth Hutchinson、M.Vidyasagar.机器人建模与控制［M］.北京： 机械工业出版社，2016： 127154.

［3］毕树生，宗光华.微操作机器人系统的研发开发［J］.中国机械工程，1990，10(9)： 10241027.

［4］蔡自兴，徐光佑.人工智能及其应用［M］.北京： 清华大学出版社，2004.

［5］Saeed B.Niku.机器人学导论： 分析、控制及应用［M］.2版.孙富春，朱纪洪，刘国栋，译.北京： 电子工业出版社，2013.3.

［6］蔡自兴，郭潘.中国工业机器人发展的若干问题［J］.机器人技术与应用，2013(3)： 912.

［7］蔡自兴，刘建勤.面向21世纪的智能机器人技术［J］.机器人技术与应用，1998，(6)： 23.

［8］霍伟.机器人动力学与控制［M］.北京： 高等教育出版社，2005.

［9］John J. Craig.机器人学导论［M］.贠超，译.北京： 机械工业出版社，2006.

［10］蔡自兴.机器人学基础［M］.北京： 机械工业出版社，2009.

［11］张涛.机器人引论［M］.北京： 机械工业出版社，2010.

［12］刘极峰，易际明.机器人技术基础［M］.北京： 高等教育出版社，2006.

［13］柳洪义，宋伟刚.机器人技术基础［M］.北京： 冶金工业出版社，2002.

［14］丁学贡.机器人控制研究［M］.杭州： 浙江大学出版社，2006.

［15］蒋新松，机器人学导论［M］.沈阳： 辽宁科学出版社，1994.


思考题与练习题
思考题
1. 技术的革新会带来对传统机器人建模方法的颠覆。
2. 如软体机器人(DH是否适用)。
3. 多传感器的融合(智能空间)。
4. 本章中所述的齐次变换与计算机图形学等有相同的地方吗？相同点在哪里？
5. 并联机器人能否用DH方法建模？
6. 如何提高柔性机器人的动力学性能？
7. 含间隙机构的机器人动力学建模需要哪些理论知识？
练习题

1. 有一旋转变换，先绕固定坐标系Z0轴转45°，再绕其X0轴转60°，最后绕其Y0轴转30°，试求该齐次坐标变换矩阵。
2. 在坐标系{A}中，点P的运动轨迹为： 首先绕Za轴转60°，又沿{A}的Xa轴移动3个单位，再沿{A}的Ya轴移动4个单位。如果点P在原来的位置为AP1=1，2，0T，用齐次坐标变换法求运动后的位置AP2。
3. 如图3.17所示的Stanford机器人，试建立其DH坐标系，并在表3.3中填写参数。


图3.17练习题1图




表3.3DH坐标系参数



连杆iαi－1ai－1diθi
1
2
3
4
5
6
4. 写出题3中Stanford机器人各关节的变换矩阵和总变换矩阵。
5. 如图3.18所示的三自由度机械手(两个旋转关节加一个平移关节，简称RPR机械手)，求末端机械手的运动学方程。
6. 如图3.19所示的二自由度机械手，手部沿固定坐标系X0轴正向以1.0m/s的速度移动，杆长l1=l2=0.5m。设在某瞬时θ1=30°，θ2=60°，求相应瞬时的关节速度。


图3.18三自由度机械手




图3.19二自由度机械手



7. 给定机器人末端坐标系T以及机器人在这个位置的雅可比矩阵的逆，在微分运动下，试求： (1)找出做微分运动的为哪一个关节，求其运动量。(2)求坐标系的变化。(3)求出进行微分运动后的新位置。(4)假如对应于坐标系T进行测量，运动到(3)中所求位置，则微分运动量为多少。


T=0102
1002
00－15
0001J－1=400000
10－1000
0－0.20000
0－0.50010
000100
100001



思考题与练习题参考答案
思考题
1. 答： (开放题)，言之有理即可。例如传统建模方法不适用于某一领域或某一工况。
2. 答： 
(开放题)，言之有理即可。目前是适用的，但是随着技术的发展，有可能出现不适用的情况。
3. 答： 
(开放题)，言之有理即可。多传感器的融合，可以使机器人更加智能，算法更加精确，测得的数据更加准确，也增加了机器人实时处理的难度。
4. 答： 
有相同的地方。都是应用齐次坐标来解决问题，机器人利用齐次坐标解决了运动学建模的问题，而计算机图形学利用齐次坐标解决了图形变换的问题。
5. 答： 
能。
6. 
答： 从机器人机构研究的角度出发，应在改善其机械结构和特性方面进行深入研究，充分利用机构冗余度、结构柔性等机械特性，在冗余驱动、欠驱动等方面想办法，从多部展冗余度柔性机器人、欠驱动柔性机器人、柔性机器人协调操作和冗余度柔性机器人协调操作等交叉领域的研究，同时，与先进的控制方法相结合，从机器人的内部特性和外部手段两方面入手，综合提高机器人的整体动力学性能。
7. 答： 
(开放题)，言之有理即可。含间隙机构的动力学模型、运动副分离准则、混沌特性、优化设计、误差理论、构建柔性等。
练习题

1. 解： 
齐次坐标变换矩阵R=Rot(Y，30°)Rot(X，60°)Rot(Z，45°)
=0.86600.50
0100
－0.500.8660
00011000
00.5－0.8660
00.8660.50
00010.707－0.70700
0.7070.70700
0010
0001
=0.9180.3060.250
0.3530.353-0.8660
0.1760.8830.4330
0001
2. 
解： 用齐次坐标变换来解答，可得实现旋转和平移的齐次复合变换矩阵为： 


T=0.5－0.86603
0.8660.504
0010
0001


已知： 


AP1=1
2
0
1


利用齐次坐标变换的复合变换矩阵，可得： 


AP2=TAP1=0.5－0.86603
0.8660.504
0010
00011
2
0
1=1.768
6.732
0
1


3. 见图3.20。


图3.20Stanford机器人连杆坐标系




表3.4DH坐标系参数



连杆iαi－1ai－1diθi
1-90°00θ1
290°0d2θ2
300d30
4-90°00
θ4590°00θ5
600d6θ6



4. 略。
5. 
解： 建立如图3.21所示坐标系，则各连杆的DH参数见表3.5。


图3.21三自由度机械手连杆坐标系





表3.5DH参数表



连杆转角θn偏距dn扭角αi－1杆长ai－1
1θ1L100
20d290°0
3θ3L200


由连杆齐次坐标变换递推公式


i－1Ti=cθi－sθi0ai－1
sθicαi－1cθicαi－1－sαi－1－disαi－1
sθisαi－1cθisαi－1cαi－1dicαi－1
0001



有


01T=cθ1－sθ100
sθ1cθ100
001L1
0001，12T=1000
00－1－d2
0100
0001，23T=cθ3－sθ300
sθ3cθ300
001L2
0001


故


03T= 01T12T23T=cθ1cθ3－cθ1sθ3sθ1sθ1L2+sθ1d2
sθ1cθ3－sθ1sθ3－cθ1－cθ1L2－cθ1d2
sθ3cθ30L1
0001


式中： sθ1=sinθ1

cθ1=cosθ1

…
三连杆操作臂的逆运动学方程： 
第一组解： 由几何关系得： 


x=L2cosθ2+L3cos(θ2+θ3)(1)
y=L2sinθ2+L3sin(θ2+θ3)(2)


将式(1)平方加式(2)平方得： 


x2+y2=L22+L23+2L2L3cosθ3


由此式可推出： 


θ3=arccosx2+y2－L22－L232L2L3
θ2=arctanyx－arctanL3sinθ3L2+L3cosθ3


第二组解： 由余弦定理x2+y2=L22L23－2L2L3cos，得： 


=arccosL22L23－(x2+y2)2L22L23
θ′3=π+
θ′2=π-2+arctanyx


6. 
解： 


X=l1c1+l2c12

Y=l1sθ1+l2s12
dX=Xθ1dθ1+Xθ2dθ2

dY=Yθ1dθ1+Yθ2dθ2
dX
dY=Xθ1Xθ2
Yθ1Yθ2dθ1
dθ2

v=vX
vY

=－l1s1－l2s12－l2s12
l1c1+l2c12l2c12θ·1
θ·2

=－(l1s1+l1s12)θ·1－l2s12θ·2

(l1c1+l2c12)θ·1+l2c12θ·2


求解得，该瞬时两关节的位置分别为30°，-60°； 速度为-2rad/s，4rad/s。
7. 略。