基于MB‑RRT的无人机二维航迹规划方法与流程
本发明涉及一种无人机二维航迹规划方法。
背景技术：
大路径规划是无人机导航和机器人技术中的一个重要问题。路径规划可以被定义为：给定一个初始状态和目标状态，寻找一个可行路径使无人机无碰撞的从初始状态运行到目标状态。路径规划有广泛的应用场景：GPS导航、无人驾驶汽车、计算机动画、路由问题、制造行业的机械臂运动和生活以及工业领域的很多方面。因此对路径规划问题的研究近年来成为一个热门的研究课题。
无人机二维航迹规划算法根据其感知能力，可以分为局部路径规划和全局路径规划，其中全局路径规划就是在已知环境地图的情况下进行规划，预先知道环境的全局信息；而局部路径规划只需要获得机器人感知范围内的环境信息，主要指障碍物信息，根据局部信息完成规划。全局二维航迹规划算法有很多，人工势场算法是典型的路径规划算法，算法在环境中建立一个人工势场，障碍物和环境边界具有斥力，目标区域具有引力，无人机根据所受力向目标区域靠近。势场法不需要进行复杂的计算，只需要计算出环境的势场即可，然而势场法在复杂环境中容易使飞行器陷入局部最小，并不适合在复杂环境以及狭窄的通道中进行规划。针对势场法的不足，部分学者提出了Dubins曲线算法以及细胞分裂算法和Delaunay三角算法等离散化搜索空间的算法，通过对障碍物或者环境空间进行建模的方法寻找最优路径。同时也有人将进化算法例如遗传算法以及粒子群算法用于解决路径规划问题，利用算法的并行性找到最优路径。然而这类算法的计算开销特别大，算法在复杂的环境以及高维数的环境中需要大量的时间去计算，无法直接应用于无人机的路径规划问题上。
基于采样的路径规划算法已被证明可以高效地解决路径规划问题，概率路线图算法(PRM)和快速扩展随机数算法(RRT)是目前主要的两种采样算法。PRM算法随机在空间生成采样点，并且对这些点进行连接，最后通过图搜索算法找到初始状态到目标区域的路径。与RPM算法相比，RRT算法采用树结构介绍了算法进行碰撞检测的次数，并且树的路径搜索比图的路径搜索更容易实现。然而RRT算法的收敛率太低，即需要通过大量的迭代才能找到最优路径，并且随着迭代次数的上升，算法也需要大量的内存。因此人们目前提出了很多针对RRT算法的变种算法以及改进算法：Nik A将例子滤波与RRT算法结合提出了PRRT算法用于局部路径规划、Stephen R将泰森多边形(Voronoi)引入树的生长中提高算法找到可行解的速度等等，其中应用最广并且效果最好的是Sertac Karaman提出的RRT*算法。RRT*算法在每次迭代后对新加入的节点及其邻近节点进行优化，这一优化操作改善了算法的收敛率，保证了算法的渐近最优性，从而使其广泛应用在路径规划领域并衍生出一系列变种算法。A.H.Qureshi为了加快RRT*算法的收敛速度，在生成随机点的同时将随机点以及目标点和初始位置三个点构成的三角形的内心作为新的随机点加入树中，使随机点一定程度上偏向了目标点；M.Jordan提出了利用两棵树生长寻找路径的方法提高了算法的收敛率。但是这些改进算法中仍然存在着一系列的问题：
(1)算法的收敛速率还有很大的提升空间；
(2)算法寻找最优路径需要进行大量的迭代，因此算法的运行需要大量的内存；
(3)算法的节点基于固定的步长生长，因此树在障碍物附近地生长具有局限性；
(4)由于算法的路径由树节点连接生成，最后生成的路径不够平滑，难以直接应用于无人机中。
技术实现要素：
为了克服已有无人机二维航迹规划方法的收敛速度较慢、内存占用偏大、在障碍物附近生长具有局限性、实用性较差的不足,本发明提供了一种收敛速度较快、内存占用空间较小、解决了在障碍物附近生长具有局限性的问题、能直接应用于无人机控制的基于MB-RRT*的无人机二维航迹规划方法。
本发明解决其技术问题所采用的技术方案是：
一种基于MB-RRT*的无人机二维航迹规划方法，包括如下步骤：
1)初始化树以及环境信息；
2)导入障碍物信息，并设置迭代次数；
3)判断是否达到迭代次数，如果是，对生成的路径点进行降采样处理，并采用插值算法优化生成路径线，结束；否则进入4)；
4)产生随机采样点Xrand，并寻找树中与随机采样点Xrand距离最近的点Xinsert；
5)根据点Xinsert生成自适应步长，根据步长生成最终插入点Xnew；
6)判断插入点Xnew到树根的距离是否大于当前的最优路径长，如果是，进入9)；如果否进入7)；
7)对路径进行碰撞检测，如果未通过进入9)，如果通过，将插入点加入树中，并对插入点周围的邻近节点进行优化；
8)对树进行连接检测，并进行连接。
9)将当前操作的目标树设置为另一颗树，回到步骤3)。
进一步，所述步骤3)中，所述降采样处理：对最终生成的轨迹点之间不断地进行碰撞检测，对最后的轨迹不断地截短直至无法截短。
再进一步，所述步骤3)中，采用构建三次贝塞尔曲线对轨迹点进行插值生成最后的路径曲线，三次贝塞尔曲线的方程如下所示：
B(t)＝P0(1-t)3+3P1t(1-t)2+3P2t2(1-t)+P3t3
其中P0为起始点，P3为目标点，P1和P2为控制点，用于控制曲线的方向，对于一系列路径点，只需要根据轨迹点不断计算两个控制点带入方程，即可生成曲线。
更进一步，所述步骤5)中，自适应步长的计算过程如下：首先通过NearestOb(x1,2μMax)寻找并计算出距离点x1最近的障碍物的距离Di，x1为查找的坐标点，μMax为设定的最大步长，μMin为设定的最小步长。根据下列公式计算出步长：
通过这一方法计算出的步长保持在范围μMin＜μ＜μMax，点x1距离障碍越近，步长越接近μMin，当以x1为中心半径为μMax的范围内没有障碍时，步长为μMax。
所述步骤4)中，在采样阶段对采样点进行筛选，即在每次生成插入点xnew后计算插入点到达根节点的长度c(xnew,xinit)，如果这个长度大于当前最优路径的长度就抛弃这个插入点。
本发明的技术构思为：虽然B-RRT*算法利用双向树同时生长提高了算法的收敛率，并且保留了RRT*算法的概率完整性，但是B-RRT*算法仍然有很多缺点需要去改善。首先，B-RRT*算法保留了RRT*算法以固定步长生长树的方式，较大的步长可以较快的生长，然而生长至障碍物附近的节点有较大概率由于无法通过碰撞检测而被抛弃，同时较小的步长虽然在障碍物附近地生长较好，但在空旷的环境地图中相对生长较慢。其次，B-RRT*算法在找到可行路径之后仍然对整个环境空间进行采样，这种采样策略容易产生大量无效节点。最后，B-RRT*算法同时也保留了RRT*算法将最后生成的路径点直接相连的方法生成最终路径，导致最后产生的路径过于粗糙，无法应用于实际情况当中。针对B-RRT*算法的这些问题，本发明提出了MB-RRT*算法，通过引入自适应步长、懒惰采样以及贝塞尔曲线优化等方法对B-RRT*算法进行改进优化。
MB-RRT*算法与B-RRT*算法相比在每次迭代过程中多了两个步骤，并且在算法迭代结束后又加入了两个处理方法。
针对背景技术中的问题(1)、(3)，MB-RRT*算法引入了自适应步长的方法去优化算法的收敛率；针对问题(1)、(2)，MB-RRT*算法引入了懒惰采样的方法在提高算法收敛率的同时减少了算法内存占用；针对问题(4)，MB-RRT*算法抛弃了B-RRT*算法直接连接路径点的过程，引入了降采样和贝塞尔三次插值算法使最终生成的路径更加平滑。
自适应步长：传统的B-RRT*算法以固定的步长μ对树进行生长，μ过小会导致树生长过慢，找到初始可行解的速度降低；μ过大会导致生长过程中的采样点很难通过碰撞检测，使采样效率低下，并且过大的步长树的生长很难通过类似窄道的环境。
针对固定步长对树的生长造成的问题，MB-RRT*算法引入了自适应步长的概念，并通过函数AutoStepSteer(x1,x2)实现了树生长过程中自适应的步长。
传统的Steer(x1,x2)函数以矢量x2-x1为方向，距离为步长μ的位置生成点xnew，而AutoStepSteer(x1,x2)首先通过NearestOb(x1,2μMax)寻找并计算出距离点x1最近的障碍物的距离Di，根据下列公式计算出步长：
通过这一方法计算出的步长保持在范围μMin＜μ＜μMax，点x1距离障碍越近，步长越接近μMin，当以x1为中心半径为μMax的范围内没有障碍时，步长为μMax。
图3和图4为节点靠近障碍物情况下的生长，可以看出传统固定步长的生长方法易导致生成的新节点Xnew进入障碍物范围而无法通过障碍检测，而通过本发明提出的自适应步长生长的节点可以在障碍物附近生成有效的采样点。
参照图5和图6，在远离障碍物的情况下，传统固定步长生长的过程由于受限于步长，使其在障碍物空旷的区域生长的较为缓慢(按原有步长生长)。而本发明提出的自适应步长的生长方式按照计算的步长最大值μmax生长，加快了空旷区域探索速度，减少了找到初始可行解的时间。
懒惰采样：传统B-RRT*算法的采样过程保留了RRT*算法的采样过程，对状态空间进行均匀采样，从而继承了RRT*算法的概率完整性。然而B-RRT*算法是采用双向树生长的算法，在算法找到可行解并开始收敛到最优解的过程中，双向树中到各自树根距离大于当前最优解的采样点对最优解的寻找并没有什么意义(算法的最优解不会大于当前最优解)。传统的B-RRT*算法对于这些采样点进行了大量的操作，并且这些采样点也占用了一定量的内存，这在一定程度上减少了算法收敛的速度。
根据B-RRT*算法的这一问题，本发明提出的MB-RRT*算法提出了懒惰采样的方法减少无效采样点。懒惰采样在采样阶段对采样点进行筛选，即在每次生成插入点xnew后计算插入点到达根节点的长度c(xnew,xinit)，如果这个长度大于当前最优路径的长度就抛弃这个插入点。采用懒惰采样可以抛弃了一系列对最优路径搜索没有帮助的新的采样点，因为最优路径节点距离总是小于或等于当前最优路径长，这避免了对无效节点进行碰撞检测，优化等操作，与此同时，抛弃这些采样点可以使算法新生成的采样点收敛在当前最优路径长度的范围内，在不影响算法寻找最优路径的同时减少了树的生长范围。因此，采用懒惰采样这一过程理论上可以在保留算法概率完整性的同时提升算法的收敛速度。
通过进行十次实验取平均值，同样进行20000次迭代，未使用懒惰采样的MB-RRT*算法的平均运行时间为12.20316秒，而使用了懒惰采样的算法的平均运行时间为10.51662秒，加入懒惰采样后的算法的运行效率得到了大幅提升。图7和图8为常规采样的MB-RRT*算法与懒惰采样的MB-RRT*算法在MP3上的运行效果对比图，以初始点为树根生长的树Tinit以及以目标点为树根生长的树Tgoal用黑细线表示。从图中黑色圆圈圈出的区域看出，加入懒惰采样后树Tinit很少在目标点附近采样生长，同时树Tgoal也很少在初始点附近采样生长，这些区域的采样生长对算法收敛到最优解并没有什么帮助，减少这一区域的采样可以树的采样生长点收敛在中间区域。
降采样与曲线拟合：B-RRT*算法对最后生成的最优路径的轨迹点进行直线连接，由于采样的随机性以及树以步长生长的生长方式，算法生成的部分路径轨迹点部分可以进行截短操作从而生成更短的路径，并且直线连接的方式使得算法最终生成一条生硬的折线路径，并不符合实际的运动状况。针对这一情况，MB-RRT*算法在算法迭代完成之后对最后的轨迹点分别进行了降采样处理和曲线拟合处理，使算法得出的最终路径为平滑的曲线。
降采样过程对最终生成的轨迹点之间不断地进行碰撞检测，对最后的轨迹不断地截短直至无法截短。
为了使算法最后生成的路径更加平滑，采用构建三次贝塞尔曲线对轨迹点进行插值生成最后的路径曲线，三次贝塞尔曲线的方程如下所示：
B(t)＝P0(1-t)3+3P1t(1-t)2+3P2t2(1-t)+P3t3
其中P0为起始点，P3为目标点，P1和P2为控制点，用于控制曲线的方向，对于一系列路径点，只需要根据轨迹点不断计算两个控制点带入方程，即可生成曲线。
图9是降采样与曲线拟合示意图，虚线为树生长最后生成的轨迹点，实线连接的端点为经过降采样函数处理后的轨迹点，红色实线为根据降采样之后的端点再进行曲线拟合后生成的最终路径。
在完成曲线拟合后需要对生成的曲线进行碰撞检测，当发现其中的一段轨迹与障碍物发生碰撞，则需要对这段轨迹部分进行插值并重新拟合。插值点可直接选取在碰撞部分的轨迹点的中点，在每次曲线拟合后需要重新进行碰撞检测从而保证最终生成轨迹的安全性。
本发明的有益效果主要表现在：提出了一种MB-RRT*(Modified B-RRT*)算法，采用了懒惰采用的方法提升了算法的收敛率并且减少了算法的内存占用；采用自适应步长的方法解决了算法在障碍物附近生长的局限性问题，并且提高了算法找到初始可行解的速度和质量；利用了降采样和三次贝塞尔插值算法实现了曲线拟合的功能，使算法最后生成相对平滑的路径。
附图说明
图1是MB-RRT*算法的流程图。
图2是B-RRT*算法的流程图。
图3是靠近障碍物情况下的固定步长生长的示意图。
图4是靠近障碍物情况下的自适应步长生长的示意图。
图5是远离障碍物情况下的固定步长生长的示意图。
图6是远离障碍物情况下的自适应步长生长的示意图。
图7是常规采样的MB-RRT*算法在MP3上的运行效果图。
图8是懒惰采样的MB-RRT*算法在MP3上的运行效果图。
图9是降采样与曲线拟合示意图。
图10是插值处理示意图，其中，(a)是插值前，(b)是插值后。
具体实施方式
下面结合附图对本发明作进一步描述。
参照图1～图10，一种基于MB-RRT*的无人机二维航迹规划方法，包括如下步骤：
1)初始化树以及环境信息；
2)导入障碍物信息，并设置迭代次数；
3)判断是否达到迭代次数，如果是，对生成的路径点进行降采样处理，并采用插值算法优化生成路径线，结束；否则进入4)；
4)产生随机采样点Xrand，并寻找树中与随机采样点Xrand距离最近的点Xinsert；
5)根据点Xinsert生成自适应步长，根据步长生成最终插入点Xnew；
6)判断插入点Xnew到树根的距离是否大于当前的最优路径长，如果是，进入9)；如果否进入7)；
7)对路径进行碰撞检测，如果未通过进入9)，如果通过，将插入点加入树中，并对插入点周围的邻近节点进行优化；
8)对树进行连接检测，并进行连接，进入9)。
9)将当前操作的目标树设置为另一颗树，回到步骤3)。
进一步，所述步骤3)中，所述降采样处理：对最终生成的轨迹点之间不断地进行碰撞检测，对最后的轨迹不断地截短直至无法截短。
再进一步，所述步骤3)中，采用构建三次贝塞尔曲线对轨迹点进行插值生成最后的路径曲线，三次贝塞尔曲线的方程如下所示：
B(t)＝P0(1-t)3+3P1t(1-t)2+3P2t2(1-t)+P3t3
其中P0为起始点，P3为目标点，P1和P2为控制点，用于控制曲线的方向，对于一系列路径点，只需要根据轨迹点不断计算两个控制点带入方程，即可生成曲线。
更进一步，所述步骤5)中，自适应步长的计算过程如下：首先通过NearestOb(x1,2μMax)寻找并计算出距离点x1最近的障碍物的距离Di，根据下列公式计算出步长：
通过这一方法计算出的步长保持在范围μMin＜μ＜μMax，点x1距离障碍越近，步长越接近μMin，当以x1为中心半径为μMax的范围内没有障碍时，步长为μMax。
所述步骤4)中，在采样阶段对采样点进行筛选，即在每次生成插入点xnew后计算插入点到达根节点的长度c(xnew,xinit)，如果这个长度大于当前最优路径的长度就抛弃这个插入点。
本实施例中，在任意环境中，算法在路径可行解存在的情况下，运行迭代次数趋于无穷大的时候能够找到可行，那么称该算法具有概率完备性。RRT*算法已经被证明具有概率完备性，同时RRT算法的双向版本B-RRT算法也被证明了具有概率完备性。本发明提出的MB-RRT*算法使用了新的采样策略，即懒惰采样，由于懒惰采样略过对最优路径没有影响的无效节点，而且算法采用双向树结构生长，保证了树的生长覆盖整个环境空间，而且算法附加的曲线拟合以及降采样操作在算法完成迭代后进行，并不对算法的概率完备性产生影响。因此算法依旧保留着RRT*算法的概率完备性。
渐近最优(Asymptotic optimality)在路径规划问题当中的定义为：另c*为当前环境下路径规划的最优解，为算法ALG在迭代到n次之后产生的最优路径解的长度，算法ALG满足渐近最优性当：
已经知道RRT算法并不具有渐近最优性[]，而Sertac Karaman和Emilio Frazzoli证明了RRT*算法具有渐近最优性，同时Matthew Jordan和Alejandro Perez证明双向树版本的RRT*算法具有渐近最优性当其两棵树的连接过程使用类似于RRT*算法加入新节点的过程，即上文提到的Connect函数。本发明提出的MB-RRT*算法并未对连接过程进行修改，单棵树的生长过程继承RRT*算法的同时引入了自适应步长，而Sertac Karaman和Emilio Frazzoli提出RRT*算法的渐近最优性使用于步长η＞0的任意步长。因此可以得出MB-RRT*算法继承了RRT*算法的渐近最优性。
将MB-RRT*算法的计算复杂度与B-RRT*算法和RRT*的计算复杂度进行对比，分析MB-RRT*算法的计算复杂度。
首先，Sample、CollisionCheck、Steer等函数均可在常数时间内完成，而AutoStepSteer仅仅在传统Steer函数中多调用了一次NearestOb方法，这一方法和CollisionCheck类似，对空间范围内的障碍物搜索，由于障碍物数量是固定的并不随迭代次数增长，所以AutoStepSteer函数也可以在常数时间内完成。其次，Sunil Arya已经证明Nearest函数需要对数的时间进行即Ω(logn)，同时与Nearest函数类似，Near函数作为一个邻近点搜索算法，它的计算时间也为logn[]。最后，MB-RRT*算法的连接过程Connect函数延续了传统B-RRT*算法的连接方法，因此其计算时间和Near函数相同，为logn。根据上述条件可以总结出，存在一个常数φ1，使得
同时结合Yasar Ayaz的定理，可以推论出，存在一个常数φ2，使得
其中x为迭代次数，为算法ALG在x迭代后所执行的步骤数量。
本实施例的MB-RRT*算法与现有的B-RRT*算法和RRT*算法的对比，得出以下结论：
(1)MB-RRT*算法与B-RRT、RRT*和B-RRT*算法相比可以以更少的时间和迭代次数找到初始可行解，并且找到的初始可行解更优；
(2)MB-RRT*算法、B-RRT*算法和RRT*算法的计算复杂度十分接近，MB-RRT*算法的额外步骤没有对计算复杂度造成太大的影响；
(3)MB-RRT*算法与B-RRT*算法和RRT*算法相比有着更快的收敛率，可以更快的收敛到最优解；
(4)MB-RRT*算法最终生成的路径线比B-RRT*算法和RRT*算法更加平滑。
- 还没有人留言评论。精彩留言会获得点赞！