辽宁省建设工程造价总站网站我国网站建设的不足

辽宁省建设工程造价总站网站,我国网站建设的不足,南阳微网站制作,莱州网站建设公司电话实现一个使用KD-Tree的RRT*路径规划算法#xff0c;并提供一个REWIRE函数#xff08;重连接优化函数#xff09;。使用Scipy的KD-Tree进行高效的最近邻搜索替代了传统的线性搜索#xff0c;大幅提高搜索效率 import numpy as np import matplotlib.pyplot as plt from scip…实现一个使用KD-Tree的RRT*路径规划算法并提供一个REWIRE函数重连接优化函数。使用Scipy的KD-Tree进行高效的最近邻搜索替代了传统的线性搜索大幅提高搜索效率importnumpyasnpimportmatplotlib.pyplotaspltfromscipy.spatialimportKDTreeimportmathimportrandomclassNode:RRT*节点类def__init__(self,x,y):self.xx self.yy self.parentNoneself.cost0.0# 从起点到当前节点的代价def__repr__(self):returnfNode({self.x:.2f},{self.y:.2f})classRRTStar:RRT*路径规划算法实现使用KD-Tree进行最近邻搜索def__init__(self,start,goal,obstacles,bounds,max_iter1000,step_size0.5,neighbor_radius2.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.startNode(start[0],start[1])self.goalNode(goal[0],goal[1])self.obstaclesobstacles self.boundsbounds self.max_itermax_iter self.step_sizestep_size self.neighbor_radiusneighbor_radius# 节点列表self.nodes[self.start]# 最终路径self.final_pathNonedefdistance(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# 提取所有节点的坐标pointsnp.array([[node.x,node.y]fornodeinself.nodes])kdtreeKDTree(points)# 查询最近邻dist,idxkdtree.query([point.x,point.y])returnself.nodes[idx]defsteer(self,from_node,to_node):生成新节点从from_node向to_node方向生长step_size距离dself.distance(from_node,to_node)# 如果距离小于步长直接返回目标节点ifdself.step_size:new_nodeNode(to_node.x,to_node.y)else:# 计算方向向量thetamath.atan2(to_node.y-from_node.y,to_node.x-from_node.x)new_xfrom_node.xself.step_size*math.cos(theta)new_yfrom_node.yself.step_size*math.sin(theta)new_nodeNode(new_x,new_y)returnnew_nodedefis_collision_free(self,node1,node2):检查两点之间路径是否与障碍物碰撞# 采样点检查碰撞num_check10foriinrange(num_check1):ti/num_check xnode1.xt*(node2.x-node1.x)ynode1.yt*(node2.y-node1.y)for(ox,oy,radius)inself.obstacles:distmath.sqrt((x-ox)**2(y-oy)**2)ifdistradius:returnFalsereturnTruedeffind_near_nodes(self,node):使用KD-Tree在半径内查找邻居节点iflen(self.nodes)2:return[]# 构建KD-Treepointsnp.array([[n.x,n.y]forninself.nodes])kdtreeKDTree(points)# 半径查询indiceskdtree.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_costfloat(inf)best_parentNonefornear_nodeinnear_nodes:# 检查是否无碰撞ifself.is_collision_free(near_node,new_node):# 计算通过near_node到达new_node的代价costnear_node.costself.distance(near_node,new_node)ifcostmin_cost:min_costcost best_parentnear_nodeifbest_parentisnotNone:new_node.parentbest_parent new_node.costmin_costreturnTruereturnFalsedefrewire(self,new_node,near_nodes):重连接函数 - RRT*算法的核心优化步骤rewire_count0fornear_nodeinnear_nodes:# 检查new_node是否可以成为near_node的更好父节点ifnear_nodenew_node.parent:continue# 检查是否无碰撞ifself.is_collision_free(new_node,near_node):# 计算通过new_node到达near_node的新代价new_costnew_node.costself.distance(new_node,near_node)# 如果新代价更小则重连接ifnew_costnear_node.cost:near_node.parentnew_node near_node.costnew_cost rewire_count1# 递归更新子节点的代价self.update_children_cost(near_node)returnrewire_countdefupdate_children_cost(self,parent_node):递归更新子节点的代价# 查找所有子节点注意这里简化处理实际需要维护子节点列表fornodeinself.nodes:ifnode.parentparent_node:node.costparent_node.costself.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_maxself.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_nodeself.random_node()# 2. 查找最近节点nearest_nodeself.nearest(random_node)ifnearest_nodeisNone:continue# 3. 生成新节点new_nodeself.steer(nearest_node,random_node)# 4. 检查碰撞ifnotself.is_collision_free(nearest_node,new_node):continue# 5. 查找邻居节点near_nodesself.find_near_nodes(new_node)# 6. 选择最优父节点ifnotself.choose_parent(new_node,near_nodes):continue# 7. 添加到节点列表self.nodes.append(new_node)# 8. 执行重连接REWIREself.rewire(new_node,near_nodes)# 9. 检查是否到达目标ifself.check_goal(new_node):# 尝试将目标节点连接到路径ifself.is_collision_free(new_node,self.goal):self.goal.parentnew_node self.goal.costnew_node.costself.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[]nodeself.goalwhilenodeisnotNone:path.append((node.x,node.y))nodenode.parent path.reverse()self.final_pathpathreturnpathdefget_path_cost(self):计算路径代价ifself.final_pathisNone:returnfloat(inf)cost0foriinrange(len(self.final_path)-1):x1,y1self.final_path[i]x2,y2self.final_path[i1]costmath.sqrt((x2-x1)**2(y2-y1)**2)returncostdefvisualize(self,showTrue):可视化结果plt.figure(figsize(10,10))# 绘制障碍物for(x,y,radius)inself.obstacles:circleplt.Circle((x,y),radius,colorgray,alpha0.5)plt.gca().add_patch(circle)# 绘制所有节点和连接fornodeinself.nodes:ifnode.parentisnotNone:plt.plot([node.x,node.parent.x],[node.y,node.parent.y],lightgray,linewidth0.5,alpha0.5)plt.plot(node.x,node.y,o,markersize3,colorblue,alpha0.3)# 绘制起点和终点plt.plot(self.start.x,self.start.y,ro,markersize10,label起点)plt.plot(self.goal.x,self.goal.y,go,markersize10,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-,linewidth2,label最终路径)print(f路径长度:{self.get_path_cost():.2f})# 设置图形属性x_min,x_max,y_min,y_maxself.bounds plt.xlim(x_min,x_max)plt.ylim(y_min,y_max)plt.grid(True,alpha0.3)plt.legend()plt.title(fRRT* 路径规划 (节点数:{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_starRRTStar(startstart,goalgoal,obstaclesobstacles,boundsbounds,max_iter2000,step_size0.5,neighbor_radius2.0)# 执行路径规划print(开始RRT*路径规划...)pathrrt_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. 创建规划器rrtRRTStar(start,goal,obstacles,bounds)# 2. 执行规划pathrrt.find_path()# 3. 可视化rrt.visualize()这个实现可以轻松集成到机器人导航、自动驾驶等系统中通过调整step_size、neighbor_radius等参数可以平衡规划速度和质量。