2. SLAM基础回顾
在进入前端里程计与扫描匹配的细节之前，有必要先把 SLAM 的整体框架重新梳理一遍。很多初学者一上手就钻研 ICP、NDT 或特征匹配，却对“这些算法在系统里处于什么位置、输入输出是什么、误差由谁负责修正”缺乏清晰认识，结果调参时只能凭感觉。本节从系统组成、前后端分工、传感器类型、坐标系与位姿表示四个角度建立全局视图，为后续章节打下概念地基。
一、SLAM 系统的四大组成
一个完整的 SLAM 系统通常可以拆解为四个相互协作的模块：传感器数据采集、前端里程计、后端优化以及建图与回环。传感器负责把物理世界转化为可计算的观测数据；前端负责在相邻时刻之间估计相对运动，输出初步的位姿轨迹；后端负责在更长的时间尺度上对轨迹和地图进行全局优化；建图与回环则负责把局部观测拼接成一致的地图，并在识别到曾经到过的位置时消除累积误差。
这四者不是简单的串联关系，而是存在反馈闭环：后端优化后的位姿会反过来修正前端的地图配准基准，回环检测的结果又会作为约束送入后端。理解这种闭环结构，是理解 SLAM 为什么比单纯里程计更复杂的关键。
二、前端与后端的分工
前端里程计的核心任务是帧间相对位姿估计。以激光 SLAM 为例，前端接收连续两帧点云，通过扫描匹配算法求出两帧之间的刚体变换，再把它累乘到当前位姿上，得到一条递推轨迹。前端的特点是频率高、延迟低、局部一致性好，但误差会随着帧数增加不断累积，长时间运行后轨迹会明显漂移。
后端优化的核心任务是全局一致性校正。它把前端输出的位姿作为初始值，把回环检测、GNSS、IMU 预积分等作为约束，构建位姿图或因子图，通过最小化整体误差来重新分配误差。后端频率低，但一旦触发就能显著修正轨迹。二者的关系可以概括为：前端负责“走得动”，后端负责“走得准”。
三、常见传感器类型与特点
SLAM 的传感器选择直接决定了前端算法的形态。下表对比了几类常见传感器在 SLAM 中的角色与局限。
| 传感器 | 主要观测 | 优势 | 局限 |
|---|---|---|---|
| 2D 激光雷达 | 单平面距离 | 精度高、成本低、算法成熟 | 缺少三维信息，易受玻璃、镜面干扰 |
| 3D 激光雷达 | 三维点云 | 信息丰富、适合室外与自动驾驶 | 数据量大、成本高、标定要求高 |
| 单目相机 | 图像特征 | 成本极低、纹理信息丰富 | 尺度不确定、受光照影响大 |
| 双目 / RGB-D | 图像 + 深度 | 可恢复尺度、适合室内 | 深度范围有限、计算量较大 |
| IMU | 角速度、加速度 | 高频、短时精度好 | 零偏漂移，单独使用不可靠 |
| 轮式编码器 | 轮速 / 位移 | 简单可靠、成本低 | 打滑时误差大，仅适用于地面机器人 |
实际系统往往采用多传感器融合。例如激光雷达 + IMU 可以在点云稀疏或退化场景下保持位姿估计稳定；视觉 + IMU 可以解决单目尺度问题。前端里程计的设计必须围绕所选传感器的观测特性展开。
四、坐标系与位姿表示
SLAM 中所有几何计算都建立在坐标系之上。常见坐标系包括：世界坐标系（也称地图坐标系，全局固定）、机器人本体坐标系（base_link，随机器人运动）、传感器坐标系（如激光雷达、相机安装位置）。它们之间通过外参标定确定固定变换，通过位姿估计确定随时间变化的变换。
位姿表示通常有两种形式：欧拉角 + 平移向量和旋转矩阵 / 四元数 + 平移向量。欧拉角直观但存在万向锁问题；旋转矩阵和四元数没有奇异点，适合连续运算。在代码中，位姿常写成齐次变换矩阵：
// 4x4 齐次变换矩阵 T，表示 base_link 在世界坐标系中的位姿
// R 为 3x3 旋转矩阵，t 为 3x1 平移向量
T = [ R t ]
[ 0 1 ]
// 位姿复合：T_world_base = T_world_odom * T_odom_base
// 位姿求逆：T_base_world = T_world_base.inverse()需要特别注意的是，位姿的“左乘”与“右乘”对应不同的物理含义：左乘表示在世界坐标系下叠加变换，右乘表示在自身坐标系下叠加变换。前端里程计中，帧间相对位姿通常是右乘到当前位姿上，而回环约束则往往以世界坐标系下的绝对位姿形式进入后端。
五、一个典型场景的复盘
假设一台差速机器人在办公楼走廊中运行激光 SLAM。前端用 2D 激光扫描匹配估计帧间位姿，频率约 10 Hz；IMU 提供 100 Hz 的角速度用于短时预测；后端每检测到一次回环就触发一次位姿图优化。运行 200 米后，前端轨迹在走廊尽头出现约 0.8 米的漂移，但回环检测识别到起点附近的特征后，后端把整条轨迹拉回，最终地图闭合误差小于 0.1 米。
这个案例说明：前端的漂移是正常现象，关键在于后端是否有足够的约束来修正它。如果回环检测失败，或者外参标定有误，后端再强也无法恢复正确轨迹。因此，前端里程计与扫描匹配的质量，直接决定了后端优化的上限。
- SLAM 系统由传感器、前端、后端、建图与回环四部分组成，彼此形成闭环。
- 前端负责高频局部位姿估计，后端负责低频全局一致性优化。
- 传感器类型决定前端算法形态，多传感器融合可提升鲁棒性。
- 坐标系与位姿表示是所有几何计算的基础，运算前必须明确参考系。
- 前端漂移不可避免，回环与后端是消除漂移的关键手段。