视觉里程计系列（1）：三维空间中刚体旋转的几种表达方式

三维空间中刚体旋转的几种表达方式

旋转矩阵

旋转矩阵是一个 3 x 3 的矩阵，在 SLAM 编程中使用比较频繁，它主要有如下特点。

• 旋转矩阵不是普通的矩阵，它有比较强的约束条件:旋转矩阵具有正交性它和它的转置矩阵的乘积是单位矩阵，且行列式值为 1。
• 旋转矩阵是可逆矩阵，它的逆矩阵（转置矩阵）表示相反方向的旋转旋转矩阵用9个元素表示3个自由度的旋转，这种表达方式是冗余的。

四元数

四元数由一个实部和三个虚部组成，是一种非常紧凑、没有奇异的表达方式在 SLAM 中应用很广泛。在编程时需要注意以下几点。

• 单位四元数才能描述旋转，所以使用四元数前必须归一化
• 在线性代数库 Eigen 中，一定要注意四元数构造及初始化的实部、虚部的顺序和内部系数存储的顺序不同。

旋转向量

旋转向量用一个旋转轴 $n$ 和旋转角 $\theta$ 描述一个旋转，所以也称轴角。不过很明显，因为旋转角度有一定的周期性（360°一圈），所以这种表达方式具有奇异性。

• 从旋转向量到旋转矩阵的转换过程称为罗德里格斯公式。很多第三方库都提供罗德里格斯函数，如 OpenCV、MATLAB 和 Eigen。
• 旋转向量和旋转矩阵的转换关系，其实对应于李代数和李群的映射，这对于理解李代数很有帮助。

欧拉角

把一次旋转分解成3次绕不同坐标轴的旋转，比如航空领域经常使用的“偏航-俯仰-滚转”（ Yaw，Pitch，Roll）就是一种欧拉角，这种表达方式最大的优势就是直观。

欧拉角在 SLAM 中使用不多，原因是它有一个致命的缺点——万向锁，即在俯仰角为土90°时，第 1 次和第 3 次旋转使用的是同一个坐轴，这样会丢失一个自由度，引起奇异性。事实上，想要无歧义地表达三维旋转，至少需要 4 个变量。

万向锁

1. 静态：即绕世界坐标系三个轴的旋转，由于物体旋转过程中坐标轴保持静止不变，所以称为静态
2. 动态：即绕物体坐标系三个轴的旋转，由于物体旋转过程中坐标轴随着物体做相同的转动，所以被称为动态

动态才有万向锁

代数的角度解释

$$\begin{gathered} R_x(\alpha )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos (\alpha )& -\sin (\alpha )\\ 0& \sin (\alpha )& cos(\alpha )\end{array}\right] \\ {R}_y(\beta )=\left[\begin{array}{ccc}\cos (\beta )& 0& \sin (\beta )\\ 0& 1& 0\\ -\sin (\beta )& 0& cos(\beta )\end{array}\right] \\ {R}_z(\theta )=\left[\begin{array}{ccc}\cos (\theta )& -\sin (\theta )& 0\\ sin(\theta )& \cos (\theta )& 0\\ 0& 0& 1\end{array}\right] \end{gathered}$$

$$\begin{array}{rl}& R_z(\theta )R_y(\frac{\pi }{2})R_x(\alpha )=\\ & =\left[\begin{array}{ccc}0& \cos (\theta )\cdot \sin (\alpha )-\cos (\alpha )\cdot \sin (\theta )& \sin (\alpha )\cdot \sin (\theta )+\cos (\alpha )\cdot \cos (\theta )\\ 0& \sin (\alpha )\cdot \sin (\theta )+\cos (\alpha )\cdot \cos (\theta )& \cos (\alpha )\cdot \sin (\theta )-\cos (\theta )\cdot \sin (\alpha )\\ -1& 0& 0\end{array}\right]\\ & =\left[\begin{array}{ccc}0& \sin (\alpha -\theta )& \cos (\alpha -\theta )\\ 0& \cos (\alpha -\theta )& -\sin (\alpha -\theta )\\ -1& 0& 0\end{array}\right]\\ & =R_y(\frac{\pi }{2})R_x(\alpha -\theta )\end{array}$$

关于 $z$ 的变换消失了。

直观理解

欧拉角描述相对于初始状态的变换，只和最终状态有关，与过程无关。