PythonRobotics 实战：从 LIDAR 测量数据到占据栅格地图（Occupancy Grid Map）的完整构建教程

PythonRobotics 实战：从 LIDAR 测量数据到占据栅格地图（Occupancy Grid Map）的完整构建教程

【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics

导读

本文基于 PythonRobotics 仓库中的 lidar_to_grid_map 教程，系统讲解如何从 LIDAR（激光雷达）的距离–角度测量数据出发，构建概率型占据栅格地图（Occupancy Grid Map）。你将掌握：CSV 测量数据的读取与极坐标→笛卡尔坐标转换、Bresenham 直线光栅化算法的原理与实现、基于队列的泛洪填充（flood fill）算法如何补全未知区域，以及完整的光线投射（ray casting）建图管线。学完后你能独立复现lidar01.csv→ 栅格地图的完整流程，并理解lidar_to_grid_map.py中每个核心函数的调用关系与参数含义。

占据栅格地图：用概率描述环境的离散网格

占据栅格地图是一种经典的概率环境表示方法，最早由 Moravec 与 Elfes 在 1985 年提出（Hans Moravec, A.E. Elfes: High resolution maps from wide angle sonar, Proc. IEEE Int. Conf. Robotics Autom. (1985)）。其核心思想是把连续环境离散化为规则网格，每个网格单元用一个概率值描述其"被占据"的可能性：

• 数值接近 1：该栅格被占据（障碍物），在教程示例图中以红色标记；
• 数值接近 0：该栅格为自由（可通行）区域，以绿色标记；
• 数值接近 0.5：该区域未知（未被观测到）。

这种"三值语义"（占据 / 自由 / 未知）是栅格地图能够用于后续路径规划、避障的关键——规划器可以在自由区域中搜索路径，同时明确知道哪些区域信息不足。

在 PythonRobotics 中，栅格地图直接以numpy array表示，索引即栅格坐标，数值即占据概率。教程配套的示例图展示了"原始场景 → 测量散点 → 最终栅格地图"三者之间的对应关系，直观呈现整个转换链条。

第一步：读取 LIDAR 测量数据（极坐标）

数据格式：CSV 中的"角度，距离"对

LIDAR 的原始输出通常是极坐标形式——每一束激光给出一个角度和对应方向的距离。教程使用的测量文件 lidar01.csv 就是这样的逗号分隔值（CSV）文件，每行两个字段：

0.008450416037156572,0.5335 0.046902201120156306,0.5345 0.08508127850753233,0.537 0.1979822644959155,0.2605 0.21189035697274505,0.2625 ...

第一列为角度（弧度），第二列为该角度上的距离（米）。仓库中该数据文件与实现脚本位于同一目录 Mapping/lidar_to_grid_map/，运行示例脚本时会从当前目录读取它。

file_read：解析 CSV 为 numpy 数组

教程给出了file_read函数，逐行拆分逗号、将字符串转为浮点数，最终返回两个numpy数组angles与distances：

def file_read(f): """ Reading LIDAR laser beams (angles and corresponding distance data) """ measures = [line.split(",") for line in open(f)] angles = [] distances = [] for measure in measures: angles.append(float(measure[0])) distances.append(float(measure[1])) angles = np.array(angles) distances = np.array(distances) return angles, distances

在仓库源码 Mapping/lidar_to_grid_map/lidar_to_grid_map.py 中，file_read的实现与教程一致，并改用with open(f) as data的上下文管理方式，确保文件句柄被正确释放。两者的解析逻辑完全相同：measure[0]是角度、measure[1]是距离。

极坐标 → 笛卡尔坐标

拿到角度与距离后，用三角函数即可得到每个测量点的笛卡尔坐标：

import math import numpy as np import matplotlib.pyplot as plt from math import cos, sin, radians, pi ang, dist = file_read("lidar01.csv") ox = np.sin(ang) * dist oy = np.cos(ang) * dist

注意这里的坐标约定：ox = sin(angle) * distance、oy = cos(angle) * distance，即角度按y 轴为 0 方向计量（与常规"从 x 轴起算"的习惯相反）。这是因为后续在栅格地图中显示时，行索引对应 x、列索引对应 y，需要与matplotlib的imshow行列方向保持一致。

绘制原始测量时，教程用红色线段把每个测量点与原点 (0,0) 相连，并翻转 y 轴方向以匹配栅格方位：

