2、SLAM基础回顾：SLAM问题定义、坐标系、刚体变换、李群与李代数
SLAM（Simultaneous Localization and Mapping，同步定位与建图）是移动机器人与自动驾驶领域的核心基础问题。在进入LiDAR/IMU/视觉多传感器融合定位的实战之前，必须对SLAM的数学骨架有清晰认知。本章不是简单罗列公式，而是从工程实现的角度，梳理SLAM问题的概率本质、多坐标系定义、刚体变换的表示方式，以及李群与李代数在优化中的核心作用。这些内容直接决定了后续融合定位中状态估计的精度、鲁棒性与计算效率。
2.1 SLAM问题的概率定义与数学本质
SLAM问题的标准定义是：在未知环境中，移动机器人利用自身搭载的传感器（LiDAR、Camera、IMU等）同时估计自身位姿（位置与姿态）并构建环境地图。从概率视角看，SLAM是一个状态估计问题——给定一系列传感器观测数据 z₁:ₖ 和控制输入 u₁:ₖ，求解机器人状态 xₖ 和地图 m 的后验概率分布：
P(xₖ, m | z₁:ₖ, u₁:ₖ)在实际工程中，我们通常采用最大后验估计（MAP）来简化问题，将SLAM转化为一个非线性最小二乘优化问题。这里需要明确几个关键概念：
- 状态变量：机器人位姿 xₖ = [Rₖ, tₖ] ∈ SE(3)，以及地图点/特征点位置 mᵢ ∈ R³。
- 运动模型：描述状态随时间演化的规律，如 IMU 预积分模型或轮式里程计模型。
- 观测模型：描述传感器如何从状态产生观测数据，如 LiDAR 点云配准、视觉特征重投影。
- 噪声假设：通常假设过程噪声和观测噪声均为零均值高斯分布，即 w ~ N(0, Q)，v ~ N(0, R)。
在融合定位场景中，SLAM问题的核心挑战在于多传感器数据的异质性——LiDAR提供几何结构信息，Camera提供纹理信息，IMU提供高频运动先验。如何统一这些异构数据到同一个状态估计框架中，正是后续章节要解决的核心问题。
2.2 坐标系定义与变换关系
多传感器融合定位的第一步是建立统一的坐标系框架。典型的坐标系包括：
| 坐标系 | 符号 | 原点位置 | 用途 |
|---|---|---|---|
| 世界坐标系 | {W} | 环境固定点 | 全局地图、轨迹基准 |
| 机器人/车体坐标系 | {B} | IMU或车体中心 | 运动控制、状态表示 |
| 传感器坐标系 | {S} | 各传感器光学/测量中心 | 原始数据表达 |
坐标系之间的变换关系通过外参（Extrinsic）标定确定。例如，LiDAR点云从传感器坐标系转换到车体坐标系：
p_B = R_BS · p_S + t_BS其中 R_BS 和 t_BS 是LiDAR相对于车体的旋转矩阵和平移向量。在融合定位系统中，外参标定的精度直接影响融合效果——即使0.1°的姿态误差或1cm的平移误差，在远距离观测时也会导致显著的投影偏差。
2.3 刚体变换与SE(3)群
刚体变换描述的是物体在三维空间中的旋转和平移，保持物体内部点之间的相对距离不变。数学上，刚体变换构成特殊欧氏群 SE(3)：
SE(3) = { T = [R t; 0 1] ∈ R⁴ˣ⁴ | R ∈ SO(3), t ∈ R³ }其中 SO(3) 是特殊正交群，表示三维旋转矩阵。刚体变换具有以下重要性质：
- 封闭性：两个刚体变换的复合仍然是刚体变换。
- 结合律：T₁ · (T₂ · T₃) = (T₁ · T₂) · T₃。
- 逆变换：T⁻¹ = [Rᵀ, -Rᵀt; 0, 1]。
- 不变量：变换前后两点间距离保持不变。
在SLAM中，刚体变换用于描述机器人位姿、传感器外参、地图点位置等。值得注意的是，旋转矩阵虽然有9个元素，但只有3个自由度，这导致在优化过程中直接对旋转矩阵求导非常困难——必须引入李群与李代数。
2.4 李群与李代数：优化中的核心工具
李群与李代数为SLAM优化提供了优雅的数学工具。核心思想是：将旋转矩阵（SO(3)）和刚体变换（SE(3)）映射到其对应的李代数（so(3)和se(3)），在向量空间中进行求导和优化，再通过指数映射回到李群。
SO(3) 与 so(3)：旋转矩阵 R 对应的李代数 so(3) 是一个三维向量 φ = [φ₁, φ₂, φ₃]ᵀ，其反对称矩阵 φ^ 满足：
R = exp(φ^) = I + sinθ/θ · φ^ + (1-cosθ)/θ² · (φ^)²其中 θ = ||φ|| 是旋转角度。这实际上就是罗德里格斯公式。在优化中，我们使用扰动模型：
∂(R·p)/∂δφ = -(R·p)^SE(3) 与 se(3)：刚体变换 T 对应的李代数 se(3) 是一个六维向量 ξ = [ρ, φ]ᵀ，其中 ρ 是平移部分，φ 是旋转部分。指数映射为：
T = exp(ξ^) = [exp(φ^), J·ρ; 0, 1]其中 J 是左雅可比矩阵。在SLAM优化中，我们通常对位姿施加左扰动或右扰动，以计算残差关于位姿的雅可比矩阵。
2.5 案例拆解：LiDAR-IMU融合中的位姿优化
以经典的 LiDAR-IMU 融合定位为例，说明李群李代数在工程中的具体应用。假设我们有一个3D LiDAR 和一个 IMU，目标是估计机器人的六自由度位姿。
问题设定：LiDAR 以 10Hz 频率输出点云，IMU 以 200Hz 频率输出角速度和加速度。在两次 LiDAR 扫描之间，IMU 提供高频运动先验。我们需要在每帧 LiDAR 数据到达时，优化当前位姿 T_WB ∈ SE(3)。
优化流程：
- 初始化：使用上一帧的位姿作为初始值，并用 IMU 预积分结果进行预测。
- 残差构建：将当前 LiDAR 点云与局部地图进行配准，构建点到平面或点到线的残差：r = nᵀ(p_W - q_W)，其中 p_W 是当前帧点云转换到世界坐标系的点，q_W 是地图中的对应点，n 是法向量。
- 雅可比计算：利用左扰动模型，计算残差关于位姿增量 δξ 的雅可比矩阵：J = ∂r/∂δξ = nᵀ · [I, -(p_W)^]。
- 迭代优化：使用高斯牛顿法求解增量 δξ* = -(JᵀJ)⁻¹Jᵀr，更新位姿 T ← T · exp(δξ^)。
关键参数与结果：在公开数据集（如 KITTI）上，使用上述方法可实现平移误差小于 1% 的定位精度。IMU 预积分的作用在于：当 LiDAR 帧率较低或点云退化（如长直走廊）时，IMU 提供短期稳定的运动约束，防止位姿估计发散。
2.6 常见误区与总结复盘
在SLAM基础学习中，以下误区值得特别注意：
- 误区一：混淆旋转矩阵的转置与逆。对于旋转矩阵，R⁻¹ = Rᵀ，但刚体变换的逆不是简单的转置，必须按 T⁻¹ = [Rᵀ, -Rᵀt; 0, 1] 计算。
- 误区二：忽视坐标系变换方向。在融合系统中，务必明确每个传感器数据的坐标系来源，建议在代码中为每个点云/图像数据添加坐标系标签。
- 误区三：直接对旋转矩阵求导。旋转矩阵的9个元素存在6个约束，直接求导会导致优化矩阵奇异。必须使用李代数参数化。
- 误区四：忽略IMU与LiDAR的时间同步。即使空间外参标定准确，时间戳不同步也会导致运动畸变，影响融合精度。