好的，下面是关于 RRT（Rapidly-exploring Random Tree）启发式算法的详细介绍，包括其基本原理、应用场景、Python 实现思路以及可视化效果，适用于机器人路径规划、自动驾驶、无人机导航等方向。
🚀【启发式算法】RRT算法详细介绍（含Python实现）
📘 一、什么是 RRT 算法？
RRT（Rapidly-exploring Random Tree） 是一种基于随机采样的启发式路径规划算法，主要用于高维空间的路径搜索，特别适合在带障碍的复杂环境中快速找到一条可行路径。
✅ 优点：速度快、适用于高维空间
❌ 缺点：路径可能不光滑、不最优（可结合优化算法改进）
🧠 二、核心思想
RRT 的目标是快速在搜索空间中生成一棵 从起点出发、朝向目标扩展的树形结构，主要流程包括：
- 随机采样一个点
- 寻找树中与该点最近的节点（Nearest）
- 从最近节点朝向随机点前进一定距离（Step Size）并生成新节点
- 若新节点在自由空间内且不碰撞，则加入树中
- 若新节点靠近目标点，则认为路径找到（可设置容差）
🔁 三、RRT 算法流程图（简述）
Start --> Sample Random Point
↓
Find Nearest Node
↓
Steer Towards Sample
↓
Collision Check
↓
[Yes] Add Node & Edge
↓
Goal Check? -------> No --> Loop
↓
Yes
Return Path
🧪 四、Python简易实现（2D环境）
🧩 环境准备
pip install matplotlib numpy
✅ 基本结构代码
import numpy as np
import matplotlib.pyplot as plt
import random
import math
class Node:
def __init__(self, x, y):
self.x = x
self.y = y
self.parent = None
class RRT:
def __init__(self, start, goal, obstacle_list, rand_area,
expand_dis=1.0, goal_sample_rate=5, max_iter=500):
self.start = Node(start[0], start[1])
self.goal = Node(goal[0], goal[1])
self.min_rand, self.max_rand = rand_area
self.expand_dis = expand_dis
self.goal_sample_rate = goal_sample_rate
self.max_iter = max_iter
self.obstacle_list = obstacle_list
self.node_list = [self.start]
✅ 主算法流程
def planning(self):
for i in range(self.max_iter):
rnd_node = self.get_random_node()
nearest_node = self.get_nearest_node(rnd_node)
new_node = self.steer(nearest_node, rnd_node, self.expand_dis)
if not self.collision_check(new_node):
continue
self.node_list.append(new_node)
if self.distance(new_node, self.goal) <= self.expand_dis:
print("Reached Goal!")
return self.generate_final_path(new_node)
return None
✅ 关键方法实现
def steer(self, from_node, to_node, extend_length):
theta = math.atan2(to_node.y - from_node.y, to_node.x - from_node.x)
new_node = Node(from_node.x + extend_length * math.cos(theta),
from_node.y + extend_length * math.sin(theta))
new_node.parent = from_node
return new_node
def get_random_node(self):
if random.randint(0, 100) > self.goal_sample_rate:
return Node(random.uniform(self.min_rand, self.max_rand),
random.uniform(self.min_rand, self.max_rand))
return self.goal
def get_nearest_node(self, node):
return min(self.node_list, key=lambda n: self.distance(n, node))
def collision_check(self, node):
for (ox, oy, r) in self.obstacle_list:
if math.hypot(ox - node.x, oy - node.y) <= r:
return False
return True
def distance(self, node1, node2):
return math.hypot(node1.x - node2.x, node1.y - node2.y)
def generate_final_path(self, node):
path = [(node.x, node.y)]
while node.parent:
node = node.parent
path.append((node.x, node.y))
return path[::-1]
🧭 五、可视化显示路径
def draw_graph(rrt, path=None):
plt.clf()
for node in rrt.node_list:
if node.parent:
plt.plot([node.x, node.parent.x], [node.y, node.parent.y], "-g")
for (ox, oy, r) in rrt.obstacle_list:
plt.plot(ox, oy, "ok", markersize=r * 10)
if path:
px, py = zip(*path)
plt.plot(px, py, "-r", linewidth=2)
plt.plot(rrt.start.x, rrt.start.y, "bs")
plt.plot(rrt.goal.x, rrt.goal.y, "rs")
plt.axis("equal")
plt.pause(0.01)
🧪 六、完整运行示例
def main():
start = (0, 0)
goal = (10, 10)
obstacles = [(5, 5, 1.0), (3, 6, 1.0), (7, 8, 1.0)]
area = (-2, 15)
rrt = RRT(start, goal, obstacles, area)
path = rrt.planning()
if path:
draw_graph(rrt, path)
plt.show()
else:
print("未找到路径")
if __name__ == '__main__':
main()
📈 七、RRT扩展版本
| 版本 | 特点 |
|---|---|
| RRT-Connect | 双向搜索，速度更快 |
| RRT* | 加入重布线机制，生成最优路径 |
| Informed-RRT* | 使用椭圆启发，优化路径效率 |
| Anytime RRT | 在时间限制内不断优化路径 |
📌 应用场景
- 机器人导航（如移动机器人、清扫机器人）
- 无人驾驶汽车路径规避
- 游戏 AI 路径搜索
- 工业机械臂运动规划
- 无人机航迹规划
发表回复