参考资料

路径规划 | 随机采样算法
路径规划: PRM 路径规划算法
Probabilistic Roadmaps (PRM)
Probabilistic roadmap

1. 基本概念

1.1 基于随机采样的路径规划算法

基于随机采样的路径规划算法适用于高维度空间，它们以概率完备性(当时间接近无限时一定有解)来代替完备性，从而提高搜索效率。
基于随机采样的路径规划算法又分为单查询算法(single-query path planning)以及渐近最优算法(asymptotically optimal path planning)，前者只要找到可行路径即可，侧重快速性，后者还会对找到的路径进行逐步优化，慢慢达到最优，侧重最优性。单查询方法包括概率路图算法(Probabilistic Road Map, PRM)、快速随机扩展树算法(Rapidly-exploring Random Tree, RRT)、RRT-Connect算法等，渐近最优算法有RRT*算法等。

1.2 概率路图算法(Probabilistic Road Map, PRM)

概率路图算法是一种典型的基于采样的路径规划方法。它主要分为两个阶段：学习阶段，查询阶段。

学习阶段

应用PRM算法进行路径规划时，首先在将连续空间转换成离散空间后，在离散空间中采样一个无碰撞的点（随机撒点，剔除落在障碍物上的点），以该点为中心，在一定的半径范围内搜索其邻域点，并将其连接形成路径，随后进行碰撞检测，若无碰撞，则保留该路径。

查询阶段

当空间中所有采样点均完成上述步骤后，再应用图搜索算法搜索出可行路径。

1.3 PRM算法的优缺点

优点

该算法原理简单，容易实现，只需要调整参数即可实现不同场景的路径规划，且不需要对环境中的障碍物进行精确建模，在高维空间和动态环境中的路径规划有很大优势。

缺点

但该算法存在狭窄通路问题：空白区域采样点密集，障碍物密集处采样点又相对较少，可能无法得到最短路径。而且参数的设置对路径规划的结果影响较大，采样点的数量、邻域的大小设置不合理均可能导致路径规划失败（如采样点设置过少导致生成的路径过少未覆盖起终点、邻域设置过大导致过多的路径无法通过碰撞检测）。

所以PRM是概率完备且不最优的算法。

1.4 PRM算法伪代码

概率路图的构建过程的伪代码如下：

如上图所示，

步骤 1、2 : 初始化两个集合，其中VVV表示随机点集，EEE表示路径集。
步骤 3~8 : 每次随机采样一个无碰撞的点，将这个无碰撞的点加入VVV中，重复nnn次。
步骤 9~16：生成概率路图。
- 步骤 10：对VVV中的每个点qqq，根据一定的距离范围选择kkk个邻域点
- 步骤 11~15：对每个领域点q′q'q′进行判断，如果qqq和q′q'q′尚未形成路径，则将其连接形成路径，随后进行碰撞检测，若无碰撞，则保留该路径。

在依据上述步骤构建完路图后，使用图搜索算法(如Dijkstra算法)在路图中搜索出一条从起点到终点的最短路径即可。

2. python代码实现

代码主体来自pythonRobotics，这里只进行了相应的注释。另外欢迎访问我的github仓库~~

