基于改进RRT算法的无人机实时航迹规划∗

（海军工程大学基础部 武汉 430033）

1 引言

随着现代军事科技的迅猛发展，军民科技的深度融合，现代战场已经越来越趋向于无人化、精准化、自动化。无人机以其目标小、成本低、可远程操控与自主控制相结合的特点，逐渐在现代战场中扮演着越来越重要的角色［1］。无人机在各类约束下寻求最优或次优的航迹规划［1］也越发重要。同时，战场环境瞬息万变，无人机在不确定环境下的实时航迹规划能力的提升尤为迫切，这就对航迹规划算法的速度、效果提出了更高的要求。

传统的航迹规划算法在实时航迹规划方面，都显现出了一定的局限和不足。传统的航迹规划算法主要分为启发式算法和数学优化算法［2］。启发式算法包括模拟退火［3］、蚁群算法［4］、遗传算法［5］、A*算法［6］、粒子群优化算法［7］等。常用的遗传算法在环境复杂时收敛速度下降，尤其在接近最优解时收敛速度会极低［5］，不满足实时航迹规划的速度要求；A*算法由于不能每次都保证结果正确，且可能因为某一估计函数陷入漫长的搜索过程中［6］，不满足实时航迹规划的速度要求；蚁群算法相比其他算法需要时间较长，且容易陷入局部最优解［4］，也不适用于实时航迹规划。数学优化算法包括最优控制、动态规划、牛顿法以及一些搜索方法如Dijkstra法［8］、Voronoi图法［9］、快速扩展随机树（RRT）法［10］等。Voronoi图法由于通常需要结合遗传算法等智能搜索算法搜索最优路径，耗时较长［9］，不适用于实时航迹规划；Dijkstra法尽管可以找到最优路径，但由于搜索效率较低［8］，不适用于实时航迹规划。

快速扩展随机树（RRT）算法作为目前广泛使用的实时航迹规划算法，具有无需对环境建模，只需通过在状态空间随机扩展节点，就可构造出可行路径，节点扩展的随机性保证了路径可以避开威胁区域，且可以同时考虑无人机动力学约束等问题。目前，国内外在路径规划方面已经通过RRT算法取得了大量实践与应用［11］。但RRT算法的不足也很明显，由于节点扩展的随机性较强，往往需要大量次数迭代才可以找到可行航迹，消耗大量时间且航迹无法保证最优。同时，由于扩展步长的固定，在靠近威胁区域时，生长树具有局限性，需耗费大量时间才可绕过威胁区域。而且，由于无人机自身存在动力学约束，节点生长无法完全随机。

因此，本文对原始RRT算法予以改进。首先，增加动态步长，通过计算当前节点与最近威胁的距离合理确定步长；同时，结合人工势场法中目标引力的概念，引入自适应目标引力，将节点生长引导向目标方向；最后，在节点选取时考虑无人机的动力学约束，使航迹规划算法更加可执行。实验证明该改进算法可以快速规划出较优航迹。

2 任务数学描述

2.1 无人机性能约束

无人机在执行一次任务期间，由于受到自身机动性能、燃油携带等限制，需要满足如下约束：

1）最小飞行距离约束：最小飞行距离，是指无人机在改变飞行状态之前需要保持直线飞行的最小距离，即当无人机的直线飞行距离超过最小飞行距离之后，才可以改变飞行姿态［12］。图1中总航程可表示为设定最小飞行距离为lmin，则最小飞行距离约束则表示为lm≥lmin。

图1 无人机航迹示意图

2）最大转弯角约束：无人机在调整方向时由于惯性以及无人机自身性能约束，改变航向的角度需要小于最大转弯角θ，如图2所示。

图2 最大转弯角示意图

3）最大航程约束：由于无人机携带燃油限制等原因，无人机执行一次任务所飞行的总航程应小于最大航程Lmax，则最大航程约束表示为。

由于无人机在飞行期间一般保持固定高度飞行，所以本文设定无人机飞行高度固定，不考虑俯冲角、爬升角等约束的影响，只涉及二维平面的航迹规划。

2.2 环境威胁建模

无人机执行任务时的航迹规划，需要考虑各类威胁情况，主要包括雷达、防空武器、地形威胁、恶劣天气等。不同种类的威胁对无人机的影响程度不同，对于不同的威胁，无人机的应对方式也不同，下面分别对地形威胁、气象威胁、雷达威胁、防空威胁进行建模。

1）地形威胁：主要指山峰、高地等地形可能对飞行造成的威胁。以圆锥体近似表示山峰、高地等，在固定高度上，地形威胁可近似表示为一个威胁半径为rT的圆周。无人机飞入地形威胁半径rT时，认为飞机损毁；飞出威胁半径rT时，认为对飞机无影响。

