RRT*算法实战用Python给机器人规划最优避障路径附完整代码1. 机器人路径规划的核心挑战在机器人自主导航领域路径规划算法需要解决三个核心问题如何在复杂环境中快速找到可行路径如何确保路径的最优性以及如何应对动态变化的障碍物传统算法如A*和Dijkstra在低维空间中表现良好但在高维配置空间如机械臂的关节空间中会面临维度灾难。RRT*Rapidly-exploring Random Tree Star算法通过随机采样和渐进优化的方式有效解决了这些问题。与基础RRT相比RRT*引入了两个关键改进父节点重选机制新节点不仅连接最近邻节点还会在邻近节点中寻找能使路径成本更低的父节点重布线优化每次添加新节点后检查是否可以通过该节点优化周围节点的路径# 重布线核心代码示例 def rewire(self, new_node, neighbor_indices): for i in neighbor_indices: neighbor self.node_list[i] # 计算通过新节点到达邻居的成本 new_cost new_node.cost self.calc_distance(new_node, neighbor) # 如果新路径成本更低且无碰撞 if new_cost neighbor.cost and self.check_collision(new_node, neighbor): neighbor.parent new_node neighbor.cost new_cost # 递归更新子节点成本 self.update_children_cost(neighbor)2. RRT*算法实现详解2.1 算法核心组件完整的RRT*实现需要以下关键组件组件功能描述实现要点节点类存储位置、父节点和路径成本包含坐标、父节点指针和累计成本字段距离度量计算节点间距离通常使用欧氏距离可根据场景调整碰撞检测确保路径段不穿过障碍物需要根据障碍物表示方式实现采样策略生成随机探索点基础实现使用均匀采样可加入目标偏置邻近节点搜索查找优化范围内的节点使用KD树加速范围查询2.2 完整算法流程初始化创建包含起点节点的树随机采样在自由空间中生成随机点有概率直接采样目标点最近邻搜索找到树中距离采样点最近的节点新节点生成从最近节点向采样点方向延伸固定步长碰撞检测检查新节点路径是否与障碍物相交父节点选择在邻近节点中寻找最优父节点重布线优化尝试通过新节点优化周围节点的路径终止条件达到最大迭代次数或找到满足条件的路径# RRT*主循环代码框架 def plan(self): self.node_list [self.start] for _ in range(self.max_iter): # 随机采样 rnd self.get_random_point() # 最近邻搜索 nearest_ind self.get_nearest_node_index(rnd) nearest_node self.node_list[nearest_ind] # 新节点生成 new_node self.steer(nearest_node, rnd) # 碰撞检测 if self.check_collision(nearest_node, new_node): # 查找邻近节点 near_inds self.find_near_nodes(new_node) # 选择最优父节点 new_node self.choose_parent(new_node, near_inds) if new_node: self.node_list.append(new_node) # 重布线优化 self.rewire(new_node, near_inds) # 检查是否到达目标 if self.is_goal_reached(new_node): return self.generate_final_course() return None # 未找到路径3. 工程实践中的关键优化3.1 动态步长调整固定步长会导致在狭窄通道中规划失败率高。采用动态步长策略def get_dynamic_step(self, nearest_node, sample_point): base_step self.step_size # 计算到最近障碍物的距离 min_dist self.get_min_obstacle_distance(nearest_node) # 在狭窄区域减小步长 if min_dist self.safe_distance: return min(base_step, min_dist * 0.5) return base_step3.2 启发式采样策略纯随机采样效率低下可采用以下改进目标偏置采样以一定概率直接采样目标点障碍物边缘采样在障碍物附近增加采样密度自适应采样根据已探索区域调整采样分布def get_random_point(self): # 5%概率直接返回目标点 if random.random() 0.05: return self.goal # 20%概率在障碍物边缘采样 if random.random() 0.2 and self.obstacles: obs random.choice(self.obstacles) return self.sample_near_obstacle(obs) # 常规均匀采样 return self.uniform_sample()3.3 并行化加速RRT*的迭代相互独立适合并行化from concurrent.futures import ThreadPoolExecutor def parallel_rrt_star(start, goal, obstacles, num_threads4): trees [Tree(start, goal) for _ in range(num_threads)] with ThreadPoolExecutor() as executor: futures [executor.submit(tree.grow, obstacles) for tree in trees] while True: for future in futures: if future.done() and future.result(): return future.result()4. ROS集成实战案例将RRT*集成到ROS导航栈中实现真实机器人避障4.1 系统架构ROS节点结构 - /rrt_star_planner (规划器节点) - 订阅/map (导航地图) - 订阅/odom (里程计) - 订阅/goal (目标位姿) - 发布/path (规划路径) - /move_base (控制节点)4.2 关键实现步骤地图处理将ROS的OccupancyGrid转换为算法可用的障碍物表示位姿转换处理坐标系转换map→odom→base_link实时重规划设置规划超时和定期重规划机制路径平滑对原始RRT*路径进行后处理# ROS集成示例代码 class RRTStarPlanner: def __init__(self): rospy.init_node(rrt_star_planner) self.map None self.start None self.goal None # ROS订阅和发布 self.map_sub rospy.Subscriber(/map, OccupancyGrid, self.map_cb) self.goal_sub rospy.Subscriber(/move_base_simple/goal, PoseStamped, self.goal_cb) self.path_pub rospy.Publisher(/global_plan, Path, queue_size1) def map_cb(self, msg): 处理地图数据 self.map msg self.resolution msg.info.resolution self.origin msg.info.origin def plan(self): 执行路径规划 if not self.map or not self.goal: return None # 转换起点和终点 start self.pose_to_grid(self.start) goal self.pose_to_grid(self.goal) # 提取障碍物 obstacles self.extract_obstacles() # 执行RRT*规划 path RRTStar(start, goal, obstacles).plan() # 转换为ROS Path消息 ros_path self.grid_to_path(path) self.path_pub.publish(ros_path) def run(self): rate rospy.Rate(10) # 10Hz while not rospy.is_shutdown(): self.plan() rate.sleep()5. 性能优化与参数调优5.1 关键参数影响参数影响推荐值最大迭代次数规划时间和路径质量5000-20000步长探索速度与精细度环境尺度的5-10%重布线半径优化范围与计算量步长的2-3倍目标偏置概率收敛速度5-10%碰撞检测精度安全性与计算成本根据机器人尺寸调整5.2 常见问题解决方案问题1狭窄通道难以通过解决方案减小步长增加采样密度或在通道入口添加引导点问题2规划时间过长解决方案使用KD树加速邻近搜索降低碰撞检测精度或设置超时返回次优解问题3动态障碍物处理解决方案定期重规划结合速度障碍物法进行局部调整# 动态障碍物处理示例 def dynamic_replan(self): while self.running: # 获取最新障碍物信息 current_obstacles self.get_obstacles() # 检查当前路径是否仍然安全 if not self.is_path_safe(self.current_path, current_obstacles): # 执行重规划 new_path self.plan(current_obstacles) if new_path: self.current_path new_path time.sleep(0.1) # 控制重规划频率6. 完整代码实现以下是一个完整可运行的RRT*实现包含可视化功能import numpy as np import matplotlib.pyplot as plt import random import math from scipy.spatial import KDTree class Node: RRT*树节点类 def __init__(self, x, y): self.x x self.y y self.cost 0.0 # 从起点到该节点的路径成本 self.parent None class RRTStar: RRT*路径规划算法实现 def __init__(self, start, goal, obstacle_list, rand_area, expand_dis0.5, path_resolution0.1, goal_sample_rate5, max_iter5000, connect_circle_dist1.5, robot_radius0.2): 初始化参数 :param start: 起点坐标 [x,y] :param goal: 目标点坐标 [x,y] :param obstacle_list: 障碍物列表 [[x,y,size],...] :param rand_area: 采样区域 [[x_min,x_max], [y_min,y_max]] :param expand_dis: 扩展步长 :param path_resolution: 路径分辨率 :param goal_sample_rate: 目标采样率(百分比) :param max_iter: 最大迭代次数 :param connect_circle_dist: 重布线半径 :param robot_radius: 机器人半径(用于碰撞检测) self.start Node(start[0], start[1]) self.goal Node(goal[0], goal[1]) self.obstacle_list obstacle_list self.rand_area rand_area self.expand_dis expand_dis self.path_resolution path_resolution self.goal_sample_rate goal_sample_rate self.max_iter max_iter self.connect_circle_dist connect_circle_dist self.robot_radius robot_radius self.node_list [] def planning(self, animationTrue): 执行路径规划 self.node_list [self.start] for i in range(self.max_iter): # 随机采样 rnd self.get_random_node() # 获取最近节点 nearest_ind self.get_nearest_node_index(rnd) nearest_node self.node_list[nearest_ind] # 向随机点方向扩展 new_node self.steer(nearest_node, rnd) # 碰撞检测 if self.check_collision(new_node): # 查找邻近节点 near_inds self.find_near_nodes(new_node) # 选择最优父节点 new_node self.choose_parent(new_node, near_inds) if new_node: self.node_list.append(new_node) # 重布线优化 self.rewire(new_node, near_inds) # 绘制动画 if animation and i % 100 0: self.draw_graph(rnd) # 生成最终路径 last_index self.search_best_goal_node() if last_index is None: return None path self.generate_final_course(last_index) return path def steer(self, from_node, to_node): 从from_node向to_node方向扩展 dist self.calc_distance(from_node, to_node) if dist self.expand_dis: return to_node theta math.atan2(to_node.y - from_node.y, to_node.x - from_node.x) new_node Node(from_node.x self.expand_dis * math.cos(theta), from_node.y self.expand_dis * math.sin(theta)) new_node.cost from_node.cost self.expand_dis new_node.parent from_node return new_node def choose_parent(self, new_node, near_inds): 在邻近节点中选择最优父节点 if not near_inds: return None # 搜索成本最低的邻近节点 costs [] for i in near_inds: near_node self.node_list[i] # 检查路径是否无碰撞 if self.check_collision_between(near_node, new_node): costs.append(near_node.cost self.calc_distance(near_node, new_node)) else: costs.append(float(inf)) min_cost min(costs) if min_cost float(inf): return None min_ind near_inds[costs.index(min_cost)] # 更新新节点的父节点和成本 new_node.cost min_cost new_node.parent self.node_list[min_ind] return new_node def rewire(self, new_node, near_inds): 重布线优化 for i in near_inds: near_node self.node_list[i] # 计算通过新节点的成本 new_cost new_node.cost self.calc_distance(new_node, near_node) # 如果新路径更优且无碰撞 if new_cost near_node.cost and self.check_collision_between(new_node, near_node): near_node.parent new_node near_node.cost new_cost # 递归更新子节点成本 self.propagate_cost_to_leaves(near_node) def propagate_cost_to_leaves(self, parent_node): 递归更新子节点成本 for node in self.node_list: if node.parent parent_node: node.cost parent_node.cost self.calc_distance(parent_node, node) self.propagate_cost_to_leaves(node) def search_best_goal_node(self): 搜索最佳目标节点 goal_inds [] for (i, node) in enumerate(self.node_list): if self.calc_distance(node, self.goal) self.expand_dis: goal_inds.append(i) # 检查是否可达 safe_goal_inds [] for i in goal_inds: t_node self.steer(self.node_list[i], self.goal) if self.check_collision(t_node): safe_goal_inds.append(i) if not safe_goal_inds: return None # 选择成本最低的节点 min_cost min([self.node_list[i].cost for i in safe_goal_inds]) for i in safe_goal_inds: if self.node_list[i].cost min_cost: return i return None def generate_final_course(self, goal_ind): 生成最终路径 path [[self.goal.x, self.goal.y]] node self.node_list[goal_ind] while node.parent is not None: path.append([node.x, node.y]) node node.parent path.append([node.x, node.y]) return path[::-1] # 反转路径 def calc_distance(self, from_node, to_node): 计算节点间距离 return math.hypot(from_node.x - to_node.x, from_node.y - to_node.y) def get_random_node(self): 获取随机节点含目标偏置 if random.randint(0, 100) self.goal_sample_rate: rnd Node(random.uniform(self.rand_area[0], self.rand_area[1]), random.uniform(self.rand_area[2], self.rand_area[3])) else: # 目标偏置采样 rnd Node(self.goal.x, self.goal.y) return rnd def get_nearest_node_index(self, rnd_node): 获取最近节点索引 dlist [self.calc_distance(node, rnd_node) for node in self.node_list] return dlist.index(min(dlist)) def find_near_nodes(self, new_node): 查找邻近节点 nnode len(self.node_list) 1 r self.connect_circle_dist * math.sqrt(math.log(nnode) / nnode) dlist [self.calc_distance(new_node, node) for node in self.node_list] near_inds [i for i, d in enumerate(dlist) if d r] return near_inds def check_collision(self, node): 检查节点是否与障碍物碰撞 for (ox, oy, size) in self.obstacle_list: dx ox - node.x dy oy - node.y d math.hypot(dx, dy) if d size self.robot_radius: return False # 碰撞 return True # 安全 def check_collision_between(self, from_node, to_node): 检查两节点间路径是否无碰撞 if not self.check_collision(from_node) or not self.check_collision(to_node): return False # 沿路径采样检查 dist self.calc_distance(from_node, to_node) step_num int(dist / self.path_resolution) for i in range(step_num): ratio i / step_num x from_node.x ratio * (to_node.x - from_node.x) y from_node.y ratio * (to_node.y - from_node.y) temp_node Node(x, y) if not self.check_collision(temp_node): return False return True def draw_graph(self, rndNone): 绘制搜索过程 plt.clf() if rnd is not None: plt.plot(rnd.x, rnd.y, ^k) for node in self.node_list: if node.parent: plt.plot([node.x, node.parent.x], [node.y, node.parent.y], -g) for (ox, oy, size) in self.obstacle_list: circle plt.Circle((ox, oy), size, colork, fillTrue) plt.gca().add_patch(circle) plt.plot(self.start.x, self.start.y, xr) plt.plot(self.goal.x, self.goal.y, xr) plt.axis([self.rand_area[0], self.rand_area[1], self.rand_area[2], self.rand_area[3]]) plt.grid(True) plt.pause(0.01) # 示例使用 def main(): print(RRT*路径规划开始) # 障碍物列表 [x, y, radius] obstacle_list [ (5, 5, 1), (3, 6, 1), (3, 8, 1), (7, 9, 1), (9, 5, 1), (8, 7, 1), (2, 3, 1), (4, 7, 1), (6, 2, 1), (7, 5, 1) ] # 创建RRT*规划器 rrt_star RRTStar(start[0, 0], goal[10, 10], rand_area[-2, 12, -2, 12], obstacle_listobstacle_list) # 执行规划 path rrt_star.planning(animationTrue) # 绘制最终路径 if path: plt.clf() rrt_star.draw_graph() plt.plot([x for (x, y) in path], [y for (x, y) in path], -r) plt.grid(True) plt.pause(0.01) plt.show() else: print(无法找到可行路径) if __name__ __main__: main()7. 进阶扩展方向7.1 三维空间扩展将RRT*扩展到三维空间需要考虑三维碰撞检测使用八叉树或KD树加速空间查询姿态约束考虑机器人的姿态和动力学限制可视化挑战三维路径的可视化呈现class Node3D: 三维节点类 def __init__(self, x, y, z): self.x x self.y y self.z z self.cost 0.0 self.parent None def check_collision_3d(node, obstacle_tree): 三维碰撞检测 dist, _ obstacle_tree.query([node.x, node.y, node.z]) return dist robot_radius7.2 动态环境适应针对动态障碍物的改进策略增量式规划在原有搜索树上继续扩展而非重新构建局部调整对受影响路径段进行局部重规划速度障碍物法结合VO进行动态避障def dynamic_replan(self, new_obstacles): 动态重规划 # 更新障碍物信息 self.obstacle_list new_obstacles # 保留部分原有节点 kept_nodes self.filter_nodes_in_free_space() self.node_list kept_nodes [self.start] # 执行规划 return self.planning(animationFalse)7.3 多机器人协同规划多机器人系统中的RRT*应用优先级规划为机器人分配不同优先级顺序规划时空路径在时间维度上扩展状态空间冲突检测检查路径在时空上的冲突def multi_robot_planning(robots, obstacles): 多机器人协同规划 paths {} for i, robot in enumerate(robots): # 将其他机器人的路径视为动态障碍物 other_paths [path for j, path in paths.items() if j ! i] dynamic_obs obstacles convert_paths_to_obstacles(other_paths) # 为当前机器人规划路径 planner RRTStar(robot.start, robot.goal, dynamic_obs, robot.area) paths[i] planner.plan() return paths
RRT*算法实战：用Python给机器人规划最优避障路径（附完整代码）
发布时间：2026/5/22 7:04:52
RRT*算法实战用Python给机器人规划最优避障路径附完整代码1. 机器人路径规划的核心挑战在机器人自主导航领域路径规划算法需要解决三个核心问题如何在复杂环境中快速找到可行路径如何确保路径的最优性以及如何应对动态变化的障碍物传统算法如A*和Dijkstra在低维空间中表现良好但在高维配置空间如机械臂的关节空间中会面临维度灾难。RRT*Rapidly-exploring Random Tree Star算法通过随机采样和渐进优化的方式有效解决了这些问题。与基础RRT相比RRT*引入了两个关键改进父节点重选机制新节点不仅连接最近邻节点还会在邻近节点中寻找能使路径成本更低的父节点重布线优化每次添加新节点后检查是否可以通过该节点优化周围节点的路径# 重布线核心代码示例 def rewire(self, new_node, neighbor_indices): for i in neighbor_indices: neighbor self.node_list[i] # 计算通过新节点到达邻居的成本 new_cost new_node.cost self.calc_distance(new_node, neighbor) # 如果新路径成本更低且无碰撞 if new_cost neighbor.cost and self.check_collision(new_node, neighbor): neighbor.parent new_node neighbor.cost new_cost # 递归更新子节点成本 self.update_children_cost(neighbor)2. RRT*算法实现详解2.1 算法核心组件完整的RRT*实现需要以下关键组件组件功能描述实现要点节点类存储位置、父节点和路径成本包含坐标、父节点指针和累计成本字段距离度量计算节点间距离通常使用欧氏距离可根据场景调整碰撞检测确保路径段不穿过障碍物需要根据障碍物表示方式实现采样策略生成随机探索点基础实现使用均匀采样可加入目标偏置邻近节点搜索查找优化范围内的节点使用KD树加速范围查询2.2 完整算法流程初始化创建包含起点节点的树随机采样在自由空间中生成随机点有概率直接采样目标点最近邻搜索找到树中距离采样点最近的节点新节点生成从最近节点向采样点方向延伸固定步长碰撞检测检查新节点路径是否与障碍物相交父节点选择在邻近节点中寻找最优父节点重布线优化尝试通过新节点优化周围节点的路径终止条件达到最大迭代次数或找到满足条件的路径# RRT*主循环代码框架 def plan(self): self.node_list [self.start] for _ in range(self.max_iter): # 随机采样 rnd self.get_random_point() # 最近邻搜索 nearest_ind self.get_nearest_node_index(rnd) nearest_node self.node_list[nearest_ind] # 新节点生成 new_node self.steer(nearest_node, rnd) # 碰撞检测 if self.check_collision(nearest_node, new_node): # 查找邻近节点 near_inds self.find_near_nodes(new_node) # 选择最优父节点 new_node self.choose_parent(new_node, near_inds) if new_node: self.node_list.append(new_node) # 重布线优化 self.rewire(new_node, near_inds) # 检查是否到达目标 if self.is_goal_reached(new_node): return self.generate_final_course() return None # 未找到路径3. 工程实践中的关键优化3.1 动态步长调整固定步长会导致在狭窄通道中规划失败率高。采用动态步长策略def get_dynamic_step(self, nearest_node, sample_point): base_step self.step_size # 计算到最近障碍物的距离 min_dist self.get_min_obstacle_distance(nearest_node) # 在狭窄区域减小步长 if min_dist self.safe_distance: return min(base_step, min_dist * 0.5) return base_step3.2 启发式采样策略纯随机采样效率低下可采用以下改进目标偏置采样以一定概率直接采样目标点障碍物边缘采样在障碍物附近增加采样密度自适应采样根据已探索区域调整采样分布def get_random_point(self): # 5%概率直接返回目标点 if random.random() 0.05: return self.goal # 20%概率在障碍物边缘采样 if random.random() 0.2 and self.obstacles: obs random.choice(self.obstacles) return self.sample_near_obstacle(obs) # 常规均匀采样 return self.uniform_sample()3.3 并行化加速RRT*的迭代相互独立适合并行化from concurrent.futures import ThreadPoolExecutor def parallel_rrt_star(start, goal, obstacles, num_threads4): trees [Tree(start, goal) for _ in range(num_threads)] with ThreadPoolExecutor() as executor: futures [executor.submit(tree.grow, obstacles) for tree in trees] while True: for future in futures: if future.done() and future.result(): return future.result()4. ROS集成实战案例将RRT*集成到ROS导航栈中实现真实机器人避障4.1 系统架构ROS节点结构 - /rrt_star_planner (规划器节点) - 订阅/map (导航地图) - 订阅/odom (里程计) - 订阅/goal (目标位姿) - 发布/path (规划路径) - /move_base (控制节点)4.2 关键实现步骤地图处理将ROS的OccupancyGrid转换为算法可用的障碍物表示位姿转换处理坐标系转换map→odom→base_link实时重规划设置规划超时和定期重规划机制路径平滑对原始RRT*路径进行后处理# ROS集成示例代码 class RRTStarPlanner: def __init__(self): rospy.init_node(rrt_star_planner) self.map None self.start None self.goal None # ROS订阅和发布 self.map_sub rospy.Subscriber(/map, OccupancyGrid, self.map_cb) self.goal_sub rospy.Subscriber(/move_base_simple/goal, PoseStamped, self.goal_cb) self.path_pub rospy.Publisher(/global_plan, Path, queue_size1) def map_cb(self, msg): 处理地图数据 self.map msg self.resolution msg.info.resolution self.origin msg.info.origin def plan(self): 执行路径规划 if not self.map or not self.goal: return None # 转换起点和终点 start self.pose_to_grid(self.start) goal self.pose_to_grid(self.goal) # 提取障碍物 obstacles self.extract_obstacles() # 执行RRT*规划 path RRTStar(start, goal, obstacles).plan() # 转换为ROS Path消息 ros_path self.grid_to_path(path) self.path_pub.publish(ros_path) def run(self): rate rospy.Rate(10) # 10Hz while not rospy.is_shutdown(): self.plan() rate.sleep()5. 性能优化与参数调优5.1 关键参数影响参数影响推荐值最大迭代次数规划时间和路径质量5000-20000步长探索速度与精细度环境尺度的5-10%重布线半径优化范围与计算量步长的2-3倍目标偏置概率收敛速度5-10%碰撞检测精度安全性与计算成本根据机器人尺寸调整5.2 常见问题解决方案问题1狭窄通道难以通过解决方案减小步长增加采样密度或在通道入口添加引导点问题2规划时间过长解决方案使用KD树加速邻近搜索降低碰撞检测精度或设置超时返回次优解问题3动态障碍物处理解决方案定期重规划结合速度障碍物法进行局部调整# 动态障碍物处理示例 def dynamic_replan(self): while self.running: # 获取最新障碍物信息 current_obstacles self.get_obstacles() # 检查当前路径是否仍然安全 if not self.is_path_safe(self.current_path, current_obstacles): # 执行重规划 new_path self.plan(current_obstacles) if new_path: self.current_path new_path time.sleep(0.1) # 控制重规划频率6. 完整代码实现以下是一个完整可运行的RRT*实现包含可视化功能import numpy as np import matplotlib.pyplot as plt import random import math from scipy.spatial import KDTree class Node: RRT*树节点类 def __init__(self, x, y): self.x x self.y y self.cost 0.0 # 从起点到该节点的路径成本 self.parent None class RRTStar: RRT*路径规划算法实现 def __init__(self, start, goal, obstacle_list, rand_area, expand_dis0.5, path_resolution0.1, goal_sample_rate5, max_iter5000, connect_circle_dist1.5, robot_radius0.2): 初始化参数 :param start: 起点坐标 [x,y] :param goal: 目标点坐标 [x,y] :param obstacle_list: 障碍物列表 [[x,y,size],...] :param rand_area: 采样区域 [[x_min,x_max], [y_min,y_max]] :param expand_dis: 扩展步长 :param path_resolution: 路径分辨率 :param goal_sample_rate: 目标采样率(百分比) :param max_iter: 最大迭代次数 :param connect_circle_dist: 重布线半径 :param robot_radius: 机器人半径(用于碰撞检测) self.start Node(start[0], start[1]) self.goal Node(goal[0], goal[1]) self.obstacle_list obstacle_list self.rand_area rand_area self.expand_dis expand_dis self.path_resolution path_resolution self.goal_sample_rate goal_sample_rate self.max_iter max_iter self.connect_circle_dist connect_circle_dist self.robot_radius robot_radius self.node_list [] def planning(self, animationTrue): 执行路径规划 self.node_list [self.start] for i in range(self.max_iter): # 随机采样 rnd self.get_random_node() # 获取最近节点 nearest_ind self.get_nearest_node_index(rnd) nearest_node self.node_list[nearest_ind] # 向随机点方向扩展 new_node self.steer(nearest_node, rnd) # 碰撞检测 if self.check_collision(new_node): # 查找邻近节点 near_inds self.find_near_nodes(new_node) # 选择最优父节点 new_node self.choose_parent(new_node, near_inds) if new_node: self.node_list.append(new_node) # 重布线优化 self.rewire(new_node, near_inds) # 绘制动画 if animation and i % 100 0: self.draw_graph(rnd) # 生成最终路径 last_index self.search_best_goal_node() if last_index is None: return None path self.generate_final_course(last_index) return path def steer(self, from_node, to_node): 从from_node向to_node方向扩展 dist self.calc_distance(from_node, to_node) if dist self.expand_dis: return to_node theta math.atan2(to_node.y - from_node.y, to_node.x - from_node.x) new_node Node(from_node.x self.expand_dis * math.cos(theta), from_node.y self.expand_dis * math.sin(theta)) new_node.cost from_node.cost self.expand_dis new_node.parent from_node return new_node def choose_parent(self, new_node, near_inds): 在邻近节点中选择最优父节点 if not near_inds: return None # 搜索成本最低的邻近节点 costs [] for i in near_inds: near_node self.node_list[i] # 检查路径是否无碰撞 if self.check_collision_between(near_node, new_node): costs.append(near_node.cost self.calc_distance(near_node, new_node)) else: costs.append(float(inf)) min_cost min(costs) if min_cost float(inf): return None min_ind near_inds[costs.index(min_cost)] # 更新新节点的父节点和成本 new_node.cost min_cost new_node.parent self.node_list[min_ind] return new_node def rewire(self, new_node, near_inds): 重布线优化 for i in near_inds: near_node self.node_list[i] # 计算通过新节点的成本 new_cost new_node.cost self.calc_distance(new_node, near_node) # 如果新路径更优且无碰撞 if new_cost near_node.cost and self.check_collision_between(new_node, near_node): near_node.parent new_node near_node.cost new_cost # 递归更新子节点成本 self.propagate_cost_to_leaves(near_node) def propagate_cost_to_leaves(self, parent_node): 递归更新子节点成本 for node in self.node_list: if node.parent parent_node: node.cost parent_node.cost self.calc_distance(parent_node, node) self.propagate_cost_to_leaves(node) def search_best_goal_node(self): 搜索最佳目标节点 goal_inds [] for (i, node) in enumerate(self.node_list): if self.calc_distance(node, self.goal) self.expand_dis: goal_inds.append(i) # 检查是否可达 safe_goal_inds [] for i in goal_inds: t_node self.steer(self.node_list[i], self.goal) if self.check_collision(t_node): safe_goal_inds.append(i) if not safe_goal_inds: return None # 选择成本最低的节点 min_cost min([self.node_list[i].cost for i in safe_goal_inds]) for i in safe_goal_inds: if self.node_list[i].cost min_cost: return i return None def generate_final_course(self, goal_ind): 生成最终路径 path [[self.goal.x, self.goal.y]] node self.node_list[goal_ind] while node.parent is not None: path.append([node.x, node.y]) node node.parent path.append([node.x, node.y]) return path[::-1] # 反转路径 def calc_distance(self, from_node, to_node): 计算节点间距离 return math.hypot(from_node.x - to_node.x, from_node.y - to_node.y) def get_random_node(self): 获取随机节点含目标偏置 if random.randint(0, 100) self.goal_sample_rate: rnd Node(random.uniform(self.rand_area[0], self.rand_area[1]), random.uniform(self.rand_area[2], self.rand_area[3])) else: # 目标偏置采样 rnd Node(self.goal.x, self.goal.y) return rnd def get_nearest_node_index(self, rnd_node): 获取最近节点索引 dlist [self.calc_distance(node, rnd_node) for node in self.node_list] return dlist.index(min(dlist)) def find_near_nodes(self, new_node): 查找邻近节点 nnode len(self.node_list) 1 r self.connect_circle_dist * math.sqrt(math.log(nnode) / nnode) dlist [self.calc_distance(new_node, node) for node in self.node_list] near_inds [i for i, d in enumerate(dlist) if d r] return near_inds def check_collision(self, node): 检查节点是否与障碍物碰撞 for (ox, oy, size) in self.obstacle_list: dx ox - node.x dy oy - node.y d math.hypot(dx, dy) if d size self.robot_radius: return False # 碰撞 return True # 安全 def check_collision_between(self, from_node, to_node): 检查两节点间路径是否无碰撞 if not self.check_collision(from_node) or not self.check_collision(to_node): return False # 沿路径采样检查 dist self.calc_distance(from_node, to_node) step_num int(dist / self.path_resolution) for i in range(step_num): ratio i / step_num x from_node.x ratio * (to_node.x - from_node.x) y from_node.y ratio * (to_node.y - from_node.y) temp_node Node(x, y) if not self.check_collision(temp_node): return False return True def draw_graph(self, rndNone): 绘制搜索过程 plt.clf() if rnd is not None: plt.plot(rnd.x, rnd.y, ^k) for node in self.node_list: if node.parent: plt.plot([node.x, node.parent.x], [node.y, node.parent.y], -g) for (ox, oy, size) in self.obstacle_list: circle plt.Circle((ox, oy), size, colork, fillTrue) plt.gca().add_patch(circle) plt.plot(self.start.x, self.start.y, xr) plt.plot(self.goal.x, self.goal.y, xr) plt.axis([self.rand_area[0], self.rand_area[1], self.rand_area[2], self.rand_area[3]]) plt.grid(True) plt.pause(0.01) # 示例使用 def main(): print(RRT*路径规划开始) # 障碍物列表 [x, y, radius] obstacle_list [ (5, 5, 1), (3, 6, 1), (3, 8, 1), (7, 9, 1), (9, 5, 1), (8, 7, 1), (2, 3, 1), (4, 7, 1), (6, 2, 1), (7, 5, 1) ] # 创建RRT*规划器 rrt_star RRTStar(start[0, 0], goal[10, 10], rand_area[-2, 12, -2, 12], obstacle_listobstacle_list) # 执行规划 path rrt_star.planning(animationTrue) # 绘制最终路径 if path: plt.clf() rrt_star.draw_graph() plt.plot([x for (x, y) in path], [y for (x, y) in path], -r) plt.grid(True) plt.pause(0.01) plt.show() else: print(无法找到可行路径) if __name__ __main__: main()7. 进阶扩展方向7.1 三维空间扩展将RRT*扩展到三维空间需要考虑三维碰撞检测使用八叉树或KD树加速空间查询姿态约束考虑机器人的姿态和动力学限制可视化挑战三维路径的可视化呈现class Node3D: 三维节点类 def __init__(self, x, y, z): self.x x self.y y self.z z self.cost 0.0 self.parent None def check_collision_3d(node, obstacle_tree): 三维碰撞检测 dist, _ obstacle_tree.query([node.x, node.y, node.z]) return dist robot_radius7.2 动态环境适应针对动态障碍物的改进策略增量式规划在原有搜索树上继续扩展而非重新构建局部调整对受影响路径段进行局部重规划速度障碍物法结合VO进行动态避障def dynamic_replan(self, new_obstacles): 动态重规划 # 更新障碍物信息 self.obstacle_list new_obstacles # 保留部分原有节点 kept_nodes self.filter_nodes_in_free_space() self.node_list kept_nodes [self.start] # 执行规划 return self.planning(animationFalse)7.3 多机器人协同规划多机器人系统中的RRT*应用优先级规划为机器人分配不同优先级顺序规划时空路径在时间维度上扩展状态空间冲突检测检查路径在时空上的冲突def multi_robot_planning(robots, obstacles): 多机器人协同规划 paths {} for i, robot in enumerate(robots): # 将其他机器人的路径视为动态障碍物 other_paths [path for j, path in paths.items() if j ! i] dynamic_obs obstacles convert_paths_to_obstacles(other_paths) # 为当前机器人规划路径 planner RRTStar(robot.start, robot.goal, dynamic_obs, robot.area) paths[i] planner.plan() return paths
