31.2.3 3D感知与场景理解：占据栅格地图

可视化演示

展示机器人激光扫描更新栅格地图、贝叶斯概率更新先验→观测→后验及逆观测模型的三区域（自由/占据/未知）概率分布。

建图动机

班长让小明细数仓库里每一粒灰尘的位置——这显然不可能，连续空间里点的数量是无限的。但班长实际只关心一个问题：这块地砖上能不能走人？把地板切成地砖，问题就从“描述无数个点”简化为“判断有限块砖”，这就是离散化的智慧。

移动机器人导航同样需要环境地图才能回答“哪里能走、哪里要绕”。连续世界的精确几何描述既昂贵又冗余，占据栅格地图干脆把空间切成网格，用一格一变量的方式牺牲细节来换计算可行。这是建图的第一动机：把不可能精确描述的世界，变成一组可以计算的单元。

技术上说，地图 m 被定义为 N 个独立格子变量的集合，每个格子 m_i 取二元值：0 表示空闲、1 表示占据。这里藏着两张关键简化：一是空间离散化，用格子代替连续坐标；二是占据假设，格子状态只用“有/无”描述，不关心障碍物到底是墙还是桌子。

$$\displaystyle m = \{ m_1, m_2, \ldots, m_N \}, \quad m_i \in \{0, 1\}$$

1. 地图的集合表达：m 表示整张地图，它由 N 个格子的状态共同组合而成。

$$m = \{ m_1, m_2, \ldots, m_N \}$$

2. 格子的二元取值：每个 m_i 只能在 0（空闲）和 1（占据）之间二选一，让复杂的几何问题退化为“是与否”的判断。

$$m_i \in \{0, 1\}$$

小明值班簿上的每一格，正是这样一个 m_i——先画红叉还是浅线，取决于他到底有没有把握。

栅格定义

小明刚开始很纠结：一个格子只写“有/没有”太武断，手电筒一晃可能看错。于是他改记“信心值”：正数表示越看越像有东西，负数表示越看越像空的，0 表示完全没把握。这个“信心值”就是占据栅格地图里的对数几率。

直接存储占据概率 p(m_i) 会带来数值问题：概率只能取 0 到 1，极端值附近加减都容易溢出边界。把概率映射到对数几率后，值域扩展为整个实数轴，更新也从乘除变为加减——这是栅格地图最关键的工程选择。

对数几率定义如下：p 越接近 1，l 越大；越接近 0，l 越小；当 p=0.5 时 l=0，恰好对应“完全未知”的先验状态。

$$\displaystyle l(m_i) = \log\frac{p(m_i)}{1 - p(m_i)}$$

1. 分子 p(m_i)：当前认为该格子被占据的概率，取值为 0 到 1 之间的小数。

$$p(m_i) \in [0, 1]$$

2. 分母 1 - p(m_i)：该格子空闲的概率，两者之比称为“发生比”，刻画占据对空闲的倍数关系。

$$1 - p(m_i) \in [0, 1]$$

3. 取对数：把发生比从 (0, +∞) 映射到 (-∞, +∞)，让后续的概率乘法更新退化为简单的加法更新。

$$\log\frac{p}{1 - p} \in (-\infty, +\infty)$$

从此小明的值班簿不再写“有/无”，而是写一排可以反复加减的数字——这就是他每天更新草图时用的“记账本”。

贝叶斯更新

小明不是每晚重新画图，而是在昨天的值班簿上“叠写”新信息：昨天的信心值 + 今晚的新观察 - 初始的偏见。为什么要减初始值？因为手电筒的观察里已经天然包含了一次“从零开始”的判断，不减去就会把出发时的先验重复记账。

数学上，每次新观测都要用贝叶斯定理把先验与似然相乘，但直接乘概率会很快饱和到 0 或 1。用对数几率表达后，这一过程退化为三项加法，这也是占据栅格地图最核心的递推公式。设 x_{1:t} 为历史位姿、z_{1:t} 为历史观测，格子 m_i 的后验对数几率可递推更新：

$$\displaystyle l(m_i \mid x_{1:t}, z_{1:t}) = l(m_i \mid x_{1:t-1}, z_{1:t-1}) + l(m_i \mid x_t, z_t) - l_0$$

1. 更新后的后验：综合到时刻 t 为止所有位姿与观测后，对该格子的最新信心分数。

$$l(m_i \mid x_{1:t}, z_{1:t})$$

2. 继承旧证据：上一时刻的后验分数作为旧笔记被完整保留，递推式让地图不用重头计算，只需增量更新。

$$l(m_i \mid x_{1:t-1}, z_{1:t-1})$$

3. 注入新观测：本次观测通过逆传感器模型折算成的分数增量，在下一节详述。

$$l(m_i \mid x_t, z_t)$$

4. 扣除先验：减去初始对数几率 l_0（通常取 0）。因为每一项都隐含先验，不扣除会重复累加，导致同一点被反复扫描时分数虚高。

