RRT 算法详细介绍（Python）

RRT（Rapidly-exploring Random Tree）快速扩展随机树是一种采样式路径规划算法，广泛应用于机器人运动规划、自动驾驶、无人机路径设计等领域。它特别适用于高维空间中的路径规划问题。下面是对 RRT 算法的详细介绍：

一、RRT 算法的核心思想

RRT 的核心思想是通过在空间中随机采样点并逐步构建一棵树形结构（搜索树），来快速探索空间并找到从起点到终点的可行路径。 RRT 偏向于快速探索未被探索的空间区域，从而快速覆盖整个搜索空间。

二、基本流程

输入：

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

步骤：

1. 初始化一棵树 T，树的根节点为起点 q_start。
2. 对于每次迭代：
   • 随机采样一个点 q_rand（可以是完全随机，也可以有一定概率采样为 q_goal，称为'目标偏向'）。
   • 在树中找到距离 q_rand 最近的节点 q_nearest。
   • 从 q_nearest 向 q_rand 移动一个固定步长 Δq，得到新的节点 q_new。
   • 如果 q_new 不在障碍物中，则将其加入树中，并将其父节点设为 q_nearest。
   • 如果 q_new 距离 q_goal 很近，可以认为找到了可行路径。
3. 如果找到路径，沿父节点回溯得到路径；否则直到达到最大迭代次数。

三、RRT 算法伪代码

def RRT(q_start, q_goal, N, Δ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, Δ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 实现示例，并配合图示说明 RRT 的执行过程。

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

 :
     ():
        .x = x
        .y = y
        .parent = 

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

 ():
     random.random() < goal_sample_rate:
         Node(goal.x, goal.y)
     Node(random.uniform(, ), random.uniform(, ))

 ():
    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
     new_node

 ():
    
     

 ():
    nodes = [start]
     _  (max_iter):
        rnd = get_random_node(goal_sample_rate, goal)
        nearest = (nodes, key= n: distance(n, rnd))
        new_node = steer(nearest, rnd)
          is_collision(new_node):
            nodes.append(new_node)
             distance(new_node, goal) < :
                goal.parent = new_node
                nodes.append(goal)
                
     nodes

 ():
    path = []
    node = last_node
     node:
        path.append((node.x, node.y))
        node = node.parent
    path = path[::-]
    plt.plot([x  x, y  path], [y  x, y  path], )

 ():
     node  nodes:
         node.parent:
            plt.plot([node.x, node.parent.x], [node.y, node.parent.y], )

start = Node(, )
goal = Node(, )
nodes = rrt(start, goal)
draw_tree(nodes)
draw_path(goal)
plt.plot(start.x, start.y, , label=)
plt.plot(goal.x, goal.y, , label=)
plt.legend()
plt.grid()
plt.axis([, , , ])
plt.title()
plt.show()