SLAM专题（5） -- 变换矩阵、旋转矩阵、欧拉角与四元数

坐标变换基础知识

外积

欧氏变换

旋转矩阵

旋转向量

欧拉角

四元数

坐标变换基础知识

外积

外积大小为，可以表示两个向量张成四边形的有向面积，引入符号，定义a^表示为一个反对称矩阵，这样就把向量的外积变换成了矩阵和向量的乘法。

同时外积可以表示向量的旋转，在右手法则下，用右手的四个指头从a转到 b，其大拇指所指向的方向就是旋转向量的方向，旋转向量的大小为 a与 b的夹角。

欧氏变换

在机器人的运动中，通常先固定一个坐标系，称为世界坐标系，如图中的定义的坐标系；同时相机也是一个坐标系，如图中的定义的坐标系。在相机视野中的某个向量P坐标为Pc，在世界坐标系下坐标则为Pw。相机运动时，两个坐标之间的转换关系由变换矩阵 T来表示。

因为相机可以看作刚体，刚体运动时能保证同一个向量在各个坐标系下的长度和夹角不会发生变化，这种变换称作欧氏变换，一个欧式变换由一个旋转和一个平移两部分组成。

旋转矩阵

首先看旋转，设单位正交基经过一次旋转变成了，对于同一个向量a ，它在两个坐标系下的的坐标分别为和，则有：

为了描述两个坐标之间的关系，对等式左右同乘，左式系数即变为单位矩阵，右式系数则变成由两组标准正交基的内积组成的矩阵：

称右式矩阵R 为旋转矩阵，旋转矩阵作用于向量 a的坐标得到向量 a`旋转后的坐标。

仔细观察旋转矩阵 R，可以发现旋转矩阵为一个行列式为1的正交矩阵。反之，行列式为1的正交矩阵也是一个旋转矩阵R。这条性质在计算增量坐标变化时常用作一条约束条件来计算新的旋转矩阵。

由于R 为正交矩阵，由正交矩阵性质知旋转矩阵R的转置和其逆矩阵等价，所以可以描述一个相反的旋转过程：

欧式变换除了旋转还有平移，平移就简单多了，直接在旋转后的向量坐标上加一个平移向量 t就可以，把旋转和平移合在一起有：

通过旋转矩阵R和平移向量 t 可以完整描述一个欧氏变换。

变换矩阵和齐次坐标

虽然上式可以完整描述一个欧氏变换，但是这里的变换不是一个线性关系，当做两次变换时有：

那么用 a 表述 c时则有：

可以想象在n多次变换后式子会变得多么复杂，为了简化形式，引入了齐次坐标和变换矩阵：

给三维向量加一维变成四维向量，称为齐次坐标；把旋转和平移写在同一个矩阵里，称为变换矩阵。

首先介绍齐次坐标：

通过添加最后一维，用四个数表示一个三维向量，这样就多了一个自由度，多了一个自由度就可以写出线性的形式。
齐次坐标的每一个分量同时乘一个非零常数仍然表示同一个点，所以一个点的齐次坐标不唯一。
用齐次坐标运算得到的也是齐次坐标，将齐次坐标转换为非齐次坐标的方法为把所有坐标除以最后一项，强制最后一项为1，然后忽略最后一项即可得到欧氏空间中的非齐次坐标。

然后介绍变换矩阵：

变换矩阵左上角为旋转矩阵，右上角为平移向量，左下角为 0向量，右下角为1.
变换矩阵的逆也表示一个反向的变换

通过引入齐次坐标和旋转矩阵，就大大简化了欧氏变换的表示：

旋转向量

旋转矩阵R用9个量来表示三个自由度的旋转，变换矩阵T用16个量来表示6个自由度的欧式变换，这样会有数据冗余。
旋转矩阵具有约束：之前讲过，旋转矩阵必须是一个行列式det（R）为1的正交矩阵，这使得估计或优化一个旋转矩阵时会使得求解变得困难。

其实最开始提到的外积就可以表示向量的旋转。坐标系的旋转可以由一个旋转轴和一个旋转角来刻画，我们选择方向与旋转轴相同、长度等于旋转角的向量，来描述旋转，称为旋转向量。易知旋转向量用三个量来表示三个自由度的旋转。

反之也可得出从旋转矩阵到旋转向量的公式：

由于旋转轴上的向量在旋转后不发生改变，所以有:

因此转轴 n 是旋转矩阵 R 特征值为1对应的特征向量，所以求解方程

det(λI - R) = 1可得旋转轴。

欧拉角

除了旋转矩阵和旋转向量，还可以用欧拉角来直观的表式旋转。

欧拉角使用三个分离的转角来表述旋转，分别为：

1. 绕物体的 Z 轴旋转，得到偏航角 yaw；

2. 绕旋转之后的 Y 轴旋转，得到俯仰角 pitch；

3. 绕旋转之后的 X 轴旋转，得到滚转角 roll。

这样我们就可以通过这样一个三维向量来描述任意旋转。

但是使用欧拉角会遇到著名的万向锁问题：当俯仰角时，第三次旋转与第一次旋转将会使用同一个轴，这样会使系统丢失一个自由度，称为奇异性问题。所以欧拉角不适用于插值和迭代，也很少用于SLAM程序，往往只用于人机交互中，方便直观理解。

四元数

旋转矩阵冗余，欧拉角奇异。事实上我们找不到不带奇异性的三维向量描述方式。比如，当使用两个坐标来表示地球表面时，如经纬度，当纬度为时，经度没有意义。

回忆刚学过的复数，我们可以用复数集 C 来表示复平面上的向量，乘上 i相当于把复向量旋转90度。那么类似的，在表达三维空间的旋转时，也有一种类似于复数的代数：四元数。四元数是一种扩展复数，在表示旋转时具有紧凑性，缺点是运算时比较复杂。

一个四元数q具有一个实部和三个虚部：

，

其中为三个虚部，且满足以下关系式：

有时也通过一个标量和一个向量来表达四元数：

s称为实部， v称为虚部。当虚部为0时称为实四元数，实部为0时称为虚四元数，虚四元数可以表示三维空间中的一个点。

为了满足的关系，定义乘上一个虚部对应旋转180度。

四元数和旋转向量之间的转换关系为：

和

第一个式子中当 θ加上2π 时，易得旋转没有改变，但是得到的四元数 q却变成了 -q 。因此两个互为相反数的四元数表示着同一个旋转。

用四元数表示点的旋转时，设空间中一点；

若要表示点n绕转轴旋转 θ ，由上式得；

旋转后的，可验证得 p`也是纯虚数，表示三维空间中的点。

最后给出四元数到旋转矩阵的转换公式，设四元数，则有：

反之有：