第三章 位姿描述和齐次变换

可以表示成等价的齐次变换式。
A B P BO P 1 1
A
A B A P B R P PBO
11
简写成 综合地表示了平移和旋转变换。
3.3.1 齐次坐标
一般来说，以N+1维矢量表达N维位置矢量的方法称为齐次 坐标表示法。
在三维直角坐标系中，一个点可以表示 为
令R=R(K,θ)，得
将方程两边的主对角线元素分别相加，得
于是可得：
再把方程两边的非对角元素成对相减得：
将上式两边平方后再相加得：
sin
1 2
(o z a y ) (a x n z ) (n y o x )
2 2 2 2 2
2
kx ky kz
oz a y 2 sin ax nz 2 sin ny ox 2 sin
第三章 位姿描述和齐次变换
3.1 相关知识回顾
一、行列式和矩阵 1. 行列式按照行（或列）展开法则：行列式等于它的任意一行 （或列）各元素与其对应的代数余子式乘积之和。
2.行矩阵
3.列矩阵
4.矩阵相等：两同型矩阵（行数和列数都相等）对应元素相等。
5.单位矩阵：主对角线元素为1，其它所 有的元素都为0的方阵。
0 .5 0 . 866 0
0 0 1
A
PBO
10 5 0
A B A 最后得： P B R P PBO
A
9 . 098 12 . 562 0

