接上一篇文章《自动驾驶SLAM基础：几何学与运动学全解析》，这篇文章提及到很多较为抽象的知识和公式，但是不要每个公式都死磕推导，有几处核心推导必须亲手走一遍。其余的，理解结论、知道怎么用、什么时候用，就足够了。

---

一、必须亲手推导的“命根子”公式

这些推导是后续写代码、调算法、排错的直接依据。如果你只是看一遍，觉得自己懂了，后面写IMU预积分或者ESKF的时候一定会卡壳。

1. 反对称矩阵算子 ${}^{\wedge }$ 和泊松方程

• 推导内容：从 $R^{\top }R=I$ 出发，两边对时间求导，得到 ${\dot{R}}^{\top }R=-R^{\top }\dot{R}$，然后定义 ${\omega }^{\wedge }=R^{\top }\dot{R}$，得出 $\dot{R}=R{\omega }^{\wedge }$。
• 为什么必须推：这是整个旋转运动学的起点。如果不亲手推一遍，就体会不到“为什么角速度会出现在 $R^{\top }\dot{R}$ 这个位置”，以及“为什么左扰动和右扰动会差一个转置”。后面IMU的陀螺积分、卡尔曼滤波的预测步，全是基于这个微分方程。推完了你才会真正理解，角速度 $\omega$ 是在车体坐标系下表达的。

2. 四元数运动学里的 $1/2$ 系数

• 推导内容：从单位四元数约束 $q^{\ast }q=1$ 求导，得到 $\dot{q}=\frac{1}{2}q[0,\omega {]}^{\top }$。
• 为什么必须推：这个 $1/2$ 是连接四元数和旋转矩阵的桥梁。你不推，就永远记不住为什么更新四元数时要乘 $0.5\omega \Delta t$，而不是 $\omega \Delta t$。在写ESKF（误差状态卡尔曼滤波）时，预测步的角速度更新如果少乘了 $1/2$，车就会在原地打转漂移。这个坑，只有亲手推过才能彻底避开。

3. 扰动模型求雅可比（以右扰动为例）

• 推导内容：对 $Ra$ 关于旋转 $R$ 求右扰动，即 ${\lim }_{\varphi \to 0}\frac{R\exp ({\varphi }^{\wedge })a-Ra}{\varphi }$，利用 $\exp ({\varphi }^{\wedge })\approx I+{\varphi }^{\wedge }$ 得到 $-R{a}^{\wedge }$。
• 为什么必须推：这是以后手写SLAM后端优化（比如视觉重投影误差、点云匹配误差）时，天天要用的技能。你不推，就只会调库，一旦库里的雅可比和你的参数化方式不匹配，你连错误都查不出来。亲手推一遍，你就永远记住了“对旋转后的点求导，结果是 $-R{a}^{\wedge }$”。

---

二、理解结论、知道怎么用就行的内容

这些公式推导过程很冗长（比如BCH公式的完整证明、四元数到旋转矩阵的迹推导），但结论非常固定，我们只需要像查字典一样记住它们。

1. 罗德里格斯公式与指数映射

• 结论：$R=\exp (w^{\wedge })=\cos \theta I+(1-\cos \theta )n{n}^{\top }+\sin \theta n^{\wedge }$。
• 怎么用：当你需要把旋转矢量转换成旋转矩阵时用。在代码里，Eigen库直接提供了 AngleAxisd 或者 SO3::exp，你不需要现场推导，知道它背后是这个公式就行。

2. BCH公式的线性近似与雅可比矩阵

• 结论：$\exp (\Delta {\varphi }^{\wedge })\exp ({\varphi }^{\wedge })=\exp ((\varphi +J_l^{-1}\Delta \varphi {)}^{\wedge })$，右雅可比 $J_r(\varphi )=J_l(-\varphi )$。
• 怎么用：在IMU预积分或者需要传播协方差的时候，你会用到 $J_r$ 的具体形式。你不需要推导它是怎么来的，但必须知道它的输入是 $\varphi$（旋转矢量），输出是一个 $3\times 3$ 矩阵。代码里通常直接调用 SO3::JacobianR() 之类的函数。

3. 四元数与旋转矩阵的转换公式

• 结论：$R=v{v}^{\top }+s^{2}I+2s{v}^{\wedge }+(v^{\wedge }{)}^2$，以及 $\theta =2\arccos s$。
• 怎么用：当你需要把四元数转成旋转矩阵时，用这些公式。但同样，Eigen里 Quaterniond 和 Matrix3d 可以互相转换，你只需要知道它们底层是这么算的，不必每次重写。

4. 加速度跨坐标系变换

• 结论：$a_1=R_{12}(a_2+2{\omega }^{\wedge }{v}_2+{\dot{\omega }}^{\wedge }{p}_2+{\omega }^{\wedge }{\omega }^{\wedge }{p}_2)$。
• 怎么用：做IMU机械编排或者GNSS/IMU组合导航时，你需要用这个公式把加速度计的测量补偿到世界坐标系。你不需要推导每一步，但必须清楚每一项代表什么（科氏、向心），以及在你的应用场景里哪些项可以忽略。

---

三、只需要了解概念的“背景知识”

这些内容是数学家的游戏，作为自动驾驶工程师，知道它们存在、大概干嘛用就行，不用深究。

• 李群 $SO(3)$ 和李代数 $\mathfrak{so}(3)$ 的抽象定义：你只需要建立“旋转矩阵住在弯曲的小区，李代数是平坦的切平面”这个几何图像。
• 四元数乘法的完整展开：公式 $q_a{q}_b=[s_a{s}_b-v_a^{\top }{v}_b,s_a{v}_b+s_b{v}_a+v_a\times v_b{]}^{\top }$ 很长，你只要知道它是分配律加外积，代码里直接用重载的 * 运算符。
• $SE(3)$ 和 $\mathfrak{se}(3)$ 的运动学：除非你要做紧耦合的激光惯性SLAM后端，否则用 $SO(3)+t$ 的方式就够了。“默认使用旋转加平移的运动学”。

四、上述公式概念的详细推导

上述公式概念的详细推导过程，见下一篇文章《【自动驾驶与SLAM技术】之必须亲手推导的“命根子”公式（下）》

---