自动驾驶SLAM基础：几何学与运动学全解析

这份笔记为你详细梳理自动驾驶与机器人SLAM技术中至关重要的“几何学”与“运动学”基础。这部分内容是整个自动驾驶学科的地基，后面的IMU预积分、激光里程计、状态估计（滤波与优化）全都构建于此。我们抛开枯燥的课本口吻，用工程师的视角，把这些概念、定理和应用场景掰开揉碎。

第一部分：几何学 —— 我们如何描述空间与姿态？

在让车动起来之前，我们得先定义清楚“车在哪儿”以及“车上的传感器怎么摆”。这就是几何学的任务。

1. 坐标系与向量运算

在一个三维欧几里得空间里，我们通常用三个相互垂直的单位向量（比如 $e_1,e_2,e_3$）来搭一个参考系。自动驾驶系统里，我们通常会定义好这几个坐标系：

• 世界坐标系（惯性系）：一张固定的“地图”，用来描述车辆的全局位姿。
• 车体坐标系：固连在车身上的参考系。通常我们规定X轴向前、Y轴向左、Z轴向上（前左上），并满足右手定则。
• 传感器坐标系：像相机、激光雷达都有自己的习惯。比如相机常用“右下前”。

坐标变换是几何学的核心。假设同一个点，在世界系下坐标是 $p_w$，在车体系下是 $p_b$，它们的关系由一个旋转矩阵 $R_{wb}$ 和平移向量 $t_{wb}$ 确定：

$$p_w=R_{wb}{p}_b+t_{wb}$$

这里我们必须养成一个好习惯：下标从右往左读。 $R_{wb}$ 的意思是，把车体系（b）里的坐标，左乘它，就变成了世界系（w）里的坐标。这样写代码和推公式时思路最顺畅。如果写成齐次形式（变换矩阵 $T$），就是：

$$p_w=\left[\begin{array}{cc}{R}_{wb}& t_{wb}\\ 0& 1\end{array}\right]p_b$$

反对称矩阵算子（${}^{\wedge }$）：
在推导中，向量外积 $a\times b$ 经常被写成矩阵乘法 $a^{\wedge }b$。这个 $a^{\wedge }$ 就是把向量变成反对称矩阵：

a∧=[0−a3a2a30−a1−a2a10] a^\\wedge = \\begin{bmatrix} 0 & -a_3 & a_2 \\\\ a_3 & 0 & -a_1 \\\\ -a_2 & a_1 & 0 \\end{bmatrix} a∧=​0a3​−a2​​−a3​0a1​​a2​−a1​0​​

这个符号在后面推导角速度、李代数时会像胶水一样，把各种叉乘运算粘成简洁的矩阵形式。

2. 描述旋转的三把“武器”

在程序中表示一个旋转，我们主要用三种方式，它们各有所长：

第一把武器：旋转矩阵（$SO(3)$）
旋转矩阵是一个 $3\times 3$ 的正交矩阵，行列式为1。它直观，但占了9个数值，有冗余约束。它属于“特殊正交群 $SO(3)$”。

第二把武器：旋转矢量（角轴 / 李代数 $\mathfrak{so}(3)$）
想象拧螺丝：拧动的角度 $\theta$ 和螺丝刀的方向 $n$，合起来就是旋转矢量 $w=\theta n$。它只有3个数值，没有约束。
它通过罗德里格斯公式（指数映射）和旋转矩阵互转：

$$R=\exp (w^{\wedge })=\cos \theta I+(1-\cos \theta )n{n}^{\top }+\sin \theta n^{\wedge }$$

反过来，从旋转矩阵求旋转矢量（对数映射），角度 $\theta$ 是：

$$\theta =\arccos \left(\frac{\mathrm{tr}(R)-1}{2}\right)$$

旋转轴 $n$ 就是 $R$ 对应特征值1的那个单位特征向量。
应用场景：在图优化后端，我们喜欢把它作为优化变量，因为它无约束，随便加减，求导方便。

第三把武器：四元数
一个实部加三个虚部：$q=q_0+q_{1}i+q_{2}j+q_{3}k=[s,v{]}^{\top }$。它只需要4个数，计算效率高，且不会像欧拉角那样遇到“万向锁”。
用四元数来旋转一个空间点 $p$（把它当成虚四元数 $[0,v{]}^{\top }$），公式是乘两遍：

$$p^{\prime }=qp{q}^{-1}$$

四元数和旋转矩阵/旋转矢量的转换关系如下。四元数到旋转矩阵：

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

四元数到旋转矢量：

$$\theta =2\arccos s,n=\frac{v}{\sin (\theta /2)}$$

应用场景：工程落地的首选。你车上的组合导航、开源SLAM框架（如LOAM、VINS）底层存储姿态几乎都用四元数。

3. 李群、李代数与BCH公式

我们可以把 $SO(3)$ 看作一个弯曲的“旋转小区”（李群），而 $\mathfrak{so}(3)$ 是这个小区每个点上的“平坦切平面”（李代数）。指数映射和对数映射就是连接两者的桥梁。

BCH公式（Baker-Campbell-Hausdorff） 是这里的重头戏。它解决的是：在旋转矩阵上乘一个微小增量，对应到李代数上加一个增量，这两者怎么换算？
一阶线性近似下，它们通过一个雅可比矩阵 $J_r$ 或 $J_l$ 联系：