$$l_0 = \log\frac{p_0}{1 - p_0}$$

值班簿上每一格的数字，就在这一次次“叠加”中向真实世界收敛：扫过十次的格子，远比扫过一次的格子更有说服力。

逆传感器模型

手电筒的光束其实非常“诚实”：光畅通无阻经过的地砖，几乎可以断定是空的——否则光会被挡住；光束尽头那个亮斑所在的地砖，大概率有东西——那是物体表面在反光；再往远处，光完全没到达的地方，只能保持“不知道”。这“光的语言”就是逆传感器模型。

传感器测距模型回答“如果环境是 m，测得 z 的概率是多少”；而建图需要反过来——“看到测量 z，格子 m_i 处于什么状态”，这就是逆传感器模型。它把一次测距 z_t（光束从当前位置出发测得的障碍距离）翻译成沿途格子的分数贡献。

对第 i 个格子，记其中心到传感器的距离为 d_i，测距噪声范围为 δ，逆传感器模型可写成分段函数：

$$\displaystyle l(m_i \mid x_t, z_t) = \begin{cases} l_{free} & d_i < z_t - \delta \\ l_{occ} & |d_i - z_t| \le \delta \\ l_0 & d_i > z_t + \delta \end{cases}$$

1. 被光束穿过的格子：格心到传感器更近，说明光束曾从它上方经过，标记为 l_free（负值，偏向空闲）。

$$d_i < z_t - \delta$$

2. 光束尽头的格子：格心距与测量值相差不到 δ，是物体表面所在的“反射环”，标记为 l_occ（正值，偏向占据）。

$$|d_i - z_t| \le \delta$$

3. 光束未到达的格子：距离更远的区域被物体遮挡，观测无法提供信息，保持先验 l_0 不变。

$$d_i > z_t + \delta$$

小明每次手电筒一扫，不是只改一个格子，而是沿光束把一整排格子的分数各归其位——这比“看到一个点改一个点”高效得多，也解释了为什么一帧激光数据能同时更新几百个格子。

更新流程

既然值班簿记的是“分数”，那最终要画成能给班长看的地图时，就要把分数翻译回“概率百分比”。分数越高翻译成概率越接近 100%，分数越低越接近 0%，分数为 0 则输出 50%——依然表示“不知道”。

从对数几率恢复占据概率，用的正是 sigmoid 函数的反变换。每一帧传感器数据到达后，机器人依次执行：先由逆传感器模型计算每个被观测格子的增量分数，再用贝叶斯递推累加，最后按需换算成概率供规划模块使用。

恢复公式如下：e^{-l} 项让分数 l 越大概率越接近 1、越小越接近 0，且结果始终落在 (0,1) 区间内，天然适配“概率”语义。

$$\displaystyle p(m_i \mid x_{1:t}, z_{1:t}) = \frac{1}{1 + e^{-l(m_i \mid x_{1:t}, z_{1:t})}}$$

1. 指数项 e^{-l}：把可正可负的分数压缩成正数，l 越大该项越接近 0。

$$e^{-l} \in (0, +\infty)$$

2. 分母 1 + e^{-l}：保证整体结果恒在 0 与 1 之间，防止概率越界。

$$1 + e^{-l} \in (1, +\infty)$$

3. 整体取倒数：完成 sigmoid 归一化，l=0 时输出 0.5，l>0 输出大于 0.5，l<0 输出小于 0.5。

$$\frac{1}{1 + e^{-l}} \in (0, 1)$$

于是值班簿的完整工作流变成：空白册子 → 每夜叠加新分数 → 需要决策时换算成概率。建图循环日复一日，地图越来越接近仓库真相。

工程实践

如果班长要求把地砖切成 1 毫米见方，小明会直接哭出来——值班簿根本画不下。格子切得越细，笔记本页数越多，写起来也越慢。真实机器人面临同样的三难：范围、分辨率与内存不可兼得。

占据栅格地图的内存占用随分辨率呈平方甚至立方增长，这是工程实现的第一道坎。其次是传感器融合：激光雷达、超声波、相机各有噪点，需要统一折算成逆传感器模型的分数贡献。此外环境并非永远静止，行人、推车会让“永恒地图”失效，必须保留在线更新通道。

假设地图长 W 宽 H，分辨率（格边长）为 δ，每格存储 B 字节，则总内存为：

$$\displaystyle M = \frac{W}{\delta} \times \frac{H}{\delta} \times B$$

1. 横向格子数 W/δ：地图长度除以单格边长，得到一行有多少个格子。

$$\frac{W}{\delta}$$

2. 纵向格子数 H/δ：同理得到列方向的格子数量，两者相乘即总格子数。

$$\frac{H}{\delta}$$

3. 乘每格字节数 B：工程上常用 1 字节存 256 级分数，或用 1 bit 只存“已知/未知”，B 越小地图越省内存。

$$B \in \{0.125, 1, 4\}$$

