1. 运动规划与采样搜索算法概述
在机器人导航、自动驾驶和游戏AI等领域，如何让智能体在复杂环境中找到一条从起点到终点的可行路径，一直是个核心问题。运动规划（Motion Planning）就是解决这类问题的关键技术，而采样搜索算法则是其中最具实用价值的一类方法。
我第一次接触RRT算法是在研究生时期的机器人课程上，当时被它那种"随机生长"的特性所吸引——就像植物向着阳光自然伸展枝干一样，RRT通过随机采样在配置空间中探索路径。后来在实际项目中，我才真正体会到这类算法的强大之处：它们不需要对环境进行精确建模，能高效处理高维空间，而且实现起来相对简单。
Python作为科学计算和算法原型开发的首选语言，拥有丰富的运动规划库支持。本文将重点解析两种经典的采样搜索算法：RRT（快速探索随机树）及其优化版本RRT*，并通过Python实现展示它们的核心特性和应用场景。
2. RRT算法原理与实现
2.1 RRT基础原理
RRT（Rapidly-exploring Random Tree）算法的核心思想是通过随机采样来快速探索配置空间。它特别适合解决高维空间中的运动规划问题，比如机械臂的路径规划或者无人机在三维环境中的导航。
算法的工作流程可以类比为在黑暗房间中摸索前进：
- 随机向某个方向伸出手（随机采样）
- 向那个方向移动一小段距离（步长限制）
- 如果碰到障碍物就停止，记录当前位置
- 重复这个过程直到找到出口
这种随机探索的特性使得RRT能够快速覆盖整个可行空间，而不需要预先构建完整的环境地图。
2.2 Python实现关键步骤
下面我们通过Python代码来拆解RRT的核心实现。这里使用matplotlib进行可视化，实际项目中可以集成到ROS或其它机器人框架中。
python复制import numpy as np
import matplotlib.pyplot as plt
class RRT:
def __init__(self, start, goal, obstacle_list, bounds, step_size=0.5):
self.start = np.array(start)
self.goal = np.array(goal)
self.obstacles = obstacle_list
self.bounds = bounds # 环境边界 [xmin, xmax, ymin, ymax]
self.step_size = step_size
self.nodes = [self.start]
self.parents = {tuple(self.start): None}
def random_sample(self):
# 以5%的概率直接采样目标点（加速收敛）
if np.random.random() < 0.05:
return self.goal
return np.array([
np.random.uniform(self.bounds[0], self.bounds[1]),
np.random.uniform(self.bounds[2], self.bounds[3])
])
def nearest_node(self, point):
distances = [np.linalg.norm(node - point) for node in self.nodes]
return self.nodes[np.argmin(distances)]
def steer(self, from_node, to_point):
direction = to_point - from_node
distance = np.linalg.norm(direction)
if distance <= self.step_size:
return to_point
return from_node + (direction / distance) * self.step_size
def is_collision_free(self, new_node, nearest_node):
# 简化的碰撞检测，实际项目中需要更精确的实现
for (ox, oy, radius) in self.obstacles:
if np.linalg.norm(np.array([ox, oy]) - new_node) <= radius:
return False
return True
def plan(self, max_iter=1000):
for _ in range(max_iter):
rand_point = self.random_sample()
nearest = self.nearest_node(rand_point)
new_point = self.steer(nearest, rand_point)
if self.is_collision_free(new_point, nearest):
self.nodes.append(new_point)
self.parents[tuple(new_point)] = tuple(nearest)
# 检查是否到达目标附近
if np.linalg.norm(new_point - self.goal) <= self.step_size:
self.nodes.append(self.goal)
self.parents[tuple(self.goal)] = tuple(new_point)
return self._reconstruct_path()
return None # 未找到路径
def _reconstruct_path(self):
path = [tuple(self.goal)]
while path[-1] != tuple(self.start):
path.append(self.parents[path[-1]])
return path[::-1] # 反转得到从起点到目标的路径
2.3 参数选择与调优经验
在实际应用中，RRT的性能很大程度上取决于几个关键参数：
-
步长(step_size)：
- 较小步长：路径更精确，但探索速度慢
- 较大步长：探索快，但可能错过狭窄通道
- 经验值：通常取环境尺寸的5-10%
-
目标偏向采样：
- 代码中设置了5%的概率直接采样目标点
- 这个值可以调整到10-20%以加快收敛，但过高会导致探索不充分
-
最大迭代次数：
- 简单环境：500-1000次足够
- 复杂环境：可能需要5000-10000次
- 可以设置超时机制而非固定迭代次数
提示：在真实机器人应用中，碰撞检测函数
is_collision_free通常是最耗时的部分。可以考虑使用空间划分数据结构（如KD-Tree）来加速邻近障碍物查询。
3. RRT*算法原理与改进
3.1 RRT*的核心优化
RRT虽然能快速找到路径，但得到的路径往往不是最优的——可能绕远路或者有不必要的转折。RRT*算法通过两个关键改进解决了这个问题：
- 重布线(Rewiring)：每当添加新节点时，检查附近现有节点是否可以通过新节点获得更短的路径
- 父节点重选：不仅连接新节点到最近的节点，而是在邻域内寻找能使路径代价最小的父节点
这些改进使得RRT*具有渐近最优性——随着迭代次数增加，路径会越来越接近理论最优解。
3.2 Python实现差异点
在RRT的基础上，我们需要增加几个关键功能：
python复制class RRTStar(RRT):
def __init__(self, start, goal, obstacle_list, bounds, step_size=0.5, search_radius=1.0):
super().__init__(start, goal, obstacle_list, bounds, step_size)
self.costs = {tuple(self.start): 0} # 从起点到各节点的路径代价
self.search_radius = search_radius
def near_nodes(self, new_point):
distances = [np.linalg.norm(node - new_point) for node in self.nodes]
return [self.nodes[i] for i in range(len(self.nodes))
if distances[i] <= self.search_radius]
def plan(self, max_iter=1000):
for _ in range(max_iter):
rand_point = self.random_sample()
nearest = self.nearest_node(rand_point)
new_point = self.steer(nearest, rand_point)
if self.is_collision_free(new_point, nearest):
near_nodes = self.near_nodes(new_point)
# 选择最小代价的父节点
min_cost = float('inf')
best_parent = None
for node in near_nodes:
cost = self.costs[tuple(node)] + np.linalg.norm(node - new_point)
if cost < min_cost and self.is_collision_free(new_point, node):
min_cost = cost
best_parent = node
if best_parent is not None:
self.nodes.append(new_point)
self.parents[tuple(new_point)] = tuple(best_parent)
self.costs[tuple(new_point)] = min_cost
# 重布线步骤
for node in near_nodes:
new_cost = self.costs[tuple(new_point)] + np.linalg.norm(node - new_point)
if new_cost < self.costs[tuple(node)] and self.is_collision_free(node, new_point):
self.parents[tuple(node)] = tuple(new_point)
self.costs[tuple(node)] = new_cost
# 检查是否到达目标
if np.linalg.norm(new_point - self.goal) <= self.step_size:
if tuple(self.goal) not in self.parents:
self.parents[tuple(self.goal)] = tuple(new_point)
self.costs[tuple(self.goal)] = self.costs[tuple(new_point)] + np.linalg.norm(new_point - self.goal)
return self._reconstruct_path()
return None
3.3 性能对比与选择建议
通过实验对比两种算法在相同环境下的表现：
| 特性 | RRT | RRT* |
|---|---|---|
| 收敛速度 | 快 | 较慢 |
| 路径质量 | 次优 | 渐近最优 |
| 计算复杂度 | O(n) | O(n log n) |
| 内存消耗 | 较低 | 较高 |
| 适用场景 | 实时性要求高 | 路径质量优先 |
选择建议：
- 当需要快速获得可行解时（如实时避障），选择RRT
- 当路径质量更重要且有时间优化时（如离线规划），选择RRT*
- 在非常复杂的环境中，可以先使用RRT快速找到初始路径，再用RRT*进行优化
4. 实际应用与性能优化
4.1 在Python运动规划库中的集成
现代Python运动规划库（如OMPL的Python绑定、PyRoboPlan等）通常都实现了RRT系列算法。以pyroboplan为例，使用RRT*只需要几行代码：
python复制from pyroboplan.planning.rrt import RRTPlanner
from pyroboplan.models.trees import RapidlyExploringRandomTree
planner = RRTPlanner(
model=robot_model,
collision_checker=collision_checker,
tree_class=RapidlyExploringRandomTree,
max_step_size=0.1,
goal_bias=0.15,
max_iterations=5000
)
path = planner.plan(start_conf, goal_conf)
4.2 加速技巧与并行化
当处理复杂环境或高维配置空间时，RRT/RRT*的性能可能成为瓶颈。以下是一些实测有效的优化方法：
-
KD-Tree加速邻近搜索：
python复制from scipy.spatial import KDTree class KDTreeRRT(RRT): def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) self.update_tree() def update_tree(self): self.kd_tree = KDTree(self.nodes) def nearest_node(self, point): _, idx = self.kd_tree.query(point) return self.nodes[idx] -
并行化采样：
- 使用多进程同时生成多个随机树
- 最先找到路径的进程终止其他进程
- Python的
multiprocessing模块适合这种任务
-
启发式采样：
- 在已知障碍物分布的区域减少采样密度
- 在狭窄通道区域增加采样密度
- 可以使用高斯混合模型(GMM)来建模采样分布
4.3 动态环境处理
原始RRT/RRT*假设环境是静态的，但现实中障碍物可能移动。扩展算法处理动态环境的方法包括：
-
增量式更新：
- 检测环境变化后，只更新受影响部分的树结构
- 保留大部分有效的树节点
-
滚动时域规划：
- 在移动机器人的局部窗口内重新规划
- 结合全局RRT路径和局部避障
-
障碍物预测：
- 对移动障碍物进行运动预测
- 在采样时避开预测的障碍物轨迹
5. 常见问题与调试技巧
5.1 算法不收敛问题排查
当RRT长时间找不到路径时，可以按以下步骤排查：
-
检查碰撞检测：
- 打印出采样点和碰撞检测结果
- 确保障碍物表示正确
-
调整采样策略：
- 增加目标偏向概率（goal_bias）
- 尝试不同的步长值
-
可视化中间过程：
python复制def visualize(self): plt.figure(figsize=(10, 10)) # 绘制障碍物 for (ox, oy, radius) in self.obstacles: circle = plt.Circle((ox, oy), radius, color='gray') plt.gca().add_patch(circle) # 绘制树结构 for node in self.parents: parent = self.parents[node] if parent is not None: plt.plot([node[0], parent[0]], [node[1], parent[1]], 'b-', lw=0.5) # 绘制起点和目标 plt.plot(self.start[0], self.start[1], 'go', markersize=10) plt.plot(self.goal[0], self.goal[1], 'ro', markersize=10) plt.axis('equal') plt.xlim(self.bounds[0], self.bounds[1]) plt.ylim(self.bounds[2], self.bounds[3]) plt.show()
5.2 路径抖动问题解决
RRT生成的路径可能包含不必要的转折，解决方法包括：
-
路径平滑：
- 使用B样条曲线拟合
- 简单的线性插值平滑
-
后处理优化：
python复制def smooth_path(self, path, max_iter=100): smoothed = path.copy() for _ in range(max_iter): # 随机选择两个点 i, j = sorted(np.random.choice(len(smoothed), 2, replace=False)) if self.is_collision_free(smoothed[i], smoothed[j]): smoothed = smoothed[:i+1] + smoothed[j:] return smoothed -
考虑运动学约束：
- 在steer函数中加入最大曲率限制
- 使用Dubins路径或Reeds-Shepp曲线连接节点
5.3 高维空间规划挑战
当配置空间维度增加时（如机械臂规划），会遇到"维度灾难"。应对策略：
-
投影降维：
- 将高维空间投影到低维子空间
- 在低维空间规划后再映射回原空间
-
分层规划：
- 先在粗粒度空间规划
- 再在局部区域进行精细规划
-
约束采样：
- 利用任务约束减少采样空间
- 例如机械臂末端执行器约束
6. 扩展与变种算法
6.1 RRT-Connect双向扩展
RRT-Connect通过同时从起点和目标点生长两棵树来加速收敛：
python复制class RRTConnect(RRT):
def __init__(self, *args, **kwargs):
super().__init__(*args, **kwargs)
self.nodes_goal = [self.goal]
self.parents_goal = {tuple(self.goal): None}
def plan(self, max_iter=1000):
for _ in range(max_iter):
# 交替扩展两棵树
if len(self.nodes) < len(self.nodes_goal):
tree_a, tree_b = self, self.nodes_goal
parents_a, parents_b = self.parents, self.parents_goal
else:
tree_a, tree_b = self.nodes_goal, self.nodes
parents_a, parents_b = self.parents_goal, self.parents
rand_point = self.random_sample()
nearest = tree_a.nearest_node(rand_point)
new_point = tree_a.steer(nearest, rand_point)
if self.is_collision_free(new_point, nearest):
tree_a.append(new_point)
parents_a[tuple(new_point)] = tuple(nearest)
# 尝试连接另一棵树
nearest_b = tree_b.nearest_node(new_point)
if np.linalg.norm(new_point - nearest_b) <= self.step_size:
if self.is_collision_free(new_point, nearest_b):
# 找到路径，重建
path_a = self._reconstruct_path(tuple(new_point), parents_a)
path_b = self._reconstruct_path(tuple(nearest_b), parents_b)
return path_a + path_b[::-1]
return None
6.2 Informed RRT* 高效优化
Informed RRT*在找到初始路径后，将采样限制在能够改进当前路径的椭圆区域内：
python复制class InformedRRTStar(RRTStar):
def __init__(self, *args, **kwargs):
super().__init__(*args, **kwargs)
self.best_cost = float('inf')
self.best_path = None
def random_sample(self):
if self.best_path is None:
return super().random_sample()
# 在椭圆区域内采样
c_min = np.linalg.norm(self.goal - self.start)
x_center = (self.start + self.goal) / 2
C = self.rotation_to_ellipse_frame(self.start, self.goal)
r1 = self.best_cost / 2
r2 = np.sqrt(self.best_cost**2 - c_min**2) / 2
while True:
rand = np.random.uniform(-1, 1, 2)
if np.linalg.norm(rand) <= 1:
rand_ball = rand * r1
rand_ellipse = C @ np.array([rand_ball[0], rand_ball[1] * (r2/r1)])
return x_center + rand_ellipse
def rotation_to_ellipse_frame(self, start, goal):
a1 = (goal - start) / np.linalg.norm(goal - start)
return np.array([[a1[0], -a1[1]], [a1[1], a1[0]]])
6.3 其他实用变种
- RRT#：结合RRT*和PRM的优点，在重布线时考虑更广范围的邻域
- RRT：针对动态环境优化，支持快速重规划
- QRRT：考虑机器人动力学约束的RRT变种
- RRT-Smart*：利用路径信息指导采样，加速收敛
在实际项目中，我经常根据具体需求混合使用这些算法的思想。比如在无人机规划中，可能会结合RRT-Connect的快速性和Informed RRT*的优化能力。