SLAM、地图构建、定位与里程计漂移

一台移动机器人需要两个彼此相关的答案：环境长什么样，以及机器人在环境里处于什么位置。**同步定位与建图（SLAM）**在机器人探索的过程中同时估计这两者。一旦有了一张稳定的地图，定位系统就可以在这张固定地图上估计位姿，而不用重建它。

SLAM 不是里程计的替代品。它用局部运动估计把传感器观测串起来，然后在环境提供了足够证据时修正它们累积的漂移。

把四件事分开

当每一项都有明确的输出时，这些术语就好懂多了：

• **里程计：**来自轮编码器、惯性感知、视觉运动或者它们融合的平滑局部运动。它随时间漂移。
• **建图：**从传感器观测和位姿出发，建立起像占据栅格这样的表示。
• **定位：**在已有地图中估计机器人的位姿。
• **SLAM：**一边建图，一边估计位姿并修正过去的估计。

导航是在这之上再加上规划和控制。一张好地图本身并不会生成一条安全的速度指令。

跟着信息流过整个系统

对于二维室内 SLAM，一个常见的数据流是：

1. 轮编码器和惯性测量单元估计局部运动。
2. 激光雷达带时间戳和传感器坐标系发布测距扫描。
3. TF2 把传感器关联到 base_link，把局部位姿关联到 odom。
4. 扫描匹配把新观测与最近的扫描或正在增长的地图对齐。
5. 后端优化一张由位姿和约束组成的图。
6. 用修正后的位姿和测距数据更新占据栅格。

里程计提供一个有用的初始猜测。没有它，扫描匹配可能要搜索大得多的空间，或者对齐到错误的重复结构上。而没有外部观测，里程计的误差就是无界的。正是这个上界决定了一台机器人能否以平行横扫的方式覆盖地面：这些行大约走过十条之后就不再平行了，所以一台会规划覆盖路径的机器还要带一个陀螺仪、一台顶置相机或者一个信标。户外的话，同一份工作就交给了经过融合而不是被跟随的 GPS。

理解扫描匹配与回环

扫描匹配找出一个能把当前测距测量与已有结构最好对齐的相对位姿。它能修正小的轮子打滑和比例误差，但它需要几何特征。一条很长的无特征走廊、一面玻璃墙、一群走动的人，或者高度重复的货架，都可能让好几个位姿看起来差不多。

回环发生在系统认出先前访问过的地方，并在当前位姿与一个更早的位姿之间加入一条约束时。优化这条约束可以把累积的误差分摊到整条轨迹上，让墙重新对齐。

回环不是自动就正确的。两条相似的走廊里一次错误匹配就能让地图折叠或撕裂。稳健的系统会做几何检查、打分，并使用可配置的接受阈值。对可疑的大幅修正要复核，而不是每闭合一个环就庆祝。

调 SLAM 之前先把机器人准备好

SLAM 无法弥补一个坏掉的输入契约。先验证这些：

• 编码器的方向、比例和时间戳；
• 直行和原地转向测试中局部里程计是否合理；
• odom → base_link → sensor 的变换是否正确；
• 测距数据单位是否正确，无效读数是否被标出；
• 各节点之间的时钟是否一致；
• 传感器安装是否刚性；
• 测距范围和视场是否足够覆盖这个环境。

建立基线时慢慢开。转速一高，运动畸变和稀疏扫描就变得难对付。如果传感器或驱动支持时序补偿，就根据实测行为去配置它，而不是假设自己不需要。

有意识地建图

从一个特征丰富的位置出发，靠走环路而不是只走长的往返路径来移动。平滑地转身，好让扫描仪从重叠的视角观察同一批结构。建图时避免在机器人旁边搬动或移动大件物品。

盯住实时地图里的墙变双、拐角被抹开、跳变、房间断开。这些都是证据，不是表面问题：

• 平行的墙变双，提示位姿或时序有误差；
• 转身时墙变弯，提示变换有问题或存在运动畸变；
• 比例逐渐漂移，提示轮子标定不足或扫描约束太弱；
• 全局突然重排，提示回环——可能是对的，也可能是错的。

只有在确认重要走廊在所选分辨率下仍然可通行之后才保存地图。更细的栅格消耗内存，而且可能把传感器噪声一起保留下来；更粗的栅格则可能把窄通道堵上。

在保存下来的地图里定位

建图之后，切换到针对已保存地图的定位模式。像自适应蒙特卡洛定位（AMCL）这样的粒子滤波定位器，从测距观测和运动估计位姿。它仍然需要一个合理的传感器模型、变换和里程计。

给一个足够接近的初始位姿，好让系统能认出正确的位置，在对称环境里尤其如此。测试恢复能力时，从小位姿误差开始，再逐步加大。“机器人被绑架”式的恢复是另一项要求，不能因为正常跟踪能工作就想当然地认为它也有。

在标准坐标系模型里，局部里程计发布 odom → base_link，而 SLAM 或定位发布 map → odom。这个全局变换负责修正漂移，同时不至于逼得局部平滑的控制坐标系去跳变。

测量精度与重复性

使用位置经过测绘或仔细测量的物理地标。让机器从不同接近方向停到每个地标上时，记录估计出的位姿。把下面几件事分开看：

• 同一地点的重复性；
• 短期局部漂移；
• 绕环之后的全局地图一致性；
• 传感器暂时丢失之后的恢复时间。

把地图与已知距离做比较，但不要只凭一张看起来顺眼的图来判断。导航需要的是正确的自由空间、正确的障碍边界和稳定的位姿，而不是漂亮的墙厚。

把扫描、里程计、变换和 SLAM 输出都记进日志，让一次失败能被重放。只在实时状态下出现的问题，诊断起来要困难得多。

常见 SLAM 故障的诊断

• **直行时墙变双：**里程计比例、传感器时间或扫描匹配三者不一致。
• **转身时地图变弯：**传感器变换、角度里程计或运动畸变错了。
• **重复走廊里位姿跳变：**扫描匹配或回环接受了一个含糊的对齐。
• **一个环都合不上：**路线缺少重叠、观测太稀疏，或者接受阈值太严。
• **定位只在初始位姿附近能用：**环境本身对称，或者全局恢复没有配置。
• **地图看着对但导航蹭墙：**足迹、膨胀层或代价地图配置错了。
• **每个 SLAM 参数怎么调都没用：**上游的里程计、TF 或传感器契约仍然是坏的。

从正交编码器标定和差速里程计开始，然后用ROS 2 TF2 坐标系验证几何链条。传感器模拟器展示了偏置和漂移为什么靠简单的平滑是去不掉的。