第二章 位姿描述和齐次变换(2010-09)

用矩阵表示为: 用矩阵表示为
Px ix iµ P = j i y y µ Pz k z iµ
ix jv j y jv k z jv
图2-4 ix k w Pµ jy k w Pv （2-7） ） P k z kw w
Px = Puvw i x = ix ( Pu iu + Pv jv + Pw k w )
Py = Puvw j y = j y ( Pu iu + Pv jv + Pw k w )
x
Pw P Pv o
(O')
v y
Pu
Pz = Puvw j z = j z ( Pu iu + Pv jv + Pw k w )
α
o
U'
v
y
u x
图2-5
方向余弦阵
z
三个基本旋转矩阵: 三个基本旋转矩阵:
W'
w
0 1 R(x, α ) = 0 cos α 0 sin α
− sin α comα 0
u
Φ
o
O'Fra Baidu bibliotek
v
y
同理： 同理：
cosφ R（y,φ ) = 0 − sin φ
三个基本旋转矩阵
R ( x, α )
µ 的旋转矩阵， 即动坐标系 ∑ O， vw绕OX轴转动α角， R ( x, α ) 的旋转矩阵，也就是 求
求出坐标系 ∑ O ' µvw 中各轴单位矢量 i µ , j v , k w 在固定坐标系 ∑ Oxyz 中各轴的投影分量，很容易得到在重合时， 中各轴的投影分量，很容易得到在重合时，有：
空间任意两直线的公法线长度公式
给定一直线过p点，具有方向矢量m，另一直线过点q，具有 方向矢量n，则：
r (m × n) ⋅ pq a= m×n
α = acr cos(
m⋅n m⋅n
)
位置描述(position)---点在坐标系的位置
一旦建立了一个坐标系，我们就能够用某个3×1位置矢量来确定该空 间内任一点的位置。对于直角坐标系{A}，空间任一点p的位置可用3×1的 列矢量AP表示。其中，px、py、pz入是点p在坐标系{A}中的三个坐标分量。 Ap的上标A代表参考坐标系{A}。我们称Ap为位置矢量，见图2．1。
旋转矩阵---举例 [例1 ] 已知转动坐标系OUVW中的两点aUVW＝(4，3， 2) T和bUVW＝(6，2，4) T，若OUVW系统绕OZ 轴转动 了60。，试求参考坐标系中的相应点axyz和bxyz。 [解]
a xyz = RZ ,600 • auvw
bxyz = RZ , 600 • buvw
(2.10)
旋转矩阵
设固定参考坐标系直角坐标为ΣOxyz， 动坐标系为 ´uvw， ， 动坐标系为ΣO´ 设固定参考坐标系直角坐标为 ， 研究旋转变换情况。 研究旋转变换情况。 初始位置时，动静坐标系重合， 、 ´ 重合，如图。 ① 初始位置时，动静坐标系重合，O、O´ 重合，如图。各轴 对应重合， 点是动坐标系ΣO´ 中的一点，且固定不变。 对应重合，设P点是动坐标系 ´uvw中的一点，且固定不变。 点是动坐标系 点在ΣO´ 中可表示为： 则P点在 ´uvw中可表示为： 点在
A 姿态矩阵 B R中的三个列矢量分别表示了{B}坐标系的单位基矢量iB , jB , k B 在{A}坐标系轴上的投影 A 姿态矩阵 B R中的三个行矢量分别表示了{A}坐标系的单位基矢量i A , j A , k A在{B}坐标系轴上的投影
位姿描述
要完全描述刚体B在空 间的位姿(位置和姿态)，通 常将物体B与某一坐标系{B} 相固接。{B}的坐标原点一 般选在物体B的特征点上， 如质心等。相对参考系{A}， 坐标系{B}的原点位置和坐 标轴的方位，分别由位置 矢量B和旋转矩阵描述。这 样，刚体B的位姿可由坐标 系{B}来描述，即有
补充：向量的点积和叉积 矩阵的乘法
a ⋅ b = axbx + a y by + az bz
i a × b = ax bx j ay by v v v az = (a y bz − az by )i + (az bx − axbz ) j + (axby − a y bx )k bz k
方向余弦
1. 方向角与方向余弦 r r r r OA = a OB = b ϕ=∠AOB(0≤ϕ≤π)为 向量
cosθ R（z,θ ) = sin θ 0
0 1
sinφ 0 0 cos φ
0 0 1
x
U'
z w
W'
- sinθ cosθ 0
v' o
O'
θ
v
y
u x
U'
绕坐标轴转动的旋转矩阵
式中，s表示 ，c表示 表示sin， 表示 表示cos。以后将一律采用此约定。 表示
Y(orientation)
x(normal)
z(approach)
A B
& & & R = [n o a ]
(2.9)
手抓坐标系
Y(orientation) x(normal) z(approach)
A B
& & & R = [n o a ]
平移坐标变换
前面讨论的是在一个坐标系中位姿的描述，在大量的机器人问题中，涉及到 用不同的坐标系来描述同一个刚体的位置及姿态问题，这就涉及到从一个坐 标系的描述到另一个坐标系的描述之间的变换关系，这种变换关系包括：平 平 移变换和旋转变换
z
i x iµ R(x,α ) = j y iµ k i z µ
i x jv j y jv k z jv
ix k w jy k w k z kw
W'
w
α
V' O'
0 1 ix = iu 0 cos α 0 sin α
− sin α cos α 0
z
Puvw = Pu iu + Pv ju + Pw k w
iu 、j v 、 w 为坐标系 k 为坐标系ΣO´uvw的单位矢 ´ 的单位矢
w o u x
P
v
(O')
点在Σoxyz中可表示为： 中可表示为： 量，则P点在 点在 中可表示为
y
Pxyz = Px i x + Py i y + Pz i z
解2：用分步计算的方法 ： ① R（x, 90°） （ °
1 0 0 1 1 P ' = 0 0 - 1 2 = − 3 0 1 0 3 2
（2-14） ）
② R（z, 90°） （ °
0 - 1 0 1 3 P '' = 1 0 0 − 3 = 1 0 0 1 2 2
Px P = y Pz
R3×3
Pu P v Pw
R3x3为二者之间的关系矩阵，我们令： 为二者之间的关系矩阵，我们令：
R 3×3 = R（y,θ ) R( z, φ ) R( x, α )
定义1： 定义 ： 当动坐标系 ∑ O 'uvw 绕固定坐标系 ∑ Oxyz 各坐标轴顺序有限 次转动时，其合成旋转矩阵为各基本旋转矩阵依旋转顺序左乘 左乘。 次转动时，其合成旋转矩阵为各基本旋转矩阵依旋转顺序左乘。 注意： 注意：旋转矩阵间不可以交换
1 0 0 R = 0 1 0 0 0 1
由图2 可知， 由图2-5可知， jv 在y轴上的投影为
k z cos α
j y cos α
， jv 在z轴上的投影
为 k z sin α , k w 在y轴上的投影为 − j y sin α ， k w 在z轴上的投影为 ，所以有： 所以有：
ϕ
Pr ju AB = AB ⋅ cos(φ )
B′
u
向量补充
已知：a=(a1,a2,a3), b=(b1,b2,b3)
cosa,b =
a1b1 +a2b2 +a3b3 a +a +a b +b +b
2 1 2 2 2 3 2 1 2 2 2 2 2 3 2
dA,B = (x2 − x1) +( y2 − y1) +(z2 − z1)
方位描述(orientation) 方位描述
物体的方位可由某个固接于此物体的坐标系描述为了规定空间某刚体B 的方位，设置一直角坐标系{B}与此刚体固接。用坐标系{B}的三个单 位主矢量xB、yB、zB相对于参考坐标系{A}方向余弦组成的3×3矩阵
来表示刚体B相对于坐标系{A}的方位。称为姿态矩阵 旋转矩阵 姿态矩阵/旋转矩阵 姿态矩阵 旋转矩阵。式中，上 标A代表参考坐标系{A}，下标B代表被描述的坐标系{B}。共有9个元素， 但只有3个是独立的。由于的三个列矢量AxB、 AyB 、和AzB 都是单位矢量， 且双双相互垂直，因而它的9个元素满足6个约束条件(正交条件)。
数学基础
机械手作为执行机构是用来保证复杂空间运动的综合刚体 综合刚体，而 综合刚体 且它自身也往往需要在机械加工或装配等过程中作为统一体进行 运动。因此，我们需要一种用以描述单一刚体位移、速度和加速 度以及动力学问题的有效而又方便的数学方法---矩阵法 矩阵法 数学描述是以四阶方阵变换三维空间点的齐次坐标为基础的， 能够将运动、变换和映射与矩阵运算联系起来
u
i x iµ 定义 旋转矩阵为：R = j y iµ k i z µ
ix jv j y jv k z jv
−1
ix k w jy k w 则 : p xyz = R Puvw k z kw
反过来： 反过来：
Puvw = R −1 Pxyz
R* R = det R
旋转矩阵---举例 [例2 ] 已知参考坐标系OXYZ中的两点aXYZ＝(4，3， 2) T和bXYZ＝(6，2，4) T，若OUVW系统绕OZ 轴转 动了60。，试求转动坐标系中的相应点aUVW和bUVW。 [解]
auvw = RZ , 600 • a xyz
T
bxyz = RZ , 600 • bxyz
T
合成旋转矩阵: 合成旋转矩阵:
例1：在动坐标中有一固定点 Po'uvw = [1 2 3]T ： ，相对固 ∑ Oxyz 做如下运动： 定参考坐标系 做如下运动：① R（x, 90°）；② （ °）；② ∑ Oxyz R(z, 90°)；③ R(y,90°)。求点 Po 'uvw 在固定参考坐标系 ° ； ° 。 下的位置。 下的位置。 解1：用画图的简单方法 ：
r a
，r b
的夹角，记作
r∧ r (a , b )
O ϕ
A B
=ϕ
方向角的余弦称为其方向余弦.
x cos α = r r
= x x2 + y 2 + z 2
z
r γ r
o
α
β
r ro r r = r r
= (cos α , cos β , cos γ )
y
x
B A
2.向量在轴u上的投影等于向量的 模乘以轴与向量的夹角的余弦：
R ∗为R的伴随矩阵， R为R的行列式，由于R是正交矩阵， det 因此R −1 = R T
旋转矩阵的几何意义
A 1) B R可以作为固连于刚体的{B}坐标系对{A}参考坐标系的姿态矩阵
A 2) B R可以作为坐标变换矩阵。它使坐标系{B} 中的P点的坐标B P变换
成坐标系{A} 中的同一个空间点P的坐标 A P A 3) B R可以作为算子。用来表示具有转动关系的两个矢量在同一坐标 系中的投影之间的关系
0 0 1 3 2 P ''' = 0 1 0 1 = 1 - 1 0 0 2 − 3
（2-15） ）
③ R（y, 90°） （ °
（2-16） ）
上述计算方法非常繁琐， 上述计算方法非常繁琐，可以通过一系列计算得到上述 结果。将式（ ）（2-15）（ ）（2-16）联写为如下形式： 结果。将式（2-14）（ ）（ ）（ ）联写为如下形式：
Puvw = Pxyz
当动坐标系ΣO´ 点回转时， ② 当动坐标系 ´uvw绕O点回转时，求P点在固定坐标系 绕 点回转时 点在固定坐标系 Σoxyz中的位置 中的位置 z
w
已知： 已知： Puvw = Pu iu + Pv ju + Pw k w P点在 ´uvw中是不变的仍然 点在ΣO´ 中是不变的仍然 点在 成立，由于 ´ 回转， 成立，由于ΣO´uvw回转，则： 回转