4. 占据栅格地图原理：贝叶斯更新、对数几率与地图构建

各位同学，欢迎来到第四章。今天我们要聊的是占据栅格地图（Occupancy Grid Map）的核心原理。说实话，这是整个地图构建模块里最“数学”的一节，但也是最有工程味道的一节。你想想看，机器人怎么知道一个格子是空的还是被占的？它凭什么敢说“这里有障碍物”？

答案就藏在贝叶斯更新公式和对数几率模型里。我个人习惯把这一章称为“地图的数学骨架”。搞懂了它，后面所有建图算法你都能一眼看穿本质。

4.1 从传感器数据到概率信念

先问一个问题：激光雷达打出一束光，返回了一个距离值。这个格子到底有没有障碍物？

答案是不确定的。传感器有噪声，测量有误差，甚至可能被动态物体干扰。所以我们不能用“是/否”这种二值逻辑，而要用概率来描述。

每个栅格单元 m_i 都有一个状态：占据（occupied）或 空闲（free）。我们用 P(m_i = 1) 表示它被占据的概率。初始时，我们通常设 P(m_i = 1) = 0.5，也就是完全未知。

4.2 贝叶斯更新公式：信念的迭代器

假设我们已经有了一组历史观测 z_{1:t-1}，当前格子状态的后验概率是 P(m_i | z_{1:t-1})。现在来了一个新观测 z_t，我们要把它融合进去。

贝叶斯更新公式长这样：

P(m_i | z_{1:t}) = 
    P(z_t | m_i) * P(m_i | z_{1:t-1})
    ------------------------------------
    P(z_t | z_{1:t-1})

嗯，这里要注意。分母 P(z_t | z_{1:t-1}) 是个归一化常数，计算起来很麻烦。而且每次更新都要算一遍，效率太低。

我在项目中遇到过这个问题：早期用原始贝叶斯公式做实时建图，CPU 直接飙到 90%。后来才发现，工程上根本没人这么干。

4.3 对数几率（log-odds）模型

对数几率的定义很简单：

l(m_i) = log( P(m_i) / (1 - P(m_i)) )

反过来，从 log-odds 恢复概率：

P(m_i) = 1 / (1 + exp(-l(m_i)))

为什么用这个？因为贝叶斯更新在对数几率空间里变成了加法：

l(m_i | z_{1:t}) = l(m_i | z_{1:t-1}) + l(m_i | z_t) - l(m_i)

其中 l(m_i) 是先验对数几率（初始值通常为 0，对应概率 0.5）。

你看，乘法变成了加法。每次新观测来，只需要加一个值。这在嵌入式系统上跑起来，简直不要太爽。

4.4 逆传感器模型：从测量到格子

现在问题来了：传感器返回的是一个距离值，我怎么知道它对应哪个格子？

这就需要逆传感器模型（Inverse Sensor Model） P(m_i | z_t)。它告诉我们：给定一次测量，某个格子被占据的概率是多少。

对于激光雷达，模型通常这样设计：

• 如果格子距离 r 小于测量值 d 且大于 0，那么它大概率是空闲的。
• 如果格子距离 r 约等于测量值 d（在误差范围内），那么它大概率是占据的。
• 如果格子距离 r 远大于 d，那么它未知（不提供信息）。

一个简化的对数几率更新值表：

格子位置	含义	log-odds 更新值
r < d - ε	空闲（free）	-1.38（对应概率 0.2）
|r - d| ≤ ε	占据（occupied）	+1.38（对应概率 0.8）
r > d + ε	未知（unknown）	0（不更新）

4.5 栅格地图的构建与更新流程

好了，理论讲完了。我们来看看实际代码怎么写。下面是一个完整的更新流程伪代码：

// 初始化：所有格子 log-odds = 0（概率 0.5）
grid = new Grid(width, height, initial_log_odds=0)

// 主循环：每来一帧激光数据
function updateMap(laser_scan, robot_pose):
    // 1. 将激光点从传感器坐标系转到世界坐标系
    global_points = transform(laser_scan.points, robot_pose)
    
    // 2. 对每个激光点，计算它穿过的格子
    for each point in global_points:
        cells_hit = bresenham(robot_pose, point)  // 射线上的格子
        cell_end = point_to_cell(point)           // 终点格子
        
        // 3. 更新射线上的格子（空闲）
        for cell in cells_hit:
            if cell != cell_end:
                grid[cell].log_odds += FREE_UPDATE  // -1.38
        
        // 4. 更新终点格子（占据）
        grid[cell_end].log_odds += OCCUPIED_UPDATE  // +1.38
    
    // 5. 可选：截断 log-odds 防止溢出
    for each cell in grid:
        grid[cell].log_odds = clamp(grid[cell].log_odds, -4.0, 4.0)

这段代码我写过不下十遍。每次写都会注意一个细节：射线上的格子不要包含终点格子。否则同一个格子既被标记为空又被标记为占，逻辑就乱了。

4.6 知识体系总览

为了让你对整个流程有个全局印象，我画了一张图：

4.7 工程实践中的几个坑

最后，分享几个我在实际项目中踩过的坑：

• log-odds 截断：不截断的话，同一个格子被观测几百次后，log-odds 会变得极大或极小，导致概率接近 0 或 1，失去更新能力。我一般截断在 ±4.0，对应概率约 0.018 和 0.982。
• 射线追踪精度：Bresenham 算法在长距离时会有偏差。我建议用更精确的射线步进算法，步长设为栅格分辨率的一半。
• 动态物体处理：如果有移动的人或车，长时间观测会导致地图“拖尾”。一个简单做法是给 log-odds 加一个衰减因子，让旧观测的影响随时间减弱。

好了，这一章的内容就是这些。贝叶斯更新 + log-odds 模型，是占据栅格地图的基石。你把它吃透了，后面学 SLAM 建图就会轻松很多。