目录
1 概率路线算法简介
它属于采样算法里面的一类。主要步骤分为两步：
1.构建概率路线图
(1)随机采样点
(2)将新采样点和距离小于阈值的 采样点连接产生图2.在图上寻找路径
(1)Dijkstra算法
(2)A*算法刚开始有起点、终点两个点作为初始化的采样点：
我们开始随机采样：
进行连接。再采样再连接：
如果采样点有障碍物，我选择不连接：
我们继续采样，我们简单的举例：
这样我们就建立了一个图了，我们再通过Dijkstra算法或者A*算法来做路径规划。
缺点是：这个是两阶段算法，速度较慢且没法寻找最优路径。
可以用作局部路径规划：
2 代码解析
先来看一下运行结果：
我们首先还是定义了起点坐标终点坐标以及障碍物的坐标。
规划的主函数如下：
# start xy goal xy 障碍物xy 机器人大小 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, ryobstacle_kd_tree = cKDTree(np.vstack((ox, oy)).T)：这行代码是用于构建一个k维树（kd-tree）的数据结构，以便于在二维空间中高效地查找最近邻的障碍物。具体来说，它使用了一个名为
cKDTree的函数来构建kd-tree。
np.vstack((ox, oy)).T:
ox和oy是一些障碍物的x和y坐标的数组。np.vstack用于垂直堆叠（将数组按垂直方向拼接在一起），((ox, oy)).T表示将这个堆叠后的矩阵进行转置，以保证每一行代表一个点，每列代表一个维度（在这里是x和y坐标）。
cKDTree():
- 这是一个用于构建kd-tree的函数，它接受一个二维数组作为输入，这个数组中的每一行代表一个点，每列代表一个维度。
假设你有以下障碍物的坐标：
ox = [1, 2, 3, 4, 5] oy = [1, 2, 3, 4, 5]那么，通过执行以下代码：
obstacle_kd_tree = cKDTree(np.vstack((ox, oy)).T)将会得到一个名为
obstacle_kd_tree的kd-tree数据结构，该结构会根据障碍物的坐标构建一个高效的查找树。你可以使用这个kd-tree来进行最近邻的搜索，例如查找离一个给定点最近的障碍物。比如，如果你想找到离点
(3, 3)最近的障碍物，你可以使用以下代码：nearest_obstacle_index = obstacle_kd_tree.query([3, 3])[1] nearest_obstacle = (ox[nearest_obstacle_index], oy[nearest_obstacle_index]) print(nearest_obstacle)这将会输出离点
(3, 3)最近的障碍物的坐标，也就是(3, 3)。我们再来看随机取样函数：
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_x ty = (random.random() * (max_y - min_y)) + min_y dist, 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_y首先计算了障碍物坐标的最大最小值，以确定随机采样点的范围。创建了两个空列表sample_x和sample_y，用于存储采样到的点的x和y坐标。使用一个循环来生成随机点，直到采样到足够数量的点（N_SAMPLE）为止。每次循环中，随机生成一个点(tx, ty)，并使用kd-tree查询该点附近的最近障碍物距离和索引。
random.random()返回一个大于等于0且小于1的随机浮点数。在上述代码片段中，random.random()被用于生成一个随机的横坐标tx和纵坐标ty，以便在地图上随机采样点。这样可以在地图范围内均匀地选取随机点，以便进行路径规划。road_map = generate_road_map(sample_x, sample_y, rr, obstacle_kd_tree)def generate_road_map(sample_x, sample_y, rr, obstacle_kd_tree): """ Road map generation sample_x: [m] x positions of sampled points sample_y: [m] y positions of sampled points rr: 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: break road_map.append(edge_id) if show_roadmap: plot_road_map(road_map, sample_x, sample_y) plt.plot(sample_x, sample_y, ".b") return road_map这段代码定义了一个名为
generate_road_map的函数，它用于生成一个道路图，以便后续的路径规划算法可以在其中搜索有效的路径。函数的主要功能如下：
创建了一个空列表
road_map用于存储道路图信息。计算了采样点的数量
n_sample。使用
cKDTree创建了一个kd-tree数据结构，用于高效地查询采样点。对于每一个采样点，循环执行以下操作：
- 使用kd-tree查询最近的采样点，并返回距离和索引。
- 初始化一个空列表
edge_id用于存储可以连接到当前点的其他采样点的索引。- 对于查询结果中的每一个点，检查是否与当前点可以连接而且没有碰撞。如果满足条件，将其索引加入到
edge_id列表中。- 如果已经找到足够数量（
N_KNN）的连接点，则结束循环。将
edge_id列表添加到road_map中，记录了与每个采样点相连接的其他采样点的索引。如果设定了
show_roadmap为True，则会调用一个名为plot_road_map的函数来可视化生成的道路图，并在图上绘制采样点。最后，返回生成的道路图。
最后我们用Dijkstra进行路径规划。完整代码如下：
""" 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 = 100 # number of sample_points N_KNN = 10 # number of edge from one sampled point MAX_EDGE_LEN = 30.0 # [m] Maximum edge length show_animation = True show_roadmap = True class Node: """ Node class for dijkstra search """ def __init__(self, x, y, cost, parent_index): self.x = x self.y = y self.cost = cost self.parent_index = parent_index def __str__(self): return str(self.x) + "," + str(self.y) + "," +\ str(self.cost) + "," + str(self.parent_index) # start xy goal xy 障碍物xy 机器人大小 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, ry def is_collision(sx, sy, gx, gy, rr, obstacle_kd_tree): x = sx y = sy dx = gx - sx dy = gy - sy yaw = math.atan2(gy - sy, gx - sx) d = math.hypot(dx, dy) if d >= 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 def generate_road_map(sample_x, sample_y, rr, obstacle_kd_tree): """ Road map generation sample_x: [m] x positions of sampled points sample_y: [m] y positions of sampled points rr: 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: break road_map.append(edge_id) if show_roadmap: plot_road_map(road_map, sample_x, sample_y) plt.plot(sample_x, sample_y, ".b") return road_map def 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_node path_found = True while True: if not open_set: print("Cannot find path") path_found = False break c_id = min(open_set, key=lambda o: open_set[o].cost) current = open_set[c_id] # show graph if 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_index goal_node.cost = current.cost break # Remove the item from the open set del open_set[c_id] # Add it to the closed set closed_set[c_id] = current # expand search grid based on motion model for i in range(len(road_map[c_id])): n_id = road_map[c_id][i] dx = sample_x[n_id] - current.x dy = sample_y[n_id] - current.y d = 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 set if n_id in open_set: if open_set[n_id].cost > node.cost: open_set[n_id].cost = node.cost open_set[n_id].parent_index = c_id else: open_set[n_id] = node if path_found is False: return [], [] # generate final course rx, ry = [goal_node.x], [goal_node.y] parent_index = goal_node.parent_index while parent_index != -1: n = closed_set[parent_index] rx.append(n.x) ry.append(n.y) parent_index = n.parent_index return rx, ry def plot_road_map(road_map, sample_x, sample_y): # pragma: no cover for 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]], "-c") 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，用于存储采样到的点的x和y坐标。 sample_x, sample_y = [], [] # 使用一个循环来生成随机点，直到采样到足够数量的点（N_SAMPLE）为止。每次循环中，随机生成一个点(tx, ty)，并使用kd-tree查询该点附近的最近障碍物距离和索引。 while len(sample_x) < N_SAMPLE: tx = (random.random() * (max_x - min_x)) + min_x ty = (random.random() * (max_y - min_y)) + min_y dist, index = obstacle_kd_tree.query([tx, ty]) # 如果该点与最近障碍物的距离大于等于rr（即避免采样到障碍物附近），则将该点加入采样列表。 if dist >= rr: sample_x.append(tx) sample_y.append(ty) # 最后，将起始点和目标点也加入到采样列表中，并返回采样到的点的x和y坐标列表。 sample_x.append(sx) sample_y.append(sy) sample_x.append(gx) sample_y.append(gy) return sample_x, sample_y def main(): print(__file__ + " start!!") # start and goal position sx = 10.0 # [m] sy = 10.0 # [m] gx = 50.0 # [m] gy = 50.0 # [m] robot_size = 1.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()
3 路径规划算法总结
完备性：是指如果在起始点和目标点间有路径解存在，那么一定可以得到解，如果得不到解那么一定说明没有解存在。
概率完备性:是指如果在起始点和目标点间有路径解存在，只要规划或搜索的时间足够长，就一定能确保找到一条路径解。
最优性：是指规划得到的路径在某个评价指标上是最优的(评价指标一般为路径的长度)。
渐进最优性：是指经过有限次规划迭代后得到的路径是接近最优的次优路径，且每次迭代后都与最优路径更加接近，是一个逐渐收敛的过程。
规划速度：RRT系列 > A*算法 > Dijkstra算法 > 基于智能算法的路径规划算法
但如果是狭长路径
RRT算法会寻找非常久的时间。
2万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