3.3 齐次坐标与齐次变换
复合变换式
A A P BR 1 0
6.矩阵的运算 （1）矩阵的加法：两同型矩阵的对应元素相加。
（2）矩阵与数相乘：该数与矩阵各元素相乘。
（3）矩阵与矩阵相乘：
（4） 矩阵的转置：把矩阵的行换成同序数的列，记为
7. 矩阵的逆（逆矩阵）
8. 分块矩阵：分块后的矩阵与普通矩阵的运算相同。
9. 正交矩阵：如果 如果
，则A为正交矩阵。它满足：
注意：位置矢量 究竟是3×1的直角坐标还是4×1的齐次坐标，应 根据上下文而定。
3.3.2 齐次变换 齐次变换矩阵是4×4的矩阵，它的完整形式可以看成是由 四个子矩阵组成：
R 3 3 T f 1 3 P3 1 旋转变换 11 透视变换 位置矢量 比例变换
②病态情况：当转角θ很小时，转轴难确定；当θ接近0°或
180°时，转轴完全不能确定，需另寻解法。 例：求复合变换 的等效转轴k和转角θ。
Class is over. Bye-Bye!
次坐标就是
，它的齐
，即满足Px=ωPx/ω，Py=ωPy/ω，
Pz=ωPz/ω（ω是非零整数）。可以看出，在三维直角坐标系中， 由于ω取值的不同，一个点的齐次坐标的表达不唯一。
齐次坐标不仅可以规定点的位置（ω为非零整数），还可以
用来规定矢量的方向（第四个元素为零时）。列向量 ( 数。 分别代表了ox，oy和oz轴的无穷远 点，用它们分别表示这三个坐标轴的方向。另外， 坐标原点， 没有意义。 代表 ）表示空间的无穷远点，a，b和c称为它的方向
一般的情况：坐标系{B}的原点既不与{A}重合，方位也不相同。
{C}系与｛Ｂ｝系原点重合， 但方位不同，所以得
{C}系与｛A｝系原点不重合， 但方位相同，所以得 和
A
A PCO PBO
进而有
例3.2 已知坐标系{B}初始位姿与{A}重合，首先{B}相对{A}的zA轴 转30°，再沿{A}的xA轴移动10个单位，并沿{A}的yA轴移动5个单 位。求位置矢量 解： 和旋转矩阵 。若 ,求 。
zB zA yB OB OA xA 30o x
B
zA zB OA yA OB 30o
30o
yB
yA
(10,5,0)
xA 30o xB
所以有：
cos 30 0 A 0 0 R R ( z , 30 ) sin 30 B 0 sin 30 cos 30 0
0 0
0 0 . 866 0 0 .5 1 0
是正交矩阵，则
行列式和矩阵的区别：矩阵是按一定方式排成的数表；行列式是
一个数。
二、直角坐标系
若基矢量相互正交，即它们在原点o处两 两相交成直角，则它们构成直角坐标系或笛卡 儿坐标系。 若按右手法则绕oz轴转900可以使ox轴转向 oy轴，则称为右手坐标系；按左手法则形成的 坐标系称左手坐标系。
斜角坐标系
A
，
A B P BT P B C B P CT P A C A P CT P
A
A B C P B T CT P
T B T CT C
A A B
从而定义复合变换
。
同理得出： 即一个坐标系变换至另一坐标系的齐次变换矩阵等于依次经历中
间坐标系各齐次变换矩阵的连乘积。
因此
尺寸链图
将上式展开得
把上式右端相乘，并利用旋转矩阵的正交性质
进行化简整理后得
k x k x Vers c R ( k , ) k x k y Vers k z s k k Vers k s y x z k y k x Vers k z s k y k y Vers c k y k z Vers k x s k z k x Vers k y s k z k y Vers k x s k z k z Vers c
3.2 位姿描述与坐标变换
3.2.1 刚体位置姿态（位姿）描述
a) 位置的描述
采用直角坐标描述点的位置，因此，刚体F的位置描述，即 OB点在{A}中描述可用一个3×1的列矢量 （位置矢量）表示，即
其中Px、Py和Pz是点OB在{A}系中的三个坐标分量。
b) 姿态（方位）的描述 采用旋转矩阵来表示刚体姿态（方位） ，即由{B}系的三个 单位主矢量相对于坐标系源自A}的方向余弦组成： xB yB zB
其中，sθ=sinθ;cθ=cosθ;Versθ=(1-cosθ)。 如果 与坐标轴重合，则可得到绕x,y和z轴旋转的基本旋转矩阵。
例：求绕过原点的轴线
转动1200的旋转矩阵
二. 等效转轴与等效转角
任何一组经过有限次基本旋转变换后的复合旋转总可以等
效成绕某一过原点的轴线转θ角的单一旋转。
对于给定的旋转矩阵R
坐标旋转
如图所示，{B}与{A}有共同的坐标原点，但方位不同。令
和
分别是{A}和{B}中的单位主矢量，点P 在两
坐标系中各坐标轴上的坐标分量分别为：
和
所以有 利用点乘的性质和上式共同求解得
将
代入上面三式中并写成矩阵形式得
上式简写为：
此式称为坐标旋转方程。其中旋转矩阵 表示了坐标系{B}相
对于{A}的方位，正好与刚体姿态的描述相同。同理也可得
所以有 I B T AT 4 4 A B
B AR 0 B A P AO B R 1 0 A
PBO 1
B
B A ARBR 0
B A
I 3 3 R PBO P AO 1 0
A
0 1
对应元素相等得
Y
R P
系转换，各齐次变换矩阵按“从右
向左”依次相乘原则进行运算（右 乘）。
RPY角
=
RPY角 反解：
2、绕动坐标系依次进行的齐次变换，按“从左向右”的原则依
次相乘(左乘）。
z-y-x欧拉角：
=
齐次变换的相对性
相对于固定坐标系运动
相对于活动坐标系运动
二.变换过程的可逆性 齐次坐标变换过程是可逆的. 若有 ，则逆变换 。
于是：
tg
(o z a y ) (a x n z ) (n y o x ) ( n x o y a z 1)
两点注意:①多值性：K和θ的值不唯一。实际上，对于任意一组 K和θ，都对应另一组-K和-θ，(K,θ) 和(k, θ+n×360)对应的转动
效果相同，θ的取值也有多种，一般取在0°到180°之间。
即两矢量方向上单位矢量的点乘等于两矢量夹角的余弦。
四、矢量的叉积（矢量积或叉乘积）
其中矢量c的模为:
叉乘积
其中θ是a和b间小于等于1800的夹角，若将a按右手法则绕c 转θ角至b，右手拇指指向为c的正方向（如上图所示），c与a、b 两者垂直。 若a和b用分量的形式表示为: 则
a和b的点乘为：
将点乘和叉乘应用于右手笛卡尔坐标系的单位矢量i，j，k，有：
和
都是正交矩阵，因此满足
由
与
互逆，可得
若把
写成行向量的形式
，则其中 满足六个约束条件
每一个元素都是一个列向量。容易得出 （称正交条件）：
旋转矩阵的几何意义：旋转矩阵在几何上表示了发生相互旋 转的两坐标系各主轴之间的相互方位关系。
因此写出三个基本的旋转矩阵，即分别绕x、y和z轴转θ角的旋转 矩阵：
在机器人研究中，齐次变换矩阵T为：
纯旋转的齐次变换矩阵中P3×1为零矩阵，即 因此写出绕x，y和z轴旋转θ角的基本齐次变换矩阵为：
，
纯平移的齐次变换矩阵中R3×3=I3×3（单位阵），因此可以 写出沿x，y和z轴移动Px，Py和Pz单位的基本平移变换阵：
给定坐标系{A}，{B}和{C}，已知{B}相对{A}的描述为 {C}相对{B}的描述为 ，则有
义求解，另一种采用坐标系依次变换的方法；②求 方法）； ③画出{0}到{3}的空间尺寸链图。
{1}
空间尺寸链图：