【SLAM数学基础】旋转的三种数学描述及其转换

1. 旋转矩阵

所有的三维旋转矩阵组成了特殊正交群（Special Orthogonal Group）SO(3)，它是一个3x3的实数矩阵，满足：

• 旋转矩阵为正交矩阵：$R\cdot R^T=I,R^{-1}=R^T;$
• 旋转矩阵的行列式为1：$\det (R)=1$.

同时，一个旋转矩阵也可以转换为四元数或旋转矢量来描述，下面会进行详细介绍。

2. 旋转矢量与旋转矩阵之间的相互转换

旋转矢量也称为角轴（Angle Axis），本身也是SO(3)对应的李代数 $\mathfrak{so}(3)$。由于 $\mathfrak{so}(3)$ 是SO(3)的切空间，所以旋转矢量也可以用于表达角速度。

假设一个旋转矢量为 $\mathbf{w}\in R^3$，且可以按照方向和大小分解为 $\mathbf{w}=\theta \mathbf{n}$，那么从旋转矢量到旋转矩阵之间的转换关系，可以由罗德里格斯公式(Rodrigues’ Formula)或者SO(3)上的指数映射(Exponential Map)来描述：
$$R=\cos \theta \cdot \mathbf{I}+(1-\cos \theta )\mathbf{n}{\mathbf{n}}^T+\sin \theta \cdot {\mathbf{n}}^{\wedge }=\exp ({\mathbf{w}}^{\wedge })$$
此处的$\exp$也可以由泰勒展开化简后得到左侧公式。为了简化符号，通常可以记大写的 $\text{Exp}$为：
$$\text{Exp}(\mathbf{w})=\exp ({\mathbf{w}}^{\wedge })$$

反过来，从旋转矩阵到旋转矢量之间的关系可以由对数映射描述:
$$\mathbf{w}=\log (R{)}^{\vee }=\text{Log}(R)$$
其角轴的角度 $\theta$ 计算方法为：
$$\theta =\arccos \left(\frac{tr(R)-1}{2}\right)$$
而其转轴 $\mathbf{n}$ 则是矩阵 $R$ 特征值1对应的单位特征向量：
$$R\mathbf{n}=\mathbf{n}$$
求解此方程，再归一化就得到了旋转转轴。

3. 四元数

三维旋转也可以由单位四元数来描述。四元数是一种扩展的复数，由一个实部和三个虚部构成。它既是紧凑的，也没有奇异性，只需要存储4个数，因此在SLAM的工程代码实践中经常使用四元数来表示三维旋转。

Hamilton四元数是一种最常用、最直观的定义方式：
$$\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k$$
其中$q_0$为实部，$q_1,q_2,q_3$为虚部。为了简化符号，可以把三个虚部元素记为虚部的矢量，那么四元数可以有标量部分 $s$ 加矢量部分 $\mathbf{v}$ 构成：
$$\mathbf{q}=[s,\mathbf{v}{]}^T$$

3.1 四元数的基本性质

四元数的基本性质包括加法、乘法、模长、共轭、逆、数乘等，这部分内容在网上的教程较多，在这里就不赘述，推荐一个博客：四元数的定义与属性。

3.2 四元数描述旋转

可以用四元数描述对一个点的旋转。
假设有一个空间三维点$\mathbf{p}=[x,y,z]\in R^3$，以及一个由单位四元数$\mathbf{q}$指定的旋转，三维点经过旋转之后变为 ${\mathbf{p}}^{\prime }$，如果使用矩阵描述，则有
$${\mathbf{p}}^{\prime }=R\mathbf{p}$$

下面介绍如何使用四元数描述这个旋转关系。

用一个虚四元数来描述三维空间点：
$$\mathbf{p}=[0,x,y,z{]}^T=[0,\mathbf{v}{]}^T$$
相当于把四元数的三个虚部与空间的3个轴相对应。那么旋转后的点 ${\mathbf{p}}^{\prime }$即可表示为：
$${\mathbf{p}}^{\prime }=\mathbf{q}\mathbf{p}{\mathbf{q}}^{-1}$$
这里的乘法均为四元数乘法，结果也是四元数，最后把 ${\mathbf{p}}^{\prime }$ 的虚部去除，即得到旋转之后的坐标。

4. 四元数与旋转矩阵、旋转矢量之间的变换

由于本篇博客主要是为了便于后续使用的时候查阅，所以下面的过程都跳过数学推导而直接给出结论。

4.1 四元数与旋转矢量之间的相互变换

设四元数为 $\mathbf{q}=[s,\mathbf{v}{]}^T$，对应的旋转矢量为 $\mathbf{w}=\theta \cdot \mathbf{n}=\theta \cdot [n_x,n_y,n_z{]}^T$，则从四元数到旋转矢量之间的变换关系：

$$\begin{gathered} \theta =2\arccos s \\ [n_x,n_y,n_z{]}^T=\frac{{\mathbf{v}}^T}{\sin \frac{\theta }{2}} \end{gathered}$$

反之，从旋转矢量到四元数之间的变换关系：
$$\begin{gathered} s=\cos \frac{\theta }{2} \\ \mathbf{v}=\mathbf{n}\cdot \sin \frac{\theta }{2} \end{gathered}$$

因此四元数可表示为：$\mathbf{q}=[\cos \frac{\theta }{2},\mathbf{n}\cdot \sin \frac{\theta }{2}{]}^T$

4.2 四元数到旋转矩阵之间的相互变换

设四元数为 $\mathbf{q}=[s,\mathbf{v}{]}^T$，则从四元数到旋转矩阵之间的变换关系：

$$R=\mathbf{v}{\mathbf{v}}^T+s^2\mathbf{I}+2s{\mathbf{v}}^{\wedge }+({\mathbf{v}}^{\wedge }{)}^2$$

反之，从旋转矩阵到四元数之间的变换关系可以先参考第2节将旋转矩阵 $R$ 转化为旋转矢量 $\mathbf{w}=\theta \cdot \mathbf{n}$，再参考4.1节将旋转矢量转换为四元数 $\mathbf{q}=[s,\mathbf{v}{]}^T$，

$$R\Rightarrow \mathbf{w}=\theta \cdot \mathbf{n}\Rightarrow s=\cos \frac{\theta }{2},\mathbf{v}=\mathbf{n}\cdot \sin \frac{\theta }{2}$$

参考教程

1. 自动驾驶与机器人中的SLAM技术：从理论到实践/高翔著. —北京：电子工业出版社，2023.8
2. 视觉SLAM十四讲：从理论到实践/高翔等著.— 2版. —北京：电子工业出版社，2019.9
3. 四元数的定义与属性