§ 2-2齐次变换及运算
刚体的运动是由转动和平移组成的。为了能用同一矩阵表示转动和平移，有必要引 (4× 4)的齐次坐标变换矩阵。
一、平移的齐次变换
首先，我们介绍点在空间直角坐标系中的平移。
如图 2-9所示，空间某一点 A，坐标为 (x,y,z)，当它平移至 A点后，坐标为 (x,y,z)以及式中,Trans(x,y,z)表示齐次坐标变换的平移算子。且也可以简写为其中第四列元素 x,y,z分别表示沿坐标轴 X,Y,Z的移动量。若算子左乘，表示坐标变换是相对固定坐标系进行的 ;假如相对动坐标系进行坐标变换,则算子应该右乘。平移的齐次变换公式（ 2-10）
同样适用于坐标系、物体等的变换。
例 2-4 有下面三种情况 (图 2-10)：动坐标系 {A}相对于固定坐标系的 X0,y0,z0轴作 (-1,2,2)平移后到
{A’}，动坐标系 {A}相对于自身坐标系（即动系）的 X、
Y,Z轴分别作 (-1,2,2)平移后到 {A“}；物体 Q相对于固定坐标系作 (2,6,0)平移后到 Q’。已知：
写出坐标系 {A},{A}以及物体 Q的矩阵表达式。
解 动坐标系 {A}的两个平移坐标变换算子均为
{A}坐标系是动系 {A}沿固定坐标系平移变换得来的，因此算子左乘,{A}的矩阵表达式为
{A}坐标系是动系 {A}沿自身坐标系作平移变换得来的，因此算子右乘,{A}的矩阵表达式为
二、旋转的齐次变换
首先我们介绍点在空间直角坐标系中的旋转，如图 2-11所示，空间某一点 A，坐标为 (x、
y,z)。当它绕 Z轴旋转 θ角后至 A点，坐标为 (x、
y,z)。 A点和 A点的坐标关系为式中,Rot(z,θ)表示齐次坐标变换时绕 Z轴的旋转算子,算子左乘表示相对于固定坐标系进行变换，算子的内容为也可简写为同理，可写出绕 X轴旋转的算子和绕 Y轴旋转的算子
图 2.12所示为点 A绕任意过原点的单位矢量此旋转 θ角的情况。
kx,ky,kz分别为此矢量在固定参考系坐标轴 X,Y,Z上的三个分量，
可以证得，绕任意过原点的单位矢量此 k转 θ角的旋转齐次变换公式为
式 (2-18)称为一般旋转齐次变换通式，它概括了绕 X轴,Y轴,Z轴进行旋转齐次变换的各种特殊情况，例如：
当 kx=1，即 ky=kz=0时，则由式 (2-18)可得到式
(2-16)；
当 ky=1，即 kx=kz=0时，则由式 (2-18)可得到式
(2-17)；
当 kz=1，即 kx=ky=0时，则由式 (2-18)可得到式
(2-15)。
反之，若给出某个旋转齐次矩阵则可根据式 (2-18)求出其等效矢量 k及等效转角 θ
式中：当 θ取 0到 180。之间的值时，式中的符号取 +号；
当转角时很小时，公式很难确定转轴；当 θ接近 0。或
180。时，转轴完全不确定。
与平移变换一样，旋转变换算子公式 (2-15)、
(2-16),(2-17)以及一般旋转变换算子公式 (2-18)，不仅仅适用于点的旋转变换,而且也适用于矢量、坐标系、
物体等旋转变换计算。若相对固定坐标系进行变换,则算子左乘；若相对动坐标系进行变换，则算子右乘。
例 2-5 已知坐标系中点 U的位置矢量 u=[7 3 2 1]T
将此点绕 Z轴旋转 90，再绕 Y轴旋转 90，如图 2-13所示，
求旋转变换后所得的点 W。
2-6 如图 2-14所示单臂操作手，手腕也具有一个自由度。已知手部起始位姿矩阵为若手臂绕 Z0轴旋转 +90，则手部到达 G2若手臂不动，
仅手部绕手腕 Zl轴旋转 +90,则手部到达 G3。写出手部坐标系 {G2}及 {G3}的矩阵表达式。
三、平移加旋转的齐次变换平移变换和旋转变换可以组合在一个齐次变换中，如例 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）的所有变换都是相对于固定坐标系ＯＸＹＺ进行的，所以各坐标变换算子应该依次左乘，即
§ 2-3工业机器人连杆参数及其齐次变换矩阵
机器人运动学的重点是研究手部的位姿和运动，而手部位姿是与机器人各杆件的尺寸、运动副类型及杆间的相互关系直接相关联的。因此在研究手部相对于机座的几何关系时，首先必须分析两相邻抨件的相互关系，即建立杆件坐标系。
一、连杆参数及连抨坐标系的建立
如图 2-16所示,连杆两端有关节 n和
n+1。该连杆尺寸可以用两个量来描述：
一个是两个关节轴线沿公垂线的距离 an,
称为连杆长度；另一个是垂直于 an的平面内两个轴线的夹角 an,称为连杆扭角。这两个参数为连杆的尺寸参数。
再考虑连杆 n与相邻连杆 n-1的关系，若它们通过关节相连，如图 2-17所示,其相对位置可用两个参数 dn和 θn来确定，其中 dn是沿关节 n轴线两个公垂线的距离,θn是垂直于关节 n轴线的平面内两个公垂线的夹角。这是表达相邻杆件关系的两个参数。这样，每个连杆可以 由四个参数所描述：其中两个描述连杆尺寸，另外两个描述连杆与相邻杆件的连接关系。对于旋转节,θn是关节变量，其它三个参数固定不变；
对于移动关节变量，其它三个参数固定不变。
连杆坐标系的建立按下面规则进行：
连杆 n坐标系 (简称 n系 )的坐标原点设在关节 n的轴编和关节 n+1的轴线的公垂线与关
n+1的轴线相交之处 (图 2-l7)。 n系的 Z轴与关 n+l的轴线重合,X轴与上述公垂线重合且方向从关节“指向关节 n十 1,y轴则按右手系确定。
现将连杆参数与坐标系的建立 (图 2-17)
归纳为表 2-1。
二、连杆坐标系之间的变换矩阵
建立了各连杆坐标系后,n-1系与 n系间的变换关系可以用坐标系的平移、旋转来实现。从 n-1系到 n系的变换，可先令 n-1系绕 Zn-1轴旋转 θn角，再沿 Zn-1轴平移 dn，然后沿 Xn轴平移 an最后 r绕 Xn轴旋转 n角，使得
n-1系与 n系重合。用一个变换矩阵 An来综合表示上述四次变换时应注意到坐标系在每次旋转或平移后发生了变动，后一次变换都是相对动系进行的，因此在运算中变换算子应该右乘。于是连杆 "的齐次变换短阵为
实际上很多机器人在设计时，常常使某些连杆参数取特别值，如使 an=0或 90。,也有使
dn=0或 an=0，从而可以简化变换矩阵 An的计算,
同时也可简化控制。
§ 24工业机器人运动学方程
一、机器人运动学方程
我们将为机器人的每一个连杆建立一个坐标系，并用齐次变换来描述这些坐标系间的相对关系，也叫相对位姿。通常把描述一个连杆坐标系与下一个连杆坐标系间相对关系的齐次变换矩阵叫做 A变换矩阵或 A矩阵。如果 Al矩阵表示第一个连杆坐标系相对于固定坐标系的位姿,A2矩阵表示第二个连抨坐标系相对于第一个连杆坐标系的位姿，那么第二个连杆坐标系在固定坐标系中的位姿可用 A1和 A2的乘积来表示：
T2=A1A2
同理，若 A3矩阵表示第三个连杆坐标系相对于第二个连杆坐标系的位姿，则有 T3=AlA2A3
如此类推，对于六连杆机器人，有下列 T6矩阵
T6=AlA2A3A4A5A6 (2-21)
此式右边表示了从固定参考系到手部坐标系的各连杆坐标系之间的变换矩阵的连乘,左边 T6表示这些变换矩阵的乘积,也就是手部坐标系相对于固定参考系的位姿,我们称式 (2-21)为机器人运动学方程。式 (2-21)计算结果 T6是一个如下的 (4× 4)矩阵：
式中，前三列表示手部的姿态，第四列表示手部的位置。
二、正向运动学及实例正向运动学主要解决机器人运动学方程的建立及手部位姿的求解问题，下面给出建立机器人运动学方程的方法及两个实例。
1.平面关节型机器人的运动学方程
图 2.18(a)所示为具有一个肩关节、一个肘关节和一个腕关节的 SCARA机器人。此类机器人的机械结构特点是三个关节轴线是相互平行的。固定坐标系 {0}和连杆 1、连杆 2、连杆 3的坐标系
{1},{2},{3}分别如图 2-18(a)所示,坐落在关节 1、关节 2、关节 3
和手部中心。坐标系 {3}也就是手部坐标系。连杆参数中 θ变量,其余参数 d,a,α均为常量。考虑到关节轴线平行，并且连杆都在一个平窗内的特点，列出 SCARA机器人连杆的参数如表 2-2所示。
该平面关节型机器人的运动学方程为
T3=AlA2A3
式中,A1表示连杆 1的坐标系 {l}相对于固定坐标系 {0}
的齐次变换矩阵； A2表示连杆 2的坐标系 {2}相对于连杆 1的坐标系 {1}的齐次变换矩阵； A3表示连杆 3的坐标系即手部坐标系 {3}相对于连杆 2的坐标系 {2}的齐次变换矩阵。参考图 2-18(b)，于是有：
式中,c123＝ cos(θ1+θ2+θ3)； s123=sin(θ1+θ2+ θ3)；
c12=cos(θ1+θ2+θ3)； s12=sin(θ1+θ2+ θ3)；
c1＝ cosθ1 s1=sinθ1。
(全书 cos可用 c表示,sin可用 s表示 )
T3是 A1,A2,A3连乘的结果，表示手部坐标系 {3}(即手部 )的位置和姿态。
于是，可写出手部位 (４＊１ )列阵为
表示手部姿态的方向
矢量 n,o,a分别为当转角变量 θ1,θ2,θ3给定时，可以算出具体数值。如图 2-18(b)所示，设 θ1 =30。,θ2 =60。,θ3 ＝ 30，则可根据平面关节型机器人运动学方程式 (2-23)求解出运动学正解，即手部的位姿矩阵表达式为
2.斯坦福机器人的运动学方程图 2-19所示为斯坦福机器人及赋给各连杆的坐标系。表 2-3给出了斯坦福机器人各连杆的参数。现在根据各连杆坐标系的关系写出齐次变换矩阵 Ai。
{1}系与 {0}系是旋转关节连接，如图 2-20(a)所示。
坐标系 {l}相对于固定坐标系 {0}的 Z。轴的旋转为变量坐标系 {l}相对于固定坐标系 {0}的 Z。轴的旋转为变量 θ1，然后绕自身坐标系 X1轴作 α1的旋转变换,α1 =-
90。所以 {2}系与 {1}系是旋转关节连接，连杆距离为 d2，
如图 2-20(b)所示。坐标系 {2}相对于坐标系 {l}的 Zl轴的旋转为变量 θ2，然后绕自身坐标系 Z2轴正向作 d2距离的平移变换及绕 X2轴作甸的旋转坐标变换，
α2 =90。
所以 {2}系与 {1}系是旋转关节连接，连距离为 d2，
如图 2-20(b)所示。坐标系 {2}相对于坐标系 {l}的 Zl轴的旋转为变量 θ2，然后绕自身坐标系 Z2轴正向作 d2距离的平移变换及绕 X2轴作：的的旋转坐标变换的 =90。所以
{3}系与 {2}系是移动关节连接如图 2-20(c)所示。坐标系 {3}相对于坐标系 {2}的 Z2轴的平移为变量 d3。所以图 2-21是斯坦福机器人手腕三个关节的示意图，
它们都是转动关节，关节变量为 θ4 θ5及 θ6并且三个关节的中心重合。
如图 2-22(a)所示，系 {4}对系 {3}的旋转变量为 θ4,
然后绕自身坐标轴 X4作问的旋转变换,,α=-90。所以
如图 2-22(b)所示，系 {5}相对于系 {4}的旋转变量为 θ5，然后绕自身坐标
轴 X5作 α5的旋转变换,α5 =90。。所以
如图 2-22(c)所示，系 {6}相对于系 {5}的旋转变量为仇，
并移动距离刃，所以
这样，所有的Ａ矩阵已建立。如果要知道非相临杆件间的关系，只要用相应的Ａ矩阵连乘即可。如：
则斯坦福机器人运动学方程为
0T6=T6=A1A2A3A4A5A6
方程 (2-30)右边的结果就是最后一个坐标系 {6}的位置和姿态矩阵，各元素均为 θ和 d的函数，当 θ和 d给出后，可以计算出斯坦福机器人手部坐标系 {6}的位置 p和姿态 n、
o,a这就是斯坦福机器人手部位姿的解，这个求解过程叫做斯坦福机器人运动学正解
例 2-8斯坦福机器人连杆参数如表 2-3所示。现已知关节变量为,θ1=90,θ2=90,d3=300mm,A=90,,θ5=90,θ6
=90。并且，机器人结构参数 d2=100mm，且 =50mm。根据斯坦福革机器人运动学方程式进行正向运动学求解，
写出手部位置及姿态 (即坐标系 {6})的齐次矩阵表普达式。
解 设图 2-19是斯坦福机器人的零位 (起始位置 )，按本例给出的关节变量进行图解，机手部及各杆件状态如图 2-23所示。
利用公式 (2-24),(2-25),(2-26),(2-27),(2-28),(2-29)
可求得矩 A1,A2,A3,A4,A5及 A6。所以,坐标系 {6}的位姿矩阵可根据运动学方程式 (2-30)求出：
式中：
假如Ｈ =0，则 n,o,a三个方向矢量不变，而位置矢量的分量 px,py,pz分别为。
代入本例给出的已知参数值和变量值，求得数值解：
该 (4× 4)矩阵即为斯坦福机器人在题目给定情况下手部的位姿矩阵,即运动学正解。
假如Ｈ =0，则
三、反向运动学及实例
上面我们说明了正向求解问题，即给出关节变量
θ和 d求出手部位姿各矢量 n,o,a和 p，这种求解方法只需将关节变量代入运动学方程中即可得出。但在机器人控制中，问题往往相反即在已知手部要到达的目标位姿的情况下如何求出关节变量，以驱动各关节的马达，使手；部的位姿得到满足，这就是反向运动学问题，也称求运动学逆解。
现以斯坦福机器人为例来介绍反向求解的一种方法。为了书写简便，假设Ｈ =0，即坐标系 {6}与坐标系
{5}原点相重合。已知斯坦福机器人的运动学方程为
取式 (3-32)等式左、右两边之第三行第四列相等，即
-pxs1+pyc1=d2
引入中间变量 r及 φ，令
px=rcosφ
至此,θ1,θ2,d3,θ4,θ5,θ6全部求出。从以上解的过程有出，这种方法就是将 9一个未知数由矩阵方程的右边移向左边，使其与其它未知数分开，解出这个未知数，再把下一个未知数移到左边，重复进行，直至解出所有未知数，所以这种方法也叫分离变量法。这是代数法的一种，它的特点是首先利用运动方程的不同形式，找出矩阵中简单表达某个未知数的元素，力求得到未知数较少的方程，然后求解。
还应注意到机器人运动学逆解问题的求解存在如下三个问题：
(1)解可能不存在。机器人具有一定的工作域，假如给定手部位置在工作域之外，则解不存在。图 2-24
所示二自由度平面关节机械手，假如给定手部位置矢量 (x,y)位于外半径为 l1十 l2与内半径为 ll1-l2l的圆环之外,
则无法求出逆解 θ1 及 θ2,即该逆解不存在。
(2)解的多重性。机器人的逆运动学问题可能出现多解。图 2-
25(a)表示一个二自由度平面关节机械手出现两个逆解的情况。对于给定的在机器人工作域内的手部位置 A(x,y)可以得到两个逆解：
θ1,θ2及 θ1,θ2。从图 (a)可知手部是不能以任意方向到达目标点儿的。增加一个手腕关节自由度，如图 (b)所示三自由度平面关节机械手即可实现手部以任意方向到达目标点 A。
在多解情况下,一定有一个最接近解，即最接近起始点的解。图 2-26(a)表示 3R机械手的手部从起始点 A
运动到目标点 B，完成实线所表示的解为最接近解，是一个“最短行程”的优化解。但是，如图 2-26(b)所示，
在有障碍存在的情况下，上述的最接近解会引起碰撞，
只能采用另一解，如图 2-26(b)中实线所示。尽管大臂、
小臂将经过“遥远”的行程，为了避免碰撞也只能用这个解，这就是解的多重性带来可供选择的好处 。 关于解的多重性的另一实例如图 2-27所表示 PUMA560机器人实现同一目标位置和姿态有四种形位，即四种解。
另外，腕部的“翻转”又可能得出两种解，其排列组合共可能有 8种解。
(3) 求解方法的多样性。机器人逆运动学求解有多种方法，一般分为两类：封闭解和数值解。不同学者对同一机器人的运动学逆解也提出不同的解法。应该从计算方法的计算效率、计算精度等要求出发，选择较好的解法。
四,X=X(q)形式运动学方程
已经讨论了用连杆坐标系之间的变换矩阵,A确定
T6建立机器人运动学方程的方法，式 (2-21)。该式前三列分别是手部坐标系的单位方向矢量 n,o,a规定了手部的姿态。这种方法在作变换运算时十分方便，但利用它作手部姿态描述并不方便。而且 n,o,a应满足正交条件 n=o× a,9个元素中只有 3个是独立的。因此，
在如何用 3个参数简便地描述手部姿态问题。“角度设定法”就是采用相对参考坐标系或相运动坐标系作三次连续转动来规定姿态的方法。于是机器人手部位姿可用一个 6维列矢量表示
或写成式中,x,y,z表示手部位置,φx,φy,φz分别是用角度设定法来规定手部姿态时绕 X轴,Y轴和 Z轴的转角。
1.RPY角法和欧拉角法
RPY角法和欧拉角法是角度设定法中常用的方法。 RPY角法是手部相对参考坐标系轴作三次连续转动获得规定的姿态：先绕 X
轴转动￠ x角，称为偏转 (Yaw)，再绕 y轴转动 φy角，称为俯仰
(Pitch)，最后绕 Z轴转动 φz角，称为翻滚 (Roil)，得到相应的旋转矩阵为
上式也称为,x-y-z”RPY角设定法。欧拉角法是手部相对运动坐标系轴作三次连续转动获得规定的姿态：如果转动顺序为立 z-y-x立,则相应的旋转矩阵为上式也称为,z-y-x”欧拉角设定法。式 (2-45)和式 (2-46)
结果恰巧完全相同。如果用其它顺序进行欧拉角三次连续转动，结果便不相同了。不论用什么角度设定法来规定手部姿态，姿态的实现，事实上是由关节变量所决定的。知道旋转矩阵则可由式 (2-45)或 (2-46)逆解出手部姿态的设定角 φx,φy,φz 并且 φx= φx(q)、
φy= φy(q),φz= φz(q)为广义关节变量,q为广义关节变量,q=[q1q2…q n]T
2.运动学方程 X=X(q)
用矩阵确定 T6即式 (2-21)可写成或写成上式表示机器人手部位姿 (n,o,a,p)与关节变量 q之间的关系，对于斯坦福机器人可写出具有 12个方程的方程组，即式 (2-31)。
对于式 (2-44)可写成：
或
上式表示机器人手部位姿 X与关节变量 q之间的关系，是用“角度设定法”规定姿态的机器人运动方程式。
式 (2-47)及式 (2-48)右边广义关节矢量 q构成关节空间，左边均是在直角坐标空间即操作空间中描述的手部位姿，仅仅描述姿态的方法不同。但是,由于式 (2-48)简洁以及描述姿态方便，以后会经常遇到。假如用 x=x(q)统一表达式 (2-47)和式 (2-48)，则应注意符号的含义旨是不同的。
刚体的运动是由转动和平移组成的。为了能用同一矩阵表示转动和平移，有必要引 (4× 4)的齐次坐标变换矩阵。
一、平移的齐次变换
首先，我们介绍点在空间直角坐标系中的平移。
如图 2-9所示，空间某一点 A，坐标为 (x,y,z)，当它平移至 A点后，坐标为 (x,y,z)以及式中,Trans(x,y,z)表示齐次坐标变换的平移算子。且也可以简写为其中第四列元素 x,y,z分别表示沿坐标轴 X,Y,Z的移动量。若算子左乘，表示坐标变换是相对固定坐标系进行的 ;假如相对动坐标系进行坐标变换,则算子应该右乘。平移的齐次变换公式（ 2-10）
同样适用于坐标系、物体等的变换。
例 2-4 有下面三种情况 (图 2-10)：动坐标系 {A}相对于固定坐标系的 X0,y0,z0轴作 (-1,2,2)平移后到
{A’}，动坐标系 {A}相对于自身坐标系（即动系）的 X、
Y,Z轴分别作 (-1,2,2)平移后到 {A“}；物体 Q相对于固定坐标系作 (2,6,0)平移后到 Q’。已知：
写出坐标系 {A},{A}以及物体 Q的矩阵表达式。
解 动坐标系 {A}的两个平移坐标变换算子均为
{A}坐标系是动系 {A}沿固定坐标系平移变换得来的，因此算子左乘,{A}的矩阵表达式为
{A}坐标系是动系 {A}沿自身坐标系作平移变换得来的，因此算子右乘,{A}的矩阵表达式为
二、旋转的齐次变换
首先我们介绍点在空间直角坐标系中的旋转，如图 2-11所示，空间某一点 A，坐标为 (x、
y,z)。当它绕 Z轴旋转 θ角后至 A点，坐标为 (x、
y,z)。 A点和 A点的坐标关系为式中,Rot(z,θ)表示齐次坐标变换时绕 Z轴的旋转算子,算子左乘表示相对于固定坐标系进行变换，算子的内容为也可简写为同理，可写出绕 X轴旋转的算子和绕 Y轴旋转的算子
图 2.12所示为点 A绕任意过原点的单位矢量此旋转 θ角的情况。
kx,ky,kz分别为此矢量在固定参考系坐标轴 X,Y,Z上的三个分量，
可以证得，绕任意过原点的单位矢量此 k转 θ角的旋转齐次变换公式为
式 (2-18)称为一般旋转齐次变换通式，它概括了绕 X轴,Y轴,Z轴进行旋转齐次变换的各种特殊情况，例如：
当 kx=1，即 ky=kz=0时，则由式 (2-18)可得到式
(2-16)；
当 ky=1，即 kx=kz=0时，则由式 (2-18)可得到式
(2-17)；
当 kz=1，即 kx=ky=0时，则由式 (2-18)可得到式
(2-15)。
反之，若给出某个旋转齐次矩阵则可根据式 (2-18)求出其等效矢量 k及等效转角 θ
式中：当 θ取 0到 180。之间的值时，式中的符号取 +号；
当转角时很小时，公式很难确定转轴；当 θ接近 0。或
180。时，转轴完全不确定。
与平移变换一样，旋转变换算子公式 (2-15)、
(2-16),(2-17)以及一般旋转变换算子公式 (2-18)，不仅仅适用于点的旋转变换,而且也适用于矢量、坐标系、
物体等旋转变换计算。若相对固定坐标系进行变换,则算子左乘；若相对动坐标系进行变换，则算子右乘。
例 2-5 已知坐标系中点 U的位置矢量 u=[7 3 2 1]T
将此点绕 Z轴旋转 90，再绕 Y轴旋转 90，如图 2-13所示，
求旋转变换后所得的点 W。
2-6 如图 2-14所示单臂操作手，手腕也具有一个自由度。已知手部起始位姿矩阵为若手臂绕 Z0轴旋转 +90，则手部到达 G2若手臂不动，
仅手部绕手腕 Zl轴旋转 +90,则手部到达 G3。写出手部坐标系 {G2}及 {G3}的矩阵表达式。
三、平移加旋转的齐次变换平移变换和旋转变换可以组合在一个齐次变换中，如例 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）的所有变换都是相对于固定坐标系ＯＸＹＺ进行的，所以各坐标变换算子应该依次左乘，即
§ 2-3工业机器人连杆参数及其齐次变换矩阵
机器人运动学的重点是研究手部的位姿和运动，而手部位姿是与机器人各杆件的尺寸、运动副类型及杆间的相互关系直接相关联的。因此在研究手部相对于机座的几何关系时，首先必须分析两相邻抨件的相互关系，即建立杆件坐标系。
一、连杆参数及连抨坐标系的建立
如图 2-16所示,连杆两端有关节 n和
n+1。该连杆尺寸可以用两个量来描述：
一个是两个关节轴线沿公垂线的距离 an,
称为连杆长度；另一个是垂直于 an的平面内两个轴线的夹角 an,称为连杆扭角。这两个参数为连杆的尺寸参数。
再考虑连杆 n与相邻连杆 n-1的关系，若它们通过关节相连，如图 2-17所示,其相对位置可用两个参数 dn和 θn来确定，其中 dn是沿关节 n轴线两个公垂线的距离,θn是垂直于关节 n轴线的平面内两个公垂线的夹角。这是表达相邻杆件关系的两个参数。这样，每个连杆可以 由四个参数所描述：其中两个描述连杆尺寸，另外两个描述连杆与相邻杆件的连接关系。对于旋转节,θn是关节变量，其它三个参数固定不变；
对于移动关节变量，其它三个参数固定不变。
连杆坐标系的建立按下面规则进行：
连杆 n坐标系 (简称 n系 )的坐标原点设在关节 n的轴编和关节 n+1的轴线的公垂线与关
n+1的轴线相交之处 (图 2-l7)。 n系的 Z轴与关 n+l的轴线重合,X轴与上述公垂线重合且方向从关节“指向关节 n十 1,y轴则按右手系确定。
现将连杆参数与坐标系的建立 (图 2-17)
归纳为表 2-1。
二、连杆坐标系之间的变换矩阵
建立了各连杆坐标系后,n-1系与 n系间的变换关系可以用坐标系的平移、旋转来实现。从 n-1系到 n系的变换，可先令 n-1系绕 Zn-1轴旋转 θn角，再沿 Zn-1轴平移 dn，然后沿 Xn轴平移 an最后 r绕 Xn轴旋转 n角，使得
n-1系与 n系重合。用一个变换矩阵 An来综合表示上述四次变换时应注意到坐标系在每次旋转或平移后发生了变动，后一次变换都是相对动系进行的，因此在运算中变换算子应该右乘。于是连杆 "的齐次变换短阵为
实际上很多机器人在设计时，常常使某些连杆参数取特别值，如使 an=0或 90。,也有使
dn=0或 an=0，从而可以简化变换矩阵 An的计算,
同时也可简化控制。
§ 24工业机器人运动学方程
一、机器人运动学方程
我们将为机器人的每一个连杆建立一个坐标系，并用齐次变换来描述这些坐标系间的相对关系，也叫相对位姿。通常把描述一个连杆坐标系与下一个连杆坐标系间相对关系的齐次变换矩阵叫做 A变换矩阵或 A矩阵。如果 Al矩阵表示第一个连杆坐标系相对于固定坐标系的位姿,A2矩阵表示第二个连抨坐标系相对于第一个连杆坐标系的位姿，那么第二个连杆坐标系在固定坐标系中的位姿可用 A1和 A2的乘积来表示：
T2=A1A2
同理，若 A3矩阵表示第三个连杆坐标系相对于第二个连杆坐标系的位姿，则有 T3=AlA2A3
如此类推，对于六连杆机器人，有下列 T6矩阵
T6=AlA2A3A4A5A6 (2-21)
此式右边表示了从固定参考系到手部坐标系的各连杆坐标系之间的变换矩阵的连乘,左边 T6表示这些变换矩阵的乘积,也就是手部坐标系相对于固定参考系的位姿,我们称式 (2-21)为机器人运动学方程。式 (2-21)计算结果 T6是一个如下的 (4× 4)矩阵：
式中，前三列表示手部的姿态，第四列表示手部的位置。
二、正向运动学及实例正向运动学主要解决机器人运动学方程的建立及手部位姿的求解问题，下面给出建立机器人运动学方程的方法及两个实例。
1.平面关节型机器人的运动学方程
图 2.18(a)所示为具有一个肩关节、一个肘关节和一个腕关节的 SCARA机器人。此类机器人的机械结构特点是三个关节轴线是相互平行的。固定坐标系 {0}和连杆 1、连杆 2、连杆 3的坐标系
{1},{2},{3}分别如图 2-18(a)所示,坐落在关节 1、关节 2、关节 3
和手部中心。坐标系 {3}也就是手部坐标系。连杆参数中 θ变量,其余参数 d,a,α均为常量。考虑到关节轴线平行，并且连杆都在一个平窗内的特点，列出 SCARA机器人连杆的参数如表 2-2所示。
该平面关节型机器人的运动学方程为
T3=AlA2A3
式中,A1表示连杆 1的坐标系 {l}相对于固定坐标系 {0}
的齐次变换矩阵； A2表示连杆 2的坐标系 {2}相对于连杆 1的坐标系 {1}的齐次变换矩阵； A3表示连杆 3的坐标系即手部坐标系 {3}相对于连杆 2的坐标系 {2}的齐次变换矩阵。参考图 2-18(b)，于是有：
式中,c123＝ cos(θ1+θ2+θ3)； s123=sin(θ1+θ2+ θ3)；
c12=cos(θ1+θ2+θ3)； s12=sin(θ1+θ2+ θ3)；
c1＝ cosθ1 s1=sinθ1。
(全书 cos可用 c表示,sin可用 s表示 )
T3是 A1,A2,A3连乘的结果，表示手部坐标系 {3}(即手部 )的位置和姿态。
于是，可写出手部位 (４＊１ )列阵为
表示手部姿态的方向
矢量 n,o,a分别为当转角变量 θ1,θ2,θ3给定时，可以算出具体数值。如图 2-18(b)所示，设 θ1 =30。,θ2 =60。,θ3 ＝ 30，则可根据平面关节型机器人运动学方程式 (2-23)求解出运动学正解，即手部的位姿矩阵表达式为
2.斯坦福机器人的运动学方程图 2-19所示为斯坦福机器人及赋给各连杆的坐标系。表 2-3给出了斯坦福机器人各连杆的参数。现在根据各连杆坐标系的关系写出齐次变换矩阵 Ai。
{1}系与 {0}系是旋转关节连接，如图 2-20(a)所示。
坐标系 {l}相对于固定坐标系 {0}的 Z。轴的旋转为变量坐标系 {l}相对于固定坐标系 {0}的 Z。轴的旋转为变量 θ1，然后绕自身坐标系 X1轴作 α1的旋转变换,α1 =-
90。所以 {2}系与 {1}系是旋转关节连接，连杆距离为 d2，
如图 2-20(b)所示。坐标系 {2}相对于坐标系 {l}的 Zl轴的旋转为变量 θ2，然后绕自身坐标系 Z2轴正向作 d2距离的平移变换及绕 X2轴作甸的旋转坐标变换，
α2 =90。
所以 {2}系与 {1}系是旋转关节连接，连距离为 d2，
如图 2-20(b)所示。坐标系 {2}相对于坐标系 {l}的 Zl轴的旋转为变量 θ2，然后绕自身坐标系 Z2轴正向作 d2距离的平移变换及绕 X2轴作：的的旋转坐标变换的 =90。所以
{3}系与 {2}系是移动关节连接如图 2-20(c)所示。坐标系 {3}相对于坐标系 {2}的 Z2轴的平移为变量 d3。所以图 2-21是斯坦福机器人手腕三个关节的示意图，
它们都是转动关节，关节变量为 θ4 θ5及 θ6并且三个关节的中心重合。
如图 2-22(a)所示，系 {4}对系 {3}的旋转变量为 θ4,
然后绕自身坐标轴 X4作问的旋转变换,,α=-90。所以
如图 2-22(b)所示，系 {5}相对于系 {4}的旋转变量为 θ5，然后绕自身坐标
轴 X5作 α5的旋转变换,α5 =90。。所以
如图 2-22(c)所示，系 {6}相对于系 {5}的旋转变量为仇，
并移动距离刃，所以
这样，所有的Ａ矩阵已建立。如果要知道非相临杆件间的关系，只要用相应的Ａ矩阵连乘即可。如：
则斯坦福机器人运动学方程为
0T6=T6=A1A2A3A4A5A6
方程 (2-30)右边的结果就是最后一个坐标系 {6}的位置和姿态矩阵，各元素均为 θ和 d的函数，当 θ和 d给出后，可以计算出斯坦福机器人手部坐标系 {6}的位置 p和姿态 n、
o,a这就是斯坦福机器人手部位姿的解，这个求解过程叫做斯坦福机器人运动学正解
例 2-8斯坦福机器人连杆参数如表 2-3所示。现已知关节变量为,θ1=90,θ2=90,d3=300mm,A=90,,θ5=90,θ6
=90。并且，机器人结构参数 d2=100mm，且 =50mm。根据斯坦福革机器人运动学方程式进行正向运动学求解，
写出手部位置及姿态 (即坐标系 {6})的齐次矩阵表普达式。
解 设图 2-19是斯坦福机器人的零位 (起始位置 )，按本例给出的关节变量进行图解，机手部及各杆件状态如图 2-23所示。
利用公式 (2-24),(2-25),(2-26),(2-27),(2-28),(2-29)
可求得矩 A1,A2,A3,A4,A5及 A6。所以,坐标系 {6}的位姿矩阵可根据运动学方程式 (2-30)求出：
式中：
假如Ｈ =0，则 n,o,a三个方向矢量不变，而位置矢量的分量 px,py,pz分别为。
代入本例给出的已知参数值和变量值，求得数值解：
该 (4× 4)矩阵即为斯坦福机器人在题目给定情况下手部的位姿矩阵,即运动学正解。
假如Ｈ =0，则
三、反向运动学及实例
上面我们说明了正向求解问题，即给出关节变量
θ和 d求出手部位姿各矢量 n,o,a和 p，这种求解方法只需将关节变量代入运动学方程中即可得出。但在机器人控制中，问题往往相反即在已知手部要到达的目标位姿的情况下如何求出关节变量，以驱动各关节的马达，使手；部的位姿得到满足，这就是反向运动学问题，也称求运动学逆解。
现以斯坦福机器人为例来介绍反向求解的一种方法。为了书写简便，假设Ｈ =0，即坐标系 {6}与坐标系
{5}原点相重合。已知斯坦福机器人的运动学方程为
取式 (3-32)等式左、右两边之第三行第四列相等，即
-pxs1+pyc1=d2
引入中间变量 r及 φ，令
px=rcosφ
至此,θ1,θ2,d3,θ4,θ5,θ6全部求出。从以上解的过程有出，这种方法就是将 9一个未知数由矩阵方程的右边移向左边，使其与其它未知数分开，解出这个未知数，再把下一个未知数移到左边，重复进行，直至解出所有未知数，所以这种方法也叫分离变量法。这是代数法的一种，它的特点是首先利用运动方程的不同形式，找出矩阵中简单表达某个未知数的元素，力求得到未知数较少的方程，然后求解。
还应注意到机器人运动学逆解问题的求解存在如下三个问题：
(1)解可能不存在。机器人具有一定的工作域，假如给定手部位置在工作域之外，则解不存在。图 2-24
所示二自由度平面关节机械手，假如给定手部位置矢量 (x,y)位于外半径为 l1十 l2与内半径为 ll1-l2l的圆环之外,
则无法求出逆解 θ1 及 θ2,即该逆解不存在。
(2)解的多重性。机器人的逆运动学问题可能出现多解。图 2-
25(a)表示一个二自由度平面关节机械手出现两个逆解的情况。对于给定的在机器人工作域内的手部位置 A(x,y)可以得到两个逆解：
θ1,θ2及 θ1,θ2。从图 (a)可知手部是不能以任意方向到达目标点儿的。增加一个手腕关节自由度，如图 (b)所示三自由度平面关节机械手即可实现手部以任意方向到达目标点 A。
在多解情况下,一定有一个最接近解，即最接近起始点的解。图 2-26(a)表示 3R机械手的手部从起始点 A
运动到目标点 B，完成实线所表示的解为最接近解，是一个“最短行程”的优化解。但是，如图 2-26(b)所示，
在有障碍存在的情况下，上述的最接近解会引起碰撞，
只能采用另一解，如图 2-26(b)中实线所示。尽管大臂、
小臂将经过“遥远”的行程，为了避免碰撞也只能用这个解，这就是解的多重性带来可供选择的好处 。 关于解的多重性的另一实例如图 2-27所表示 PUMA560机器人实现同一目标位置和姿态有四种形位，即四种解。
另外，腕部的“翻转”又可能得出两种解，其排列组合共可能有 8种解。
(3) 求解方法的多样性。机器人逆运动学求解有多种方法，一般分为两类：封闭解和数值解。不同学者对同一机器人的运动学逆解也提出不同的解法。应该从计算方法的计算效率、计算精度等要求出发，选择较好的解法。
四,X=X(q)形式运动学方程
已经讨论了用连杆坐标系之间的变换矩阵,A确定
T6建立机器人运动学方程的方法，式 (2-21)。该式前三列分别是手部坐标系的单位方向矢量 n,o,a规定了手部的姿态。这种方法在作变换运算时十分方便，但利用它作手部姿态描述并不方便。而且 n,o,a应满足正交条件 n=o× a,9个元素中只有 3个是独立的。因此，
在如何用 3个参数简便地描述手部姿态问题。“角度设定法”就是采用相对参考坐标系或相运动坐标系作三次连续转动来规定姿态的方法。于是机器人手部位姿可用一个 6维列矢量表示
或写成式中,x,y,z表示手部位置,φx,φy,φz分别是用角度设定法来规定手部姿态时绕 X轴,Y轴和 Z轴的转角。
1.RPY角法和欧拉角法
RPY角法和欧拉角法是角度设定法中常用的方法。 RPY角法是手部相对参考坐标系轴作三次连续转动获得规定的姿态：先绕 X
轴转动￠ x角，称为偏转 (Yaw)，再绕 y轴转动 φy角，称为俯仰
(Pitch)，最后绕 Z轴转动 φz角，称为翻滚 (Roil)，得到相应的旋转矩阵为
上式也称为,x-y-z”RPY角设定法。欧拉角法是手部相对运动坐标系轴作三次连续转动获得规定的姿态：如果转动顺序为立 z-y-x立,则相应的旋转矩阵为上式也称为,z-y-x”欧拉角设定法。式 (2-45)和式 (2-46)
结果恰巧完全相同。如果用其它顺序进行欧拉角三次连续转动，结果便不相同了。不论用什么角度设定法来规定手部姿态，姿态的实现，事实上是由关节变量所决定的。知道旋转矩阵则可由式 (2-45)或 (2-46)逆解出手部姿态的设定角 φx,φy,φz 并且 φx= φx(q)、
φy= φy(q),φz= φz(q)为广义关节变量,q为广义关节变量,q=[q1q2…q n]T
2.运动学方程 X=X(q)
用矩阵确定 T6即式 (2-21)可写成或写成上式表示机器人手部位姿 (n,o,a,p)与关节变量 q之间的关系，对于斯坦福机器人可写出具有 12个方程的方程组，即式 (2-31)。
对于式 (2-44)可写成：
或
上式表示机器人手部位姿 X与关节变量 q之间的关系，是用“角度设定法”规定姿态的机器人运动方程式。
式 (2-47)及式 (2-48)右边广义关节矢量 q构成关节空间，左边均是在直角坐标空间即操作空间中描述的手部位姿，仅仅描述姿态的方法不同。但是,由于式 (2-48)简洁以及描述姿态方便，以后会经常遇到。假如用 x=x(q)统一表达式 (2-47)和式 (2-48)，则应注意符号的含义旨是不同的。
