SLAM占据栅格地图深度解析：从贝叶斯更新到Bresenham建图

占据栅格地图SLAM贝叶斯更新

于 2026-10-01 06:24:59 修改

·本内容遵循CC 4.0 BY-SA版权协议

占了半年多的坑，终于把这篇《SLAM（二）：占据栅格地图》写出来了。上一篇我们从SLAM的整体框架聊到经典算法流派，这篇就把地图表示这件事单独拎出来讲透——为什么SLAM的最终产物常常不是点云，而是一张栅格地图？栅格地图里的每个格子到底存的什么值？一帧激光数据又是怎么一步步变成地图上那些黑白格子的？

如果你正准备入手SLAM、正在调ROS的map_server、或者面试时被问到“占据栅格地图的更新公式怎么推导”，这篇应该能帮你把原理和实操串起来。我会从建图者视角出发，把概率更新、坐标变换、射线遍历、参数调优和常见坑都过一遍，尽量不堆公式，但真正核心的公式还是会一步步拆开讲。

1. 为什么SLAM的终点是一张栅格地图，而不是一堆点云

先说一个很多初学者都会有的疑惑：激光雷达出来明明是点云，视觉SLAM出来至少也是稀疏特征点，为什么到了“建图”这一步，最终交出去的往往是占据栅格地图？

1.1 特征地图与栅格地图的取舍

点云和特征点地图擅长表达“几何”，却不擅长表达“可通行性”。机器人导航时关心的问题不是“这里有一堵墙的轮廓”，而是“这个区域我能不能走、那里有没有障碍物、墙体到底占多厚”。占据栅格地图直接回答的就是这类问题——每个格子要么是占据、要么是空闲、要么是未知，这种离散化的表示天然适合做路径规划和避障。

在SLAM任务里，地图表示通常有三条主流路线：

特征点地图：精度高、数据量小，适合做视觉里程计和回环检测，但很难直接用于导航。
点云地图：几何信息完整，适合做展示、重定位和高精度测量，但体量大，也没有显式的自由空间信息。
占据栅格地图：把空间切分成均匀格子，每个格子存一个“被占据的概率”，计算简单、更新方便，能直接对接代价地图和路径规划。

我个人的体感是：如果你做的是纯定位研究，特征点和点云够用；如果你的目标是让机器人在环境里自己走起来，那占据栅格地图几乎是绕不开的中间表示。

1.2 栅格地图在整个SLAM系统中的位置

把占据栅格地图放回SLAM系统里看，它其实扮演的是“输出层”的角色。前端负责帧间匹配、后端负责优化轨迹、回环检测负责修正累计漂移，最终得到的是机器人位姿序列和传感器观测，而占据栅格地图负责把这些位姿与观测融合成一个可用的环境模型。

这里有个关键点：栅格地图建图通常假设“位姿已知”。在建图过程中，机器人位姿来自里程计、激光匹配或后端优化结果，栅格更新模块不负责估计位姿，它只负责在给定位姿下，把当前帧激光数据投影到地图坐标系中，并用概率方式更新格子。这就是经典的“建图-定位分离”思想——先解决“我在哪”，再解决“环境长什么样”。

部分SLAM框架（比如Gmapping）在实现时是把定位和建图耦合在一起的，但底层逻辑依然是：每一帧激光到达时，先通过粒子滤波或扫描匹配估计出当前位姿，然后用该位姿将激光点转进地图栅格，更新格子状态。理解了这个分离逻辑，你去看Gmapping、Cartographer的源码时会轻松很多。

2. 占据栅格地图的核心：概率更新与log-odds推导

栅格地图表面上是二维数组，但每个格子存的并不是简单的0或1，而是一个概率值。为什么不用二值？因为传感器有噪声，一帧激光说“这里有个障碍物”可能出错，但十帧激光都说“这里有个障碍物”，那可信度就很高了。概率表示的本质，就是用多次观测的累积来压低噪声影响。

2.1 占据概率：从“有没有东西”到“有多确定”

