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与distancesdef 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栅格图来演示该算法的效果——把直线经过的栅格置 1import 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, breshenTrue)源码见 Mapping/lidar_to_grid_map/lidar_to_grid_map.py它提供了两条建图路径由布尔参数breshen切换路径一Bresenham 光线投射breshenTrue默认计算栅格地图尺寸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 .初始化占据图全部置为 0.5未知即np.ones((x_w, y_w)) / 2逐束激光投射对每个测量点计算其栅格坐标(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路径二泛洪填充breshenFalse该路径先调用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.020.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, whichminor, colorw, linewidth .6, alpha 0.5) plt.colorbar() plt.show()运行时输出地图尺寸The grid map is 150 x 100 .在 2 cm 分辨率下lidar01.csv的扫描范围被离散化为150 × 100个栅格单元。绘图细节说明cmapPiYG_r使用红–绿反色 colormap绿色表示自由、红色表示占据与教程开头的语义描述一致plt.clim(-0.4, 1.4)扩展颜色映射范围让 0自由、0.5未知、1占据三个典型值在色阶上对比更明显set_xticks / set_yticks配合grid(whichminor)在每个栅格单元边界绘制白色细网格线便于观察离散化效果。源码 Mapping/lidar_to_grid_map/lidar_to_grid_map.py 中的main()函数与本示例逻辑一致但以breshenTrueBresenham 光线投射为默认路径并在左侧子图同步绘制原始测量散点红色射线与传感器原点蓝色圆点方便左右对照。最终占据栅格地图绿色自由区域、红色占据栅格与白色网格线注上图为教程正文中的最终结果图文件位于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)把 02π 的角度空间划分为多个桶预先为每个桶记录该角度方向上的所有栅格源码 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_resolutionxyreso栅格分辨率即每个栅格代表的实际边长米教程示例0.02ray_casting_grid_map.py中0.25越小地图越精细但栅格数量按平方增长内存与耗时随之增加yawreso偏航角分辨率弧度用于角度分桶ray_casting_grid_map.py中np.deg2rad(10.0)只影响角度分桶方案教程主线的 Bresenham 路径不使用breshen建图方式开关TrueBresenham 光线投射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创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考