第二章 机器人数学基础

§ 2.4
物体的变换及逆变换
(a) 变换前的坐标系
(b) 变换后的坐标系
图2.8 对楔形物体的变换

§ 2.4
物体的变换及逆变换
齐次变换的逆变换
B B p = C T Cp
A
p= T p= T T p
A B B A B B C C
A C
B T = AT CT B
§ 2.4
物体的变换及逆变换
齐次变换的逆变换 定义复合变换
A
p
px = py pz
其中px，py，pz是点p 在坐标系{A}中的三个坐标 分量。 Ap的上标A代表参考 坐标系{A} 。
§ 2.1
二、方位的描述（旋转矩阵） 方位的描述（旋转矩阵）
刚体位姿描述
为了规定空间某刚体B的方位，设置一直角坐标系{B}与此刚体固接 。用坐标系{B}的三个单位主矢量xB，yB，zB相对于参考坐标系{A}的方 向余弦组成的3×3矩阵
§ 2.5
通用旋转变换
Rot ( f , θ) = CRot ( z , θ)C 1
nx n CRot ( z , θ)C 1 = y nz 0
ox oy oz 0
ax ay az 0
0 cθ sθ 0 sθ cθ 0 0 0 1 0 0
0 0 n x 0 0 o x 0 0 a x 0 1 0
刚体位姿描述
三、位姿描述 为了完全描述刚体B在空间的位姿（位置和姿态），通常将物体B与 某一坐标系{B}相固接。 {B}的坐标原点一般选在物体B的特征点上，如 质心或对称中心等。相对参考系 {A}，坐标系{B}的原点位置和坐标轴 A 的方位，分别由位置矢量ApB。和旋转矩阵B R 描述 ，这样刚体B的位姿 可由坐标系{B}来描述，即：
§ 2.3
齐次坐标变换
(a) 图a表示u绕z轴旋转90°至v，再绕 y轴旋转90°至w
(b) 图b表示u绕y轴旋转90°至v，再绕 z轴旋转90°至w1
§ 2.3
齐次坐标变换
把旋转变换与平移变换结合起来，变换结果如下图所示 。
上图表示u绕z轴旋转90°至v，再绕y轴旋转90°至w，再进行 平移变换4i-3j+7k的结果
§ 2.1
Ax Ax Ax
刚体位姿描述
B B
B
Ay B
= AyB AyB = AzB AzB =1 = AyB AzB = AzB AxB =0
A B
可见，旋转矩阵
A B
R 是正交的，并且满足条件
A A R 1 = B R T , B R = 1
上标T表示转置 ， 为行列式符号 。 对应于轴x，y或z作转角为θ的旋转变换，其旋转矩阵分别为
zB {B} zA {A}
Ap
yB xB oB yA
{B}相对于 {A}的平移矢量。 点p在坐标系{B}中的位置为Bp， ， 则点p在坐标系{A}中的位置为Ap可由 矢量相加得出： Ap= Bp + Ap B。 上式为坐标平移方程 坐标平移方程。 坐标平移方程
Ap
B。
oA xA
§ 2.2
二、坐标旋转
即绕矢量 f 旋转等价于绕坐标系{C}的z轴旋转，即有
Rot ( f , θ) = Rot (c z , θ)
T为参考坐标系 T=CS CS S=C-1T C S表示T相对于坐标系{C}的位置。对S求解得： T S T绕矢量 f 旋转等价于S绕坐标系{C}的z轴旋转 S
Rot ( f , θ)T = CRot ( z , θ) S Rot ( f , θ)T = CRot ( z , θ)C 1T
物体的变换及逆变换
我们可以用描述空间一点的变换方法来描述物体在 空间的位置和方向。例如，图2.8(a)所示物体可由固定该 物体的坐标系内的六个点来表示。我们可对上述楔形物 体的六个点变换如下：
0 1 0 0 0 1 4 1 1 1 0 0 0 0 0 0 1 0 0 0 0 2 0 0 1 1 1 1 1 1 1 4 4 6 0 4 4 1 1 1 = 2 0 0 0 0 0 1 1 1 1 1 1 6 4 4 1 1 1 0 4 4 1 1 1
a b = a x bx + a y b y + a z bz
而两矢量的交积为另一个与此两相乘矢量所决定的平面垂直的矢量
a × b = (a y bz a z b y )i + (a z bx a x bz ) j + (a x b y a y bx )k
两矢量的交积
i a × b = ax bx
ωp x ωp z y px = , py = , pz = ω ω ω
可以看出，直角坐标系Oxyz原点的齐次坐标为(0，0，0，α)。 α为 非零实数。齐次坐标(1，0，0，0)T表示Ox轴的无穷远点，同理齐次坐标 (0，1，0，())T和(0，0，1，0) T分别指向Oy轴和0z轴的无穷远点，三维 空间的位置矢量的齐次坐标表达并不是惟一的。但若将ω取为1，则位置 矢量变换后的齐次坐标和矢量的实际坐标就相同了。在机器人学的应用 中ω总是取为1。
Tx ,α
0 cos β 0 0 , Ty ,β = sin β 0 1 0
齐次平移矩阵Ttran使Ouvw坐标系的原点平移到参考坐标系的[dx，dy ，dz ]T ，而保持坐标轴平行。 1 0 0 d x 0 1 0 d y Ttran = 0 0 1 d z 0 0 0 1 基本齐次变换阵可以相乘以求得合成齐次变换阵，可是矩阵乘法是不 可交换的，必须注意这些矩阵的相乘次序。也就是说：若动坐标系O uvw绕 (或沿)Oxyz系（固定坐标系 固定坐标系）主轴转动(或平移)则用相应的基本齐次阵左乘 固定坐标系 左乘 齐次变换矩阵；若动坐标系O uvw绕(或沿)它自己的主轴（运动坐标系 运动坐标系）转 运动坐标系 动(或平移)，则用相应的基本齐次旋转(或平移)矩阵右乘 右乘齐次变换矩阵。 右乘
B T S T = B T GT GT S T
(a) 机械手与环境间的运动关系
(b) 对应的有向变换图
§ 2.5
通用旋转变换
设 f 为坐标系{C}的z轴上的单位矢量
nx n C= y nz 0
ox oy oz 0
ax ay az 0
0 0 0 1
f = ax i + a y j + az k
R3×3 T = O1×3
P3×1 旋转矩阵3×3 = I1×1 O1×3
位置矢量3 ×1 1
若三维空间的位置矢量P表示成齐次坐标，即P＝(px py pz 1) T， 那么利用变换矩阵的概念，对纯转动，3 × 3旋转矩阵可扩展成4 × 4 齐次变换矩阵
齐次变换 规定两矢量的点积为一标量
1 0 R( x,θ ) = 0 cθ 0 sθ
cθ sθ 0 0 cθ 0 sθ sθ R( y, θ) = 0 1 0 R( z, θ) = sθ cθ 0 0 0 1 sθ 0 cθ cθ
式中，s表示sin，c表示cos 。以后将一律采用此约定。
§ 2.1
可以类似用 A R 描述{A} 相对于{B}的方位。 A B R 和 B R 都是正交矩阵，两者互逆。根据正交 A 矩阵的性质有：
B A A A R = B R 1 = B RT
B
xA
xB
§ 2.2
三、复合变换
坐标变换
yC yB yA
Ap Ap
Bp
xB xC
坐标系{B} 的原点与{A}的原点既 不重合，两者的方位又不同时，用位 置矢量ApB。描述{B}的坐标原点相对 B 于{A} 的位置，用旋转矩阵 A R 描述 {B}相对于{A} 的方位，则任一点p在 坐标系{A} 和{B}的描述Ap和Bp具有如 和 下变换关系
j ay by
k az bz
§ 2.3
0 0 1 0 cos α sin α = 0 sin α cos α 0 0 0
齐次坐标变换
0 sin β 0 cos θ sin θ sin θ cos θ 1 0 0 , Tz ,θ = 0 0 0 cos β 0 0 0 1 0 0 0 0 0 0 1 0 0 1
：
A B p Bo C R 1 0 B A B pCo B R C R = 1 0 A B
A BR A B T = AT CT = C B 0
R B pCo + A p Bo 1
§ 2.4
物体的变换及逆变换
变换方程初步 必须建立机器人各连杆之间，机器人与周围环境之间的 运动关系，用于描述机器人的操作。要规定各种坐标系来描 述机器人与环境的相对位姿关系。在下图 (a)中，位姿关系 可用相应的齐次变换来描述，机器人控制和规划的目标与其 他变换之间的关系可用空间尺寸链（有向变换图）来表示， 如图 (b)所示 。
A B
R=
[
A
xB
A
yB
r11 A z B = r21 r31
]
r12 r22 r32
A
r13 r23 r33
来表示刚体B相对于坐标系{A}的方位。B R称为旋转矩阵，上标A代表参 A 考坐标系{A} ，下标B代表参考坐标系{B} 。B R有9个元素，其中只有3个 A 是独立的。因为 B R 的三个列矢量AxB， AyB 和AzB 都是单位主矢量，且两 两相互垂直，因而它的9个元素满足6个约束条件（正交条件）
4 × 4齐次变换矩阵把在O uvw 坐标系中用齐次坐标表示的矢量映 射到Oxyz参考坐标系中去，即
Pxyz = TPuvw
nx n T = y nz 0 ox oy oz 0 ax ay az 0 px p y n o a = pz 0 0 0 1 p 1
§ 2.4
物体位置描述
A A p = B R B p+ ApB。