Somebody has to win, so why not be me？ 总有一个人要赢的，为什么不是我呢？

— 科比 布莱恩特

🏰代码及环境配置：请参考 环境配置和代码运行!

基于图搜索的路径规划算法主要用于低维度空间上的路径规划问题，它在这类问题中往往具备较好的完备性。但其需要对环境进行完整建模的性质，导致在高维度空间上往往表现出维度灾难，因此基于采样的运动规划方法应运而生。

随机性采样方法通过在空间中随机生成采样点来探索路径。常见的随机性采样算法包括概率路图（PRM）和快速扩展随机树（RRT）。这些算法通过随机选择节点和连接路径，以概率完备性（当时间接近无限时一定有解）来代替完备性，从而提高了搜索效率。

PRM （概率路线图，Probabilistic Road Map）算法是一种基于随机性采样的路径规划算法。它通过在自由空间中随机生成一组节点，并使用简单的局部规划方法来连接这些节点，形成一个图。然后，通过在这个图上搜索路径，找到从起点到终点的可行路径。PRM 在处理高维度空间中的复杂路径规划问题时，表现出较高的效率和灵活性。

3.1.1 PRM算法两个步骤

1. 学习阶段（预处理阶段）：对状态空间内的安全区域均匀随机采样n个点，每个采样点分别与一定距离内的临近采样点连接，并丢弃掉与障碍物发生碰撞的轨迹，最终得到一个连通图。
2. 查询阶段：对于给定的一对初始状态和目标状态，分别将其连接到上述连通图中，运用图搜索的方法（Dijkstra，A*等）找出一条可行路径。

3.1.2 PRM算法伪代码

学习阶段伪代码：

1. Line1~Line2: 初始化两个集，其中V表示随机点集，E表示路径集。
2. Line3~Line8: 每次随机采样一个无碰撞的点，直到V中的点集个数达到n。
3. Line9~Line16: 生成概率路图。
1. Line10: 对V中的每个点q，根据一定距离选择最近的k个临域点
2. Line11~Line15: 对每个临域点q’进行判断，若q和q’之间尚无路径，则将其连接形成路径，随后进行碰撞检测，若无碰撞，留下该路径。

查询阶段：使用图搜索算法在Road Map中搜索出一条从起点到终点的最短路径（此处不在详细展开）

3.1.3 PRM算法可视化（Fig.1 ~ Fig.8）

3.1.4 PRM算法优缺点

优点

• 原理简单，容易实现。
• 概率完备性，在处理高维空间时非常灵活高效。

缺点

• 构建的Road Map覆盖了整个状态空间，对于确定的start point和goal point是比较浪费资源的。
• 参数的设置对结果影响很大，采样点的数量、领域的大小设置不合理均可能导致路径规划失败。
• 存在狭窄通路问题：空白区域采样点密集，障碍物区域采样点较少，可能无法得到最优路径。
• 最终的路径不满足动力学约束。

3.1.5 PRM算法关键点

1. 如何选择随机采样？
1. 在整个空间内（包含feasible region和infeasible region）均匀随机采样。
2. 最近的k个邻域点如何选择？

1. k-近邻搜索（KNN）：寻找距离当前节点最近的k个邻域点。

2. 半径邻域搜索（Radius Nearest Neighbor）: 找到某个半径距离范围内所有的邻居节点。

3. 可变半径PRM： 把r为半径的圆作为采样节点个数n的函数，采样点较少情况下，r可以取大一点，采样点足够多的时候，r取小一点，即PRM*算法.

   
   

4. 高效数据结构：采用KD树（kd tree）等。

3. 点与点之间的局部path如何生成？
1. 直线插值：节点之间通过直线连接
4. 局部路径的碰撞检测
1. 可以增量式取点或者二分法取点，判断点是否在障碍物区域内（如下图所示）。
2. 碰撞检测部分可参考1.2节。

3.1.c PRM代码解析

创建局部地图，并设置障碍物，使用PRM的方式进行路径规划，具体代码可参考：

python3 tests/sampling_based_planning/probabilistic_road_map.py

主函数及结果图如下：

def main(rng=None):
    # start and goal position.
    start_x = 10.0     # unit: m
    start_y = 10.0     # unit: m
    goal_x = 50.0      # unit: m
    goal_y = 50.0      # unit: m
    robot_size = 5.0   # unit: m
    
    # construct environment info.
    border_x, border_y, ox, oy = construct_env_info()
        
    # run the prm planning.
    ox.extend(border_x)
    oy.extend(border_y)
    rx, ry = prm_planning(start_x, start_y, goal_x, goal_y, ox, oy, robot_size, rng=rng)

3.1.c.1 PRM算法的核心部分

主要包含了PRM算法的两个步骤：学习阶段和查询阶段。

def prm_planning(start_x, start_y, goal_x, goal_y,
                 obstacle_x_list, obstacle_y_list, robot_radius, *, 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: (Optional) Random generator
    :return:
    """
    obstacle_kd_tree = KDTree(np.vstack((obstacle_x_list, obstacle_y_list)).T)
    
    # Uniform random sampling, and discard points that collide with obstacles.
    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)
                                       
    # Generate road map.                                   
    road_map = generate_road_map(sample_x, sample_y,
                                 robot_radius, obstacle_kd_tree)
  
  # Use the dijkstra to search a shortest path.
    rx, ry = dijkstra_planning(
        start_x, start_y, goal_x, goal_y, road_map, sample_x, sample_y)

    return rx, ry

3.1.c.2 学习阶段

主要包括均匀随机撒点和生成road map，使用KD Tree的方式来加速建图

def 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_x
        ty = (rng.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
    
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
    robot_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):
        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)
    return road_map

3.1.c.3 查询阶段

采用dijkstra的图搜方式找寻最短路径，此处不再展开。

🏎️自动驾驶小白说官网：https://www.helloxiaobai.cn