一种基于快速扩展随机树的无人机航路规划方法
技术领域
本发明涉及路径规划领域，具体设计了一种基于快速扩展随机树的路径规划方法，在无人驾驶、机械臂控制和无人机航路规划等领域有广泛的应用。
背景技术
路径规划一直是智能机器人领域的热点话题，其目的是在任务空间中寻找一条从起始点到目标点之间的无碰撞连续路径，一般地，采用图搜索算法可以获取最短路径，但此类算法的规划速度受环境大小影响较大。快速扩展随机树(RRT/rapidly exploringrandom tree)算法(参见Lavalle S M.Rapidly-exploring Random Trees:ANew Tool forPath Planning[Z].USA:Computer Science Dept,lowa State Univ,1998.)摒弃了路径最优特性，通过在空间中随机采样的方案对空间进行探索，在环境适应性和求解速度上有较大优势，其随机树可被视为空间中已探索的安全区域，当随机树扩展到目标点时，便可生成可行的安全路径。由于随机树的结构与随机点的生成顺序密切相关，导致传统RRT算法生成的路径迂回曲折且不是全局最优，考虑到无人机的运动特性，由RRT*算法生成的可行路径不能作为无人机的规划轨迹。RRT*算法(参见Karaman,S.,&Frazzoli,E.(2011).Sampling-based algorithms for optimal motion planning.The International Journal ofRobotics Research,30(7),846–894.)增加了随机树的局部重构环节，使得算法具备渐进最优的特性，并且增加了新增节点的父节点重选环节，该环节减少了从起始点到点路径的代价，从而减少算法收敛到最优路径所需时间。在此基础上，如何使算法快速收敛到最优路径成为研究热点，Informed-RRT*利用RRT*算法的初始解构建椭圆采样区域(参见GammellJ D,Srinivasa S S,Barfoot T D.Informed RRT*:Optimal Sampling-based PathPlanning Focused via Direct Sampling of an Admissible Ellipsoidal Heuristic[J].2014.)，从而有效提高采样点的利用率，但当构建采样区域超过原本的采样空间时，算法不能起到加速作用；RRT*-smart也在RRT*算法的初始解上进行优化(参见Islam F,NasirJ,Malik U,et al.RRT*-Smart:Rapid convergence implementation of RRT*towardsoptimal solution[C].Mechatronics and Automation(ICMA),2012InternationalConference on.IEEE,2012.)，利用节点删节和智能采样手段减少优化时间，但算法的性能容易受到初始解影响，只能得到与初始解类别相同的优化结果；Quick-RRT*利用随机树顶点之间的连接关系增加搜寻节点数量(参见Jeong I B,Lee S J,Kim J H.Quick-RRT*:Triangular Inequality-based Implementation of RRT*with Improved InitialSolution and Convergence Rate[J].Expert Systems with Applications,2019,123(JUN.):82-90.)，也能缩短生成路径长度的效果，但是增大搜寻节点数量无疑会导致消耗时间增大，因此在实际应用中常常受到限制。
由于基于采样的路径规划方案无法在有限时间内得到最优路径，在实际应用场景中，往往不需要优化到最短路径，特别是对于无人机这样的高速运动对象，它们对航路规划时间要求较高。因此获得可行路径的时间也应该作为衡量航路规划算法的重要指标，而上述航迹规划方法均不能在时间和路径长度之间取得良好的平衡。
发明内容
要解决的技术问题
当前基于采样的路径规划算法均侧重于如何提高收敛速度，算法获得的初始可行路径的长度则不被重视，欲得到符合无人机飞行的期望路径，需要额外时间优化。为了提高路径规划算法效率，使其满足无人机应用场景，本发明提出一种基于快速扩展随机树的改进型的路径规划方法，通过进一步减少新增节点添加到随机树后到根节点的代价，从而减小生成路径的长度，缩短优化时间所需时间，使其快速满足无人机对航迹规划算法需求。
技术方案
一种基于快速扩展随机树的无人机航路路径规划方法，其特征在于步骤如下：
步骤1：障碍物的位置和尺寸已知的无人机航路规划环境Map，起始点xstart，目标区域Xgoal，环境X＝Xobs∪Xfree，其中Xobs为障碍物所在区域，Xfree为无障碍物区域，因此有Xobs∩Xfree＝φ；同时对参数进行初始化，包括：最大循环次数Nmax，循环计数变量ncount＝1；局部重构半径Drewire；随机树G＝(V,E)，其中V←{xstart}为随机树的节点列表，E←φ则表示节点之间的扩展关系；
步骤2：判断循环次数ncount是否超过最大循环次数Nmax：当循环次数ncount超过设定的最大循环次数Nmax时，则跳转至步骤15；当循环次数ncount小于等于最大循环次数Nmax时，进入步骤3；
步骤3：循环次数ncount增加1，然后进入步骤4；
步骤4：在环境Map中无障碍物区域随机生成新增节点xnew；节点生成后，进入步骤5；
步骤5：在已知随机树包含的节点列表V中寻找与新节点xnew空间距离最近的节点，并将其记为xnearest；进入步骤6；
步骤6：判别点xnearest到xnew之间直线路径上是否含有障碍物，若存在障碍物，则返回步骤2；若不存在障碍物，则进入步骤7；
步骤7：沿着xnearest所在支线向随机树的根节点进行逐个检测，找到一个与xnew之间的局部路径安全且其父节点与xnew之间存在障碍物的节点，如果将其记为xbest，其父节点记为Parent(xbest)，它们需要满足：{xbest-xnew}∈Xfree且
步骤8：判断xnew-parent是否创建成功，如果xnew-parent≠φ且xnew-parent≠xbest，则表示xnew-parent创建成功，进入步骤9；否则，进入步骤13；
步骤9：将xnew-parent加入到随机树G上，使其父节点为Parent(xbest)，即将xnew-parent加入到树的节点列表V，并将连线{Parent(xbest)-xnew-parent}加入E；然后进入步骤10；
步骤10：将xnew加入到随机树G上，使其父节点为xnew-parent，即将xnew加入到树的节点列表V，并将连线{xnew-parent-xnew}加入E；然后进入步骤11；
步骤11：在以xnew为圆心，Drewire为半径的区域内对随机树进行局部重构，即对于该区域内的任一节点xnear，记xnear沿着树到起始点xstart的路径长度为Cost(xnear)，xnew沿着树到起始点xstart的路径长度为Cost(xnew)，如果Cost(xnear)＞Cost(xnew)+Distance(xnear,xnew)，则将xnear的父节点更改为xnew；然后进入步骤12；
步骤12：判别新增节点xnew是否等于目标区域Xgoal内，若xnew∈Xgoal，则存在可行路径，进入步骤14；若
步骤13：将xnew加入到随机树G上，使其父节点为xbest，即将xnew加入到树的节点列表V，并将连线{xbest-xnew}加入E；然后进入步骤11；
步骤14：按照随机树G节点之间的关系生成可行路径path，然后进入步骤15；
步骤15：若无人机航路路径规划成功，则输出无人机航路路径path；若失败，则返回失败。
有益效果
本发明提出了一种基于快速扩展随机树的无人机航路规划方法，使用随机点作为新增点替换传统算法的定步长生成新增点的方案，极大程度上提高了随机树的扩展速度，减少了环境的维度和大小对规划算法的约束强度，能更好的适应于无人机工作的三维场景；通过为新增节点创建父节点的方式使得局部路径更加靠近障碍物，从而减少了新增节点添加到随机树后到根节点的代价，提高了生成符合无人机航迹的速度；本发明提供了一种随机树的扩展方案，该方案可以与采样方案和优化方案结合，从而可以在收敛速度和规划速度上进一步优化，具有十分重要的工程应用价值。
附图说明
图1是本发明设计的快速扩展随机树改进方法的流程图；
图2是本发明的规划方法中为新增节点创建父节点的流程图；
图3是本发明的规划方法中为新增节点创建父节点的示意图；
图4是本发明的规划方法中局部重构的示意图；
图5是地图环境信息；
图6是规划方法在二维场景中首次探索到目标点后生成的路径；
图7是本发明所生成路径与时间关系曲线；
具体实施方式
现结合实施例、附图对本发明作进一步描述：
本发明为一种路径规划方案，并将其命名为Fast-RRT*(F-RRT*)，F-RRT*算法为RRT*算法的改进版本，主要改进在于新增点如何被加入随机树，算法整体可以被划分为15个步骤，对应图1所示的流程图，流程图中长方形为执行步骤，菱形表示判断步骤，其内的函数或变量处理过程为该步骤对应的名称或内容。算法的具体步骤内容如下：
步骤1：获取所需的相关参数，包括：起始点xstart，目标区域Xgoal，环境X＝Xobs∪Xfree，其中Xobs为障碍物所在区域，Xfree为无障碍物区域，因此有Xobs∩Xfree＝φ。同时对算法中的参数进行初始化，包括：最大循环次数Nmax，循环计数变量ncount＝1，新节点创建的过程参数Da；局部重构半径Drewire；随机树G＝(V,E)，其中V←{xstart}为随机树的节点列表，E←φ则表示节点之间的扩展关系。
步骤2：判断循环次数ncount是否超过最大循环次数Nmax。为了避免陷入死循环，当循环次数ncount超过设定的最大循环次数Nmax时，则跳转至步骤15；当循环次数ncount小于等于最大循环次数Nmax时，进入步骤3。
步骤3：为了防止程序陷入死循环，循环次数ncount增加1，然后进入步骤4；
步骤4：在Xfree区域中随机进行随机采样，得到新节点xnew，该步骤操作内容用函数SampleFree进行表述。xnew生成后，进入步骤5。
步骤5：在随机树包含的节点列表V中寻找与新节点xnew空间距离最近的节点，并将其记为xnearest。该步骤操作内容用函数Nearest进行描述。然后进入步骤6。
步骤6：判别xnearest到xnew之间的局部路径是否安全，即线段xnearest-xnew满足{xnearest-xnew}∈Xfree时，则认为局部路径安全，进入步骤7；反之，若
步骤7：沿着xnearest所在支线向随机树的根节点进行逐个检测，找到一个与xnew之间的局部路径安全且其父节点与xnew之间存在障碍物的节点，如果将其记为xbest，其父节点记为Parent(xbest)，它们需要满足：{xbest-xnew}∈Xfree且
步骤8：判断xnew-parent是否创建成功，如果xnew-parent≠φ且xnew-parent≠xbest，则表示xnew-parent创建成功，进入步骤9；否则，进入步骤13。
步骤9：将xnew-parent加入到随机树G上，使其父节点为Parent(xbest)，即将xnew-parent加入到树的节点列表V，并将连线{Parent(xbest)-xnew-parent}加入E。在流程图中使用函数Addnode表示节点添加的树的过程，函数Addnode的输入参数为：G，xp，xa，执行内容为将xa加入到G，且以xp为父节点。然后进入步骤10。
步骤10：将xnew加入到随机树G上，使其父节点为xnew-parent，即将xnew加入到树的节点列表V，并将连线{xnew-parent-xnew}加入E。然后进入步骤11。
步骤11：在以xnew为圆心，Drewire为半径的区域内对随机树进行局部重构。即对于该区域内的任一节点xnear，记xnear沿着树到起始点xstart的路径长度为Cost(xnear)，xnew沿着树到起始点xstart的路径长度为Cost(xnew)，如果Cost(xnear)＞Cost(xnew)+Distance(xnear,xnew)，则将xnear的父节点更改为xnew，在图1的流程图中用函数Rewire表示步骤11的操作内容，函数Rewire改变了树G中节点之间的连接关系，所以输入与返回均为随机树。图4可以形象的表示步骤11的操作结果。然后进入步骤12；
步骤12：判别新增节点xnew是否等于目标区域Xgoal内，若xnew∈Xgoal，则存在可行路径，进入步骤14；若
步骤13：将xnew加入到随机树G上，使其父节点为xbest，即将xnew加入到树的节点列表V，并将连线{xbest-xnew}加入E。然后进入步骤11。
步骤14：按照随机树G节点之间的关系生成可行路径path，流程图中使用函数Findpath表示步骤14的操作内容，函数Findpath的输入为随机树，返回有序的离散点表示规划路径。然后进入步骤15。
步骤15：若路径规划成功，则输出路径path，若失败，则返回失败。
采用matlab作为仿真平台，按照算法编写二维的路径规划程序，并设计实验进行验证。
1、路径规划可行性
实验目的：验证路径规划算法可行性。
实验流程：图1所示流程。
实验方案：起始点xstart＝(5,5)；目标点xgoal＝(45,45)；Xgoal为xgoal以为圆心，半径为2的圆形区域；如图5所示地图被用于仿真，其中黑色方块为障碍物Xobs，白色区域为安全区域Xfree；最大循环次数Nmax＝200；局部重构半径Drewire＝4；新增节点迭代精度Da＝1.5；。
结果分析：在二维场景中进行两次仿真实验，得到图6所示的随机树和生成轨迹，其中细直线为随机树节点之间连接关系，即表示随机树对环境的探索范围，三角形点为生成路径的空间离散点，加粗的虚线部分为路径，由图可知，尽管生成路径并非最优，但随机树扩展到目标点仅进行了少量的探索，即在对生成路径长度要求不高的情况下，算法能实现快速路径规划；并且生成路径均以最优的方式靠近障碍物，这是由于采用了为新增节点创建父节点的方案。
2、算法收敛速度对比实验
实验目的：比较F-RRT*和RRT*算法的收敛速度，从而突出F-RRT*算法的在收敛速度方面的优势。
实验流程：F-RRT*算法在检测到可行路径后不会停止，即图1中的步骤12省略，即从步骤11直接进入步骤2。
实验方案：起始点xstart＝(5,5)；目标点xgoal＝(45,45)；Xgoal为xgoal以为圆心，半径为2的圆形区域；如图5所示地图被用于仿真，其中黑色方块为障碍物Xobs，白色区域为安全区域Xfree；最大循环次数Nmax＝80000；局部重构半径Drewire＝4；新增节点迭代精度Da＝1.5；RRT*算法采取相同的参数。在此条件下进行50次静态实验，并对其路径长度进行分析。
结果分析：图7为50次静态实验的数据结果，其中x轴为时间，y轴为路径长度，曲线表示算法50仿真过程中在该时间点生成路径的平均长度，其中实线表示F-RRT*算法生成路径的长度，虚线则对应RRT*算法的实验结果。由图可知，随着规划时间增加，两个算法生成路径长度均在不断变小，但F-RRT*算法在相同时间内规划处的路径长度明显比RRT*要短，且更早的达到最优。