import math
import numpy as np
import matplotlib.pyplot as plt
from scipy.spatial import KDTree
from celluloid import Camera  # 保存动图时用，pip install celluloid# parameter
N_SAMPLE = 500  # 采样点数目，即随机点集V的大小
N_KNN = 10  # 一个采样点的领域点个数
MAX_EDGE_LEN = 30.0  # [m] Maximum edge lengthshow_animation = True"""
kd-tree用于快速查找nearest-neighborquery(self, x[, k, eps, p, distance_upper_bound]): 查询kd-tree附近的邻居"""class Node:"""Node class for dijkstra search"""def __init__(self, x, y, cost, parent_index):self.x = xself.y = yself.cost = cost # 每条边权值self.parent_index = parent_indexdef __str__(self):return str(self.x) + "," + str(self.y) + "," +\str(self.cost) + "," + str(self.parent_index)def prm_planning(start_x, start_y, goal_x, goal_y, obstacle_x_list, obstacle_y_list, robot_radius, *, camara=None,rng=None):"""Run probabilistic road map planning:param start_x: start x position:param start_y: start y position:param goal_x: goal x position:param goal_y: goal y position:param obstacle_x_list: obstacle x positions:param obstacle_y_list: obstacle y positions:param robot_radius: robot radius:param rng: 随机数构造器:return:"""obstacle_kd_tree = KDTree(np.vstack((obstacle_x_list, obstacle_y_list)).T)# 采样点集生成sample_x, sample_y = sample_points(start_x, start_y, goal_x, goal_y,robot_radius,obstacle_x_list, obstacle_y_list,obstacle_kd_tree, rng)if show_animation:plt.plot(sample_x, sample_y, ".b")# 生成概率路图road_map = generate_road_map(sample_x, sample_y, robot_radius, obstacle_kd_tree)# 使用迪杰斯特拉规划路径rx, ry = dijkstra_planning(start_x, start_y, goal_x, goal_y, road_map, sample_x, sample_y,camara)return rx, rydef is_collision(sx, sy, gx, gy, rr, obstacle_kd_tree):"""判断是否发生碰撞,true碰撞，false不碰rr: 机器人半径"""x = sxy = sydx = gx - sxdy = gy - syyaw = math.atan2(gy - sy, gx - sx)d = math.hypot(dx, dy)if d >= MAX_EDGE_LEN:return TrueD = rrn_step = round(d / D)for i in range(n_step):dist, _ = obstacle_kd_tree.query([x, y])  # 查询kd-tree附近的邻居if dist <= rr:return True  # collisionx += D * math.cos(yaw)y += D * math.sin(yaw)# goal point checkdist, _ = obstacle_kd_tree.query([gx, gy])if dist <= rr: return True  # collisionreturn False  # OKdef generate_road_map(sample_x, sample_y, rr, obstacle_kd_tree):"""概率路图生成sample_x: [m] x positions of sampled pointssample_y: [m] y positions of sampled pointsrobot_radius: Robot Radius[m]obstacle_kd_tree: KDTree object of obstacles"""road_map = []n_sample = len(sample_x)sample_kd_tree = KDTree(np.vstack((sample_x, sample_y)).T)for (i, ix, iy) in zip(range(n_sample), sample_x, sample_y):# 对V中的每个点q，选择k个邻域点dists, indexes = sample_kd_tree.query([ix, iy], k=n_sample)edge_id = []for ii in range(1, len(indexes)):nx = sample_x[indexes[ii]]ny = sample_y[indexes[ii]]# 对每个领域点$q'$进行判断，如果$q$和$q'$尚未形成路径，则将其连接形成路径并进行碰撞检测，若无碰撞，则保留该路径。if not is_collision(ix, iy, nx, ny, rr, obstacle_kd_tree):edge_id.append(indexes[ii])if len(edge_id) >= N_KNN:breakroad_map.append(edge_id)#  plot_road_map(road_map, sample_x, sample_y)return road_mapdef dijkstra_planning(sx, sy, gx, gy, road_map, sample_x, sample_y,camara):"""s_x: start x position [m]s_y: start y position [m]goal_x: goal x position [m]goal_y: goal y position [m]obstacle_x_list: x position list of Obstacles [m]obstacle_y_list: y position list of Obstacles [m]robot_radius: robot radius [m]road_map: 构建好的路图 [m]sample_x: 采样点集x [m]sample_y: 采样点集y [m]@return: Two lists of path coordinates ([x1, x2, ...], [y1, y2, ...]), empty list when no path was found"""start_node = Node(sx, sy, 0.0, -1)goal_node = Node(gx, gy, 0.0, -1)# 使用字典的方式构造开闭集合# openList表由待考察的节点组成， closeList表由已经考察过的节点组成。open_set, closed_set = dict(), dict()open_set[len(road_map) - 2] = start_nodepath_found = True# 步骤与A星算法一致while True:# 如果open_set是空的if not open_set:print("Cannot find path")path_found = Falsebreakc_id = min(open_set, key=lambda o: open_set[o].cost)current = open_set[c_id]# show graphif show_animation and len(closed_set.keys()) % 2 == 0:# for stopping simulation with the esc key.plt.gcf().canvas.mpl_connect('key_release_event',lambda event: [exit(0) if event.key == 'escape' else None])plt.plot(current.x, current.y, "xg")plt.pause(0.001)if camara !=None:camara.snap()if c_id == (len(road_map) - 1):print("goal is found!")goal_node.parent_index = current.parent_indexgoal_node.cost = current.costbreak# Remove the item from the open setdel open_set[c_id]# Add it to the closed setclosed_set[c_id] = current# expand search grid based on motion modelfor i in range(len(road_map[c_id])):n_id = road_map[c_id][i]dx = sample_x[n_id] - current.xdy = sample_y[n_id] - current.yd = math.hypot(dx, dy)node = Node(sample_x[n_id], sample_y[n_id],current.cost + d, c_id)if n_id in closed_set:continue# Otherwise if it is already in the open setif n_id in open_set:if open_set[n_id].cost > node.cost:open_set[n_id].cost = node.costopen_set[n_id].parent_index = c_idelse:open_set[n_id] = nodeif path_found is False:return [], []# generate final courserx, ry = [goal_node.x], [goal_node.y]parent_index = goal_node.parent_indexwhile parent_index != -1:n = closed_set[parent_index]rx.append(n.x)ry.append(n.y)parent_index = n.parent_indexreturn rx, rydef sample_points(sx, sy, gx, gy, rr, ox, oy, obstacle_kd_tree, rng):"""采样点集生成"""max_x = max(ox)max_y = max(oy)min_x = min(ox)min_y = min(oy)sample_x, sample_y = [], []if rng is None:rng = np.random.default_rng()while len(sample_x) <= N_SAMPLE:tx = (rng.random() * (max_x - min_x)) + min_xty = (rng.random() * (max_y - min_y)) + min_y# 在障碍物中查询离[tx, ty]最近的点的距离dist, index = obstacle_kd_tree.query([tx, ty])# 距离大于机器人半径，说明没有碰撞，将这个无碰撞的点加入V中，重复n次。if dist >= rr:sample_x.append(tx)sample_y.append(ty)# 别忘了起点和目标点sample_x.append(sx)sample_y.append(sy)sample_x.append(gx)sample_y.append(gy)return sample_x, sample_ydef plot_road_map(road_map, sample_x, sample_y):  # pragma: no coverfor i, _ in enumerate(road_map):for ii in range(len(road_map[i])):ind = road_map[i][ii]plt.plot([sample_x[i], sample_x[ind]],[sample_y[i], sample_y[ind]], "-k")def main(rng=None):print( " start!!")fig = plt.figure(1)# camara = Camera(fig)  # 保存动图时使用camara = None# start and goal positionsx = 10.0  # [m]sy = 10.0  # [m]gx = 50.0  # [m]gy = 50.0  # [m]robot_size = 5.0  # [m]ox = []oy = []for i in range(60):ox.append(i)oy.append(0.0)for i in range(60):ox.append(60.0)oy.append(i)for i in range(61):ox.append(i)oy.append(60.0)for i in range(61):ox.append(0.0)oy.append(i)for i in range(40):ox.append(20.0)oy.append(i)for i in range(40):ox.append(40.0)oy.append(60.0 - i)if show_animation:plt.plot(ox, oy, ".k")plt.plot(sx, sy, "^r")plt.plot(gx, gy, "^c")plt.grid(True)plt.axis("equal")if camara != None:camara.snap()rx, ry = prm_planning(sx, sy, gx, gy, ox, oy, robot_size,camara=camara, rng=rng)assert rx, 'Cannot found path'if show_animation:plt.plot(rx, ry, "-r")plt.pause(0.001)if camara!=None:camara.snap()animation = camara.animate()animation.save('trajectory.gif')plt.savefig("result.png")plt.show()if __name__ == '__main__':main()

实现效果如下：

【自动驾驶】基于采样的路径规划算法——PRM（含python实现）相关推荐

Apollo学习笔记（24）基于采样的路径规划算法
之前的文章都是基于搜索的路径算法,这两天在又学习了一下基于采样的路径规划算法,这里做一下记录,最后会奉上大神的链接基于采样的路径规划算法大致可以分为综合查询方法和单一查询方法两种. 前者首先构建路线 ...
基于采样的路径规划算法RRT和代码实现
文章目录前言一.概率路图法 1.1 采样阶段 1.2 搜索阶段 1.3 Lazy collision-checking 二.快速扩展随机树 2.1 RRT算法流程 2.2 RRT 算法改进 2.3 ...
基于采样的路径规划算法总结：RRT-Matlab实现
任务说明在一张大小800*800具有障碍物的地图里实现RRT算法算法流程流程图流程描述 Sample()函数在地图上随机采样一个点Xrand 遍历树T得到距离Xrand最近的点Xnear 扩展 ...
自动驾驶路径规划——基于概率采样的路径规划算法（RRT、RRT*）
目录 1. RRT算法背景 1.1 RRT算法核心思想 1.2 RRT算法优缺点 2. 经典RRT算法 2.1 RRT算法流程 2.2 RRT伪代码 3. 基于目标概率采样 4. RRT*算法 4.1 ...
第3章第1节-基于采样的路径规划-PRMRRT及其优化
基于采样的路径规划(Sampling Based-Planners) 与基于search方法所不同的是,基于采样的路径规划不需要遍历空间所有点,而是通过在空间中随机撒点,通过线段连点构成路图/树(没有 ...
自动驾驶路径规划——基于概率采样的路径规划算法（PRM）
目录 1. PRM算法流程 1.1 预处理 1.2 搜索 2. PRM算法案例 2.1 构型采样 2.2 邻域计算 2.3 图搜索(A*搜索) 3. 采样数量的影响 4. 采样策略 4.1 基于障碍物 ...
无人车路径规划算法—（3）基于搜索的路径规划算法（BFS/DFS/Dijkstra）
1.BFS(广度优先搜索) && DFS(深度优先搜索) 广度优先遍历图的方式为,一次性访问当前顶点的所有未访问状态相邻顶点,并依次对每个相邻顶点执行同样处理.因为要依次对每个相邻顶点 ...
路径规划算法--PRM，从原理到Matlab实现
注: 本篇博客转载自运动规划入门 | 3. 白话PRM,从原理到Matlab实现,老师讲的太好了,我这里转载留存下. 上一回,我们讲完了A的工作原理,与Dijkstar相比A确实有一定程度上的优化,但 ...
无人车路径规划算法---（4）基于搜索的路径规划算法 II（贪心/Astar）
上篇博客中介绍了一些基本的图搜索算法,其中也重点介绍了基于势场来实现的Dijkstra算法.本篇博客将介绍关于Heuristic Function的图搜索算法开源了一个结合Dijkstra,Gree ...

【自动驾驶】基于采样的路径规划算法——PRM（含python实现）

文章目录