机器人学第三章

3.1 刚体位姿描述 3.1.1 位置的描述（位置矢量）
在运动学中，可以认为物体是刚体。如 图，设置参考坐标系OXYZ，在刚体上 建立附体直角坐标系QUVW（称为运动 坐标系Moving Frame） 在直角坐标系中，一个点的位置可以用 3×1位置矢量来表示。 如点P的位置可 以写成
3.3.2 齐次变换矩阵 （Homogeneous Transformation Matrix） 所谓齐次变换矩阵是一4×4矩阵，是由姿态矩阵 和位置矩阵合成的，具体合成形式如下：
R H 0
r11 p r21 1 r31 0
r12 r22 r32 0
r13 r23 r33 0
例2 点的平移：将上例中的p点沿x轴平移3个单位， 沿y轴平移5个单位，沿z轴平移7个单位，求平移 后点的新坐标。 解：本例中的齐次变换矩阵为
1 0 H 0 0 0 1 0 0 0 0 1 0 3 5 7 1
平移后的新位置为
1 0 p3 Hp1 0 0 0 1 0 0 0 0 1 0 3 1 4 4 9 5 . 10 7 3 1 1 1
例3.5 上例中，如果变换均相对于运动坐标系进行，
求变换后的新位姿。 解：由于变换相对于运动坐标系进行，
H Rot ( x,90)Trans (3,5,7) 1 0 0 0 1 0 0 0 0 0 1 0 0 0 1 0 0 1 0 0 0 1 0 0 0 1 0 0 . 0 0 1 0 3 7 5 1 0 1 0 0 0 0 1 0 3 1 0 5 . 7 0 1 0 0 1 0 0 0 0 1 0 0 0 0 1
1 0 Rot ( x, ) 0 0 0 cos sin 0 0 1 0 0 0 sin cos 0 sin 0 cos 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 0 1
cos 0 Rot ( y , ) sin 0 cos sin Rot ( z , ) 0 0
sin cos 0 0
当连续进行多次变换时，其变换结果 是几个变换矩阵的乘积。当变换相对于 固定坐标系（即参考坐标系）时，按变 换顺序将矩阵左乘；当变换相对于运动 坐标系（即新坐标系）时，按变换顺序 将矩阵右乘。
例3.4 某一坐标系O1UVW变换前与参考 坐标系重合，首先将其绕X轴旋转90º ，
x y P z w
当w变化时，该式表示三维空间中一系列点的集 合，所有点都位于坐标系原点和p点的连线上， 当w等于无穷大时，表示原点；当w等于0时， 表示无穷远处的点，代表一个方向。 T T T 一般用 1 0 0 0 ， 0 1 0 0 ， 0 0 1 0 T 0 0 0 1 表示x,y,z三个坐标轴的方向，而用 表示原点。
本例中，如果先平移，后旋转，结果又如何？
3.3.3 齐次变换矩阵的运算 通过上述例子可以看出，平移变换和旋 转变换都可以通过齐次变换矩阵来实现。 平移齐次变换矩阵
1 0 Trans ( a, b, c ) 0 0 0 1 0 0 0 0 1 0 a b c 1
旋转齐次变换矩阵

3.3.4 齐次变换矩阵的逆矩阵 齐次变换矩阵由于其本身的特点，其逆矩阵的 求法比较简单。
nx n H y nz 0 ox oy oz 0 ax ay az 0 px py pz 1

则其逆矩阵为
H 1 nx o x ax 0 ny oy ay 0 nz oz az 0 n. p o. p a. p 1
px py pz 1
左上角3×3矩阵为姿态矩阵；右上角3×1矩阵为 位置矩阵。齐次变换矩阵可以将刚体的位置和 姿态用一个矩阵表达。
齐次变换矩阵的用途 例1 点的旋转：参考坐标系中有一点
1 4 P 1 3 1
求该点绕z轴旋转90°后的新位置。 解：绕z轴旋转90°的齐次变换矩阵为

B
A B
R
A
PBo

3.2 坐标变换（Coordinate Transformation）
空间中一点在不同的坐标系中的描述 是不同的。为了在不同坐标系中正确地描 述同一点，就需要采用坐标变换。 3.2.1 坐标平移变换 设坐标系﹛B﹜与﹛A﹜具有相同的方位， 但原点不重合，坐标系﹛B﹜在参考坐标 A pBo p在 系﹛A﹜中的位置矢量为 ，点 B ﹛B﹜中的位置矢量为 ，则它相对于参 p 考坐标系﹛A﹜的位置矢量为 A p ApBo Bp
3.4 姿态的表示方法
前述用3×3旋转矩阵表示刚体（运动坐
标系）的姿态，由于该矩阵的正交性，
其9个元素中只有3个是独立的。下面介
绍RPY姿态表示法和欧拉角姿态表示法。
3.4.1 RPY表示法
RPY角是描述船舶在海中航行时姿态的 一种方法。如图， 将绕z轴的旋转称为 回转（Roll）， 将绕y轴的旋转称为俯 仰（Pitch），将绕x轴的旋转称为偏转 （Yaw）。操作臂手爪姿态的规定如此 相似， 沿手指向外为z轴， 两手指连线 的方向为y轴，x轴按右手定则确定。

