3. 占据栅格地图构建：概率栅格地图原理、激光雷达逆传感器模型、gmapping与cartographer算法对比
各位同学，欢迎来到第三章。前两章我们搞定了激光雷达的底层驱动和点云预处理，现在终于要进入一个核心环节——地图构建。
你想想看，机器人要在未知环境里导航，它首先得知道“我在哪”以及“周围长什么样”。地图就是它的“眼睛”和“记忆”。这一章，我们就来聊聊占据栅格地图（Occupancy Grid Map）是怎么建起来的，以及主流的两种SLAM方案——gmapping和Cartographer，到底有什么区别。
本章核心知识点
- 概率栅格地图的数学原理与更新公式
- 激光雷达逆传感器模型（Inverse Sensor Model）
- gmapping与Cartographer算法核心差异对比
- 实际工程中的选型建议与避坑指南
3.1 概率栅格地图原理
先问一个问题：地图里的每个格子，到底该标记为“有障碍物”还是“没有障碍物”？
直接给0或1太武断了。传感器有噪声，测量有误差，你永远无法100%确定某个格子到底有没有东西。所以，我们引入概率。
每个栅格（cell）维护一个概率值 p(m_i = 1)，表示该格子被占据的概率。初始时，我们通常设为0.5，表示完全未知。
当激光雷达打出一束射线，我们就要根据测量结果来更新这个概率。更新公式用的是贝叶斯法则的变体——对数几率（Log-Odds）形式。
// 对数几率更新公式
l(m_i | z_t, x_t) = l(m_i | z_t, x_t) + l(m_i) - l(m_i_prior)
// 其中：
// l(m_i) = log( p(m_i) / (1 - p(m_i)) )
// l(m_i_prior) 是初始先验对数几率
// l(m_i | z_t, x_t) 是逆传感器模型给出的测量对数几率为什么用对数几率？因为加法比乘法好算，而且数值稳定。我早期做项目时直接拿概率相乘，结果浮点数下溢得一塌糊涂，后来老老实实换成了Log-Odds，问题立刻解决。
个人经验：在实际嵌入式平台上，建议将概率值映射到整数区间（比如0~100），用查表法代替对数运算，能省下不少CPU周期。我在STM32H7上做过测试，整数查表比浮点log快了一个数量级。
3.2 激光雷达逆传感器模型
逆传感器模型，说白了就是：给定一次激光测量结果，反过来推断每个栅格被占据的概率是多少。
举个例子。激光雷达打出一束射线，测到距离是3米。那么：
- 射线路径上（0~3米）的格子：大概率是空闲（free）
- 终点（3米处）的格子：大概率是占据（occupied）
- 终点之后的格子：未知（unknown），因为没有信息
但实际没这么简单。激光有波束宽度，有测量噪声，还有多路径反射。所以我们需要一个概率模型来描述这种不确定性。
我常用的一个简化模型长这样：
// 逆传感器模型伪代码
function inverseSensorModel(z, x, cell_position):
r = distance(cell_position, x) // 格子到传感器中心的距离
phi = angle(cell_position, x) // 格子相对于传感器朝向的角度
if r > z + alpha/2 or |phi| > beta/2:
return l_prior // 超出测量范围，保持先验
if |r - z| < epsilon:
return l_occ // 终点附近，标记为占据
if r < z:
return l_free // 路径上，标记为空闲
return l_prior这里的 alpha 是距离不确定性，beta 是角度不确定性，epsilon 是终点判定阈值。这些参数需要根据实际激光雷达的噪声特性来调。
我曾经踩过的坑：有一次在玻璃幕墙环境下测试，激光直接穿透玻璃打到后面的墙上，导致地图上出现大量“鬼影”障碍物。后来我在逆传感器模型里加入了最大距离截断和多回波处理，才把这个问题压下去。如果你也在做室内导航，务必注意玻璃、镜面等反射材质。
3.3 gmapping vs Cartographer：核心差异对比
这两个算法是ROS生态里最常用的2D激光SLAM方案。很多新手问我：“老师，我该用哪个？”
我的回答是：看场景。下面这张表可以帮你快速决策。
| 对比维度 | gmapping | Cartographer |
|---|---|---|
| 核心思想 | 粒子滤波（RBPF） | 图优化（Graph SLAM） |
| 地图表示 | 概率栅格地图 | 概率栅格地图 + 子图（Submap） |
| 闭环检测 | 依赖粒子多样性，弱 | 基于分支定界搜索，强 |
| 计算资源 | 低（适合嵌入式） | 高（需要GPU或强CPU） |
| 建图精度 | 中等，易漂移 | 高，闭环后全局一致 |
| 实时性 | 好，轻量级 | 较好，但需调参 |
| 适用场景 | 小场景、低成本硬件 | 大场景、高精度需求 |
我个人的经验是：如果你用的是树莓派+RPLIDAR这种低成本组合，gmapping是更稳妥的选择。但如果你做的是仓储机器人、商用清洁机器人，需要在大面积环境里稳定运行，那Cartographer是必选项。
3.4 算法流程对比：一张图看懂
下面这张SVG图，我把两个算法的核心流程画了出来。你可以对照着看，理解它们各自的关键步骤。
3.5 工程选型建议与避坑
最后，我结合自己的项目经验，给你几条实在的建议。
- 不要盲目追求“最新”。Cartographer虽然精度高，但参数调起来非常痛苦。我见过有人调了两个月还没跑通闭环。如果你的场景只有几十平米，gmapping完全够用。
- 注意传感器同步。无论是gmapping还是Cartographer，对激光雷达和里程计的时序同步要求都很高。我习惯在驱动层就打上硬件时间戳，而不是靠ROS的近似时间同步。
- 地图分辨率不是越高越好。5cm分辨率在很多场景下已经足够。分辨率太高，不仅内存爆炸，而且更新频率跟不上，反而容易导致地图不一致。
- 逆传感器模型的参数要实测标定。不要照搬里的数值。拿一把卷尺，在几个距离上实测激光的噪声分布，然后反推模型参数。这一步偷懒，后面建图质量会大打折扣。
一个小技巧：在调试阶段，把每个栅格的对数几率值实时可视化出来，而不是只看最终的二值地图。这样你能直观看到“不确定性”的分布，快速定位传感器噪声或运动畸变的问题。
好了，这一章的内容就到这里。占据栅格地图是SLAM的基石，逆传感器模型是连接测量和地图的桥梁，而gmapping和Cartographer则是两种典型的实现路径。理解了这些，你就能根据实际需求做出合理的技术选型。