我们把每个格子用一个随机变量表示，记作 (p(m_i=1))，即第 i 个格子被占据的概率。初始时没有任何信息，默认 (p=0.5)，表示“未知”。随着激光不断扫过，这个概率值会按贝叶斯规则更新。

直接更新概率有一个实操上的麻烦：贝叶斯公式里涉及到乘法，多个条件概率连乘时数值会很快趋近0或1，造成数值下溢，而且乘法的累加形式不便于工程实现。业界通用的做法是把概率转换到**对数几率（log-odds）**空间，用加法代替乘法。

公式是这样的：

[ l_i = \log \frac{p(m_i=1)}{p(m_i=0)} ]

反过来：

[ p(m_i=1) = \frac{1}{1 + \exp(-l_i)} ]

初始 (p=0.5) 时，(l=0)。每来一帧观测，对数几率值直接相加：

[ l_i \leftarrow l_i + \Delta l ]

(\Delta l) 根据当前栅格是被“测量为占据”还是“测量为空闲”来决定，通常取固定常量。这种做法在工程上极好用：不用管连乘、不用归一化，只需要一个数组做加减法，最终要输出地图时再用 sigmoid 把 log-odds 转回概率。

2.2 反演测量模型与正演模型：对数几率更新的来龙去脉

知道怎么更新还不够，还得知道更新量 (\Delta l) 为什么是那个值。这就要说到传感器的两种建模方式。

正演测量模型 (p(z \mid m)) 描述的是“如果环境地图已知，那么传感器会观测到什么”。这个模型对应激光雷达的物理特性：光束打中物体返回测距值，没打中返回最大量程。但正演模型在栅格更新里很难直接使用，因为要穷举所有格子的状态组合，计算量爆炸。

实际工程里用的是反演测量模型 (p(m \mid z))，它直白地回答：当激光雷达在某位姿下测到一个值 (z) 时，地图上哪些格子更可能是占据、哪些更可能是空闲。常用的简化模型是：

激光命中点所在的栅格：测量为“占据”；
从传感器位置到命中点之间穿过的所有栅格：测量为“空闲”；
没有激光覆盖到的栅格：不更新，保持原值。

在这个模型下，我们可以用固定的占据更新值和空闲更新值来更新log-odds。假设 (l_{occ}) 和 (l_{free}) 分别对应“测量为占据”和“测量为空闲”的对数几率增量，那么每个格子的更新规则就是两条if语句的事。

2.3 一个小例子的完整概率演算

我习惯用一个小例子帮自己理清整个更新流程。假设某格子初始未知，即 (p=0.5)，(l=0)。传感器打开，一帧激光从格子上方扫过，该格子被判定为“空闲”。

取占据更新量 (l_{occ}=0.9)，空闲更新量 (l_{free}=-0.4)，更新后：

[ l = 0 + (-0.4) = -0.4 ]

转回概率：

[ p = \frac{1}{1 + \exp(0.4)} \approx 0.401 ]

注意，此时格子概率只是略低于0.5，并不能代表十分确信“这里没东西”。但如果连续5帧激光都穿过这个格子，每帧都加-0.4，则：

[ l = -0.4 \times 5 = -2.0 ]

[ p = \frac{1}{1 + \exp(2.0)} \approx 0.119 ]

这下置信度就上来了。反过来，如果某个格子连续多次被激光命中，累加 (l_{occ})，概率会迅速逼近0.9以上。这就是占据栅格地图“越观测越确定”的本质。

3. 从一帧激光数据到地图更新：坐标变换与射线遍历实操

原理清楚了，接下来是工程实现。这一节带你走一遍最核心的流程：把一帧激光扫描数据转成栅格坐标，再用射线遍历更新地图。

3.1 从传感器坐标系到栅格坐标：三步变换

激光雷达测到的每个点，最初表达在“传感器坐标系”下，通常是极坐标形式：距离 (r) 和角度 (\theta)。要放进地图的栅格数组里，需要经过三步变换：

极坐标转直角坐标，得到传感器坐标系下的点：

[ x_s = r \cos\theta, \quad y_s = r \sin\theta ]

