第2章 机器人运动学—数学基础[可打印版,含习题]

机器人运动学
第二章 数学基础—齐次坐标和齐次变换
杜志江、纪军红 • 科学园C1栋机器人研究所
206室 • 86414462-12 • duzj01@hit.edu.cn • junhong.ji@hit.edu.cn
参考教材 • 付京逊《机器人学》 • 蔡自兴《机器人学》
2.1 引言
机器人位置和姿态的描述
i jk
K
K
K
a ×b = ax a y az = (aybz − azby )i + (azbx − axbz ) j + (axby − aybx )k
bx by bz
2.2.2 平面的齐次坐标
• 平面齐次坐标由行矩阵P=[a b c d ]来表示 • 当点v=[x y z w]T处于平面P内时，矩阵乘积PV=O，或记为
H
=
Trans
(a
b
c)
=
⎢⎢0 ⎢0
1 0
0 1
b⎥⎥ c⎥
⎢⎣0 0 0 1⎥⎦
w′
o′ v′
u′
b
a
注意：平移矩阵间可以交换，
x
平移和旋转矩阵间不可以交换
z c
oy
2.2.4 相对变换
举例说明：
例1：动坐标系∑0′起始位置与固定参考坐标系∑0重合,动坐标系 ∑0′做如下运动：①R(Z,90º) ②R（y,90º） ③Trans(4，-3, 7) ，求合成矩阵
Pu
u
图
ix jy kz
kw kw kw
⎤ ⎥ ⎥ ⎥ ⎥⎦
⎡ ⎢ ⎢ ⎣⎢
Pμ Pv Pw
⎤ ⎥ ⎥ ⎥⎦
（2-7）
⎡ ⎢
ix
iμ
定义 旋转矩阵为：R = ⎢ jy iμ
⎢⎢⎣kz iμ
ix jv jy jv kz jv
ix
kw
⎤ ⎥
jy kw ⎥
kz
kw
⎥ ⎥⎦
则 : pxyz = RPuvw
对应重合，设P点是动坐标系ΣO´uvw中的一点，且固定不变。
则P点在ΣO´uvw中可表示为：
z
Puvw = Pu iu + Pv ju + Pw kw
iu 、jv 、kw 为坐标系ΣO´uvw的单位矢
w
P
量，则P点在Σoxyz中可表示为：
o
v
(O')
y
Pxyz = Px ix + Py iy + Pz iz Puvw = Pxyz
K i+
b
K j+
c
K k)
解释为某个平面的外法线，此
mm m
平面沿着法线方向与坐标原点的距离为。− d
mቤተ መጻሕፍቲ ባይዱ
[例]：
因此一个平行于x、y轴，且在z轴上的坐标为单位距离的 平面P可以表示为：P = [0 0 1 −1] 或 P = [0 0 2 − 2]
有： PV=
⎪⎨⎧>=
0 0
⎪⎩< 0
v点在平面上方 v点在平面上 v点在平面下方
z
⎡ ⎢
ix
iμ
R(x,α ) = ⎢ jy iμ
⎢ ⎣⎢
k
z
iμ
ix jv jy jv kz jv
ix
kw
⎤ ⎥
jy kw ⎥
kz
k
w
⎥ ⎥⎦
⎡1 ix = iu ⎢⎢0
⎢⎣0
0
cosα sin α
0⎤
−
sin
α
⎥ ⎥
comα ⎥⎦
W'
α
U'
u x
w
O' α
o
图2-5
V'
vy
方向余弦阵
三个基本旋转矩阵:
反过来： Puvw = R −1 Pxyz
R−1 = R* det R
R∗为R的伴随矩阵，det R为R的行列式，由于R是正交矩阵，
因此R −1 = R T
2.2.2 旋转齐次变换
用齐次坐标变换来表示式（2-7）
⎡Px ⎤ ⎡
0⎤⎡Pu ⎤
⎢⎢Py
⎥ ⎥
=
⎢ ⎢
R
0⎥⎥⎢⎢
Pv
⎥ ⎥
⎢ ⎢ ⎣
Pz 1
P'' = ⎢⎢1 0 0 0⎥⎥⎢⎢− 3⎥⎥ = ⎢⎢1⎥⎥ ⎢0 0 1 0⎥⎢ 2 ⎥ ⎢2⎥
⎢⎣0
0
0
1⎥⎦
⎢ ⎣
1
⎥ ⎦
⎢⎣1⎥⎦
⎡ 0 0 1 0⎤⎡3⎤ ⎡ 2 ⎤
P '''
=
⎢ ⎢
0
1
0
0⎥⎥ ⎢⎢1⎥⎥
=
⎢ ⎢
1
⎥ ⎥
⎢-1 0 0 0⎥⎢2⎥ ⎢− 3⎥
⎢ ⎣
0
0
0
1⎥⎦ ⎢⎣1⎥⎦
u
x
② 当动坐标系ΣO´uvw绕O点回转时，求P点在固定坐标系
Σoxyz中的位置
z
w
已知： Puvw = Pu iu + Pv ju + Pw kw
P点在ΣO´uvw中是不变的仍然
Pw P
成立，由于ΣO´uvw回转，则：
v
Px = Puvw ix = ix (Pu iu + Pv jv + Pw kw )
⎦⎣ 1 ⎦
R4x4为二者之间的关系矩阵，我们令：
R4×4 = R（y,θ )R(z,φ)R(x,α)
定义1： 当动坐标系 ∑ O'uvw 绕固定坐标系 ∑ Oxyz 各坐标轴顺序有限次
转动时，其合成旋转矩阵为各基本旋转矩阵依旋转顺序左乘。
注意：旋转矩阵间不可以交换
平移齐次变换矩阵
⎡1 0 0 a⎤
z
z
V
o x
z y
x
几个特定意义的齐次坐标：
• [0, 0, 0, n]T—坐标原点矢量的齐次坐标，n为 任意非零比例系数
• [1 0 0 0]T—指向无穷远处的OX轴 • [0 1 0 0]T—指向无穷远处的OY轴
• [0 0 1 0]T—指向无穷远处的OZ轴
2个常用的公式：
a ⋅b = axbx + ayby + azbz
解2：用分步计算的方法
① R（x, 90°）
⎡1 0 0 0⎤⎡1⎤ ⎡ 1 ⎤
P' = ⎢⎢0 0 -1 0⎥⎥⎢⎢2⎥⎥ = ⎢⎢− 3⎥⎥ ⎢0 1 0 0⎥⎢3⎥ ⎢ 2 ⎥
⎢⎣0
0
0
1⎥⎦⎣⎢1⎥⎦
⎢ ⎣
1
⎥ ⎦
② R（z, 90°） ③ R（y, 90°）
⎡0 -1 0 0⎤⎡ 1 ⎤ ⎡3⎤
节变量空间之间的关系
动画示例
运动学研究的问题
Where is my hand?
Direct Kinematics HERE!
运动学正问题
运动学逆问题
How do I put my hand here?
Inverse Kinematics: Choose these angles!
• 丹纳维特（Denavit）和哈顿贝格（Hartenberg） 于1955年提出了一种矩阵代数方法解决机器人的 运动学问题—D-H方法
解1：用画图的方法：
z
z
z
w
w′
v′
v″
z
v```
7
o′ u```
w```
o(o′) v y
u x
o(o′) u′ y
o
x
x w″
u″ y
-3 oy
4 x
解2：用计算的方法
根据定义1，我们有： T = Trans(4, 3, 7) R(y, 90D ) R(Z,90D )
⎡0 0 1 4 ⎤
=
⎢⎢1 ⎢0
0 1
0 0
− 3⎥⎥ 7⎥
⎢⎣0 0 0
1
⎥ ⎦
（2-20）
以上均以固定坐标系多轴为变换基准，因此矩阵左乘。
如果我们做如下变换，也可以得到相同的结果：
例2：①先平移Trans (4,-3,7)；②绕当前 v′轴转动90º；
③绕当前 w′′ 轴转动90º；求合成旋转矩阵。
解1：用画图的方法
z w
v u o(o′) y
• 机器人可以用一个开环关节链来建模
• 由数个驱动器驱动的转动或移动关节串联而成
• 一端固定在基座上，另一端是自由的，安装工具，用以 操纵物体
• 人们感兴趣的是操作机末端执行
n
器相对于固定参考坐标数的空间 几何描述，也就是机器人的运动 学问题
• 机器人的运动学即是研究机器人
o
a
θi
手臂末端执行器位置和姿态与关
即动坐标系 ∑ O，μvw绕OX轴转动α角，求 R(x,α ) 的旋转矩阵，也就是 求出坐标系 ∑ O'μvw中各轴单位矢量 iμ , jv , k w 在固定坐标系 ∑ Oxyz 中各轴的投影分量，很容易得到在重合时，有：
⎡1 0 0⎤ R = ⎢⎢0 1 0⎥⎥
⎣⎢0 0 1⎥⎦
由图2-5可知， jv 在y轴上的投影为 jy cos α ， jv 在z轴上的投影 为 kz sinα , kw 在y轴上的投影为 − jy sinα ， kw 在z轴上的投影为 kz cosα ，所以有：
o
Pv
y
(O')
Py = Puvw jy = jy (Pu iu + Pv jv + Pw kw ) x Pz = Puvw jz = jz (Pu iu + Pv jv + Pw kw )
用矩阵表示为:
⎡ Px
⎢ ⎢
Py
⎢⎣ Pz
⎤ ⎥ ⎥ ⎥⎦
=
⎡ ⎢
ix
⎢ jy
⎢⎣⎢kz
iμ iμ iμ
ix jv jy jv kz jv
[0 0 1 -1]⎢⎢⎢00⎥⎥⎥ = -1 < 0
⎢⎣1⎥⎦
与点矢 [0 0 0 0]T 相仿，平面 [0 0 0 0] 也没有意义
2.2 旋转矩阵及旋转齐次变换
2.2.1 旋转矩阵
设固定参考坐标系直角坐标为ΣOxyz，动坐标系为ΣO´uvw， 研究旋转变换情况。
① 初始位置时，动静坐标系重合，O、O´ 重合，如图。各轴
x
z
w′ o′ v′ u′
o
x
y w″
z
o′
v″
u″
o
y
x
解2：用计算的方法
z
v```
⎡0 0 1 4 ⎤ T = Trans(4, 3, 7) R(y, 90o ) R(Z,90o ) = ⎢⎢1 0 0 − 3⎥⎥
⎢0 1 0 7 ⎥
o′ u``` w```
o
y
⎢⎣0 0 0
1
⎥ ⎦
x
（2-21）
同理：
⎡1
R(x,α ) = ⎢⎢0
⎢⎣0
0
cosα sin α
0⎤
−
sin
α
⎥ ⎥
comα ⎥⎦
⎡ cosφ 0 sinφ ⎤
R（y,φ
)
=
⎢ ⎢
0
1
0
⎥ ⎥
⎢⎣− sin φ 0 cosφ ⎥⎦
⎡cosθ - sinθ 0⎤
R（z,θ ) = ⎢⎢sinθ cosθ 0⎥⎥
⎢⎣ 0
0 1⎥⎦
z
W'
⎢ ⎣
1
⎥ ⎦
（2-14） （2-15） （2-16）
上述计算方法非常繁琐，可以通过一系列计算得到上述 结果。将式（2-14）（2-15）（2-16）联写为如下形式：
⎡Px ⎤ ⎡
⎢ ⎢
Py
⎥ ⎥
=
⎢ ⎢
R4×4
⎢ ⎢
Pz
⎥ ⎥
⎢ ⎢
⎣1⎦ ⎣
⎤⎡ Pu ⎤
⎥ ⎥
⎢ ⎢
Pv
⎥ ⎥
⎥ ⎥
⎢⎢Pw
⎥ ⎥
• 具有直观的几何意义
• 能表达动力学、计算机视觉和比例变换问题
• 其数学基础即是齐次变换
2.2 点和面的齐次坐标
2.2.1 点的齐次坐标
• 一般来说，n维空间的齐次坐标表示是一个（n+1）维空间 实体。有一个特定的投影附加于n维空间，也可以把它看作 一个附加于每个矢量的特定坐标—比例系数。
vK
=
⎥ ⎥ ⎦
⎢ ⎢⎣0
0
0
0⎥ 1⎥⎦
⎢ ⎢ ⎣
Pw 1
⎥ ⎥ ⎦
⎡Pu ⎤ ⎡
0⎤⎡Px ⎤
⎢ ⎢
Pv
⎥ ⎥
=
⎢ ⎢
R −1
0⎥⎥
⎢ ⎢
Py
⎥ ⎥
⎢ ⎢ ⎣
Pw 1
⎥ ⎥ ⎦
⎢ ⎢⎣0
0
0
0⎥ 1⎥⎦
⎢ ⎢ ⎣
Pz 1
⎥ ⎥ ⎦
2.2.3 三个基本旋转矩阵和合成旋转矩阵
三个基本旋转矩阵
R(x,α )
⎡x ⎤
PV = [a
b
c
d
]⎢⎢zy
⎥ ⎥
=
ax
+
by
+
cz
+
dw
=
0
⎢⎣w⎥⎦
如果定义一个常数m= a2 + b2 + c2 ，则有：
x a + y b + z c =−d wm wm wm m
=
(x
K i+
y
K j+
z
K k)
⋅
(
a
K i+
b
K j+
c
K k)
ww w mm m
可以把矢量 ( a
在机器人的运动分析中，总是取w=1 。
[例]：
KKKK V = 3i + 4 j + 5k
可以表示为： V=[3 4 5 1]T
或 V=[6 8 10 2]T 或 V=[-12 -16 -20 -4]T
齐次坐标与三维直角坐标的区别
• V点在ΣOXYZ坐标系中表示 是唯一的（x、y、z）
• 而在齐次坐标中表示可 以是多值的。不同的表 示方法代表的V点在空间 位置上不变。
K ai
+
K bj
+
K ck
式中i, j, k为x, y, z 轴上的单位矢量，
列矩阵 ⎡x ⎤
a= x
y
, b=
z , c=
，w为比例系数
w ww
V
=
⎢y ⎢z
⎥ ⎥
= [x
y
z
w]T
显然，齐次坐标表达并不是唯一的，随
⎣⎢w⎥⎦
w值的不同而不同。在计算机图学中，w
作为通用比例因子，它可取任意正值，但
w
Φ
o O'
u x
U'
z w
W'
o
θ
O'
u
x
U'
vy
v' vy
合成旋转矩阵:
例1：在动坐标中有一固定点 Po'uvw = [1 2 3 1]T ，相对固定参
考坐标系 ∑ Oxyz 做如下运动：① R（x, 90°）；② R(z, 90°)；③ R(y,90°)。求点 Po'uvw 在固定参考坐标系 ∑ Oxyz 下 的位置。 解1：用画图的简单方法
例如：点 V=[10 20 1 1]T 必定处于此平面内，而点 V=[0 0 2 1]T 处于平 P 的上方点V=[0 0 0 1]T处于P平面下方。因为：
⎡10⎤
[0 0 −10 10]⎢⎢⎢210⎥⎥⎥ = 0
⎢ ⎣
1
⎥ ⎦
⎡0⎤
[0 0 1 −1]⎢⎢⎢02⎥⎥⎥ = 1 > 0
⎢⎣1⎥⎦
⎡0⎤
式（2-20）和式（2-21）无论在形式上，还是在结果上都是 一致的。因此我们有如下的结论：
动坐标系在固定坐标系中的齐次变换有2种情况：
定义1：如果所有的变换都是相对于固定坐标系中各坐标轴旋 转或平移，则依次左乘，称为绝对变换。