SLAM 是什么？一条链路讲透：传感器 → 数据处理 → 前端匹配 → 子图与关键帧 → 闭环检测 → 图优化

以 Cartographer 为主线，把「同步定位与建图」拆成六个环节，一次讲清楚。

前言

如果你刚接触机器人、自动驾驶或者激光雷达，第一个绕不开的词大概率是 SLAM。

SLAM 是 Simultaneous Localization and Mapping 的缩写，中文叫「同步定位与建图」。听起来很玄，但它要回答的问题朴素得不行：

一台不知道自己在哪里的机器人，如何一边走，一边画出周围的地图？

本文不堆公式，也不假设你有 SLAM 基础。我会沿着数据流动的方向，把整条链路拆成六个环节讲一遍：

1. 传感器——机器人的感官
2. 数据处理——从原始数据到可用信息
3. 前端匹配——当前帧对齐到局部地图
4. 子图与关键帧——把扫描贴成地图
5. 后端闭环——识别「我回来过」
6. 图优化——把整张图拉正

文中涉及的具体实现，以 Google 开源的 Cartographer 为例。它是目前工程上最值得读的 2D/3D SLAM 系统之一。

---

一、SLAM 到底难在哪：一个鸡生蛋问题

先看机器人睁开眼面对的处境：没有地图，也不知道自己站在哪里。

它想问两件事：

• 我在哪里？ —— 定位（Localization）
• 周围是什么？ —— 建图（Mapping）

麻烦的是这两件事互为前提：

• 定位需要一张地图作参照，才能知道自己在图中的位置；
• 建图又需要知道每一帧扫描是在哪个位置拍下的，才能把数据拼起来。

定位依赖地图，建图依赖定位，谁也离不开谁——这就是经典的「鸡生蛋问题」。

SLAM 给出的答案是：不要拆开解，同时求解。把「位姿」和「地图」当作同一个优化问题里的两组未知量，一起估计、一起更新。想通这一点，后面所有的模块设计就都顺理成章了。

---

二、传感器：机器人的感官

SLAM 的第一步，是让机器人拥有感官。传感器是它感知世界的唯一入口。工程上最常用的有四类。

2.1 激光雷达 LiDAR

发射激光、测量到障碍物的距离，一圈扫下来，就是环境的一张「剖面图」。

• 优点：测距直接、精度高，可以直接用来建图；几何信息是「量」出来的，不依赖纹理。
• 缺点：成本高；雨雾、镜面反射会干扰测距。
• 产出：一帧扫描（scan），本质是一堆 (角度, 距离) 或 (x, y) 点——也就是点云。

2.2 惯性测量单元 IMU

输出三轴加速度与角速度。

• 优点：频率极高（100–1000 Hz），短时间内非常准。
• 缺点：靠积分推算姿态，会随时间慢慢漂移，必须靠外部观测校正。

2.3 轮式里程计 Odometry

靠编码器数轮子转了多少圈，推算位移。

• 优点：连续、便宜、短距离平滑可靠。
• 缺点：一旦打滑就失效，误差会悄悄累积。

2.4 相机（视觉 SLAM）

信息丰富、成本低，但受光照和纹理影响大。属于另一条技术路线，本文不展开。

2.5 为什么要融合

每一种传感器都有短板：

传感器	强项	短板
激光雷达	几何精度高，可直接建图	贵，怕雨雾/镜面
IMU	高频、短时精确	积分漂移
轮式里程计	连续、便宜	打滑即失效
相机	信息丰富、便宜	怕光照、怕弱纹理

所以真实系统从不单打独斗，而是做多传感器融合——用 IMU 的高频补雷达的低频，用雷达的绝对观测校正 IMU 的漂移，用里程计提供平滑的短时约束。

---

三、数据处理：从原始数据到可用信息

传感器吐出来的原始数据并不能直接用：它带着噪声、冗余，而且各传感器的时间戳并不对齐。在进入匹配之前，通常要走完四步。

3.1 时间同步

雷达可能只有 10 Hz，IMU 却有 100 Hz。必须把不同频率、不同延迟的数据归算到同一时间基准，否则「同一时刻」的姿态和扫描对不上，匹配从一开始就错了。

3.2 带通滤波（量程过滤）

按最小 / 最大量程过滤，丢掉太近和太远的点：

• 太近的点往往是机器人自身遮挡（比如车体、支架）；
• 太远的点已经超出可信量程，信噪比极低。

Cartographer 里对应 min_range / max_range 参数。

3.3 体素降采样（Voxel Filter）

一帧 3D 扫描动辄几万个点，直接参与匹配计算量爆炸。做法是：把空间切成体素格子，每个格子里只保留一个点（通常是质心）。