从传感器坐标系转到机器人坐标系。多数情况下激光雷达安装在机器人底盘上，存在一个固定的外参变换（平移加旋转）。
从机器人坐标系转到世界（地图）坐标系。这一步用的是SLAM系统输出的机器人位姿 ( (x, y, \theta))。

完成坐标变换后，得到地图坐标系下的点 ((x_w, y_w))，接下来把它映射到栅格上：

[ g_x = \left\lfloor \frac{x_w - \text{origin}_x}{\text{resolution}} \right\rfloor ]

[ g_y = \left\lfloor \frac{y_w - \text{origin}_y}{\text{resolution}} \right\rfloor ]

其中 resolution 是栅格分辨率，单位是米/格；origin 是地图原点在真实世界坐标中的位置。这组公式决定栅格坐标和世界坐标的对应关系，ROS 里 map 的元数据（origin、resolution、width、height）就是干这个用的。

这里面有个容易踩的坑：栅格数组的行列顺序和图像坐标一致（y轴向下），而机器人坐标系的y轴向上，如果不做翻转，建出来的图会上下颠倒。我见过不少新手在这个地方栽跟头，现象就是地图倒是能出来，但和实际环境是镜像关系。

3.2 用Bresenham直线算法标记射线路径

假设传感器在栅格 ((x_0, y_0))，激光命中点在栅格 ((x_1, y_1))。从传感器到命中点之间，激光是“穿过”了一片区域的，这些区域没有障碍物，应该被标记为“空闲”。怎么找出这条射线上经过的所有格子？

答案是画直线算法，最常用的是Bresenham。它的核心思想是：给定两个栅格点，用一个整数运算的递推过程，逐步找出两点间经过的所有栅格单元。这个算法的好处是计算量小，不需要浮点运算，非常适合在嵌入式设备或高频更新场景下使用。

伪代码大致长这样：

TEXT

function bresenham(x0, y0, x1, y1):

points = []

dx = abs(x1 - x0)

dy = -abs(y1 - y0)

sx = 1 if x0 < x1 else -1

sy = 1 if y0 < y1 else -1

err = dx + dy

while true:

points.append((x0, y0))

if x0 == x1 and y0 == y1:

break

e2 = 2 * err

if e2 >= dy:

err += dy

x0 += sx

if e2 <= dx:

err += dx

y0 += sy

return points

实际操作时，把返回的points里除终点外的所有栅格标记为“空闲”，把终点栅格标记为“占据”。这样一帧激光的每个命中点都对应一条射线，整帧数据遍历完，地图更新就完成了。

3.3 代码示例：最小占据栅格建图器

为了让你能直接跑起来看效果，我写了一个最小可运行的Python示例，模拟机器人在一个房间模型里移动并发射激光，实时更新栅格地图。代码刻意精简，去掉ROS依赖，方便直接运行和调试。

PYTHON

import numpy as np

import math

class OccupancyGridMap:

def __init__(self, width, height, resolution, origin_x, origin_y):

self.width = width

self.height = height

self.resolution = resolution

self.origin_x = origin_x

self.origin_y = origin_y

# log-odds 空间存储，初始全为0，即概率0.5（未知）

self.log_odds = np.zeros((height, width), dtype=np.float32)

self.l_occ = 0.9

self.l_free = -0.4

def world_to_grid(self, x, y):

gx = int(math.floor((x - self.origin_x) / self.resolution))

gy = int(math.floor((y - self.origin_y) / self.resolution))

return gx, gy

def update_ray(self, robot_x, robot_y, hit_x, hit_y):

x0, y0 = self.world_to_grid(robot_x, robot_y)

x1, y1 = self.world_to_grid(hit_x, hit_y)

# 越界检查

if not (0 <= x1 < self.width and 0 <= y1 < self.height):

return

points = self._bresenham(x0, y0, x1, y1)

# 射线上除终点外标记空闲

for (gx, gy) in points[:-1]:

if 0 <= gx < self.width and 0 <= gy < self.height:

self.log_odds[gy, gx] += self.l_free

# 终点标记占据

self.log_odds[gy, gx] += self.l_occ

def _bresenham(self, x0, y0, x1, y1):

points = []

dx = abs(x1 - x0)

dy = -abs(y1 - y0)

