1. 提升在复杂环境中使用RRT算法族解决路径规划问题的实践能力。

9.1 RRT算法介绍

快速扩展随机树(Rapidly-exploring Random Trees，RRT)是一种用于路径规划的算法，特别适用于机器人、自动驾驶车辆和其他自主系统的运动规划问题。该算法通过在自由空间中随机采样并迭代地构建一棵树来寻找从起点到目标点的可行路径。

RRT是近十几年得到广泛发展与应用的基于采样的运动规划算法，它由美国爱荷华州立大学的 Steven M. LaValle 教授在1998 年提出。RRT 算法是一种在多维空间中有效率的规划方法。原始的 RRT 算法是通过一个初始点作为根节点，通过随机采样，增加叶子节点的方式，生成一个随机扩展树，当随机树中的叶子节点包含了目标点或进入了目标区域，便可以在随机树中找到一条由树节点组成的从初始点到目标点的路径。

RRT 算法的主要优点之一是它的随机性和快速性。通过随机采样并迭代地扩展树，RRT 能够有效地探索大型自由空间，并在有限时间内找到可行路径。此外，RRT 算法对于高维空间中的运动规划问题也很有效，因为它不需要显式地构建整个空间的表示，而是在需要时根据采样点来动态扩展搜索树。

注意：RRT算法也有一些局限性，例如在某些情况下可能会找到次优路径，以及对于高度动态的环境或需要考虑更多因素的问题可能不够灵活。因此，有时候需要结合其他算法或改进来解决特定的路径规划问题。

9.2 RRT算法的定义与实现

RRT算法通过随机采样和树结构迭代地探索搜索空间，直到找到连接起点和目标点的路径。算法不断生成新节点，并将其连接到最近邻节点，直到达到最大迭代次数或找到路径。最终，从树中提取路径作为解。

9.2.1 RRT算法的实现步骤

实现RRT算法的基本步骤如下所示。

（1）初始化：将起点添加到搜索树中作为唯一节点。

（2）循环：在搜索树中不断迭代，直到找到目标点或达到最大迭代次数。

1. 采样：在自由空间中随机采样一个点作为新的探索点。
2. 最近邻搜索：在搜索树中找到与新采样点最近的节点。
3. 扩展：从最近邻节点向采样点之间的直线路径上进行一步扩展，并检查是否碰撞了障碍物。
4. 连接：如果扩展路径是无碰撞的，则将新的节点添加到搜索树中，并将连接它的边添加到树中。
5. 目标检测：检查新添加的节点是否足够接近目标点，如果是，则算法终止。

（3）路径提取：当算法终止时，从搜索树中提取从起点到目标点的路径。

9.2.2 原始的RRT算法

在介绍 RRT 算法之前，先说明一下路径的表示方法。我们用一个有向图来表示路径G=(V,E)

，那么一条可行的路径就是一个顶点的序列(v1​,v2​,v3​,…,vn​)

，v1=qinit

，vn=qgoal

。同时，为了使该序列构成一条有效路径，要求每一对相邻顶点(𝑣𝑖 , 𝑣𝑖+1) 都是图中的一条有向边，即满足(vi,vi+1)∈E

，其中1≤i≤n-1

表示边。这样，路径规划问题就转化为：如何利用采样得到的节点不断扩展图 G，以找到一条从初始节点通往目标节点的路径。

“原始的 RRT 算法”指的是最初提出的、最基本版本的 RRT（Rapidly-exploring Random Tree）算法。这个版本是由 Steven M. LaValle 在他的论文 "Rapidly-exploring Random Trees: A New Tool for Path Planning" 中提出的。原始的 RRT 算法是用于路径规划的一种基本方法，其核心思想是通过随机采样和树结构迭代地探索搜索空间，直到找到连接起点和目标点的路径。在算法的每次迭代中，随机生成一个点并通过扩展树来探索新的空间区域，以此逐步构建一棵树。通过这种方式，RRT 算法能够在自由空间中快速生成路径，并且在有限时间内收敛到一条可行路径。