这一步几乎是无损的——因为它丢掉的只是「同一小块空间里的重复观测」，而匹配关心的是几何结构，不是点的绝对数量。

3.4 运动畸变校正

这是最容易被忽略、但必须做的一步。

激光雷达不是瞬间拍完一帧的。一帧扫描的这段时间里（比如 100 ms），机器人本身在移动、在转弯。于是同一帧里的点，实际上是在不同位姿下测到的——直接用会让墙壁「歪掉」。

解决思路：按每个点的时间戳，结合这段时间内的运动估计，把点统一扳回到同一个时刻（通常是帧起始或帧结束时刻）。Cartographer 里由 MotionFilter 与累积点云的体素滤波配合完成。

本环节的输出：干净、稀疏、时间对齐的一帧扫描——这才是一份可以直接参与匹配的数据。

---

四、前端匹配（Local SLAM）：把当前帧对齐到子图

现在进入前端。它的任务很具体：把当前这一帧扫描，对齐到已有的局部地图上。

4.1 位姿外推

如果没有任何先验，扫描匹配就得在一个巨大的搜索空间里盲目找位置和角度——既慢又容易掉进局部最优。

所以第一步是位姿外推：用 IMU 和里程计预测下一时刻的位姿，作为搜索的初值。

有了靠谱的初值，匹配只需要在初值附近做小幅微调，效率和鲁棒性都大幅提升。

4.2 扫描匹配

在初值附近搜索最优位姿，使当前帧与子图的重合度最高。

Cartographer 用了两个匹配器配合：

• Ceres 扫描匹配器（CeresScanMatcher）：把匹配写成连续优化问题，用非线性最小二乘求最优解。目标大致是让扫描点落在占据概率高的地方：

  $$\arg \underset{\xi }{\min }\sum _k(1-M_{\text{smooth}}(S_k(\xi )){)}^2$$

  其中 $\xi$ 是待求位姿，$S_k(\xi )$ 是第 $k$ 个点按位姿 $\xi$ 变换后的坐标，$M_{\text{smooth}}$ 是平滑后的占据概率栅格。点落在墙上的概率越高，代价越小。

• 实时相关匹配器（RealTimeCorrelativeScanMatcher）：在离散的位姿网格上暴力搜索，配合分支定界加速（详见第六节）。它的作用是给 Ceres 提供一个足够好的初值，避免连续优化掉进局部极小。

工程实现上，Ceres 匹配通常分三段递进：先只优化平移 → 再只优化旋转 → 最后联合优化，逐步放开自由度。

4.3 前端的边界

匹配的输出，就是机器人当前这一刻的位姿 $(x,y,\theta )$。

前端的特点是：快，能实时跑。代价是只看局部——每一帧都只和附近的地图对齐，误差会一点点累积，最终表现为漂移。

这正是后端要解决的问题。

---

五、子图与关键帧：把扫描贴成地图

匹配得到位姿之后，要把这一帧扫描「贴」到地图上。Cartographer 的做法是累积成一张局部地图——子图。

5.1 子图 Submap

子图是一张概率栅格：每个格子记录「被占据的概率」。多次观测取平均，噪声自然被抹平。

这里有个很漂亮的工程思想：不追求每一帧都精确，而靠多次观测的平均来消除噪声。单次测距可能偏差 3 cm，但几十次观测叠加后，墙的位置会收敛到很准的地方。

5.2 关键帧 Keyframe

但并不是每一帧都要记下来。

当机器人位移超过一定距离 $d$、或转角超过一定角度 $\theta$，才插入一个关键帧。

关键帧是位姿图里的锚点：它记住「我在这个位置、看到了什么」。两个关键帧之间用里程计约束相连。这样既保留了必要的信息，又把数据量压到了可控范围。

5.3 混合栅格 Hybrid Grid

子图规模变大后，内存会成为问题。Cartographer 的折中方案是混合栅格：

• 近处用概率栅格，保证精度；
• 远处用 TSDF（截断符号距离场）压缩存储，节省内存。

本质是「精度与内存的折中」——离得近的地方机器人还要反复用，离得远的地方只需要保留大致轮廓。

---

六、后端闭环（Global SLAM）：识别「我回来过」

现在进入后端。它要处理前端留下的那个问题：漂移。

6.1 位姿图 Pose Graph

前端走过的每一步，都会被记成位姿图里的一条边：

• 节点 = 关键帧与子图；
• 边 = 它们之间的相对位姿约束。

前端的里程计只连相邻节点。这意味着一圈走下来，图是一个「环」，但首尾两个节点之间没有边——所以首尾对不上。

6.2 漂移 Drift

