视觉SLAM十四讲学习笔记——三维空间刚体运动（ch3~旋转向量和欧拉角）

一、旋转向量

有了旋转矩阵来描述旋转，有了变换矩阵描述一个六自由度的三维刚体运动。
$SO(n)$是特殊正交群 (Special Orthogonal Group)：
S O ( n ) = { R ∈ R n × n ∣ R R T = I , det ⁡ ( R ) = 1 } . SO(n)=\{\boldsymbol{R}\in\mathbb{R}^{n\times n}|\boldsymbol{RR}^T=\boldsymbol{I},\det(\boldsymbol{R})=1\}. SO(n)={R∈Rn×n∣RRT=I,det(R)=1}.
$$\left[\begin{array}{ccc}{e}_1^T{e}_1^{{}^{\prime }}& e_1^T{e}_2^{{}^{\prime }}& e_1^T{e}_3^{{}^{\prime }}\\ e_2^T{e}_1^{{}^{\prime }}& e_2^T{e}_2^{{}^{\prime }}& e_2^T{e}_3\\ e_3^T{e}_1^{{}^{\prime }}& e_3^T{e}_2^{{}^{\prime }}& e_3^T{e}_3^{{}^{\prime }}\end{array}\right]$$
但是，矩阵表示方式至少有以下两个缺点:

1. $SO(3)$的旋转矩阵有九个量，但一次旋转只有三个自由度。因此这种表达方式是冗余的。同理，变换矩阵用十六个量表达了六自由度的变换。那么，是否有更紧凑的表示呢?
2. 旋转矩阵自身带有约束:它必须是个正交矩阵，且行列式为 1。变换矩阵也是如此。当我们想要估计或优化一个旋转矩阵/变换矩阵时，这些约束会使得求解变得更困难。

对于坐标系的旋转，任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是，可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量，称为旋转向量(或轴角，Axis-Angle)。这种表示法只需一个三维向量即可描述旋转。同样，对于变换矩阵我们使用一个旋转向量和一个平移向量即可表达一次变换。这时的维数正好是六维。事实上，旋转向量就是下章准备介绍的李代数。所以它的详细内容留到下章，本章内只需知道旋转可以这样表示即可。
旋转向量和旋转矩阵之间是如何转换的呢?假设有一个旋转轴为$n$，角度为$\theta$的旋转，显然，它对应的旋转向量为0n。由旋转向量到旋转矩阵的过程由罗德里格斯公式(Rodrigues’s Formula)表明，转换的结果:
$$\mathbf{R}=\cos \theta I+\left(1-\cos \theta \right){\mathbf{nn}}^T+\sin \theta {\mathbf{n}}^{\wedge }.$$
符号^是向量到反对称的转换符。
$$\mathbf{a}\times \mathbf{b}=\left[\begin{array}{ccc}\mathbf{i}& \mathbf{j}& \mathbf{k}\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}\right]=\left[\begin{array}{c}{a}_2{b}_3-a_3{b}_2\\ a_3{b}_1-a_1{b}_3\\ a_1{b}_2-a_2{b}_1\end{array}\right]=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]\mathbf{b}\triangleq {\mathbf{a}}^{\wedge }\mathbf{b}.$$
反之，我们也可以计算从一个旋转矩阵到旋转向量的转换。对于转角 $\theta$，有:
$$\theta =\arccos (\frac{\mathrm{tr}(\mathbf{R})-1}{2}).$$
推理过程省略。
关于转轴 $n$，由于旋转轴上的向量在旋转后不发生改变，说明
$$\mathbf{Rn}=\mathbf{n}.$$

因此，转轴 n 是矩阵 R特征值 1对应的特征向量。求解此方程，再归一化，就得到了旋转轴。
也可以从“旋转轴经过旋转之后不变”的几何角度看待这个方程。

二 、欧拉角

无论是旋转矩阵、旋转向量，虽然它们能描述旋转，但是非常不直观的。当我们看到一个旋转矩阵或旋转向量时，很难想象出来这个旋转究竟是什么样的。当它们变换时，我们也不知道物体是向哪个方向在转动。而欧拉角则提供了一种非常直观的方式来描述旋转一一它使用了三个分离的转角，把一个旋转分解成三次绕不同轴的旋转当然，由于分解方式有许多种，所以欧拉角也存在着不同的定义方法。比如说，当我先绕X 轴旋转，再绕Y轴，最后绕之轴，就得到了一个 XYZ 轴的旋转。同理，可以定义ZYZ、ZYX 等等旋转方式。
欧拉角当中比较常用的一种，便是用“偏航-俯仰-滚转”(yaw-pitch-roll)三个角度来描述一个旋转的。由于它等价于 ZYX 轴的旋转，我们就以 ZYX 为例。假设一个刚体的前方 (朝向我们的方向)为X轴，右侧为Y轴，上方为之轴，见下图。

那么，ZYX 转角相当于把任意旋转分解成以下三个轴上的转角:

1. 绕物体的Z轴旋转，得到偏航角yaw；
2. 绕旋转之后的Y轴旋转，得到俯仰角pitch；
3. 绕旋转之后的X轴旋转，得到滚转角roll。
   
   此时，我们可以使用$[r,p,y{]}^T$这样一个三维的向量描述任意旋转。可以从这个向量想象出旋转的过程。其他的欧拉角亦是通过这种方式，把旋转分解到三个轴上，得到一个三维的向量，只不过选用的轴，以及选用的顺序不一样。这里介绍的rpy 角是比较常用的一种。不同的欧拉角是按照旋转轴的顺序来称呼的。例如，rpy 角的旋转顺序是 ZYX。同样，也有XYZ,ZYZ 这样欧拉角一但是它们就没有专门的名字了。值得一提的是，大部分领域在使用欧拉角时有各自的坐标方向和顺序上的习惯，不一定和这里说的相同。
   欧拉角的一个重大缺点是会碰到著名的万向锁问题 (Gimbal Lock ):在俯仰角为$\pm 90°$时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度 (由三次旋转变成了两次旋转)。这被称为奇异性问题，在其他形式的欧拉角中也同样存在。理论上可以证明，只要我们想用三个实数来表达三维旋转时，都会不可避免地碰到奇异性问题。由于这种原理，欧拉角不适于插值和迭代，往往只用于人机交互中。我们也很少在 SLAM 程序中直接使用欧拉角表达姿态，同样不会在滤波或优化中使用欧拉角表达旋转(因为它具有奇异性)。不过，若你想验证自己算法是否有错时，转换成欧拉角能够快速辨认结果的正确与否