Probabilistic Road Map

原理

PRM的英文拼写是Probabilistic Road Map，翻译过来即是概率路线图。PRM与以A*为代表的基于图搜索的路径搜索算法区别很大，其是一种基于采样的方法，大概是为了解决基于图搜索算法在高维空间搜索过于缓慢的问题吧。

下面简单叙述下PRM的算法思想：首先在采样空间内随机生成采样点，其次剔除掉在障碍物区域的采样点，然后找到每个采样点的最近几个点并进行连接，之后检查连接线是否与障碍物干涉并删除干涉的连接线，最后使用dijkstra或者A*在基于连接线构成的图上从起点到终点搜索出一条路径。

参考资料

补充说明：Lazy collision-checking与一般PRM的区别在于：PRM首先进行连接线的碰撞检测再使用使用dijkstra或者A*生成的路径，而Lazy collision-checking先使用dijkstra或者A*生成路径，然后删除掉与障碍物干涉的连接线，并在引起碰撞的两个路径点之间重新搜索一条路径。

例子

该例子来源于开源代码。

"""
Probabilistic Road Map (PRM) Planner
author: Atsushi Sakai (@Atsushi_twi)
"""import random
import math
import numpy as np
import matplotlib.pyplot as plt
from scipy.spatial import cKDTree# parameter
N_SAMPLE = 500  # number of sample_points
N_KNN = 10  # number of edge from one sampled point
MAX_EDGE_LEN = 30.0  # [m] Maximum edge lengthshow_animation = Trueclass Node:"""Node class for dijkstra search"""def __init__(self, x, y, cost, parent_index):self.x = xself.y = yself.cost = costself.parent_index = parent_indexdef __str__(self):return str(self.x) + "," + str(self.y) + "," +\str(self.cost) + "," + str(self.parent_index)def prm_planning(sx, sy, gx, gy, ox, oy, rr):obstacle_kd_tree = cKDTree(np.vstack((ox, oy)).T)sample_x, sample_y = sample_points(sx, sy, gx, gy,rr, ox, oy, obstacle_kd_tree)if show_animation:plt.plot(sample_x, sample_y, ".b")road_map = generate_road_map(sample_x, sample_y, rr, obstacle_kd_tree)rx, ry = dijkstra_planning(sx, sy, gx, gy, road_map, sample_x, sample_y)return rx, rydef is_collision(sx, sy, gx, gy, rr, obstacle_kd_tree):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])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):"""Road map generationsample_x: [m] x positions of sampled pointssample_y: [m] y positions of sampled pointsrr: Robot Radius[m]obstacle_kd_tree: KDTree object of obstacles"""road_map = []n_sample = len(sample_x)sample_kd_tree = cKDTree(np.vstack((sample_x, sample_y)).T)for (i, ix, iy) in zip(range(n_sample), sample_x, sample_y):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]]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):"""s_x: start x position [m]s_y: start y position [m]gx: goal x position [m]gy: goal y position [m]ox: x position list of Obstacles [m]oy: y position list of Obstacles [m]rr: robot radius [m]road_map: ??? [m]sample_x: ??? [m]sample_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)open_set, closed_set = dict(), dict()open_set[len(road_map) - 2] = start_nodepath_found = Truewhile True: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 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 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 sample_points(sx, sy, gx, gy, rr, ox, oy, obstacle_kd_tree):max_x = max(ox)max_y = max(oy)min_x = min(ox)min_y = min(oy)sample_x, sample_y = [], []while len(sample_x) <= N_SAMPLE:tx = (random.random() * (max_x - min_x)) + min_xty = (random.random() * (max_y - min_y)) + min_ydist, index = obstacle_kd_tree.query([tx, ty])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 main():print(__file__ + " start!!")# 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")rx, ry = prm_planning(sx, sy, gx, gy, ox, oy, robot_size)assert rx, 'Cannot found path'if show_animation:plt.plot(rx, ry, "-r")plt.pause(0.001)plt.show()if __name__ == '__main__':main()

Probabilistic Road Map相关推荐

对Probabilistic Road Map(PRM)概率路图路径规划方法的理解
PRM的总体思路基于采样的路径规划方法在原理上与基于搜索的路径方法有较大区别.基于搜索的路径方法,如A*和Dijkstra,常常用于grid地图.它们需要搜索目标点到终点间的所有栅格.基于采样的路径 ...
数据科学概论Learning Road Map
<数据科学概论>的学习路线图(Learning Road Map) 2022-03-25 Revision 本文档同时在如下网址提供: 数据科学概论学习路线图(Learning Roadm ...
shanghai road map and the operational time for 12306 system
it is challenging to know that the the 12306 will not open for booking between 23:30 and 5:00
shanghai road map
what is the requirement for the industry? what do you want to become a qualified consultant?
2019年前端road map
加油
自动驾驶汽车的规划与控制
1. 概念与意义自动驾驶汽车作为一个复杂的软硬件结合系统,其安全,可靠地运行需要车载硬件,传感器集成.感知.预测以及规划控制等多个模块的协同配合工作.感知预测和规划控制的紧密配合非常重要.这里的规划 ...
【规划】常用算法大汇总
常用规划算法一.图搜索 1. DFS&BFS.GBFS 2. Dijkstra算法和A*算法 3. A*变种 3.1 混合A*算法 3.2 LPA*算法 3.3 其它二.采样 1. 随机性 ...
2 Robotics: Computational Motion Planning 第2+3+4周课后习题解答
Computational Motion Planning 第2+3+4周 2 Robotics: Computational Motion Planning WEEK - 2 Quiz Config ...
无人驾驶汽车路径规划概述
无人驾驶汽车路径规划概述原地址:http://imgtec.eetrend.com/blog/2019/100018447.html 无人驾驶汽车路径规划是指在一定的环境模型基础上,给定无人驾驶汽车 ...
【宾夕法尼亚大学机器人课程学习】Motion Planning
目录: 一.定义二.Grassfire Algorithm 三.Dijkstra's Algorithm狄克斯特拉算法四.A_star Algorithm A*算法五.构形空间configura ...

Probabilistic Road Map

原理

参考资料

例子

Probabilistic Road Map相关推荐

最新文章

热门文章