sx = 1 if x0 < x1 else -1

sy = 1 if y0 < y1 else -1

err = dx + dy

while True:

points.append((x0, y0))

if x0 == x1 and y0 == y1:

break

e2 = 2 * err

if e2 >= dy:

err += dy

x0 += sx

if e2 <= dx:

err += dx

y0 += sy

return points

def probability_map(self):

return 1.0 - 1.0 / (1.0 + np.exp(self.log_odds))

这段代码的核心逻辑就是：维护一个 log-odds 数组，来一帧激光就更新对应射线上的格子。注意没有写“未知格子初始概率”的额外逻辑，因为 log-odds 为0时概率正好是0.5，天然表示未知。

3.4 建图主循环：位姿、扫描更新与地图保存

有了地图类和更新函数，接下来模拟一帧帧激光扫描。假设机器人在一个带墙的房间里直线前进，每隔0.1米发一束模拟激光，命中点通过射线求交得到：

PYTHON

def simulate_laser(robot_x, robot_y, angle, walls):

# 简化模拟：在有限范围内按步长推进，检测是否撞墙

for r in np.arange(0.1, 10.0, 0.05):

x = robot_x + r * math.cos(angle)

y = robot_y + r * math.sin(angle)

if any(abs(x - wx) < 0.05 and abs(y - wy) < 0.05 for (wx, wy) in walls):

return x, y

return robot_x + 10.0 * math.cos(angle), robot_y + 10.0 * math.sin(angle)

grid_map = OccupancyGridMap(200, 200, 0.05, -5.0, -5.0)

walls = []

for i in range(100):

walls.append((-4.0 + i * 0.08, -4.0))

walls.append((-4.0 + i * 0.08, 4.0))

walls.append((-4.0, -4.0 + i * 0.08))

walls.append((4.0, -4.0 + i * 0.08))

# 模拟机器人沿x轴前进，每帧激光覆盖-90度到90度

for robot_x in np.arange(-3.0, 3.0, 0.15):

robot_y = 0.0

for angle_deg in range(-90, 91, 3):

angle = math.radians(angle_deg)

hx, hy = simulate_laser(robot_x, robot_y, angle, walls)

grid_map.update_ray(robot_x, robot_y, hx, hy)

prob = grid_map.probability_map()

# 概率大于0.7视为占据，小于0.3视为空闲，中间视为未知

occupancy = np.zeros_like(prob)

occupancy[prob > 0.7] = 1

occupancy[prob < 0.3] = -1

跑完这段代码，你会得到一个二维数组，其中1代表墙、-1代表空闲、0代表未知。如果想可视化，直接 plt.imshow(occupancy, cmap='gray') 就能看到一面由点构成的墙。这个例子虽然简单，但把建图主循环的关键要素——位姿输入、激光模拟、射线更新、概率输出——全部串起来了。

实际工程中，主循环还会处理更多细节，比如多帧激光的时间同步、位姿插值、运动畸变去除，但核心骨架和上面这段代码是一致的。先把最小闭环跑通，再去加工程细节，学习效率会高很多。

4. 栅格地图质量的工程影响因素与调参思路

代码能跑出地图只是第一步，地图好不好用是另一回事。占据栅格地图的最终质量，不只看“格子更新得对不对”，还受分辨率、更新参数、传感器噪声、动态物体等多方面因素影响。这一节把工程里最常遇到的几个影响因素挨个过一遍。

4.1 分辨率与地图尺寸：一个带计算的取舍

分辨率决定了一个栅格代表物理世界多大面积。分辨率越高，地图越精细，但内存和计算量按平方增长。选分辨率时需要考虑两个约束：传感器角分辨率、下游导航的最小通过尺寸。

举个例子。假设激光雷达的角分辨率是0.5度，在10米处相邻两束激光的间距大约是：

[ d = 10 \times \tan(0.5^\circ) \approx 0.087\text{ m} ]

如果栅格分辨率设成0.01米，一束激光的命中点周围会有大量栅格根本没有激光覆盖，地图会显得稀疏；如果设成0.2米，小障碍物和窄通道就会被直接抹平。通常2D激光SLAM选0.05米是比较稳妥的起点。

