【SLAM数学基础】旋转的三种数学描述及其转换
1. 旋转矩阵
所有的三维旋转矩阵组成了特殊正交群（Special Orthogonal Group）SO(3)，它是一个3x3的实数矩阵，满足：
- 旋转矩阵为正交矩阵： R ⋅ R T = I , R − 1 = R T ; R\cdot R^T = I,~ R^{-1} = R^T; R⋅RT=I, R−1=RT;
- 旋转矩阵的行列式为1： det ( R ) = 1 \det(R)=1 det(R)=1.
同时，一个旋转矩阵也可以转换为四元数或旋转矢量来描述，下面会进行详细介绍。
2. 旋转矢量与旋转矩阵之间的相互转换
旋转矢量也称为角轴（Angle Axis），本身也是SO(3)对应的李代数 s o ( 3 ) \mathfrak{so}(3) so(3)。由于 s o ( 3 ) \mathfrak{so}(3) so(3) 是SO(3)的切空间，所以旋转矢量也可以用于表达角速度。
假设一个旋转矢量为 w ∈ R 3 \bm{w} \in R^3 w∈R3，且可以按照方向和大小分解为 w = θ n \bm{w} =\theta\bm{n} w=θn，那么从旋转矢量到旋转矩阵之间的转换关系，可以由罗德里格斯公式(Rodrigues’ Formula)或者SO(3)上的指数映射(Exponential Map)来描述：
R = cos θ ⋅ I + ( 1 − cos θ ) n n T + sin θ ⋅ n ∧ = exp ( w ∧ ) R=\cos\theta \cdot \bm{I} + (1-\cos \theta) \bm{n}\bm{n}^T + \sin\theta\cdot \bm{n}^{\wedge} = \exp(\bm{w}^{\wedge}) R=cosθ⋅I+(1−cosθ)nnT+sinθ⋅n∧=exp(w∧)
此处的 exp \exp exp也可以由泰勒展开化简后得到左侧公式。为了简化符号，通常可以记大写的 Exp \text{Exp} Exp为：
Exp ( w ) = exp ( w ∧ ) \text{Exp}(\bm{w}) = \exp(\bm{w}^{\wedge}) Exp(w)=exp(w∧)
反过来，从旋转矩阵到旋转矢量之间的关系可以由对数映射描述:
w = log ( R ) ∨ = Log ( R ) \bm{w} = \log(R)^{\vee} = \text{Log}(R) w=log(R)∨=Log(R)
其角轴的角度 θ \theta θ 计算方法为：
θ = arccos ( t r ( R ) − 1 2 ) \theta = \arccos\left(\frac{tr(R) - 1}{2}\right) θ=arccos(2tr(R)−1)
而其转轴 n \bm{n} n 则是矩阵 R R R 特征值1对应的单位特征向量：
R n = n R\bm{n} = \bm{n} Rn=n
求解此方程，再归一化就得到了旋转转轴。
3. 四元数
三维旋转也可以由单位四元数来描述。四元数是一种扩展的复数，由一个实部和三个虚部构成。它既是紧凑的，也没有奇异性，只需要存储4个数，因此在SLAM的工程代码实践中经常使用四元数来表示三维旋转。
Hamilton四元数是一种最常用、最直观的定义方式：
q = q 0 + q 1 i + q 2 j + q 3 k \bm{q}=q_0 +q_1 i + q_2 j + q_3 k q=q0+q1i+q2j+q3k
其中 q 0 q_0 q0为实部， q 1 , q 2 , q 3 q_1,~q_2,~q_3 q1, q2, q3为虚部。为了简化符号，可以把三个虚部元素记为虚部的矢量，那么四元数可以有标量部分 s s s 加矢量部分 v \bm{v} v 构成：
q = [ s , v ] T \bm{q}=[s, \bm{v}]^T q=[s,v]T
3.1 四元数的基本性质
四元数的基本性质包括加法、乘法、模长、共轭、逆、数乘等，这部分内容在网上的教程较多，在这里就不赘述，推荐一个博客：四元数的定义与属性。
3.2 四元数描述旋转
可以用四元数描述对一个点的旋转。
假设有一个空间三维点 p = [ x , y , z ] ∈ R 3 \bm{p} = [x,y,z] \in R^3 p=[x,y,z]∈R3，以及一个由单位四元数 q \bm{q} q指定的旋转，三维点经过旋转之后变为 p ′ \bm{p}' p′，如果使用矩阵描述，则有
p ′ = R p \bm{p}' = R \bm{p} p′=Rp
下面介绍如何使用四元数描述这个旋转关系。
用一个虚四元数来描述三维空间点：
p = [ 0 , x , y , z ] T = [ 0 , v ] T \bm{p} = [0, x, y, z]^T = [0, \bm{v}]^T p=[0,x,y,z]T=[0,v]T
相当于把四元数的三个虚部与空间的3个轴相对应。那么旋转后的点 p ′ \bm{p}' p′即可表示为：
p ′ = q p q − 1 \bm{p}' = \bm{q} \bm{p} \bm{q}^{-1} p′=qpq−1
这里的乘法均为四元数乘法，结果也是四元数，最后把 p ′ \bm{p}' p′ 的虚部去除，即得到旋转之后的坐标。
4. 四元数与旋转矩阵、旋转矢量之间的变换
由于本篇博客主要是为了便于后续使用的时候查阅，所以下面的过程都跳过数学推导而直接给出结论。
4.1 四元数与旋转矢量之间的相互变换
设四元数为 q = [ s , v ] T \bm{q}=[s, \bm{v}]^T q=[s,v]T，对应的旋转矢量为 w = θ ⋅ n = θ ⋅ [ n x , n y , n z ] T \bm{w} =\theta\cdot \bm{n} = \theta\cdot[n_x, n_y, n_z]^T w=θ⋅n=θ⋅[nx,ny,nz]T，则从四元数到旋转矢量之间的变换关系：
θ = 2 arccos s [ n x , n y , n z ] T = v T sin θ 2 \theta = 2 \arccos s \\ [n_x, n_y, n_z]^T = \frac{\bm{v}^T}{\sin\frac{\theta}{2}} θ=2arccoss[nx,ny,nz]T=sin2θvT
反之，从旋转矢量到四元数之间的变换关系：
s = cos θ 2 v = n ⋅ sin θ 2 s = \cos\frac{\theta}{2} \\ \bm{v} = \bm{n} \cdot \sin\frac{\theta}{2} s=cos2θv=n⋅sin2θ
4.2 四元数到旋转矩阵之间的相互变换
设四元数为 q = [ s , v ] T \bm{q}=[s, \bm{v}]^T q=[s,v]T，则从四元数到旋转矩阵之间的变换关系：
R = v v T + s 2 I + 2 s v ∧ + ( v ∧ ) 2 R = \bm{v} \bm{v}^T + s^2 \bm{I} + 2s \bm{v}^{\wedge} + (\bm{v}^{\wedge})^2 R=vvT+s2I+2sv∧+(v∧)2
反之，从旋转矩阵到四元数之间的变换关系可以先参考第2节将旋转矩阵 R R R 转化为旋转矢量 w = θ ⋅ n \bm{w} =\theta\cdot \bm{n} w=θ⋅n，再参考4.1节将旋转矢量转换为四元数 q = [ s , v ] T \bm{q}=[s, \bm{v}]^T q=[s,v]T，
R ⇒ w = θ ⋅ n ⇒ s = cos θ 2 , v = n ⋅ sin θ 2 R\quad \Rightarrow \quad \bm{w} =\theta\cdot \bm{n} \quad \Rightarrow \quad s = \cos\frac{\theta}{2}, ~\bm{v} = \bm{n}\cdot\sin\frac{\theta}{2} R⇒w=θ⋅n⇒s=cos2θ, v=n⋅sin2θ
参考教程
- 自动驾驶与机器人中的SLAM技术：从理论到实践/高翔著. —北京：电子工业出版社，2023.8
- 视觉SLAM十四讲：从理论到实践/高翔等著.— 2版. —北京：电子工业出版社，2019.9
- 四元数的定义与属性