【启发式算法】RRT算法详细介绍（Python）

【Linux我做主 · 启发式算法专栏】RRT算法详细介绍（Python完整实现）——从原理到可视化，一文吃透路径规划神器

大家好，我是重阳。上期我们刚入门 Python，今天直接进入启发式算法硬核——RRT（Rapidly-exploring Random Tree，快速探索随机树）！

RRT 是机器人路径规划领域里程碑式算法（2000 年 Lavalle 提出），专治高维空间 + 复杂障碍物场景。相比 A*、Dijkstra，它不依赖网格，采样式随机扩展，实时性极强。2026 年仍广泛用于自动驾驶、无人机、机械臂、游戏 AI。

本文从原理讲到Python 完整代码 + 可视化，零基础也能 30 分钟跑通第一个 RRT 路径！

1. RRT 到底是什么？（核心原理图）

RRT 构建一棵随机树，从起点开始，不断向随机采样点扩展，直到到达目标点。

经典 RRT 扩展过程图（一步步看懂）：

直观理解：

• 每次随机采样一个点 q_rand
• 找到树上最近的节点 q_nearest
• 向 q_rand 方向扩展固定步长 step_size 得到新节点 q_new
• 若 q_new 无碰撞，则加入树
• 重复直到离目标足够近

RRT vs RRT* 对比图（RRT* 会重连优化路径，更优）：

2. RRT 算法完整流程（伪代码 + 图）

伪代码（最清晰版）：

Initialize Tree T with start node
while not reach goal:
    q_rand = random_sample()          # 均匀随机采样（或目标偏置）
    q_nearest = nearest(T, q_rand)    # 欧氏距离最近节点
    q_new = steer(q_nearest, q_rand, step_size)  # 扩展步长
    if collision_free(q_nearest, q_new):
        add q_new to T
        if distance(q_new, goal) < threshold:
            return path
return "No path found"

流程图（改进版 RRT 也类似）：

关键参数：

• step_size：扩展步长（太小慢，太大易卡）
• max_iter：最大迭代次数
• goal_bias：目标偏置概率（10%-30% 更快收敛）

3. RRT 优缺点（生产必知）

优点：

• 高维友好（6DOF 机械臂、无人机 3D 空间轻松搞定）
• 概率完备（足够迭代几乎一定找到路径）
• 实时性强（无需预处理地图）

缺点：

• 路径不最优（锯齿状）
• 收敛慢（基础 RRT）
• 随机性导致每次结果不同

改进版：

• RRT*：重连 + 路径优化（最常用）
• Bidirectional RRT：双树（起点+终点同时扩展，超快）
• Informed RRT*：椭圆采样（更快）

4. Python 完整实现 + 可视化（直接复制运行！）

依赖：pip install numpy matplotlib

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle

class RRT:
    def __init__(self, start, goal, map_size=(20, 20), step_size=0.5, max_iter=1000):
        self.start = np.array(start)
        self.goal = np.array(goal)
        self.map_size = map_size
        self.step_size = step_size
        self.max_iter = max_iter
        self.tree = [self.start]          # 节点列表
        self.parent = {0: -1}             # 父节点索引
        self.obstacles = [                # 简单矩形障碍
            (5, 5, 4, 4), (12, 8, 3, 5)
        ]

    def nearest(self, point):
        distances = [np.linalg.norm(p - point) for p in self.tree]
        return np.argmin(distances)

    def steer(self, nearest_idx, rand_point):
        nearest = self.tree[nearest_idx]
        direction = rand_point - nearest
        dist = np.linalg.norm(direction)
        if dist < self.step_size:
            return rand_point
        return nearest + (direction / dist) * self.step_size

    def collision_free(self, p1, p2):
        for obs in self.obstacles:
            ox, oy, ow, oh = obs
            # 简单线段-矩形碰撞检测
            if (min(p1[0], p2[0]) < ox + ow and max(p1[0], p2[0]) > ox and
                min(p1[1], p2[1]) < oy + oh and max(p1[1], p2[1]) > oy):
                return False
        return True

    def plan(self):
        for i in range(self.max_iter):
            # 目标偏置采样（30%概率采样目标）
            if np.random.rand() < 0.3:
                rand_point = self.goal
            else:
                rand_point = np.random.uniform(0, self.map_size[0], 2)

            nearest_idx = self.nearest(rand_point)
            new_point = self.steer(nearest_idx, rand_point)

            if self.collision_free(self.tree[nearest_idx], new_point):
                self.tree.append(new_point)
                self.parent[len(self.tree)-1] = nearest_idx

                if np.linalg.norm(new_point - self.goal) < 0.5:
                    # 重建路径
                    path = []
                    idx = len(self.tree) - 1
                    while idx != -1:
                        path.append(self.tree[idx])
                        idx = self.parent[idx]
                    return path[::-1]
        return None

# ==================== 可视化 ====================
def visualize(rrt, path):
    plt.figure(figsize=(8, 8))
    plt.xlim(0, rrt.map_size[0])
    plt.ylim(0, rrt.map_size[1])
    plt.grid(True)

    # 障碍物
    for obs in rrt.obstacles:
        plt.gca().add_patch(Rectangle((obs[0], obs[1]), obs[2], obs[3], color='red', alpha=0.7))

    # 树
    for i in range(1, len(rrt.tree)):
        parent = rrt.tree[rrt.parent[i]]
        plt.plot([parent[0], rrt.tree[i][0]], [parent[1], rrt.tree[i][1]], 'b-', alpha=0.5)

    # 路径
    if path:
        path = np.array(path)
        plt.plot(path[:, 0], path[:, 1], 'g-', linewidth=3, label='Found Path')

    plt.plot(rrt.start[0], rrt.start[1], 'go', markersize=10, label='Start')
    plt.plot(rrt.goal[0], rrt.goal[1], 'ro', markersize=10, label='Goal')
    plt.legend()
    plt.title('RRT Path Planning (Python)')
    plt.show()

# ==================== 运行 ====================
if __name__ == "__main__":
    rrt = RRT(start=(1, 1), goal=(18, 18), step_size=0.8, max_iter=2000)
    path = rrt.plan()
    visualize(rrt, path)

运行效果（典型可视化）：

真实运行：路径会从绿色起点随机生长，最终连通红色终点！

5. RRT 进阶与生产建议

• RRT*：加 rewire 重连优化（路径更短）
• 双向 RRT：起点+终点同时建树（速度翻倍）
• 实际项目：ROS2 + MoveIt2、PX4 无人机、Autoware 自动驾驶都内置 RRT 变种

Linux 运行建议：

python rrt_demo.py

6. 一句话总结 + 行动建议

RRT 本质：随机采样 + 局部扩展，用最简单方式解决最复杂的路径规划问题。Python 实现只需 100 行，效果却惊人！

今天就行动：

1. 复制上面代码保存 rrt.py
2. python rrt.py 看可视化
3. 修改障碍物、起点终点，玩出自己的地图！

想看 下一期？
评论区打 1（RRT* 完整代码 + 优化）、2（Bidirectional RRT 双树版）、3（ROS2 + RRT 机器人实战）、4（3D RRT 无人机路径规划），我立刻出！🚀

推荐资源：

• 原论文：Rapidly-Exploring Random Trees (Lavalle 1998)
• 中文教程：B站 “RRT 路径规划” 系列
• GitHub：搜索 “python-rrt” 更多开源实现

启发式算法我做主，从今天起，你也能写机器人路径规划了！🤖

Linux + 算法，咱们持续硬核，下期见！💻