启发式算法里RRT的详尽剖析及Python实践

深度解析启发式算法中的RRT及Python实现

文章引言

此篇文章是博主在研习人工智能领域时，用于自身学习、研讨或品鉴而记录的学习笔记，依据博主对相关领域的理解予以撰写。文章归类于👉启发式算法专栏：【启发式算法】（8）---《RRT算法详细介绍（Python）》

一、RRT算法的核心思想

RRT的核心要义是借助在空间里随机抽取采样点，逐步构建起树形结构的搜索树，以此高效探寻空间并找寻从起始点到终点的可行路径。RRT侧重于迅速探索尚未涉足的空间区域，进而全面覆盖整个搜索空间。

二、基本流程

输入内容：

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

具体步骤：

1. 初始化一棵树 T，树的根节点为起点 q_start。
2. 每一次迭代操作：
3. 随机获取一个采样点 q_rand（可能完全随机，也可能以一定概率采样为 q_goal，此为“目标偏向”）。
4. 在树中找出离 q_rand 最近的节点 q_nearest。
5. 从 q_nearest 朝着 q_rand 移动固定步长 Δq，得到新节点 q_new。
6. 若 q_new 不在障碍物区域内，将其加入树中，并把其父节点设为 q_nearest。
7. 若 q_new 离 q_goal 很近，可认为已找到可行路径。
8. 若找到路径，沿父节点回溯获取路径；否则直到达到最大迭代次数。

三、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] RRT算法实现

下面提供一个简化版的Python实现示例，并配合图示说明RRT的执行过程。

"""《RRT算法》
    时间：2025.06.16
    作者：不去幼儿园
"""
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')

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

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算法，可在以下github库中查看：RRT算法

[Results] 运行结果

图示说明：

运行上述代码后会呈现出如下效果：绿色的线条代表RRT生成的搜索树结构，红色路径是最终从起点到终点规划出的路径，蓝色标记起点，绿色标记终点。

[Notice] 注意事项

• 每一步都是从树中最近的节点朝着随机点延伸。
• 最终形成一条连接起点到终点的路径。
• 可在is_collision()中添加障碍物检测逻辑来模拟真实环境。

# 环境配置
Python                  3.11.5
torch                   2.1.0
torchvision             0.16.0
gym                     0.26.2

四、RRT的特点

优点：

• 非常适合高维空间的路径规划。
• 易于实现。
• 对复杂环境有较好的适应能力。

缺点：

• 路径并非最优，常呈“锯齿状”。
• 随机性较强，规划时间不稳定。
• 在障碍物密集区域效果欠佳。

五、改进版本：RRT*

RRT*（RRT Star）是RRT的优化版本，引入了“路径优化”机制：每次加入新节点时，不仅连接最近点，还会尝试重新连接周围节点以获取更短路径，理论上可得到渐近最优解。

六、应用场景

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

更多启发式算法文章，可前往：【启发式算法】专栏

博客系博主自用笔记，若有误导深表歉意。文章若有不当之处，还望理解与指正。因部分文字、图片等源于互联网，无法核实真实出处，若涉及相关争议，请联系博主删除。如有错误、疑问及侵权，欢迎评论留言联系作者，或添加VX：Rainbook_2 联系作者。✨