算法原理与Python实现：机器人路径规划核心)
1. 项目缘起当自动驾驶车面对“迷宫”时我们为何需要PRM想象一下你刚拿到驾照被要求开车穿过一个完全陌生、堆满了随机障碍物的巨型停车场。没有地图没有车道线你只能看到车周围一小片区域。你的本能是什么大概率是先慢慢开东瞅瞅西看看摸清几条能走通的“小路”把这些小路在脑子里连成一张网然后再规划出一条从A到B的具体路线。这个“先探路、再联网、最后规划”的朴素思想恰恰就是概率路图Probabilistic Roadmap, PRM算法的核心。在自动驾驶、机器人导航这些领域PRM是一种经典的基于采样的路径规划算法。它特别擅长解决所谓“高维构型空间Configuration Space”中的路径规划问题。听起来很玄乎其实很简单把机器人或车辆所有可能的姿态位置、朝向等抽象成一个多维空间里的一个点。在这个空间里障碍物会膨胀成一片“禁区”。规划路径就是在这个充满禁区的空间里找一条连接起点和终点的安全曲线。为什么不用更“精确”的算法比如A搜索一个精细的网格地图因为对于多自由度的机器人比如机械臂或者需要考虑连续转向的车辆构型空间维度太高把整个空间都划分成网格离散化会导致计算量爆炸这就是“维度灾难”。PRM聪明的地方在于它不试图去理解整个空间的完整结构而是通过随机采样来“管中窥豹”用有限的采样点构建一个能反映空间连通性的近似路网然后在这个稀疏的路网上用图搜索算法如Dijkstra或A快速找路。所以当你看到自动驾驶测试车在复杂园区、地下车库或者事故现场般的临时路况中缓缓寻路时底层很可能就有PRM或其变种算法在默默工作。它不追求一次性算无遗策而是用随机性来对抗复杂世界的不可知先建立一个可行的“道路骨架”。今天我们就彻底拆解PRM并用Python从零实现它看看这个算法是如何在代码中“思考”的。2. PRM算法核心原理三步构建一张“逃生路线图”PRM算法主要分为两个阶段学习阶段Learning Phase和查询阶段Query Phase。学习阶段是离线的目标是在已知的障碍物地图中构建出路图查询阶段是在线的当给定具体的起点和终点后利用已构建的路图快速查询路径。2.1 学习阶段撒点、连线、建图这个阶段可以类比为在灾难多发区预先勘探并绘制一份紧急疏散路线图。2.1.1 随机采样Sampling这是PRM的第一步也是最体现“概率”的一步。算法在整个自由的构型空间里随机扔出N个点比如1000个。这些点就是潜在的路标或途径点。采样的策略直接影响路图的质量。最基础的是均匀随机采样就像漫天花雨撒豆子。但在实践中为了提高效率往往会采用一些启发式采样比如在狭窄通道附近进行偏向性采样以确保能“幸运地”把点撒进通道里。# 伪代码示意在二维平面[x_min, x_max] x [y_min, y_max]内均匀随机采样 import random def sample_free_space(num_samples, x_limits, y_limits, obstacles): samples [] while len(samples) num_samples: x random.uniform(x_limits[0], x_limits[1]) y random.uniform(y_limits[0], y_limits[1]) candidate_point (x, y) # 关键检查采样点必须在自由空间不能落在障碍物内 if not in_obstacle(candidate_point, obstacles): samples.append(candidate_point) return samples这里的一个核心细节是碰撞检测Collision Checking。in_obstacle函数是路径规划的基石也是最耗时的部分之一。对于圆形障碍物就是判断点是否在圆内对于多边形障碍物则常用射线法或分离轴定理。在自动驾驶中车辆不是一个点而是一个有形状的刚体因此需要将车辆轮廓“膨胀”到障碍物上或者对采样点代表的车辆位姿进行轮廓级别的碰撞检测这比点检测复杂得多。2.1.2 邻域连接Local Planning采样得到一堆孤立的点后下一步是尝试把它们连接起来形成边。但不是所有点之间都尝试连接那样计算量是O(N²)。PRM采用“邻域”策略对于每一个采样点只寻找它附近一定距离内例如半径R内的K个最近邻点。 然后对每一对“点-近邻点”尝试用一条简单的局部路径通常就是直线连接它们并检查这条局部路径是否全程都不会撞上障碍物。这个过程就是“局部规划”。如果这条直线是安全的那么就在这两个点之间建立一条无向边边的权重通常是两点间的欧氏距离。# 伪代码示意为每个采样点连接邻居 from scipy.spatial import KDTree import math def build_roadmap(samples, connection_radius, obstacles): roadmap {} # 图用邻接表表示 tree KDTree(samples) # 建立KD树用于快速最近邻搜索 for i, point in enumerate(samples): roadmap[i] [] # 初始化该点的邻接列表 # 查询半径内的所有邻居点索引和距离 # 注意返回的索引包括自己需要排除 neighbor_indices tree.query_ball_point(point, connection_radius) for idx in neighbor_indices: if idx i: continue # 跳过自身 neighbor_point samples[idx] # 关键检查两点间的直线路径是否无碰撞 if is_path_free(point, neighbor_point, obstacles): distance math.dist(point, neighbor_point) roadmap[i].append((idx, distance)) # 记录邻居索引和边权 return roadmap, samples这里的“坑”在于局部规划器的选择和碰撞检测的频率。用直线作为局部规划器最简单但在复杂障碍物环境下即使两点都不在障碍物内连接它们的直线也可能穿过障碍物。因此is_path_free函数需要对这条线段进行密集的离散采样并对每一个采样位姿进行碰撞检测。采样步长越小检测越精确但计算成本也越高。这是一个需要权衡的参数。2.1.3 路图构建Roadmap Construction经过上述两步我们得到了一张图Graph节点是采样点边是安全的局部路径。这张图就是我们的概率路图。它是对真实自由空间连通性的一个概率性近似。采样点越多路图越稠密对空间的描述越精确但构建时间也越长。2.2 查询阶段在图上的寻路当实际的路径规划请求到来时例如车辆要从位姿A到B查询阶段开始工作将起点和终点连接到路图首先将起点和终点视为两个新的节点。为它们各自寻找路图中最近的若干个节点邻居并尝试用局部规划器连接。如果连接成功就把起点/终点和这些邻居节点连接起来加入到图中。在图搜索路径现在起点和终点都已经成为了路图的一部分。接下来就可以使用标准的图搜索算法如Dijkstra算法寻找最短路径或A*算法在启发式引导下更快搜索在这张扩展后的路图上寻找一条从起点节点到终点节点的路径。路径平滑可选由于路图是由随机采样点构成的搜索出来的路径可能像折线一样拐来拐去不适合车辆直接跟踪。因此后处理中常常加入路径平滑步骤比如用梯度下降或样条曲线对路径进行优化使其更平滑、更符合车辆动力学。注意查询阶段可能会失败。失败的原因主要有两个一是起点或终点处于“陷阱区域”如被障碍物紧紧包围无法连接到主路图二是主路图本身由于采样不足是不连通的即存在多个孤立的子图而起点和终点恰好位于不同的子图中。解决方法是增加采样点或者针对起点/终点区域进行局部增量采样。3. Python实现详解从零搭建一个二维PRM规划器理论说得再多不如代码跑一遍。我们来实现一个解决二维平面点机器人路径规划的PRM。环境是简单的但流程是完整的。3.1 环境与问题定义假设我们有一个100x100的二维空间里面有若干个圆形障碍物。我们的“机器人”是一个点需要从起点(10,10)运动到终点(90,90)。import numpy as np import matplotlib.pyplot as plt import random from scipy.spatial import KDTree import networkx as nx import math # 1. 定义环境 class Environment: def __init__(self, x_range(0, 100), y_range(0, 100)): self.x_range x_range self.y_range y_range self.obstacles [] # 存储障碍物每个障碍物用字典表示例如圆形障碍物{type: circle, center: (x,y), radius: r} def add_circle_obstacle(self, center, radius): self.obstacles.append({type: circle, center: np.array(center), radius: radius}) def is_point_in_obstacle(self, point): 碰撞检测点是否在任意障碍物内 point np.array(point) for obs in self.obstacles: if obs[type] circle: if np.linalg.norm(point - obs[center]) obs[radius]: return True return False def is_path_free(self, point1, point2, num_checks20): 碰撞检测线段point1-point2是否与障碍物相交 # 在线段上均匀采样多个点进行检查 for i in range(num_checks1): alpha i / num_checks check_point point1 * (1 - alpha) point2 * alpha if self.is_point_in_obstacle(check_point): return False return True def plot(self, axNone): 绘制环境 if ax is None: fig, ax plt.subplots(figsize(10, 10)) ax.set_xlim(self.x_range) ax.set_ylim(self.y_range) ax.set_aspect(equal) for obs in self.obstacles: if obs[type] circle: circle plt.Circle(obs[center], obs[radius], colorgray, alpha0.7) ax.add_patch(circle) return ax # 创建环境并添加障碍物 env Environment() env.add_circle_obstacle((30, 30), 12) env.add_circle_obstacle((70, 50), 15) env.add_circle_obstacle((40, 70), 10) env.add_circle_obstacle((60, 20), 8) start (10, 10) goal (90, 90)3.2 PRM规划器类实现我们将PRM的核心流程封装成一个类。class PRMPlanner: def __init__(self, environment, n_samples300, k_neighbors10, connection_radius20.0): self.env environment self.n_samples n_samples # 采样点数量 self.k k_neighbors # 每个点尝试连接的最近邻数量 self.connection_radius connection_radius # 连接半径限制 self.samples [] # 采样点列表 self.graph nx.Graph() # 使用networkx的图来存储路图 self.kd_tree None # KD树用于快速近邻搜索 def generate_random_samples(self): 在自由空间内生成随机采样点 samples [] while len(samples) self.n_samples: x random.uniform(self.env.x_range[0], self.env.x_range[1]) y random.uniform(self.env.y_range[0], self.env.y_range[1]) sample (x, y) if not self.env.is_point_in_obstacle(sample): samples.append(sample) self.samples np.array(samples) # 构建KD树 if len(self.samples) 0: self.kd_tree KDTree(self.samples) return self.samples def build_roadmap(self): 构建概率路图 if len(self.samples) 0: self.generate_random_samples() # 将所有采样点作为节点加入图 for i, node in enumerate(self.samples): self.graph.add_node(i, posnode) # 为每个节点连接邻居 for i, node in enumerate(self.samples): # 方法1查询半径内的所有邻居 (使用query_ball_point) # neighbor_indices self.kd_tree.query_ball_point(node, self.connection_radius) # 方法2查询最近的k个邻居 (更常用) distances, neighbor_indices self.kd_tree.query(node, kmin(self.k1, len(self.samples))) # k1因为包含自己 for dist, idx in zip(distances[1:], neighbor_indices[1:]): # 跳过第一个自己 # 检查距离是否在连接半径内如果使用方法2需要此检查 if dist self.connection_radius: continue # 检查路径是否无碰撞 if self.env.is_path_free(node, self.samples[idx]): # 添加边权重为距离 self.graph.add_edge(i, idx, weightdist) print(f路图构建完成。节点数{self.graph.number_of_nodes()} 边数{self.graph.number_of_edges()}) def plan(self, start, goal): 查询路径连接起点终点并在路图上搜索 # 将起点和终点作为临时节点 start_node_idx len(self.samples) # 给起点一个虚拟索引 goal_node_idx start_node_idx 1 # 给终点一个虚拟索引 # 创建用于查询的临时图 query_graph self.graph.copy() # 为起点寻找邻居并连接 start_neighbor_dists, start_neighbor_idxs self.kd_tree.query(start, kmin(self.k, len(self.samples))) start_connected False for dist, idx in zip(start_neighbor_dists, start_neighbor_idxs): if self.env.is_path_free(start, self.samples[idx]): query_graph.add_edge(start_node_idx, idx, weightdist) start_connected True # 为终点寻找邻居并连接 goal_neighbor_dists, goal_neighbor_idxs self.kd_tree.query(goal, kmin(self.k, len(self.samples))) goal_connected False for dist, idx in zip(goal_neighbor_dists, goal_neighbor_idxs): if self.env.is_path_free(goal, self.samples[idx]): query_graph.add_edge(goal_node_idx, idx, weightdist) goal_connected True if not start_connected or not goal_connected: print(警告起点或终点无法连接到路图) return None # 使用Dijkstra算法搜索最短路径 try: path_node_indices nx.shortest_path(query_graph, sourcestart_node_idx, targetgoal_node_idx, weightweight) except nx.NetworkXNoPath: print(路径不存在起点和终点在图中的连通分量不同。) return None # 将节点索引转换为实际坐标 path_coords [] for idx in path_node_indices: if idx start_node_idx: path_coords.append(start) elif idx goal_node_idx: path_coords.append(goal) else: path_coords.append(self.samples[idx]) return np.array(path_coords) def smooth_path(self, path, max_iter100, tolerance0.01): 简单的路径平滑使用梯度下降思想缩短路径贪心算法 if path is None or len(path) 3: return path smoothed_path path.copy() for _ in range(max_iter): changed False for i in range(1, len(smoothed_path)-1): # 尝试将当前点替换为前后点的中点如果新路径无碰撞且更短 new_point (smoothed_path[i-1] smoothed_path[i1]) / 2.0 if (self.env.is_path_free(smoothed_path[i-1], new_point) and self.env.is_path_free(new_point, smoothed_path[i1])): # 计算新旧两条边的长度差 old_len (np.linalg.norm(smoothed_path[i] - smoothed_path[i-1]) np.linalg.norm(smoothed_path[i1] - smoothed_path[i])) new_len (np.linalg.norm(new_point - smoothed_path[i-1]) np.linalg.norm(smoothed_path[i1] - new_point)) if new_len old_len: # 如果新路径更短 smoothed_path[i] new_point changed True if not changed: break return smoothed_path3.3 运行与可视化现在让我们运行这个规划器并可视化整个过程。# 实例化规划器并构建路图 prm PRMPlanner(env, n_samples200, k_neighbors15, connection_radius25.0) samples prm.generate_random_samples() prm.build_roadmap() # 规划路径 raw_path prm.plan(start, goal) if raw_path is not None: # 路径平滑 smoothed_path prm.smooth_path(raw_path, max_iter50) # 可视化 fig, (ax1, ax2) plt.subplots(1, 2, figsize(16, 8)) # 图1显示路图和原始路径 ax1 env.plot(ax1) ax1.set_title(PRM Roadmap and Raw Path) # 绘制所有采样点 ax1.scatter(samples[:, 0], samples[:, 1], s5, cblue, alpha0.6, labelSamples) # 绘制路图的边从graph中提取 for (u, v) in prm.graph.edges(): pu prm.samples[u] pv prm.samples[v] ax1.plot([pu[0], pv[0]], [pu[1], pv[1]], gray, linewidth0.5, alpha0.5) # 绘制起点终点 ax1.scatter(start[0], start[1], s100, cgreen, markers, edgecolorsblack, labelStart) ax1.scatter(goal[0], goal[1], s100, cred, marker*, edgecolorsblack, labelGoal) # 绘制原始路径 if raw_path is not None: ax1.plot(raw_path[:, 0], raw_path[:, 1], orange, linewidth3, labelRaw Path, zorder5) ax1.legend() # 图2显示平滑后的路径 ax2 env.plot(ax2) ax2.set_title(Smoothed Path) ax2.scatter(start[0], start[1], s100, cgreen, markers, edgecolorsblack, labelStart) ax2.scatter(goal[0], goal[1], s100, cred, marker*, edgecolorsblack, labelGoal) if smoothed_path is not None: ax2.plot(smoothed_path[:, 0], smoothed_path[:, 1], lime, linewidth3, labelSmoothed Path, zorder5) ax2.legend() plt.tight_layout() plt.show() print(f原始路径长度 {np.sum(np.linalg.norm(np.diff(raw_path, axis0), axis1)):.2f}) print(f平滑后路径长度 {np.sum(np.linalg.norm(np.diff(smoothed_path, axis0), axis1)):.2f}) else: print(路径规划失败)运行这段代码你会看到两张图。第一张图展示了随机采样的蓝色点、由灰色细线连接构成的路图、以及一条从起点绿色方块到终点红色五角星的橙色折线原始路径。这条折线就是直接在路图节点上搜索出的最短路径。第二张图展示了经过平滑优化后的绿色路径它更短、更直接也更适合机器人跟踪。4. PRM的实战陷阱与调优心法纸上得来终觉浅绝知此事要躬行。实现一个能跑的PRM demo不难但要让它在复杂的真实场景中稳定可靠以下几个坑你必须心里有数。4.1 采样策略的玄学均匀随机只是开始我们用的是最简单的均匀随机采样。这在障碍物稀疏的开放空间没问题但在狭窄通道narrow passage场景下成功率极低。因为点很难被“幸运地”撒进狭窄的通道里导致路图在通道处断裂规划失败。解决方案高斯桥采样Gaussian Bridge Sampling在已采样的两个点之间按高斯分布采样第三个点。如果新点位于障碍物内而两个旧点都在自由空间那么这个新点很可能就在狭窄通道的“墙壁”附近通过简单的扰动比如向中点移动就有可能把它拉进通道。这能显著提高在狭窄区域采到点的概率。障碍物边界采样Obstacle-Based Sampling直接在障碍物的边界附近采样。因为通道往往位于障碍物之间在边界采样更容易发现通道入口。自适应采样Adaptive Sampling先构建一个初步的路图识别出图中哪些区域节点稀疏可能包含未探索的通道然后在这些区域进行第二轮重点采样。实操建议在项目中可以先从均匀采样开始快速验证流程。一旦遇到规划失败率高的场景首要怀疑对象就是采样策略。实现一个混合采样策略如80%均匀20%高斯桥往往是性价比很高的改进。4.2 连接策略的权衡半径R还是最近邻K在build_roadmap函数中我们使用了K近邻法并辅以连接半径检查。这里有两个关键参数k_neighbors和connection_radius。连接半径R太小每个节点只能连接到非常近的邻居路图会变得非常稀疏甚至形成多个孤岛连通性差。即使起点终点都能连接到路图它们也可能属于不同的孤岛导致搜索失败。连接半径R太大每个节点尝试连接的邻居太多导致is_path_free碰撞检测的次数呈平方增长计算量剧增。而且尝试连接很远的点中间直线路径穿过障碍物的概率极大大部分检测都是徒劳。最近邻数量KK是R的一个软性补充。它确保了即使某个点处在空旷地带周围R半径内点很少也能至少尝试连接K个点保证一定的连接度。但K太大同样有计算量问题。调优心法没有银弹参数。一个实用的方法是根据环境的尺度动态设置R。例如R可以设置为环境对角线长度的某个比例如5%。K则可以设为一个适中的值如10-15。更高级的做法是使用RRT快速探索随机树中类似的思想让R随着采样点的密度自适应变化在点密集的区域R减小在点稀疏的区域R增大。4.3 碰撞检测性能瓶颈与工程化is_point_in_obstacle和is_path_free是PRM中被调用最频繁的函数尤其是在局部规划连线时。线段碰撞检测需要对线段进行离散采样采样步长是一个关键参数。步长太大可能会“跳过”细小的障碍物导致碰撞漏检规划出危险的路径。步长太小检测点数量激增计算开销巨大。工程优化技巧空间划分与粗略检测在检测前先用AABB轴对齐包围盒或包围球等简单形状对障碍物和线段进行快速相交测试。如果不相交直接返回安全避免昂贵的精确检测。距离场Distance Field预计算对于静态环境可以预先计算整个空间的到最近障碍物的距离场。碰撞检测时只需查询线段上各点的距离值如果都大于机器人的半径或安全距离则路径安全。这用空间换取了极快的检测速度。分层检测先以较大步长检测如果发现可能碰撞再在可疑区间用小步长精细检测。4.4 路径质量从“能走”到“好走”PRM搜索出的原始路径是节点间的直线拼接转折生硬。对于轮式机器人或车辆这种路径是无法直接跟踪的。路径平滑我们实现了一个简单的贪心平滑算法。工业级应用会使用更强大的方法如梯度下降法、B样条曲线拟合或弹性带算法Elastic Band。这些方法不仅缩短路径还能确保路径的曲率连续满足车辆运动学约束。考虑动力学基础的PRM只考虑几何路径无碰撞。更高级的PRM变种如Kinodynamic RRT会在采样和连接时直接考虑速度和加速度等动力学约束规划出的路径本身就是动力学可行的。一个常见的误区过度平滑可能导致路径贴近障碍物。因此平滑过程中必须持续进行碰撞检测确保平滑后的路径依然安全。5. 从PRM到更高级的采样规划算法PRM奠定了采样规划的基础但它主要适用于多查询Multi-Query场景即环境固定需要多次在不同起点终点间规划。对于单次查询且环境复杂或动态的情况它的后代们表现更出色。RRT快速探索随机树针对单次查询优化。不像PRM漫无目的地全局撒点RRT像一棵生长中的树从起点开始主动向未探索区域特别是目标方向生长收敛速度通常比PRM快。这是目前应用更广泛的算法之一。RRT最优RRT*RRT的优化版本在生长过程中会不断地“重布线”和“重新选择父节点”使得生成的树在概率意义上渐近最优即路径长度越来越短。它结合了RRT的快速探索和渐进最优的特性。Informed RRT*在找到一条初始路径后它将采样区域限制在一个椭圆形的“启发式”区域内这个椭圆以起点和终点为焦点以当前最佳路径长度为长轴。这使算法后期能集中精力优化当前路径而不是继续盲目探索整个空间效率大幅提升。如何选择如果你的场景是静态的、已知的并且需要频繁规划比如仓库AGV的多任务调度离线构建一个精细的PRM路图是高效的。如果你的场景是未知的、动态的或者只是单次规划比如无人机紧急避障那么像RRT*这类单查询算法更适合。回过头看PRM的魅力在于其思想的简洁与强大用随机性来应对复杂性用图结构来抽象连通性。它可能不是最快、也不是最优的但它为无数机器人、自动驾驶汽车和智能体打开了一扇在复杂世界中寻找可行路径的大门。理解并实现了它你就拿到了进入基于采样的规划算法世界的第一把钥匙。下次当你看到自动驾驶汽车在车流中自如变道时或许就能联想到这背后可能正运行着PRM思想演化而来的、更加精巧的算法在默默计算着一条条安全的轨迹。
概率路图(PRM)算法原理与Python实现：机器人路径规划核心
发布时间：2026/7/29 4:24:58
1. 项目缘起当自动驾驶车面对“迷宫”时我们为何需要PRM想象一下你刚拿到驾照被要求开车穿过一个完全陌生、堆满了随机障碍物的巨型停车场。没有地图没有车道线你只能看到车周围一小片区域。你的本能是什么大概率是先慢慢开东瞅瞅西看看摸清几条能走通的“小路”把这些小路在脑子里连成一张网然后再规划出一条从A到B的具体路线。这个“先探路、再联网、最后规划”的朴素思想恰恰就是概率路图Probabilistic Roadmap, PRM算法的核心。在自动驾驶、机器人导航这些领域PRM是一种经典的基于采样的路径规划算法。它特别擅长解决所谓“高维构型空间Configuration Space”中的路径规划问题。听起来很玄乎其实很简单把机器人或车辆所有可能的姿态位置、朝向等抽象成一个多维空间里的一个点。在这个空间里障碍物会膨胀成一片“禁区”。规划路径就是在这个充满禁区的空间里找一条连接起点和终点的安全曲线。为什么不用更“精确”的算法比如A搜索一个精细的网格地图因为对于多自由度的机器人比如机械臂或者需要考虑连续转向的车辆构型空间维度太高把整个空间都划分成网格离散化会导致计算量爆炸这就是“维度灾难”。PRM聪明的地方在于它不试图去理解整个空间的完整结构而是通过随机采样来“管中窥豹”用有限的采样点构建一个能反映空间连通性的近似路网然后在这个稀疏的路网上用图搜索算法如Dijkstra或A快速找路。所以当你看到自动驾驶测试车在复杂园区、地下车库或者事故现场般的临时路况中缓缓寻路时底层很可能就有PRM或其变种算法在默默工作。它不追求一次性算无遗策而是用随机性来对抗复杂世界的不可知先建立一个可行的“道路骨架”。今天我们就彻底拆解PRM并用Python从零实现它看看这个算法是如何在代码中“思考”的。2. PRM算法核心原理三步构建一张“逃生路线图”PRM算法主要分为两个阶段学习阶段Learning Phase和查询阶段Query Phase。学习阶段是离线的目标是在已知的障碍物地图中构建出路图查询阶段是在线的当给定具体的起点和终点后利用已构建的路图快速查询路径。2.1 学习阶段撒点、连线、建图这个阶段可以类比为在灾难多发区预先勘探并绘制一份紧急疏散路线图。2.1.1 随机采样Sampling这是PRM的第一步也是最体现“概率”的一步。算法在整个自由的构型空间里随机扔出N个点比如1000个。这些点就是潜在的路标或途径点。采样的策略直接影响路图的质量。最基础的是均匀随机采样就像漫天花雨撒豆子。但在实践中为了提高效率往往会采用一些启发式采样比如在狭窄通道附近进行偏向性采样以确保能“幸运地”把点撒进通道里。# 伪代码示意在二维平面[x_min, x_max] x [y_min, y_max]内均匀随机采样 import random def sample_free_space(num_samples, x_limits, y_limits, obstacles): samples [] while len(samples) num_samples: x random.uniform(x_limits[0], x_limits[1]) y random.uniform(y_limits[0], y_limits[1]) candidate_point (x, y) # 关键检查采样点必须在自由空间不能落在障碍物内 if not in_obstacle(candidate_point, obstacles): samples.append(candidate_point) return samples这里的一个核心细节是碰撞检测Collision Checking。in_obstacle函数是路径规划的基石也是最耗时的部分之一。对于圆形障碍物就是判断点是否在圆内对于多边形障碍物则常用射线法或分离轴定理。在自动驾驶中车辆不是一个点而是一个有形状的刚体因此需要将车辆轮廓“膨胀”到障碍物上或者对采样点代表的车辆位姿进行轮廓级别的碰撞检测这比点检测复杂得多。2.1.2 邻域连接Local Planning采样得到一堆孤立的点后下一步是尝试把它们连接起来形成边。但不是所有点之间都尝试连接那样计算量是O(N²)。PRM采用“邻域”策略对于每一个采样点只寻找它附近一定距离内例如半径R内的K个最近邻点。 然后对每一对“点-近邻点”尝试用一条简单的局部路径通常就是直线连接它们并检查这条局部路径是否全程都不会撞上障碍物。这个过程就是“局部规划”。如果这条直线是安全的那么就在这两个点之间建立一条无向边边的权重通常是两点间的欧氏距离。# 伪代码示意为每个采样点连接邻居 from scipy.spatial import KDTree import math def build_roadmap(samples, connection_radius, obstacles): roadmap {} # 图用邻接表表示 tree KDTree(samples) # 建立KD树用于快速最近邻搜索 for i, point in enumerate(samples): roadmap[i] [] # 初始化该点的邻接列表 # 查询半径内的所有邻居点索引和距离 # 注意返回的索引包括自己需要排除 neighbor_indices tree.query_ball_point(point, connection_radius) for idx in neighbor_indices: if idx i: continue # 跳过自身 neighbor_point samples[idx] # 关键检查两点间的直线路径是否无碰撞 if is_path_free(point, neighbor_point, obstacles): distance math.dist(point, neighbor_point) roadmap[i].append((idx, distance)) # 记录邻居索引和边权 return roadmap, samples这里的“坑”在于局部规划器的选择和碰撞检测的频率。用直线作为局部规划器最简单但在复杂障碍物环境下即使两点都不在障碍物内连接它们的直线也可能穿过障碍物。因此is_path_free函数需要对这条线段进行密集的离散采样并对每一个采样位姿进行碰撞检测。采样步长越小检测越精确但计算成本也越高。这是一个需要权衡的参数。2.1.3 路图构建Roadmap Construction经过上述两步我们得到了一张图Graph节点是采样点边是安全的局部路径。这张图就是我们的概率路图。它是对真实自由空间连通性的一个概率性近似。采样点越多路图越稠密对空间的描述越精确但构建时间也越长。2.2 查询阶段在图上的寻路当实际的路径规划请求到来时例如车辆要从位姿A到B查询阶段开始工作将起点和终点连接到路图首先将起点和终点视为两个新的节点。为它们各自寻找路图中最近的若干个节点邻居并尝试用局部规划器连接。如果连接成功就把起点/终点和这些邻居节点连接起来加入到图中。在图搜索路径现在起点和终点都已经成为了路图的一部分。接下来就可以使用标准的图搜索算法如Dijkstra算法寻找最短路径或A*算法在启发式引导下更快搜索在这张扩展后的路图上寻找一条从起点节点到终点节点的路径。路径平滑可选由于路图是由随机采样点构成的搜索出来的路径可能像折线一样拐来拐去不适合车辆直接跟踪。因此后处理中常常加入路径平滑步骤比如用梯度下降或样条曲线对路径进行优化使其更平滑、更符合车辆动力学。注意查询阶段可能会失败。失败的原因主要有两个一是起点或终点处于“陷阱区域”如被障碍物紧紧包围无法连接到主路图二是主路图本身由于采样不足是不连通的即存在多个孤立的子图而起点和终点恰好位于不同的子图中。解决方法是增加采样点或者针对起点/终点区域进行局部增量采样。3. Python实现详解从零搭建一个二维PRM规划器理论说得再多不如代码跑一遍。我们来实现一个解决二维平面点机器人路径规划的PRM。环境是简单的但流程是完整的。3.1 环境与问题定义假设我们有一个100x100的二维空间里面有若干个圆形障碍物。我们的“机器人”是一个点需要从起点(10,10)运动到终点(90,90)。import numpy as np import matplotlib.pyplot as plt import random from scipy.spatial import KDTree import networkx as nx import math # 1. 定义环境 class Environment: def __init__(self, x_range(0, 100), y_range(0, 100)): self.x_range x_range self.y_range y_range self.obstacles [] # 存储障碍物每个障碍物用字典表示例如圆形障碍物{type: circle, center: (x,y), radius: r} def add_circle_obstacle(self, center, radius): self.obstacles.append({type: circle, center: np.array(center), radius: radius}) def is_point_in_obstacle(self, point): 碰撞检测点是否在任意障碍物内 point np.array(point) for obs in self.obstacles: if obs[type] circle: if np.linalg.norm(point - obs[center]) obs[radius]: return True return False def is_path_free(self, point1, point2, num_checks20): 碰撞检测线段point1-point2是否与障碍物相交 # 在线段上均匀采样多个点进行检查 for i in range(num_checks1): alpha i / num_checks check_point point1 * (1 - alpha) point2 * alpha if self.is_point_in_obstacle(check_point): return False return True def plot(self, axNone): 绘制环境 if ax is None: fig, ax plt.subplots(figsize(10, 10)) ax.set_xlim(self.x_range) ax.set_ylim(self.y_range) ax.set_aspect(equal) for obs in self.obstacles: if obs[type] circle: circle plt.Circle(obs[center], obs[radius], colorgray, alpha0.7) ax.add_patch(circle) return ax # 创建环境并添加障碍物 env Environment() env.add_circle_obstacle((30, 30), 12) env.add_circle_obstacle((70, 50), 15) env.add_circle_obstacle((40, 70), 10) env.add_circle_obstacle((60, 20), 8) start (10, 10) goal (90, 90)3.2 PRM规划器类实现我们将PRM的核心流程封装成一个类。class PRMPlanner: def __init__(self, environment, n_samples300, k_neighbors10, connection_radius20.0): self.env environment self.n_samples n_samples # 采样点数量 self.k k_neighbors # 每个点尝试连接的最近邻数量 self.connection_radius connection_radius # 连接半径限制 self.samples [] # 采样点列表 self.graph nx.Graph() # 使用networkx的图来存储路图 self.kd_tree None # KD树用于快速近邻搜索 def generate_random_samples(self): 在自由空间内生成随机采样点 samples [] while len(samples) self.n_samples: x random.uniform(self.env.x_range[0], self.env.x_range[1]) y random.uniform(self.env.y_range[0], self.env.y_range[1]) sample (x, y) if not self.env.is_point_in_obstacle(sample): samples.append(sample) self.samples np.array(samples) # 构建KD树 if len(self.samples) 0: self.kd_tree KDTree(self.samples) return self.samples def build_roadmap(self): 构建概率路图 if len(self.samples) 0: self.generate_random_samples() # 将所有采样点作为节点加入图 for i, node in enumerate(self.samples): self.graph.add_node(i, posnode) # 为每个节点连接邻居 for i, node in enumerate(self.samples): # 方法1查询半径内的所有邻居 (使用query_ball_point) # neighbor_indices self.kd_tree.query_ball_point(node, self.connection_radius) # 方法2查询最近的k个邻居 (更常用) distances, neighbor_indices self.kd_tree.query(node, kmin(self.k1, len(self.samples))) # k1因为包含自己 for dist, idx in zip(distances[1:], neighbor_indices[1:]): # 跳过第一个自己 # 检查距离是否在连接半径内如果使用方法2需要此检查 if dist self.connection_radius: continue # 检查路径是否无碰撞 if self.env.is_path_free(node, self.samples[idx]): # 添加边权重为距离 self.graph.add_edge(i, idx, weightdist) print(f路图构建完成。节点数{self.graph.number_of_nodes()} 边数{self.graph.number_of_edges()}) def plan(self, start, goal): 查询路径连接起点终点并在路图上搜索 # 将起点和终点作为临时节点 start_node_idx len(self.samples) # 给起点一个虚拟索引 goal_node_idx start_node_idx 1 # 给终点一个虚拟索引 # 创建用于查询的临时图 query_graph self.graph.copy() # 为起点寻找邻居并连接 start_neighbor_dists, start_neighbor_idxs self.kd_tree.query(start, kmin(self.k, len(self.samples))) start_connected False for dist, idx in zip(start_neighbor_dists, start_neighbor_idxs): if self.env.is_path_free(start, self.samples[idx]): query_graph.add_edge(start_node_idx, idx, weightdist) start_connected True # 为终点寻找邻居并连接 goal_neighbor_dists, goal_neighbor_idxs self.kd_tree.query(goal, kmin(self.k, len(self.samples))) goal_connected False for dist, idx in zip(goal_neighbor_dists, goal_neighbor_idxs): if self.env.is_path_free(goal, self.samples[idx]): query_graph.add_edge(goal_node_idx, idx, weightdist) goal_connected True if not start_connected or not goal_connected: print(警告起点或终点无法连接到路图) return None # 使用Dijkstra算法搜索最短路径 try: path_node_indices nx.shortest_path(query_graph, sourcestart_node_idx, targetgoal_node_idx, weightweight) except nx.NetworkXNoPath: print(路径不存在起点和终点在图中的连通分量不同。) return None # 将节点索引转换为实际坐标 path_coords [] for idx in path_node_indices: if idx start_node_idx: path_coords.append(start) elif idx goal_node_idx: path_coords.append(goal) else: path_coords.append(self.samples[idx]) return np.array(path_coords) def smooth_path(self, path, max_iter100, tolerance0.01): 简单的路径平滑使用梯度下降思想缩短路径贪心算法 if path is None or len(path) 3: return path smoothed_path path.copy() for _ in range(max_iter): changed False for i in range(1, len(smoothed_path)-1): # 尝试将当前点替换为前后点的中点如果新路径无碰撞且更短 new_point (smoothed_path[i-1] smoothed_path[i1]) / 2.0 if (self.env.is_path_free(smoothed_path[i-1], new_point) and self.env.is_path_free(new_point, smoothed_path[i1])): # 计算新旧两条边的长度差 old_len (np.linalg.norm(smoothed_path[i] - smoothed_path[i-1]) np.linalg.norm(smoothed_path[i1] - smoothed_path[i])) new_len (np.linalg.norm(new_point - smoothed_path[i-1]) np.linalg.norm(smoothed_path[i1] - new_point)) if new_len old_len: # 如果新路径更短 smoothed_path[i] new_point changed True if not changed: break return smoothed_path3.3 运行与可视化现在让我们运行这个规划器并可视化整个过程。# 实例化规划器并构建路图 prm PRMPlanner(env, n_samples200, k_neighbors15, connection_radius25.0) samples prm.generate_random_samples() prm.build_roadmap() # 规划路径 raw_path prm.plan(start, goal) if raw_path is not None: # 路径平滑 smoothed_path prm.smooth_path(raw_path, max_iter50) # 可视化 fig, (ax1, ax2) plt.subplots(1, 2, figsize(16, 8)) # 图1显示路图和原始路径 ax1 env.plot(ax1) ax1.set_title(PRM Roadmap and Raw Path) # 绘制所有采样点 ax1.scatter(samples[:, 0], samples[:, 1], s5, cblue, alpha0.6, labelSamples) # 绘制路图的边从graph中提取 for (u, v) in prm.graph.edges(): pu prm.samples[u] pv prm.samples[v] ax1.plot([pu[0], pv[0]], [pu[1], pv[1]], gray, linewidth0.5, alpha0.5) # 绘制起点终点 ax1.scatter(start[0], start[1], s100, cgreen, markers, edgecolorsblack, labelStart) ax1.scatter(goal[0], goal[1], s100, cred, marker*, edgecolorsblack, labelGoal) # 绘制原始路径 if raw_path is not None: ax1.plot(raw_path[:, 0], raw_path[:, 1], orange, linewidth3, labelRaw Path, zorder5) ax1.legend() # 图2显示平滑后的路径 ax2 env.plot(ax2) ax2.set_title(Smoothed Path) ax2.scatter(start[0], start[1], s100, cgreen, markers, edgecolorsblack, labelStart) ax2.scatter(goal[0], goal[1], s100, cred, marker*, edgecolorsblack, labelGoal) if smoothed_path is not None: ax2.plot(smoothed_path[:, 0], smoothed_path[:, 1], lime, linewidth3, labelSmoothed Path, zorder5) ax2.legend() plt.tight_layout() plt.show() print(f原始路径长度 {np.sum(np.linalg.norm(np.diff(raw_path, axis0), axis1)):.2f}) print(f平滑后路径长度 {np.sum(np.linalg.norm(np.diff(smoothed_path, axis0), axis1)):.2f}) else: print(路径规划失败)运行这段代码你会看到两张图。第一张图展示了随机采样的蓝色点、由灰色细线连接构成的路图、以及一条从起点绿色方块到终点红色五角星的橙色折线原始路径。这条折线就是直接在路图节点上搜索出的最短路径。第二张图展示了经过平滑优化后的绿色路径它更短、更直接也更适合机器人跟踪。4. PRM的实战陷阱与调优心法纸上得来终觉浅绝知此事要躬行。实现一个能跑的PRM demo不难但要让它在复杂的真实场景中稳定可靠以下几个坑你必须心里有数。4.1 采样策略的玄学均匀随机只是开始我们用的是最简单的均匀随机采样。这在障碍物稀疏的开放空间没问题但在狭窄通道narrow passage场景下成功率极低。因为点很难被“幸运地”撒进狭窄的通道里导致路图在通道处断裂规划失败。解决方案高斯桥采样Gaussian Bridge Sampling在已采样的两个点之间按高斯分布采样第三个点。如果新点位于障碍物内而两个旧点都在自由空间那么这个新点很可能就在狭窄通道的“墙壁”附近通过简单的扰动比如向中点移动就有可能把它拉进通道。这能显著提高在狭窄区域采到点的概率。障碍物边界采样Obstacle-Based Sampling直接在障碍物的边界附近采样。因为通道往往位于障碍物之间在边界采样更容易发现通道入口。自适应采样Adaptive Sampling先构建一个初步的路图识别出图中哪些区域节点稀疏可能包含未探索的通道然后在这些区域进行第二轮重点采样。实操建议在项目中可以先从均匀采样开始快速验证流程。一旦遇到规划失败率高的场景首要怀疑对象就是采样策略。实现一个混合采样策略如80%均匀20%高斯桥往往是性价比很高的改进。4.2 连接策略的权衡半径R还是最近邻K在build_roadmap函数中我们使用了K近邻法并辅以连接半径检查。这里有两个关键参数k_neighbors和connection_radius。连接半径R太小每个节点只能连接到非常近的邻居路图会变得非常稀疏甚至形成多个孤岛连通性差。即使起点终点都能连接到路图它们也可能属于不同的孤岛导致搜索失败。连接半径R太大每个节点尝试连接的邻居太多导致is_path_free碰撞检测的次数呈平方增长计算量剧增。而且尝试连接很远的点中间直线路径穿过障碍物的概率极大大部分检测都是徒劳。最近邻数量KK是R的一个软性补充。它确保了即使某个点处在空旷地带周围R半径内点很少也能至少尝试连接K个点保证一定的连接度。但K太大同样有计算量问题。调优心法没有银弹参数。一个实用的方法是根据环境的尺度动态设置R。例如R可以设置为环境对角线长度的某个比例如5%。K则可以设为一个适中的值如10-15。更高级的做法是使用RRT快速探索随机树中类似的思想让R随着采样点的密度自适应变化在点密集的区域R减小在点稀疏的区域R增大。4.3 碰撞检测性能瓶颈与工程化is_point_in_obstacle和is_path_free是PRM中被调用最频繁的函数尤其是在局部规划连线时。线段碰撞检测需要对线段进行离散采样采样步长是一个关键参数。步长太大可能会“跳过”细小的障碍物导致碰撞漏检规划出危险的路径。步长太小检测点数量激增计算开销巨大。工程优化技巧空间划分与粗略检测在检测前先用AABB轴对齐包围盒或包围球等简单形状对障碍物和线段进行快速相交测试。如果不相交直接返回安全避免昂贵的精确检测。距离场Distance Field预计算对于静态环境可以预先计算整个空间的到最近障碍物的距离场。碰撞检测时只需查询线段上各点的距离值如果都大于机器人的半径或安全距离则路径安全。这用空间换取了极快的检测速度。分层检测先以较大步长检测如果发现可能碰撞再在可疑区间用小步长精细检测。4.4 路径质量从“能走”到“好走”PRM搜索出的原始路径是节点间的直线拼接转折生硬。对于轮式机器人或车辆这种路径是无法直接跟踪的。路径平滑我们实现了一个简单的贪心平滑算法。工业级应用会使用更强大的方法如梯度下降法、B样条曲线拟合或弹性带算法Elastic Band。这些方法不仅缩短路径还能确保路径的曲率连续满足车辆运动学约束。考虑动力学基础的PRM只考虑几何路径无碰撞。更高级的PRM变种如Kinodynamic RRT会在采样和连接时直接考虑速度和加速度等动力学约束规划出的路径本身就是动力学可行的。一个常见的误区过度平滑可能导致路径贴近障碍物。因此平滑过程中必须持续进行碰撞检测确保平滑后的路径依然安全。5. 从PRM到更高级的采样规划算法PRM奠定了采样规划的基础但它主要适用于多查询Multi-Query场景即环境固定需要多次在不同起点终点间规划。对于单次查询且环境复杂或动态的情况它的后代们表现更出色。RRT快速探索随机树针对单次查询优化。不像PRM漫无目的地全局撒点RRT像一棵生长中的树从起点开始主动向未探索区域特别是目标方向生长收敛速度通常比PRM快。这是目前应用更广泛的算法之一。RRT最优RRT*RRT的优化版本在生长过程中会不断地“重布线”和“重新选择父节点”使得生成的树在概率意义上渐近最优即路径长度越来越短。它结合了RRT的快速探索和渐进最优的特性。Informed RRT*在找到一条初始路径后它将采样区域限制在一个椭圆形的“启发式”区域内这个椭圆以起点和终点为焦点以当前最佳路径长度为长轴。这使算法后期能集中精力优化当前路径而不是继续盲目探索整个空间效率大幅提升。如何选择如果你的场景是静态的、已知的并且需要频繁规划比如仓库AGV的多任务调度离线构建一个精细的PRM路图是高效的。如果你的场景是未知的、动态的或者只是单次规划比如无人机紧急避障那么像RRT*这类单查询算法更适合。回过头看PRM的魅力在于其思想的简洁与强大用随机性来应对复杂性用图结构来抽象连通性。它可能不是最快、也不是最优的但它为无数机器人、自动驾驶汽车和智能体打开了一扇在复杂世界中寻找可行路径的大门。理解并实现了它你就拿到了进入基于采样的规划算法世界的第一把钥匙。下次当你看到自动驾驶汽车在车流中自如变道时或许就能联想到这背后可能正运行着PRM思想演化而来的、更加精巧的算法在默默计算着一条条安全的轨迹。
