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，文章来源：https://www.toymoban.com/news/detail-729841.html
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θ文章来源地址https://www.toymoban.com/news/detail-729841.html
参考教程
- 自动驾驶与机器人中的SLAM技术：从理论到实践/高翔著. —北京：电子工业出版社，2023.8
- 视觉SLAM十四讲：从理论到实践/高翔等著.— 2版. —北京：电子工业出版社，2019.9
- 四元数的定义与属性
到了这里，关于【SLAM数学基础】旋转的三种数学描述及其转换的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！