例如下面的例子实现了基本的RRT算法，在这个例子中，主要的RRT算法实现在 main 函数的for 循环中。在每次迭代中，都会随机采样一个点 q_rand，然后找到树中最接近该点的节点 q_nearest，并使用 extend 函数将该随机点扩展到树中。最后，将扩展后的节点加入树中。

实例9-1：使用原始的RRT算法寻找路径（codes/9/yuan.py）

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

import math import random 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) # 扩展函数 def extend(nearest, rand, max_distance): d = distance(nearest, rand) if d <= max_distance: return rand else: theta = math.atan2(rand.y - nearest.y, rand.x - nearest.x) x = nearest.x + max_distance * math.cos(theta) y = nearest.y + max_distance * math.sin(theta) return Node(x, y) # 主函数 def main(): q_init = Node(0, 0) # 起始点 q_goal = Node(10, 10) # 目标点 V = [q_init] # 存储节点的集合 iterations = 100 # 迭代次数 max_distance = 1.0 # 最大扩展距离 for i in range(iterations): # 采样随机点 q_rand = Node(random.uniform(0, 100), random.uniform(0, 100)) # 找到距离随机点最近的节点 q_nearest = V[0] for node in V: if distance(node, q_rand) < distance(q_nearest, q_rand): q_nearest = node # 扩展树 q_new = extend(q_nearest, q_rand, max_distance) V.append(q_new) # 输出节点集合 print("Nodes:") for node in V: print("(", node.x, ",", node.y, ")", end=" ") print() # 绘制节点和路径 plt.figure(figsize=(8, 8)) plt.plot([node.x for node in V], [node.y for node in V], 'bo', markersize=3) # 绘制节点 plt.plot([q_init.x, q_goal.x], [q_init.y, q_goal.y], 'r-') # 绘制起点和终点之间的路径 plt.plot(q_init.x, q_init.y, 'go', label='Start') # 绘制起点 plt.plot(q_goal.x, q_goal.y, 'ro', label='Goal') # 绘制终点 plt.xlabel('X') plt.ylabel('Y') plt.title('RRT Algorithm Visualization') plt.legend() plt.grid(True) plt.show() if __name__ == "__main__": main()

上述代码实现了基本的RRT（Rapidly-exploring Random Tree）算法，具体实现流程如下所示。

（1）首先，定义类Node，用于表示二维空间中的节点，每个节点有 x 和 y 坐标。

（2）然后，定义了一个计算两个节点之间距离的函数 distance，以及一个扩展节点的函数 extend。distance 函数使用欧几里得距离公式计算两个节点之间的距离，extend 函数用于在当前最近节点和随机生成的节点之间进行扩展，保证扩展距离不超过最大距离。

（3）接着，在主函数main中定义了起始节点 q_init 和目标节点 q_goal，并创建了一个存储节点的集合 V，将起始节点加入集合。

（4）在迭代过程中，程序将执行如下所示的操作：

1. 采样随机点：生成一个随机的二维坐标点作为新的随机点 q_rand。
2. 找到距离随机点最近的节点：遍历节点集合 V，找到与随机点 q_rand 最近的节点 q_nearest。
3. 扩展树：使用 extend 函数将最近节点 q_nearest 和随机点 q_rand 之间的距离进行扩展，生成一个新的节点 q_new，并将其加入节点集合 V 中。
4. 以上过程会持续进行指定的迭代次数。

（5）最后，将生成的节点和起点到终点之间的路径进行可视化展示，如图9-1所示。使用 matplotlib 库绘制了生成的节点和路径，节点用蓝色圆圈表示，起点用绿色圆圈表示，终点用红色圆圈表示，路径用红色线段表示。展示了算法在迭代过程中生成的节点分布情况以及起点到终点的路径。

图9-1 节点和路径的可视化

9.2.3 基于概率P的RRT算法

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

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

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

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

实例9-2：使用基于概率P的RRT算法（codes/9/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）最后，打印输出了生成的节点集合，并绘制了路径信息的可视化图，如图9-2所示。这样，可以通过输出结果和可视化图形了解算法的执行情况和生成的路径信息。

图9-2 路径信息的可视化图