为了不让值班簿失控，工程师发明了分层栅格、动态分配、多分辨率地图等“压缩笔记本”的手段——本质都是在精度与体积之间做取舍。小明只负责一条走廊，用小尺度的笔记本就够了；负责整个园区，则要分层分区记录。

典型应用

值班簿画完的最终读者是巡逻路线规划员：他看一眼图上的红叉与通路，就能绕开货架、挑最短的路走。若是把平面地砖换成三维货架空间，这本“值班簿”就升级成“货架全景立体册”——每个小立方体记录哪里有空位、哪里有货物。

占据栅格地图最直接的应用是路径规划：规划器把概率图二值化为“可通行/障碍”，再交给 A* 等搜索算法找路。其次是辅助定位：机器人把实时激光扫描与栅格图匹配，校正自身位置。向三维推广则得到占据体素地图，是自动驾驶与机械臂避障的基础。

二值化决策使用阈值 τ（常取 0.5~0.65），将概率图转化为机器人可判读的通行图：

$$\displaystyle \hat{m}_i = \begin{cases} 1 & p(m_i \mid x_{1:t}, z_{1:t}) \ge \tau \\ 0 & p(m_i \mid x_{1:t}, z_{1:t}) < \tau \end{cases}$$

1. 阈值 τ：一个可调的门限，决定地图对障碍的敏感程度，调高更保守、调低更激进。

$$\tau \in [0.5, 0.65]$$

2. 判定为占据：后验概率不低于门限时输出 1，规划器禁止机器人进入该格子。

$$p(m_i) \ge \tau \Rightarrow \hat{m}_i = 1$$

3. 判定为空闲：概率低于门限时输出 0，视为可通行区域。

$$p(m_i) < \tau \Rightarrow \hat{m}_i = 0$$

从漆黑的仓库到能自动巡逻的机器人，中间只隔着一本不断被书写的值班簿：感知把世界切成格子，概率把噪声熬成信念，规划把信念变成脚步。

AI 应用场景

Python 演示

import math  class OccupancyGridMap:    """2D occupancy grid map with log-odds updates."""     def __init__(self, width, height, l_occ=0.9, l_free=-0.7):        self.width = width        self.height = height        self.l_occ = l_occ        self.l_free = l_free        self.log_odds = [0.0] * (width * height)     def to_probability(self, log_odds):        return 1.0 / (1.0 + math.exp(-log_odds))     def update_scan(self, robot_x, robot_y, angle, max_range, is_obstacle):        """Cast a beam from the robot and update cells along it."""        for r in range(1, max_range + 1):            gx = int(round(robot_x + r * math.cos(angle)))            gy = int(round(robot_y + r * math.sin(angle)))            if not (0 <= gx < self.width and 0 <= gy < self.height):                break            idx = gy * self.width + gx            if is_obstacle(gx, gy):                self.log_odds[idx] = min(self.log_odds[idx] + self.l_occ, 5.0)                break            self.log_odds[idx] = max(self.log_odds[idx] + self.l_free, -5.0)     def probability_at(self, x, y):        return self.to_probability(self.log_odds[y * self.width + x])  def ground_truth(x, y):    """True wall area used to simulate the real environment."""    return 10 <= x <= 13 and 8 <= y <= 10  def main():    grid_map = OccupancyGridMap(30, 30)    poses = [(3, 3), (10, 3), (20, 3), (20, 12),             (20, 20), (10, 20), (3, 20), (3, 12)]    for px, py in poses:        for angle_deg in range(0, 360, 30):            grid_map.update_scan(px, py, math.radians(angle_deg), 25, ground_truth)     print("建图完成, 关键格子的占据概率:")    for x, y in [(3, 3), (11, 9), (6, 6), (18, 18)]:        print("  (%2d,%2d): %.3f" % (x, y, grid_map.probability_at(x, y)))  if __name__ == "__main__":    main()

1. self.log_odds = [0.0] * (width * height)用一维数组存储二维栅格，初值 0 表示“完全未知”（对应概率 0.5）。
2. def update_scan(self, robot_x, robot_y, angle, max_range, is_obstacle):模拟一束“手电筒光”，从机器人位置出发沿 angle 方向逐格前进。
3. if is_obstacle(gx, gy):命中真实障碍格时，按逆传感器模型把该格分数上调 l_occ，并 break 截断波束。
4. self.log_odds[idx] = max(self.log_odds[idx] + self.l_free, -5.0)波束穿过的格子下调分数记录“空闲”，-5.0 是下限截断，防止数值溢出。
5. return 1.0 / (1.0 + math.exp(-log_odds))sigmoid 反变换，把对数几率兑换成 0~1 的占据概率。
6. for angle_deg in range(0, 360, 30):每个位置发 12 束波，模拟多波束测距传感器的一帧扫描。
7. if __name__ == "__main__":提供可执行入口，直接运行即可看到建图结果。

常见误区

面试题