plt.figure(figsize=(6,10)) plt.plot([oy, np.zeros(np.size(oy))], [ox, np.zeros(np.size(oy))], "ro-") # lines from 0,0 to the measurement points plt.axis("equal") bottom, top = plt.ylim() # return the current ylim plt.ylim((top, bottom)) # rescale y axis, to match the grid orientation plt.grid(True) plt.show()

运行后得到的即是雷达原始扫描的可视化——红色射线从原点向四周发散，射线端点即障碍物的探测点。

第二步：用 Bresenham 算法在栅格上画直线

原始测量只是离散的点，要变成栅格地图，需要把"从传感器到障碍物之间的空间"在栅格上逐格标记出来。这一步依赖计算机图形学中的Bresenham 直线算法：给定两个栅格坐标，它用纯整数运算高效地枚举出两点间经过的所有栅格单元。

在源码 Mapping/lidar_to_grid_map/lidar_to_grid_map.py 中，bresenham(start, end)的实现要点如下：

• 陡峭判断：若abs(dy) > abs(dx)则交换 x/y 坐标，保证算法沿 x 轴主方向推进；
• 方向规整：若起点 x 大于终点 x，则交换起终点并记录swapped，最后反转结果序列；
• 误差累积：用error = int(dx / 2.0)初始化，每步error -= abs(dy)，当误差为负时 y 方向步进并回补error += dx，这是 Bresenham 算法的经典整数误差累积技巧；
• 返回numpy数组：每个元素是[x, y]形式的栅格坐标。

函数 docstring 中还给出了一个可验证的示例（同时也作为 doctest 样例）：

>>> points1 = bresenham((4, 4), (6, 10)) np.array([[4,4], [4,5], [5,6], [5,7], [5,8], [6,9], [6,10]])

教程用一张初始值全为 0.5（未知）的50 x 50栅格图来演示该算法的效果——把直线经过的栅格置 1：

import lidar_to_grid_map as lg map1 = np.ones((50, 50)) * 0.5 line = lg.bresenham((2, 2), (40, 30)) for l in line: map1[l[0]][l[1]] = 1 plt.imshow(map1) plt.colorbar() plt.show()

再画两条边即可围出一个封闭区域，为后面的泛洪填充做准备：

line = lg.bresenham((2, 30), (40, 30)) for l in line: map1[l[0]][l[1]] = 1 line = lg.bresenham((2, 30), (2, 2)) for l in line: map1[l[0]][l[1]] = 1 plt.imshow(map1) plt.colorbar() plt.show()

第三步：泛洪填充（flood fill）补全自由区域

Bresenham 直线只标记了障碍物边界（以及部分自由区域），边界内部的区域仍是 0.5（未知）。教程用基于队列的泛洪填充算法从给定中心点出发，向四邻域扩散填充：只要邻居是 0.5 就标记为 0（自由），并在遇到障碍物（1.0）或地图边界时停止扩张。

from collections import deque def flood_fill(cpoint, pmap): """ cpoint: starting point (x,y) of fill pmap: occupancy map generated from Bresenham ray-tracing """ # Fill empty areas with queue method sx, sy = pmap.shape fringe = deque() fringe.appendleft(cpoint) while fringe: n = fringe.pop() nx, ny = n # West if nx > 0: if pmap[nx - 1, ny] == 0.5: pmap[nx - 1, ny] = 0.0 fringe.appendleft((nx - 1, ny)) # East if nx < sx - 1: if pmap[nx + 1, ny] == 0.5: pmap[nx + 1, ny] = 0.0 fringe.appendleft((nx + 1, ny)) # North if ny > 0: if pmap[nx, ny - 1] == 0.5: pmap[nx, ny - 1] = 0.0 fringe.appendleft((nx, ny - 1)) # South if ny < sy - 1: if pmap[nx, ny + 1] == 0.5: pmap[nx, ny + 1] = 0.0 fringe.appendleft((nx, ny + 1))

算法要点：

• 使用collections.deque双端队列作为待处理边界（fringe），从中心点开始，向西、东、北、南四个方向逐一检查；
• 仅当邻居值恰好等于 0.5（未知）时才标记为 0.0 并入队，因此障碍物（1.0）与自由区域（0.0）都不会被重复处理，天然保证算法终止；
• 每个栅格最多入队一次，时间复杂度为 O(栅格数)，非常高效。

源码 Mapping/lidar_to_grid_map/lidar_to_grid_map.py 中的flood_fill(center_point, occupancy_map)与教程完全一致，区别仅在于参数命名更明确。