2）气象威胁：主要指飓风、雷暴等恶劣气象条件对无人机造成的威胁。以圆柱体近似表示飓风、雷暴等，在固定高度上，气象威胁可近似表示为一个威胁半径为rc的圆周。无人机飞入气象威胁半径rc时，认为飞机损毁；飞出威胁半径rc时，认为对飞机无影响。

3）雷达威胁：主要指敌方雷达对我方无人机的探测威胁。由于现代军事科技发展，雷达大部分为360度全向雷达，在二维平面上，其探测范围可以看似为圆周。由于无人机与雷达距离越近，被探测到的可能性越大［13］，所以本文将雷达探测区域以不同的探测半径，分为绝对探测区域和相对探测区域两个同心圆，如图3所示。当无人机飞入绝对探测半径rRmin时，认为飞机被探测并损毁；当飞入绝对探测半径rRmin与相对探测半径rRmax之间时，认为可能被探测；当飞出相对探测半径rRmax时，认为对飞机无影响。

4）防空威胁：主要指敌方防空导弹、高射火炮等防空武器对无人机造成的威胁。在二维平面上，防空武器的威胁半径可以近似看做圆周［14～15］，由于无人机与防空武器的距离越近，受到的打击威胁越大，所以本文将防空威胁区域以不同的打击半径，分为绝对打击区域和相对打击区域两个同心圆，如图4所示。当无人机飞入绝对打击半径rMmin时，认为飞机被击落；当飞入绝对打击半径rMmin与相对打击半径rMmax之间时，认为可能被击落；当飞出相对打击半径rMmax时，认为对飞机无影响。

图3 雷达威胁示意图

图4 防空威胁示意图

由于要保证无人机成功执行任务，防止无人机出现损毁的可能，本文将雷达威胁和防空威胁的最大威胁范围视为威胁范围，即均为一个以最大威胁半径为半径的圆周，当飞入威胁区域时，认为无人机损毁。

3 RRT算法的改进策略

3.1 增加动态步长

图5 动态步长示意图

在传统RRT算法进行航迹规划时，如果设定的步长较小，会导致随机树生长过慢，影响求解速度；如果设定的步长较大，在威胁环境较多的情况下，新节点会很难通过威胁检测，同样影响求解速度。为了提高航迹规划效率，本文引入了动态步长的策略。如图5所示，Pk为环境中第k个威胁源，qrand为随机采样点，qnear为随机树中距离qrand最近的节点，qgoal为目标点，qini为起始点，qn为由起始点向外扩展的第n个节点，qnew为新节点。ρ为搜索步长，ρmin'ρmax为设定好的最小、最大步长，Dthr为qnear与其最近威胁的边缘之间的距离。

在确定qrand之后，搜索并计算距离qrand最近的威胁边缘之间的距离 Dthr，并根据设定的ρmin'ρmax以及动态步长公式：

确定qrand的步长。通过这一方法确定的步长保持在最大与最小步长范围之间，当qrand距离威胁越近，步长越接近 ρmin，当以qrand为圆心，ρmax为半径范围内没有威胁时，步长为ρmax。

图6为靠近威胁的情况下，传统步长与动态步长的区别。可以看出，在靠近威胁的情况下，传统步长可能会因为无法通过威胁检测而进行重新采样，从而影响航迹规划效率，而动态步长则可以通过缩小步长而通过威胁检测。

图6 靠近威胁情况下，传统步长与动态步长区别示意图

图7 为远离威胁的情况下，传统步长与动态步长的区别。可以看出，在远离威胁的情况下，传统步长由于步长相对较小，会导致随机树生长缓慢，从而影响航迹规划效率，而动态步长则可以通过增大步长，提高随机树的生长速度，从而提高航迹规划的效率。

图7 远离威胁情况下，传统步长与动态步长区别示意图

3.2 增加自适应目标引力

在进行航迹规划时，我们既希望可以有一定的随机性从而避开威胁区域，又希望可以尽可能地向目标方向行进。基本RRT算法由于在扩展节点时从全局无威胁空间进行采样，随机性较强，得到的最终航迹往往不是最优航迹，且算法耗时较长。因此，本文引入人工势场法［16］中的引力思想，在保证随机树具有一定随机性的同时，引导随机树向目标方向生长，优化航迹的同时缩短了规划时间，提高了算法的实时性。同时根据与威胁区域的距离设定自适应引力系数，保证随机树可以准确避开威胁区域的同时更快的向目标区域生长。

改进的核心思想是在每个节点qn都增加一个节点生长函数F(qn)，表示为

其中，F(qn)为从节点qn到新节点的生长函数，R(qn)为从节点qn到新节点的随机生长函数，G(qn)为从节点qn到新节点的目标引力函数。

由人工势场中的目标对qnear的引力势能［16］：

可以得到目标对qnear的引力：

