【视觉SLAM:二、三维空间刚体运动】
刚体运动描述的是三维空间中刚体的旋转和平移。SLAM、机器人学、计算机视觉等领域中，经常需要用数学工具表达刚体的旋转与位姿变换。以下从旋转矩阵、旋转向量和欧拉角、四元数、相似、仿射与射影变换，以及可视化演示五个方面详细介绍三维刚体运动的相关内容。
旋转矩阵
旋转矩阵是一种常用的刚体旋转表示方法。它在数学上是一个正交矩阵，维度为3×3，表示坐标系的旋转变换。
点、向量和坐标系
- 在三维空间中，点或向量的位置用坐标表示。
- 坐标是相对于某个参考坐标系的。如果参考坐标系发生旋转或平移，则点的坐标表示也会发生变化。
坐标系间的欧式变换
-
欧式变换是刚体运动中最基本的变换，它包括旋转和平移。
-
数学表示为： p ′ = R p + t \mathbf{p}^{\prime}=\mathbf{Rp}+\mathbf{t} p′=Rp+t
其中：- R ∈ R 3 × 3 \mathbf{R}\in\mathbb{R}^{3\times3} R∈R3×3：旋转矩阵，表示坐标系的旋转。
- t ∈ R 3 \mathbf{t}\in\mathbb{R}^3 t∈R3：平移向量，表示坐标系的平移。
-
变换矩阵与齐次坐标
- 为了更方便地描述旋转和平移，可以使用齐次坐标和变换矩阵：
T
=
[
R
t
0
⊤
1
]
,
p
′
=
T
⋅
p
homogeneous
\mathbf{T}=\begin{bmatrix}\mathbf{R}&\mathbf{t}\\\mathbf{0}^\top&1\end{bmatrix},\quad\mathbf{p}^{\prime}=\mathbf{T}\cdot\mathbf{p}_{\text{homogeneous}}
T=[R0⊤t1],p′=T⋅phomogeneous
其中：- T ∈ S E ( 3 ) \mathbf{T}\in SE(3) T∈SE(3)：变换矩阵，包含旋转和平移。
- 齐次坐标 p homogeneous = [ x , y , z , 1 ] ⊤ . \mathbf{p}_{\text{homogeneous}}=[x,y,z,1]^{\top}. phomogeneous=[x,y,z,1]⊤.
- 变换矩阵的优点：
- 统一了旋转和平移的表示。
- 通过矩阵乘法可以连续组合多个变换。
- 为了更方便地描述旋转和平移，可以使用齐次坐标和变换矩阵：
T
=
[
R
t
0
⊤
1
]
,
p
′
=
T
⋅
p
homogeneous
\mathbf{T}=\begin{bmatrix}\mathbf{R}&\mathbf{t}\\\mathbf{0}^\top&1\end{bmatrix},\quad\mathbf{p}^{\prime}=\mathbf{T}\cdot\mathbf{p}_{\text{homogeneous}}
T=[R0⊤t1],p′=T⋅phomogeneous
旋转向量和欧拉角
旋转向量
- 定义：旋转向量𝜔是旋转轴𝑛和旋转角度𝜃的组合，用于表示三维空间中的旋转： ω = θ n , ∥ n ∥ = 1 \omega=\theta\mathbf{n},\quad\|\mathbf{n}\|=1 ω=θn,∥n∥=1
- 作用：旋转向量是一种紧凑的旋转表示方法（仅用 3 个参数），可以方便地转换为旋转矩阵或四元数。
- 从旋转向量到旋转矩阵：
- 通过 Rodrigues 公式将旋转向量𝜔转化为旋转矩阵：
R
=
I
+
sin
θ
n
^
+
(
1
−
cos
θ
)
n
^
2
\mathbf{R}=\mathbf{I}+\sin\theta\mathbf{\hat{n}}+(1-\cos\theta)\mathbf{\hat{n}}^2
R=I+sinθn^+(1−cosθ)n^2
其中， n ^ \mathbf{\hat{n}} n^是旋转轴的反对称矩阵。
- 通过 Rodrigues 公式将旋转向量𝜔转化为旋转矩阵：
R
=
I
+
sin
θ
n
^
+
(
1
−
cos
θ
)
n
^
2
\mathbf{R}=\mathbf{I}+\sin\theta\mathbf{\hat{n}}+(1-\cos\theta)\mathbf{\hat{n}}^2
R=I+sinθn^+(1−cosθ)n^2
欧拉角
- 定义：欧拉角是通过三个角度表示三维旋转，通常是绕固定坐标轴（如 XYZ）的旋转：
- 绕𝑍轴旋转角度 𝛼（Yaw）。
- 绕𝑌轴旋转角度𝛽（Pitch）。
- 绕𝑋轴旋转角度𝛾（Roll）。
- 优点：直观易理解，适合描述旋转的物理意义。
- 缺点：存在奇异性（万向锁问题），即在某些角度下无法唯一表示旋转。
四元数
四元数是一种代数结构，用于表示和计算三维旋转。相比旋转矩阵和欧拉角，四元数具有更好的数值稳定性和计算效率。
四元数的定义
- 四元数由一个实数部分和一个三维向量部分组成：
q
=
[
w
,
x
,
y
,
z
]
,
w
∈
R
,
v
=
[
x
,
y
,
z
]
∈
R
3
\mathbf{q}=[w,x,y,z],\quad w\in\mathbb{R},\quad\mathbf{v}=[x,y,z]\in\mathbb{R}^3
q=[w,x,y,z],w∈R,v=[x,y,z]∈R3
或记为： q = w + v \mathbf{q}=w+\mathbf{v} q=w+v
四元数的运算
- 加法：按分量相加。
- 乘法：定义特殊的乘法规则，非交换： q 1 ⋅ q 2 = ( w 1 w 2 − v 1 ⋅ v 2 ) + ( w 1 v 2 + w 2 v 1 + v 1 × v 2 ) \mathbf{q}_1\cdot\mathbf{q}_2=(w_1w_2-\mathbf{v}_1\cdot\mathbf{v}_2)+(w_1\mathbf{v}_2+w_2\mathbf{v}_1+\mathbf{v}_1\times\mathbf{v}_2) q1⋅q2=(w1w2−v1⋅v2)+(w1v2+w2v1+v1×v2)
用四元数表示旋转
- 旋转角度𝜃和旋转轴𝑛对应的四元数表示为： q = [ cos θ 2 , sin θ 2 n ] \mathbf{q}=\left[\cos\frac{\theta}{2},\sin\frac{\theta}{2}\mathbf{n}\right] q=[cos2θ,sin2θn]
- 旋转公式：
p
′
=
q
⋅
p
⋅
q
−
1
\mathbf{p}^{\prime}=\mathbf{q}\cdot\mathbf{p}\cdot\mathbf{q}^{-1}
p′=q⋅p⋅q−1
其中，𝑝是点的四元数表示。
四元数到其他旋转表示的转换
- 四元数→旋转矩阵： R = [ 1 − 2 y 2 − 2 z 2 2 x y − 2 z w 2 x z + 2 y w 2 x y + 2 z w 1 − 2 x 2 − 2 z 2 2 y z − 2 x w 2 x z − 2 y w 2 y z + 2 x w 1 − 2 x 2 − 2 y 2 ] \mathbf{R}=\begin{bmatrix}1-2y^2-2z^2&2xy-2zw&2xz+2yw\\2xy+2zw&1-2x^2-2z^2&2yz-2xw\\2xz-2yw&2yz+2xw&1-2x^2-2y^2\end{bmatrix} R= 1−2y2−2z22xy+2zw2xz−2yw2xy−2zw1−2x2−2z22yz+2xw2xz+2yw2yz−2xw1−2x2−2y2
- 四元数→欧拉角：通过数学推导计算出对应角度。
相似、仿射与射影变换
相似变换
- 相似变换是刚体运动加上一个全局缩放因子𝑠： p ′ = s R p + t \mathbf{p}^{\prime}=s\mathbf{Rp}+\mathbf{t} p′=sRp+t
仿射变换
- 仿射变换不仅包含旋转和平移，还可以引入剪切、缩放等非刚体变换：
p ′ = A p + t \mathbf{p}^{\prime}=\mathbf{Ap}+\mathbf{t} p′=Ap+t
其中， A ∈ R 3 × 3 \mathbf{A}\in\mathbb{R}^{3\times3} A∈R3×3。
射影变换
- 射影变换允许非线性变换，例如透视投影：
p
′
=
H
p
homogeneous
\mathbf{p}^{\prime}=\mathbf{H}\mathbf{p}_{\text{homogeneous}}
p′=Hphomogeneous
其中， H ∈ R 4 × 4 \mathbf{H}\in\mathbb{R}^{4\times4} H∈R4×4 是投影矩阵。
可视化演示
- 显示运动轨迹
- 可用 Matplotlib 或 Pangolin 绘制三维轨迹，显示机器人在三维空间中的运动路径。
- 显示相机的位姿
- 将相机的位姿表示为一个坐标系，通过旋转矩阵和平移向量绘制相机在空间中的位置和朝向。
- 位姿可视化效果
总结
三维空间中的刚体运动描述了刚体在空间中的旋转和平移。我们通过旋转矩阵、旋转向量和欧拉角、四元数等方式表示旋转，通过变换矩阵统一旋转和平移的表示。进一步，仿射变换和射影变换拓展了变换的范围，可用于更复杂的场景分析与建模。