在示例栅格图上，从中心点(10, 20)开始填充，黄色边界（Bresenham 画出的线）包围的内部区域全部变为 0（自由）：

flood_fill((10, 20), map1) plt.imshow(map1) plt.colorbar() plt.show()

第四步：完整建图管线 generate_ray_casting_grid_map

教程最后把上述所有环节串成完整管线，用仓库中的lidar_to_grid_map.py生成最终栅格地图。核心函数是generate_ray_casting_grid_map(ox, oy, xy_resolution, breshen=True)（源码见 Mapping/lidar_to_grid_map/lidar_to_grid_map.py），它提供了两条建图路径，由布尔参数breshen切换：

路径一：Bresenham 光线投射（breshen=True，默认）

1. 计算栅格地图尺寸：calc_grid_map_config根据所有测量点的最小/最大坐标向外扩展EXTEND_AREA = 1.0米，再除以xy_resolution得到栅格宽高（源码 Mapping/lidar_to_grid_map/lidar_to_grid_map.py），并打印The grid map is X x Y .；
2. 初始化占据图：全部置为 0.5（未知），即np.ones((x_w, y_w)) / 2；
3. 逐束激光投射：对每个测量点，计算其栅格坐标(ix, iy)，用bresenham((center_x, center_y), (ix, iy))从传感器中心到障碍点画线，线上所有栅格置 0（自由），障碍点本身及其相邻三个栅格置 1（占据，相当于对障碍物做膨胀）：

   occupancy_map[ix][iy] = 1.0 # occupied area 1.0 occupancy_map[ix + 1][iy] = 1.0 # extend the occupied area occupancy_map[ix][iy + 1] = 1.0 # extend the occupied area occupancy_map[ix + 1][iy + 1] = 1.0 # extend the occupied area

路径二：泛洪填充（breshen=False）

该路径先调用init_flood_fill做初步射线标记，再调用flood_fill补全所有自由区域，最后同样把每个障碍点及其相邻三个栅格置 1（源码 Mapping/lidar_to_grid_map/lidar_to_grid_map.py）。两种路径殊途同归，最终都返回(occupancy_map, min_x, max_x, min_y, max_y, xy_resolution)六元组，其中min_x/max_x/min_y/max_y记录了地图在真实坐标系下的边界，xy_resolution为栅格分辨率。

在真实数据上运行

教程用lidar01.csv的真实测量数据，配合xyreso = 0.02（0.02 米/格，即 2 cm 分辨率）构建栅格地图：

xyreso = 0.02 # x-y grid resolution ang, dist = file_read("lidar01.csv") ox = np.sin(ang) * dist oy = np.cos(ang) * dist pmap, minx, maxx, miny, maxy, xyreso = lg.generate_ray_casting_grid_map(ox, oy, xyreso, False) xyres = np.array(pmap).shape plt.figure(figsize=(20,8)) plt.subplot(122) plt.imshow(pmap, cmap = "PiYG_r") plt.clim(-0.4, 1.4) plt.gca().set_xticks(np.arange(-.5, xyres[1], 1), minor = True) plt.gca().set_yticks(np.arange(-.5, xyres[0], 1), minor = True) plt.grid(True, which="minor", color="w", linewidth = .6, alpha = 0.5) plt.colorbar() plt.show()

运行时输出地图尺寸：

The grid map is 150 x 100 .

在 2 cm 分辨率下，lidar01.csv的扫描范围被离散化为150 × 100个栅格单元。绘图细节说明：

• cmap="PiYG_r"：使用红–绿反色 colormap，绿色表示自由、红色表示占据，与教程开头的语义描述一致；
• plt.clim(-0.4, 1.4)：扩展颜色映射范围，让 0（自由）、0.5（未知）、1（占据）三个典型值在色阶上对比更明显；
• set_xticks / set_yticks配合grid(which="minor")：在每个栅格单元边界绘制白色细网格线，便于观察离散化效果。

源码 Mapping/lidar_to_grid_map/lidar_to_grid_map.py 中的main()函数与本示例逻辑一致，但以breshen=True（Bresenham 光线投射）为默认路径，并在左侧子图同步绘制原始测量散点（红色射线）与传感器原点（蓝色圆点），方便左右对照。

最终占据栅格地图：绿色自由区域、红色占据栅格与白色网格线

