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

RRT算法实战用Python从零实现机器人路径规划在机器人导航和自动驾驶领域路径规划是核心挑战之一。想象一下当你需要让机器人从客厅的沙发移动到厨房的冰箱前它需要避开茶几、宠物和散落的玩具——这就是路径规划要解决的问题。而RRT快速探索随机树算法正是解决这类问题的利器。不同于传统的A*或Dijkstra算法RRT特别适合处理高维空间和非完整约束下的路径规划问题。它不需要预先构建完整的地图而是通过随机采样和树形扩展来探索环境。这种特性使得RRT在复杂、动态环境中表现出色从工业机械臂到火星探测车都能看到它的身影。本文将带你从零开始实现RRT算法不仅会深入解析其工作原理还会提供完整的Python代码实现。我们会重点关注算法核心逻辑的Python实现关键参数的调优技巧路径平滑和优化的方法实时可视化展示算法运行过程1. 环境搭建与基础实现1.1 初始化环境首先我们需要创建一个二维空间来表示机器人的工作环境。这个环境将包含障碍物、起点和终点位置。以下是创建基础环境的代码import numpy as np import matplotlib.pyplot as plt from matplotlib.patches import Rectangle class Environment: def __init__(self, width, height): self.width width self.height height self.obstacles [] def add_obstacle(self, x, y, w, h): self.obstacles.append((x, y, w, h)) def is_collision_free(self, point): x, y point for (ox, oy, w, h) in self.obstacles: if ox x oxw and oy y oyh: return False return 0 x self.width and 0 y self.height1.2 RRT节点与树结构RRT算法的核心是构建一棵探索树。我们需要定义树的节点和基本操作class Node: def __init__(self, point): self.point point # (x,y)坐标 self.parent None self.children [] def add_child(self, child_node): self.children.append(child_node) child_node.parent self class RRTTree: def __init__(self, start_point): self.root Node(start_point) self.nodes [self.root] def add_node(self, point, parent_node): new_node Node(point) parent_node.add_child(new_node) self.nodes.append(new_node) return new_node def nearest_neighbor(self, point): min_dist float(inf) nearest_node None for node in self.nodes: dist np.linalg.norm(np.array(node.point) - np.array(point)) if dist min_dist: min_dist dist nearest_node node return nearest_node2. RRT核心算法实现2.1 基本RRT算法流程现在我们可以实现RRT的核心算法了。以下是算法的基本步骤初始化树仅包含起点随机采样空间中的一个点在树中找到距离采样点最近的节点从最近节点向采样点方向延伸一个步长检查新节点是否与障碍物碰撞若无碰撞将新节点加入树中重复2-6步直到新节点接近目标点def basic_rrt(env, start, goal, max_iter1000, step_size20): tree RRTTree(start) for _ in range(max_iter): # 随机采样 if np.random.rand() 0.1: # 10%概率直接采样目标点 rand_point goal else: rand_point (np.random.rand()*env.width, np.random.rand()*env.height) # 找到最近节点 nearest_node tree.nearest_neighbor(rand_point) # 计算新节点方向 direction np.array(rand_point) - np.array(nearest_node.point) distance np.linalg.norm(direction) if distance 0: direction direction / distance new_point np.array(nearest_node.point) direction * min(step_size, distance) new_point tuple(new_point) # 碰撞检测 if env.is_collision_free(new_point): new_node tree.add_node(new_point, nearest_node) # 检查是否到达目标 if np.linalg.norm(np.array(new_point) - np.array(goal)) step_size: path [] current new_node while current: path.append(current.point) current current.parent return tree, path[::-1] # 反转路径 return tree, None # 未找到路径2.2 可视化实现为了直观理解算法运行过程我们可以添加可视化功能def plot_rrt(env, tree, pathNone): plt.figure(figsize(10, 10)) # 绘制障碍物 for (x, y, w, h) in env.obstacles: plt.gca().add_patch(Rectangle((x, y), w, h, colorgray)) # 绘制树结构 for node in tree.nodes: if node.parent: plt.plot([node.point[0], node.parent.point[0]], [node.point[1], node.parent.point[1]], b-, alpha0.3) # 绘制路径 if path: plt.plot([p[0] for p in path], [p[1] for p in path], r-, linewidth2) plt.xlim(0, env.width) plt.ylim(0, env.height) plt.grid(True) plt.show()3. 算法优化与调参技巧3.1 关键参数影响分析RRT算法的性能很大程度上取决于几个关键参数参数典型值范围影响调优建议步长(step_size)5-50步长越大探索越快但可能错过狭窄通道根据环境复杂度调整复杂环境用小步长最大迭代次数(max_iter)500-5000迭代次数越多找到路径概率越高但耗时增加根据环境大小调整大环境需要更多迭代目标偏向概率0.05-0.2越高越倾向于向目标生长但可能陷入局部最优通常设为5%-10%连接半径1.5-3倍步长影响最终路径优化程度后期优化时使用初始实现可不考虑3.2 RRT*优化算法基本RRT找到的路径往往不够优化我们可以实现RRT算法来获得更优路径。RRT在RRT基础上增加了两个关键步骤邻近节点选择在新节点周围一定半径内寻找多个可能的父节点重布线优化尝试用新节点优化已有节点的父节点def rrt_star(env, start, goal, max_iter2000, step_size20, connect_radius30): tree RRTTree(start) for _ in range(max_iter): # 随机采样与基本RRT相同 if np.random.rand() 0.1: rand_point goal else: rand_point (np.random.rand()*env.width, np.random.rand()*env.height) # 找到最近节点 nearest_node tree.nearest_neighbor(rand_point) # 计算新节点 direction np.array(rand_point) - np.array(nearest_node.point) distance np.linalg.norm(direction) if distance 0: direction direction / distance new_point np.array(nearest_node.point) direction * min(step_size, distance) new_point tuple(new_point) if not env.is_collision_free(new_point): continue # 寻找邻近节点 neighbor_nodes [] for node in tree.nodes: if np.linalg.norm(np.array(node.point) - np.array(new_point)) connect_radius: neighbor_nodes.append(node) # 选择最优父节点 min_cost float(inf) best_parent None for node in neighbor_nodes: # 计算从起点经该节点到新节点的成本 cost node_cost(node) np.linalg.norm(np.array(node.point) - np.array(new_point)) if cost min_cost and is_path_clear(env, node.point, new_point): min_cost cost best_parent node if best_parent is None: best_parent nearest_node # 添加新节点 new_node tree.add_node(new_point, best_parent) # 重布线 for node in neighbor_nodes: if node best_parent: continue # 计算经新节点到该节点的成本 new_cost node_cost(new_node) np.linalg.norm(np.array(new_node.point) - np.array(node.point)) if new_cost node_cost(node) and is_path_clear(env, new_node.point, node.point): # 改变父节点 if node.parent: node.parent.children.remove(node) node.parent new_node new_node.children.append(node) # 检查是否到达目标 if np.linalg.norm(np.array(new_point) - np.array(goal)) step_size: path [] current new_node while current: path.append(current.point) current current.parent return tree, path[::-1] return tree, None def node_cost(node): cost 0 while node.parent: cost np.linalg.norm(np.array(node.point) - np.array(node.parent.point)) node node.parent return cost def is_path_clear(env, p1, p2, num_checks10): for i in range(num_checks1): t i / num_checks x p1[0] * (1-t) p2[0] * t y p1[1] * (1-t) p2[1] * t if not env.is_collision_free((x, y)): return False return True4. 实际应用与性能优化4.1 动态障碍物处理在实际应用中环境往往是动态变化的。我们可以通过以下策略使RRT适应动态环境定期重新规划每隔一定时间或当检测到环境变化时重新运行RRT增量式更新在原有树结构基础上继续扩展而不是完全重新构建局部修复只重新规划受障碍物影响的部分路径def dynamic_rrt(env, start, goal, obstacles_changed_func, max_iter1000, step_size20, replan_interval10): tree RRTTree(start) path None iterations_since_last_replan 0 for i in range(max_iter): iterations_since_last_replan 1 # 检查是否需要重新规划 if iterations_since_last_replan replan_interval and obstacles_changed_func(): # 保留部分树结构移除远离当前位置的节点 current_pos get_robot_position() # 假设有这个函数 tree prune_tree(tree, current_pos) iterations_since_last_replan 0 # 其余部分与基本RRT相同 if np.random.rand() 0.1: rand_point goal else: rand_point (np.random.rand()*env.width, np.random.rand()*env.height) nearest_node tree.nearest_neighbor(rand_point) direction np.array(rand_point) - np.array(nearest_node.point) distance np.linalg.norm(direction) if distance 0: direction direction / distance new_point np.array(nearest_node.point) direction * min(step_size, distance) new_point tuple(new_point) if env.is_collision_free(new_point): new_node tree.add_node(new_point, nearest_node) if np.linalg.norm(np.array(new_point) - np.array(goal)) step_size: path [] current new_node while current: path.append(current.point) current current.parent return tree, path[::-1] return tree, path4.2 性能优化技巧当处理大型环境或实时应用时RRT的性能至关重要。以下是一些优化技巧KD-Tree加速最近邻搜索使用空间分区数据结构大幅提高最近邻查询速度并行化采样在多核CPU上并行处理多个采样点自适应步长根据环境复杂度动态调整步长缓存碰撞检测结果避免重复计算相同区域的碰撞检测from scipy.spatial import KDTree class FastRRTTree(RRTTree): def __init__(self, start_point): super().__init__(start_point) self.kd_tree KDTree([start_point]) self.points [start_point] def add_node(self, point, parent_node): new_node super().add_node(point, parent_node) self.points.append(point) self.kd_tree KDTree(self.points) # 简单实现实际应增量更新 return new_node def nearest_neighbor(self, point): _, idx self.kd_tree.query(point) return self.nodes[idx]5. 完整案例演示让我们通过一个完整的例子演示如何使用上述代码实现路径规划# 创建环境 env Environment(800, 600) env.add_obstacle(200, 150, 50, 300) env.add_obstacle(400, 250, 50, 300) env.add_obstacle(600, 150, 50, 300) # 定义起点和终点 start (50, 300) goal (750, 300) # 运行RRT算法 tree, path basic_rrt(env, start, goal, max_iter2000, step_size30) # 可视化结果 plot_rrt(env, tree, path) # 运行RRT*算法 tree_star, path_star rrt_star(env, start, goal, max_iter2000, step_size30, connect_radius50) # 比较两种算法的路径长度 if path and path_star: def path_length(p): return sum(np.linalg.norm(np.array(p[i])-np.array(p[i-1])) for i in range(1,len(p))) print(f基本RRT路径长度: {path_length(path):.2f}) print(fRRT*路径长度: {path_length(path_star):.2f})在实际项目中RRT算法的参数需要根据具体场景进行调整。例如在狭窄通道较多的环境中应该使用较小的步长而在开阔区域可以使用较大步长加快探索速度。