这份笔记为你详细梳理自动驾驶与机器人SLAM技术中至关重要的"几何学"与"运动学"基础。这部分内容是整个自动驾驶学科的地基，后面的IMU预积分、激光里程计、状态估计（滤波与优化）全都构建于此。我们抛开枯燥的课本口吻，用工程师的视角，把这些概念、定理和应用场景掰开揉碎。
第一部分：几何学 ------ 我们如何描述空间与姿态？
在让车动起来之前，我们得先定义清楚"车在哪儿"以及"车上的传感器怎么摆"。这就是几何学的任务。
1. 坐标系与向量运算
在一个三维欧几里得空间里，我们通常用三个相互垂直的单位向量（比如 e1,e2,e3e_1, e_2, e_3e1,e2,e3）来搭一个参考系。自动驾驶系统里，我们通常会定义好这几个坐标系：
- 世界坐标系（惯性系）：一张固定的"地图"，用来描述车辆的全局位姿。
- 车体坐标系：固连在车身上的参考系。通常我们规定X轴向前、Y轴向左、Z轴向上（前左上），并满足右手定则。
- 传感器坐标系：像相机、激光雷达都有自己的习惯。比如相机常用"右下前"。
坐标变换 是几何学的核心。假设同一个点，在世界系下坐标是 pwp_wpw，在车体系下是 pbp_bpb，它们的关系由一个旋转矩阵 RwbR_{wb}Rwb 和平移向量 twbt_{wb}twb 确定：
pw=Rwbpb+twb p_w = R_{wb} p_b + t_{wb} pw=Rwbpb+twb
这里我们必须养成一个好习惯：下标从右往左读。 RwbR_{wb}Rwb 的意思是，把车体系（b）里的坐标，左乘它，就变成了世界系（w）里的坐标。这样写代码和推公式时思路最顺畅。如果写成齐次形式（变换矩阵 TTT），就是：
pw=Rwbtwb01pb p_w = \begin{bmatrix} R_{wb} & t_{wb} \\ 0 & 1 \end{bmatrix} p_b pw=Rwb0twb1pb
反对称矩阵算子（∧^\wedge∧） ：
在推导中，向量外积 a×ba \times ba×b 经常被写成矩阵乘法 a∧ba^\wedge ba∧b。这个 a∧a^\wedgea∧ 就是把向量变成反对称矩阵：
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−a30a1a2−a10
这个符号在后面推导角速度、李代数时会像胶水一样，把各种叉乘运算粘成简洁的矩阵形式。
2. 描述旋转的三把"武器"
在程序中表示一个旋转，我们主要用三种方式，它们各有所长：
第一把武器：旋转矩阵（SO(3)SO(3)SO(3)）
旋转矩阵是一个 3×33\times33×3 的正交矩阵，行列式为1。它直观，但占了9个数值，有冗余约束。它属于"特殊正交群 SO(3)SO(3)SO(3)"。
第二把武器：旋转矢量（角轴 / 李代数 so(3)\mathfrak{so}(3)so(3)）
想象拧螺丝：拧动的角度 θ\thetaθ 和螺丝刀的方向 nnn，合起来就是旋转矢量 w=θnw = \theta nw=θn。它只有3个数值，没有约束。
它通过罗德里格斯公式（指数映射）和旋转矩阵互转：
R=exp(w∧)=cosθI+(1−cosθ)nn⊤+sinθn∧ R = \exp(w^\wedge) = \cos\theta I + (1-\cos\theta)nn^\top + \sin\theta n^\wedge R=exp(w∧)=cosθI+(1−cosθ)nn⊤+sinθn∧
反过来，从旋转矩阵求旋转矢量（对数映射），角度 θ\thetaθ 是：
θ=arccos(tr(R)−12) \theta = \arccos\left( \frac{\operatorname{tr}(R)-1}{2} \right) θ=arccos(2tr(R)−1)
旋转轴 nnn 就是 RRR 对应特征值1的那个单位特征向量。
应用场景：在图优化后端，我们喜欢把它作为优化变量，因为它无约束，随便加减，求导方便。
第三把武器：四元数
一个实部加三个虚部：q=q0+q1i+q2j+q3k=s,v⊤q = q_0 + q_1 i + q_2 j + q_3 k = s, v^\topq=q0+q1i+q2j+q3k=s,v⊤。它只需要4个数，计算效率高，且不会像欧拉角那样遇到"万向锁" 。
用四元数来旋转一个空间点 ppp（把它当成虚四元数 0,v⊤0, v^\top0,v⊤），公式是乘两遍：
p′=qpq−1 p' = q p q^{-1} p′=qpq−1
四元数和旋转矩阵/旋转矢量的转换关系如下。四元数到旋转矩阵：
R=vv⊤+s2I+2sv∧+(v∧)2 R = vv^\top + s^2 I + 2sv^\wedge + (v^\wedge)^2 R=vv⊤+s2I+2sv∧+(v∧)2
四元数到旋转矢量：
θ=2arccoss,n=vsin(θ/2) \theta = 2\arccos s, \quad n = \frac{v}{\sin(\theta/2)} θ=2arccoss,n=sin(θ/2)v
应用场景：工程落地的首选。你车上的组合导航、开源SLAM框架（如LOAM、VINS）底层存储姿态几乎都用四元数。
3. 李群、李代数与BCH公式
我们可以把 SO(3)SO(3)SO(3) 看作一个弯曲的"旋转小区"（李群），而 so(3)\mathfrak{so}(3)so(3) 是这个小区每个点上的"平坦切平面"（李代数）。指数映射和对数映射就是连接两者的桥梁。
BCH公式（Baker-Campbell-Hausdorff） 是这里的重头戏。它解决的是：在旋转矩阵上乘一个微小增量，对应到李代数上加一个增量，这两者怎么换算？
一阶线性近似下，它们通过一个雅可比矩阵 JrJ_rJr 或 JlJ_lJl 联系：
exp(Δϕ∧)exp(ϕ∧)=exp((ϕ+Jl−1(ϕ)Δϕ)∧) \exp(\Delta\phi^\wedge)\exp(\phi^\wedge) = \exp((\phi + J_l^{-1}(\phi)\Delta\phi)^\wedge) exp(Δϕ∧)exp(ϕ∧)=exp((ϕ+Jl−1(ϕ)Δϕ)∧)
应用场景：这是IMU预积分的灵魂。当我们在优化算法里微调某一帧的位姿时，需要快速知道这个微调会怎么影响前后帧的IMU测量残差，这个"影响关系"就是通过BCH的雅可比传递的。
第二部分：运动学 ------ 车动起来之后，数学怎么追？
有了静态的几何描述 ，我们来看看这些量随时间变化时，会产生什么物理规律。这是连接传感器数据和车辆状态的桥梁。
1. 泊松方程：角速度的源头
我们知道旋转矩阵满足 R⊤R=IR^\top R = IR⊤R=I。两边对时间求导，会得到一个非常优美的方程：
R˙⊤R+R⊤R˙=0⇒R⊤R˙=−(R⊤R˙)⊤ \dot{R}^\top R + R^\top \dot{R} = 0 \quad \Rightarrow \quad R^\top \dot{R} = -(R^\top \dot{R})^\top R˙⊤R+R⊤R˙=0⇒R⊤R˙=−(R⊤R˙)⊤
这说明 R⊤R˙R^\top \dot{R}R⊤R˙ 是一个反对称矩阵。于是我们可以定义瞬时角速度 ω\omegaω，令 ω∧=R⊤R˙\omega^\wedge = R^\top \dot{R}ω∧=R⊤R˙。这就引出了泊松方程：
R˙=Rω∧ \dot{R} = R \omega^\wedge R˙=Rω∧
ω\omegaω 就是陀螺仪直接测量的物理量。这个方程的解非常漂亮，它直接是指数映射的形式：
R(t)=R(t0)exp(ω∧Δt) R(t) = R(t_0) \exp(\omega^\wedge \Delta t) R(t)=R(t0)exp(ω∧Δt)
离散化后，就是当前旋转乘上一个由角速度算出来的小增量。这是IMU进行姿态积分的核心代码逻辑。
2. 四元数运动学：那个关键的"1/2"系数
如果我们用四元数来表示旋转，对单位性约束 q∗q=1q^* q = 1q∗q=1 求导，可以得到四元数的运动学方程：
q˙=qϖ \dot{q} = q \varpi q˙=qϖ
其中 ϖ\varpiϖ 是纯虚四元数。为了让四元数的角速度 ω\omegaω 和 SO(3)SO(3)SO(3) 里的 ω\omegaω 统一物理意义，我们把它改写为：
q˙=12q0,ω⊤ \dot{q} = \frac{1}{2} q 0, \\omega^\top q˙=21q0,ω⊤
为什么会有个 1/21/21/2 ？
因为四元数旋转一个点要乘两遍（qpq−1q p q^{-1}qpq−1），所以它的"旋转速度"天然就是 SO(3)SO(3)SO(3) 里的一半。这个 1/21/21/2 是连接两种数学语言的桥梁。
在代码里，如果用四元数更新姿态，离散公式就是：
qnew≈qold⊗1,12ωΔt q_{\text{new}} \approx q_{\text{old}} \otimes \left 1, \\frac{1}{2} \\omega \\Delta t \\right qnew≈qold⊗1,21ωΔt
注意：长时间更新后一定要对四元数重新归一化 ，否则数值误差会累积导致它不是单位四元数。而旋转矩阵就不存在这个问题，因为 exp(ω∧)\exp(\omega^\wedge)exp(ω∧) 天生就在 SO(3)SO(3)SO(3) 上。
3. 线速度与加速度：没你想的那么简单
这是区分"理论派"和"实战派"的关键。同一个空间点，在两个有相对旋转的坐标系下，速度是不一样的。
假设坐标系1和2，点 ppp 在2系下的坐标是 p2p_2p2，2系相对于1系有角速度 ω\omegaω。那么 ppp 在1系下的速度 v1v_1v1 是：
v1=R12(ω∧p2+v2) v_1 = R_{12} (\omega^\wedge p_2 + v_2) v1=R12(ω∧p2+v2)
多出来的 ω∧p2\omega^\wedge p_2ω∧p2 就是旋转带来的附加速度。
加速度就更复杂了，会多出三项科氏、向心等加速度：
a1=R12(a2+2ω∧v2⏟科氏加速度+ω˙∧p2⏟角加速度项+ω∧ω∧p2⏟向心加速度) a_1 = R_{12} (a_2 + \underbrace{2\omega^\wedge v_2}{\text{科氏加速度}} + \underbrace{\dot{\omega}^\wedge p_2}{\text{角加速度项}} + \underbrace{\omega^\wedge \omega^\wedge p_2}_{\text{向心加速度}}) a1=R12(a2+科氏加速度 2ω∧v2+角加速度项 ω˙∧p2+向心加速度 ω∧ω∧p2)
应用场景：你车上的IMU（加速度计）测到的根本不是单纯的车在世界上跑的加速度。它测的是"比力"，里面包含了重力、车自身的线加速度，以及上面这一堆乱七八糟的科氏力、向心力。在做GNSS/IMU组合导航时，这些项一个都不能少，否则车一转弯定位就会往外飘。
4. 扰动模型：求导的"懒人神器"
当我们在SLAM后端写优化代码时，经常要对旋转矩阵 RRR 求导。如果直接对9个矩阵元素求导，雅可比矩阵是 9×99\times99×9 的，算起来想死。
扰动模型 说：别直接动 RRR，我们在它右边（或左边）乘一个微小的扰动 ϕ\phiϕ，然后对 ϕ\phiϕ 求导。这样雅可比就是 3×33\times33×3 的，非常简洁。
比如，对旋转后的点 RaRaRa 关于旋转 RRR 求右扰动的雅可比：
∂Ra∂ϕ=limϕ→0R(I+ϕ∧)a−Raϕ=−Ra∧ \frac{\partial R a}{\partial \phi} = \lim_{\phi \to 0} \frac{R(I + \phi^\wedge)a - Ra}{\phi} = -R a^\wedge ∂ϕ∂Ra=ϕ→0limϕR(I+ϕ∧)a−Ra=−Ra∧
这个 −Ra∧-R a^\wedge−Ra∧ 会在你写视觉重投影误差、激光点云匹配误差的雅可比时反复出现。记住这个模式，能让你手写优化器时事半功倍。
总结：这些知识如何服务于你的自动驾驶与机器人目标？
- 建图与定位（SLAM）：几何学是你的语言。不懂坐标变换，激光雷达和相机就对不齐；不懂运动学，IMU的数据就是一堆没用的正弦波。
- 状态估计（滤波与优化）：李群、李代数、BCH公式是把"旋转"这个非线性问题塞进线性滤波器（如卡尔曼滤波）里的转换器。扰动模型是你手算雅可比、写优化代码的底气。
- 工程避坑指南：四元数为啥要归一化？角速度更新为啥有1/2？加速度计为啥总飘？这些底层原理就是用来给你排错用的。当你代码跑飞时，回来翻翻这些推导，往往能找到bug的根源。
建议学习者尝试编写一段简单的运动学仿真代码（比如模拟车辆以固定线速度和角速度画圆），分别用旋转矩阵和四元数来实现更新，观察它们的轨迹是否重合。