实现一个使用KD-Tree的RRT*路径规划算法，并提供一个REWIRE函数（重连接优化函数）。使用Scipy的KD-Tree进行高效的最近邻搜索替代了传统的线性搜索，大幅提高搜索效率
importnumpyasnpimportmatplotlib.pyplotaspltfromscipy.spatialimportKDTreeimportmathimportrandomclassNode:"""RRT*节点类"""def__init__(self,x,y):self.x=x self.y=y self.parent=Noneself.cost=0.0# 从起点到当前节点的代价def__repr__(self):returnf"Node({self.x:.2f},{self.y:.2f})"classRRTStar:"""RRT*路径规划算法实现，使用KD-Tree进行最近邻搜索"""def__init__(self,start,goal,obstacles,bounds,max_iter=1000,step_size=0.5,neighbor_radius=2.0):""" 初始化RRT*算法 参数: start: 起点坐标 (x, y) goal: 终点坐标 (x, y) obstacles: 障碍物列表，每个障碍物为(x, y, radius) bounds: 地图边界 (x_min, x_max, y_min, y_max) max_iter: 最大迭代次数 step_size: 步长 neighbor_radius: 邻居搜索半径 """self.start=Node(start[0],start[1])self.goal=Node(goal[0],goal[1])self.obstacles=obstacles self.bounds=bounds self.max_iter=max_iter self.step_size=step_size self.neighbor_radius=neighbor_radius# 节点列表self.nodes=[self.start]# 最终路径self.final_path=Nonedefdistance(self,node1,node2):"""计算两个节点之间的欧几里得距离"""returnmath.sqrt((node1.x-node2.x)**2+(node1.y-node2.y)**2)defnearest(self,point):"""使用KD-Tree查找最近节点（替代线性搜索）"""# 构建KD-Treeiflen(self.nodes)==0:returnNone# 提取所有节点的坐标points=np.array([[node.x,node.y]fornodeinself.nodes])kdtree=KDTree(points)# 查询最近邻dist,idx=kdtree.query([point.x,point.y])returnself.nodes[idx]defsteer(self,from_node,to_node):"""生成新节点（从from_node向to_node方向生长step_size距离）"""d=self.distance(from_node,to_node)# 如果距离小于步长，直接返回目标节点ifd<=self.step_size:new_node=Node(to_node.x,to_node.y)else:# 计算方向向量theta=math.atan2(to_node.y-from_node.y,to_node.x-from_node.x)new_x=from_node.x+self.step_size*math.cos(theta)new_y=from_node.y+self.step_size*math.sin(theta)new_node=Node(new_x,new_y)returnnew_nodedefis_collision_free(self,node1,node2):"""检查两点之间路径是否与障碍物碰撞"""# 采样点检查碰撞num_check=10foriinrange(num_check+1):t=i/num_check x=node1.x+t*(node2.x-node1.x)y=node1.y+t*(node2.y-node1.y)for(ox,oy,radius)inself.obstacles:dist=math.sqrt((x-ox)**2+(y-oy)**2)ifdist<=radius:returnFalsereturnTruedeffind_near_nodes(self,node):"""使用KD-Tree在半径内查找邻居节点"""iflen(self.nodes)<2:return[]# 构建KD-Treepoints=np.array([[n.x,n.y]forninself.nodes])kdtree=KDTree(points)# 半径查询indices=kdtree.query_ball_point([node.x,node.y],self.neighbor_radius)# 排除节点自身（如果是已存在的节点）near_nodes=[self.nodes[i]foriinindicesifself.nodes[i]!=node]returnnear_nodesdefchoose_parent(self,new_node,near_nodes):"""为new_node选择最优父节点"""min_cost=float('inf')best_parent=Nonefornear_nodeinnear_nodes:# 检查是否无碰撞ifself.is_collision_free(near_node,new_node):# 计算通过near_node到达new_node的代价cost=near_node.cost+self.distance(near_node,new_node)ifcost<min_cost:min_cost=cost best_parent=near_nodeifbest_parentisnotNone:new_node.parent=best_parent new_node.cost=min_costreturnTruereturnFalsedefrewire(self,new_node,near_nodes):"""重连接函数 - RRT*算法的核心优化步骤"""rewire_count=0fornear_nodeinnear_nodes:# 检查new_node是否可以成为near_node的更好父节点ifnear_node==new_node.parent:continue# 检查是否无碰撞ifself.is_collision_free(new_node,near_node):# 计算通过new_node到达near_node的新代价new_cost=new_node.cost+self.distance(new_node,near_node)# 如果新代价更小，则重连接ifnew_cost<near_node.cost:near_node.parent=new_node near_node.cost=new_cost rewire_count+=1# 递归更新子节点的代价self.update_children_cost(near_node)returnrewire_countdefupdate_children_cost(self,parent_node):"""递归更新子节点的代价"""# 查找所有子节点（注意：这里简化处理，实际需要维护子节点列表）fornodeinself.nodes:ifnode.parent==parent_node:node.cost=parent_node.cost+self.distance(parent_node,node)self.update_children_cost(node)defrandom_node(self):"""生成随机节点（90%偏向目标点）"""ifrandom.random()>0.1:returnself.goal x_min,x_max,y_min,y_max=self.boundsreturnNode(random.uniform(x_min,x_max),random.uniform(y_min,y_max))defcheck_goal(self,node):"""检查是否到达目标点附近"""returnself.distance(node,self.goal)<=self.step_sizedeffind_path(self):"""执行RRT*路径规划"""foriterationinrange(self.max_iter):# 1. 生成随机节点random_node=self.random_node()# 2. 查找最近节点nearest_node=self.nearest(random_node)ifnearest_nodeisNone:continue# 3. 生成新节点new_node=self.steer(nearest_node,random_node)# 4. 检查碰撞ifnotself.is_collision_free(nearest_node,new_node):continue# 5. 查找邻居节点near_nodes=self.find_near_nodes(new_node)# 6. 选择最优父节点ifnotself.choose_parent(new_node,near_nodes):continue# 7. 添加到节点列表self.nodes.append(new_node)# 8. 执行重连接（REWIRE）self.rewire(new_node,near_nodes)# 9. 检查是否到达目标ifself.check_goal(new_node):# 尝试将目标节点连接到路径ifself.is_collision_free(new_node,self.goal):self.goal.parent=new_node self.goal.cost=new_node.cost+self.distance(new_node,self.goal)self.nodes.append(self.goal)print(f"找到路径！迭代次数:{iteration}, 节点数:{len(self.nodes)}")break# 提取最终路径returnself.extract_path()defextract_path(self):"""从目标节点回溯提取路径"""ifself.goal.parentisNone:returnNonepath=[]node=self.goalwhilenodeisnotNone:path.append((node.x,node.y))node=node.parent path.reverse()self.final_path=pathreturnpathdefget_path_cost(self):"""计算路径代价"""ifself.final_pathisNone:returnfloat('inf')cost=0foriinrange(len(self.final_path)-1):x1,y1=self.final_path[i]x2,y2=self.final_path[i+1]cost+=math.sqrt((x2-x1)**2+(y2-y1)**2)returncostdefvisualize(self,show=True):"""可视化结果"""plt.figure(figsize=(10,10))# 绘制障碍物for(x,y,radius)inself.obstacles:circle=plt.Circle((x,y),radius,color='gray',alpha=0.5)plt.gca().add_patch(circle)# 绘制所有节点和连接fornodeinself.nodes:ifnode.parentisnotNone:plt.plot([node.x,node.parent.x],[node.y,node.parent.y],'lightgray',linewidth=0.5,alpha=0.5)plt.plot(node.x,node.y,'o',markersize=3,color='blue',alpha=0.3)# 绘制起点和终点plt.plot(self.start.x,self.start.y,'ro',markersize=10,label='起点')plt.plot(self.goal.x,self.goal.y,'go',markersize=10,label='终点')# 绘制最终路径ifself.final_pathisnotNone:path_x=[p[0]forpinself.final_path]path_y=[p[1]forpinself.final_path]plt.plot(path_x,path_y,'r-',linewidth=2,label='最终路径')print(f"路径长度:{self.get_path_cost():.2f}")# 设置图形属性x_min,x_max,y_min,y_max=self.bounds plt.xlim(x_min,x_max)plt.ylim(y_min,y_max)plt.grid(True,alpha=0.3)plt.legend()plt.title(f'RRT* 路径规划 (节点数:{len(self.nodes)})')plt.xlabel('X')plt.ylabel('Y')ifshow:plt.show()returnplt.gcf()# 示例使用defmain():# 设置参数start=(0,0)goal=(10,10)bounds=(-2,12,-2,12)# (x_min, x_max, y_min, y_max)# 创建障碍物obstacles=[(3,3,1.5),(6,6,1.2),(8,2,1.0),(4,8,1.3),(7,8,1.0),(2,5,0.8),(5,2,0.7),(9,6,1.1)]# 创建RRT*规划器rrt_star=RRTStar(start=start,goal=goal,obstacles=obstacles,bounds=bounds,max_iter=2000,step_size=0.5,neighbor_radius=2.0)# 执行路径规划print("开始RRT*路径规划...")path=rrt_star.find_path()ifpathisNone:print("未找到路径！")else:print(f"找到路径，包含{len(path)}个点")print(f"路径总代价:{rrt_star.get_path_cost():.2f}")# 可视化rrt_star.visualize()if__name__=="__main__":main()这个实现包含了RRT*算法的核心特性：
关键特点：
- KD-Tree优化：
· 使用Scipy的KD-Tree进行高效的最近邻搜索
· 替代了传统的线性搜索，大幅提高搜索效率 - REWIRE函数：
· rewire()函数是RRT*算法的核心优化步骤
· 对新节点的邻居进行重连接优化
· 确保树结构始终保持最优连接 - RRT*算法步骤：
· 随机采样
· 最近邻查找
· 新节点生成
· 碰撞检测
· 选择最优父节点
· 重连接优化 - 性能优势：
· KD-Tree使最近邻搜索从O(n)提升到O(log n)
· 支持半径查询，高效找到邻居节点
· 适合处理大规模节点的情况
使用说明：
# 1. 创建规划器rrt=RRTStar(start,goal,obstacles,bounds)# 2. 执行规划path=rrt.find_path()# 3. 可视化rrt.visualize()这个实现可以轻松集成到机器人导航、自动驾驶等系统中，通过调整step_size、neighbor_radius等参数可以平衡规划速度和质量。
