RRT路径规划改进算法(快速扩展随机树)

ComputerEngineeringandApplications计算机工程与应用2011，47（3）1引言路径规划是智能机器人研究领域的一个重要方向，是指按照一定的性能指标，机器人从所处的环境中搜索到一条从初始状态开始到目标状态、可以避开障碍物的最优或次优路径。传统的路径规划算法有多边形拟合法、栅格法、人工势场法、遗传算法等。但这些方法都需要在一个确定的空间内对障碍物进行建模，计算复杂度与机器人自由度呈指数关系，不适合解决多自由度机器人在复杂环境中的规划[1-4]。基于快速扩展随机树（RRT）的路径规划算法，通过对状态空间中的采样点进行碰撞检测，避免了对空间的建模，能够有效地解决高维空间和复杂约束的路径规划问题[5-6]。该方法的特点是能够快速有效地搜索高维空间，通过状态空间的随机采样点，把搜索导向空白区域，从而寻找到一条从起始点到目标点的规划路径，适合解决多自由度机器人在复杂环境下和动态环境中的路径规划[7]。RRT算法由于利用随机采样生成扩展随机树，导致对同一任务重复规划产生不同的路径，尤其在动态环境下，机器人在完成同一任务时采用的路径不稳定，从而影响任务完成的效率，并且对机器人机体造成损害。针对这一问题，JimBruce提出ERRT（ExtendRRT）算法，DaveFerguson提出DRRT（Dynam-icRRT）算法，ZuckerMatt提出MP-RRT（MultipartiteRRT）算法，都通过利用上周期随机树的信息对RRT算法进行了改进，一定程度上提高了动态环境下RRT算法的稳定性[8-10]。上述改进算法虽然在一定程度上提高了基本RRT算法的稳定性，但是容易导致生成路径远离最优解。本文提出一种新的RRT改进算法，使用基本RRT算法规划出一条路径，同时通过对上周期的扩展树进行剪枝和连接，得到另外一条适用于本周期环境的路径。通过比较这两条路径，从中选择一条较优路径。此算法进一步提高了RRT算法在动态环境的稳定性，同时可以保证规划的路径能逼近最优路径。2RRT算法2.1基本RRT算法RRT是一种多维空间中有效率的规划方法。它以一个初基于对比优化的RRT路径规划改进算法冯林，贾菁辉FENGLin，JIAJinghui大连理工大学计算机科学与工程系，辽宁大连116023DepartmentofComputerScienceandEngineering，DalianUniversityofTechnology，Dalian，Liaoning116023，ChinaE-mail：[e-mail]，JIAJinghui.ImprovedalgorithmofRRTpathplanningbasedoncomparisonoptimization.ComputerEngi-neeringandApplications，2011，47（3）：210-213.Abstract：BecausethebasicRapidly-exploringRandomTree（RRT）pathplanningisunstableandnotoptimalindynamicen-vironment，animprovedalgorithmofrobotsRRTpathplanningbasedoncomparisonoptimizationisproposed.Ineachcircle，astablepathcanbeobtainedbytrimmingandreplanningthetreeinlastcircle，andanewpathcanbeplannedviatheba-sicRRTmethod.Comparingthesetwopaths，theoptimalonecanbefound.Fromtheresultsofsimulationsandexperimentsonmobilerobots，itconcludesthatthisalgorithmcanimprovethestabilityofRRTpathplanningindynamicenvironment，andensuresthatthepathisalmostoptimal.Keywords：pathplanning；Rapidly-exploringRandomTree（RRT）；dynamicenvironment摘要：针对动态环境下机器人RRT路径规划算法缺乏稳定性和偏离最优解的问题，提出一种基于对比优化的RRT路径规划改进算法。算法在新一周期的环境下，通过对上一周期路径树进行剪枝和重新规划得到一条稳定的路径，同时利用基本RRT算法规划出一条新路径，通过对比两条路径得到较优解。仿真和真实机器人实验结果均表明，改进的算法提高了动态复杂环境下RRT路径规划的稳定性，并保证了规划的路径逼近最优解。关键词：路径规划；扩展随机树；动态环境DOI：10.3778/j.issn.1002-8331.2011.03.062文章编号：1002-8331（2011）03-0210-04文献标识码：A中图分类号：TP24基金项目：国家自然科学基金（theNationalNaturalScienceFoundationofChinaunderGrantNo.60773213）；辽宁省自然科学基金（theNatu-ralScienceFoundationofLiaoningProvinceofChinaunderGrantNo.20071092）。作者简介：冯林（1975—），男，博士，教授，研究领域为图像压缩，配准及融合，演化算法；贾菁辉（1984—），男，硕士，研究领域为机器人控制，人工智能。收稿日期：2009-09-08修回日期：2009-11-092102011，47（3）始点作为根节点，通过随机采样增加叶子节点的方式，生成一个随机扩展树，当随机树中的叶子节点包含了目标点或进入了目标区域，便可以在随机树中找到一条由从初始点到目标点的路径。基本RRT算法如图1所示。在随机树的生长过程中（FunctionRRTPlan1~10行），初始传入的随机树T只包含一个节点：根节点qinit。首先Choose-Target函数从状态空间中随即选择一个采样点qtarget（3行）；然后，Nearest函数从随机树中选择一个距离qtarget最近的节点qnearest（4行）；最后，Extend函数通过从qnearest向qTarget扩展一段距离，得到一个新的节点qnew（7行）。如果qnew与障碍物发生碰撞，则Extend函数返回空，放弃这次生长，否则将qnew加入到随机树中。重复上述步骤直到qnearest和目标点qgaol距离小于一个阈值，则代表随机树到达了目标点，算法返回成功（5~6行）。为了使算法可控，加入了运行时间上限或搜索的节点总数上限（2行）。如果在限制期间内无法到达目标点，则算法返回失败。为了加快随机树到达目标点的速度，改进方法是：在随机树每次的生长过程中，根据随机概率来决定qtarget是目标点还是随机点。在ChooseTarget函数（11~16行）中设定参数Goal-Prob，每次得到一个0到1.0的随机值p，当0pGoalProb的时候，随机树朝目标点生长（13~14行）；当GoalProbp1.0（15~16行）时，随机树朝一个随机方向生长。2.2相关改进算法基本RRT算法存在动态环境中规划路径不稳定的问题。由于随机树在自由空间中随机的选择生长方向，且环境中的障碍物是运动的，从而导致了每个周期随机树的生长差别很大，所以生成的路径不稳定。ERRT、DRRT、MP-RRT等RRT算法在一定程度上改进了这一问题。ERRT提出了路径节点集合（WayPointCache）的概念，将一周期的路径节点存入该集合中，下一周期按照一定比例从路径节点集合中随机选择节点作为ChooseTarget函数的返回值。DRRT和MP-RRT都是通过对上周期随机树与新环境进行碰撞检测，保留可用的子树，在其基础上进行重新规划。不同之处在于，DRRT只保留含有根结点的子树，在其基础上扩展到目标点；而MP-RRT保留了全部的可用子树，从中随机选择进行连接，生成新的路径。但是，上述算法规划出的路径有时会远离最优路径，因为上述算法在路径规划中是基于历史的路径信息，降低了扩展树生长的随机性，从而减少了达到最优路径的概率。尤其在DRRT和MP-RRT中，若初始生成的路径非较优，则容易导致之后规划继续保持这条非优路径。3基于对比优化的RRT改进算法基本RRT算法生成的路径具有随机性，而改进的RRT算法通过历史信息使新规划出的路径在一定程度上保持稳定。文中结合以上两种方法的特点，每周期规划出两条路径：一条稳定路径和一条随机路径。稳定路径是通过对历史路径行剪枝和重新规划，使其尽可能接近上周期的路径；随机路径是由基本的RRT方法得出。通过对比两条路径，得到一条较优的路径。该算法可以防止路径陷入不优的稳定状态，使之逐渐逼近较优路径，称算法为CompareRRT算法，简写为CRRT。CRRT主算法描述如图2。首先通过基本RRT算法进行路径规划，得到随机扩展树Tnew（2~3行）。T为上周期的随机树，如果T为空或本周期机器人的目标点与上周期发生了大的变化，则直接返回Tnew（4~5行）；否则，对T进行剪枝和重新规划（7行），将修正后的T与Tnew进行比较，返回一条较优路径（8行）。其中，对T进行剪枝和重新规划的算法见函数Trim-AndReplan（9~20行）：首先，将T中与动态障碍物发生碰撞的节点删除（11~13行）。T被分成若干子树，形成森林F。在F中，将根节点不属于上周期路径节点的子树删除（14~16行）。按照从初始点到目标点的顺序，F中的子树排列为T1，T2，…，Tn。然后，重新构造T，以本周期的机器人状态qinit作为T的根节点。使用基本RRT算法，依次将T生发到T1，T2，…，Tn的根节点，将子树连接起来。最后，将T生发到目标节点qgoal。图3所示为剪枝和重新规划过程，其中最左边的节点为初FunctionRRTPlan：BOOL（env：environment，T：RRTTree，qgoal：node）1Varqtarget，qnearest，qnew：node2While（searchtime/spaceremaining）do3qtarget=ChooseTarget（qgoal）4qnearest=Nearest（T，qtarget）5If（Distance（qnearest，qgoal）DistanceThreshold）then6Returntrue7qnew=Extend（qnearest，qtarget）8If（qnew≠NULL）then9T.AddNode（qnew）10ReturnfalseFunctionChooseTarget（qgoal：node）：node11Varp：real12p=Random（0，1.0）13if0pGoalProbthen14Returngoal15ElseifGoalProbp1.0then16ReturnRandomNode（）；图1基本RRT算法FunctionCRRTPathPlan：RRTTree（env：environment，T：RRTTree，qinit：node，qgaol：node）1VarTnew：rrtTree2Tnew.add（qinit）3RRTPlan（env，Tbasic，qgoal）4IfEmpty（T）orNotSimilarGoal（）then5ReturnPnew6Else7TrimAndReplan（env，T，qinit，qgoal）8ReturnChooseTree（T，Tnew）FunctionTrimAndReplan（env：environmentT：RRTTree，qinit：node，qgoal：node）9VarF：RRTFore