简介基于SLAM的规划算法仿真复现Python源码包面向需要完成毕业设计、课程设计以及SLAM算法学习与二次开发的学生和开发者。包内共42个文件主要包含Python源码覆盖FastSLAM1、FastSLAM2、EKF SLAM等经典算法实现、仿真图像与GIF动画展示算法运行结果、配置文件如yaml/yml环境与参数配置、说明文档包括Markdown笔记和PDF说明等整体大小约11.55MB。其中提供了完整的2D SLAM示例程序内含地图构建、数据解析、配置管理等功能模块并附有运行依赖清单算法实现均经过测试可直接运行并输出可视化结果便于理解SLAM原理和规划策略。代码结构清晰注释明了便于在此基础上进行功能扩展或算法改进是完成课程设计、毕业设计及项目初期立项的实用参考。目前已有482人浏览学习值得下载使用。1. 从SLAM地图到规划算法仿真你到底卡在哪一步搭一个移动机器人系统时最磨人的往往不是SLAM本身而是SLAM跑通之后那个高分辨率栅格地图丢给路径规划模块时规划器要么直接飞出可通行区域要么绕远路要么在狭窄过道里不停重规划。很多人在这一站卡了一两个星期。其实问题不在算法实现而在你忽略了一个前置步骤SLAM输出的地图模型和规划器要求的地图模型并不是同一种东西。这个以“基于SLAM的规划算法仿真复现python源码.zip”为主题的工程核心就是把这条链路拆开用纯Python在本地把建图结果、地图转换、路径规划和仿真可视化完整串起来。这篇博客直接面向那些拿着SLAM建图数据、想快速评估规划算法效果的人也适合准备SLAM面试、想从源码层面讲清楚“建图如何服务于导航”的工程师。你会发现一个几百行代码的Python仿真环境足够复现A*、RRT*等主流路径规划算法并能在你真实采集的地图上做交互式调试。接下来所有内容均基于常见工具链不依赖特定的商业软件。2. 占据栅格地图与规划算法选型SLAM输出的东西到底怎么用2.1 栅格地图不是图像是概率分布SLAM建图常见产物是栅格地图Occupancy Grid Map比如ROS里的map.pgm配合map.yaml。像素的灰度值不是单纯的颜色而是该像素被障碍物占据的概率。白色通常代表空闲free黑色代表占据occupied灰色代表未知unknown。处理时最关键的一步是按阈值把灰度图转成0/1/2的三值地图常见阈值是灰度值小于某值视为占据大于另一值视为空闲中间视为未知。分辨率是另一个决定性参数。例如一个10m乘10m的区域地图分辨率0.05m就是200x200个栅格。分辨率越大内存和计算量按平方增长。规划时通常不需要原始分辨率而是对地图做降采样或膨胀后使用否则A*搜索节点过多实时性很难保证。我在实际复现时一般先读取map.yaml里的resolution和origin再决定要不要重采样到0.1m或0.2m作为规划地图。2.1.1 为什么直接使用SLAM原始地图会引发“碰撞假象”很多人在仿真中遇到规划路径穿过障碍物不是算法错了而是地图阈值没设对。SLAM建图时激光雷达对玻璃、黑墙、低反光物体的处理经常出现“伪占据”直接套用默认阈值会让算法以为某个区域完全不可通行。我一般会把灰度地图打印成直方图看看分布选取两个谷底的中间值作为阈值。另外对地图做一次形态学膨胀能显著减少路径贴墙的问题。这些都是规划前必须处理的地图层面的“坑”。2.2 规划算法分类图搜索、采样、插值路径规划算法按策略大致分成三类。第一类是基于图搜索典型如Dijkstra、A*它们把栅格地图看作图节点搜索路径时保证分辨率完备性即只要存在路径就能在给定离散粒度下找到。第二类是基于采样典型如RRT、RRT*适合高维空间或连续空间不需要显式栅格化所有点但在栅格地图上需要做碰撞检测。第三类是基于插值或优化比如多项式规划、贝塞尔轨迹生成常用于生成平滑轨迹但前提是先有一个无碰撞的几何路径。在做SLAM仿真复现时最优先复现A和RRT因为既能验证地图模型是否正确又能对比不同策略在同样地图上的效果。2.2.1 A*在不同应用场景下的属性取舍在扫地机器人这种二维平面场景里A配合8邻域搜索启发函数用欧氏距离或对角距离效果就很稳定。但在泊车路径规划算法里A需要加入车辆运动学约束变成Hybrid A*。这里注意如果你的目标是把SLAM和规划做成一个通用仿真框架先把经典的网格A*复现清楚再加上状态栅格扩展否则一上来就做带非完整约束的规划调试难度会成倍增加。这也是常见做法中的推荐顺序。RRT则适合处理窄通道地图。它不需要将地图离散成网格只需随机采样点并连接入树中再通过rewire操作优化代价。但RRT在网格地图上的表现很依赖距离度量和采样分布这一点后续会在仿真中验证。3. Python仿真环境与地图模型把SLAM输出变成自治系统可用的栅格3.1 搭建最小可运行的地图模块仿真复现不需要安装庞大框架Python标准库加上numpy和matplotlib就够。地图模块的核心是维护一个二维数组并提供查询和修改接口。我习惯用OccupancyGrid类封装它内部存储一个float数组值域0到10表示空闲1表示占据未知用0.5表示。对外提供is_free、is_occupied和get_resolution方法。这样规划算法不需要关心地图数据从哪里来无论是SLAM输出还是随机生成的测试地图都能统一处理。下面是一个最小实现文件名为occupancy_grid.py。import numpy as np class OccupancyGrid: def __init__(self, data, resolution0.1, origin(0, 0)): data: 2D numpy array, value in [0, 1], 1 means occupied resolution: meters per cell origin: (x, y) of cell (0, 0) in world coords self.data np.asarray(data, dtypenp.float32) self.resolution resolution self.origin origin def is_free(self, x, y): Check if world coordinate (x, y) is free. col int((x - self.origin[0]) / self.resolution) row int((y - self.origin[1]) / self.resolution) if row 0 or row self.data.shape[0] or col 0 or col self.data.shape[1]: return False return self.data[row, col] 0.2 def inflate(self, radius_meters): Inflate obstacles by given radius in meters. radius_cells max(1, int(radius_meters / self.resolution)) # Use binary dilation via scipy or simple cv2.dilate import cv2 binary (self.data 0.5).astype(np.uint8) kernel cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (radius_cells * 2 1, radius_cells * 2 1)) inflated cv2.dilate(binary, kernel) # Merge inflated obstacles into original map inflated_map np.maximum(self.data, inflated) return OccupancyGrid(inflated_map, self.resolution, self.origin)3.1.1 逻辑说明与参数说明is_free方法把世界坐标转换为栅格行列坐标。注意numpy的行是y方向列是x方向这跟笛卡尔坐标系的x-y有转置关系容易搞错。inflate方法用来做障碍物膨胀这里的cv2.MORPH_ELLIPSE能生成椭圆形结构元素接近机器人轮廓。参数radius_meters需要根据机器人物理半径设定例如机器人半径0.3m膨胀半径至少设为0.3m再额外预留0.1m安全距离。膨胀后地图会“变胖”一圈路径自然远离障碍物。从SLAM生成这类地图时需要先读取数据。假设你已经把激光SLAM建图结果保存成CSV或NumPy数组直接读入即可。如果是ROS环境里的pgm文件可以用cv2.imread加载灰度图然后除以255得到0-1数据。有一点要留意pgm的原点通常在地图中心或某个角落加载时必须用map.yaml的origin字段对齐坐标系否则路径会整体偏移。3.2 地图可视化与数据流验证地图模块正确性直接影响后续所有规划效果所以一定要先做可视化验证。我用以下方式验证随机生成或加载一张SLAM真实地图打印栅格大小和占据比例并显示膨胀前和膨胀后的对比图。import matplotlib.pyplot as plt import numpy as np from occupancy_grid import OccupancyGrid # Generate a simple test map: 50x50 with a few obstacles map_data np.zeros((50, 50), dtypenp.float32) map_data[20:30, 10:15] 1.0 # vertical wall map_data[10, 30:40] 1.0 # horizontal wall grid OccupancyGrid(map_data, resolution0.1) inflated grid.inflate(0.3) fig, axes plt.subplots(1, 2, figsize(10, 5)) axes[0].imshow(grid.data, cmapgray_r) axes[0].set_title(original SLAM grid) axes[1].imshow(inflated.data, cmapgray_r) axes[1].set_title(inflated grid) plt.show()运行后如果两幅图差异明显说明膨胀代码生效。这里我故意用局部障碍物测试因为比整个地图更直观。实践中你要用真实SLAM地图替换map_data比如从np.load(my_slam_map.npy)载入。这一步做好了规划仿真才算迈出第一步。4. 在SLAM地图上复现A和RRT代码、参数与真实地图测试4.1 A*栅格规划从8邻域到性能调优A是复现规划算法的基本功。它的核心是维护两个集合开放集和闭合集每次从开放集中取代价最小的节点扩展。代价函数f(n)g(n)h(n)其中g是从起点到当前节点的实际代价h是启发式估计代价。在8邻域地图中h通常取欧氏距离或对角距离。下面这段代码实现了标准的8邻域A同时返回路径和访问节点数便于分析性能。import heapq import numpy as np from occupancy_grid import OccupancyGrid def a_star(grid, start, goal, heuristiceuclidean): grid: OccupancyGrid object start, goal: (x, y) world coordinates return: list of (x, y) waypoints, or None if not found start_cell grid_to_cell(grid, start) goal_cell grid_to_cell(grid, goal) open_list [] heapq.heappush(open_list, (0.0, start_cell)) came_from {} g_score {start_cell: 0.0} f_score {start_cell: heuristic_cost(start_cell, goal_cell, heuristic)} while open_list: current heapq.heappop(open_list)[1] if current goal_cell: return reconstruct_path(came_from, current, grid) for dy in [-1, 0, 1]: for dx in [-1, 0, 1]: if dx 0 and dy 0: continue neighbor (current[0] dx, current[1] dy) # Check bounds and collision if not grid.is_free_cell(neighbor): continue tentative_g g_score[current] (1.414 if dx ! 0 and dy ! 0 else 1.0) if tentative_g g_score.get(neighbor, float(inf)): came_from[neighbor] current g_score[neighbor] tentative_g f tentative_g heuristic_cost(neighbor, goal_cell, heuristic) heapq.heappush(open_list, (f, neighbor)) return None4.1.1 代码逻辑说明和参数解析grid.is_free_cell是3.1节中is_free的栅格坐标版本你需要补一个方法。heuristic_cost用欧氏距离计算reconstruct_path从目标点回溯came_from表得到路径。这里有个性能关键点对neighbor做is_free检查时会在每个循环调用多次如果地图是200x200且搜索范围大性能可能会崩。常见优化是预先计算一张布尔可通行表把碰撞检测变成O(1)数组索引。参数方面最容易调的是启发权重。标准A的权重为1修改为大于1就变成weighted A搜索速度更快但路径可能次优。在SLAM地图上通常权重1.2到2之间能明显减少扩展节点数。如果你复现出来路径贴墙先检查地图膨胀而不是调启发。另外8邻域相比4邻域路径更短但转向更频繁需要后续对路径做平滑。4.2 RRT*采样规划碰撞检测与渐近最优RRT相比标准RRT多了重新连接过程。它在每次迭代生成新节点后不仅尝试连接最近节点还会搜索半径范围内的其他节点看是否能用更低代价连接到当前新节点。这个操作让路径逐步向最优靠拢。下面是简化版RRT实现适用于二维连续地图。import numpy as np from occupancy_grid import OccupancyGrid def rrt_star(grid, start, goal, max_iter5000, step_size0.5, rewire_radius2.0): grid: OccupancyGrid with inflated obstacles start, goal: (x, y) world coords nodes [np.array(start, dtypefloat)] parent [] # parallel list of parent indices costs [0.0] goal_threshold step_size * 0.5 for _ in range(max_iter): # Sample random point (with 10% bias to goal) if np.random.rand() 0.1: rand_point np.array(goal, dtypefloat) else: margin 0.5 x_min, x_max grid.origin[0] margin, grid.origin[0] grid.data.shape[1] * grid.resolution - margin y_min, y_max grid.origin[1] margin, grid.origin[1] grid.data.shape[0] * grid.resolution - margin rand_point np.array([np.random.uniform(x_min, x_max), np.random.uniform(y_min, y_max)]) # Find nearest node distances [np.linalg.norm(np.array(node) - rand_point) for node in nodes] nearest_idx int(np.argmin(distances)) nearest_point nodes[nearest_idx] # Steer toward random point direction rand_point - nearest_point dist np.linalg.norm(direction) if dist 1e-6: continue new_point nearest_point (direction / dist) * min(step_size, dist) # Collision check along segment if not is_segment_free(grid, nearest_point, new_point): continue # Find neighbors within rewire radius neighbors [] for i, node in enumerate(nodes): if np.linalg.norm(np.array(node) - new_point) rewire_radius: neighbors.append(i) # Choose parent with lowest cost best_parent nearest_idx best_cost costs[nearest_idx] np.linalg.norm(new_point - nearest_point) for i in neighbors: potential_cost costs[i] np.linalg.norm(new_point - np.array(nodes[i])) if potential_cost best_cost and is_segment_free(grid, nodes[i], new_point): best_parent i best_cost potential_cost # Add new node nodes.append(new_point) parent.append(best_parent) costs.append(best_cost) # Rewire neighbors for i in neighbors: potential_cost best_cost np.linalg.norm(nodes[i] - new_point) if potential_cost costs[i] and is_segment_free(grid, new_point, nodes[i]): parent[i] len(nodes) - 1 costs[i] potential_cost # Check goal if np.linalg.norm(new_point - np.array(goal)) goal_threshold: return reconstruct_rrt_path(nodes, parent, len(nodes) - 1) # If max_iter reached, return best path if any node near goal return None4.2.1 参数调试关键点step_size决定树的分辨率。如果地图是0.05m分辨率step_size取0.3到0.5m比较合适太小导致迭代次数爆炸太大容易穿越窄通道。rewire_radius是渐近最优的核心太小会退化为RRT路径不平滑太大会让每次迭代O(n)的搜索开销太高。我一般设为step_size的3到4倍。max_iter不要固定可以观察节点数和路径代价的变化来动态调整仿真阶段用5000次迭代足够看到效果。还有一个必须检查的点is_segment_free要对线段上的采样点做碰撞检测。做法是线段插值每隔0.1倍step_size取样一次调用grid.is_free。如果碰撞检测精度不足路径可能会穿过地图边缘或未知区域。同时也是SLAM地图中未知区域的处理未知区域在仿真中可以视为空闲也可以视为占据取决于你的导航策略。安全起见我视为空闲但增加路径代价这样规划倾向于走已知区域。4.3 真实SLAM地图上的仿真测试与对比有了A和RRT现在用一张从SLAM建图得到的走廊地图来做对比测试。假设已经用3.1节的方法加载地图并膨胀0.2m起点在左下角开阔区域终点在右上角狭长通道后。我会写一段测试脚本统计路径代价、规划时间、转折点数然后把结果输出为文件。import time from a_star import a_star from rrt_star import rrt_star grid load_slam_grid(corridor.npy) # function to load grid grid.inflate(0.2) start (1.0, 1.0) goal (9.0, 9.0) for name, planner in [(A*, a_star), (RRT*, rrt_star)]: t0 time.time() if name A*: path planner(grid, start, goal) else: np.random.seed(42) # important for reproducible RRT* path planner(grid, start, goal, max_iter5000, step_size0.4, rewire_radius1.2) elapsed time.time() - t0 if path is None: print(f{name}: no path found) continue # Compute path length length 0.0 for i in range(len(path) - 1): length np.linalg.norm(np.array(path[i1]) - np.array(path[i])) print(f{name}: path length {length:.2f} m, time {elapsed:.2f} s)4.3.1 结果解读与可复现性在仿真中我通常会发现A路径长度稳定但拐点较多RRT首次运行可能得到不同结果因为采样有随机性。这就是为什么np.random.seed(42)必不可少。如果想要更公平的对比建议让RRT在固定随机种子下重复运行多次取平均值。同时还要注意规划时间受地图大小影响很大一张200x200地图上A如果超过0.5秒就要检查是否用了8邻域且数据结构没有优化。RRT*规划时间通常和迭代次数成正比不要盲目加到10000次先看2000次够不够到达目标。表两种算法在测试地图上的典型表现仅供参考算法路径长度规划时间拐点数量窄通道敏感度A*11.3 m0.08 s6较高需膨胀RRT*11.8 m1.25 s14低需调step_size注意这张表只是典型趋势不是标准结果。你可以通过调整启发权重和rewire半径来改善。比如A*权重从1.0改到1.5规划时间可能降到0.04s代价是路径长度增加3%。这需要你根据自己的应用取舍。5. 复现结果的验证技巧与一处关键补丁5.1 用固定随机种子做批量回归测试仿真复现最怕的是算法结果不确定无法判断改动是否有益。我通常的做法是写一个批量测试脚本随机生成20张不同障碍密度的地图对每个算法每个参数组合运行5次统计平均代价和平均规划时间。这比单张地图调参更有说服力。在批量测试时固定随机种子要分成两层第一层固定地图生成种子第二层固定规划器种子。这样才能区分地图复杂度的影响和算法随机性的影响。下面是一段批量测试的关键代码。np.random.seed(0) for map_id in range(20): map_data generate_random_map(size80, obstacle_density0.15 0.01 * map_id) grid OccupancyGrid(map_data, resolution0.1).inflate(0.2) for trial in range(5): np.random.seed(100 * map_id trial) start (0.5, 0.5) goal (7.5, 7.5) # run planner and aggregate metrics这段代码的要点是np.random.seed(100 * map_id trial)同时受地图编号和试验编号影响。如果你把地图生成和规划器都用同一个全局随机状态很难说清性能变化时地图导致的还是算法导致的。批量测试后可以把平均路径代价、成功率、规划时间都打印出来你也就能判断哪个参数组合在“成功率优先”和“时间优先”场景下更合适。5.2 遇到“路径穿过未知区域”时你需要的补丁SLAM建图通常会有不少未知区域特别是远离激光扫描范围的地方。A和RRT默认把未知区域当成空闲这会导致规划出一条穿越“未知黑洞”的捷径。真实系统里这样很危险。这里给出一个简单补丁在is_free方法里增加状态判断删除原本的返回值改成区分空闲和未知。def is_free_or_unknown(self, x, y): col int((x - self.origin[0]) / self.resolution) row int((y - self.origin[1]) / self.resolution) if not (0 row self.data.shape[0] and 0 col self.data.shape[1]): return False value self.data[row, col] # value 0.5 means unknown return value 0.5 # treat unknown as free for now你可能会想“这不还是当作空闲吗”区别在于有了这个方法后续你可以随时在这里加代价。比如value 0.3 and value 0.6时返回“可通行但代价加倍”A的g_score就会绕开未知区域而RRT的rewire也更倾向于已知区域。这正是仿真复现和真实部署之间的重要桥梁也是在SLAM面试里能展现深度的细节。5.3 最后一个调试技巧把路径平滑做进仿真很多人在模拟器里看到A*路径锯齿状就急着换算法。其实用简单的梯度下降法处理一下路径点就够了对每个路径点除了首尾应用“拉普拉斯平滑”即把它向相邻两点的中点移动一小步。重复10次左右路径就会变得顺滑但注意平滑后要重新做一遍碰撞检测否则可能穿越障碍。平滑是在规划之后、轨迹跟踪之前做的效果立竿见影。你的仿真复现到了这一步已经能把“SLAM建图 → 地图处理 → 规划 → 后处理”整条链路打通可以拿去应对绝大多数路径规划相关的仿真评估任务了。本文还有配套的精品资源点击获取
基于SLAM地图的路径规划算法仿真复现：A*与RRT*实战解析
简介基于SLAM的规划算法仿真复现Python源码包面向需要完成毕业设计、课程设计以及SLAM算法学习与二次开发的学生和开发者。包内共42个文件主要包含Python源码覆盖FastSLAM1、FastSLAM2、EKF SLAM等经典算法实现、仿真图像与GIF动画展示算法运行结果、配置文件如yaml/yml环境与参数配置、说明文档包括Markdown笔记和PDF说明等整体大小约11.55MB。其中提供了完整的2D SLAM示例程序内含地图构建、数据解析、配置管理等功能模块并附有运行依赖清单算法实现均经过测试可直接运行并输出可视化结果便于理解SLAM原理和规划策略。代码结构清晰注释明了便于在此基础上进行功能扩展或算法改进是完成课程设计、毕业设计及项目初期立项的实用参考。目前已有482人浏览学习值得下载使用。1. 从SLAM地图到规划算法仿真你到底卡在哪一步搭一个移动机器人系统时最磨人的往往不是SLAM本身而是SLAM跑通之后那个高分辨率栅格地图丢给路径规划模块时规划器要么直接飞出可通行区域要么绕远路要么在狭窄过道里不停重规划。很多人在这一站卡了一两个星期。其实问题不在算法实现而在你忽略了一个前置步骤SLAM输出的地图模型和规划器要求的地图模型并不是同一种东西。这个以“基于SLAM的规划算法仿真复现python源码.zip”为主题的工程核心就是把这条链路拆开用纯Python在本地把建图结果、地图转换、路径规划和仿真可视化完整串起来。这篇博客直接面向那些拿着SLAM建图数据、想快速评估规划算法效果的人也适合准备SLAM面试、想从源码层面讲清楚“建图如何服务于导航”的工程师。你会发现一个几百行代码的Python仿真环境足够复现A*、RRT*等主流路径规划算法并能在你真实采集的地图上做交互式调试。接下来所有内容均基于常见工具链不依赖特定的商业软件。2. 占据栅格地图与规划算法选型SLAM输出的东西到底怎么用2.1 栅格地图不是图像是概率分布SLAM建图常见产物是栅格地图Occupancy Grid Map比如ROS里的map.pgm配合map.yaml。像素的灰度值不是单纯的颜色而是该像素被障碍物占据的概率。白色通常代表空闲free黑色代表占据occupied灰色代表未知unknown。处理时最关键的一步是按阈值把灰度图转成0/1/2的三值地图常见阈值是灰度值小于某值视为占据大于另一值视为空闲中间视为未知。分辨率是另一个决定性参数。例如一个10m乘10m的区域地图分辨率0.05m就是200x200个栅格。分辨率越大内存和计算量按平方增长。规划时通常不需要原始分辨率而是对地图做降采样或膨胀后使用否则A*搜索节点过多实时性很难保证。我在实际复现时一般先读取map.yaml里的resolution和origin再决定要不要重采样到0.1m或0.2m作为规划地图。2.1.1 为什么直接使用SLAM原始地图会引发“碰撞假象”很多人在仿真中遇到规划路径穿过障碍物不是算法错了而是地图阈值没设对。SLAM建图时激光雷达对玻璃、黑墙、低反光物体的处理经常出现“伪占据”直接套用默认阈值会让算法以为某个区域完全不可通行。我一般会把灰度地图打印成直方图看看分布选取两个谷底的中间值作为阈值。另外对地图做一次形态学膨胀能显著减少路径贴墙的问题。这些都是规划前必须处理的地图层面的“坑”。2.2 规划算法分类图搜索、采样、插值路径规划算法按策略大致分成三类。第一类是基于图搜索典型如Dijkstra、A*它们把栅格地图看作图节点搜索路径时保证分辨率完备性即只要存在路径就能在给定离散粒度下找到。第二类是基于采样典型如RRT、RRT*适合高维空间或连续空间不需要显式栅格化所有点但在栅格地图上需要做碰撞检测。第三类是基于插值或优化比如多项式规划、贝塞尔轨迹生成常用于生成平滑轨迹但前提是先有一个无碰撞的几何路径。在做SLAM仿真复现时最优先复现A和RRT因为既能验证地图模型是否正确又能对比不同策略在同样地图上的效果。2.2.1 A*在不同应用场景下的属性取舍在扫地机器人这种二维平面场景里A配合8邻域搜索启发函数用欧氏距离或对角距离效果就很稳定。但在泊车路径规划算法里A需要加入车辆运动学约束变成Hybrid A*。这里注意如果你的目标是把SLAM和规划做成一个通用仿真框架先把经典的网格A*复现清楚再加上状态栅格扩展否则一上来就做带非完整约束的规划调试难度会成倍增加。这也是常见做法中的推荐顺序。RRT则适合处理窄通道地图。它不需要将地图离散成网格只需随机采样点并连接入树中再通过rewire操作优化代价。但RRT在网格地图上的表现很依赖距离度量和采样分布这一点后续会在仿真中验证。3. Python仿真环境与地图模型把SLAM输出变成自治系统可用的栅格3.1 搭建最小可运行的地图模块仿真复现不需要安装庞大框架Python标准库加上numpy和matplotlib就够。地图模块的核心是维护一个二维数组并提供查询和修改接口。我习惯用OccupancyGrid类封装它内部存储一个float数组值域0到10表示空闲1表示占据未知用0.5表示。对外提供is_free、is_occupied和get_resolution方法。这样规划算法不需要关心地图数据从哪里来无论是SLAM输出还是随机生成的测试地图都能统一处理。下面是一个最小实现文件名为occupancy_grid.py。import numpy as np class OccupancyGrid: def __init__(self, data, resolution0.1, origin(0, 0)): data: 2D numpy array, value in [0, 1], 1 means occupied resolution: meters per cell origin: (x, y) of cell (0, 0) in world coords self.data np.asarray(data, dtypenp.float32) self.resolution resolution self.origin origin def is_free(self, x, y): Check if world coordinate (x, y) is free. col int((x - self.origin[0]) / self.resolution) row int((y - self.origin[1]) / self.resolution) if row 0 or row self.data.shape[0] or col 0 or col self.data.shape[1]: return False return self.data[row, col] 0.2 def inflate(self, radius_meters): Inflate obstacles by given radius in meters. radius_cells max(1, int(radius_meters / self.resolution)) # Use binary dilation via scipy or simple cv2.dilate import cv2 binary (self.data 0.5).astype(np.uint8) kernel cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (radius_cells * 2 1, radius_cells * 2 1)) inflated cv2.dilate(binary, kernel) # Merge inflated obstacles into original map inflated_map np.maximum(self.data, inflated) return OccupancyGrid(inflated_map, self.resolution, self.origin)3.1.1 逻辑说明与参数说明is_free方法把世界坐标转换为栅格行列坐标。注意numpy的行是y方向列是x方向这跟笛卡尔坐标系的x-y有转置关系容易搞错。inflate方法用来做障碍物膨胀这里的cv2.MORPH_ELLIPSE能生成椭圆形结构元素接近机器人轮廓。参数radius_meters需要根据机器人物理半径设定例如机器人半径0.3m膨胀半径至少设为0.3m再额外预留0.1m安全距离。膨胀后地图会“变胖”一圈路径自然远离障碍物。从SLAM生成这类地图时需要先读取数据。假设你已经把激光SLAM建图结果保存成CSV或NumPy数组直接读入即可。如果是ROS环境里的pgm文件可以用cv2.imread加载灰度图然后除以255得到0-1数据。有一点要留意pgm的原点通常在地图中心或某个角落加载时必须用map.yaml的origin字段对齐坐标系否则路径会整体偏移。3.2 地图可视化与数据流验证地图模块正确性直接影响后续所有规划效果所以一定要先做可视化验证。我用以下方式验证随机生成或加载一张SLAM真实地图打印栅格大小和占据比例并显示膨胀前和膨胀后的对比图。import matplotlib.pyplot as plt import numpy as np from occupancy_grid import OccupancyGrid # Generate a simple test map: 50x50 with a few obstacles map_data np.zeros((50, 50), dtypenp.float32) map_data[20:30, 10:15] 1.0 # vertical wall map_data[10, 30:40] 1.0 # horizontal wall grid OccupancyGrid(map_data, resolution0.1) inflated grid.inflate(0.3) fig, axes plt.subplots(1, 2, figsize(10, 5)) axes[0].imshow(grid.data, cmapgray_r) axes[0].set_title(original SLAM grid) axes[1].imshow(inflated.data, cmapgray_r) axes[1].set_title(inflated grid) plt.show()运行后如果两幅图差异明显说明膨胀代码生效。这里我故意用局部障碍物测试因为比整个地图更直观。实践中你要用真实SLAM地图替换map_data比如从np.load(my_slam_map.npy)载入。这一步做好了规划仿真才算迈出第一步。4. 在SLAM地图上复现A和RRT代码、参数与真实地图测试4.1 A*栅格规划从8邻域到性能调优A是复现规划算法的基本功。它的核心是维护两个集合开放集和闭合集每次从开放集中取代价最小的节点扩展。代价函数f(n)g(n)h(n)其中g是从起点到当前节点的实际代价h是启发式估计代价。在8邻域地图中h通常取欧氏距离或对角距离。下面这段代码实现了标准的8邻域A同时返回路径和访问节点数便于分析性能。import heapq import numpy as np from occupancy_grid import OccupancyGrid def a_star(grid, start, goal, heuristiceuclidean): grid: OccupancyGrid object start, goal: (x, y) world coordinates return: list of (x, y) waypoints, or None if not found start_cell grid_to_cell(grid, start) goal_cell grid_to_cell(grid, goal) open_list [] heapq.heappush(open_list, (0.0, start_cell)) came_from {} g_score {start_cell: 0.0} f_score {start_cell: heuristic_cost(start_cell, goal_cell, heuristic)} while open_list: current heapq.heappop(open_list)[1] if current goal_cell: return reconstruct_path(came_from, current, grid) for dy in [-1, 0, 1]: for dx in [-1, 0, 1]: if dx 0 and dy 0: continue neighbor (current[0] dx, current[1] dy) # Check bounds and collision if not grid.is_free_cell(neighbor): continue tentative_g g_score[current] (1.414 if dx ! 0 and dy ! 0 else 1.0) if tentative_g g_score.get(neighbor, float(inf)): came_from[neighbor] current g_score[neighbor] tentative_g f tentative_g heuristic_cost(neighbor, goal_cell, heuristic) heapq.heappush(open_list, (f, neighbor)) return None4.1.1 代码逻辑说明和参数解析grid.is_free_cell是3.1节中is_free的栅格坐标版本你需要补一个方法。heuristic_cost用欧氏距离计算reconstruct_path从目标点回溯came_from表得到路径。这里有个性能关键点对neighbor做is_free检查时会在每个循环调用多次如果地图是200x200且搜索范围大性能可能会崩。常见优化是预先计算一张布尔可通行表把碰撞检测变成O(1)数组索引。参数方面最容易调的是启发权重。标准A的权重为1修改为大于1就变成weighted A搜索速度更快但路径可能次优。在SLAM地图上通常权重1.2到2之间能明显减少扩展节点数。如果你复现出来路径贴墙先检查地图膨胀而不是调启发。另外8邻域相比4邻域路径更短但转向更频繁需要后续对路径做平滑。4.2 RRT*采样规划碰撞检测与渐近最优RRT相比标准RRT多了重新连接过程。它在每次迭代生成新节点后不仅尝试连接最近节点还会搜索半径范围内的其他节点看是否能用更低代价连接到当前新节点。这个操作让路径逐步向最优靠拢。下面是简化版RRT实现适用于二维连续地图。import numpy as np from occupancy_grid import OccupancyGrid def rrt_star(grid, start, goal, max_iter5000, step_size0.5, rewire_radius2.0): grid: OccupancyGrid with inflated obstacles start, goal: (x, y) world coords nodes [np.array(start, dtypefloat)] parent [] # parallel list of parent indices costs [0.0] goal_threshold step_size * 0.5 for _ in range(max_iter): # Sample random point (with 10% bias to goal) if np.random.rand() 0.1: rand_point np.array(goal, dtypefloat) else: margin 0.5 x_min, x_max grid.origin[0] margin, grid.origin[0] grid.data.shape[1] * grid.resolution - margin y_min, y_max grid.origin[1] margin, grid.origin[1] grid.data.shape[0] * grid.resolution - margin rand_point np.array([np.random.uniform(x_min, x_max), np.random.uniform(y_min, y_max)]) # Find nearest node distances [np.linalg.norm(np.array(node) - rand_point) for node in nodes] nearest_idx int(np.argmin(distances)) nearest_point nodes[nearest_idx] # Steer toward random point direction rand_point - nearest_point dist np.linalg.norm(direction) if dist 1e-6: continue new_point nearest_point (direction / dist) * min(step_size, dist) # Collision check along segment if not is_segment_free(grid, nearest_point, new_point): continue # Find neighbors within rewire radius neighbors [] for i, node in enumerate(nodes): if np.linalg.norm(np.array(node) - new_point) rewire_radius: neighbors.append(i) # Choose parent with lowest cost best_parent nearest_idx best_cost costs[nearest_idx] np.linalg.norm(new_point - nearest_point) for i in neighbors: potential_cost costs[i] np.linalg.norm(new_point - np.array(nodes[i])) if potential_cost best_cost and is_segment_free(grid, nodes[i], new_point): best_parent i best_cost potential_cost # Add new node nodes.append(new_point) parent.append(best_parent) costs.append(best_cost) # Rewire neighbors for i in neighbors: potential_cost best_cost np.linalg.norm(nodes[i] - new_point) if potential_cost costs[i] and is_segment_free(grid, new_point, nodes[i]): parent[i] len(nodes) - 1 costs[i] potential_cost # Check goal if np.linalg.norm(new_point - np.array(goal)) goal_threshold: return reconstruct_rrt_path(nodes, parent, len(nodes) - 1) # If max_iter reached, return best path if any node near goal return None4.2.1 参数调试关键点step_size决定树的分辨率。如果地图是0.05m分辨率step_size取0.3到0.5m比较合适太小导致迭代次数爆炸太大容易穿越窄通道。rewire_radius是渐近最优的核心太小会退化为RRT路径不平滑太大会让每次迭代O(n)的搜索开销太高。我一般设为step_size的3到4倍。max_iter不要固定可以观察节点数和路径代价的变化来动态调整仿真阶段用5000次迭代足够看到效果。还有一个必须检查的点is_segment_free要对线段上的采样点做碰撞检测。做法是线段插值每隔0.1倍step_size取样一次调用grid.is_free。如果碰撞检测精度不足路径可能会穿过地图边缘或未知区域。同时也是SLAM地图中未知区域的处理未知区域在仿真中可以视为空闲也可以视为占据取决于你的导航策略。安全起见我视为空闲但增加路径代价这样规划倾向于走已知区域。4.3 真实SLAM地图上的仿真测试与对比有了A和RRT现在用一张从SLAM建图得到的走廊地图来做对比测试。假设已经用3.1节的方法加载地图并膨胀0.2m起点在左下角开阔区域终点在右上角狭长通道后。我会写一段测试脚本统计路径代价、规划时间、转折点数然后把结果输出为文件。import time from a_star import a_star from rrt_star import rrt_star grid load_slam_grid(corridor.npy) # function to load grid grid.inflate(0.2) start (1.0, 1.0) goal (9.0, 9.0) for name, planner in [(A*, a_star), (RRT*, rrt_star)]: t0 time.time() if name A*: path planner(grid, start, goal) else: np.random.seed(42) # important for reproducible RRT* path planner(grid, start, goal, max_iter5000, step_size0.4, rewire_radius1.2) elapsed time.time() - t0 if path is None: print(f{name}: no path found) continue # Compute path length length 0.0 for i in range(len(path) - 1): length np.linalg.norm(np.array(path[i1]) - np.array(path[i])) print(f{name}: path length {length:.2f} m, time {elapsed:.2f} s)4.3.1 结果解读与可复现性在仿真中我通常会发现A路径长度稳定但拐点较多RRT首次运行可能得到不同结果因为采样有随机性。这就是为什么np.random.seed(42)必不可少。如果想要更公平的对比建议让RRT在固定随机种子下重复运行多次取平均值。同时还要注意规划时间受地图大小影响很大一张200x200地图上A如果超过0.5秒就要检查是否用了8邻域且数据结构没有优化。RRT*规划时间通常和迭代次数成正比不要盲目加到10000次先看2000次够不够到达目标。表两种算法在测试地图上的典型表现仅供参考算法路径长度规划时间拐点数量窄通道敏感度A*11.3 m0.08 s6较高需膨胀RRT*11.8 m1.25 s14低需调step_size注意这张表只是典型趋势不是标准结果。你可以通过调整启发权重和rewire半径来改善。比如A*权重从1.0改到1.5规划时间可能降到0.04s代价是路径长度增加3%。这需要你根据自己的应用取舍。5. 复现结果的验证技巧与一处关键补丁5.1 用固定随机种子做批量回归测试仿真复现最怕的是算法结果不确定无法判断改动是否有益。我通常的做法是写一个批量测试脚本随机生成20张不同障碍密度的地图对每个算法每个参数组合运行5次统计平均代价和平均规划时间。这比单张地图调参更有说服力。在批量测试时固定随机种子要分成两层第一层固定地图生成种子第二层固定规划器种子。这样才能区分地图复杂度的影响和算法随机性的影响。下面是一段批量测试的关键代码。np.random.seed(0) for map_id in range(20): map_data generate_random_map(size80, obstacle_density0.15 0.01 * map_id) grid OccupancyGrid(map_data, resolution0.1).inflate(0.2) for trial in range(5): np.random.seed(100 * map_id trial) start (0.5, 0.5) goal (7.5, 7.5) # run planner and aggregate metrics这段代码的要点是np.random.seed(100 * map_id trial)同时受地图编号和试验编号影响。如果你把地图生成和规划器都用同一个全局随机状态很难说清性能变化时地图导致的还是算法导致的。批量测试后可以把平均路径代价、成功率、规划时间都打印出来你也就能判断哪个参数组合在“成功率优先”和“时间优先”场景下更合适。5.2 遇到“路径穿过未知区域”时你需要的补丁SLAM建图通常会有不少未知区域特别是远离激光扫描范围的地方。A和RRT默认把未知区域当成空闲这会导致规划出一条穿越“未知黑洞”的捷径。真实系统里这样很危险。这里给出一个简单补丁在is_free方法里增加状态判断删除原本的返回值改成区分空闲和未知。def is_free_or_unknown(self, x, y): col int((x - self.origin[0]) / self.resolution) row int((y - self.origin[1]) / self.resolution) if not (0 row self.data.shape[0] and 0 col self.data.shape[1]): return False value self.data[row, col] # value 0.5 means unknown return value 0.5 # treat unknown as free for now你可能会想“这不还是当作空闲吗”区别在于有了这个方法后续你可以随时在这里加代价。比如value 0.3 and value 0.6时返回“可通行但代价加倍”A的g_score就会绕开未知区域而RRT的rewire也更倾向于已知区域。这正是仿真复现和真实部署之间的重要桥梁也是在SLAM面试里能展现深度的细节。5.3 最后一个调试技巧把路径平滑做进仿真很多人在模拟器里看到A*路径锯齿状就急着换算法。其实用简单的梯度下降法处理一下路径点就够了对每个路径点除了首尾应用“拉普拉斯平滑”即把它向相邻两点的中点移动一小步。重复10次左右路径就会变得顺滑但注意平滑后要重新做一遍碰撞检测否则可能穿越障碍。平滑是在规划之后、轨迹跟踪之前做的效果立竿见影。你的仿真复现到了这一步已经能把“SLAM建图 → 地图处理 → 规划 → 后处理”整条链路打通可以拿去应对绝大多数路径规划相关的仿真评估任务了。本文还有配套的精品资源点击获取
