好的，我们来详细介绍一下快速扩展随机树算法（Rapidly-exploring Random Tree, RRT），这是一种在机器人路径规划中广泛使用的单查询算法，特别擅长在高维空间或存在障碍物的复杂环境中寻找可行路径。它属于基于采样的规划算法。
1. RRT算法核心思想
RRT的核心思想是通过随机采样的方式在构型空间（Configuration Space）中逐步构建一棵搜索树（Tree）。这棵树从起点开始生长，不断尝试向空间中随机选取的点扩展，直到树的某个节点足够接近目标点。
2. 算法基本步骤
以下是RRT算法的基本流程：
初始化：
- 将起点 $q_{start}$ 作为树的根节点。
- 初始化一个空的节点集合 $V$（存储树的所有节点）和一个空的边集合 $E$（存储节点间的连接关系）。
- 将 $q_{start}$ 加入 $V$。
迭代扩展：
- 随机采样：在构型空间 $C$ 中随机采样一个点 $q_{rand}$。
- 寻找最近邻点：在当前树 $T = (V, E)$ 的所有节点中，找到距离 $q_{rand}$ 最近的节点 $q_{near}$。距离度量通常使用欧氏距离 $d(q_1, q_2) = ||q_1 – q_2||$。
- 扩展新节点：从 $q_{near}$ 向 $q_{rand}$ 的方向扩展一小步长 $\\delta$，得到一个新的候选点 $q_{new}$： $$ q_{new} = q_{near} + \\delta \\cdot \\frac{q_{rand} – q_{near}}{||q_{rand} – q_{near}||} $$ 如果 $||q_{rand} – q_{near}|| < \\delta$，则 $q_{new} = q_{rand}$。
- 碰撞检测：检查从 $q_{near}$ 到 $q_{new}$ 的路径段是否与障碍物发生碰撞。
- 添加节点和边：如果路径段无碰撞，则将 $q_{new}$ 加入节点集合 $V$，并将边 $(q_{near}, q_{new})$ 加入边集合 $E$。
终止条件：
- 重复步骤2，直到达到最大迭代次数 $K$，或者 $q_{new}$ 与目标点 $q_{goal}$ 的距离小于某个阈值 $\\epsilon$。
- 如果 $q_{new}$ 足够接近 $q_{goal}$，则将其与 $q_{goal}$ 连接（同样需进行碰撞检测），并将 $q_{goal}$ 加入树中，算法成功找到路径。
路径提取：
- 如果找到了连接 $q_{start}$ 和 $q_{goal}$ 的路径，则从 $q_{goal}$ 节点回溯到 $q_{start}$ 节点（通过父节点指针），即可得到从起点到目标点的路径。
3. RRT算法特点
- 优点：
- 概率完备性：只要存在可行路径，在足够长的运行时间下，算法找到路径的概率趋近于1。
- 适用于高维空间。
- 计算效率相对较高，易于实现。
- 对障碍物形状没有特殊要求。
- 缺点：
- 找到的路径通常不是最优的（可能绕远）。
- 路径可能比较“迂回”，不够平滑。
- 在狭窄通道中效率可能较低。
4. Python实现示例
以下是一个简化版的RRT算法Python实现，假设构型空间是二维的（如 $(x, y)$ 平面），障碍物用矩形表示。代码主要展示RRT的核心逻辑。
import numpy as np
import matplotlib.pyplot as plt
class Node:
def __init__(self, pos, parent=None):
self.pos = np.array(pos) # 节点位置, 例如 [x, y]
self.parent = parent # 父节点
def distance(q1, q2):
return np.linalg.norm(q1 – q2)
def is_collision_free(q_near, q_new, obstacles):
# 简化碰撞检测: 假设两点间直线连接，检查线段是否与任何障碍物相交
# 这里障碍物 obstacles 是一个列表，每个元素是 [x_min, y_min, x_max, y_max]
# 实际应用中需要更精细的碰撞检测
# 本示例省略精确实现，假设总是无碰撞
return True
def rrt_planner(start, goal, bounds, obstacles, max_iter=1000, step_size=0.5, goal_threshold=0.5):
# 初始化树
start_node = Node(start)
goal_node = Node(goal)
nodes = [start_node] # 节点列表
for _ in range(max_iter):
# 1. 随机采样 (以一定概率直接采样目标点，加速收敛)
if np.random.rand() < 0.05:
q_rand = goal_node.pos
else:
q_rand = np.array([np.random.uniform(bounds[0], bounds[1]),
np.random.uniform(bounds[2], bounds[3])])
# 2. 寻找最近邻节点
min_dist = float('inf')
nearest_node = None
for node in nodes:
dist = distance(node.pos, q_rand)
if dist < min_dist:
min_dist = dist
nearest_node = node
# 3. 扩展新节点
direction = q_rand – nearest_node.pos
length = np.linalg.norm(direction)
if length > step_size:
direction = direction / length * step_size
q_new_pos = nearest_node.pos + direction
# 4. 碰撞检测
if not is_collision_free(nearest_node.pos, q_new_pos, obstacles):
continue # 发生碰撞，跳过
# 5. 添加新节点
new_node = Node(q_new_pos, parent=nearest_node)
nodes.append(new_node)
# 6. 检查是否到达目标
if distance(new_node.pos, goal_node.pos) < goal_threshold:
# 尝试连接目标点
if is_collision_free(new_node.pos, goal_node.pos, obstacles):
goal_node.parent = new_node
nodes.append(goal_node)
print("Path found!")
return nodes, goal_node
print("Path not found within max iterations.")
return nodes, None
def extract_path(goal_node):
path = []
current = goal_node
while current is not None:
path.append(current.pos)
current = current.parent
path.reverse()
return path
# 示例用法
start = [0, 0]
goal = [9, 9]
bounds = [0, 10, 0, 10] # x_min, x_max, y_min, y_max
obstacles = [] # 假设没有障碍物
nodes, final_goal = rrt_planner(start, goal, bounds, obstacles)
if final_goal:
path = extract_path(final_goal)
print("Path:", path)
# 可视化代码 (略)
5. 应用场景
RRT及其变种（如 RRT*, Informed RRT*）广泛应用于：
- 机器人运动规划（移动机器人、机械臂）
- 自动驾驶车辆的路径规划
- 无人机导航
- 动画角色运动生成
希望这个详细的介绍和示例代码能帮助你理解RRT算法！
