Part 4: Algorithms · The robot's brain
Chapter 9
SLAM - 机器人的“第一天上班认路”
你第一天去一个陌生的大商场，没有楼层导览图。你只能边走边记 - “刚才经过了一个星巴克，右转有个扶梯，扶梯对面是优衣库”。走着走着你发现“咦，这个星巴克我见过”，于是你意识到自己绕了一圈回到了起点。这时候你脑中那张草图突然变得更准确了 - 因为你知道起点和终点是同一个位置，之前走的那些弯路可以被校正过来。
你同时在做两件事：建一张心理地图（mapping），以及根据这张不断完善的地图推断“我现在在哪”（localization）。两件事互相依赖 - 地图越准，定位越准；定位越准，地图也越准。
这就是 SLAM - Simultaneous Localization and Mapping，同时定位与建图。
没有地图，也不知道自己在哪
机器人被放到一个全新环境里，面对的是一个鸡生蛋蛋生鸡的问题：要定位自己，需要一张地图；要建地图，又需要知道自己在哪。SLAM 就是同时解决这两个问题的算法框架。
为什么不能提前给一张地图？有些场景可以 - 比如固定厂房可以预先扫描一张平面图导入系统。但大量场景不行：临时搭建的展会现场、每天都在变的仓库、从没去过的客户家。即使有预置地图，机器人也需要实时定位自己在地图上的位置，而定位本身又依赖传感器和地图的匹配。所以 SLAM 几乎是所有移动机器人的标配能力。
如果你写过 Web 应用，可以这样理解：SLAM 就像一个不断迭代的数据库。每走一步，传感器（第 4 章讲的 LiDAR、相机、IMU）提供新数据，SLAM 把这些数据融合进已有的“地图数据库”中，同时更新“当前位置”这个字段。只不过这个数据库的更新频率是每秒几十次，而且一旦写入了错误的数据，后面的数据全会被带歪。
SLAM 的核心循环
把 SLAM 拆开来看，它每一帧都在重复一个循环：
感知 - 传感器获取当前环境的一帧数据。LiDAR 扫出一圈点云，相机拍下一张图，IMU 报告当前的加速度和角速度。
匹配 - 把这一帧的数据和上一帧（或者地图中已有的数据）对齐。“这堵墙上次看到过，它的位置说明我往前走了 0.3 米，往左偏了 2 度。” 这一步叫 scan matching（扫描匹配）或者 feature matching（特征匹配），是整个 SLAM 的精度命脉。
更新位置 - 根据匹配结果，更新机器人的位姿估计（位置 + 朝向）。
扩展地图 - 如果这一帧看到了地图里还没有的区域，把新数据添加进地图。
这个循环跑得越快越好。LiDAR SLAM 通常以 10-20Hz 的频率运行 - 每秒定位十到二十次。跑慢了，机器人已经移动了一大截，匹配就容易对不上。
前端和后端 - 两种时间尺度的问题
SLAM 系统内部分成两个模块，处理两种不同尺度的问题：
前端（Front-end） 负责帧间匹配 - “这一帧和上一帧怎么对齐”。它处理的是高频、局部的问题。每来一帧新数据，前端都要快速算出机器人移动了多少。速度是第一优先级，因为下游的控制系统在等这个结果。前端的典型做法是 ICP（Iterative Closest Point，迭代最近点）算法或者特征点匹配。
问题是，帧间匹配不可能完美。每一帧都有微小的误差 - 0.1 度的角度偏差，1 毫米的位移误差。这些误差会随着时间不断累积。机器人走一条 100 米的直线，累积误差可能让最终定位偏了半米。如果它走一个大圈回到起点，终点和起点的位置会对不上 - 这叫做漂移（drift）。
后端（Back-end） 就是来解决漂移问题的。它处理的是低频、全局的问题。后端维护一个叫做 位姿图（pose graph） 的数据结构 - 每个节点是机器人在某一时刻的位姿，边是两个位姿之间的约束关系。当机器人走了一大圈回到之前去过的地方，后端通过 回环检测（loop closure） 发现“我又看到了这个场景”，然后在位姿图中加一条新的约束边，再用图优化算法（比如 g2o、GTSAM）把所有位姿一起调整，让整张图在全局上自洽。
打个比方：前端像是你一步一步认路，后端像是你走完一圈之后拿出纸笔，把路线图整体校正一遍 - “既然起点和终点是同一个地方，那中间的弯路肯定画歪了，修一修。”
回环检测的质量直接决定了最终地图的精度。检测不到回环，漂移永远消不掉；误检了一个回环（把两个相似但不同的地方当成同一个），整张地图会被拉扯变形，比不做优化还糟。
主流算法和工具
SLAM 的算法选择主要取决于你用什么传感器。不同传感器输入，对应不同的算法流派。
slam_toolbox - 2D LiDAR SLAM 的标准选择
如果你的机器人是一台在平面上跑的服务机器人或 AGV，装了一个 2D LiDAR，那 slam_toolbox 基本是默认选项。它是 ROS 2 Navigation 生态的官方推荐，由 Steve Macenski（Nav2 的核心维护者）开发。输入是 2D 激光扫描数据，输出是一张 2D 占用栅格图（Occupancy Grid Map） - 后面会详细讲这个。
slam_toolbox 支持在线建图（机器人边走边建）和离线优化（录一段数据回来慢慢优化），还支持增量建图 - 在已有地图基础上更新，不需要每次从零开始。对于大部分室内平面导航场景，它够用了。
ORB-SLAM3 - 纯视觉 SLAM 的经典
如果你的机器人只有相机没有 LiDAR（比如出于成本考虑），ORB-SLAM3 是学术界和工业界都认可的选择。它从图像中提取 ORB 特征点（角点之类的视觉标志），通过追踪这些特征点在连续帧之间的移动来估计机器人的运动。
ORB-SLAM3 支持单目、双目和 RGB-D 相机，还支持视觉惯性模式（相机 + IMU）。它的后端优化做得非常扎实，回环检测用的是 DBoW2 词袋模型 - 把每一帧的特征描述压缩成一个“视觉指纹”，通过对比指纹来判断是否回到了同一个地方。
缺点也明显：纯视觉 SLAM 对光照变化极其敏感。从室内走到室外、从阴影走到阳光下，特征点会大面积丢失，系统可能直接“跟丢”。
LIO-SAM - LiDAR + IMU 紧耦合，精度标杆
如果你有 3D LiDAR 和 IMU，LIO-SAM 是目前精度最高的开源方案之一。“LIO”代表 LiDAR-Inertial Odometry - 它把 LiDAR 的点云匹配和 IMU 的惯性测量紧密耦合在一起。IMU 提供高频（几百赫兹）的运动预测，LiDAR 提供低频但精确的环境匹配，两者互补。
LIO-SAM 的后端基于因子图优化（factor graph，用 GTSAM 库），支持 GPS 因子、回环因子等多种约束。它适合户外、大场景的高精度建图。很多自动驾驶和室外机器人项目用它。
代价是硬件要求高 - 一个 3D LiDAR 至少几千块，计算量也比 2D SLAM 大一个量级。
VINS-Fusion - 相机 + IMU 的轻量级方案
在 LiDAR 太贵或太大的场景下（比如无人机、小型机器人），VINS-Fusion 是一个优秀的视觉惯性 SLAM 方案。它融合相机和 IMU 数据，支持单目 + IMU、双目 + IMU，甚至可以融合 GPS。
VINS-Fusion 的前端是光流追踪（不是特征点匹配，是直接追踪像素块的移动），后端是滑动窗口优化。它的初始化过程比较讲究 - 需要机器人做一些特定运动（比如旋转）来估计 IMU 的偏差，否则系统起不来。
| 方案 | 传感器 | 适用场景 | 精度 | 计算量 | 成本 |
|---|---|---|---|---|---|
| slam_toolbox | 2D LiDAR | 室内平面导航 | 中 | 低 | 低 |
| ORB-SLAM3 | 相机（+IMU） | 通用场景 | 中高 | 中 | 低 |
| LIO-SAM | 3D LiDAR + IMU | 室内外大场景 | 高 | 高 | 高 |
| VINS-Fusion | 相机 + IMU | 无人机/小型机器人 | 中高 | 中 | 低 |
选择的核心逻辑是：传感器决定算法，场景决定传感器。室内平面送货机器人用 2D LiDAR + slam_toolbox 就够了，没必要上 3D LiDAR；户外巡检机器人需要高精度地图，LIO-SAM 才撑得住。
占用栅格图 - SLAM 的主要输出
SLAM 最重要的输出物是占用栅格图（Occupancy Grid Map） - 下一章 Nav2 做路径规划全靠它。
想象一张方格纸覆盖在你的建筑平面图上。每个小格子存一个值：
- 黑色（值 = 100） - 这里有障碍物（墙、柜子、桌腿）
- 白色（值 = 0） - 这里是空地，可以通行
- 灰色（值 = -1） - 未知区域，传感器还没扫到过
就这么简单。机器人在环境中走一圈，SLAM 输出一张这样的灰度图片 - 它就是机器人理解的“地图”。
分辨率（resolution） 是栅格图最关键的参数，表示每个格子对应现实中多大。常见值是 0.05 米 - 也就是每个格子代表 5 厘米 x 5 厘米。
这里有一个 trade-off：分辨率设得太粗（比如 0.2 米），一把椅子的腿可能正好卡在格子边界上被忽略掉，机器人直接撞上去。分辨率设得太细（比如 0.01 米），一个 100 米 x 100 米的场景就需要 1 亿个格子，内存爆炸，路径规划的搜索空间也会变得巨大。0.05 米是大部分室内场景的甜点值 - 它能分辨出 5 厘米以上的障碍物，同时保持合理的计算量。
栅格图通常保存为两个文件：一张 .pgm 图片（灰度的地图本身）和一个 .yaml 配置文件（记录分辨率、原点坐标等元数据）。Nav2 的 map_server 节点负责加载这两个文件，把地图发布到 ROS 2 topic 上，供导航系统使用。
开发者踩坑指南
SLAM 在论文里看起来很优雅，在实际部署中会让你怀疑人生。这里是几个最常见的坑。
长走廊问题
想象一条 50 米长、两侧都是白墙的走廊。对 LiDAR 来说，走廊中间任何一个位置看起来都差不多 - 左边一堵墙，右边一堵墙，前面一堵墙。SLAM 的匹配算法无法区分“我在走廊的 20 米处”和“我在走廊的 30 米处”，因为传感器看到的数据几乎一样。这就是所谓的感知混叠（perceptual aliasing） - 不同位置的观测长得太像，SLAM 就迷路了。
解决办法：在走廊里放一些视觉标志物（哪怕是不同颜色的贴纸），让环境不再完全对称。或者融合 IMU 数据 - IMU 虽然会漂移，但在短距离内能告诉你“我确实往前走了 10 米”，帮助 SLAM 度过这段没有特征的区域。实在不行还有个笨办法：在走廊天花板上贴不同的 AprilTag 标签，让视觉系统有东西可认。
动态环境
SLAM 假设环境是静态的 - 墙不会动，柜子不会移位。但真实世界里人会走来走去、叉车会穿梭、门会开关。一个在建图时正好路过的人会被当成“障碍物”画进地图里，等人走了，地图上就多了一个幽灵障碍。
大部分 SLAM 系统没有内建的动态物体处理能力。实际项目中常见的做法是：建图时尽量挑人少的时段（凌晨去扫一遍），或者多次建图做对比，把不一致的部分过滤掉。有些方案（比如基于语义分割的动态过滤）能自动识别和剔除移动物体，但这增加了计算开销。
玻璃和镜面
LiDAR 发射的激光在玻璃上会直接穿透或者被镜面反射到别的方向。结果是：玻璃门对 LiDAR 来说是“不存在的”，机器人会直接撞上去；镜子会制造出虚假的反射点，让 SLAM 以为那个方向有一条并不存在的走廊。
这是 LiDAR 的物理局限，软件层面很难完全解决。工程上的常见应对：在玻璃门上贴不透明贴纸，或者在 SLAM 输出的地图上手动补画玻璃墙。更讲究的做法是融合超声波传感器 - 超声波能检测到玻璃（声波会被玻璃反射），弥补 LiDAR 的盲区。
光照变化对视觉 SLAM 的打击
纯视觉 SLAM 靠追踪图像特征点来定位。当光照剧烈变化时 - 比如从室内走到室外、太阳被云遮住又出来、经过一扇大窗户 - 图像亮度突变，大量特征点会丢失或者错配，SLAM 前端直接跟丢。
这也是为什么很多工业场景宁可多花钱用 LiDAR 也不纯靠相机 - LiDAR 是主动传感器，发射自己的激光，不受环境光影响。如果你必须用视觉 SLAM，至少要融合 IMU：在特征点丢失的那零点几秒里，IMU 能靠惯性推算维持一个粗糙的位姿估计，给视觉系统争取到恢复追踪的时间。
SLAM 的输出喂给谁
SLAM 产出两样东西：一张地图和一个实时更新的机器人位姿。
地图交给 Nav2 的地图服务器，成为全局路径规划的基础 - “从 A 点到 B 点，哪条路最短且不撞墙”。位姿信息通过 TF（坐标变换）系统广播出去，告诉所有下游节点“机器人现在在地图上的什么位置”。Nav2 的 costmap 会在这张静态地图上叠加实时传感器数据，形成动态的代价地图，但底层那张静态地图就是 SLAM 建出来的。
建图通常只做一次（或者定期更新）。建完之后，机器人日常运行时切换到纯定位模式 - 拿着已有的地图，只做 localization 不做 mapping。这时候常用的工具是 AMCL（Adaptive Monte Carlo Localization），它用粒子滤波在已有地图上持续追踪机器人位置。这个话题在下一章 Nav2 里会详细展开。