机器人学第二章(数学基础)

数学基础
2.2.1 点向量（Point vectors）
点向量描述空间的一个点在某个坐标系的空 间位置。同一个点在不同坐标系的描述及位置向 量的值也不同。如图2.1中，点p在E坐标系上表示 为 Ev，在H坐标系上表示为 Hu，且v ≠ u。一个点
z
p
c
•
E
v
0
x by
向量可表示为
a
u
H y
v = ai + bj + ck
x
通常用一个（n + 1）维列矩阵表示，即除 x、
y、z 三个方向上的分量外，再加一个比例因子 w ，
z
0
图2.1 点向量的描述
即
v = [ x y z w ]T
其中 a = x/w, b = y/w, c = z/w。
已知两个向量
a = ax i + ay j + az k b = bx i + by j + bz k
v' vy
• 丹纳维特（Denavit）和哈顿贝格（Hartenberg） 于1955年提出了一种矩阵代数方法解决机器人 的运动学问题—D-H方法
• 具有直观的几何意义
• 能表达动力学、计算机视觉和比例变换问题
• 其数学基础即是齐次变换
2.2 点齐次坐标
2.2.1 点的齐次坐标
• 一般来说，n维空间的齐次坐标表示是一个（n+1）维空间 实体。有一个特定的投影附加于n维空间，也可以把它看作 一个附加于每个矢量的特定坐标—比例系数。
v
ai
bj
ck
式中i, j, k为x, y, z 轴上的单位矢量，
列矩阵 x
a= x
y
, b=
z , c=
，w为比例系数
w ww
V
y z
x
y
z
wT
显然，齐次坐标表达并不是唯一的，随
w
w值的不同而不同。在计算机图学中，w
作为通用比例因子，它可取任意正值，但
在机器人的运动分析中，总是取w=1 。
用矩阵表示为:
Px
Py
Pz
ix
jy
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）
ix
i
定义 旋转矩阵为：R jy i
kz i
ix jv jy jv kz jv
ix
kw
jy kw
kz
c1 1
x x wx px
则：T
Baidu Nhomakorabea
y
y
wy
p
y
z
0
z
0
wz 0
pz 1
T反映了∑O´在∑O中的位置和姿态，即表示了该坐标系原点
和各坐标轴单位矢量在固定坐标系中的位置和姿态。 该矩阵可以由4个子矩阵组成，写成如下形式：
T
R33
f13
P31 w11
旋转矩阵 透视矩阵
位置矢量 比例系数
x/w+a
y + bw
y/w+b
z + cw
z/w+c
w
1
注意：平移矩阵间可以交换， 平移和旋转矩阵间不可以交换
2.2.4 相对变换
举例说明：
例1：动坐标系∑0′起始位置与固定参考坐标系∑0重合,动坐标系 ∑0′做如下运动：①R(Z,90º) ②R（y,90º） ③Trans(4，-3, 7) ，求合成矩阵
[例]：
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）
z
• 而在齐次坐标中表示可 以是多值的。不同的表 示方法代表的V点在空间 位置上不变。
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
u
x
② 当动坐标系ΣO´uvw绕O点回转时，求P点在固定坐标系
Σoxyz中的位置
上述计算方法非常繁琐，可以通过一系列计算得到上述 结果。将式（2-14）（2-15）（2-16）联写为如下形式：
Px
Py
R44
Pz 1
Pu
Pv
Pw 1
R4x4为二者之间的关系矩阵，我们令：
R44 R（y, )R(z,)R(x, )
定义1： 当动坐标系 O'uvw 绕固定坐标系 Oxyz 各坐标轴顺序有限次
②绕X轴转动60º；③绕Y轴转动90º。求T。
cos30 sin 30 0 0
R1
s
in
30
0
cos30 0
0 0 1 0
0
0 0 1
1 0
0 0
R2
0 0
c os60 sin 60
为 kz sin , kw 在y轴上的投影为 jy sin ， kw 在z轴上的投影为
kz cos ，所以有：
z
ix
i
ix jv
ix
kw
W'
R(x, ) jy i jy jv jy kw
w
k
z
i
kz jv
kz
k
w
V'
1 ix iu 0
0
0
cos s in
0
sin
cos
U'
u
x
也就是说，动坐标系绕自身坐标轴做齐次变换，要达到绕固 定坐标系相等的结果，就应该用相反的顺序。
• 机器人用到相对变换的
时候比较多
z
• 例如机械手抓一个杯子，
如右图所示，手爪需要
H
转动一个角度才抓的牢，
相对于固定坐标系表达
太麻烦，可以直接根据
手爪的坐标系表示
y
• 但也要知道在∑O中的位
o
姿，就用右乘的概念。
转动时，其合成旋转矩阵为各基本旋转矩阵依旋转顺序左乘。
注意：旋转矩阵间不可以交换
z
平移齐次变换矩阵
c
1 0 0 a H Trans(a b c) 0 1 0 b
0 0 1 c 0 0 0 1
w′
o′ v′
u′
b
a x
oy
因此对向量 u = [ x y z w ]T，经H变换为向量v可表示为
x + aw
x
2.2.6 齐次交换矩阵的几何意义
x x wx t1
设T= y y wy t2 ，有一个手爪，已知其在∑O的位置，设一个
z z wz t3
0
0
0
1
该坐标系∑O´，已知， o' a1 b1 c1 ，那么∑O´在∑O中的齐次坐
1 0 0 a1
标变换为
T1
0 0
0
1 0 0
0 1 0
b1
，如果手爪转了一个角度，
z
w
已知： Puvw Pu iu Pv ju Pw kw
P点在ΣO´uvw中是不变的仍然 成立，由于ΣO´uvw回转，则：
Pw P
v
o
Pv
y
Px Puvw ix ix (Pu iu Pv jv Pw kw )
(O')
Py Puvw jy jy (Pu iu Pv jv Pw kw ) x Pz Puvw jz jz (Pu iu Pv jv Pw kw )
（ 2.3）
a×b =
ijk
ax ay az bx by bz
（2.4）
旋转矩阵
设固定参考坐标系直角坐标为ΣOxyz，动坐标系为ΣO´uvw， 研究旋转变换情况。
① 初始位置时，动静坐标系重合，O、O´重合，如图。各轴对
应重合，设P点是动坐标系ΣO´uvw中的一点，且固定不变。则
P点在ΣO´uvw中可表示为：
kw
则 : pxyz RPuvw
反过来： Puvw R1 Pxyz
R 1 R* det R
由刚体的等距变换可知：
pT xyz
pxyz
pT uvw
puvw
将上式代入，可得： RT R I
R为正交矩阵。
R为R的伴随矩阵，det R为R的行列式，由于R是正交矩阵， 因此R1 RT
由图可知，jv 在y轴上的投影为jy cos ，jv 在z轴上的投影
• 机器人的运动学即是研究机器人 手臂末端执行器位置和姿态与关
n
o
a
i
节变量空间之间的关系
运动学研究的问题
Where is my hand?
Direct Kinematics HERE!
运动学正问题
运动学逆问题
How do I put my hand here?
Inverse Kinematics: Choose these angles!
动坐标系在固定坐标系中的齐次变换有2种情况：
定义1：如果所有的变换都是相对于固定坐标系中各坐标轴旋 转或平移，则依次左乘，称为绝对变换。
定义2：如果动坐标系相对于自身坐标系的当前坐标轴旋转或 平移，则齐次变换为依次右乘，称为相对变换。
结果均为动坐标系在固定坐标中的位姿（位置+姿态）。相 对于固定坐标系，轴相当于 X轴，v轴相对于 Y轴，w轴相当于 Z轴。
考坐标系 Oxyz 做如下运动：① R（x, 90°）；② R(z, 90°)；③ R(y,90°)。求点 Po'uvw在固定参考坐标系 Oxyz下 的位置。 解1：用画图的简单方法
解2：用分步计算的方法
① R（x, 90°）
1 0 0 01 1
P' 0 0 -1 02 3 0 1 0 03 2
机器人运动学
第二章 数学基础
2.1 引言
机器人位置和姿态的描述
• 机器人可以用一个开环关节链来建模
• 由数个驱动器驱动的转动或移动关节串联而成
• 一端固定在基座上，另一端是自由的，安装工具，用以 操纵物体
• 人们感兴趣的是操作机末端执行 器相对于固定参考坐标数的空间 几何描述，也就是机器人的运动 学问题
z
V
o x
z y
x
图2-2
2.2 旋转齐次变换
用齐次坐标变换来表示式旋转变换：
Px
0Pu
Py
R
0
Pv
Pz 1
0
0
0
0 1
Pw 1
Pu
0Px
Pv
R 1
0
Py
Pw 1
0
0
0
0 1
Pz 1
2.2.3 合成旋转矩阵:
例1：在动坐标中有一固定点 Po'uvw 1 2 3 1T ，相对固定参
向量的点积是标量。用“ ·”来定义向量点积，即
（2.1）
a ·b = ax bx + ay by + az bz
（2.2 ）
向量的叉积是一个垂直于由叉积的两个向量构成的平面的向量。用“×” 表示叉积，即
a × b = ( ay bz ¯az by ) i + ( az bx ¯ax bz ) j + ( ax by ¯ay by ) k 可用行列式表示为
o
x
y
w″
z
o′
v″
u″
oy
x
解2：用计算的方法
z
v```
0 0 1 4 T Trans(4, 3, 7) R(y,90o ) R(Z,90o ) 1 0 0 3
o′ u``` w```
o
y
0 1 0 7
0 0 0
1
x
式（2-20）和式（2-21）无论在形式上，还是在结果上都是 一致的。因此我们有如下的结论：
O'
o
图2-5
vy
方向余弦阵
三个基本旋转矩阵:
同理：
1
R(x, ) 0
0
0
cos s in
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
0
0
0
11
1
② R（z, 90°） ③ R（y, 90°）
0 -1 0 0 1 3
P'' 1 0 0 0 3 1 0 0 1 0 2 2
0
0
0
1
1
1
0 0 1 03 2
P '''
0
1
0
0 1
1
-1 0 0 02 3
0
0
0
1 1
1
（2-14） （2-15） （2-16）
x x wx 为姿态矩阵，表示动坐标系∑O´在固定参考
R 33 y z
y z
wy
wz
坐标系∑O中的姿态，即表示∑O´各坐标轴单 位矢量在∑O各轴上的投影
P31 [ px py pz ]T
为位置矢量矩阵，代表动坐标系∑O´坐标原 点在固定参考坐标系∑O中的位置
f13 [0
0
0]
为透视变换矩阵，在视觉中进行图像计算， 一般置为0
w11 [1]
为比例系数
如果需要求解∑O在∑O´中的位置和姿态，此时的齐次变换矩 阵为 T 1 ，即求逆矩阵：
T1
R
T 33
(
RT
P)31
0 0 0
1
其中：
p
pxi
py
j
pzk
这些式子以后经常遇到， 在机器人计算中，所要 求的就是齐次变换矩阵
习题1：
∑O´与∑O初始重合，∑O´作如下运动：①绕Z轴转动30º；
解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,90) R(Z,90)
0 0 1 4
1 0 0 3
0 1 0 7
0 0 0
1
（2-20）
以上均以固定坐标系多轴为变换基准，因此矩阵左乘。
如果我们做如下变换，也可以得到相同的结果：
例2：①先平移Trans (4,-3,7)；②绕当前 v轴转动90º；
③绕当前 w 轴转动90º；求合成旋转矩阵。
解1：用画图的方法
z w
v u o(o′) y
x
z
w′ o′ v′ u′