一种基于EB‑RRT的无人机航迹规划方法与流程
本发明属于无人机运动规划领域，具体涉及一种无人机自主航迹规划方法。
背景技术：
航迹规划是无人机导航和机器人技术中的一个重要问题，其基本定义为：给定一个初始状态和目标状态，寻找一个可行航迹使无人机无碰撞的从初始状态运行到目标状态。航迹规划有广泛的应用场景：gps导航、无人驾驶汽车、计算机动画、路由问题、制造行业的机械臂运动和生活以及工业领域的很多方面。因此对航迹规划问题的研究近年来成为一个研究热点。
无人机航迹规划算法根据其感知能力，可以分为局部航迹规划和全局航迹规划，其中全局航迹规划就是在已知环境地图的情况下进行规划，预先知道环境的全局信息；而局部航迹规划只需要获得机器人感知范围内的环境信息，主要指障碍物信息，根据局部信息完成规划。全局航迹规划算法很多，人工势场算法是典型的航迹规划算法(参照文献1：kpvalavanis.advanceinunmannedaerialvehicles[m].stateofartandroadtoautonomy,2007；即无人机的研究进展)，算法在环境中建立人工势场，障碍物和环境边界具有斥力，目标区域具有引力，无人机根据所受力向目标区域靠近。势场法不需要进行复杂的计算，只需要计算出环境的势场即可，然而势场法在复杂环境中容易使飞行器陷入局部最小，并不适合在复杂环境以及狭窄的通道中进行规划。针对势场法的不足，提出了dubins曲线算法以及细胞分裂算法(参照文献2：ade,jcaves.human-automationcollaborativerrtforuavmissionpathplanning[m].massachusettsinstituteoftechnology,2010；即用于无人机路径规划的人工自动化协作rrt)和delaunay三角算法(参照文献3：hhtriharminto.,asprabuwono.uavdynamicpathplanningforinterceptingofamovingtarget:areview[j].communicationsincomputerandinformationscience,2013,376:206-219；即用于拦截动态目标的无人机动态路径规划)等离散化搜索空间的算法，通过对障碍物或者环境空间进行建模的方法寻找最优航迹。同时也有人将进化计算如遗传算法(参照文献4：jhholland.adaptationinnaturalandartificialsystems[m].mitpress,1992；即自然和人工系统中的适应)、粒子群算法(参照文献5：robergev.,tarbouchim.,labonteg.comparisonofparallelgeneticalgorithmandparticleswarmoptimizationforrealtimeuavpathplanning[j].ieeetransactionsonindustrialinformatics,2013,9(1):132-141；即平行遗传算法和粒子群优化作用于实时无人机路径规划的比较)用于解决航迹规划问题，利用算法的进化操作和迭代过程找到最优航迹。然而这类算法的计算开销特别大，算法在复杂的环境以及高维数的环境中需要大量的时间去计算，无法直接使用在无人机的航迹规划应用中。
基于采样的航迹规划算法已被证明可以高效地解决航迹规划问题，概率路线图算法(prm)(参照文献6：lkavraki.,psvestka.probabilisticroadmapsforpathplanninginhigh-dimensionalconfigurationspaces[j].ieeetransactionsonrobotics&automations,1996,12(4):566-580；即高维空间中路径规划的概率路线图)和快速扩展随机数算法(rrt)(参照文献7：smlavalle.,jjkuffner.randomizedkinodynamicplanning[j].ieeeinternationalconferenceonrobotics&automation,1999,1(5):473-479；即随机动力学规划)是目前主要的两种采样算法。prm算法随机在空间生成采样点，并且对这些点进行连接，最后通过图搜索算法找到初始状态到目标区域的航迹。与rpm算法相比，rrt算法采用树结构描述碰撞检测的次数，且树的航迹搜索比图的航迹搜索更容易实现。然而rrt算法的收敛率太低，即需要通过大量的迭代才能找到最优航迹，并且随着迭代次数的上升，算法也需要大量的内存。因此人们目前提出了很多针对rrt算法的变种算法以及改进算法。nika将粒子滤波与rrt算法结合提出了prrt算法(参照文献8：namelchior.,rsimmons.particlerrtforpathplanningwithuncertainty[j].ieeeinternationalconferenceonrobotics&automation,2007,1617–1624；即具有不确定性的粒子rrt路径规划)用于局部航迹规划、stephenr将泰森多边形(voronoi)引入树的生长中提高rrt找到可行解的速度(参照文献9：lindemannsr,lavallesm.incrementallyreducingdispersionbyincreasingvoronoibiasinrrts[c]//ieeeinternationalconferenceonroboticsandautomation,2004.proceedings.icra.ieee,2004:3251-3257vol.4；即增加在rrts的voronoi偏见逐步减少色散)。其中应用最广并且效果最好的是sertackaraman提出的rrt*算法(参照文献10：jeonjh,karamans,frazzolie.anytimecomputationoftime-optimaloff-roadvehiclemaneuversusingtherrt[j].2011,413(1):3276-3282；即使用rrt随时计算最优越野车机动)。rrt*算法在每次迭代后对新加入的节点及其邻近节点进行优化，这一优化操作改善了算法的收敛率，保证了算法的渐近最优性，从而使其广泛应用在航迹规划领域并衍生出一系列变种算法。a.h.qureshi为了加快rrt*算法的收敛速度(参照文献11：ahqureshi.,smumtaz.,kfiqbal.,yayaz.triangulargeometrybasedoptimalmotionplanningusingrrt*-motionplanner[j].ieeeinternationalworkshoponadvancedmotioncontrol,2014,380-385；即基于rrt*三角几何的最佳运动规划)，在生成随机点的同时将随机点以及目标点和初始位置三个点构成的三角形的内心作为新的随机点加入树中，使随机点一定程度上偏向了目标点；m.jordan提出了利用两棵树生长寻找航迹的方法提高了算法的收敛率(参照文献12：mjordan.,aperez.optimalbidirectionalrapidly-exploringrandomtrees[r].mit-csail-tr-2013-021,2013；即最有双向快速扩展随机生成树)；xzhang等人提出了基于自学习策略和混合偏差抽样方案以提高规划效率(参照文献13：zhangx,lüttekef,zieglerc,etal.self-learningrrt*algorithmformobilerobotmotionplanningincomplexenvironments[m]//intelligentautonomoussystems13.2016:57-69；即在复杂环境中的移动机器人运动规划自学习rrt*算法)；kbaizid等人在安全性优化，提出rrs(rapidly-exploringrandomsnakes)(参照文献14：baizidk,chellalir,luzar,etal.rrs:rapidly-exploringrandomsnakesanewmethodformobilerobotpathplanning[m]//intelligentautonomoussystems13.springerinternationalpublishing,2014:291-305；即rrs：基于快速扩展随机生成的移动机器人蛇形路径规划的新方法)；在此基础上，人们通过对这些算法的优化来解决实际问题。jxiong等人基于rrt解决分段装配路径规划任务(参照文献15：xiongj,huy,wub,etal.minimum-costrapid-growingrandomtreesforsegmentedassemblypathplanning[j].theinternationaljournalofadvancedmanufacturingtechnology,2015,77(5):1043-1055；即面向分段装配路径鬼的最小代价快速增长随机树)；kyang等人采用有效的样条曲线参数化来代替系统动力学的数值积分，保证路径曲率的连续性(参照文献16：yangk.anefficientspline-basedrrtpathplannerfornon-holonomicrobotsinclutteredenvironments[c]//internationalconferenceonunmannedaircraftsystems.2013:288-297；即一种高效的基于样条的rrt路径规划，适用于混乱环境中的非完整机器人)。这些改进算法在实现无人机的航迹规划时，仍然存在以下问题：
(1)算法的收敛速度还有很大的提升空间；
(2)算法的节点在障碍物附近地生长不具有灵活性；
(3)算法寻找附近节点集需要进行大量的遍历操作，因此算法的运行时间较长；
(4)由于算法的航迹由树节点连接生成，最后生成的航迹不够平滑，难以满足无人机实际应用。
技术实现要素：
为了克服现有的无人机航迹规划方法的收敛速度较慢、灵活性较差、运行效率较低、实用性较差的不足，本发明提供一种收敛速度较快、灵活性较好、运行效率较高、实用性良好的基于eb-rrt的无人机航迹规划方法。
为了解决上述技术问题本发明提供如下技术方案：
一种基于eb-rrt的无人机航迹规划方法，包括以下步骤：
step1：导入环境信息，并对地图环境进行栅格分区处理，为后续寻找附近节点集做准备；
step2：产生一个随机点xrand，寻找已有节点中距离随机点最近的节点xnearst；
step3：根据步长算出插入点xnew，若根节点到xnew的距离与xnew到终点的欧氏距离之和大于当前最优路径的长度，则跳过本次迭代；否则，进入step4；
step4：对xnew点是否在障碍物内进行检测，若不通过，则收集xnearst周围环境信息，在其周围free区域内随机采样一个新的插入点xnew；
step5：将xnew插入树中，并在相应的栅格内遍历寻找xnew的附近节点集并对其附近节点的路径进行优化；
step6：对插入xnew点后的两棵树进行连接检测，若不能，则交换两棵树，继续采样随机点；若能，则说明已经找到可行路径，对其路径点集进行降采样处理，再采用贝塞尔三次插值算法优化生成新的路径。
进一步，所述step4中，在原xnew处于障碍物范围内时，在节点xnearst将周围环境划分为9个区域，分别标记为1、2、3、4、5、6、7、8、9，自身所在点为区域5，通过函数addinform对其他8个区域进行信息收集，区分为障碍区和非障碍区，函数choosedirection在剩余的6个非障碍区内随机取一点作为新xnew，再对xnearst和xnew之间进行障碍检测
再进一步，所述step1中，每次插入一个新的节点xnew后，需要通过函数nearnodes寻找一个与其欧式距离小于γ的点集v1∈v，再对该点集中的所有点判断是否需要进行航迹修正；
每次寻找点集，都需要对生长树t中的所有旧节点进行遍历，对二维地图进行简单的栅格划分，由细胞分解算法将整个地图空间分为障碍区和非障碍区，将宽为w，高为h的地图空间划分为m*n个宽为wgrid，高为hgrid的区域，其中
wgrid＝w/m
hgrid＝h/n
在插入新节点xnew后，只需要遍历xnew所在区域以及其周围8个区域内的所有旧节点；为了保证距离其欧式距离小于γ的所有点均在这9个区域内，须满足
min{wgrid,hgrid}>max(γ)
又因为
γ＝k(logn/n)1/d
所以
γ≤k(log2/2)1/d
只需要满足
wgrid≥k(log2/2)1/d
hgrid≥k(log2/2)1/d
将wgrid,hgrid设较大值，使得m和n为整数值。
更进一步，所述step6中，对初始轨迹点的遍历以确定是否存在点xi，使得其与起点或者终点相连能够通过障碍检测来判断是否删除多余轨迹点，降采样的处理过程如下：
6.1：遍历路径点，若xi能够与起点相连，并且不产生碰撞，则删除起点到xi之间的路径点；若xi能够与终点相连，并且不产生碰撞，则删除xi到终点之间的路径点；
6.2：对间隔span的两点进行相连并进行碰撞检测，若通过碰撞检测，则删除该两点间的路径点；若未通过，则检测下一组间隔span的两点；
6.3：若遍历一次没有删除路径点，则令span自增1，直到span值大小与剩余路径点个数相同。
所述step6中，选取在转折点处三阶贝塞尔曲线两个端点和两个控制点的过程，xi为某一转折点，线段
b(t)＝p0(1-t)3+3p1t(1-t)2+3p2t2(1-t)+p3t3
其中，0<t<1；
计算出曲线上点，即生成曲线。
本发明的基于eb-rrt的无人机航迹规划方法，提出了一种eb-rrt(efficientb-rrt*)算法，提出自我避障提高算法收敛速度并减少内存占用；采用栅格分区的方法缩短附近节点的遍历时间；最后利用合理的降采样和三次贝塞尔插值算法对折点进行光滑化的处理，使算法最终生成相对平滑的航迹，为无人机实际飞行提供可行航迹规划方法，最后通过相关实验验证了算法的有效性，并与其他算法比较验证eb-rrt的性能。
附图说明
图1是一种基于eb-rrt的无人机航迹规划方法的流程图。
图2是mb-rrt*(a)和eb-rrt(b)在障碍附近的生长情况示意图，其中，(a)表示mb-rrt*算法，(b)表示eb-rrt算法。
图3是栅格分区的示意图。
图4是mb-rrt*和eb-rrt降采样效果图，其中，(a)表示mb-rrt*算法，(b)表示eb-rrt算法。
图5是曲线取点拟合过程示意图。
具体实施方式
下面结合附图对本发明做进一步说明。
下面结合附图对本发明做进一步说明。
参照图1，一种基于eb-rrt的无人机航迹规划方法，包括以下步骤：
step1：导入环境信息，并对地图环境进行栅格分区处理，为后续寻找附近节点集做准备。
step2：产生一个随机点xrand，寻找已有节点中距离随机点最近的节点xnearst。
step3：根据步长算出插入点xnew。若根节点到xnew的距离与xnew到终点的欧氏距离之和大于当前最优路径的长度，则跳过本次迭代；否则，进入step4。
step4：对xnew点是否在障碍物内进行检测，若不通过，则收集xnearst周围环境信息，在其周围free区域内随机采样一个新的插入点xnew。
step5：将xnew插入树中，并在相应的栅格内遍历寻找xnew的附近节点集并对其附近节点的路径进行优化。
step6：对插入xnew点后的两棵树进行连接检测，若不能，则交换两棵树，继续采样随机点；若能，则说明已经找到可行路径，对其路径点集进行降采样处理，再采用贝塞尔三次插值算法优化生成新的路径。
自我避障：mb-rrt*以及传统的b-rrt*等算法首先通过sample函数产生一个随机点xrand后，再通过nearstnode寻找树t旧节点中距离xrand最近的节点xnearst，以矢量
虽然mb-rrt*采用自适应步长的方法使得在障碍物附近生长的步长保持在范围μmin<μ<μmax,但若xrand在障碍物周围μmin范围内，则仍然会导致生成的新节点xnew进入障碍物范围而被舍弃，使该次迭代未插入新的节点。这种情况在狭长通道以及障碍物比较密集的空间下发生的概率较高。
本发明提出的eb-rrt算法采用的自我避障方法能够避免这种不必要的迭代次数的发生，使生长树每次都能探索新的空间，增大两棵树连接的可能性，提高算法找到可行路径的效率。
图2-a为mb-rrt*的自适应步长的生长方法，可以看到在距离障碍物的长度小于最短步长μmin时，仍然会导致新节点xnew处于障碍物范围内而被舍弃。图2-b为eb-rrt采用自我避障方法，在原xnew处于障碍物范围内时，在节点xnearst将周围环境划分为9个区域，分别标记为1、2、3、4、5、6、7、8、9，自身所在点为区域5，通过函数addinform对其他8个区域进行信息收集，区分为障碍区和非障碍区，图中2、3为障碍区，1、4、6、7、8、9为非障碍区，函数choosedirection在剩余的6个非障碍区内随机取一点作为新xnew，再对xnearst和xnew之间进行障碍检测，防止在a区域内点作为新xnew。
采用了自我避障方法后的eb-rrt算法和引入自适应步长方法的mb-rrt*算法相比，扩大了障碍物周围的探索范围，增强了避障的能力，因此有效地减少找到初次可行路径的迭代次数和运行时间。
栅格分区：继承了rrt*概率完备性的b-rrt*和mb-rrt*算法在双向树生长过程中，每次插入一个新的节点xnew后，需要通过函数nearnodes寻找一个与其欧式距离小于γ的点集v1∈v，再对该点集中的所有点判断是否需要进行航迹修正。在这一过程中，每次寻找点集，都需要对生长树t中的所有旧节点进行遍历。然而，随着搜索范围的扩大，插入的节点也不断增多，遍历的时间也会呈线性增长。
针对这个问题，eb-rrt引入的栅格分区方法能够在保留概率完备性的基础上极大地缩短遍历时间，以此来提高算法的运行效率。
为了减少加载地图信息后初始化需要的准备时间，只对二维地图进行简单的栅格划分，不考虑障碍信息，由ahmadabbadi和vaclavprenosil提出的细胞分解算法(celldecompositionalgorithms)(参照文献17：abbadia,prenosilv.collidedpathreplanningindynamicenvironmentsusingrrtandcelldecompositionalgorithms[m]//modellingandsimulationforautonomoussystems.springerinternationalpublishing,2015:131-143；即基于rrt和细胞分裂算法在动态环境中的碰撞路径重规划)可将整个地图空间分为障碍区和非障碍区。图3将宽为w，高为h的地图空间划分为m*n个宽为wgrid，高为hgrid的区域，其中
wgrid＝w/m
hgrid＝h/n
在插入新节点xnew后，只需要遍历xnew所在区域以及其周围8个区域内的所有旧节点即可。为了保证距离其欧式距离小于γ的所有点均在这9个区域内，须满足
min{wgrid,hgrid}>max(γ)
又因为
γ＝k(logn/n)1/d
所以
γ≤k(log2/2)1/d
只需要满足
wgrid≥k(log2/2)1/d
hgrid≥k(log2/2)1/d
实际中为了考虑内存的开销以及便于操作，可以将wgrid,hgrid设较大值，使得m,n恰好为整数值。
折点光滑：mb-rrt*算法在对最后生成的最优航迹轨迹点进行降采样的过程虽然能够使最后的轨迹点尽量少，但是存在图4-a的这种情况，图中黑色部分为障碍区，蓝色部分为xstart和以其为根节点的起点生长树tstart，绿色部分为xgoal和以其为根节点的终点生长树tgoal，红色部分为当前最优解的初始轨迹，灰色折线为降采样后的轨迹。通过图4-a和图4-b的对比可以看到，在被删除的轨迹点中存在一点xi，其与终点xgoal相连且能够通过碰撞检测，而不需要经过xi+1,…,xgoal-1，且图4-b中的最终轨迹远小于图4-a中的航迹。
因此，eb-rrt算法采用的降采样算法可以在减少轨迹点的基础上，适当缩减初始轨迹长度，达到优化效果。eb-rrt采用的降采样算法是在mb-rrt*的基础上增加了对初始轨迹点的遍历以确定是否存在点xi，使得其与起点或者终点相连能够通过障碍检测来判断是否删除多余轨迹点。所述降采样算法的处理过程如下：
6.1：遍历路径点，若xi能够与起点相连，并且不产生碰撞，则删除起点到xi之间的路径点；若xi能够与终点相连，并且不产生碰撞，则删除xi到终点之间的路径点；
6.2：对间隔span的两点进行相连并进行碰撞检测，若通过碰撞检测，则删除该两点间的路径点；若未通过，则检测下一组间隔span的两点；
6.3：若遍历一次没有删除路径点，则令span自增1，直到span值大小与剩余路径点个数相同。
为了使经过降采样后的折线轨迹在转折点处变得光滑,eb-rrt采用不同于mb-rrt*算法中选取控制点的方式用贝塞尔曲线拟合。图5为在转折点处三阶贝塞尔曲线两个端点和两个控制点的选取过程，蓝色曲线为最后拟合出来的三阶贝塞尔曲线。其中，xi为某一转折点，线段
b(t)＝p0(1-t)3+3p1t(1-t)2+3p2t2(1-t)+p3t3
其中，0<t<1；
计算出曲线上点，即生成曲线。
可见，d越大，该转折点处路径长度就越小，同时又要满足路径的可行性，所以只需要线段
本发明的基于eb-rrt的无人机航迹规划方法，提出了一种eb-rrt(efficientb-rrt*)算法，提出自我避障提高算法收敛速度并减少内存占用；采用栅格分区的方法缩短附近节点的遍历时间；最后利用合理的降采样和三次贝塞尔插值算法对折点进行光滑化的处理，使算法最终生成相对平滑的航迹，为无人机实际飞行提供可行航迹规划方法，最后通过相关实验验证了算法的有效性，并与其他算法比较验证eb-rrt的性能。
- 还没有人留言评论。精彩留言会获得点赞！