0 1 Rot ( x, ) 0 cos 0 sin
sin cos 0
cos 0 sin Rot ( y, ) 0 1 0 sin 0 cos cos sin 0 Rot ( z, ) sin cos 0 0 0 1
例3 例1中的点p首先绕z轴旋转90º ,然后平移[3,5,7] 个单位。 解：作上述旋转平移的齐次变换矩阵为
0 1 H 0 0 1 0 0 0 0 0 1 0 3 5 7 1
变换后点的新位置为
0 1 P 4 HP 1 0 0 1 0 0 0 0 0 1 0 3 1 1 4 6 5 . 10 7 3 1 1 1
然后将其平移[5，3，7，1]T；求变换
后的新位姿。
解：变换前运动坐标系与参考系重合，所
以其位姿矩阵为
1 0 A 0 0 0 1 0 0 0 0 1 0 0 0 0 1
由于变换是相对于参考系进行，所以变换后新 坐标系的位姿为
H Trans (3,5,7) Rot ( x,90) 1 0 0 0 1 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 0 3 1 0 5 . 7 0 1 0 3 5 7 1 0 0 1 0 0 1 0 0 0 1 0 0 . 0 0 1 0 0 1 0 0 0 0 1 0 0 0 0 1
3.2.2 坐标旋转变换

设坐标系﹛B﹜与﹛A﹜具有共同的原点， 但两者的方位不同，点p在坐标系﹛B﹜ B 中的位置矢量为 ，则该点在参考坐标 p 系﹛A﹜中的位置矢量为
A
p R p
A B B

式中 R 为坐标系﹛B﹜相对于坐标系 ﹛A﹜的姿态矩阵，或称旋转矩阵。
A B
3.3 齐次坐标和齐次变换
采用RPY方法描述手爪姿态的规则如下： 将手爪坐标系首先
绕x轴转动γ角，然
后绕y轴转动β角，
最后绕z轴转动α角。源自文库
而且每次转动都是 相对于参考系进行 的。
由于每次转动都是相对于参考系进行的，按照矩阵 左乘的规则，则获得的姿态矩阵为
0 c s 0 c 0 s 1 0 . 0 .0 c s Rot ( , , ) s c 0 1 0 0 1 0 s 0 c 0 s c cc cs sc csc ss s c s s c c s s c c s cs cc s
c cos , s sin ，依此类推。 其中，
现在讨论姿态的逆问题（RPY的逆解），即当位
姿矩阵已知时，求α，β，γ角度应为多少，
令
nx n y nz ox oy oz ax cc sc ay az s cs sc ss cc cs csc ss ssc cs cc

px P p y pz


在涉及到多个坐标系时，为了注明是在 哪一个坐标系中，可以利用左上标表示， 如
px A P py pz


表示该矢量是在坐标系A中定义的。
3.1.2 姿态（方位）的描述（旋转矩阵）

为了规定空间某刚体B的方位，在刚体上 固联一直角坐标系{B}，用坐标系{B}的 yB、 z B 相对于坐标系 三个单位主矢量 xB 、 {A}的方向余弦组成的3×3矩阵
3.3.1 点的齐次坐标 三维空间中某点的坐标可以写成 则该点的齐次坐标为
x wx y wy P z wz 1 w
x P y z
其中w是一不为0的数。
齐次坐标是用四个坐标值表示三维空间中的点
A B
R xB
A

A
yB
A
zB

或
r11 r12 A r22 B R r21 r31 r32
r13 r23 r33
来表示刚体B相对于参考坐标系﹛A﹜的 方位或姿态。称为旋转矩阵。当相对关 系明确时，可以去掉左侧的上下标。 旋转矩阵是正交矩阵，其9个元素中只有 三个是独立的。 绕x轴、 y轴、 z轴旋转θ角的旋转矩阵分 别为
cos 90 sin 90 H 0 0 sin 90 cos 90 0 0 0 0 1 0 0 0 0 1
旋转后的新位置为
0 1 P 2 HP 1 0 0 1 0 0 0 0 0 1 0 0 1 4 4 1 0 . 3 0 3 1 1 1
3.1.3 位姿的描述 为了描述刚体B在空间的位置和姿态，通 常将表示姿态的坐标系﹛B﹜固联在刚体 B上，其原点选择有特征的点，如质心或 某一顶点、中点等。坐标系﹛B﹜的原点 在参考坐标系﹛A﹜的位置矢量称为刚体 B在参考坐标系中的位置，坐标系﹛B﹜ 在参考坐标系中的姿态矩阵称为刚体在参 考坐标系中的姿态。可以写成
3 刚体位姿描述和齐次变换

在研究机器人运动学时，常常涉及到物 体在空间的位置和姿态，这里所指的物 体包括机器人的连杆、工具和工件等等。 为了描述物体的位姿，首先要建立参考 坐标系（Reference Coordinate System或 Fixed Frame）。参考坐标系一般设置为 直角坐标系，特殊情况下也可以采用其 他坐标系。