5.3 激光 SLAM 与占据栅格
5.3 激光 SLAM 与占据栅格
本节摘要：激光前端的核心是扫描匹配——把当前激光帧与已有地图或上一帧对齐，本质是 3.4 配准的二维化身。相关性扫描匹配与分枝定界是主流求解器；占据栅格用对数几率更新维护地图。本节含可复算的占据更新实现。
换激光兵种上场。视觉怕黑、怕晃、怕没纹理，激光雷达全不怕——主动发光、直接量距，仓库夜间作业、隧道巡检都是它的主场。二维激光 SLAM 是最好的入门标本：问题降到平面，每个技术环节都能摊开看清楚，而它的思想原封不动地活在三维激光 SLAM 与占据网格导航里。
扫描匹配：3.4 的平面化身
二维激光 SLAM 的前端干的事，与 3.4 节点云配准一模一样：找刚体变换（平面三参数：平移二加旋转一）使当前扫描与地图对齐。主流求解器两条路：相关性扫描匹配把候选位姿离散成网格，对每个候选打分（扫描点落到地图占据格的置信度之和），粗到细逐层细化——暴力但 GPU 友好、对初值几乎无要求；基于优化的匹配从某个初值出发，把每个扫描点对到地图最近表面再迭代（点到线、点到面 ICP 的二维版），快但吃初值。工程标配是两者接力：相关性匹配给出粗位姿，优化匹配精修。
占据打分函数是关键设计：地图不是二值的，而是每格一个概率——落入占据概率高的格子得分高。这让匹配对地图噪声天然免疫。
占据栅格：对数几率更新
激光地图的载体是占据栅格：每格维护"被占据的概率"。更新用对数几率（log odds）而不是直接乘概率——加法运算、避免数值下溢、且先验可显式表达。每束激光沿射线更新两个假设：终点处"占据"的证据，路径上各格"空闲"的证据——一束光既看见了障碍，也看见了它途中的空旷。
# 占据栅格的对数几率更新（可复算） import numpy as np log_occ, log_free = 0.9, -0.4 # 命中加 0.9，穿过减 0.4（占据证据更强） grid = np.zeros((200, 200)) # 对数几率为 0：先验 50% lo, hi = -4.0, 4.0 # 饱和上下限 def update(grid, free_cells, hit_cell): for c in free_cells: grid[c] = np.clip(grid[c] + log_free, lo, hi) grid[hit_cell] = np.clip(grid[hit_cell] + log_occ, lo, hi) return grid # 同一格子被同一束激光穿过三次、命中一次的演化 g = 0.0 for _ in range(3): g = np.clip(g + log_free, lo, hi) print('穿过三次后的概率: %.3f' % (1 - 1/(1+np.exp(g)))) # 输出: 穿过三次后的概率: 0.293 g = np.clip(g + log_occ, lo, hi) print('随后命中一次的概率: %.3f' % (1 - 1/(1+np.exp(g)))) # 输出: 随后命中一次的概率: 0.598
结果解读：三次"穿过"把概率压到零点二九，但一次"命中"就拉回零点六——占据证据的权重设计得比空闲证据强，因为动态物体（行人走过）不应轻易洗掉静态墙。变式：动态环境常再加一个"反证据"通道与遗忘因子，让搬走的箱子能从地图上淡出。
实战演练：手算一个分支搜索
背景：理解分枝定界为什么快。机器人上一时刻在格坐标（一百，一百），本帧扫描需要匹配，候选窗口二十一乘二十一乘二十四朝向共一万余假设。操作：分析两级搜索的裁剪量。
# 分枝定界 vs 暴力枚举的候选数对比 w = 21 # 平移搜索窗口 21x21 rot = 24 # 朝向离散 24 份 brute = w * w * rot print('暴力枚举候选数: %d' % brute) # 输出: 暴力枚举候选数: 10584 # 分枝定界：粗层 5x5x6 = 150 个节点，仅最优分支下探 coarse = 5 * 5 * 6 refined = coarse + 4 * 4 * 4 # 每个粗节点只下探最优的 4x4x4 print('分枝定界评估数: 约 %d' % refined) # 输出: 分枝定界评估数: 约 394 print('裁剪率: %.1f%%' % (100*(1 - refined/brute))) # 输出: 裁剪率: 96.3%
结果解读：分支定界用"粗层上界分数"保证不漏掉最优解的前提下砍掉九成六的计算，这正是主流激光 SLAM 能在笔记本处理器上实时跑的算力来源。变式：上界估计不紧（粗层分数偏乐观）时裁剪失效退化为暴力，工程上靠限制树深兜底。
⚠️ 常见坑：长走廊或空旷大厅里激光"看不出走了多远"——两侧平行墙对平移方向的约束极弱，扫描匹配在走廊方向滑动，地图拉出重影。对策：接入轮速计或 IMU 提供走廊方向的约束（5.4 节），或依赖回环拉回。
💡 关键直觉：占据栅格更新是"每束光都是一次双重证词"——它证明途中空旷、同时证明终点有物；一次观测写两处账，这是激光建图信息效率高的原因。
从二维到三维：升级时变什么
把本节的二维方案升级成三维激光 SLAM，有三处实质变化。其一，扫描匹配从三参数变成六参数，搜索空间爆炸，相关性匹配的暴力枚举不再可行，实际系统改用"点对线、点对面"的迭代匹配加由粗到精的金字塔；其二，地图载体从平面栅格换成子地图序列——每扫一段攒一个局部点云地图，全局靠位姿图串联，直接维护一张全局稠密三维栅格的内存代价不可承受；其三，退化检测从走廊问题扩展成特征值分析——把匹配的法方程矩阵做特征值分解，最小特征向量方向就是最不可观的方向，系统据此自动报告"某方向退化"并请求其他传感器补刀。这三条变化的每一条，都能在主流开源三维激光系统的设计文档里找到对应实现。
本节要点回顾
- 扫描匹配是二维化的 3.4 配准，相关性匹配抗初值差、优化匹配精修；
- 分枝定界用粗层上界裁剪九成以上候选，实时性的算力来源；
- 占据栅格用对数几率更新，一束光同时写空闲与占据两处账；
- 几何退化（长走廊）是激光前端的软肋，需外部约束补刀；
- 三维激光 SLAM 与本节同构：点云配准加子地图加位姿图。
视觉与激光各有软肋，把两支兵种合编是下一节的主题。