RRT算法实战：从零开始用Python实现机器人路径规划（附完整代码）

RRT算法实战：从零开始用Python实现机器人路径规划

在机器人导航和自动驾驶领域，路径规划是让机器人在复杂环境中自主移动的核心技术。而RRT（快速扩展随机树）算法因其简单高效的特点，成为解决高维空间路径规划问题的利器。本文将带你用Python从零实现RRT算法，并通过可视化直观理解其工作原理。

1. 环境准备与基础概念

在开始编码之前，我们需要先搭建开发环境并理解RRT算法的基本思想。RRT算法通过在地图中随机采样点并逐步扩展树结构来寻找路径，特别适合处理高维空间和非完整约束的路径规划问题。

1.1 安装必要的Python库

我们将使用以下Python库来实现RRT算法：

pip install numpy matplotlib

• numpy：用于高效的数学运算和矩阵操作
• matplotlib：用于路径和地图的可视化展示

1.2 RRT算法核心思想

RRT算法的基本流程可以概括为：

1. 初始化树结构，起点作为树的根节点
2. 在地图空间内随机采样一个点
3. 在现有树中找到距离采样点最近的节点
4. 从最近节点向采样点方向扩展一步
5. 检查新节点是否与障碍物碰撞
6. 若无碰撞，则将新节点加入树中
7. 重复上述过程直到到达目标点附近

RRT算法的优势在于它不需要对环境进行精确建模，能够有效处理高维空间问题，并且计算复杂度与空间维度呈线性关系。

2. 构建基础RRT算法实现

现在让我们开始用Python实现基础RRT算法。我们将分步骤构建各个功能模块。

2.1 地图表示与障碍物检测

首先需要定义地图环境和障碍物检测方法：

import numpy as np import matplotlib.pyplot as plt from matplotlib.patches import Rectangle class Map: def __init__(self, width, height, obstacles): self.width = width self.height = height self.obstacles = obstacles # 障碍物列表，每个障碍物表示为(x,y,width,height) def is_collision_free(self, point): x, y = point # 检查是否超出地图边界 if x < 0 or x > self.width or y < 0 or y > self.height: return False # 检查是否与任何障碍物碰撞 for (ox, oy, w, h) in self.obstacles: if ox <= x <= ox+w and oy <= y <= oy+h: return False return True

2.2 RRT算法核心实现

接下来实现RRT算法的主逻辑：

class RRT: def __init__(self, start, goal, map, step_size=5, max_iter=1000): self.start = np.array(start) self.goal = np.array(goal) self.map = map self.step_size = step_size self.max_iter = max_iter self.nodes = [self.start] # 树节点列表 self.parents = {0: -1} # 父节点索引映射 self.path = [] def sample_point(self): # 以10%概率采样目标点，加快收敛 if np.random.random() < 0.1: return self.goal # 在地图范围内随机采样 x = np.random.uniform(0, self.map.width) y = np.random.uniform(0, self.map.height) return np.array([x, y]) def find_nearest_node(self, point): distances = [np.linalg.norm(node - point) for node in self.nodes] nearest_idx = np.argmin(distances) return nearest_idx, self.nodes[nearest_idx] 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 plan(self): for i in range(self.max_iter): # 随机采样 rand_point = self.sample_point() # 找到最近节点 nearest_idx, nearest_node = self.find_nearest_node(rand_point) # 向采样点方向扩展 new_node = self.steer(nearest_node, rand_point) # 碰撞检测 if self.map.is_collision_free(new_node): self.nodes.append(new_node) new_idx = len(self.nodes) - 1 self.parents[new_idx] = nearest_idx # 检查是否到达目标附近 if np.linalg.norm(new_node - self.goal) <= self.step_size: self._extract_path(new_idx) return True return False def _extract_path(self, goal_idx): self.path = [] current_idx = goal_idx while current_idx != -1: self.path.append(self.nodes[current_idx]) current_idx = self.parents[current_idx] self.path.reverse()

3. 可视化与结果分析

实现算法后，我们需要可视化展示规划结果并分析算法性能。

3.1 可视化实现

def visualize(rrt, map): plt.figure(figsize=(10, 10)) # 绘制障碍物 for (x, y, w, h) in map.obstacles: plt.gca().add_patch(Rectangle((x, y), w, h, color='gray')) # 绘制树结构 for i, node in enumerate(rrt.nodes): if i == 0: continue # 跳过根节点 parent_node = rrt.nodes[rrt.parents[i]] plt.plot([node[0], parent_node[0]], [node[1], parent_node[1]], 'b-', alpha=0.3) # 绘制起点和终点 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') # 绘制路径 if rrt.path: path_x = [p[0] for p in rrt.path] path_y = [p[1] for p in rrt.path] plt.plot(path_x, path_y, 'r-', linewidth=2, label='Path') plt.xlim(0, map.width) plt.ylim(0, map.height) plt.legend() plt.grid(True) plt.title('RRT Path Planning') plt.show()

3.2 运行示例

让我们创建一个包含障碍物的地图并运行RRT算法：

# 定义地图和障碍物 obstacles = [ (20, 20, 20, 40), (60, 30, 20, 20), (40, 60, 30, 20) ] map = Map(100, 100, obstacles) # 初始化RRT rrt = RRT(start=(10, 10), goal=(90, 90), map=map, step_size=5, max_iter=1000) # 执行路径规划 success = rrt.plan() if success: print("Path found!") visualize(rrt, map) else: print("Failed to find path")

3.3 算法性能分析

RRT算法的性能受多个参数影响：

在实际应用中，可以通过以下方式优化RRT算法：

• 自适应步长：根据环境复杂度动态调整步长
• 双向RRT：同时从起点和目标点生长两棵树，加快收敛
• 路径平滑：对找到的路径进行后处理，去除不必要的转折

