4. SLAM数学基础
激光SLAM的工程调试，表面上看是在调参、看轨迹、查点云，但真正决定系统是否稳定的，是背后的数学骨架。坐标系变换决定了点云如何“对齐”，旋转表示决定了姿态更新是否会出现奇异，李群李代数决定了优化时能不能正确求导，概率图模型决定了后端如何建模约束，而最小二乘则是所有这些约束最终被求解的统一出口。理解这五者之间的关系，才能在调试时判断问题出在“模型建错了”还是“求解器没收敛”。
一、坐标系变换：从雷达系到世界系
激光SLAM中至少涉及四类坐标系：雷达坐标系（Lidar）、机体/基座坐标系（Base）、里程计坐标系（Odom）和世界/地图坐标系（Map）。一次扫描点 p_L 要进入地图，需要经过外参、位姿和全局修正的链式变换：
p_M = T_MO · T_OB · T_BL · p_L
其中 T_BL 为雷达外参，T_OB 为里程计位姿，T_MO 为地图修正
工程上最常见的错误是外参方向搞反。例如把 T_BL 当成 T_LB 使用，点云会整体反向旋转，表现为“建图时墙面重影、回环后地图撕裂”。调试时应先固定一个已知标定物，单独验证外参链，再接入里程计。
二、旋转表示：四元数、旋转矩阵与欧拉角
旋转有三种常用表示：旋转矩阵 R∈SO(3) 无奇异但冗余且需正交约束；欧拉角直观但存在万向锁；四元数紧凑且插值平滑，是SLAM中最常用的姿态存储形式。它们之间的转换关系如下表：
| 表示 | 维度 | 优点 | 缺点 | 典型用途 |
|---|---|---|---|---|
| 旋转矩阵 R | 9（约束6） | 无奇异、可直接乘 | 冗余、需正交化 | 点云变换、优化内部 |
| 欧拉角 | 3 | 直观易读 | 万向锁、多解 | 人机界面、参数配置 |
| 四元数 q | 4（约束1） | 紧凑、插值平滑 | 符号二义、需归一化 | 位姿存储、IMU融合 |
| 旋转向量 | 3 | 无冗余、适合求导 | 大角度需注意模长 | 李代数、误差状态 |
三、李群李代数：为什么优化要绕这一圈
位姿属于李群 SE(3)，它不能像普通向量一样直接做加法。若直接对旋转矩阵加一个微小量，结果会脱离 SO(3) 流形，正交性被破坏。李代数的价值在于：把流形上的更新映射到切空间 se(3) 中，用普通向量 ξ 表示增量，再通过指数映射 Exp(ξ) 回到群上：
T_new = Exp(δξ) · T_old
J = ∂(Exp(ξ)·p) / ∂ξ // 扰动模型下的雅可比
在激光SLAM中，点面残差对位姿的雅可比通常采用“左扰动”模型，这样雅可比形式统一、便于链式求导。若调试时发现优化“越迭代越差”，常见原因是雅可比符号或扰动方向写反，导致梯度上升而非下降。
四、概率图模型：把SLAM写成因子图
SLAM的后端本质上是一个最大后验估计问题。设位姿序列为 X，观测为 Z，则：
X* = argmax P(X|Z) ∝ argmax P(Z|X)·P(X)
= argmin Σ ‖ r_odom ‖² + Σ ‖ r_lidar ‖² + Σ ‖ r_loop ‖²
在因子图中，位姿是变量节点，里程计、激光匹配、回环、GPS、IMU预积分都是因子。每个因子对应一个残差项和一份信息矩阵。调试时如果发现“回环后轨迹被拉飞”，往往是回环因子的信息矩阵过大，压过了里程计和激光匹配的约束。此时应检查各因子的协方差配置，而不是盲目调迭代次数。
五、案例：一次“点云重影”的排查
某履带机器人在室内建图时，墙面出现约5cm重影，回环后地图局部撕裂。排查过程如下：
- 单独播放雷达数据，关闭里程计，确认点云本身无重影，排除雷达硬件问题。
- 检查外参文件，发现
extrinsic_rot的符号与标定报告相反，修正后重影减小但未消失。 - 打印四元数模长，发现部分帧模长偏离1达0.02，归一化后姿态跳变消失。
- 检查后端信息矩阵，回环权重被设为里程计的100倍，调整为10倍后地图撕裂消失。
这个案例说明：坐标系变换、旋转表示、概率图模型和最小二乘并不是四个独立知识点，而是一条从数据到求解的完整链路。任何一环出错，最终都会表现为“地图不对”，但根因可能完全不同。
六、复盘清单：调试时按顺序问自己
- 外参矩阵的方向和单位是否正确？旋转部分是否正交？
- 四元数是否归一化？欧拉角是否在奇异区附近被使用？
- 李代数扰动是左乘还是右乘？雅可比是否与残差定义一致？
- 因子图是否漏加约束？各因子的信息矩阵量级是否合理？
- 最小二乘迭代中残差是否单调下降？H矩阵条件数是否过大？