PythonRobotics 中的 Voronoi 路图Voronoi Road-Map路径规划基于 Dijkstra 的图搜索实现解析【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics本指南围绕 PythonRobotics 仓库中的 Voronoi Road-Map 规划器展开讲解如何利用 Voronoi 图顶点生成最大安全距离的路径候选点再通过 Dijkstra 图搜索得到从起点到终点的可行路径。阅读完本文你将掌握该规划器的算法流程、核心类与关键参数如N_KNN、MAX_EDGE_LEN、机器人半径并能在本地直接运行 voronoi_road_map.py 复现带实时动画的规划结果。Voronoi Road-Map 方法概述Voronoi Road-MapVoronoi 路图是一类基于 Voronoi 图构建路径候选网络的路径规划方法。其核心思想是把障碍物视为空间中的点集构造这些点的 Voronoi 图Voronoi 图的边或顶点天然处于“离最近障碍物最远”的位置因此沿着它们行走的路径拥有最大的安全裕度非常适合机器人在障碍环境中导航。在 PythonRobotics 中该模块的实现位于 PathPlanning/VoronoiRoadMap/voronoi_road_map.py对应的官方文档为 vrm_planner_main.rst。文档中对该规划器的运行过程做了如下图示约定蓝色点Voronoi 采样点Voronoi 顶点青色叉号Dijkstra 方法搜索过程中访问过的节点红色线最终输出的 Voronoi Road-Map 路径。整个规划流程可以概括为三步Voronoi 采样生成候选节点 → 基于碰撞检测构建路图 → 用 Dijkstra 搜索最短路径。整体算法流程VoronoiRoadMapPlanner的入口是planning()方法它把整条流水线串在一起见 voronoi_road_map.py#L29-L42def planning(self, sx, sy, gx, gy, ox, oy, robot_radius): obstacle_tree cKDTree(np.vstack((ox, oy)).T) sample_x, sample_y self.voronoi_sampling(sx, sy, gx, gy, ox, oy) if show_animation: # pragma: no cover plt.plot(sample_x, sample_y, .b) road_map_info self.generate_road_map_info( sample_x, sample_y, robot_radius, obstacle_tree) rx, ry DijkstraSearch(show_animation).search(sx, sy, gx, gy, sample_x, sample_y, road_map_info) return rx, ry其输入参数含义如下参数含义sx, sy起点坐标单位mgx, gy终点坐标单位mox, oy所有障碍物点的 x / y 坐标数组robot_radius机器人半径单位m用于碰撞检测与膨胀内部流程为用scipy.spatial.cKDTree把障碍物点组织成 KD-Tree为后续高效的近邻查询与碰撞检测做准备voronoi_sampling()计算障碍物点集的 Voronoi 顶点并将起点、终点追加进采样点集合即文档图中的“蓝色点”generate_road_map_info()依据机器人半径在采样点之间做碰撞检测生成无碰撞的边构成路图DijkstraSearch.search()在路图上执行 Dijkstra 图搜索得到最终路径即文档图中的“红色线”。核心参数N_KNN与MAX_EDGE_LEN在VoronoiRoadMapPlanner.__init__中定义了两个对路图形态影响最大的参数voronoi_road_map.py#L24-L27self.N_KNN 10 # number of edge from one sampled point self.MAX_EDGE_LEN 30.0 # [m] Maximum edge lengthN_KNN默认 10每个采样点最多可连接的近邻节点数。它限制了路图的度数稀疏程度值越大路图越稠密路径选择的自由度越高但 Dijkstra 搜索的边展开量也越大值越小路图越稀疏搜索更快但可能因连通性不足而找不到路径。MAX_EDGE_LEN默认 30.0 m单条边的最大长度。超过该长度的边直接视为无效不可行避免生成跨越空旷区域的长边这既符合“沿 Voronoi 结构行走”的语义也能显著减少无效边的计算量。Voronoi 采样从障碍物点集生成路径候选节点voronoi_sampling()是静态方法直接使用scipy.spatial.Voronoi构造障碍物点集的 Voronoi 图voronoi_road_map.py#L118-L132staticmethod def voronoi_sampling(sx, sy, gx, gy, ox, oy): oxy np.vstack((ox, oy)).T # generate voronoi point vor Voronoi(oxy) sample_x [ix for [ix, _] in vor.vertices] sample_y [iy for [_, iy] in vor.vertices] sample_x.append(sx) sample_y.append(sy) sample_x.append(gx) sample_y.append(gy) return sample_x, sample_y关键点在于只取vor.verticesVoronoi 图的顶点而不是整条 Voronoi 边。这些顶点是三条或更多 Voronoi 边的交汇处在几何上位于多个障碍物的“等距最远”位置天然具有最大安全间隙。随后把起点(sx, sy)与终点(gx, gy)追加进采样列表保证路图必然包含起点与终点两个节点——这是后续 Dijkstra 搜索能够连通起点与终点的前提。碰撞检测is_collision()在构建路图时需要判断两个采样点之间的边是否与障碍物冲突。is_collision()采用沿边步进采样 KD-Tree 最近邻查询的方式voronoi_road_map.py#L44-L70def is_collision(self, sx, sy, gx, gy, rr, obstacle_kd_tree): ... if d self.MAX_EDGE_LEN: return True D rr n_step round(d / D) for i in range(n_step): dist, _ obstacle_kd_tree.query([x, y]) if dist rr: return True # collision x D * math.cos(yaw) y D * math.sin(yaw) # goal point check dist, _ obstacle_kd_tree.query([gx, gy]) if dist rr: return True # collision return False # OK其判定逻辑为若边长超过MAX_EDGE_LEN直接判定为碰撞返回True以机器人半径rr为步长D将整条边离散为n_step个采样点逐点向障碍物 KD-Tree 查询最近距离若某点距离障碍物小于等于rr则判定碰撞对终点做同样的最近距离检查。这种做法的物理含义是把机器人近似视为半径为rr的圆只要圆心沿线任意位置到最近障碍物的距离大于rr就认为该边可通行。由于采样步长等于机器人半径边上的障碍物间隙不会被漏检属于一种简单而有效的保守碰撞检测。路图构建generate_road_map_info()采样点之间的边关系由generate_road_map_info()生成voronoi_road_map.py#L72-L106。它对每个采样点执行一次 KD-Tree 全量近邻查询按距离从小到大遍历其他节点把通过碰撞检测的节点加入该点的邻接表直到达到N_KNN个邻接边为止for (i, ix, iy) in zip(range(n_sample), node_x, node_y): dists, indexes node_tree.query([ix, iy], kn_sample) edge_id [] for ii in range(1, len(indexes)): nx node_x[indexes[ii]] ny node_y[indexes[ii]] if not self.is_collision(ix, iy, nx, ny, rr, obstacle_tree): edge_id.append(indexes[ii]) if len(edge_id) self.N_KNN: break road_map.append(edge_id)最终返回的road_map是一个邻接表第i个元素是节点i可以直接到达的节点编号列表这也是后续 Dijkstra 搜索所需的“边信息”。此外类中还提供了plot_road_map()静态方法可用黑色线段把整个路图可视化出来调试路图形态时非常有用。Dijkstra 图搜索DijkstraSearch完成路图构建后路径搜索由独立的DijkstraSearch类完成实现在 PathPlanning/VoronoiRoadMap/dijkstra_search.py。该搜索器是经典 Dijkstra 算法在图结构上的实现用open_set待扩展节点与close_set已扩展节点两个字典管理搜索状态每次从open_set中取出代价最小的节点min(open_set, keylambda o: open_set[o].cost)进行扩展沿邻接表edge_ids_list展开邻居用欧氏距离math.hypot(dx, dy)作为边权累积节点代价若邻居已在close_set中则跳过若已在open_set中且新路径代价更小则更新实现“松弛”操作搜索过程中偶数次扩展时会用xg绘制青色叉号即文档图中所说的“Cyan crosses mean searched points with Dijkstra method”。search()结束后通过generate_final_path()沿parent指针从目标节点回溯到起点反转后得到完整路径点序列(rx, ry)即最终红色路径。值得注意的细节是find_id()与is_same_node()使用**欧氏距离 ≤ 0.1m**作为节点“同一性”判据用于把起点/终点匹配到采样点集合中的对应节点由于起点与终点已被voronoi_sampling()显式加入节点集合Dijkstra 能天然地在路图中把它们连接起来DijkstraSearch是一个通用组件仓库中的可见性路图Visibility Road-Map规划器同样复用了它见 visibility_road_map.py#L39-L45这印证了“路图方法 图搜索”这一架构的通用性。运行示例与场景复现模块自带的main()函数voronoi_road_map.py#L135-L186构建了一个典型的走廊式障碍场景起点(10.0, 10.0)终点(50.0, 50.0)机器人半径robot_size 5.0单位均为 m障碍物由三段构成60 m × 60 m 的边界围墙下、右、上、左四条边以及两面内部隔墙——x 20.0处的竖向墙和x 40.0处的竖向墙后者顶部留出 20 m 缺口从而形成类似“Z 字型”的绕行通道黑点绘制障碍物红色三角为起点青色三角为终点。在仓库根目录下直接运行即可看到实时规划动画python PathPlanning/VoronoiRoadMap/voronoi_road_map.py运行时会依次显示黑色障碍物与起终点标记 → 蓝色 Voronoi 采样点 → 青色叉号的 Dijkstra 搜索过程支持按Esc键退出动画→ 最终的红色路径。若想关闭动画、仅做数值求解可将文件顶部的show_animation True改为False。程序的依赖为仓库 requirements/requirements.txt 中声明的科学计算栈numpy、scipy提供Voronoi与cKDTree、matplotlib可视化。按 requirements/environment.yml 配置环境后即可运行本模块。测试与验证在测试目录tests/中虽然未单独为 Voronoi Road-Map 规划器建立独立的测试文件但其图搜索组件被可见性路图测试间接覆盖test_visibility_road_map_planner.py 导入了PathPlanning.VisibilityRoadMap.visibility_road_map而后者内部正是复用了 VoronoiRoadMap/dijkstra_search.py 中的DijkstraSearch完成路径搜索。测试通过conftest.run_this_test()以-W error严格警告模式运行main()断言整个“建图 搜索”流程不抛异常、能找到可行路径。这说明DijkstraSearch的搜索正确性经过了回归验证Voronoi 路图规划器可以直接按同样方式接入测试。参数调优与使用建议基于源码实现可以总结出以下调优方向均可在 voronoi_road_map.py#L24-L27 处修改机器人半径robot_radius直接决定碰撞检测的保守程度。半径越大可通过的窄缝越少路图越稀疏甚至可能无解main()中通过assert rx保证路径存在N_KNN增大可提升路图连通性与路径质量但会线性增加 Dijkstra 的边展开量在窄缝场景下过小的N_KNN可能导致某些 Voronoi 顶点成为“孤岛”MAX_EDGE_LEN在空旷环境中可适当增大避免因边被截断而丢失可行连接在密集障碍环境中减小则能过滤掉大量无意义的远距离候选边加速建图障碍物表示该实现要求障碍物以点集ox, oy形式输入因为 Voronoi 图是对点集构造的。实际使用时可将多边形障碍物边界离散为点云后传入。Voronoi Road-Map 方法的典型适用场景是障碍物可近似为点集、且对“最大安全间隙”有要求的结构化环境。其优点是路径天然远离障碍物、图规模远小于栅格法局限性则是 Voronoi 图对障碍物的离散点表示敏感且得到的路径并非最短路径——如需更平滑或更短的轨迹可在此基础上叠加样条平滑等后处理步骤仓库 PathPlanning/CubicSpline 等模块可作参考。【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考 PythonRobotics 中的 Voronoi 路图（Voronoi Road-Map）路径规划：基于 Dijkstra 的图搜索实现解析
PythonRobotics 中的 Voronoi 路图Voronoi Road-Map路径规划基于 Dijkstra 的图搜索实现解析【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics本指南围绕 PythonRobotics 仓库中的 Voronoi Road-Map 规划器展开讲解如何利用 Voronoi 图顶点生成最大安全距离的路径候选点再通过 Dijkstra 图搜索得到从起点到终点的可行路径。阅读完本文你将掌握该规划器的算法流程、核心类与关键参数如N_KNN、MAX_EDGE_LEN、机器人半径并能在本地直接运行 voronoi_road_map.py 复现带实时动画的规划结果。Voronoi Road-Map 方法概述Voronoi Road-MapVoronoi 路图是一类基于 Voronoi 图构建路径候选网络的路径规划方法。其核心思想是把障碍物视为空间中的点集构造这些点的 Voronoi 图Voronoi 图的边或顶点天然处于“离最近障碍物最远”的位置因此沿着它们行走的路径拥有最大的安全裕度非常适合机器人在障碍环境中导航。在 PythonRobotics 中该模块的实现位于 PathPlanning/VoronoiRoadMap/voronoi_road_map.py对应的官方文档为 vrm_planner_main.rst。文档中对该规划器的运行过程做了如下图示约定蓝色点Voronoi 采样点Voronoi 顶点青色叉号Dijkstra 方法搜索过程中访问过的节点红色线最终输出的 Voronoi Road-Map 路径。整个规划流程可以概括为三步Voronoi 采样生成候选节点 → 基于碰撞检测构建路图 → 用 Dijkstra 搜索最短路径。整体算法流程VoronoiRoadMapPlanner的入口是planning()方法它把整条流水线串在一起见 voronoi_road_map.py#L29-L42def planning(self, sx, sy, gx, gy, ox, oy, robot_radius): obstacle_tree cKDTree(np.vstack((ox, oy)).T) sample_x, sample_y self.voronoi_sampling(sx, sy, gx, gy, ox, oy) if show_animation: # pragma: no cover plt.plot(sample_x, sample_y, .b) road_map_info self.generate_road_map_info( sample_x, sample_y, robot_radius, obstacle_tree) rx, ry DijkstraSearch(show_animation).search(sx, sy, gx, gy, sample_x, sample_y, road_map_info) return rx, ry其输入参数含义如下参数含义sx, sy起点坐标单位mgx, gy终点坐标单位mox, oy所有障碍物点的 x / y 坐标数组robot_radius机器人半径单位m用于碰撞检测与膨胀内部流程为用scipy.spatial.cKDTree把障碍物点组织成 KD-Tree为后续高效的近邻查询与碰撞检测做准备voronoi_sampling()计算障碍物点集的 Voronoi 顶点并将起点、终点追加进采样点集合即文档图中的“蓝色点”generate_road_map_info()依据机器人半径在采样点之间做碰撞检测生成无碰撞的边构成路图DijkstraSearch.search()在路图上执行 Dijkstra 图搜索得到最终路径即文档图中的“红色线”。核心参数N_KNN与MAX_EDGE_LEN在VoronoiRoadMapPlanner.__init__中定义了两个对路图形态影响最大的参数voronoi_road_map.py#L24-L27self.N_KNN 10 # number of edge from one sampled point self.MAX_EDGE_LEN 30.0 # [m] Maximum edge lengthN_KNN默认 10每个采样点最多可连接的近邻节点数。它限制了路图的度数稀疏程度值越大路图越稠密路径选择的自由度越高但 Dijkstra 搜索的边展开量也越大值越小路图越稀疏搜索更快但可能因连通性不足而找不到路径。MAX_EDGE_LEN默认 30.0 m单条边的最大长度。超过该长度的边直接视为无效不可行避免生成跨越空旷区域的长边这既符合“沿 Voronoi 结构行走”的语义也能显著减少无效边的计算量。Voronoi 采样从障碍物点集生成路径候选节点voronoi_sampling()是静态方法直接使用scipy.spatial.Voronoi构造障碍物点集的 Voronoi 图voronoi_road_map.py#L118-L132staticmethod def voronoi_sampling(sx, sy, gx, gy, ox, oy): oxy np.vstack((ox, oy)).T # generate voronoi point vor Voronoi(oxy) sample_x [ix for [ix, _] in vor.vertices] sample_y [iy for [_, iy] in vor.vertices] sample_x.append(sx) sample_y.append(sy) sample_x.append(gx) sample_y.append(gy) return sample_x, sample_y关键点在于只取vor.verticesVoronoi 图的顶点而不是整条 Voronoi 边。这些顶点是三条或更多 Voronoi 边的交汇处在几何上位于多个障碍物的“等距最远”位置天然具有最大安全间隙。随后把起点(sx, sy)与终点(gx, gy)追加进采样列表保证路图必然包含起点与终点两个节点——这是后续 Dijkstra 搜索能够连通起点与终点的前提。碰撞检测is_collision()在构建路图时需要判断两个采样点之间的边是否与障碍物冲突。is_collision()采用沿边步进采样 KD-Tree 最近邻查询的方式voronoi_road_map.py#L44-L70def is_collision(self, sx, sy, gx, gy, rr, obstacle_kd_tree): ... if d self.MAX_EDGE_LEN: return True D rr n_step round(d / D) for i in range(n_step): dist, _ obstacle_kd_tree.query([x, y]) if dist rr: return True # collision x D * math.cos(yaw) y D * math.sin(yaw) # goal point check dist, _ obstacle_kd_tree.query([gx, gy]) if dist rr: return True # collision return False # OK其判定逻辑为若边长超过MAX_EDGE_LEN直接判定为碰撞返回True以机器人半径rr为步长D将整条边离散为n_step个采样点逐点向障碍物 KD-Tree 查询最近距离若某点距离障碍物小于等于rr则判定碰撞对终点做同样的最近距离检查。这种做法的物理含义是把机器人近似视为半径为rr的圆只要圆心沿线任意位置到最近障碍物的距离大于rr就认为该边可通行。由于采样步长等于机器人半径边上的障碍物间隙不会被漏检属于一种简单而有效的保守碰撞检测。路图构建generate_road_map_info()采样点之间的边关系由generate_road_map_info()生成voronoi_road_map.py#L72-L106。它对每个采样点执行一次 KD-Tree 全量近邻查询按距离从小到大遍历其他节点把通过碰撞检测的节点加入该点的邻接表直到达到N_KNN个邻接边为止for (i, ix, iy) in zip(range(n_sample), node_x, node_y): dists, indexes node_tree.query([ix, iy], kn_sample) edge_id [] for ii in range(1, len(indexes)): nx node_x[indexes[ii]] ny node_y[indexes[ii]] if not self.is_collision(ix, iy, nx, ny, rr, obstacle_tree): edge_id.append(indexes[ii]) if len(edge_id) self.N_KNN: break road_map.append(edge_id)最终返回的road_map是一个邻接表第i个元素是节点i可以直接到达的节点编号列表这也是后续 Dijkstra 搜索所需的“边信息”。此外类中还提供了plot_road_map()静态方法可用黑色线段把整个路图可视化出来调试路图形态时非常有用。Dijkstra 图搜索DijkstraSearch完成路图构建后路径搜索由独立的DijkstraSearch类完成实现在 PathPlanning/VoronoiRoadMap/dijkstra_search.py。该搜索器是经典 Dijkstra 算法在图结构上的实现用open_set待扩展节点与close_set已扩展节点两个字典管理搜索状态每次从open_set中取出代价最小的节点min(open_set, keylambda o: open_set[o].cost)进行扩展沿邻接表edge_ids_list展开邻居用欧氏距离math.hypot(dx, dy)作为边权累积节点代价若邻居已在close_set中则跳过若已在open_set中且新路径代价更小则更新实现“松弛”操作搜索过程中偶数次扩展时会用xg绘制青色叉号即文档图中所说的“Cyan crosses mean searched points with Dijkstra method”。search()结束后通过generate_final_path()沿parent指针从目标节点回溯到起点反转后得到完整路径点序列(rx, ry)即最终红色路径。值得注意的细节是find_id()与is_same_node()使用**欧氏距离 ≤ 0.1m**作为节点“同一性”判据用于把起点/终点匹配到采样点集合中的对应节点由于起点与终点已被voronoi_sampling()显式加入节点集合Dijkstra 能天然地在路图中把它们连接起来DijkstraSearch是一个通用组件仓库中的可见性路图Visibility Road-Map规划器同样复用了它见 visibility_road_map.py#L39-L45这印证了“路图方法 图搜索”这一架构的通用性。运行示例与场景复现模块自带的main()函数voronoi_road_map.py#L135-L186构建了一个典型的走廊式障碍场景起点(10.0, 10.0)终点(50.0, 50.0)机器人半径robot_size 5.0单位均为 m障碍物由三段构成60 m × 60 m 的边界围墙下、右、上、左四条边以及两面内部隔墙——x 20.0处的竖向墙和x 40.0处的竖向墙后者顶部留出 20 m 缺口从而形成类似“Z 字型”的绕行通道黑点绘制障碍物红色三角为起点青色三角为终点。在仓库根目录下直接运行即可看到实时规划动画python PathPlanning/VoronoiRoadMap/voronoi_road_map.py运行时会依次显示黑色障碍物与起终点标记 → 蓝色 Voronoi 采样点 → 青色叉号的 Dijkstra 搜索过程支持按Esc键退出动画→ 最终的红色路径。若想关闭动画、仅做数值求解可将文件顶部的show_animation True改为False。程序的依赖为仓库 requirements/requirements.txt 中声明的科学计算栈numpy、scipy提供Voronoi与cKDTree、matplotlib可视化。按 requirements/environment.yml 配置环境后即可运行本模块。测试与验证在测试目录tests/中虽然未单独为 Voronoi Road-Map 规划器建立独立的测试文件但其图搜索组件被可见性路图测试间接覆盖test_visibility_road_map_planner.py 导入了PathPlanning.VisibilityRoadMap.visibility_road_map而后者内部正是复用了 VoronoiRoadMap/dijkstra_search.py 中的DijkstraSearch完成路径搜索。测试通过conftest.run_this_test()以-W error严格警告模式运行main()断言整个“建图 搜索”流程不抛异常、能找到可行路径。这说明DijkstraSearch的搜索正确性经过了回归验证Voronoi 路图规划器可以直接按同样方式接入测试。参数调优与使用建议基于源码实现可以总结出以下调优方向均可在 voronoi_road_map.py#L24-L27 处修改机器人半径robot_radius直接决定碰撞检测的保守程度。半径越大可通过的窄缝越少路图越稀疏甚至可能无解main()中通过assert rx保证路径存在N_KNN增大可提升路图连通性与路径质量但会线性增加 Dijkstra 的边展开量在窄缝场景下过小的N_KNN可能导致某些 Voronoi 顶点成为“孤岛”MAX_EDGE_LEN在空旷环境中可适当增大避免因边被截断而丢失可行连接在密集障碍环境中减小则能过滤掉大量无意义的远距离候选边加速建图障碍物表示该实现要求障碍物以点集ox, oy形式输入因为 Voronoi 图是对点集构造的。实际使用时可将多边形障碍物边界离散为点云后传入。Voronoi Road-Map 方法的典型适用场景是障碍物可近似为点集、且对“最大安全间隙”有要求的结构化环境。其优点是路径天然远离障碍物、图规模远小于栅格法局限性则是 Voronoi 图对障碍物的离散点表示敏感且得到的路径并非最短路径——如需更平滑或更短的轨迹可在此基础上叠加样条平滑等后处理步骤仓库 PathPlanning/CubicSpline 等模块可作参考。【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考