（7-2-02）RRT算法：RRT算法的定义与实现(2)基于概率P的RRT算法

7.2.3  基于概率P的RRT算法

基于概率 P 的 RRT（Probabilistic Roadmap Planning）是一种路径规划算法，是 RRT 算法的一种变体。与传统的 RRT 算法不同，基于概率 P 的 RRT 引入了概率因素来指导树的生长，以提高路径搜索的效率。

在传统的 RRT 算法中，随机采样点的选择是均匀随机的，而在基于概率 P 的 RRT 中，采样点的选择是依据一定的概率分布进行的。这个概率分布通常会考虑到环境的特性，比如障碍物的分布情况、起点和终点的位置等。通过合理选择概率分布，可以使得采样点更有可能出现在搜索空间中有意义的区域，从而加速路径搜索的过程。

基于概率 P 的 RRT 算法仍然遵循 RRT 的基本思想，即通过不断生长树来探索搜索空间，并最终找到起点到目标点的可行路径。但是，通过引入概率因素，该算法能够更加智能地选择扩展方向，从而更快地收敛到解。这种方法通常可以提高算法的效率，并且在某些情况下能够更好地应对复杂的环境和任务需求。

基于概率 P 的 RRT算法的主要改变就是在于 Extend 函数时，增加了一个ChoseTarget 函数。这个函数不会全都使用𝑞𝑟𝑎𝑛𝑑作为扩展的方向，而是一个概率 P 来选择进行扩展，以 1-P 的概率选择𝑞𝑔𝑜𝑎𝑙来 进行扩展。这样的好处就会加快了收敛速度，不过需要选择好对应的 P 概率。

下面是一个基于概率P的RRT算法的完整例子，包括了节点结构的定义、距离计算函数、扩展函数以及主函数实现。

实例7-2：使用基于概率P的RRT算法（codes/7/prrt.py）

实例文件prrt.py的具体实现代码如下所示。

import random
import math
import matplotlib.pyplot as plt

# 定义节点结构
class Node:
    def __init__(self, x, y):
        self.x = x
        self.y = y

# 计算两个节点之间的距离
def distance(n1, n2):
    return math.sqrt((n1.x - n2.x) ** 2 + (n1.y - n2.y) ** 2)

# 根据概率P选择目标点
def chose_target(qrand, qgoal, P):
    if random.random() < P:
        return qgoal
    else:
        return qrand

# 找到距离目标点最近的节点
def nearest(V, q):
    nearest_node = V[0]
    min_dist = distance(nearest_node, q)
    for node in V:
        dist = distance(node, q)
        if dist < min_dist:
            nearest_node = node
            min_dist = dist
    return nearest_node

# 在节点qnearest和目标点q之间进行局部扩展，得到新节点
def steer(qnearest, q):
    theta = math.atan2(q.y - qnearest.y, q.x - qnearest.x)
    x = qnearest.x + math.cos(theta)
    y = qnearest.y + math.sin(theta)
    return Node(x, y)

# 判断从节点qnearest到节点qnew的路径是否避开障碍物
def obstacle_free(qnearest, qnew):
    # 在这里假设路径是始终可行的
    return True

# Extend函数，实现基于概率P的RRT算法的扩展操作
def extend(V, qrand, qgoal, P):
    q = chose_target(qrand, qgoal, P)
    qnearest = nearest(V, q)
    qnew = steer(qnearest, q)
    if obstacle_free(qnearest, qnew):
        V.append(qnew)
    return V

# 可视化函数，绘制节点集合和边集合
def visualize(V, E, q_init, q_goal):
    fig, ax = plt.subplots()

    for node in V:
        ax.plot(node.x, node.y, 'bo')

    for edge in E:
        ax.plot([edge[0].x, edge[1].x], [edge[0].y, edge[1].y], 'k-')

    ax.plot(q_init.x, q_init.y, 'go')  # 起始点
    ax.plot(q_goal.x, q_goal.y, 'ro')  # 目标点

    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_title('Probabilistic RRT Visualization')

    plt.show()

# 主函数
def main():
    random.seed()  # 初始化随机种子

    q_init = Node(0, 0)  # 起始点
    q_goal = Node(10, 10)  # 目标点
    V = [q_init]  # 存储节点的集合
    E = []  # 存储边的集合

    iterations = 50  # 迭代次数
    P = 0.9  # 概率P

    for _ in range(iterations):
        # 采样随机点
        q_rand = Node(random.randint(0, 100), random.randint(0, 100))

        # 扩展树
        q_nearest = nearest(V, q_rand)
        q_new = steer(q_nearest, q_rand)
        if obstacle_free(q_nearest, q_new):
            V.append(q_new)
            E.append((q_nearest, q_new))

    # 输出节点集合和边集合
    print("Nodes:", end=" ")
    for node in V:
        print(f"({node.x}, {node.y})", end=" ")
    print()

    # 可视化
    visualize(V, E, q_init, q_goal)

if __name__ == "__main__":
    main()

上述代码的实现流程如下所示：

（1）首先，定义了节点结构 Node，包含节点的 x 和 y 坐标，并实现了一些基本的几何计算函数，如计算两个节点之间的距离、选择目标点、找到距离目标点最近的节点等。

（2）然后，在主函数main中初始化了起始点和目标点，并创建了节点集合 V 和边集合 E。通过随机选择节点进行迭代，每次迭代根据概率 P 选择目标点，在当前树中找到距离目标点最近的节点，然后在这个最近节点和目标点之间进行局部扩展，生成新的节点。如果新节点的路径不与障碍物相交，就将新节点添加到节点集合中，并将扩展过程中的边添加到边集合中。

（3）接着，利用 matplotlib 库实现了可视化功能，将节点集合和边集合绘制在二维坐标系中，并标记了起始点和目标点。这样可以直观地观察到基于概率 P 的 RRT 算法在搜索空间中的探索过程和生成的路径。

（1）最后，打印输出了生成的节点集合，并绘制了路径信息的可视化图，如图7-3所示。这样，可以通过输出结果和可视化图形了解算法的执行情况和生成的路径信息。

图7-3  路径信息的可视化图