RRT路径规划算法实验报告

RRT路径规划算法实验报告一、实验背景与目的路径规划是机器人领域的核心研究方向之一，其目标是在存在障碍物的环境中，为机器人找到一条从起始点到目标点的无碰撞路径。快速扩展随机树（Rapidly-exploringRandomTrees,RRT）算法作为一种基于采样的路径规划方法，凭借其对高维空间的适应性和概率完备性，在移动机器人导航、无人机路径规划、机械臂运动规划等领域得到广泛应用。本次实验旨在通过理论学习与代码实现，深入理解RRT算法的基本原理、运行机制及性能特点。具体实验目标包括：掌握RRT算法的核心思想与步骤，理解随机采样、最近邻搜索、局部路径生成及碰撞检测等关键环节的作用；基于Python语言实现RRT算法，构建包含起始点、目标点及障碍物的二维仿真环境，验证算法在不同场景下的路径规划能力；分析算法参数（如扩展步长、采样次数、目标偏向概率等）对路径规划结果的影响，总结参数调优策略；对比RRT算法与传统路径规划算法（如A*算法）的优缺点，探讨RRT算法的适用场景与改进方向。二、RRT算法原理概述2.1核心思想RRT算法的核心思想是通过随机采样空间中的点，逐步构建一棵以起始点为根节点的随机树，使树的分支不断向未探索区域扩展，最终与目标点连接形成可行路径。该算法通过随机采样的方式探索环境，无需对环境进行精确建模，适用于复杂、高维的非结构化环境。2.2基本步骤RRT算法的基本运行步骤如下：初始化随机树：以起始点为根节点，初始化一棵随机树，树中仅包含起始点一个节点。随机采样点生成：在环境空间中随机采样一个点，记为rand_point。为提高算法收敛速度，可引入目标偏向机制，以一定概率直接将目标点作为采样点。最近邻节点搜索：在已构建的随机树中，搜索与rand_point距离最近的节点，记为nearest_node。局部路径生成：从nearest_node出发，向rand_point方向移动固定步长，生成一个新的节点new_node。若移动步长超过nearest_node到rand_point的距离，则直接将rand_point作为new_node。碰撞检测：检查nearest_node到new_node的路径是否与障碍物发生碰撞。若路径无碰撞，则将new_node添加到随机树中，并记录其与nearest_node的连接关系；若路径存在碰撞，则舍弃new_node，返回步骤2重新采样。目标可达性判断：检查new_node是否到达目标点附近（即与目标点的距离小于设定的阈值）。若到达目标点，则从new_node回溯至起始点，得到完整的路径；若未到达目标点，则重复步骤2至步骤5，直到达到最大采样次数或找到可行路径。2.3关键技术环节2.3.1最近邻搜索最近邻搜索是RRT算法的关键步骤之一，其效率直接影响算法的整体性能。常用的最近邻搜索方法包括线性扫描法、k-d树法等。线性扫描法通过计算随机树中所有节点与采样点的距离，选择距离最小的节点，实现简单但时间复杂度较高（O(n)，n为树中节点数量）；k-d树法通过构建二叉树结构对节点进行划分，可将最近邻搜索的时间复杂度降低至O(logn)，适用于节点数量较多的场景。2.3.2碰撞检测碰撞检测用于判断新生成的节点及路径是否与障碍物发生碰撞，是保证路径可行性的核心环节。常用的碰撞检测方法包括：包围盒检测：为障碍物建立包围盒（如矩形包围盒、圆形包围盒），通过判断路径是否与包围盒相交进行初步检测，适用于快速排除明显不碰撞的情况；精确几何检测：针对障碍物的精确几何形状（如多边形、圆形），通过计算路径与障碍物边界的交点，判断是否发生碰撞，适用于对精度要求较高的场景。2.3.3目标偏向策略为提高RRT算法的收敛速度，可引入目标偏向策略，即每次采样时以一定概率直接选择目标点作为采样点。该策略使随机树更倾向于向目标点方向扩展，减少无效采样次数，尤其适用于目标点周围障碍物较少的场景。三、实验环境搭建与代码实现3.1实验环境配置本次实验基于Python语言实现，主要依赖以下库：NumPy：用于数值计算与矩阵操作，实现随机采样点生成、距离计算等功能；Matplotlib：用于可视化仿真环境、随机树及规划路径；Shapely：用于几何图形的碰撞检测，支持多边形、圆形等障碍物的精确碰撞判断。实验环境配置步骤如下：安装Python3.8及以上版本；通过pip命令安装依赖库：pipinstallnumpymatplotlibshapely3.2仿真环境构建本次实验构建了一个二维平面仿真环境，环境尺寸为100m×100m，包含以下元素：起始点：坐标为(10,10)；目标点：坐标为(90,90)；障碍物：设置5个矩形障碍物和3个圆形障碍物，障碍物的位置与尺寸如下表所示：障碍物类型位置坐标（x,y）尺寸参数矩形1(30,30)宽20m，高20m矩形2(60,20)宽15m，高30m矩形3(20,60)宽25m，高15m矩形4(70,60)宽18m，高22m矩形5(45,45)宽10m，高10m圆形1(40,70)半径8m圆形2(65,40)半径10m圆形3(80,20)半径6m3.3代码实现以下是RRT算法的核心代码实现：importnumpyasnpimportmatplotlib.pyplotaspltfromshapely.geometryimportPoint,LineString,Polygonfromshapely.opsimportunary_unionclassRRT:def__init__(self,start,goal,obstacle_list,rand_area,step_size=2.0,max_iter=5000,goal_sample_rate=0.1):"""初始化RRT算法参数:paramstart:起始点坐标(x,y):paramgoal:目标点坐标(x,y):paramobstacle_list:障碍物列表，包含矩形和圆形障碍物:paramrand_area:随机采样区域(x_min,x_max,y_min,y_max):paramstep_size:扩展步长:parammax_iter:最大采样次数:paramgoal_sample_rate:目标偏向概率"""self.start=Node(start[0],start[1])self.goal=Node(goal[0],goal[1])self.obstacle_list=obstacle_listself.rand_area=rand_areaself.step_size=step_sizeself.max_iter=max_iterself.goal_sample_rate=goal_sample_rateself.node_list=[self.start]defplanning(self):"""执行RRT路径规划:return:规划得到的路径，若未找到路径则返回None"""foriinrange(self.max_iter):#生成随机采样点rnd_node=self.get_random_node()#找到最近邻节点nearest_ind=self.get_nearest_node_index(self.node_list,rnd_node)nearest_node=self.node_list[nearest_ind]#生成新节点new_node=self.steer(nearest_node,rnd_node,self.step_size)#碰撞检测ifself.check_collision(new_node,self.obstacle_list):self.node_list.append(new_node)#检查是否到达目标点ifself.calc_dist_to_goal(new_node.x,new_node.y)<=self.step_size:final_node=self.steer(new_node,self.goal,self.step_size)ifself.check_collision(final_node,self.obstacle_list):returnself.generate_final_course(len(self.node_list)-1)returnNonedefget_random_node(self):"""生成随机采样点，引入目标偏向策略"""ifnp.random.rand()>self.goal_sample_rate:x=np.random.uniform(self.rand_area[0],self.rand_area[1])y=np.random.uniform(self.rand_area[2],self.rand_area[3])rnd=Node(x,y)else:rnd=Node(self.goal.x,self.goal.y)returnrnd@staticmethoddefget_nearest_node_index(node_list,rnd_node):"""找到最近邻节点的索引"""distances=[(node.x-rnd_node.x)**2+(node.y-rnd_node.y)**2fornodeinnode_list]nearest_ind=distances.index(min(distances))returnnearest_ind@staticmethoddefsteer(from_node,to_node,extend_length):"""从from_node向to_node方向扩展，生成新节点"""new_node=Node(from_node.x,from_node.y)dx=to_node.x-from_node.xdy=to_node.y-from_node.ydist=np.hypot(dx,dy)ifdist<=extend_length:new_node.x=to_node.xnew_node.y=to_node.yelse:new_node.x+=extend_length*dx/distnew_node.y+=extend_length*dy/distnew_node.parent=from_nodereturnnew_nodedefcheck_collision(self,node,obstacle_list):"""碰撞检测，检查节点与障碍物是否碰撞"""ifnodeisNone:returnFalseforobstacleinobstacle_list:ifobstacle['type']=='rect':#矩形障碍物碰撞检测polygon=Polygon([(obstacle['x']-obstacle['width']/2,obstacle['y']-obstacle['height']/2),(obstacle['x']+obstacle['width']/2,obstacle['y']-obstacle['height']/2),(obstacle['x']+obstacle['width']/2,obstacle['y']+obstacle['height']/2),(obstacle['x']-obstacle['width']/2,obstacle['y']+obstacle['height']/2)])line=LineString([(node.parent.x,node.parent.y),(node.x,node.y)])ifersects(line):returnFalseelifobstacle['type']=='circle':#圆形障碍物碰撞检测circle=Point(obstacle['x'],obstacle['y']).buffer(obstacle['radius'])line=LineString([(node.parent.x,node.parent.y),(node.x,node.y)])ifersects(line):returnFalsereturnTruedefcalc_dist_to_goal(self,x,y):"""计算当前点到目标点的距离"""dx=x-self.goal.xdy=y-self.goal.yreturnnp.hypot(dx,dy)defgenerate_final_course(self,goal_ind):"""生成最终路径，从目标点回溯至起始点"""path=[(self.goal.x,self.goal.y)]node=self.node_list[goal_ind]whilenode.parentisnotNone:path.append((node.x,node.y))node=node.parentpath.append((self.start.x,self.start.y))returnpath[::-1]classNode:"""节点类，用于存储随机树的节点信息"""def__init__(self,x,y):self.x=xself.y=yself.parent=Nonedefplot_environment(rrt,path=None):"""可视化仿真环境与路径规划结果"""plt.figure(figsize=(10,10))#绘制障碍物forobstacleinrrt.obstacle_list:ifobstacle['type']=='rect':x=obstacle['x']y=obstacle['y']width=obstacle['width']height=obstacle['height']rect=plt.Rectangle((x-width/2,y-height/2),width,height,color='gray',alpha=0.5)plt.gca().add_patch(rect)elifobstacle['type']=='circle':x=obstacle['x']y=obstacle['y']radius=obstacle['radius']circle=plt.Circle((x,y),radius,color='gray',alpha=0.5)plt.gca().add_patch(circle)#绘制随机树fornodeinrrt.node_list:ifnode.parentisnotNone:plt.plot([node.x,node.parent.x],[node.y,node.parent.y],'-g',linewidth=0.5)#绘制起始点和目标点plt.plot(rrt.start.x,rrt.start.y,'ro',markersize=10,label='Start')plt.plot(rrt.goal.x,rrt.goal.y,'bo',markersize=10,label='Goal')#绘制规划路径ifpathisnotNone:path=np.array(path)plt.plot(path[:,0],path[:,1],'-r',linewidth=2,label='Path')plt.xlabel('X(m)')plt.ylabel('Y(m)')plt.title('RRTPathPlanning')plt.xlim(rrt.rand_area[0],rrt.rand_area[1])plt.ylim(rrt.rand_area[2],rrt.rand_area[3])plt.legend()plt.grid(True)plt.show()if__name__=='__main__':#构建障碍物列表obstacle_list=[{'type':'rect','x':30,'y':30,'width':20,'height':20},{'type':'rect','x':60,'y':20,'width':15,'height':30},{'type':'rect','x':20,'y':60,'width':25,'height':15},{'type':'rect','x':70,'y':60,'width':18,'height':22},{'type':'rect','x':45,'y':45,'width':10,'height':10},{'type':'circle','x':40,'y':70,'radius':8},{'type':'circle','x':65,'y':40,'radius':10},{'type':'circle','x':80,'y':20,'radius':6}]#初始化RRT算法rrt=RRT(start=[10,10],goal=[90,90],obstacle_list=obstacle_list,rand_area=[0,100,0,100],step_size=2.0,max_iter=5000,goal_sample_rate=0.1)#执行路径规划path=rrt.planning()#可视化结果plot_environment(rrt,path)ifpathisnotNone:print("路径规划成功！")print("路径节点坐标：")fori,pointinenumerate(path):print(f"节点{i+1}:({point[0]:.2f},{point[1]:.2f})")else:print("未找到可行路径，请调整参数或增加采样次数！")四、实验结果与分析4.1基本场景路径规划结果在默认参数设置（扩展步长2.0m，最大采样次数5000，目标偏向概率0.1）下，RRT算法成功规划出一条从起始点(10,10)到目标点(90,90)的无碰撞路径。路径规划结果如图1所示，随机树从起始点出发，通过随机采样逐步扩展，避开所有障碍物后到达目标点。规划得到的路径总长度约为156.8m，包含79个路径节点，算法运行时间约为2.3s。从路径形态来看，RRT算法规划的路径呈现出明显的“锯齿状”，这是由于算法通过随机采样扩展路径，缺乏对路径平滑性的优化。虽然路径存在一定的冗余，但能够保证在复杂障碍物环境中找到可行路径，验证了算法的概率完备性。4.2参数对路径规划结果的影响为分析算法参数对路径规划结果的影响，本次实验分别调整扩展步长、目标偏向概率及最大采样次数，对比不同参数下的路径长度、规划时间及成功率。4.2.1扩展步长的影响扩展步长决定了随机树每次扩展的距离，直接影响算法的收敛速度与路径质量。实验设置目标偏向概率为0.1，最大采样次数为5000，分别测试步长为1.0m、2.0m、3.0m、4.0m时的路径规划结果，结果如下表所示：扩展步长（m）路径长度（m）规划时间（s）路径节点数成功率1.0148.23.714995%2.0156.82.379100%3.0165.41.55690%4.0178.11.14575%从实验结果可以看出：随着扩展步长的增大，路径长度逐渐增加，路径节点数逐渐减少。这是由于步长增大后，随机树的扩展速度加快，但容易错过更优的路径，导致路径冗余增加；算法规划时间随步长增大而减少，因为步长越大，生成的节点数越少，计算量降低；算法成功率随步长增大先升高后降低。步长过小时，随机树扩展速度慢，容易在最大采样次数内无法到达目标点；步长过大时，随机树容易跳过狭窄通道，导致无法找到可行路径。在本次实验场景中，步长为2.0m时算法成功率最高，达到100%。4.2.2目标偏向概率的影响目标偏向概率决定了采样时选择目标点的概率，影响随机树向目标点扩展的倾向性。实验设置扩展步长为2.0m，最大采样次数为5000，分别测试目标偏向概率为0.0、0.1、0.2、0.3时的路径规划结果，结果如下表所示：目标偏向概率路径长度（m）规划时间（s）路径节点数成功率0.0162.53.18290%0.1156.82.379100%0.2152.31.877100%0.3149.71.57595%实验结果表明：随着目标偏向概率的增大，路径长度逐渐减小，规划时间逐渐缩短。这是因为目标偏向概率越高，随机树越倾向于向目标点方向扩展，减少了无效采样，提高了收敛速度；算法成功率在目标偏向概率为0.1和0.2时达到100%，当概率超过0.2后，成功率略有下降。这是由于目标偏向概率过高时，随机树过度向目标点扩展，可能忽略周围障碍物的存在，导致路径与障碍物碰撞，需要重新采样，反而降低了规划效率。4.2.3最大采样次数的影响最大采样次数决定了算法的最大运行时间，直接影响算法的概率完备性。实验设置扩展步长为2.0m，目标偏向概率为0.1，分别测试最大采样次数为1000、3000、5000、7000时的路径规划结果，结果如下表所示：最大采样次数路径长度（m）规划时间（s）路径节点数成功率1000168.30.88560%3000159.21.68090%5000156.82.379100%7000155.13.078100%从实验结果可以看出：随着最大采样次数的增加，路径长度逐渐减小，因为算法有更多机会探索更优的路径；规划时间随采样次数增加而线性增长，因为每次采样都需要进行最近邻搜索、碰撞检测等计算；算法成功率随采样次数增加而提高，当采样次数达到5000时，成功率达到100%。这验证了RRT算法的概率完备性，即只要存在可行路径，当采样次数足够多时，算法一定能找到路径。4.3与A*算法的对比分析为进一步分析RRT算法的性能，本次实验将RRT算法与传统的A算法进行对比。A算法是一种基于启发式搜索的路径规划算法，通过代价函数评估节点优先级，能够找到最优路径。实验在相同的仿真环境中运行A*算法，对比结果如下表所示：算法类型路径长度（m）规划时间（s）路径平滑性环境适应性最优性RRT156.82.3较差强非最优A*127.30.5较好弱最优对比结果表明：路径长度与最优性：A*算法通过启发式搜索能够找到最短路径，路径长度明显短于RRT算法；而RRT算法由于随机采样的特性，规划的路径存在冗余，不保证最优性；规划时间：在二维简单环境中，A算法的规划时间远短于RRT算法，因为A算法通过代价函数引导搜索，避免了大量无效采样；路径平滑性：A*算法规划的路径更平滑，而RRT算法的路径呈现锯齿状，需要后续进行平滑处理；环境适应性：RRT算法无需对环境进行精确建模，适用于复杂、高维的非结构化环境；而A*算法依赖于环境的栅格化建模，在高维环境中计算量呈指数增长，适应性较差。五、RRT算法的改进方向虽然RRT算法在复杂环境路径规划中具有明显优势，但也存在路径冗余、收敛速度慢、非最优性等缺点。针对这些问题，研究者提出了多种改进算法，主要包括以下方向：