3.5 SLAM：边定位边建图
3.5 SLAM：边定位边建图
本节摘要：定位需要地图，建图需要定位——SLAM 要同时解出这对互为前提的未知量。本节讲清它为什么不是死锁而是可分解的估计问题，拆解前端-后端-回环的框架分工，并用一段数字演示里程计漂移如何被回环摁住。
最初这个问题被普遍认为无解。1986 年前后，研究者在讨论一个看似自指的命题：机器人要定位就得有地图，要建图就得知道自己在哪——鸡生蛋蛋生鸡，哪来的初始条件？转机来自换一个问法：不要按先后顺序解两个问题，而是把"机器人轨迹 + 路标位置"全部当成未知量，构造一个联合概率估计问题。未知量虽然多，但观测把它们彼此拴住了——每帧扫描既约束了机器人位姿，也约束了地图。这就是 SLAM（Simultaneous Localization and Mapping，同时定位与建图）从哲学困境变成数值问题的那一刻。
框架三段：前端、后端、回环
现代 SLAM 系统普遍拆成三段，各管一件事：
前端（里程计层）：逐帧处理传感器数据，输出相邻两帧间的相对运动——激光前端做扫描匹配（ICP 或 NDt 类算法），视觉前端做特征跟踪与对极几何。前端快（要跟传感器帧率），但误差逐帧累积：每帧只有 0.5% 的配准误差，一百帧之后就是半米级的漂移。
后端（优化层）：把前端给的相对约束拼成全局问题——一张"位姿图"：节点是各时刻的机器人位姿，边是帧间相对运动（带信息矩阵）。求解就是最小化所有边的加权残差（图优化，常用高斯-牛顿或 LM 迭代）。前端漂了一百帧，后端不能凭空救——它只是把误差"摊匀"，没有新的绝对信息进来，全局一致性仍无从谈起。
回环检测（命门）：识别"我回到了到过的地方"。回到起点那一刻，一个跨越上百帧的绝对约束诞生了，图优化把这个强约束的修正量沿着位姿图传播回去，累积误差被整体摁平。视觉 SLAM 用词袋模型比对场景外观，激光 SLAM 用扫描的局部几何指纹。回环检测的两类错误各有代价：误报（把没回过的地方认成回环）会让地图强行折叠出鬼影；漏报只是损失一次修正机会——所以工程上对回环判决的置信阈值设得非常高，并要求几何验证（回环位姿处的实际扫描要能对上）。
图：SLAM 系统的前端-后端-回环架构
用数字感受漂移与回环
一段闭环路径：机器人出发、绕行、回到起点，前端每帧位移估计偏大 0.5%。取 400 帧：
drift_rate = 0.005 n_frames = 400 path_len = 120.0 # 米，轨迹总长 final_offset = path_len * drift_rate # 0.6 米：回到起点时差了半米多 print(final_offset) # 0.6 —— 起点重合处出现 0.6 m 的"裂缝" # 回环闭合后：图优化把 0.6 m 按信息量分配到全部 400 个位姿上 # 每个位姿平均修正 1.5 mm，中段约 0.3 mm —— 误差从"尾端爆发"变"全程摊薄" per_pose = final_offset / n_frames print(per_pose * 1000) # 1.5 mm
0.6 米与 1.5 毫米的对比就是后端与回环的全部价值。也解释了为什么 SLAM 系统的现场评价标准常是两句话："跑一圈回来地图不裂吗"（回环能力），"跑十圈地图不糊吗"（长期多回环的一致性）。
两条技术路线的分工
激光 SLAM：几何直接、室内鲁棒，2D 应用（扫地机、AGV）已是成熟商品；缺点是依赖结构化几何、走廊等退化场景（长直墙只有一维约束）会漂，雨雪粉尘对 2D/3D 雷达是重创。
视觉 SLAM：传感器便宜、信息丰富（纹理、语义），缺点是对光照与快速运动敏感、尺度不确定（单目）。视觉惯性里程计（VIO）用 IMU 补尺度和短时高频，成为手机 AR 与无人机的标配。当下趋势是激光-视觉-惯导三源融合：雷达给几何、相机给语义、IMU 给高频，互为校验。
⚠️ 常见坑：把 SLAM 当定位用。SLAM 输出的定位精度依赖环境特征丰富度，空旷、动态物体多的环境里它会退化甚至丢失；生产系统要准备退化策略（降级到里程计+航位推算、停机等待），不能默认它永远在线。
本节要点
- SLAM 把"鸡生蛋"改写成联合估计问题：轨迹与地图作为整体被观测相互拴住。
- 前端追帧率、后端算总账、回环供绝对信息；没有回环，后端只能摊匀误差而不能消除。
- 回环检测是命门：误报折叠地图，漏报损失修正，故判决阈值高且必须几何验证。
- 激光路线吃几何结构，视觉路线吃纹理语义，VIO 与多源融合是工程主流。
信息矩阵：优化器里的"方差方言"
后端的图优化里，每条边除了残差还携带信息矩阵——协方差的逆，表达"这条约束有多可信"。前端给出帧间运动估计时顺带给出它的协方差（扫描匹配的收敛情况决定），信息矩阵即由它构造。优化的本质于是清晰可见：所有边的加权残差平方和最小，权重是可信度。退化场景在信息矩阵里的表现是"某些方向信息量趋零"——长走廊里前后方向的约束信息充足，侧向几乎为零，优化器于是知道这个位姿"只有一维可信"。把信息量诊断做成在线监控，退化场景（走廊、隧道、空旷大厅）就能被提前识别并触发对策（降速、依赖其他传感器、请求人工复核）。
鲁棒核函数：为粗大误差上保险
回环误匹配、动态物体残影会给优化问题塞进错误约束，一条强错误边足以把整张图拉歪。鲁棒核（Huber、Cauchy 等）的做法是对大残差"压低权重"——残差越大越不信任。效果等价于"先按正常权重迭代，残差大的边逐渐降权直至几乎出局"。工程配置要点：核函数的阈值与传感器噪声量级挂钩（激光 SLAM 常取分米级，视觉取像素级），阈值过小会把真实的大位移回环也压掉。调试这类参数的标准方法是注入人工误差的回放测试：往数据里加一条已知错误的回环，看系统能否正确识别并剔除。
问题：建好的地图能拿去给另一台机器人用吗
可以，这正是 SLAM 的工程红利：地图是可复用资产。复用要过三关。坐标系关：新机器人的传感器安装位姿不同，重做外参标定即可，地图本身不变。分辨率关：新机器人精度需求更高时，粗地图需要重建成细版本——地图分辨率在创建时就应按最苛刻的下游需求选。语义关：地图里标注的语义（充电桩位置、禁行区）是否随场地变化维护——地图的几何部分衰减慢，语义部分衰减快，两者要分开管理。复用做得好的团队，新机器人入场部署时间能从周级压缩到天级。