地图尺寸和分辨率一起决定数组大小。一个100米×100米的区域，如果用0.05米分辨率，栅格数是：

[ \frac{100}{0.05} \times \frac{100}{0.05} = 2000 \times 2000 = 4,000,000 \text{ cells} ]

每个格子存float32，仅log-odds数组就需要16MB。如果后续再做多分辨率地图或者3D体素化，内存会涨得很快。这也是为什么栅格地图在2D导航场景下特别流行，但到了3D场景大家会转向OctoMap这类自适应分辨率数据结构。

4.2 更新参数：占据增量与空闲增量的取值逻辑

我在3.3节的代码里取了 (l_{occ}=0.9)、(l_{free}=-0.4)，这两个值不是随便拍的，它们表达了传感器模型的两个重要性质：

命中点判定为占据的置信度应该高于射线穿过区域判定为空闲的置信度。因为激光命中往往意味着“这里确实有物体”，而空闲判定依赖“光束没打到东西”，受动态遮挡和最大量程截断影响更大。
这两个值的绝对值差异决定了地图的“灵敏度”。(l_{occ})太大，地图容易把噪声点也当成墙；(l_{free})太大，细长障碍物容易被射线“抹掉”。

实际调参时，我一般从 (l_{occ}=0.85)、(l_{free}=-0.4) 起步，然后观察两类现象：如果地图上出现大量“鬼影墙”，适当调低 (l_{occ})；如果真实墙体出现断裂（尤其细柱子被打掉），适当调高 (l_{occ}) 或降低 (l_{free}) 的绝对值。

还有一个容易被忽略的最终阈值问题。建图完成后，从概率地图到二值地图需要设两个阈值：概率大于0.7判为占据、小于0.3判为空闲、中间保持未知。阈值和更新增量是一个配套系统，不要单独调一个。比如你调大了 (l_{occ})，占据判定阈值也可以适当调高，否则很容易把半确定的格子也当成墙。

4.3 传感器噪声与动态物体的处理思路

激光雷达的噪声来源主要有三类：测量噪声、最大量程截断、动态物体。测量噪声会让命中点位置发生偏移，反映在地图上就是墙体外侧出现若干个散射状小概率占据格。最大量程截断会让本来超出量程的射线变成“空闲”判定，如果这个方向实际有墙但超出了量程，墙反而会被抹掉。

处理测量噪声我用的办法是在射线更新前加一个范围滤波：距离小于0.1米或大于最大量程95%的命中点直接丢弃，不参与更新。这个操作简单粗暴但非常有效，能去掉大量无意义更新。

动态物体（比如路过的行人）是栅格地图的天然敌人。如果建图时环境里有人走动，地图上会出现一条半透明的“人形轨迹”。缓解手段有两个：一是采用多帧投票策略，某格只有被连续多帧判为占据才累加 (l_{occ})，间歇性出现的不稳定占据会被平滑掉；二是建图完成后做一次“清理”步骤，把出现频率低于阈值的占据格重置为未知。Cartographer里虽然没直接做这种清理，但它的submap创建逻辑里包含了相似的思想。

4.4 地图坐标、原点与下游导航的对接

地图建完不是终点，还要让导航模块用起来。ROS生态里这个环节基本已经被标准化：map.yaml里写入分辨率、原点、尺寸，map.pgm里存灰度图（像素值0-255，越黑占据概率越高），map_server负责读取并发布成 costmap。

这里我想强调一个经验：发布地图时的原点（origin）最好保持建图时的原点一致，并且整张地图的坐标系要和机器人的里程计坐标系校准。如果建图完成后你重新设置了地图原点，或者机器人底盘换了安装位置，而代价地图里的机器人footprint没有同步调整，导航时会出现“地图看着正常，但机器人始终偏着一截”的诡异问题。

我见过一个典型案例：地图本身建得很标准，但机器人一上导航就像喝醉了酒一样绕圈，最后发现是 map 的 origin 和 base_link 之间的TF树里多了一个90度的旋转偏移。所以如果你遇到“地图没问题、导航就是乱跑”的排查场景，第一反应应该是检查TF，而不是怀疑建图。