4. RRT算法变种与进阶实现

基础RRT算法虽然简单有效，但仍有一些局限性。下面介绍两种改进算法：RRT和Informed RRT。

4.1 RRT*算法实现

RRT*在RRT基础上增加了路径优化机制：

class RRTStar(RRT): def __init__(self, start, goal, map, step_size=5, max_iter=1000, search_radius=20): super().__init__(start, goal, map, step_size, max_iter) self.search_radius = search_radius self.costs = {0: 0} # 从起点到各节点的累积成本 def plan(self): for i in range(self.max_iter): rand_point = self.sample_point() nearest_idx, nearest_node = self.find_nearest_node(rand_point) new_node = self.steer(nearest_node, rand_point) if self.map.is_collision_free(new_node): # 在搜索半径内寻找邻近节点 neighbor_indices = self._find_near_nodes(new_node) # 选择最优父节点 best_idx = self._choose_best_parent(new_node, neighbor_indices) if best_idx is not None: self.nodes.append(new_node) new_idx = len(self.nodes) - 1 self.parents[new_idx] = best_idx self.costs[new_idx] = self.costs[best_idx] + np.linalg.norm(new_node - self.nodes[best_idx]) # 重布线优化 self._rewire(new_idx, neighbor_indices) # 检查是否到达目标 if np.linalg.norm(new_node - self.goal) <= self.step_size: self._extract_path(new_idx) return True return False def _find_near_nodes(self, node): neighbor_indices = [] for i, n in enumerate(self.nodes): if np.linalg.norm(n - node) <= self.search_radius: neighbor_indices.append(i) return neighbor_indices def _choose_best_parent(self, new_node, neighbor_indices): if not neighbor_indices: return None costs = [] for idx in neighbor_indices: cost = self.costs[idx] + np.linalg.norm(new_node - self.nodes[idx]) if self._check_collision(self.nodes[idx], new_node): costs.append(cost) else: costs.append(float('inf')) min_cost_idx = np.argmin(costs) if costs[min_cost_idx] != float('inf'): return neighbor_indices[min_cost_idx] return None def _rewire(self, new_idx, neighbor_indices): new_node = self.nodes[new_idx] for idx in neighbor_indices: if idx == new_idx: continue node = self.nodes[idx] new_cost = self.costs[new_idx] + np.linalg.norm(node - new_node) if new_cost < self.costs[idx] and self._check_collision(new_node, node): self.parents[idx] = new_idx self.costs[idx] = new_cost def _check_collision(self, from_node, to_node): # 简单的线性插值碰撞检测 steps = int(np.linalg.norm(to_node - from_node) / self.step_size) + 2 for t in np.linspace(0, 1, steps): point = from_node + t * (to_node - from_node) if not self.map.is_collision_free(point): return False return True

4.2 Informed RRT*算法优化

Informed RRT*在找到初始路径后，将采样限制在一个椭圆区域内：

class InformedRRTStar(RRTStar): def __init__(self, start, goal, map, step_size=5, max_iter=1000, search_radius=20): super().__init__(start, goal, map, step_size, max_iter, search_radius) self.best_path_cost = float('inf') self.best_path = None self.found_path = False def plan(self): for i in range(self.max_iter): if self.found_path: rand_point = self._sample_in_ellipse() else: rand_point = self.sample_point() nearest_idx, nearest_node = self.find_nearest_node(rand_point) new_node = self.steer(nearest_node, rand_point) if self.map.is_collision_free(new_node): neighbor_indices = self._find_near_nodes(new_node) best_idx = self._choose_best_parent(new_node, neighbor_indices) if best_idx is not None: self.nodes.append(new_node) new_idx = len(self.nodes) - 1 self.parents[new_idx] = best_idx self.costs[new_idx] = self.costs[best_idx] + np.linalg.norm(new_node - self.nodes[best_idx]) self._rewire(new_idx, neighbor_indices) # 检查是否到达目标 if np.linalg.norm(new_node - self.goal) <= self.step_size: path_cost = self.costs[new_idx] + np.linalg.norm(new_node - self.goal) if path_cost < self.best_path_cost: self.best_path_cost = path_cost self._extract_path(new_idx) self.best_path = self.path.copy() self.found_path = True return self.found_path def _sample_in_ellipse(self): # 基于当前最优路径的椭圆采样 c_min = np.linalg.norm(self.goal - self.start) c_max = self.best_path_cost if c_max <= c_min: return self.sample_point() center = (self.start + self.goal) / 2 rotation = self._rotation_to_standard_frame(self.start, self.goal) a = c_max / 2 b = np.sqrt(c_max**2 - c_min**2) / 2 # 在单位圆内采样 while True: r = np.random.uniform(0, 1) theta = np.random.uniform(0, 2*np.pi) x = r * np.cos(theta) y = r * np.sin(theta) if x**2 + y**2 <= 1: break # 变换到椭圆 x *= a y *= b point = np.array([x, y]) point = rotation.T @ point + center return point def _rotation_to_standard_frame(self, start, goal): # 计算旋转矩阵 d = goal - start L = np.linalg.norm(d) u = d / L return np.array([[u[0], -u[1]], [u[1], u[0]]])

4.3 算法对比与选择指南

三种RRT变体的特点比较：

选择建议：

• 快速原型验证：使用基础RRT
• 实际应用需要较优路径：选择RRT*
• 对路径质量要求高：使用Informed RRT*
• 计算资源有限：考虑RRT或限制RRT*的搜索半径

在实际机器人应用中，我通常会先使用RRT快速验证路径可行性，然后再切换到RRT或Informed RRT进行优化。对于动态环境，可以考虑结合局部规划器使用RRT作为全局规划器。