改进RRT算法在无人机航迹规划中的应用∗

江泽强 倪霄汉 王 鑫 姜元杰

（1.国防大学训练部公共平台中心 北京 100091）（2.95894部队 北京 102211）

1 引言

无人机航迹规划是指在给定起始点和目标点位置，并综合考虑无人机的飞行约束条件下，寻找一条从起始点到目标点的最优或可行航迹，以保证无人机能够完成飞行任务［1］。目前，常见的航迹规划算法主要有A*算法、遗传算法、蚁群算法、粒子群算法等［2～5］。但是随着现代战争进程的发展及战场情报体系的日益完善，航迹规划需要考虑的约束条件越来越多，对无人机的航迹规划算法的求解效率也提出了越来越高的要求［6］。快速扩展随机树（Rapidly-Exploring Random Tree，RRT）算法是一种基于采样的单查询随机搜索算法，1998年由S.M.LaValle首次提出［7］。该算法相对于其他算法，计算简单且不需要对空间建模，可直接根据当前环境状况快速有效地搜索规划空间，在高维空间中速度优势尤为明显，且该算法在可行路径的搜索概率意义上是完全的，已成功应用于许多非完整系统规划问题中，包括移动机器人运动规划和飞行器路径规划等问题［8］。

虽然RRT算法具有诸多优良特性，但其缺点也较为明显，因而限制了进一步应用:1）在搜索过程中并未对航迹综合代价进行考虑，而是在规划空间内进行均匀一致搜索，导致无谓损耗代价较大；2）节点选择的随机性使生成的航迹随机性很大，同一条件下的规划过程缺乏可重复性；3）基本RRT算法随机性太强，得出的往往是可行航迹而非最优或次优航迹。针对这些不足，研究者提出了很多改进方法［9～15］，在保证可行性的同时提高了算法的搜索效率。但是目前多数研究针对于机器人，其任务环境比较简单，而无人机的任务环境往往更复杂，对算法性能有着更高的要求。因此，本文对RRT算法的节点的采样方式和节点扩展方式进行了改进，来降低随机性对算法的影响，从而达到提高航迹规划效率、改善航迹规划质量的目的。

2 RRT算法基本原理

基本RRT的节点扩展过程如图1所示［8］，将起始点xinit作为树的根节点，当扩展新节点时，首先从规划空间内随机选择一点xrand，，然后在当前RRT树中找到距离xrand最近的节点xnear，并以一定的扩展步长L来计算新节点xnew，如果在向新节点xnew行进的过程中遇到障碍或威胁，则需要重新选择xrand迭代计算；如果新点xnew是可行的，则将新点xnew添加到随机树中，并建立节点之间的连接关系，即xnew是xnear的子节点。

在整个随机树的扩展过程中，随机点xrand的选取原则如下:在采样时以一定偏向概率PG选取目标点xgoal为随机点进行扩展，以概率1-PG在未探索区域内随机选取一个点作为随机点进行扩展。当随机树的叶节点中包含了终点xgoal或者与终点xgoal距离小于阈值ε时，则认为已到达目标点，随机树扩展停止。此时，便可以在随机树中找到一条以树节点组成的从初始点xinit到终点xgoal的规划路径。

图1 RRT的节点扩展示意图

3 RRT算法改进策略

3.1 改进节点的采样方式

由基本RRT算法可知，在航迹规划时扩展树中的节点均由xrand扩展而来，xrand节点选取是否合适将直接影响最终生成航迹的质量。因此，为了得到质量更高的xrand节点，本文引入了多重随机采样策略，即从规划空间中产生一组随机采样｛1，2，…，n｝），然后对这一组点中选择最优的xrand进行扩展。

为了得到最优xrand，则需要有相应的评价函数来对该组节点进行评价。如图2所示，由基础数学知识可得最短航迹一定是起始点xinit和终点xgoal的连线，因此对x1rad和x2rad比较可知选择离最短航迹近的xrand可缩短剩余航迹长度。同时，为了加速RRT朝目标方向扩展，比较和可得，离终d点xgoal越近剩余航迹距离越短。

因此，本文在对采样点进行评价时，引入了距离和方向两个要素，即:

如图2所示，其中d表示采样点到终点的欧氏距离，α表示起始点到目标点与采样点到目标点的夹角，ω1、ω2分别是距离和角度的权重系数。

图2 节点评价函数构造示意图

由于距离和方向的量纲不统一，因此还需对该评价函数中的d和α进行归一化处理，即计算k个采样点的距离和角度值，然后求出其平均值d和α，即:

求出均值后，再对每一个采样点对应的距离和角度进行归一化处理，因此评价函数变为

其中:

进行处理后，即可根据采样点距离和角度的贡献，选择评价函数值最小的点作为最终采样节点。采用该方法进行采样看似增加了计算量，但由于采样节点质量提升，使得扩展过程中的无效探索次数显著减少，从而提高了算法的整体搜索效率。

3.2 改进节点的扩展方式

3.2.1 自适应转弯角调整

