机器人运动学及其数学基础

• 具有直观的几何意义 • 能表达动力学、计算机视觉和
比例变换问题 • 为以后的比例变换、透视变换
等打下基础
T
=
µ µ
x y
µ
z
θx θy θz
wx wy wz
p
x
py
p
z
0 0 0 1
【2】
2.1 点和面的齐次坐标 2.1.1 点的齐次坐标
• 一般来说，n维空间的齐次坐标表示是一个（n+1）维空间实体。有一个 特定的投影附加于n维空间，也可以把它看作一个附加于每个矢量的特定 坐标—比例系数。
1 0 0 R = 0 1 0
0 0 1
【11】
ix
iµ
ix jv
ix
kw
R(x,α )
=
jy
iµ
jy jv
jy
k
w
W'
kz iµ kz jv kz kw
α
1 0
0
ix = iu 0
cosα
−
sin
α
U'
u
0 sinα cosα x
z w
O' α
o
图2-5
V'
vy
方向余弦阵
【12】
三个基本旋转矩阵:
有： PV=
= 0 < 0
v点在平面上 v点在平面下方
例如：点 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
【13】
合成旋转矩阵:
例1：在动坐标中有一固定点 Po'uvw = [1 2 3 1]T，相对固定参
考坐标系 ∑ Oxyz 做如下运动：① R（x, 90°）；② R(z, 90°)；③ R(y,90°)。求运动后点 Po'uvw 在固定参考坐标系 ∑ Oxyz 下的位置。 解1：用画图的简单方法
z
z
w
w′
v u o(o′) y
x
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,
90 o
)
R(Z,90o
)
=
1 0
0 1
0 0
− 3 7
o′ w```
u```
o
y
0 0 0 1
x
（2-21）
【20】
式（2-20）和式（2-21）无论在形式上，还是在结果上都是 一致的。因此我们有如下的结论：
【16】
平移齐次变换矩阵
1 0 0 a
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
【17】
2.2.4 相对变换
举例说明： 例1：动坐标系∑0′起始位置与固定参考坐标系∑0重合,动坐标系 ∑0′做如下运动：①R(Z,90º) ②R（y,90º） ③Trans(4，-3, 7)，
用矩阵表示为:
Px Py Pz
=
ix
j
y
kz
iµ iµ iµ
ix jv jy jv kz jv
Pu
u
图2-4
ix jy kz
kw kw kw
Pµ Pv Pw
（2-7）
【9】
ix
iµ
定义
旋转矩阵为：R
=
jy
iµ
kz iµ
ix jv jy jv kz jv
ix
kw
求合成矩阵
解1：用画图的方法：
z
z
z
v```
7
z
o′
w
u```
w′ v′
v″
w```
o(o′) v y
u x
o(o′)
x
u′ y
o x w″
u″ y
-3 oy
4 x
【18】
解2：用计算的方法
根据定义1，我们有： T = Trans(4, - 3, 7) R(y, 90o ) R(Z,90o )
0 0 1 4
1
0
[0 0 1 -1]00 = -1 < 0
1
【7】
2.2 旋转矩阵及旋转齐次变换 2.2.1 旋转矩阵
设固定参考坐标系直角坐标为ΣOxyz，动坐标系为ΣO´uvw， 研究旋转变换情况。
① 初始位置时，动静坐标系重合，O、O´ 重合，如图。各轴对
应重合，设P点是动坐标系ΣO´uvw中的一点，且固定不变。则P
jy
kw
kz kw
则 : pxyz = RPuvw
反过来： 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
P
u
Py
Pz
=
x
PV = [a
b
c
d
]
y z
=
ax
+
by
+
cz
+
dw
=
0
w
与点矢 [0 0 0 0]T 相仿，平面 [0 0 0 0] 也没有意义
【6】
点和平面间的位置关系
设一个平行于x、y轴，且在z轴上的坐标为单位距离的平面 P可以表示为：P = [0 0 1 −1] 或 P = [0 0 2 − 2]
> 0 v点在平面上方
w
作为通用比例因子，它可取任意正值，但
在机器人的运动分析中，总是取w=1 。 【3】
齐次坐标与三维直角坐标的区别
• V点在ΣOXYZ坐标系中表示是唯一的（a、 b、c）
• 而在齐次坐标中表示可以是多值的。不 同的表示方法代表的V点在空间位置上
不变。
z
z
V
o x
z y
x
【4】
几个特定意义的齐次坐标：
1
R(x,α ) = 0
0
0
cosα sin α
0
−
sin
α
cosα
同理：
cosφ 0 sinφ
R（y,φ
)
=
0
1
0
− sinφ 0 cosφ
cosθ - sinθ 0
R（z,θ ) = sinθ cosθ 0
0
0 1
z
W'
w
Φ
o O'
u x
U'
z w
W'
o
θ
O'
u
x
U'
vy
v' vy
• [0 0 0 n]T—坐标原点矢量的齐次坐标，n为任意非零 比例系数
• [1 0 0 0]T — 指向无穷远处的OX轴 • [0 1 0 0]T — 指向无穷远处的OY轴 • [0 0 1 0]T — 指向无穷远处的OZ轴 • [0 0 0 0]T — 没有意义
2个常用的公式：
点乘: a ⋅ b = axbx + ayby + azbz
也就是说，动坐标系绕自身坐标轴做齐次变换，要达到绕固 定坐标系相等的结果，就应该用相反的顺序。
【21】
右乘的意义：
• 机器人用到相对变换的时 z 候比较多
• 例如机械手抓一个杯子，
如右图所示，手爪需要转
动一个角度才抓的牢，相
对于固定坐标系表达太麻
烦，可以直接根据手爪的
y
坐标系表示 • 但也要知道在∑O中的位姿， o
n
o
a
ቤተ መጻሕፍቲ ባይዱ
固定参考坐标数的空间几何描述，
θi
也就是机器人的运动学问题
• 机器人的运动学即是研究机器人
手臂末端执行器位置和姿态与关
节变量空间之间的关系
【1】
1955年丹纳维特（Denavit）和哈顿伯格（Hartenberg）提出了 一种采用矩阵代数方法解决机器人的运动学问题— D-H方法，
其数学基础即是齐次变换
动坐标系在固定坐标系中的齐次变换有2种情况： 定义1：如果所有的变换都是相对于固定坐标系中各坐标轴旋 转或平移，则依次左乘，称为绝对变换。 定义2：如果动坐标系相对于自身坐标系的当前坐标轴旋转或
平移，则齐次变换为依次右乘，称为相对变换。
结果均为动坐标系在固定坐标中的位姿（位置+姿态）。相 对于固定坐标系，µ轴相当于X轴，v轴相对于Y轴，w轴相当于Z轴。
i jk
v
v
v
叉乘: a ×b = ax a y az = (aybz − azby )i + (azbx − axbz ) j + (axby − aybx )k
bx by bz
【5】
2.1.2 平面的齐次坐标
• 平面齐次坐标由行矩阵P=[a b c d ]来表示 • 当点v=[x y z w]T处于平面P内时，矩阵乘积PV=0，或记为
• 串联机器人可以用一个开环关节链来建模
• 由数个驱动器驱动的转动或移动关节串联而成
• 一端固定在基座上，另一端是自由的，安装工具（末端
执行器），用以操纵物体，或完成各种任务 • 关节的相对运动导致杆件的运动，
使末端执行器定位于所需要的方
位上 • 在一般机器人应用问题中，人们
感兴趣的是：末端执行器相对于
【14】
解2：用分步计算的方法
① R（x, 90°）
1 0 0 01 1
P'
=
0 0
0 1
-1 0
0 0
2 3
=
− 3 2
0 0 0 11 1
② R（z, 90°）
0 -1 0 0 1 3
P ''
=
1 0
0 0
0 1
0− 3 0 2
=
1 2
0 0 0 1 1 1
③ R（y, 90°）
点在ΣO´uvw中可表示为：
z
Puvw = Pu iu + Pv jv + Pw kw
iu 、jv 、kw 为坐标系ΣO´uvw的单位矢 量，则P点在Σoxyz中可表示为：
Pxyz = Px ix + Py jy + Pz kz Puvw = Pxyz
w
P
o
v
(O')
u
x
y
【8】
② 当动坐标系ΣO´uvw绕O点回转时，求P点在固定坐标系Σoxyz
中的位置 Puvw = Pu iu + Pv jv + Pw kw
w
z
已知：
P点在ΣO´uvw中是不变的仍然成
Pw
P
立，由于ΣO´uvw回转，则：
v
Px = Puvw ix = (Pu iu + Pv jv + Pw kw)ix
o
Pv
y
(O')
Py = Puvw jy = (Pu iu + Pv jv + Pw kw ) jy x Pz = Puvw kz = (Pu iu + Pv jv + Pw kw )kz
• 引入齐次坐标的目的是为了表示几何变换的旋转、平移和缩放
一个点矢：
v v
=
v ai
+
v bj
+
v ck
列矩阵
式中i, j, k为x, y, z 轴上的单位矢量，
a= x , b=y , c=z ，w为比例系数 w ww
x
V
=
y z
= [x
y
z
w]T
显然，齐次坐标表达并不是唯一的，随 w值的不同而不同。在计算机图学中，w
R
0 Pv
0
Pw
1 0 0 0 1 1
Pu Pv
Pw
=
R −1
0 0
Px Py
0
Pz
1 0 0 0 1 1
【10】
2.2.3 三个基本旋转矩阵和合成旋转矩阵
三个基本旋转矩阵
R(x,α )
即动坐标系 ∑ O，µvw绕OX轴转动α角，求 R(x,α ) 的旋转矩阵，也就是 求出坐标系 ∑ O'µvw中各轴单位矢量 iµ , jv , k w在固定坐标系 ∑ Oxyz 中各轴的投影分量，很容易得到在两个坐标系重合时，有：
Y
使r 轴处于XZ平面内
ry
2. 绕Y 轴转-β角，使r 轴与OZ轴重合
3. 绕OZ轴转动φ角 4. 绕Y 轴转β角 5. 绕X 轴转-α角
A
B
r
O
1
rx
α
5C
β
rz
2
D
3 A'
4
B'
Z
X
【23】
由定义1和定义2，上述5次旋转的合成旋转矩阵为：
R r,φ = R x,−α R y,β R z,φ R y,−β R x,α
A
（2-25）
B
r
由上图容易求出：
O
1
r x
=
R3×3
Pu Pv
Pw
1 0 0 0 1 1
R3x3为二者之间的关系矩阵，我们令：
R3×3 = R（y,θ )R(z,φ)R(x,α)
定义1：
当动坐标系 ∑ O'uvw
∑
绕固定坐标系
Oxyz
各坐标轴顺序有限次
转动时，其合成旋转矩阵为各基本旋转矩阵依旋转顺序左乘。
注意：旋转矩阵间不可以交换
就用右乘的概念。
x
H
【22】
2.2.5 绕通过原点的任意轴旋转的齐次变换
• 有时动坐标系∑O´可能绕过原点O的分量分别为rx、ry、rz的任 意单位矢量r 转动φ角。
• 研究这种转动的好处是可用∑O´绕某轴r 的一次转动代替绕∑O
各坐标轴的数次转动
• 为推导此旋转矩阵，可作下述5步变换：
1. 绕X 轴转α角，
=
1 0
0 1
0 0
− 3
7
0 0 0 1
（2-20）
以上均以固定坐标系多轴为变换基准，因此矩阵左乘。 如果我们做如下变换，也可以得到相同的结果：
例2：①先平移Trans (4,-3,7)；②绕当前 v′ 轴转动90º；
③绕当前 w′′ 轴转动90º；求合成旋转矩阵。
【19】
解1：用画图的方法
1 0
0 cosβ 0 sinβ cosφ - sinφ 0
= 0
cosα
sinα
0
1
0
sinφ
cosφ
0
0 − sinα cosα - sinβ 0 cosβ 0
0 1
Y
cosβ 0 - sinβ 1 0
0
0
1
0
0
cosα
-
sinα
ry
sinβ 0 cosβ 0 sinα cosα
0 0 1 03 2
P '''
=
0
-1
1 0
0 0
0 0
1 2
=
1
− 3
0 0 0 11 1
（2-14）
（2-15） （2-16）
【15】
上述计算方法非常繁琐，可以通过一系列计算得到上述 结果。将式（2-14）（2-15）（2-16）联写为如下形式：
Px Py
Pz