三维空间刚体运动

参考书目《视觉SLAM十四讲》高翔张涛等著

参考链接 https://zhuanlan.zhihu.com/p/32937868

一、旋转矩阵

1. 点、向量和坐标系

我们一般用三维空间中的点来表示我们的物体，向量来表示点在空间中的移动，简单定义在这边不再赘述。

假定我们现在有向量 $a和b$ 用 $<a,b>$ 表示向量之间夹角，那么对外积而言我们定义 a与b的外积 = a^b ，外积的结果是一个向量，根据右手定则方向垂直于这两向量形成的平面，大小为 $|a||b|sin<a,b>$ ,我们把 a^ 称为 反对称矩阵 。

2. 坐标系间的欧式变换

对于两个坐标系之间的变化，我们可以用 一个旋转和一个平移 表示，这种运动称为 刚体运动 。

我们的 欧式变换 就是由 旋转和平移 组成。假定我们某点在 $[e_1,e_2,e_3]^T$ 坐标系下坐标为 $[a_1,a_2,a_3]^T$ , 在 $[{e_1}^{‘},{e_2}^{‘},{e_3}^{‘}]$ 下坐标为 $[{a_1}^{‘},{a_2}^{‘},{a_3}^{‘}]^T$ 。

我们对左右两边同乘 $[{e_1}^{T},{e_2}^{T},{e_3}^{T}]^T$ ,左边的系数就变成了单位矩阵。

我们把中间的矩阵定义为 $R旋转矩阵(Rotation \quad Matrix)$ , 该矩阵各分量是两个坐标系基的内积，由于基向量模长为1，所以实际上为各基向量夹角的余弦值，所以这个矩阵也称为 方向余弦矩阵(Direction Cosine Matrix) ，或者 旋转矩阵 。

旋转矩阵是行列式为1的正交矩阵，反之，行列式为1的矩阵也是一个旋转矩阵。我们可以将旋转矩阵的集合定义如下，称为 特殊正交群 。
$$
SO(n) = { R \in R^{n \times n} |RR^T=I, det(R)=1 }
$$
通过旋转矩阵，就可以直接谈论两个坐标系之间的旋转变化，而不用再从基谈起。

在欧式变换中当我们把旋转和平移写在一个式子中，如下
$$
a_1 = R_{12} a_2 + t_{12}
$$
其中 $a_1,a_2$ 分别代表 a 在两个坐标系下的坐标， $R_{12}$ 为把坐标系2的向量变换到坐标系1， $t_{12}$ 代表平移向量。这样我们就可以实现一个点在两个坐标系下的转换。

3. 变换矩阵和齐次坐标

我们上面引入的变换公式不是线性关系，当进行多次变换的话，算出的结果会很复杂，所以我们引入齐次坐标和变换矩阵。

这样写的好处是可以把旋转和平移写在一个矩阵内，使得整个关系变成线性关系。矩阵 T 称为 变换矩阵（Transform Matrix） 。这个矩阵又被称为 特殊欧式群（Special Euclidean Group） 。

二、旋转向量和欧拉角

1. 旋转向量

在三维空间中，任意旋转都可以用 一个旋转轴和一个旋转角 来刻画，我们定义一个向量，方向与旋转轴一致，长度等于旋转角，这种向量被称为 旋转向量（轴角/角轴，Axis-Angle） ，只需要一个三维向量即可描述旋转。

假定旋转轴为一个单位长度的向量 n ，角度为 $\theta$ ,那么向量 $\theta n$ 可表示这个旋转。从旋转向量到旋转矩阵的转换公式由 罗德里格斯公式（Rodrigues’s Formula） 表明。
$$
R = cos \theta I + (1-cos \theta )nn^T+ sin \theta \hat n
$$
我们也可以反推出 $ \theta $ 公式。
$$
\theta = arccos \frac {tr(R)-1}{2}
$$
关于转轴 n 在旋转之后不发生改变
$$
Rn = n
$$

2. 欧拉角

除了旋转向量，我们也可以用 欧拉角 来紧凑地描述旋转。一个旋转可以分解成3次分别绕X，Y，Z 轴的旋转来表示，在航空摄影测量中，一般用 “翻转 - 航偏 - 俯仰”（roll - yaw - pitch) ，也即 XZY 来表示。先绕X 轴旋转roll 角度，再绕Y 轴旋转yaw 角度，最后按照Z 轴旋转pitch 角度。这三个旋转矩阵相乘就得到了总的旋转矩阵。

此时，就可以用 $[r,p,y]^T$ 这三个量来描述任意旋转，这种表示方式会比其他方式更直观、更易理解。

但欧拉角会有一个重大缺点，就是著名的 万向锁问题（Gimbal Lock） ，在俯仰角为正负90度时，第一次旋转与第三次旋转将使用同一个轴，使得系统失去一个自由度，这被称为 奇异性 问题。

三、四元数

既然我们已经有了旋转向量和欧拉角，为什么还有个四元数（Quaternion）？因为欧拉角和旋转向量具有奇异性（万向锁问题）。不存在不带有奇异性的三维向量描述方式。因此我们需要用到四元数，它既是紧凑的，也没有奇异性。

定义：一个四元数包含一个实部和三个虚部。
$$
q = q_0 + q_1i + q_2j + q_3k = [s,v]
$$
其中，后面的等式将四元数表达成一个标量和一个向量， $i,j,k$ 表示四元数的三个虚部，满足：

若一个四元数虚部全为0，则它是一个实四元数；如其实部为零，则称它为虚四元素。而且，一个虚四元数对应一个空间点。

我们能用单位四元数来表示三维空间中的任意一个旋转。我们先考虑下复数。在负数中，乘以i 表示在复平面内旋转90度。但在四元数中，情形却有微妙的不同：乘以i 表示旋转180度，这样才能保证 $ij=-k$ 的性质。而 $i^2 = -1$ ，说明绕i 轴旋转360度后得到一个相反的东西，而要旋转720度（两周）才能得到它原先的样子。

假设某个旋转的旋转向量为 $ \theta n$ , 则
$$
q=[cos \frac {\theta}{2} ,n_x sin \frac {\theta}{2}, n_ysin \frac {\theta}{2},n_zsin\frac {\theta}{2}]^T
$$
反之则有
$$
\theta = 2 arccosq_0
$$

$$
[n_x,n_y,n_z]^T=[q_1,q_2,q_3]^T/sin \frac {\theta}{2}
$$

上式给人一种“转了一半”的感觉。将上式中的 $\theta$ 加上 $2\pi$ 后得到一个相同的旋转，但是对应的四元数却变成了 $-q$ . 所以，在四元数中，任意的旋转都可以由两个互为相反数的四元数表示。

而四元数和旋转矩阵的关系为：