kp为引力系数，‖qgoal-qnear‖ 为qgoal与qnear之间几何距离的绝对值。根据式（4）可以构造出节点qn到新节点的目标引力函数G(qn)：

在增加目标引力思想之后，RRT算法生成新节点时，通过计算目标引力函数来指导新节点在一定程度上向目标生长。

在基本RRT算法中，从节点qn到新节点的随机生长函数R(qn)为

将式（5）、（6）带入式（2）中可以得到从节点 qn到新节点的生长函数F(qn)为

根据式（7）可以得到增加目标引力思想后的新节点生长公式：

可以看出，引力系数的大小直接影响着新节点的生长方向。由于我们既希望随机树生长时可以有一定的随机性来避开威胁区域，又希望可以尽可能地向目标方向行进，所以本文引入了自适应引力系数：

即当远离威胁的情况下，kp取较大值，减小随机树的随机性，引导新节点尽可能向目标生长，同时，结合动态步长的确定，进一步提高航迹规划的效率；当靠近威胁的情况下，kp取较小值，使随机树可以避开威胁区域，向无威胁区域生长，同时，减小动态步长，以顺利通过威胁检测。

3.3 考虑无人机性能约束

1）最小飞行距离约束：考虑2.1节中的最小飞行距离约束，结合提出的动态步长策略，可知最小步长ρmin应不小于最小飞行距离Lmin，即

本文为加快求解速度，提高航迹规划效率，设定 ρmin=Lmin。

2）最大转弯角约束：考虑2.1节中的最大转弯角约束，如图8，令航迹段qnear_rootqnear和qnearqnew在二维平面的水平投影为

图8 最大转弯角示意图

3）最大航程约束：考虑最大航程约束，实际总航程L应小于最大航程Lmax，即

设最大转弯角为θ，则新节点qnew必须满足

4 改进RRT算法流程图

基于改进RRT算法的无人机航迹规划的具体步骤如下：

图9 改进RRT算法流程图

5 仿真实验与结果分析

实验环境：Windows 10，Intel（R）Core（TM）i7-6700HQ CPU@2.60GHz，内存8G。

编译工具：Matlab 2015b。

参数设置：无人机任务区域为500×500，X轴Y轴范围均为［0，500］，起点位置的坐标为（10，490），终点位置的坐标为（490，10）。动态步长 ρmin=20，ρmax=40，引力系数，最大转弯角为60°，最大航程 Lmax=1000。迭代次数为10000，实验次数为100。

分别在任务区域随机生成5个，10个，20个大小不一的威胁圆，并且分别运用原始RRT算法和改进RRT算法在相同环境下进行航迹规划，记录扩展节点数、航迹长度、运行时间，取100次实验结果的平均值，并计算航迹长度缩短率，同时分别记录两种算法进行实验的失败次数。仿真效果如图10所示，三种威胁数量情况下，航迹均明显缩短，且接近最优航迹。

图10 不同威胁数量下，仿真效果对比图

表1 仿真数据对比

从表1中可以看出，扩展节点数方面，三种威胁数量情况下，节点数均减少约50%；航迹长度方面，三种威胁数量情况下，改进RRT算法相比原始RRT算法的平均航迹长度缩短率分别为5.92%、8.98%、1.92%，并且由于航迹长度基数较大，实际航迹长度明显缩短；运行时间方面，改进RRT算法相比原始RRT算法，除5个威胁情况下时间缩短之外，其余时间均略有增加，但仍处于毫秒级别，完全可以满足无人机实时航迹规划的需求。

实验结果表明，基于改进RRT算法的无人机航迹规划算法，在简单、复杂的环境下，均可以快速、有效地构造出避开威胁区域，同时满足无人机性能约束的较优航迹。相比原始RRT算法，在航迹长度方面有较大提高，且消耗时间较短，满足无人机实时航迹规划的需求。

6 结语

本文针对原始RRT算法在无人机航迹规划方面，存在的节点生长随机性过强，航迹无法保证最优；搜索步长固定，在靠近威胁区域生长树具有局限性；没有考虑无人机动力学约束等问题，提出了改进RRT算法，并进行了仿真实验。改进算法通过增加动态步长，解决了靠近威胁区域生长树局限的问题；通过在节点生长时增加自适应目标引力，将节点生长向目标方向引导，解决了RRT算法随机性过强的问题；最后在节点选取时考虑无人机动力学约束，使规划出的航迹满足无人机的实际性能要求。实验结果表明，改进RRT算法可以快速、有效地规划出较优航迹，满足无人机实时航迹规划的需求。同时，本文只在二维平面内进行航迹规划，与实际的三维环境航迹规划还有一定差距，在未来的工作中，将进一步研究三维环境下的航迹规划，同时深入研究不确定环境下的实时航迹规划。