第二章 机器人运动学
机器人，尤其是其中最为常用的关节型机器人，由若干个关节所联系起来的一种开链，其一端固结在机座上，另一端安装有末端执行器。已知所有关节变量确定机器人末端执行器的位姿或者由末端手的位姿计算出每一个关节变量值是机器人运动学研究的主要内容。
本章主要介绍机器人运动学，首先介绍了 1.1齐次坐标与齐次变换
在描述刚体（如零件、工具或机械手）间关系时，要用到点、向量、坐标系、平移、旋转以及变换等概念，这些概念可用齐次矩阵来表示。
1.1.1空间点的表示
在指定的直角坐标系{A }中，空间任一点P （图2-1）的位置可用3⨯1的列矢量P 表
A
示：
A
P =p x
[
p y
A
p z （2.1）
A
]
其中p x ，p y ，p z 为点P 的三个坐标分量，P 的上标A 代表参考坐标系{A }，称P 为位置矢量。
图2-1位置表示
1.1.2空间向量的表示
将一个n 维空间的点用n +1维坐标表示，则该n +1维坐标即为n 维坐标的齐次坐标，即：
A
P =p x
[
p y
T
p z 1 （2.2）
]
在上式中加入一个比例因子w ，点P 表示为：
A
P =a x
[
b y c z
T
w （2.3）
]
其中，a x =p x w ，b y =p y w ，c z =p z w 。式2.2和2.3表示同一个点P 。 起始于原点，终止于P 点的空间向量也可以采用齐次矩阵形式表示：
P =a x b y
[
c z
T
w （2.4）
]
若比例因子w 变化，向量的大小也会发生变化，w 大于1，向量所有的分量都变大，如果w 小于1，向量所有的分量都变小，w 等于1，各分量的大小保持不变。w 等于0表示该向量的方向，称为方向向量。如图2-2中，i 、j 、k 分别表示直角坐标系中X 、Y 、Z 坐标轴的单位矢量，用齐次坐标表示为：
T T T
X =[1000] Y =[0100]Z =[0010] （2.5）
图2-2中所示的矢量u 的方向表示为：
u =[cos αcos β
cos γ
T
0] （2.6）
其中α、β、γ分别为矢量u 与坐标轴的夹角。 1.1.3刚体位姿的表示
为了研究机器人的运动，往往不仅要表示空间某个点的位置，而且需要表示刚体的姿态。指定一个坐标系与此刚体固接，再将此坐标系在空间表示出来，该坐标系称为动坐标系。如
Y 'Z '为固接在刚体上的一个动坐标系，动坐标系的图2-3所示，O '为刚体上任一点，O 'X '
原点与固定坐标系原点之间做一个向量P 来表示动坐标系的位置，即为式2.2。动坐标系的姿态可由其坐标轴方向来表示，令n 、o 、a 分别为X '、Y '、Z '坐标轴的单位向量，每个向量都由其所在固定坐标系中的三个分量表示：
⎧n =n x
⎪
⎨o =o x ⎪a =a
x ⎩
[[[
n y o y a y
n z o z a z
]
0] （2.7） 0]
T T T
动坐标系的位姿可以由三个表示方向的单位向量以及第四个位置向量来表示，式中前三个向量是w =0的方向向量，表示该坐标系的三个单位向量n 、o 和a 的方向，而第四个
w =1的向量表示动坐标系的原点相对于固定坐标系的位置。与单位向量不同，向量P 的长
度十分重要，因而比例因子为1。
F =[n o a
⎡n x
⎢n p ]=⎢y
⎢n z ⎢⎣0
o x o y o z 0a x a y a z 0
p x ⎤p y ⎥⎥ （2.8） p z ⎥⎥1⎦
由于动坐标系一直固接在该刚体上，只要动坐标系在空间表示出来，刚体相对于固定坐
标系的位姿也可以采用式2-8进行描述。空间中的刚体具有沿着X 、Y 和Z 三轴的移动和绕三轴的旋转，因此需要6条独立的信息描述刚体的位姿。而式2-8中给出了12条信息，其中9条为姿态信息，3条为位置信息。三个单位向量n 、o 和a 两两正交，因而它的9个元素满足6个约束条件：
n ⋅o =n ⋅a =a ⋅o =0 （2.9）
n =o =a =1 （2.10）
1.2齐次变换
变换为空间的一个运动，由旋转和平移组成。当空间的一个坐标系相对于固定的参考坐
标系运动时，这一运动可以用类似于表示坐标系的方式来表示。变换本身就是坐标系位姿的变换，分为以下几种形式：
1.2.1平移的齐次变换
一个坐标系（或刚体）在空间中以不变的姿态运动，该变换为平移变换。坐标系的单位向量保持同一方向不变，所有改变只是坐标系原点相对于参考坐标系的变化，如图2-2所示。相对于固定参考系的新的坐标系的位置可以用原来坐标系的原点位置向量加上表示位移的向量求得。变换矩阵为：
⎡1
⎢0T =⎢
⎢0⎢⎣0
0100
0d x ⎤0d y ⎥⎥ （2.9） 1d z ⎥
⎥01⎦
其中d x ，d y 和d z 是纯平移向量d 相对于参考坐标系x ，y 和z 轴的三个向量，矩阵T 的前三列等同于单位矩阵，表示没有旋转运动，最后一列表示平移运动。若坐标系平移前的位姿矩阵为：
F old
⎡nx ox ax P x ⎤
⎢ny oy ay P ⎥
y ⎥ （2.9） =⎢
⎢nz oz az P z ⎥⎢⎥0001⎣⎦
则，坐标系平移后的位姿矩阵为：
F new
⎡1⎢0=⎢⎢0⎢⎣0
0100
0d x ⎤⎡nx ox ax P x +d x ⎤
⎢ny oy ay P +d ⎥0d y ⎥y y ⎥⎥⨯F old =⎢ （2.9） ⎢nz oz az P z +d z ⎥1d z ⎥
⎥⎢⎥01⎦0001⎣⎦
也可表示为：
F new =Trans (d x , d y , d z ) ⨯F old （2.9）
对比式子2.9和2.9，坐标系平移变换前后的位姿矩阵维数相同，方向向量保持不变，
仅是位置向量发生变换，即为d 和P 相加的结果。
1.2.2旋转的齐次变换
空间某点P ，坐标为(P x , P y , P z ) ，当它绕x 轴旋转角后至P '点，坐标为(P x ', P y ', P z ') ，
P 点和P '点的坐标关系为：
⎧P x '=P x
⎪
⎨P y =P y cos θ-P z sin θ （2.9） ⎪P =P sin θ+P cos θ
y z ⎩z
写成矩阵形式为：
⎡P x '⎤⎡1
⎢⎥⎢ ⎢P y '⎥=⎢0
⎢⎣P z '⎥⎦⎢⎣0
也可表示为：
0c o θs
s i n θ
⎤⎡P x ⎤
⎢P ⎥-s i n θ⎥⎥⎢y ⎥ （2.9）
c o θs ⎥⎦⎢⎣P z ⎥⎦
P '=Rot (x , θ) ⨯P （2.9）
Rot (x , θ) 表示齐次坐标变换时绕x 轴的旋转变换矩阵，第一列表示相对于x 轴的位置，
其值为1，0，0，它表示沿着x 轴的坐标没有改变。习惯上采用符号C θ表示cos θ，S θ表示sin θ，因此旋转矩阵表示为：
⎡10
Rot (x , θ) =⎢⎢0C θ
⎢⎣0S θ
将上式矩阵写成4⨯4的齐次变换矩阵为：
0⎤
-S θ⎥⎥ （2.9） C θ⎥⎦
⎡10⎢0C θ
Rot (x , θ) =⎢
⎢0S θ⎢
⎣00
同理，绕y 轴和z 轴的旋转矩阵分别为：
0-S θC θ0
0⎤0⎥⎥ （2.9） 0⎥⎥1⎦
⎡C θ⎢0
Rot (y , θ) =⎢
⎢-S θ⎢⎣0
1.2.3复合变换
0S θ100C θ00
0⎤⎡C θ
⎢S θ0⎥⎥ Rot (z , θ) =⎢⎢00⎥
⎥⎢1⎦⎣0
-S θ
C θ000010
0⎤0⎥⎥ （2.9） 0⎥⎥1⎦
复合变换是由固定参考坐标系或当前运动坐标系的一系列沿轴平移和绕轴旋转变换所
组成的。任何变换都可以分解为按一定顺序的一组平移和旋转变换，将两种变换组合在一个齐次变换中，称为复合变换。在计算变换矩阵时，要考虑算子左、右乘规则：如果相对于固定坐标系进行变换，则算子左乘；如果相对于动坐标系进行变换，则算子右乘。
动坐标系(, , )相对于参考坐标系(x , y , z )依次进行下面三种变换：（1）绕x 轴旋转α
度；（2）接着沿x 轴平移l 1，沿z 轴平移l 3；（3）最后，绕y 轴旋转β度。
点P 固连在动坐标系上，动坐标系与固定坐标系重合，随着动坐标系(, , )相对于固
定坐标系旋转或平移时，坐标系中的P 点相对于固定坐标系也跟着改变，第一步变换后，P 点相对于固定坐标系的坐标为：
P 1, xyz =Rot (x , α) ⨯P ； （2.9）
第二步变换后，P 点相对于固定坐标系的坐标为：
P 2, xyz =Trans (l 1, 0, l 3) ⨯P (l 1, 0, l 3) ⨯Rot (x , α) ⨯P ； （2.9） 1, xyz =Trans
第三步变换后，P 点相对于固定坐标系的坐标为：
P (l 1, 0, l 3) ⨯Rot (x , α) ⨯P ； （2.9） 3, xyz =Rot (y , β) ⨯P 2, xyz =Rot (y , β) ⨯Trans
每一步变换都是用变换矩阵左乘P 点的坐标得到该点相对于固定坐标系的坐标，矩阵
的书写顺序和进行变换的顺序正好相反，变换的顺序很重要，如果颠倒了变换的顺序，结果将完全不同。
1.2.4相对于动坐标系的变换
上述介绍的变换是相对于固定坐标系进行的，所有的平移距离和旋转角度都是相对于参
考坐标系轴来测量的。在实际应用中，也有可能相对于动坐标系或当前坐标系的轴变换。点
P 固连在动坐标系(, , )上，依次进行下面三种变换：（1）绕轴旋转α度；（2）接着
沿轴平移l 1，沿轴平移l 3；（3）最后，绕轴旋转β度。
为了计算当前坐标系中点的坐标相对于参考坐标系的变化，这时需要右乘变换矩阵而不
是左乘。第一步变换后，P 点相对于固定坐标系的坐标为：
P 1, xyz =Rot (, α) ⨯P ； 第二步变换后，P 点相对于固定坐标系的坐标为：
P 2, xyz =Rot (, α) ⨯Trans (l 1, 0, l 3) ⨯P ； 第三步变换后，P 点相对于固定坐标系的坐标为：
P 3, xyz =Rot (, α) ⨯Trans (l 1, 0, l 3) ⨯Rot (, β) ⨯P ； 1.3机器人位姿分析 1.3.1杆件坐标系的建立
2.9）
2.9）
2.9）
（ （ （
第二章 机器人运动学
机器人，尤其是其中最为常用的关节型机器人，由若干个关节所联系起来的一种开链，其一端固结在机座上，另一端安装有末端执行器。已知所有关节变量确定机器人末端执行器的位姿或者由末端手的位姿计算出每一个关节变量值是机器人运动学研究的主要内容。
本章主要介绍机器人运动学，首先介绍了 1.1齐次坐标与齐次变换
在描述刚体（如零件、工具或机械手）间关系时，要用到点、向量、坐标系、平移、旋转以及变换等概念，这些概念可用齐次矩阵来表示。
1.1.1空间点的表示
在指定的直角坐标系{A }中，空间任一点P （图2-1）的位置可用3⨯1的列矢量P 表
A
示：
A
P =p x
[
p y
A
p z （2.1）
A
]
其中p x ，p y ，p z 为点P 的三个坐标分量，P 的上标A 代表参考坐标系{A }，称P 为位置矢量。
图2-1位置表示
1.1.2空间向量的表示
将一个n 维空间的点用n +1维坐标表示，则该n +1维坐标即为n 维坐标的齐次坐标，即：
A
P =p x
[
p y
T
p z 1 （2.2）
]
在上式中加入一个比例因子w ，点P 表示为：
A
P =a x
[
b y c z
T
w （2.3）
]
其中，a x =p x w ，b y =p y w ，c z =p z w 。式2.2和2.3表示同一个点P 。 起始于原点，终止于P 点的空间向量也可以采用齐次矩阵形式表示：
P =a x b y
[
c z
T
w （2.4）
]
若比例因子w 变化，向量的大小也会发生变化，w 大于1，向量所有的分量都变大，如果w 小于1，向量所有的分量都变小，w 等于1，各分量的大小保持不变。w 等于0表示该向量的方向，称为方向向量。如图2-2中，i 、j 、k 分别表示直角坐标系中X 、Y 、Z 坐标轴的单位矢量，用齐次坐标表示为：
T T T
X =[1000] Y =[0100]Z =[0010] （2.5）
图2-2中所示的矢量u 的方向表示为：
u =[cos αcos β
cos γ
T
0] （2.6）
其中α、β、γ分别为矢量u 与坐标轴的夹角。 1.1.3刚体位姿的表示
为了研究机器人的运动，往往不仅要表示空间某个点的位置，而且需要表示刚体的姿态。指定一个坐标系与此刚体固接，再将此坐标系在空间表示出来，该坐标系称为动坐标系。如
Y 'Z '为固接在刚体上的一个动坐标系，动坐标系的图2-3所示，O '为刚体上任一点，O 'X '
原点与固定坐标系原点之间做一个向量P 来表示动坐标系的位置，即为式2.2。动坐标系的姿态可由其坐标轴方向来表示，令n 、o 、a 分别为X '、Y '、Z '坐标轴的单位向量，每个向量都由其所在固定坐标系中的三个分量表示：
⎧n =n x
⎪
⎨o =o x ⎪a =a
x ⎩
[[[
n y o y a y
n z o z a z
]
0] （2.7） 0]
T T T
动坐标系的位姿可以由三个表示方向的单位向量以及第四个位置向量来表示，式中前三个向量是w =0的方向向量，表示该坐标系的三个单位向量n 、o 和a 的方向，而第四个
w =1的向量表示动坐标系的原点相对于固定坐标系的位置。与单位向量不同，向量P 的长
度十分重要，因而比例因子为1。
F =[n o a
⎡n x
⎢n p ]=⎢y
⎢n z ⎢⎣0
o x o y o z 0a x a y a z 0
p x ⎤p y ⎥⎥ （2.8） p z ⎥⎥1⎦
由于动坐标系一直固接在该刚体上，只要动坐标系在空间表示出来，刚体相对于固定坐
标系的位姿也可以采用式2-8进行描述。空间中的刚体具有沿着X 、Y 和Z 三轴的移动和绕三轴的旋转，因此需要6条独立的信息描述刚体的位姿。而式2-8中给出了12条信息，其中9条为姿态信息，3条为位置信息。三个单位向量n 、o 和a 两两正交，因而它的9个元素满足6个约束条件：
n ⋅o =n ⋅a =a ⋅o =0 （2.9）
n =o =a =1 （2.10）
1.2齐次变换
变换为空间的一个运动，由旋转和平移组成。当空间的一个坐标系相对于固定的参考坐
标系运动时，这一运动可以用类似于表示坐标系的方式来表示。变换本身就是坐标系位姿的变换，分为以下几种形式：
1.2.1平移的齐次变换
一个坐标系（或刚体）在空间中以不变的姿态运动，该变换为平移变换。坐标系的单位向量保持同一方向不变，所有改变只是坐标系原点相对于参考坐标系的变化，如图2-2所示。相对于固定参考系的新的坐标系的位置可以用原来坐标系的原点位置向量加上表示位移的向量求得。变换矩阵为：
⎡1
⎢0T =⎢
⎢0⎢⎣0
0100
0d x ⎤0d y ⎥⎥ （2.9） 1d z ⎥
⎥01⎦
其中d x ，d y 和d z 是纯平移向量d 相对于参考坐标系x ，y 和z 轴的三个向量，矩阵T 的前三列等同于单位矩阵，表示没有旋转运动，最后一列表示平移运动。若坐标系平移前的位姿矩阵为：
F old
⎡nx ox ax P x ⎤
⎢ny oy ay P ⎥
y ⎥ （2.9） =⎢
⎢nz oz az P z ⎥⎢⎥0001⎣⎦
则，坐标系平移后的位姿矩阵为：
F new
⎡1⎢0=⎢⎢0⎢⎣0
0100
0d x ⎤⎡nx ox ax P x +d x ⎤
⎢ny oy ay P +d ⎥0d y ⎥y y ⎥⎥⨯F old =⎢ （2.9） ⎢nz oz az P z +d z ⎥1d z ⎥
⎥⎢⎥01⎦0001⎣⎦
也可表示为：
F new =Trans (d x , d y , d z ) ⨯F old （2.9）
对比式子2.9和2.9，坐标系平移变换前后的位姿矩阵维数相同，方向向量保持不变，
仅是位置向量发生变换，即为d 和P 相加的结果。
1.2.2旋转的齐次变换
空间某点P ，坐标为(P x , P y , P z ) ，当它绕x 轴旋转角后至P '点，坐标为(P x ', P y ', P z ') ，
P 点和P '点的坐标关系为：
⎧P x '=P x
⎪
⎨P y =P y cos θ-P z sin θ （2.9） ⎪P =P sin θ+P cos θ
y z ⎩z
写成矩阵形式为：
⎡P x '⎤⎡1
⎢⎥⎢ ⎢P y '⎥=⎢0
⎢⎣P z '⎥⎦⎢⎣0
也可表示为：
0c o θs
s i n θ
⎤⎡P x ⎤
⎢P ⎥-s i n θ⎥⎥⎢y ⎥ （2.9）
c o θs ⎥⎦⎢⎣P z ⎥⎦
P '=Rot (x , θ) ⨯P （2.9）
Rot (x , θ) 表示齐次坐标变换时绕x 轴的旋转变换矩阵，第一列表示相对于x 轴的位置，
其值为1，0，0，它表示沿着x 轴的坐标没有改变。习惯上采用符号C θ表示cos θ，S θ表示sin θ，因此旋转矩阵表示为：
⎡10
Rot (x , θ) =⎢⎢0C θ
⎢⎣0S θ
将上式矩阵写成4⨯4的齐次变换矩阵为：
0⎤
-S θ⎥⎥ （2.9） C θ⎥⎦
⎡10⎢0C θ
Rot (x , θ) =⎢
⎢0S θ⎢
⎣00
同理，绕y 轴和z 轴的旋转矩阵分别为：
0-S θC θ0
0⎤0⎥⎥ （2.9） 0⎥⎥1⎦
⎡C θ⎢0
Rot (y , θ) =⎢
⎢-S θ⎢⎣0
1.2.3复合变换
0S θ100C θ00
0⎤⎡C θ
⎢S θ0⎥⎥ Rot (z , θ) =⎢⎢00⎥
⎥⎢1⎦⎣0
-S θ
C θ000010
0⎤0⎥⎥ （2.9） 0⎥⎥1⎦
复合变换是由固定参考坐标系或当前运动坐标系的一系列沿轴平移和绕轴旋转变换所
组成的。任何变换都可以分解为按一定顺序的一组平移和旋转变换，将两种变换组合在一个齐次变换中，称为复合变换。在计算变换矩阵时，要考虑算子左、右乘规则：如果相对于固定坐标系进行变换，则算子左乘；如果相对于动坐标系进行变换，则算子右乘。
动坐标系(, , )相对于参考坐标系(x , y , z )依次进行下面三种变换：（1）绕x 轴旋转α
度；（2）接着沿x 轴平移l 1，沿z 轴平移l 3；（3）最后，绕y 轴旋转β度。
点P 固连在动坐标系上，动坐标系与固定坐标系重合，随着动坐标系(, , )相对于固
定坐标系旋转或平移时，坐标系中的P 点相对于固定坐标系也跟着改变，第一步变换后，P 点相对于固定坐标系的坐标为：
P 1, xyz =Rot (x , α) ⨯P ； （2.9）
第二步变换后，P 点相对于固定坐标系的坐标为：
P 2, xyz =Trans (l 1, 0, l 3) ⨯P (l 1, 0, l 3) ⨯Rot (x , α) ⨯P ； （2.9） 1, xyz =Trans
第三步变换后，P 点相对于固定坐标系的坐标为：
P (l 1, 0, l 3) ⨯Rot (x , α) ⨯P ； （2.9） 3, xyz =Rot (y , β) ⨯P 2, xyz =Rot (y , β) ⨯Trans
每一步变换都是用变换矩阵左乘P 点的坐标得到该点相对于固定坐标系的坐标，矩阵
的书写顺序和进行变换的顺序正好相反，变换的顺序很重要，如果颠倒了变换的顺序，结果将完全不同。
1.2.4相对于动坐标系的变换
上述介绍的变换是相对于固定坐标系进行的，所有的平移距离和旋转角度都是相对于参
考坐标系轴来测量的。在实际应用中，也有可能相对于动坐标系或当前坐标系的轴变换。点
P 固连在动坐标系(, , )上，依次进行下面三种变换：（1）绕轴旋转α度；（2）接着
沿轴平移l 1，沿轴平移l 3；（3）最后，绕轴旋转β度。
为了计算当前坐标系中点的坐标相对于参考坐标系的变化，这时需要右乘变换矩阵而不
是左乘。第一步变换后，P 点相对于固定坐标系的坐标为：
P 1, xyz =Rot (, α) ⨯P ； 第二步变换后，P 点相对于固定坐标系的坐标为：
P 2, xyz =Rot (, α) ⨯Trans (l 1, 0, l 3) ⨯P ； 第三步变换后，P 点相对于固定坐标系的坐标为：
P 3, xyz =Rot (, α) ⨯Trans (l 1, 0, l 3) ⨯Rot (, β) ⨯P ； 1.3机器人位姿分析 1.3.1杆件坐标系的建立
2.9）
2.9）
2.9）
（ （ （