自动驾驶算法（八）：基于概率图算法的路径规划

算法（八）：基于概率图算法的路径规划"/>

自动驾驶算法（八）：基于概率图算法的路径规划

目录

1 概率路线算法简介

2 代码解析

3 路径规划算法总结

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, ry

        obstacle_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_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_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 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)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 的函数，它用于生成一个道路图，以便后续的路径规划算法可以在其中搜索有效的路径。

函数的主要功能如下：

1. 创建了一个空列表 road_map 用于存储道路图信息。

2. 计算了采样点的数量 n_sample。

3. 使用 cKDTree 创建了一个kd-tree数据结构，用于高效地查询采样点。

4. 对于每一个采样点，循环执行以下操作：

   • 使用kd-tree查询最近的采样点，并返回距离和索引。
   • 初始化一个空列表 edge_id 用于存储可以连接到当前点的其他采样点的索引。
   • 对于查询结果中的每一个点，检查是否与当前点可以连接而且没有碰撞。如果满足条件，将其索引加入到 edge_id 列表中。
   • 如果已经找到足够数量（N_KNN）的连接点，则结束循环。

5. 将 edge_id 列表添加到 road_map 中，记录了与每个采样点相连接的其他采样点的索引。

6. 如果设定了 show_roadmap 为 True，则会调用一个名为 plot_road_map 的函数来可视化生成的道路图，并在图上绘制采样点。

7. 最后，返回生成的道路图。

        最后我们用Dijkstra进行路径规划。完整代码如下：

"""Probabilistic Road Map (PRM) Plannerauthor: 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 lengthshow_animation = True
show_roadmap = 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)# 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, 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)if show_roadmap:plot_road_map(road_map, sample_x, sample_y)plt.plot(sample_x, sample_y, ".b")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]], "-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_xty = (random.random() * (max_y - min_y)) + min_ydist, 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_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 = 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算法会寻找非常久的时间。