$$\exp (\Delta {\varphi }^{\wedge })\exp ({\varphi }^{\wedge })=\exp ((\varphi +J_l^{-1}(\varphi )\Delta \varphi {)}^{\wedge })$$

应用场景：这是IMU预积分的灵魂。当我们在优化算法里微调某一帧的位姿时，需要快速知道这个微调会怎么影响前后帧的IMU测量残差，这个“影响关系”就是通过BCH的雅可比传递的。

第二部分：运动学 —— 车动起来之后，数学怎么追？

有了静态的几何描述，我们来看看这些量随时间变化时，会产生什么物理规律。这是连接传感器数据和车辆状态的桥梁。

1. 泊松方程：角速度的源头

我们知道旋转矩阵满足 $R^{\top }R=I$。两边对时间求导，会得到一个非常优美的方程：

$${\dot{R}}^{\top }R+R^{\top }\dot{R}=0\Rightarrow R^{\top }\dot{R}=-(R^{\top }\dot{R}{)}^{\top }$$

这说明 $R^{\top }\dot{R}$ 是一个反对称矩阵。于是我们可以定义瞬时角速度 $\omega$，令 ${\omega }^{\wedge }=R^{\top }\dot{R}$。这就引出了泊松方程：

$$\dot{R}=R{\omega }^{\wedge }$$

$\omega$ 就是陀螺仪直接测量的物理量。这个方程的解非常漂亮，它直接是指数映射的形式：

$$R(t)=R(t_0)\exp ({\omega }^{\wedge }\Delta t)$$

离散化后，就是当前旋转乘上一个由角速度算出来的小增量。这是IMU进行姿态积分的核心代码逻辑。

2. 四元数运动学：那个关键的“1/2”系数

如果我们用四元数来表示旋转，对单位性约束 $q^{\ast }q=1$ 求导，可以得到四元数的运动学方程：

$$\dot{q}=q\varpi$$

其中 $\varpi$ 是纯虚四元数。为了让四元数的角速度 $\omega$ 和 $SO(3)$ 里的 $\omega$ 统一物理意义，我们把它改写为：

$$\dot{q}=\frac{1}{2}q[0,\omega {]}^{\top }$$

为什么会有个 $1/2$ ？
因为四元数旋转一个点要乘两遍（$qp{q}^{-1}$），所以它的“旋转速度”天然就是 $SO(3)$ 里的一半。这个 $1/2$ 是连接两种数学语言的桥梁。
在代码里，如果用四元数更新姿态，离散公式就是：

$$q_{\text{new}}\approx q_{\text{old}}\otimes \left[1,\frac{1}{2}\omega \Delta t\right]$$

注意：长时间更新后一定要对四元数重新归一化，否则数值误差会累积导致它不是单位四元数。而旋转矩阵就不存在这个问题，因为 $\exp ({\omega }^{\wedge })$ 天生就在 $SO(3)$ 上。

3. 线速度与加速度：没你想的那么简单

这是区分“理论派”和“实战派”的关键。同一个空间点，在两个有相对旋转的坐标系下，速度是不一样的。
假设坐标系1和2，点 $p$ 在2系下的坐标是 $p_2$，2系相对于1系有角速度 $\omega$。那么 $p$ 在1系下的速度 $v_1$ 是：

$$v_1=R_{12}({\omega }^{\wedge }{p}_2+v_2)$$

多出来的 ${\omega }^{\wedge }{p}_2$ 就是旋转带来的附加速度。

加速度就更复杂了，会多出三项科氏、向心等加速度：

$$a_1=R_{12}(a_2+\underset{\text{科氏加速度}}{2{\omega }^{\wedge }{v}_2}+\underset{\text{角加速度项}}{{\dot{\omega }}^{\wedge }{p}_2}+\underset{\text{向心加速度}}{{\omega }^{\wedge }{\omega }^{\wedge }{p}_2})$$

应用场景：你车上的IMU（加速度计）测到的根本不是单纯的车在世界上跑的加速度。它测的是“比力”，里面包含了重力、车自身的线加速度，以及上面这一堆乱七八糟的科氏力、向心力。在做GNSS/IMU组合导航时，这些项一个都不能少，否则车一转弯定位就会往外飘。

4. 扰动模型：求导的“懒人神器”

当我们在SLAM后端写优化代码时，经常要对旋转矩阵 $R$ 求导。如果直接对9个矩阵元素求导，雅可比矩阵是 $9\times 9$ 的，算起来想死。
扰动模型说：别直接动 $R$，我们在它右边（或左边）乘一个微小的扰动 $\varphi$，然后对 $\varphi$ 求导。这样雅可比就是 $3\times 3$ 的，非常简洁。
比如，对旋转后的点 $Ra$ 关于旋转 $R$ 求右扰动的雅可比：

$$\frac{\partial Ra}{\partial \varphi }=\underset{\varphi \to 0}{\lim }\frac{R(I+{\varphi }^{\wedge })a-Ra}{\varphi }=-R{a}^{\wedge }$$

这个 $-R{a}^{\wedge }$ 会在你写视觉重投影误差、激光点云匹配误差的雅可比时反复出现。记住这个模式，能让你手写优化器时事半功倍。

总结：这些知识如何服务于你的自动驾驶与机器人目标？

建议学习者尝试编写一段简单的运动学仿真代码（比如模拟车辆以固定线速度和角速度画圆），分别用旋转矩阵和四元数来实现更新，观察它们的轨迹是否重合。