在进行节点扩展时，需要满足无人机的飞行约束条件，因此还需要根据无人机的最大转弯角φmax和扩展步长L等限制条件来对节点进行扩展。如图3所示，当确定的采样点xrand在可行域之外时，由于不满足无人机的转弯角约束而被舍弃。而RRT算法是一种随机算法，在实际扩展过程中会有大量节点因为未能满足约束要求而被舍弃，造成了大量计算资源被浪费。因此，本文通过引入随机变量rk对扩展节点的转弯角进行动态调整，使扩展出的节点能够包含在无人机的可行域范围内。

图3 改进节点扩展示意图

因此，改进后的xnew计算方式为

其中，rk的取值决定了节点的偏转方向和偏转角度，为了加速向目标方向扩展，减少无效探索次数，通过判断xnear与xgoal的相对位置来确定rk的取值范围，即:

1）当xgoal在可行域上方时，rk∈［0，1］

2）当xgoal在可行域下方时，rk∈［-1，0）

3.2.2 自适应步长调整

传统的RRT算法在进行扩展时，通常以固定步长L进行扩展，如图4所示当扩展节点在威胁区内或经过威胁区时，即代表此次扩展失败。这就导致在威胁较多时或部分威胁密集区域容易使探索过程陷入局部极小区域，无效探索次数上升，浪费了计算资源。为了避免上述缺陷，本文结合最小直飞航程约束l对扩展步长进行了动态调整，在有效规避威胁的情况下使算法加速朝着目标方向扩展，及时跳出局部极小区域。

图4 固定步长扩展示意图

具体方法如图5所示。

1）根据无人机的最小直飞航程约束l将固定扩展步长L分为 n份记为l1，l2，…，ln-1，ln，等分点分别记为p1，p2，…，pn-1；

2）从点 p1开始逐步判断 l1，l2，…，ln-1，ln与是否穿越障碍；

3）假设ln-1为首次与障碍物相交，则可知ln-2到l1均在威胁外，pn-2为最后一个不与威胁相交的等分点，即xnew=pn-2；

4）将xnew加入扩展树T，完成此次扩展。

图5 自适应步长扩展示意图

3.3 改进后航迹规划算法流程

Step1:设xinit为无人机的初始位置，初始化搜索树T，初始化无人机的航迹规划空间。

Step2:按照以下步骤扩展搜索树T:

Step2.1:设偏向概率为PG，p为［0，1］的随机数，如果p＜PG，则直接将xgoal作为随机点xrand；否则，根据3.1节中改进的节点采样方式在规划空间内产生一组随机点（k∈｛1，2，…，n｝），然后根据式（4）对这一组采样点进行评价择优，选取评价函数值最小的点作为xrand。确定好xrand后，进入Step2.2。

Step2.2:在搜索树T中找出距离xrand距离最近的节点xnear，并结合无人机的飞行约束，根据3.2中改进的节点扩展方式计算xnew节点，然后进入Step2.3。

Step2.3:如果xnew与xnear的连线不在任何威胁范围内，则将xnew加入搜索树T，否则返回Step2.1。

Step2.4:如果||xnew-xgoal||≤ε，即认为已经到达目标点，进入Step3；如果搜索树的节点探索次数超过最大探索次数Nmax时，则表示此次扩展失败，强制结束该算法，输出失败信息；否则，返回Step2。

Step3:从目标点逆向回溯至扩展树的根节点，并绘制从xinit到xgoal的规划航迹。

4 仿真实验

为了验证本文改进策略的有效性，本文在Windows 7操作系统上基于Matlab编程实现该算法，在 Inte®CoreTMi7-3770K CPU@3.5GHz，内存16G的PC机上进行仿真实验。假设规划空间大小为200km×200km，空间内存在的威胁均以黑色实心圆表示，起始点坐标为（0，0），目标点坐标为（180，190），步长L为8，最小直飞航程约束l为2，偏向概率PG为0.2，最大转弯角φmax为60°，采样数k为5，ω1、ω2均取0.5，最大探索次数Nmax为5000。

本文共设置了四组对比实验，每组实验进行30次，统计平均值。其中实验1为未经改进的基本RRT算法，实验2仅对节点的采样方式进行了改进，实验3仅对节点的扩展方式进行了改进，实验4同时对节点的采样方式和扩展方式进行了改进。所得实验数据如表1所示，规划出的最优路径如图6所示。

表1 对比实验数据

从表1所得结果可以看出实验2中对节点的采样方式进行改进后扩展节点质量得到提高，平均节点数减少，规划出的航迹长度明显减小，但由于探索次数增加，规划效率并未得到显著提升；实验3中对节点的扩展方式进行改进后提高了节点探索的成功率，降低了无效探索次数，规划时间明显降低，规划出的航迹长度也明显减少，但由于探索时节点质量不高，因而产生的节点数较多；实验4中同时采用了实验2和实验3中的改进策略后，探索次数和节点数均有所下降，平均规划时间减少，算法效率得到提升，还进一步缩短了规划出的航迹长度。同时，从图6的最优路径结果示意图中也可以看出实验4所得出的最优航迹较为平滑，更加接近最优解，也更加符合无人机航迹规划的要求。

图6 最优路径结果示意图

5 结语

针对RRT算法在无人机航迹规划时存在的不足，本文在基本RRT算法的基础上进行了优化。通过对节点采样方式和扩展方式的改进，降低了RRT算法的随机性，提高采样节点质量，有效减少了航迹规划中的无效探索次数，降低了规划时间，90ms的平均规划时间足以满足无人机快速航迹规划要求。