注：上图为教程正文中的最终结果图（文件位于docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_14_1.png），展示了PiYG_r色带下的完整占据栅格地图效果。

源码中的相关实现与测试验证

光线投射的另一实现：ray_casting_grid_map.py

除教程主讲的lidar_to_grid_map.py外，仓库还有另一个相关实现 Mapping/ray_casting_grid_map/ray_casting_grid_map.py，由 Atsushi Sakai 编写，思路略有不同：

• 采用角度分桶预计算（pre_casting）：按yawreso（偏航角分辨率，默认np.deg2rad(10.0)）把 0～2π 的角度空间划分为多个桶，预先为每个桶记录该角度方向上的所有栅格（源码 Mapping/ray_casting_grid_map/ray_casting_grid_map.py）；
• 建图时，对每个测量点，仅需在该点所属的角度桶内比较距离：桶内距离大于测量距离的栅格标记为 0.5（自由），测量点所在栅格标记为 1.0（占据）（源码 Mapping/ray_casting_grid_map/ray_casting_grid_map.py）；
• 其EXTEND_AREA = 10.0米，xyreso = 0.25米，分辨率比教程示例粗。

该实现通过预计算把"逐射线画线"优化为"按角度查表 + 距离比较"，是理解光线投射建图性能优化的好材料，但其教学主线仍以本教程的lidar_to_grid_map.py为准。

测试用例

仓库测试 tests/test_ray_casting_grid_map.py 覆盖了该模块的端到端运行：

import conftest # Add root path to sys.path from Mapping.ray_casting_grid_map import ray_casting_grid_map as m def test1(): m.show_animation = False m.main() if __name__ == '__main__': conftest.run_this_test(__file__)

该测试通过 tests/conftest.py 将仓库根目录加入sys.path，调用main()验证建图管线可完整运行，并把show_animation置为False以关闭交互式动画，确保在 CI 环境下无界面也能通过。这印证了"读取测量 → 计算栅格地图配置 → 光线投射 → 可视化"是一条可自动化验证的完整链路。

参数速查与实践建议

参数	含义	默认值 / 教程取值	影响
xy_resolution（xyreso）	栅格分辨率，即每个栅格代表的实际边长（米）	教程示例0.02，ray_casting_grid_map.py中0.25	越小地图越精细，但栅格数量按平方增长，内存与耗时随之增加
yawreso	偏航角分辨率（弧度），用于角度分桶	ray_casting_grid_map.py中np.deg2rad(10.0)	只影响角度分桶方案，教程主线的 Bresenham 路径不使用
breshen	建图方式开关	True（Bresenham 光线投射）	True逐束激光画线；False走泛洪填充路径
EXTEND_AREA	地图边界外扩距离（米）	lidar_to_grid_map.py中1.0	决定地图在测量范围外保留多少余量
占据值 / 自由值 / 未知值	栅格三态语义	1.0/0.0/0.5	路径规划与避障依赖这三种状态做决策

实践建议：

• 理解坐标约定：ox = sin(angle) * dist、oy = cos(angle) * dist，绘制时注意 y 轴翻转，否则栅格地图的方位会与真实场景镜像；
• 分辨率与成本的权衡：0.02米精度可得到 150 × 100 的栅格地图；若场景更大或计算资源有限，可适当调大xy_resolution；
• 未知区域的价值：栅格地图的 0.5（未知）状态是后续规划算法（如 D* Lite、A* 变体）判断"信息不足区域"的重要依据，不要在建图后丢弃这一信息；
• 运行示例：在仓库根目录执行python Mapping/lidar_to_grid_map/lidar_to_grid_map.py即可看到完整建图效果（该脚本需在Mapping/lidar_to_grid_map/目录下能找到lidar01.csv数据文件）。

小结

本教程走通了"LIDAR 测量数据 → 占据栅格地图"的完整链路：file_read解析 CSV 极坐标 → 三角函数转笛卡尔坐标 →bresenham光栅化射线 →flood_fill补全自由区域 →generate_ray_casting_grid_map输出最终占据图。每一步都有仓库源码（Mapping/lidar_to_grid_map/lidar_to_grid_map.py）与测试（tests/test_ray_casting_grid_map.py）佐证，可直接运行、修改与扩展。掌握这条管线后，你可以将任意"角度 + 距离"格式的 2D 激光数据转换为可供路径规划使用的概率栅格地图，这正是移动机器人环境感知与导航的基础环节。

【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics