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 进行二维空间的路径规划与可视化。