第2章 工业机器人运动学old.docx
《第2章 工业机器人运动学old.docx》由会员分享，可在线阅读，更多相关《第2章 工业机器人运动学old.docx（49页珍藏版）》请在冰点文库上搜索。
第2章工业机器人运动学old
注：
1）2008年春季讲课用；2）带下划线的黑体字为板书内容；3）公式及带波浪线的部分为必讲内容
第2章工业机器人运动学
2.1引言
通过上一章的学习我们知道，从机构学的角度看，工业机器人可以认为是用一系列关节连接起来的连杆所组成的开链机构。
工业机器人运动学研究的是各连杆之间的位移关系、速度关系和加速度关系。
本章仅研究位移关系，重点是研究手部相对于机座的位姿与各连杆之间的相互关系。
“位姿”是“位置和姿态”的简称。
工业机器人手部相对于机座的位姿与工业机器人各连杆之间的相互关系直接相关。
为了便于数学上的分析，一般选定一个与机座固联的坐标系，称为固定坐标系，并为每一个连杆（包括手部）选定一个与之固联的坐标系，称为连杆坐标系。
一般把机座也视为一个连杆，即零号连杆。
这样，连杆之间的相互关系可以用连杆坐标系之间的相互关系来描述。
工业机器人手部相对机座的位姿就是固联在手部的坐标系相对固定坐标系的位姿。
工业机器人运动学主要包括正向运动学和反向运动学两类问题。
正向运动学是在已知各个关节变量的前提下，解决如何建立工业机器人运动学方程，以及如何求解手部相对固定坐标系位姿的问题。
反向运动学则是在已知手部要到达目标位姿的前提下，解决如何求出关节变量的问题。
反向运动学也称为求运动学逆解。
在工业机器人控制中，先根据工作任务的要求确定手部要到达的目标位姿，然后根据反向运动学求出关节变量，控制器以求出的关节变量为目标值，对各关节的驱动元件发出控制命令，驱动关节运动，使手部到达并呈现目标位姿。
可见，工业机器人反向运动学是工业机器人控制的基础。
在后面的介绍中我们会发现，正向运动学又是反向运动学的基础。
工业机器人相邻连杆之间的相对运动不是旋转运动，就是平移运动，这种运动体现在连接两个连杆的关节上。
物理上的旋转运动或平移运动在数学上可以用矩阵代数来表达，这种表达称之为坐标变换。
与旋转运动对应的是旋转变换，与平移运动对应的是平移变换。
坐标系之间的运动关系可以用矩阵之间的乘法运算来表达。
用坐标变换来描述坐标系（刚体）之间的运动关系是工业机器人运动学分析的基础。
在工业机器人运动学分析中要注意下面四个问题：
1）工业机器人操作臂可以看成是一个开式运动链，它是由一系列连杆通过转动或移动关节串联起来的。
开链的一端固定在机座上，另一端是自由的。
自由端安装着手爪（或工具，统称手部或末端执行器），用以操作物体，完成各种作业。
关节变量的改变导致连杆的运动，从而导致手爪位姿的变化。
2）在开链机构简图中，关节符号只表示了运动关系。
在实际结构中，关节由驱动器驱动，驱动器一般要通过减速装置（如用电机或马达驱动）或机构（如用油缸驱动）来驱动操作臂运动，实现要求的关节变量。
3）为了研究操作臂各连杆之间的位移关系，可在每个连杆上固联一个坐标系，然后描述这些坐标系之间的关系。
Denavit和Hartenberg提出一种通用的方法，用一个4×4的齐次变换矩阵描述相邻两连杆的空间关系，从而推导出“手部坐标系”相对于“固定坐标系”的齐次变换矩阵，建立操作臂的运动方程。
4）在轨迹规划时，人们最感兴趣的是手部相对于固定坐标系的位姿。
2.2齐次坐标及对象物的描述
齐次变换具有较直观的几何意义，非常适合描述坐标系之间的变换关系。
另外，齐次变换可以将旋转变换与平移变换用一个矩阵来表达，关系明确，表达简洁。
所以常用于解决工业机器人运动学问题。
下面我们先介绍有关齐次坐标和齐次变换的内容。
2.2.1点的位置描述
如图2-1所示，在选定的三维空间直角坐标系{A}中，空间任一点P的坐标可以用一个（3×1）列阵（或称三维列向量）Ap表示，即：
（2-1）
式中：
X，y，z是点P在坐标系{A}中的三个坐标分量；
Ap的左上标A代表选定的参考坐标系。
2.2.2齐次坐标
如果用四个数组成的（4×1）列阵（或称四维列向量）表示三维空间直角坐标系{A}中的点P，即：
（2-2）
则定义列阵[xyz1]T为三维空间点P的齐次坐标。
必须注意，齐次坐标的表示不是唯一的。
如果将列阵p中的元素同乘一非零系数w后，仍然代表同一点P，即：
（2-3）
式中：
x＝a/w，y＝b/w，z＝c/w。
2.2.3坐标轴的描述
如图2-2所示，i、j、k分别是直角坐标系中X、Y、Z坐标轴的单位矢量，若用齐次坐标来描述X、Y、Z轴，则定义下面三个（4×1）列阵分别为单位矢量i、j、k（即X、Y、Z坐标轴）的方向列阵。
i＝[1000]T
j＝[0100]T
k＝[0010]T
图2-2中所示矢量v的单位矢量h的方向列阵为：
h＝[abc0]T＝[coscoscos0]T（2-4）
式中，、、分别是矢量v与坐标轴X、Y、Z的夹角，0180，0180，0180。
cos、cos、cos称为矢量v的方向余弦，且满足cos2+cos2+cos2＝1。
综上所述，可得下面两点结论：
1）（4×1）列阵[abcw]T中第四个元素不为零，则表示空间某点的位置；
2）（4×1）列阵[abc0]T中第四个元素为零，且a2+b2+c2＝1，则表示某个坐标轴（或某个矢量）的方向，[abc0]T称为该矢量的方向列阵。
表示坐标原点的（4×1）列阵定义为：
o＝[000]T≠0
[例2-l]用齐次坐标分别写出图2-3中矢量u、v、w的方向列阵。
解：
矢量u：
u＝[coscoscos0]T＝[0.00.70710.70710]T
矢量v：
v＝[coscoscos0]T＝[0.70710.00.70710]T
矢量w：
w＝[coscoscos0]T＝[0.50.50.70710]T
2.2.4动坐标系位姿的描述
对动坐标系位姿的描述就是相对固定坐标系对动坐标系原点位置的描述以及对动坐标系三个坐标轴方向的描述，现以两个实例说明。
1）刚体位姿的描述
组成工业机器人的每一个连杆都可以看作是一个刚体。
若给定了刚体上某一点的位置和该刚体在空间的姿态，则这个刚体在空间上是完全确定的。
设有一刚体Q，如图2-4所示，在刚体上选任一点O，建立与刚体固连的坐标系OXYZ，称为动坐标系。
O点在固定坐标系OXYZ中的位置可用齐次坐标形式的（4×1）列阵表示为：
（2-5）
刚体姿态可用动坐标系三个坐标轴的方向来表示。
令n、o、a分别为X、Y、Z坐标轴的单位方向矢量，每个单位方向矢量在固定坐标系上的分量为动坐标系该坐标轴的方向余弦，用齐次坐标列阵分别表示为：
n＝[ixiyiz0]T＝[cosXcosXcosX0]T
o＝[oxoyoz0]T＝[cosYcosYcosY0]T（2-6）
a＝[axayaz0]T＝[cosZcosZcosZ0]T
式中：
X、Y、Z分别为X、Y、Z坐标轴与X坐标轴的夹角；
X、Y、Z分别为X、Y、Z坐标轴与Y坐标轴的夹角；
X、Y、Z分别为X、Y、Z坐标轴与Z坐标轴的夹角。
因此，图2-4中刚体的位姿可用下面的（4×4）矩阵来描述：
（2-7）
很明显，对刚体Q位姿的描述就是对固连于刚体Q的坐标系OXYZ位姿的描述。
[例2-2]图2-5表示固连于刚体的坐标系{B}位于OB点，xb＝10，yb＝6，zb＝0。
ZB轴和ZA轴与纸面垂直，坐标系{B}相对固定坐标系{A}有一个30的偏转，试写出表示刚体位姿的坐标系{B}的（4×4）矩阵表达式。
解：
XB的方向列阵：
n＝[cos30cos60cos900]T＝[0.8660.50.00]T
YB的方向列阵：
o＝[cos120cos30cos900]T＝[-0.50.8660.00]T
ZB的方向列阵：
a＝[cos90cos90cos00]T＝[0.00.01.00]T
坐标系{B}的位置列阵：
p＝[10.06.00.01]T
所以，坐标系{B}的（4×4）矩阵表达式为：
2）手部位姿的表示
工业机器人手部的位姿也可以用固连于手部的坐标系{B}的位姿来表示，如图2-6所示。
坐标系{B}可以这样来确定：
取手部的中心点OB为原点；关节轴为ZB轴，ZB轴的单位方向矢量a称为接近矢量，指向朝外；两个手指的连线为YB轴，指向可任意选定，YB轴的单位方向矢量o称为姿态矢量；XB轴与YB轴及ZB轴垂直，XB轴的单位方向矢量n称为法向矢量，且n＝o×a，指向符合右手法则。
手部的位置矢量为固定坐标系原点指向手部坐标系{B}原点的矢量p，手部的方向矢量为n、o、a。
于是，手部的位姿可用（4×4）矩阵表示为：
（2-8）
[例2-3]图2-7表示手部抓握物体Q，物体为边长2个单位的正立方体，写出表达该手部位姿的矩阵式。
解：
因为物体Q的形心与手部坐标系OXYZ的坐标原点O重合，固定坐标系原点O为正立方体的后下方左侧顶点，所以手部位置的（4×1）列阵为：
p＝[1111]T
设n、o、a为手部坐标系三个坐标轴的单位方向矢量，由图2-7可知：
矢量n的方向角为：
X＝90X＝180X＝90
矢量o的方向角为：
Y＝180Y＝90Y＝90
矢量a的方向角为：
Z＝90Z＝90Z＝180
于是有：
n＝[cosXcosXcosX0]T＝[0-100]T
o＝[cosYcosYcosY0]T＝[-1000]T
a＝[cosZcosZcosZ0]T＝[00-10]T
根据式（2-8）可知，表达该手部位姿的矩阵式为：
2.2.5目标物的齐次矩阵表示
设有一楔块Q如图2-8所示，坐标系OXYZ为固定坐标系，坐标系OXYZ为与楔块Q固连的动坐标系。
在图（a）情况下，动坐标系OXYZ与固定坐标系OXYZ重合。
楔块Q的位置和姿态可用6个点的齐次坐标来描述，在图（a）情况下，其矩阵表达式为：
若让楔块Q先绕Z轴旋转90，再绕Y轴旋转90，最后沿X轴方向平移4，则楔块成为图（b）之情况。
此时楔块用新的6个点的齐次坐标来描述它的位置和姿态，其矩阵表达式为：
这个矩阵是根据图2-8（b）直接写出来的，后面讲完齐次变换以后将会知道，这个矩阵可以由图2-8（a）对应的矩阵计算得到，见[例2-7]。
2.3齐次变换及运算
刚体的运动是由转动和平移组成的。
为了能用同一矩阵表示转动和平移，有必要引入（4×4）的齐次坐标变换矩阵。
2.3.1平移的齐次变换
我们首先介绍点在空间直角坐标系中的平移。
如图2-9所示，空间某一点A的坐标为（x，y，z），当它平移至A点后，坐标为（x，y，z），且有：
x＝x+x
y＝y+y（2-9）
z＝z+z
或写成如下矩阵形式：
也可以简写为：
a＝Trans（x，y，z）a（2-10）
式中，Trans（x，y，z）表示齐次坐标变换的平移算子，且：
（2-11）
其中，第四列元素x，y，z分别表示沿坐标轴X，Y，Z的移动量。
齐次坐标变换的运算规则：
若算子左乘，表示坐标变换是相对固定坐标系进行的；假如相对动坐标系进行坐标变换，则算子应该右乘。
平移齐次变换公式（2-10）同样适用于坐标系、物体等的变换，这时最右端为一个（4×n）的矩阵。
对于坐标系的变换，n=4；对于物体的变换，n=描述物体的顶点个数。
[例2-4]图2-10中有下面三种情况：
1）动坐标系{A}相对于固定坐标系作（-1，2，2）平移后到{A}；2）动坐标系{A}相对于自身坐标系（即动坐标系）作（-1，2，2）平移后到{A}；3）物体Q相对于固定坐标系作（2，6，0）平移后到Q。
已知：
试计算出坐标系{A}、{A}以及物体Q的矩阵表达式。
解：
动坐标系{A}的两个平移坐标变换算子均为：
{A}坐标系是动坐标系{A}相对于固定坐标系作平移变换得来的，变换算子应该左乘，因此，{A}的矩阵表达式为：
从这个（4×4）的矩阵可以看出，O在O0X0Y0Z0坐标系中的坐标为（0，3，3）。
{A}坐标系是动坐标系{A}相对于自身（动坐标系）作平移变换得来的，变换算子应该右乘，因此，{A}的矩阵表达式为：
从这个（4×4）的矩阵可以看出，O在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），即（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轴旋转角后至A点，坐标为（x，y，z）。
A点和A点的坐标关系为：
（2-12）
或用矩阵表示为：
A点和A点的齐次坐标分别为[xyz1]T和[xyz1]T，因此A点的旋转齐次变换过程为：
（2-13）
也可简写为：
a＝Rot（z，）a（2-14）
式中，Rot（z，）表示齐次坐标变换时绕Z轴的旋转算子，算子的内容为：
（2-15）
式中：
c＝cos；s＝sin。
同理，可写出绕X轴的旋转算子和绕Y轴的旋转算子，其内容为：
（2-16）
（2-17）
图2-12所示为点A绕任意过原点的单位矢量k旋转角的情况。
kx，ky，kz分别为单位矢量k在固定坐标系坐标轴X、Y、Z上的三个分量（方向余弦），且kx2+ky2+kz2＝1。
可以证得，绕任意过原点的单位矢量k转角的旋转齐次变换公式为：
（2-18）
式中；vers＝（1-cos）。
式（2-18）称为一般旋转齐次变换的通式，绕X轴、Y轴、Z轴进行的旋转齐次变换是其特殊情况，例如：
当kx＝1，ky＝kz＝0时，即绕X轴旋转，则由式（2-18）可得到式（2-16）；
当ky＝1，kx＝kz＝0时，即绕Y轴旋转，则由式（2-18）可得到式（2-17）；
当kz＝1，kx＝ky＝0时，即绕Z轴旋转，则由式（2-18）可得到式（2-15）。
反之，若给出某个旋转齐次变换矩阵：
则可根据式（2-18）求出其等效转轴的单位矢量k及等效转角，计算公式为：
（2-19）
式中：
当取0到180之间的值时，式中的符号取+号。
当转角很小时，公式（2-19）很难确定转轴。
当接近0或180时，转轴完全不确定。
与平移变换一样，旋转变换算子公式（2-15）、（2-16）、（2-17）以及一般旋转变换算子公式（2-18），不仅适用于点的旋转变换，而且也适用于矢量、坐标系、物体等的旋转变换。
若相对固定坐标系进行变换，则算子左乘；若相对动坐标系进行变换，则算于右乘。
[例2-5]已知坐标系中点U的位置矢量u＝[7321]T，将此点绕Z轴旋转90，再绕Y轴旋转90，如图2-13所示，求旋转变换后所得的点W。
解：
[例2-6]如图2-14所示单臂操作手，手腕也具有一个自由度。
已知手部起始位姿矩阵为：
若手臂绕Z0轴旋转+90，则手部到达G2；若手臂不动，仅手部绕手腕Z1轴旋转+90，则手部到达G3。
写出手部坐标系{G2}及{G3}的矩阵表达式。
解：
手臂绕Z0轴转动是相对固定坐标系作旋转变换，所以，算子应该左乘，即：
手部绕手腕Z1轴旋转是相对动坐标系作旋转变换，所以，算子应该右乘，即：
讲课时对照图分析这两个矩阵。
2.3.3平移加旋转的齐次变换
平移变换算子和旋转变换算子可以组合在一个（4×4）的矩阵中。
若例2-5中的点W还要作4i-3j+7k的平移，如图2-15所示，则只要再左乘上平移变换算子，即可得到最后E点的列阵表达式，即：
式中：
为平移加旋转的复合变换矩阵。
讲课时分析一下其组成特点。
[例2-7]图2-8所示的楔块Q，在图（a）情况下描述它的齐次矩阵为：
试证明楔块经过绕固定坐标系OXYZ的Z轴旋转90，再绕Y轴旋转90，最后沿X轴方向平移4后[见图2-8（b）]的齐次矩阵表达式为：
证明：
因为楔块从图（a）至图（b）的所有变换都是相对于固定坐标系OXYZ进行的，所以各坐标变换算子应该依次左乘，即：
式中：
即为楔块平移加旋转的复合变换矩阵。
讲课时分析一下其组成特点。
证毕。
2.3.4旋量的概念
旋量Screw（k，r，）表示沿k轴移动r，并绕k轴转动角的综合齐次变换。
旋量Screw（k，r，）与移动和转动发生的先后次序无关，只要它们连续即可，即：
Screw（k，r，）＝Rot（k，）Trans（k，r）＝Trans（k，r）Rot（k，）（2-19a）
例如，沿Z轴移动r，并绕Z轴转动角的综合变换为：
式中：
c表示cos，s表示sin。
2.4工业机器人连杆参数及其齐次变换矩阵
工业机器人可以认为是一系列通过关节连接起来的连杆组成的开链机构（P5图1-4）。
手部相对固定坐标系的位姿与各连杆之间的相互关系直接相关。
因此，在研究手部相对于机座的几何关系时，首先要分析两个相邻连杆之间的关系，这种关系可以用固连于相邻连杆上的坐标系之间的关系来描述。
为此，我们首先必须建立连杆坐标系。
下面介绍一种由Denavit和Hartenberg提出的通用方法，即D—H法。
2.4.1连杆参数及连杆坐标系的建立
工业机器人相邻连杆之间的关系，与连杆自身的特征和连杆之间的连接方式有关。
因此，我们首先应该清楚如何对连杆的特征和连接方式进行描述。
一、连杆特征的描述
如图2-16所示，连杆i两端有关节i和i+1。
该连杆的特征可以用两个参数来描述：
一个是两个关节轴线沿公垂线的距离li，称为连杆长度；另一个是在垂直于公垂线的平面内两个轴线的夹角i，称为连杆扭角。
这两个参数为表述连杆特征的尺寸参数。
连杆长度li恒为非负数，但连杆扭角i可正、可负。
i的正负是这样规定的：
公垂线的正向规定为从关节i指向关节i+1，按右手法则从轴线i绕公垂线转至轴线i+1，逆时针为正，瞬时针为负。
两轴线平行时，i＝0；两轴线相交时，li＝0，此时扭角i为两轴线的夹角，正负与Xi轴选向有关。
二、连杆连接方式的描述
如图2-17所示，连杆i与连杆i-1通过关节i相连，因此，关节i的轴线有两条公垂线与它垂直。
两条公垂线的相对位置可用两个参数di和i来确定，其中di是沿关节i轴线测量的两个公垂线与i轴线交点的距离，当关节轴线相交时，di为i轴线上两交点的距离；i是在关节i轴线的垂直平面内两个公垂线的夹角，当公垂线不存在时，对旋转关节i仍然存在。
di和i是表达相邻连杆连接关系的参数。
di和i都可正、可负（详见表2-1）。
这样，相邻两个连杆之间的关系可以由四个参数所描述：
其中两个参数（li和i）描述连杆i的尺寸；另外两个参数（di和i）描述连杆i和连杆i-1之间的连接关系。
对于旋转关节，i是关节变量，其它三个参数固定不变；对于移动关节，di是关节变量，其它三个参数固定不变。
（对照图2-17解释，一个关节即为一个自由度）
三、连杆坐标系的建立
D-H法要求按下面规则建立连杆i的坐标系{i}（简称i系）：
1）坐标系{i}与连杆i固连。
Zi轴与关节i+1的轴线重合，指向任意；Xi轴与连杆i的两个关节轴线的公垂线重合，方向从关节i指向关节i+1。
当li＝0时，取Xi＝Zi-1×Zi，但Xi轴取向影响i正负（图2-16）；
2）坐标系{i}的Yi轴按右手法则规定，即Yi＝Zi×Xi；
3）坐标系{i}的原点Oi取在Xi和Zi的交点上。
当关节i的轴线与关节i+1的轴线相交时，原点Oi取在两轴线的交点上；当关节i的轴线与关节i+1的轴线平行时，原点Oi取在使di＝0的地方。
（对照图2-17解释）
图2-17画出了坐标系{i-1}和{i}的设定位姿（为什么叫设定位姿）。
在建立连杆坐标系时，下面四点值得注意：
1）连杆坐标系的建立不是唯一的。
例如，虽然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的参数
名称
含义
正负号
性质
i
转角
Xi-l轴绕Zi-l轴转至与Xi轴平行时的转角
按右手法则确定
转动关节为变量
移动关节为常量
di
距离
Xi-l轴沿Zi-l方向移动至与Xi轴相交时发生的位移
与Zi-l正向一致为正
转动关节为常量
移动关节为变量
li
长度
Zi-l轴沿Xi方向移动至与Zi轴相交时移动的距离
恒为非负数
常量
i
扭角
Zi-l轴绕Xi轴转至与Zi轴平行时的转角
按右手法则确定
常量
连杆i的坐标系OiXiYiZi
原点Oi
坐标轴Zi
坐标轴Xi
坐标轴Yi
位于连杆i两关节轴线之公垂线与关节i+1轴线的交点处
与关节i+1的轴线重合，方向任意确定
沿连杆i两关节轴线的公垂线，并指向i+1关节
按右手法则确定
2.4.2连杆坐标系之间的变换矩阵
建立了各连杆的坐标系后，i-1系和i系之间的变换关系可以用坐标系的平移、旋转来实现。
从i-1系到i系的变换，可先令i-1系绕Zi-l轴旋转i角（旋转后Xi-l轴与Xi轴平行），再沿Zi-l轴平移di（平移后Xi-l轴与Xi轴重合），然后沿Xi轴平移li（平移后Oi-l点与Oi点重合），最后绕Xi轴旋转i角（旋转后Zi-l轴与Zi轴重合，Yi-l轴与Yi轴也自然重合），最后使得i-1系和i系重合。
建立了各连杆的坐标系后，i-1系和i系之间的变换关系可以用坐标系的平移、旋转来实现。
假设i系和i-1系原来是重合的，先令i系绕
升级会员 