5. 在地图中发现问题的排查实例与进阶方向

占据栅格地图学到这里，你能跑通一个最小建图器，也知道参数怎么调了。但真正常见的“建图不当”问题，往往不是更新公式出错，而是整个流程中某个环节的“隐性假设”被打破了。这一节分享几个排查实例和进阶方向。

5.1 一张模糊地图背后是哪一步出了问题

假设你建出来的地图整体轮廓是对的，但墙体两侧出现明显的“虚影”和“双重墙”。这通常不是栅格更新公式的问题，而是位姿不准导致的。因为栅格建图的前提是“位姿已知”，如果位姿有偏差，同样的墙不同帧之间会映射到不同的栅格上，多帧叠加后就会出现“糊墙”。

处理手段是分两步走：先检查里程计标定是否准确，尤其轮式机器人要确认轮距和轮径；再看前端匹配是否正常，比如激光匹配初值给得是否合理。Gmapping和Cartographer里都有位姿预测和修正机制，但最终精度上限仍然受制于里程计/IMU的输入质量。这里有个反直觉的点：栅格更新本身不会让地图变清晰，它只会让多次观测的统计结果稳定下来；地图的清晰度上限在定位精度，不在建图模块。

5.2 追赶移动物体导致地图出现“长尾”的排查

动态物体在静止环境中留下的“长尾”，是另一个高频问题。我调试过程中发现，这种现象和更新参数的交互方式很有意思：如果 (l_{free}) 的绝对值设置得比 (l_{occ}) 小很多，那么动态物体扫过某区域时，空闲更新不足以抵消该物体被检测为占据的更新，于是轨迹会被保留下来。

排查顺序是这样的：先确认动态物体的确存在于建图时段；再检查是否需要降低 (l_{occ})；若地图用于长期导航，则考虑采用多帧一致性的后处理。不过要提醒的是，“后处理清理”多半是事后补救，最可靠的办法还是让建图时环境尽量静止，或者使用语义信息滤除动态物体。

5.3 从2D走到3D：体素占据和OctoMap

2D占据栅格地图能解决导航问题，但遇到立体环境（比如多层货架、无人机巡检）时就不够用了。3D场景常用的扩展是 OctoMap，它用八叉树结构存储体素的占据概率，好处是在空旷区域能自动合并大块空闲体素，节省内存。

从2D栅格到OctoMap的思维迁移并不难：核心仍然是对数几率更新，只是从“二维数组”变成“树状结构”，射线遍历从“Bresenham画线”变成“沿射线访问八叉树节点”。工程上，如果后续要做三维重建和自主探索，先把2D占据栅格地图的原理吃透，再切换OctoMap，学习成本不会太高。

5.4 对齐ROS生态：从零写一版自己的栅格地图发布器

如果不想直接调Gmapping全家桶，可以试着自己写一个节点，读取激光话题和里程计话题，在回调里做坐标变换和栅格更新，最后用 nav_msgs/OccupancyGrid 消息发布地图。

nav_msgs/OccupancyGrid 的data字段是一个整数数组，0表示空闲、100表示占据、-1表示未知。转换逻辑很简单：

PYTHON

prob = grid_map.probability_map()

data = np.full(prob.shape, -1, dtype=np.int8)

data[prob > 0.7] = 100

data[prob < 0.3] = 0

# 注意ROS坐标约定，行对应y轴，发布时可能需要翻转

这步做完，你就能在Rviz里实时看到自己写的地图慢慢成型。很多框架里的建图模块其实就是这套逻辑的工程化实现，自己动手写一遍之后，再去读源码，理解效率会完全不一样。

做SLAM这些年，我最大的感受是：占据栅格地图看起来是个非常基础的概念，但几乎所有后续模块——代价地图、路径规划、自主探索、动态避障——都建立在它之上。你把这套概率更新的直觉建立起来，后面的路会顺畅很多。如果读完这篇收获了一点点，下一篇我们继续聊SLAM的核心组件，建议先把代码按照上面的例子自己跑一遍，踩一遍坑，再往下走。