核心思想
RRT(Rapidly-exploring Random Tree,快速扩展随机树)是一种基于采样的路径规划算法,广泛应用于机器人运动规划、自动驾驶及无人机路径设计等领域,尤其适用于高维空间的路径规划问题。
其核心思想是通过在空间中随机采样点,并逐步构建一棵树形结构(搜索树),以快速探索未知区域并寻找从起点到终点的可行路径。该算法具有强烈的空间探索偏向性,能够高效覆盖整个搜索空间。
基本流程
输入参数
- 起点
q_start - 终点
q_goal - 空间约束(如障碍物、边界等)
- 扩展步长
Δq - 最大迭代次数
N
执行步骤
- 初始化搜索树
T,将根节点设为起点q_start。 - 循环迭代(最多
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的距离小于设定阈值,则认为已找到可行路径,终止迭代。
- 随机采样:在空间中随机生成一个点
- 路径回溯:若成功到达目标区域,则从终点沿父节点指针回溯至起点,得到完整路径;若达到最大迭代次数仍未找到,则返回失败。
算法伪代码
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 进行二维空间的路径规划与可视化。


