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-Map（Voronoi 路图）是一类基于 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-L42）：

def 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

其输入参数含义如下：

内部流程为：

1. 用scipy.spatial.cKDTree把障碍物点组织成 KD-Tree，为后续高效的近邻查询与碰撞检测做准备；
2. voronoi_sampling()计算障碍物点集的 Voronoi 顶点，并将起点、终点追加进采样点集合（即文档图中的“蓝色点”）；
3. generate_road_map_info()依据机器人半径在采样点之间做碰撞检测，生成无碰撞的边，构成路图；
4. DijkstraSearch.search()在路图上执行 Dijkstra 图搜索，得到最终路径（即文档图中的“红色线”）。

核心参数：N_KNN与MAX_EDGE_LEN

在VoronoiRoadMapPlanner.__init__中定义了两个对路图形态影响最大的参数（voronoi_road_map.py#L24-L27）：

self.N_KNN = 10 # number of edge from one sampled point self.MAX_EDGE_LEN = 30.0 # [m] Maximum edge length

• N_KNN（默认 10）：每个采样点最多可连接的近邻节点数。它限制了路图的度数（稀疏程度）：值越大，路图越稠密，路径选择的自由度越高，但 Dijkstra 搜索的边展开量也越大；值越小，路图越稀疏，搜索更快但可能因连通性不足而找不到路径。
• MAX_EDGE_LEN（默认 30.0 m）：单条边的最大长度。超过该长度的边直接视为无效（不可行），避免生成跨越空旷区域的长边，这既符合“沿 Voronoi 结构行走”的语义，也能显著减少无效边的计算量。

Voronoi 采样：从障碍物点集生成路径候选节点

voronoi_sampling()是静态方法，直接使用scipy.spatial.Voronoi构造障碍物点集的 Voronoi 图（voronoi_road_map.py#L118-L132）：

@staticmethod 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.vertices（Voronoi 图的顶点），而不是整条 Voronoi 边。这些顶点是三条或更多 Voronoi 边的交汇处，在几何上位于多个障碍物的“等距最远”位置，天然具有最大安全间隙。随后把起点(sx, sy)与终点(gx, gy)追加进采样列表，保证路图必然包含起点与终点两个节点——这是后续 Dijkstra 搜索能够连通起点与终点的前提。

碰撞检测：is_collision()

在构建路图时，需要判断两个采样点之间的边是否与障碍物冲突。is_collision()采用沿边步进采样 + KD-Tree 最近邻查询的方式（voronoi_road_map.py#L44-L70）：

def 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

其判定逻辑为：

1. 若边长超过MAX_EDGE_LEN，直接判定为碰撞（返回True）；
2. 以机器人半径rr为步长D，将整条边离散为n_step个采样点，逐点向障碍物 KD-Tree 查询最近距离，若某点距离障碍物小于等于rr则判定碰撞；
3. 对终点做同样的最近距离检查。

这种做法的物理含义是：把机器人近似视为半径为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], k=n_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, key=lambda 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.1（m）**作为节点“同一性”判据，用于把起点/终点匹配到采样点集合中的对应节点；
• 由于起点与终点已被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