RRT快速扩展随机树算法详解与Python实现

核心思想

RRT（Rapidly-exploring Random Tree，快速扩展随机树）是一种基于采样的路径规划算法，广泛应用于机器人运动规划、自动驾驶及无人机路径设计等领域，尤其适用于高维空间的路径规划问题。

其核心思想是通过在空间中随机采样点，并逐步构建一棵树形结构（搜索树），以快速探索未知区域并寻找从起点到终点的可行路径。该算法具有强烈的空间探索偏向性，能够高效覆盖整个搜索空间。

基本流程

输入参数

• 起点 q_start
• 终点 q_goal
• 空间约束（如障碍物、边界等）
• 扩展步长 Δq
• 最大迭代次数 N

执行步骤

1. 初始化搜索树 T，将根节点设为起点 q_start。
2. 循环迭代（最多 N 次）：
   • 随机采样：在空间中随机生成一个点 q_rand（可设置一定概率直接采样为 q_goal，即'目标偏向'策略）。
   • 寻找最近节点：在树 T 中查找距离 q_rand 最近的节点 q_nearest。
   • 扩展节点：从 q_nearest 向 q_rand 方向延伸固定步长 Δq，生成新节点 q_new。
   • 碰撞检测：若 q_new 及其与 q_nearest 的连线不与障碍物相交，则将 q_new 加入树 T，并设置其父节点为 q_nearest。
   • 目标判定：若 q_new 与 q_goal 的距离小于设定阈值，则认为已找到可行路径，终止迭代。
3. 路径回溯：若成功到达目标区域，则从终点沿父节点指针回溯至起点，得到完整路径；若达到最大迭代次数仍未找到，则返回失败。

算法伪代码

def RRT(q_start, q_goal, N, delta_q):
    T = Tree(q_start)
    for i in range(N):
        q_rand = random_sample()
        q_nearest = nearest_node(T, q_rand)
        q_new = steer(q_nearest, q_rand, delta_q)
        if is_valid(q_nearest, q_new):
            T.add_node(q_new, parent=q_nearest)
            if distance(q_new, q_goal) < threshold:
                return extract_path(T, q_new)
    return failure

Python 实现示例

以下为简化版的 Python 实现，使用 numpy 和 matplotlib 进行二维空间的路径规划与可视化。

import numpy as np
import matplotlib.pyplot as plt
import random

class Node:
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.parent = None

def distance(n1, n2):
    return np.hypot(n1.x - n2.x, n1.y - n2.y)

def get_random_node(goal_sample_rate, goal):
    if random.random() < goal_sample_rate:
        return Node(goal.x, goal.y)
    return Node(random.uniform(0, 100), random.uniform(0, 100))

def steer(from_node, to_node, extend_length=5.0):
    dist = distance(from_node, to_node)
    theta = np.arctan2(to_node.y - from_node.y, to_node.x - from_node.x)
    new_x = from_node.x + extend_length * np.cos(theta)
    new_y = from_node.y + extend_length * np.sin(theta)
    new_node = Node(new_x, new_y)
    new_node.parent = from_node
    return new_node

def is_collision(node):
    # 简化处理：假设当前环境无障碍物
    return False

def rrt(start, goal, max_iter=500, goal_sample_rate=0.05):
    nodes = [start]
    for _ in range(max_iter):
        rnd = get_random_node(goal_sample_rate, goal)
        nearest = min(nodes, key=lambda n: distance(n, rnd))
        new_node = steer(nearest, rnd)
        if not is_collision(new_node):
            nodes.append(new_node)
            if distance(new_node, goal) < 5.0:
                goal.parent = new_node
                nodes.append(goal)
                break
    return nodes

def draw_path(last_node):
    path = []
    node = last_node
    while node:
        path.append((node.x, node.y))
        node = node.parent
    path = path[::-1]
    plt.plot([x for x, y in path], [y for x, y in path], '-r', label='Path')

def draw_tree(nodes):
    for node in nodes:
        if node.parent:
            plt.plot([node.x, node.parent.x], [node.y, node.parent.y], '-g', alpha=0.5)

if __name__ == '__main__':
    start = Node(10, 10)
    goal = Node(90, 90)
    nodes = rrt(start, goal)
    
    draw_tree(nodes)
    draw_path(goal)
    plt.plot(start.x, start.y, "bs", label="Start")
    plt.plot(goal.x, goal.y, "gs", label="Goal")
    plt.legend()
    plt.grid(True)
    plt.axis([0, 100, 0, 100])
    plt.title("RRT Path Planning (No Obstacles)")
    plt.show()

运行结果说明

运行上述代码后，将生成可视化窗口：

• 🌲 绿色线条：表示 RRT 算法在探索过程中生成的随机搜索树结构。
• 🔴 红色路径：表示最终从起点回溯至终点的规划路径。
• 🔵/🟢 方块标记：分别代表起点与终点。

注意事项

• 算法每一步均从树中距离随机点最近的节点出发进行定向扩展。
• 实际应用中需在 is_collision() 函数中补充具体的障碍物检测逻辑（如几何碰撞检测或栅格地图查询）。
• 随机采样特性导致每次运行的树形结构与路径可能不同，可通过调整 goal_sample_rate 和 extend_length 平衡探索效率与路径质量。

算法特点

优点

• 非常适合高维空间的路径规划，无需对环境进行显式建模。
• 算法逻辑清晰，易于实现与扩展。
• 对复杂、非凸环境具有良好的适应能力。

缺点

• 生成的路径通常非最优，常呈现'锯齿状'，需后续进行路径平滑处理。
• 随机性强，规划时间存在波动，难以保证实时性。
• 在狭窄通道或障碍物密集区域，采样效率较低，容易陷入局部探索。

改进版本：RRT*

RRT*（RRT Star）是 RRT 的渐进最优改进版本，主要引入了**重布线（Rewiring）**机制：

• 在每次加入新节点时，不仅连接最近邻节点，还会搜索其邻域范围内的其他节点，尝试通过新节点建立更短的连接路径。
• 随着迭代次数增加，理论上可收敛至全局最优路径（渐近最优性）。

应用场景

• 机器人自主导航与避障
• 无人机三维空间路径规划
• 自动驾驶车辆轨迹生成
• 多自由度机械臂运动规划