当机器人绕一圈回到起点时，两条路线对不上：明明走过同一个地方，位置却偏了。

举个具体的量级：走了 100 m，回来可能偏了 2 m。误差就是这么一点点攒出来的。

6.3 闭环检测

于是后端做闭环检测：把当前扫描拿去和历史子图逐一比对，寻找「我曾经来过这里」的证据。

一旦找到可靠的匹配，就在位姿图里新增一条闭环约束边——告诉优化器：「这两个节点其实是同一个地方。」

6.4 分支定界加速

逐一比对很慢，因为位姿搜索空间很大。Cartographer 用分支定界（Branch and Bound）来加速：

1. 粗分辨率扫描：先在低分辨率栅格上快速算一个上界，剪掉大量不可能的候选；
2. 逐层细化：只在还有希望的候选上逐层提高分辨率，最终精确求解。

这是一棵「搜索树」的剪枝过程——由粗到细，把指数级的搜索压成可实时运行的计算量。Cartographer 在 FastCorrelativeScanMatcher 中实现了它。

---

七、图优化：把整张图拉正

闭环约束加进来了，但新的矛盾也随之出现：这条新边和原有的里程计边并不完全自洽。

每条边都希望两端的相对位姿符合它的观测，可由于漂移，它们彼此冲突。

7.1 目标

图优化的任务，是找一组节点位置，让所有约束的总误差最小：

$$\underset{\xi }{\min }\sum _{ij}\rho \left(e_{ij}^{\top }{\Omega }_{ij}{e}_{ij}\right)$$

其中：

• $\xi$ 是所有节点位姿；
• $e_{ij}$ 是约束 $(i,j)$ 的残差（观测相对位姿与当前估计相对位姿之差）；
• ${\Omega }_{ij}$ 是信息矩阵，表示这条约束有多「可信」；
• $\rho (\cdot )$ 是鲁棒核函数，用来抑制离群约束的影响。

约束越多、闭环越多，结果越稳。

7.2 方法：稀疏位姿调整

Cartographer 用的是 Sparse Pose Adjustment（SPA）——把位姿图当成一个大型稀疏最小二乘问题求解。因为每条边只连接两个节点，雅可比矩阵极其稀疏，可以用稀疏 Cholesky 分解高效求解。

7.3 关键直觉

这是整个后端最值得记住的一点：

一处的闭环大误差，会「均匀分摊」到整张图上，而不是只挪动闭环的两个端点。

如果只修正闭环两端的节点，地图会在局部被硬掰出一个畸形。而全局优化的结果是：每个节点都稍微动一点，总误差最小，地图整体自洽。

7.4 结果

当残差被压到最小，地图被整体「拉正」：

• 走廊不再错位；
• 回到起点时严丝合缝；
• 得到一张全局一致的地图。

---

八、把整条链路串起来

环节	输入	输出	核心手段	特点
传感器	物理世界	原始点云 / 加速度 / 编码器	多传感器融合	互补短板
数据处理	原始数据	干净、稀疏、时间对齐的一帧	时间同步、带通滤波、体素降采样、畸变校正	决定上限
前端匹配	当前帧 + 子图	位姿 $(x,y,\theta )$	位姿外推 + Ceres / 相关匹配	快，局部准确
子图与关键帧	位姿 + 扫描	概率栅格子图 / 关键帧	概率栅格、混合栅格	把扫描贴成地图
后端闭环	当前帧 + 历史子图	闭环约束	分支定界搜索	找回「我来过」
图优化	位姿图 + 闭环约束	全局一致的地图	稀疏位姿调整 SPA	慢，全局一致

一句话概括两条主线的分工：

• 前端（Local SLAM） 负责局部准确——快，但会漂移；
• 后端（Global SLAM） 负责全局一致——慢，但能纠正累积误差。

两者配合，才构成 Cartographer 这类现代 SLAM 系统的骨架。

---

九、一句话记住 SLAM

如果这篇长文你只记住一句话，我希望是这句：

SLAM，就是一边走，一边画，一边纠正。

• 「一边走」——传感器在持续观测；
• 「一边画」——前端把扫描贴成子图；
• 「一边纠正」——后端闭环 + 图优化把漂移拉正。

三者缺一不可，也正好对应了本文的六个环节。

---

延伸阅读

• Cartographer 官方仓库
• Cartographer 论文：Real-Time Loop Closure in 2D LIDAR SLAM（ICRA 2016）
• 关键词：Local SLAM、Global SLAM、Submap、Pose Graph、Branch and Bound、Sparse Pose Adjustment

如果这篇对你有帮助，欢迎点赞收藏；有讲得不清楚的地方，也欢迎在评论区指出，我会补充。