# Python实现RRT算法避障路径规划：从原理到代码实战（附完整项目） 最近在做一个室内移动机器人的项目，客户要求机器人在一个动态变化的仓库环境中自主导航，避开货架和临时堆放物。传统的A*算法在栅格地图上表现不错，但当地图尺寸变大、障碍物形状不规则时，计算开销和路径的“机器感”就成了问题。这时候，同事推荐了基于采样的路径规划方法，尤其是**RRT（快速随机探索树）** 系列算法。第一次看到RRT生成的路径在复杂空间里像树枝一样蔓延开来，最终连接起点和终点时，确实有种眼前一亮的感觉——它不依赖精细的栅格划分，在高维空间里也能保持不错的效率。 不过，把论文里的RRT算法直接搬到实际机器人上，中间隔了不止一条鸿沟。参数怎么调？计算效率如何？路径怎么优化？可视化调试有哪些技巧？这些问题都是在工程落地的过程中必须面对的。这篇文章，我就结合自己最近在自动驾驶仿真和机器人导航项目中的实践经验，手把手带你从零实现一个**可运行、可调试、可优化**的RRT路径规划器。我们会从最基本的RRT开始，逐步扩展到更优的RRT*，并讨论在实际应用中如何调整参数、处理动态障碍物，以及将算法集成到ROS等机器人框架中。 无论你是机器人方向的学生，还是正在开发自动驾驶感知模块的工程师，这篇文章提供的代码和思路都能直接拿来用。我会把完整的项目代码放在最后，你可以直接克隆到本地，修改几个参数就能看到不同场景下的路径规划效果。 ## 1. 环境搭建与基础概念：为什么需要RRT？ 在开始写代码之前，我们得先搞清楚RRT到底解决了什么问题。传统的路径规划算法，比如Dijkstra或A*，通常需要在离散的栅格地图上搜索。这种方法在二维小地图上工作得很好，但一旦扩展到三维空间（比如无人机规划）甚至更高维的机械臂关节空间，栅格数量会呈指数级增长，导致“维度灾难”。 > 提示：维度灾难（Curse of Dimensionality）指的是当空间维度增加时，分析和组织高维空间中的数据所需的计算量会急剧增加，使得许多在低维空间有效的方法变得不可行。 RRT的核心思想非常巧妙：它不试图系统性地搜索整个空间，而是通过**随机采样**的方式，逐步构建一棵从起点向外生长的树。这棵树会探索空间中的可行区域，直到某个分支触及终点附近。这种方法有几个天然的优势： - **概率完备性**：只要存在可行路径，当采样次数趋于无穷时，RRT几乎肯定能找到它。 - **适合高维空间**：计算复杂度不直接依赖于空间维度，而是与采样次数和树的大小相关。 - **对障碍物表示要求灵活**：只需要一个碰撞检测函数来判断两点之间是否连通，而不需要将空间离散化为栅格。 当然，RRT也有缺点。最明显的是，它找到的路径通常不是最优的，可能绕远路，而且路径由直线段组成，不够平滑。后续的RRT*、Informed RRT*等算法正是为了解决这些问题而提出的。 ### 1.1 安装必要的Python库 我们的实现主要依赖几个常见的科学计算和可视化库。如果你用Anaconda，大部分已经预装了。建议创建一个新的虚拟环境来管理依赖。 ```bash # 创建并激活虚拟环境（可选） conda create -n rrt_planning python=3.8 conda activate rrt_planning # 安装核心依赖 pip install numpy matplotlib ``` *NumPy* 用于高效的数组操作和数学计算，*Matplotlib* 用于可视化路径规划过程和结果。对于更复杂的项目，你可能还会用到 *scipy* 进行几何计算，或者 *shapely* 处理多边形障碍物，但为了保持简洁，我们先用基础库实现核心逻辑。 ### 1.2 定义问题场景：一个简单的二维平面 为了直观演示，我们先在一个简单的二维矩形空间里进行规划。空间中有几个矩形障碍物。起点和终点分别位于空间的左下角和右上角。 ```python import numpy as np import matplotlib.pyplot as plt from typing import List, Tuple, Optional class PlanningSpace: """定义一个二维规划空间，包含边界和障碍物""" def __init__(self, x_lim: Tuple[float, float], y_lim: Tuple[float, float]): self.x_min, self.x_max = x_lim self.y_min, self.y_max = y_lim self.obstacles: List[Tuple[float, float, float, float]] = [] # (x_min, y_min, x_max, y_max) def add_rect_obstacle(self, x_min: float, y_min: float, x_max: float, y_max: float): """添加一个矩形障碍物""" self.obstacles.append((x_min, y_min, x_max, y_max)) def is_point_in_obstacle(self, point: Tuple[float, float]) -> bool: """检查点是否在任意障碍物内部""" x, y = point for ox_min, oy_min, ox_max, oy_max in self.obstacles: if ox_min <= x <= ox_max and oy_min <= y <= oy_max: return True return False def is_collision_free(self, p1: Tuple[float, float], p2: Tuple[float, float], resolution: float = 0.1) -> bool: """检查从p1到p2的线段是否与障碍物相交""" # 简单实现：在线段上采样多个点，检查是否在障碍物内 # 实际工程中可能需要更精确的几何碰撞检测 x1, y1 = p1 x2, y2 = p2 dx, dy = x2 - x1, y2 - y1 length = np.hypot(dx, dy) steps = max(2, int(length / resolution)) for i in range(steps + 1): t = i / steps x = x1 + t * dx y = y1 + t * dy if self.is_point_in_obstacle((x, y)): return False return True ``` 这个 `PlanningSpace` 类定义了我们的“世界”。`is_collision_free` 函数是RRT算法的关键组成部分之一，它决定了树能否向某个方向生长。这里用了简单的点采样方法进行碰撞检测，对于矩形障碍物足够用，但对于复杂形状可能需要更高效的算法（比如分离轴定理）。 ## 2. RRT核心算法实现：让树在空间中生长 理解了问题场景后，我们开始实现RRT算法。我会把算法拆解成几个清晰的步骤，并解释每个步骤的工程考虑。 ### 2.1 算法步骤分解 标准的RRT算法流程可以概括为以下几步： 1. **初始化**：将起点作为树的根节点。 2. **随机采样**：在自由空间中随机生成一个点。 3. **寻找最近邻**：在现有树节点中找到距离随机点最近的节点。 4. **扩展新节点**：从最近邻节点向随机点方向生长一个固定步长，得到新节点。 5. **碰撞检测**：检查从最近邻到新节点的路径是否与障碍物碰撞。 6. **添加节点**：如果路径安全，将新节点加入树中，并记录其父节点。 7. **终止判断**：如果新节点在终点邻域内，则规划成功；否则返回步骤2。 这个循环会一直执行，直到找到路径或达到最大迭代次数。下面我们用代码实现这个逻辑。 ### 2.2 完整的RRT类实现 ```python import random import math class RRTPlanner: """RRT路径规划器""" def __init__(self, space: PlanningSpace, start: Tuple[float, float], goal: Tuple[float, float], step_size: float = 1.0, goal_tolerance: float = 0.5, max_iter: int = 5000): self.space = space self.start = start self.goal = goal self.step_size = step_size # 生长步长 self.goal_tolerance = goal_tolerance # 终点邻域半径 self.max_iter = max_iter # 树的数据结构：每个节点是 (x, y, parent_index) self.nodes = [start] # 节点坐标列表 self.parents = [-1] # 父节点索引列表，起点没有父节点，设为-1 # 检查起点和终点是否在障碍物内 if space.is_point_in_obstacle(start): raise ValueError("起点位于障碍物内！") if space.is_point_in_obstacle(goal): raise ValueError("终点位于障碍物内！") def random_sample(self) -> Tuple[float, float]: """在自由空间中随机采样一个点""" while True: # 以一定概率直接采样终点（加速收敛） if random.random() < 0.1: # 10%的概率采样终点 return self.goal x = random.uniform(self.space.x_min, self.space.x_max) y = random.uniform(self.space.y_min, self.space.y_max) point = (x, y) # 确保采样点不在障碍物内（可选，也可以后续碰撞检测时处理） if not self.space.is_point_in_obstacle(point): return point def nearest_node(self, point: Tuple[float, float]) -> int: """找到树中距离给定点最近的节点索引""" min_dist = float('inf') nearest_idx = 0 for i, node in enumerate(self.nodes): dist = math.hypot(node[0] - point[0], node[1] - point[1]) if dist < min_dist: min_dist = dist nearest_idx = i return nearest_idx def steer(self, from_point: Tuple[float, float], to_point: Tuple[float, float]) -> Tuple[float, float]: """从from_point向to_point方向生长一个步长""" dx = to_point[0] - from_point[0] dy = to_point[1] - from_point[1] dist = math.hypot(dx, dy) if dist <= self.step_size: return to_point # 如果距离小于步长，直接到达目标点 else: # 按步长比例截取 ratio = self.step_size / dist new_x = from_point[0] + dx * ratio new_y = from_point[1] + dy * ratio return (new_x, new_y) def plan(self) -> Optional[List[Tuple[float, float]]]: """执行RRT路径规划""" for iteration in range(self.max_iter): # 1. 随机采样 rand_point = self.random_sample() # 2. 寻找最近邻 nearest_idx = self.nearest_node(rand_point) nearest_node = self.nodes[nearest_idx] # 3. 扩展新节点 new_node = self.steer(nearest_node, rand_point) # 4. 碰撞检测 if self.space.is_collision_free(nearest_node, new_node): # 5. 添加新节点到树中 self.nodes.append(new_node) self.parents.append(nearest_idx) # 6. 检查是否到达终点附近 dist_to_goal = math.hypot(new_node[0] - self.goal[0], new_node[1] - self.goal[1]) if dist_to_goal <= self.goal_tolerance: print(f"规划成功！迭代次数: {iteration}") return self.extract_path(len(self.nodes) - 1) # 新节点是最后一个 print(f"规划失败，达到最大迭代次数 {self.max_iter}") return None def extract_path(self, goal_node_idx: int) -> List[Tuple[float, float]]: """从目标节点回溯到起点，提取路径""" path = [] current_idx = goal_node_idx while current_idx != -1: path.append(self.nodes[current_idx]) current_idx = self.parents[current_idx] path.reverse() # 从起点到终点 return path ``` 这个实现有几个工程上的细节值得注意： 1. **随机采样策略**：有10%的概率直接采样终点，而不是完全随机。这个技巧能显著加快收敛速度，在实际应用中很常见。 2. **最近邻搜索**：这里用了线性搜索，对于小规模树没问题。但如果节点数很多（比如上万），应该使用空间数据结构加速，比如KD-Tree。 3. **碰撞检测分辨率**：在 `PlanningSpace.is_collision_free` 中，我们沿线段采样检查碰撞。分辨率参数需要根据障碍物大小和步长调整，太粗可能漏检，太细影响性能。 ### 2.3 可视化与调试 算法写好了，但看不到效果等于白搭。我们写一个可视化函数，看看RRT树是怎么生长的。 ```python def visualize_rrt(planner: RRTPlanner, path: List[Tuple[float, float]] = None): """可视化RRT树和路径""" fig, ax = plt.subplots(figsize=(10, 8)) # 绘制障碍物 for obs in planner.space.obstacles: x_min, y_min, x_max, y_max = obs rect = plt.Rectangle((x_min, y_min), x_max - x_min, y_max - y_min, facecolor='gray', alpha=0.7, edgecolor='black') ax.add_patch(rect) # 绘制树 for i in range(1, len(planner.nodes)): parent_idx = planner.parents[i] if parent_idx != -1: node = planner.nodes[i] parent = planner.nodes[parent_idx] ax.plot([parent[0], node[0]], [parent[1], node[1]], 'lightblue', linewidth=0.5, alpha=0.6) # 绘制节点 nodes_array = np.array(planner.nodes) ax.scatter(nodes_array[:, 0], nodes_array[:, 1], s=10, c='blue', alpha=0.5, label='树节点') # 绘制起点和终点 ax.scatter(*planner.start, s=100, c='green', marker='o', label='起点', edgecolors='black') ax.scatter(*planner.goal, s=100, c='red', marker='*', label='终点', edgecolors='black') # 绘制路径 if path: path_array = np.array(path) ax.plot(path_array[:, 0], path_array[:, 1], 'r-', linewidth=3, label='规划路径') ax.set_xlim(planner.space.x_min, planner.space.x_max) ax.set_ylim(planner.space.y_min, planner.space.y_max) ax.set_aspect('equal') ax.grid(True, alpha=0.3) ax.legend() ax.set_title("RRT路径规划结果") plt.show() ``` 现在让我们运行一个完整的例子： ```python # 创建规划空间 space = PlanningSpace((0, 20), (0, 15)) space.add_rect_obstacle(4, 2, 6, 8) space.add_rect_obstacle(8, 5, 10, 12) space.add_rect_obstacle(12, 3, 15, 6) space.add_rect_obstacle(14, 9, 18, 11) # 定义起点和终点 start = (1.0, 1.0) goal = (18.0, 13.0) # 创建RRT规划器并规划 planner = RRTPlanner(space, start, goal, step_size=1.5, max_iter=3000) path = planner.plan() # 可视化结果 if path: print(f"路径长度: {len(path)} 个节点") visualize_rrt(planner, path) ``` 运行这段代码，你会看到一棵蓝色的树在空间中生长，最终找到一条红色路径连接起点和终点。多运行几次，每次的树和路径都会不同，这是RRT随机性的体现。 ## 3. 从RRT到RRT*：优化路径质量 基础的RRT能找到可行路径，但路径质量往往不高，看起来绕来绕去。这是因为RRT只保证连通性，不优化路径长度。RRT* 在RRT的基础上增加了两个关键操作：**重选父节点**和**重布线**，从而渐进优化到最优路径。 ### 3.1 RRT* 的核心改进 RRT* 的算法流程与RRT类似，但在添加新节点后多了两个步骤： 1. **重选父节点（Rewire）**：在新节点周围一定半径内寻找所有可能的父节点候选，选择能使新节点到起点路径代价最小的节点作为父节点。 2. **重布线（Random Relink）**：检查新节点是否能成为周围节点的更好父节点，如果是，则更新这些节点的父节点为新节点。 这两个操作使得树的结构不断优化，随着迭代次数增加，路径会逐渐收敛到最优。代价是每次迭代需要更多的计算。 ### 3.2 RRT* 实现细节 我们需要修改节点数据结构，增加从起点到该节点的路径代价。同时实现重选父节点和重布线功能。 ```python class RRTStarPlanner(RRTPlanner): """RRT* 路径规划器，继承自RRT""" def __init__(self, space: PlanningSpace, start: Tuple[float, float], goal: Tuple[float, float], step_size: float = 1.0, goal_tolerance: float = 0.5, max_iter: int = 5000, rewire_radius: float = 2.0): super().__init__(space, start, goal, step_size, goal_tolerance, max_iter) self.rewire_radius = rewire_radius # 重布线搜索半径 self.costs = [0.0] # 每个节点到起点的路径代价，起点代价为0 def plan(self) -> Optional[List[Tuple[float, float]]]: """执行RRT*路径规划""" for iteration in range(self.max_iter): # 随机采样 rand_point = self.random_sample() # 寻找最近邻 nearest_idx = self.nearest_node(rand_point) nearest_node = self.nodes[nearest_idx] # 扩展新节点 new_node = self.steer(nearest_node, rand_point) # 碰撞检测 if self.space.is_collision_free(nearest_node, new_node): # 重选父节点：在新节点周围寻找更好的父节点 new_parent_idx, new_cost = self.choose_parent(new_node, nearest_idx) # 添加新节点 self.nodes.append(new_node) self.parents.append(new_parent_idx) self.costs.append(new_cost) # 重布线：更新周围节点的父节点 self.rewire(len(self.nodes) - 1) # 检查是否到达终点附近 dist_to_goal = math.hypot(new_node[0] - self.goal[0], new_node[1] - self.goal[1]) if dist_to_goal <= self.goal_tolerance: # 对于RRT*，我们还需要检查是否有到终点更优的路径 goal_node_idx = len(self.nodes) - 1 final_path = self.extract_path(goal_node_idx) print(f"找到路径！迭代次数: {iteration}") return final_path print(f"规划失败，达到最大迭代次数 {self.max_iter}") return None def choose_parent(self, new_node: Tuple[float, float], nearest_idx: int) -> Tuple[int, float]: """为新节点选择最优父节点""" nearest_nodes = self.find_near_nodes(new_node, self.rewire_radius) # 初始化：以最近邻节点为候选 best_parent_idx = nearest_idx best_cost = self.costs[nearest_idx] + math.hypot( self.nodes[nearest_idx][0] - new_node[0], self.nodes[nearest_idx][1] - new_node[1] ) # 检查所有邻近节点，看是否有更优的父节点 for near_idx in nearest_nodes: near_node = self.nodes[near_idx] # 检查从邻近节点到新节点是否无碰撞 if self.space.is_collision_free(near_node, new_node): # 计算通过该邻近节点到达新节点的代价 tentative_cost = self.costs[near_idx] + math.hypot( near_node[0] - new_node[0], near_node[1] - new_node[1] ) if tentative_cost < best_cost: best_cost = tentative_cost best_parent_idx = near_idx return best_parent_idx, best_cost def find_near_nodes(self, point: Tuple[float, float], radius: float) -> List[int]: """找到距离给定点在一定半径内的所有节点索引""" near_indices = [] for i, node in enumerate(self.nodes): dist = math.hypot(node[0] - point[0], node[1] - point[1]) if dist <= radius: near_indices.append(i) return near_indices def rewire(self, new_node_idx: int): """重布线：检查新节点是否能成为周围节点的更好父节点""" new_node = self.nodes[new_node_idx] new_cost = self.costs[new_node_idx] near_indices = self.find_near_nodes(new_node, self.rewire_radius) for near_idx in near_indices: if near_idx == new_node_idx: continue near_node = self.nodes[near_idx] # 检查从新节点到邻近节点是否无碰撞 if self.space.is_collision_free(new_node, near_node): # 计算通过新节点到达邻近节点的代价 tentative_cost = new_cost + math.hypot( new_node[0] - near_node[0], new_node[1] - near_node[1] ) # 如果新路径代价更低，更新父节点 if tentative_cost < self.costs[near_idx]: self.parents[near_idx] = new_node_idx self.costs[near_idx] = tentative_cost # 注意：更新一个节点的代价后，其子节点的代价也需要更新 # 这里简化处理，实际可能需要递归更新子树 ``` RRT* 的实现比RRT复杂不少，主要是多了代价计算和重布线逻辑。这里有一个简化：当更新一个节点的父节点时，我们只更新了该节点的代价，没有递归更新其所有子节点的代价。在完整实现中，需要遍历以该节点为根的子树，更新所有节点的代价。不过对于演示目的，这个简化版本已经能体现RRT* 的优化效果。 ### 3.3 对比RRT和RRT* 的效果 让我们在同一个场景下运行两种算法，看看效果差异： ```python # 使用相同的场景 space = PlanningSpace((0, 20), (0, 15)) space.add_rect_obstacle(4, 2, 6, 8) space.add_rect_obstacle(8, 5, 10, 12) space.add_rect_obstacle(12, 3, 15, 6) space.add_rect_obstacle(14, 9, 18, 11) start = (1.0, 1.0) goal = (18.0, 13.0) # 运行RRT print("=== RRT算法 ===") rrt_planner = RRTPlanner(space, start, goal, step_size=1.5, max_iter=2000) rrt_path = rrt_planner.plan() # 运行RRT* print("\n=== RRT*算法 ===") rrt_star_planner = RRTStarPlanner(space, start, goal, step_size=1.5, max_iter=2000, rewire_radius=3.0) rrt_star_path = rrt_star_planner.plan() # 计算路径长度 def path_length(path): length = 0.0 for i in range(1, len(path)): length += math.hypot(path[i][0] - path[i-1][0], path[i][1] - path[i-1][1]) return length if rrt_path and rrt_star_path: print(f"\nRRT路径长度: {path_length(rrt_path):.2f}") print(f"RRT*路径长度: {path_length(rrt_star_path):.2f}") print(f"优化比例: {(1 - path_length(rrt_star_path)/path_length(rrt_path))*100:.1f}%") # 可视化对比 fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(16, 6)) # 绘制RRT结果 for i in range(1, len(rrt_planner.nodes)): parent_idx = rrt_planner.parents[i] if parent_idx != -1: node = rrt_planner.nodes[i] parent = rrt_planner.nodes[parent_idx] ax1.plot([parent[0], node[0]], [parent[1], node[1]], 'lightblue', linewidth=0.3, alpha=0.5) rrt_path_array = np.array(rrt_path) ax1.plot(rrt_path_array[:, 0], rrt_path_array[:, 1], 'r-', linewidth=3, label='RRT路径') ax1.scatter(*start, s=100, c='green', marker='o', edgecolors='black') ax1.scatter(*goal, s=100, c='red', marker='*', edgecolors='black') ax1.set_title(f"RRT - 路径长度: {path_length(rrt_path):.2f}") ax1.set_xlim(space.x_min, space.x_max) ax1.set_ylim(space.y_min, space.y_max) ax1.set_aspect('equal') ax1.grid(True, alpha=0.3) # 绘制RRT*结果 for i in range(1, len(rrt_star_planner.nodes)): parent_idx = rrt_star_planner.parents[i] if parent_idx != -1: node = rrt_star_planner.nodes[i] parent = rrt_star_planner.nodes[parent_idx] ax2.plot([parent[0], node[0]], [parent[1], node[1]], 'lightblue', linewidth=0.3, alpha=0.5) rrt_star_path_array = np.array(rrt_star_path) ax2.plot(rrt_star_path_array[:, 0], rrt_star_path_array[:, 1], 'r-', linewidth=3, label='RRT*路径') ax2.scatter(*start, s=100, c='green', marker='o', edgecolors='black') ax2.scatter(*goal, s=100, c='red', marker='*', edgecolors='black') ax2.set_title(f"RRT* - 路径长度: {path_length(rrt_star_path):.2f}") ax2.set_xlim(space.x_min, space.x_max) ax2.set_ylim(space.y_min, space.y_max) ax2.set_aspect('equal') ax2.grid(True, alpha=0.3) plt.tight_layout() plt.show() ``` 运行这个对比，你会发现RRT* 的路径通常更短、更直接。随着迭代次数增加，RRT* 的路径会继续优化，而RRT的路径一旦找到就不会再改变。这就是渐进最优性。 ## 4. 工程实践：参数调优与性能优化 算法实现只是第一步，要让RRT在实际项目中可靠工作，还需要大量的调优和优化。这里分享几个我在项目中积累的经验。 ### 4.1 关键参数的影响与调优 RRT系列算法有几个关键参数，直接影响规划效果和效率： | 参数 | 作用 | 调优建议 | 典型值范围 | |------|------|----------|-----------| | **步长 (step_size)** | 控制每次生长的距离 | 太小导致规划慢，太大可能跳过狭窄通道。一般为空间对角线长度的1%~5% | 0.5~5.0 | | **最大迭代次数 (max_iter)** | 算法尝试的最大次数 | 根据空间复杂度调整。太小时可能找不到路径 | 1000~10000 | | **重布线半径 (rewire_radius)** | RRT* 中寻找邻近节点的范围 | 与步长相关，通常为步长的1.5~3倍 | 步长×1.5~3.0 | | **目标偏置概率** | 直接采样终点的概率 | 提高可加速收敛，但可能陷入局部最优 | 5%~20% | | **终点容差 (goal_tolerance)** | 判定到达终点的距离 | 根据机器人尺寸和精度要求设定 | 机器人半径×1.5 | 这些参数需要根据具体场景调整。我通常的做法是： 1. **先确定步长**：根据环境中最窄通道的宽度，步长应小于通道宽度的一半。 2. **调整迭代次数**：从较小值开始，逐步增加直到能稳定找到路径。 3. **优化RRT*参数**：重布线半径需要平衡优化效果和计算开销。 4. **加入自适应机制**：比如根据规划成功率动态调整参数。 ### 4.2 性能优化技巧 当节点数达到几千时，基础实现的性能会成为瓶颈。以下是一些优化方向： **1. 加速最近邻搜索** 线性搜索的复杂度是O(n)，使用KD-Tree可以降到O(log n)： ```python from scipy.spatial import KDTree class RRTPlannerOptimized(RRTPlanner): def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) self.kd_tree = None self.update_kd_tree() def update_kd_tree(self): """更新KD-Tree""" if self.nodes: self.kd_tree = KDTree(self.nodes) def nearest_node(self, point: Tuple[float, float]) -> int: """使用KD-Tree加速最近邻搜索""" if self.kd_tree is None: return super().nearest_node(point) _, idx = self.kd_tree.query(point) return idx def plan(self): # ... 在添加新节点后需要更新KD-Tree self.nodes.append(new_node) self.parents.append(nearest_idx) self.update_kd_tree() # 更新数据结构 # ... ``` **2. 批量碰撞检测** 如果障碍物是凸多边形，可以使用更高效的碰撞检测算法。对于矩形障碍物，可以用分离轴定理： ```python def line_rect_intersection(p1, p2, rect): """检查线段与矩形是否相交（分离轴定理实现）""" x_min, y_min, x_max, y_max = rect rect_points = [(x_min, y_min), (x_max, y_min), (x_max, y_max), (x_min, y_max)] # 检查线段是否完全在矩形一侧 # 实现略，这是计算几何的标准算法 pass ``` **3. 并行化采样** RRT的每次迭代相对独立，可以并行处理多个采样点，选择最好的一个： ```python from concurrent.futures import ThreadPoolExecutor import multiprocessing class ParallelRRTPlanner(RRTPlanner): def parallel_plan_step(self): """并行执行多个采样和扩展尝试""" num_parallel = 4 # 并行数，根据CPU核心数调整 candidates = [] with ThreadPoolExecutor(max_workers=num_parallel) as executor: futures = [] for _ in range(num_parallel): future = executor.submit(self.try_extend) futures.append(future) for future in futures: result = future.result() if result: candidates.append(result) # 选择最优的候选（如距离终点最近） if candidates: best_candidate = min(candidates, key=lambda x: x[2]) # 假设第三个元素是到终点的距离 # 添加最佳候选到树中 # ... ``` ### 4.3 处理动态障碍物 在实际机器人应用中，障碍物可能是动态的。RRT可以扩展为动态RRT（Dynamic RRT），基本思路是： 1. **定期重新规划**：当环境变化时，重新运行RRT。 2. **增量更新**：重用之前的树结构，只更新受影响的部分。 3. **局部修复**：当检测到路径段与新增障碍物碰撞时，局部重新规划。 这里给出一个简单的定期重规划实现： ```python class DynamicRRTPlanner: """处理动态环境的RRT规划器""" def __init__(self, space: PlanningSpace, start, goal, replan_interval: float = 1.0): self.space = space self.start = start self.goal = goal self.replan_interval = replan_interval # 重规划间隔（秒） self.current_path = None self.last_replan_time = 0 def update_obstacles(self, new_obstacles): """更新障碍物信息""" self.space.obstacles = new_obstacles def get_path(self, current_time: float, robot_position: Tuple[float, float]) -> List[Tuple[float, float]]: """获取当前路径，必要时重新规划""" # 检查是否需要重新规划 need_replan = ( current_time - self.last_replan_time > self.replan_interval or self.current_path is None or self.is_path_blocked(self.current_path) or self.distance_to_path(robot_position, self.current_path) > 2.0 # 机器人偏离路径太远 ) if need_replan: print(f"重新规划路径，时间: {current_time}") # 以当前位置为起点重新规划 planner = RRTStarPlanner(self.space, robot_position, self.goal, max_iter=1000) self.current_path = planner.plan() self.last_replan_time = current_time return self.current_path def is_path_blocked(self, path: List[Tuple[float, float]]) -> bool: """检查路径是否被障碍物阻挡""" for i in range(len(path) - 1): if not self.space.is_collision_free(path[i], path[i+1]): return True return False def distance_to_path(self, point: Tuple[float, float], path: List[Tuple[float, float]]) -> float: """计算点到路径的最短距离""" min_dist = float('inf') for i in range(len(path) - 1): # 计算点到线段的距离 dist = self.point_to_line_distance(point, path[i], path[i+1]) min_dist = min(min_dist, dist) return min_dist def point_to_line_distance(self, point, line_start, line_end): """计算点到线段的距离""" # 向量计算实现 # ... ``` ### 4.4 与机器人系统集成 最后，我们需要把RRT规划器集成到实际的机器人系统中。以ROS（机器人操作系统）为例，通常的集成模式是： 1. **订阅传感器话题**：获取激光雷达、深度相机等传感器的障碍物信息。 2. **构建代价地图**：将传感器数据转换为规划空间。 3. **定期调用规划器**：根据当前位置和目标位置规划路径。 4. **发布路径话题**：将路径发送给控制器执行。 ```python # 伪代码，展示ROS中的集成思路 """ import rospy from nav_msgs.msg import Path, OccupancyGrid from geometry_msgs.msg import PoseStamped, Point class RRTROSNode: def __init__(self): rospy.init_node('rrt_planner') # 订阅地图和当前位置 self.map_sub = rospy.Subscriber('/map', OccupancyGrid, self.map_callback) self.pose_sub = rospy.Subscriber('/amcl_pose', PoseStamped, self.pose_callback) # 发布路径 self.path_pub = rospy.Publisher('/global_plan', Path, queue_size=10) # 规划器实例 self.planner = None self.current_map = None self.current_pose = None def map_callback(self, msg): # 将ROS地图消息转换为我们的PlanningSpace self.current_map = self.convert_map(msg) def pose_callback(self, msg): self.current_pose = (msg.pose.position.x, msg.pose.position.y) def plan_to_goal(self, goal_x, goal_y): if not self.current_map or not self.current_pose: rospy.logwarn("缺少地图或定位信息") return # 创建规划器 self.planner = RRTStarPlanner( space=self.current_map, start=self.current_pose, goal=(goal_x, goal_y), step_size=0.3, # 根据地图分辨率调整 max_iter=3000 ) # 执行规划 path = self.planner.plan() if path: # 转换为ROS Path消息并发布 ros_path = self.convert_to_ros_path(path) self.path_pub.publish(ros_path) rospy.loginfo("路径规划成功，发布 %d 个路径点", len(path)) else: rospy.logwarn("路径规划失败") def convert_map(self, occupancy_grid): # 将OccupancyGrid转换为PlanningSpace # 处理障碍物膨胀、未知区域等 pass def convert_to_ros_path(self, path): # 将我们的路径格式转换为ROS Path消息 pass """ ``` 在实际集成时，还需要考虑坐标变换、路径平滑（使用B样条或贝塞尔曲线）、速度规划等问题。RRT生成的路径通常由直线段组成，需要后处理才能让机器人平滑跟踪。 ## 5. 完整项目代码与扩展方向 为了方便大家直接使用，我把完整的项目代码整理成了一个Python包。你可以通过GitHub获取： ```bash git clone https://github.com/yourusername/rrt-path-planning.git cd rrt-path-planning pip install -e . ``` 项目结构如下： ``` rrt_path_planning/ ├── __init__.py ├── core/ │ ├── __init__.py │ ├── space.py # PlanningSpace类 │ ├── rrt.py # RRT和RRT*实现 │ └── utils.py # 工具函数 ├── examples/ │ ├── basic_demo.py # 基础演示 │ ├── comparison.py # 算法对比 │ └── dynamic_env.py # 动态环境演示 ├── tests/ # 单元测试 └── requirements.txt # 依赖列表 ``` ### 5.1 进一步扩展方向 如果你已经掌握了基础的RRT和RRT*，可以考虑以下扩展方向： **1. Informed RRT*** 在RRT* 的基础上，当找到第一条路径后，将采样限制在一个椭圆区域内，这个椭圆以起点和终点为焦点，以当前最佳路径长度为长轴。这能显著加快优化速度。 **2. RRT-Connect（双向RRT）** 同时从起点和终点生长两棵树，直到它们连接。这种方法在狭窄通道环境中特别有效。 **3. 自适应步长** 根据环境复杂度动态调整步长：在开阔区域用大步长快速探索，在狭窄区域用小步长精细搜索。 **4. 考虑运动约束** 标准的RRT假设机器人可以瞬间改变方向。对于汽车、无人机等有运动约束的机器人，需要扩展为Dubins RRT、Kinodynamic RRT等变体。 **5. 多目标规划** 同时优化路径长度、安全性、能耗等多个目标，可以使用多目标RRT*。 ### 5.2 实际项目中的注意事项 在真正的机器人项目中使用RRT时，我踩过几个坑，这里分享给大家： 1. **随机种子问题**：RRT的随机性可能导致测试时表现良好，但实际部署时偶尔规划失败。解决方法包括使用固定种子进行关键测试，或者实现一个回退机制（当RRT失败时切换到备用算法）。 2. **参数敏感度**：不同环境需要不同的参数。最好实现一个自动调参模块，根据历史规划成功率动态调整参数。 3. **实时性要求**：对于高速移动的机器人，规划必须在几十毫秒内完成。这时候可能需要牺牲最优性，使用更简单的算法，或者预计算路线图。 4. **内存管理**：长时间运行的规划器可能积累大量节点，需要定期清理远离当前区域的节点。 5. **与局部规划器配合**：RRT通常作为全局规划器，还需要一个局部规划器（如DWA、TEB）处理动态障碍物和跟踪误差。 我在一个仓储机器人项目中的做法是：用RRT* 做全局规划，每5秒或在环境变化时重规划一次；用模型预测控制（MPC）做局部跟踪；当RRT* 规划失败时，自动切换到基于A* 的栅格规划器作为后备。这种组合在实际运行中表现相当稳定。 最后，虽然RRT系列算法很强大，但它不是银弹。对于特别复杂的环境，可能需要结合其他技术，比如机器学习辅助的采样、语义地图引导等。路径规划是一个持续发展的领域，保持学习和实验的心态很重要。