张 康，陈建平

（南京航空航天大学航空学院，南京 210016）

0 引言

航迹规划是指在任务空间中找到无人机（Unmanned Aerial Vehicle，UAV）从初始状态到指定目标状态的可行航迹，其应用背景十分广泛，如无人驾驶汽车、计算机辅助外科手术、移动机器人等［1-3］。

现有的航迹规划算法主要分为确定性方法和随机性方法。确定性方法有人工势场法［4］、A*算法［5］、智能算法等。人工势场法需要根据环境信息预先构建势场，容易陷入局部最优，且在狭窄通道里会出现摆动现象，不适合复杂环境的应用；A*算法在高维空间中会出现组合爆炸的问题；以粒子群［6］为代表的智能算法通常会用来求解满足性能指标的最优航迹问题，但一般需要大量迭代才能收敛，计算开销大，运算时间长。

概率路线图（Probabilistic Road Map，PRM）法［7］和快速扩展随机树（Rapidly-exploring Random Tree，RRT）法［8］是当前使用最广泛的随机采样方法，相较于确定性方法的优势在于使用碰撞检测技术避免了对障碍物的显示建模，直接在当前任务空间中生成可行航迹，具有实时处理复杂环境下航迹规划问题的能力。PRM 算法的性能对障碍物形状严重依赖，难以在不确定环境中发挥作用。相比之下，RRT 算法对环境波动不敏感，且支持非完整约束。这些优点使得RRT 在近年来得到了广泛的应用和发展。最具代表性的是文献［9］中提出的具有渐进最优性的快速扩展随机树（Rapidly-exploring Random Tree star，RRT*）算法，它针对RRT算法非最优性的缺点，在每次迭代中通过对新扩展节点的近邻优化来保证算法的渐进最优性。但该算法仍然存在一些不足：1）寻路没有方向性，规划过程中会出现很多重复探索；2）没有利用已有树的信息，对整个空间采样导致添加了大量不在航迹附近的无用节点，收敛速度缓慢。

为了增强RRT 算法的鲁棒性，文献［10］中提出了基于动态域的快速扩展随机树（Dynamic Domain Rapidly-exploring Random Tree，DDRRT）算法。文献［11］中则通过引入地图的代价模型，改善了DDRRT可能造成的对采样空间过度约减的问题。

为了提高RRT*的收敛速率，文献［12］中通过对剪枝处理后的可行路径节点附近集中采样，从而减少了大量的无用采样。文献［13］中借鉴文献［12］的思路提出了可调边界的渐进最优快速扩展随机树（RRT*-Adjustable Bounds，RRT*-AB）算法，在迭代中一旦找到可行路径，便在起始点和目标点之间建立一个可调整边界的连通域作为采样区域，同样达到了集中采样的效果。文献［14］中提出的带启发式采样的渐进最优快速扩展随机树（Informed-RRT*）算法将采样空间限制在一个超椭球里来达到优化采样空间的效果。文献［15］中提出的固定节点数的渐进最优快速扩展随机树（RRT*-Fixed Nodes，RRT*-FN）算法通过限制节点的最大数量来减少算法的占用内存。文献［16］中将Informed-RRT*和RRT*-FN 算法结合，通过启发式采样和删除低权重的叶子节点的方法来加快收敛速度。文献［17］中通过机器学习的方法得到新的非均匀抽样分布，该分布只在非障碍区域采样，极大减少了碰撞检测次数。

针对RRT*算法在复杂环境下的寻路效率不足、收敛速度缓慢的问题，本文提出了基于采样空间自调整的渐进最优快速扩展随机树（Adjust Sampling space-RRT*，AS-RRT*）算法。该算法在迭代过程中的采样空间随着树的不断生长而自动调整，算法流程分为搜索和优化两个阶段：搜索阶段不采取近邻优化，在迭代中根据当前树生长情况选择合适的采样策略，目的是更快速地找到初始航迹；优化阶段根据算法的近邻优化次数，周期性地更新高质量节点，并通过学习高质量节点产生新的抽样分布、删除低质量节点来保证树在最优路径附近高效生长，加快了收敛速度，降低了算法的内存占用。在不同类型的环境下与传统的RRT*算法进行了对比仿真实验，结果验证了本文算法的有效性。

1 相关工作

1.1 RRT*

RRT*算法［9］是RRT 算法中具有渐进最优性质的优化版本，在每次成功扩展节点后通过和近邻节点互相重选父节点来降低树的总代价，当采样量足够大时，算法会最终收敛到最优航迹。该算法大致流程如算法1 所示。首先，初始状态xinit被添加到树的根节点中（第1）行）；主循环第2）～12）行表示在N次迭代后终止。只有通过碰撞检查的边才能添加到树中。近邻优化确保新节点连接到搜索树中的最佳顶点。碰撞检测技术不需要约束的显式表达，具有实时处理复杂环境下航迹优化问题的能力。

Sample：随机地在状态空间均匀采样，生成一个状态点。

Nearest：通过一个度量函数Distance 来衡量节点与采样点的距离，函数Nearest 返回搜索树T中与采样点距离最近的节点。

Steer：给定两点xnearest和xrand，Steer 函数返回扩展的新节点xnew，在满足最大生长步长r的情况下，使新节点xnew尽可能地靠近xrand。

CollisionCheck：检查连接到新节点的航迹是否通过碰撞检测，满足则返回ture，否则返回false。

Near：从搜索树T中筛选出与新节点的距离小于γ的节点，组成一个节点集并返回。

其中：γ=k(logn/n)1/d表示近邻优化半径；k是与规划空间尺寸有关的常数；n为节点数量；d为规划空间的维数。

Chooseparent：遍历节点集Vnear为xnew重新分配父节点指针，返回使xnew代价最低的父节点。

Rewire：如果重连接后代价更低，则Vnear中的节点重新分配父节点指针到xnew。

RRT*算法虽然保证了RRT 算法的渐进最优性，但由于没有在探索和优化做一个合理的权衡，一成不变地对整个空间进行均匀采样，导致了很多优化操作不能集中在最优航迹附近，也就是说空间中那些没有降低最终航迹代价的采样是无用的，浪费了大量计算资源。

1.2 改进RRT*-FN

Informed-RRT*算法［14］是为了减少无用采样、提高收敛速度的一个RRT*改进版本。该算法通过当前可行航迹的最小代价来生成一个超椭球采样空间，减少了在无用区域的采样。RRT*-FN 算法［15］通过预设值最大节点数量来减少算法所占用的内存。改进RRT*-FN［16］结合了前两者的优点，在搜索到初始航迹后通过启发式采样把采样空间限制在椭圆子集和航迹点的邻近区域，在达到预设最大节点数量后删去不在启发采样区域的叶子节点，进一步加快了收敛速度。改进RRT*-FN算法的大致流程如算法2。

改进RRT*-FN 在RRT*基础上增添了启发式采样和节点删除的步骤，虽然有效提升了收敛速度，减少了计算占用内存，但是仍有一些不足之处：1）在找到初始航迹前使用RRT*算法均匀搜索整个空间，造成了许多对无用区域的近邻优化操作；2）椭圆子集采样更适合最终航迹长度和起始点到目标点的直线距离相差不大的情况，否则，椭圆区域可能会大到覆盖整个采样空间。

2 问题定义

令规划任务的状态空间通过集合X⊂Rn来表示，n∈N为状态空间维数。Xobs⊂X用来表示空间中的障碍区域，Xfree=Xobs/X表示空间中的自由区域；xstart∈Xfree为起始点，xgoal∈Xfree为目标点；对于自由空间中任意两状态点x1∈Xfree，x2∈Xfree，定义一个连续函数π：[0，s]来表示连接两点的一段可行航迹（π(0)=x1，π(s)=x2），s表示航迹代价；令空间中所有的可行航迹πf∈Xfree（πf(0)=xinit，πf(s)=xgoal）由一个集合Σf表示。算法将通过构造搜索树T来找寻航迹，主要考虑了以下两个问题。

问题1 在限定搜索时间里找到一条可行航迹πf∈Σf（πf(0)=xinit，πf(s)=xgoal）。

问题2 在有限时间里不断优化可行航迹πf∈Σf，使航迹代价s最小。

3 AS-RRT*

为了减少寻路时间、降低航迹代价，本文提出的AS-RRT*算法将流程分为搜索和优化两个阶段，分别采用不同的采样策略和扩展策略，目的是同时保证快速性和优化性。算法大致流程如算法3所示。

算法在初始化参数后进入搜索阶段（第2）～5）行），基于树的生长情况的自适应选择向光区采样和背光区采样（第3）行），引入一个基于碰撞增量的代价模型来降低障碍物附近节点被扩展的概率（第4）行），此阶段不考虑近邻节点的优化，目的是快速找到一条可行航迹（第6）行）。在找到初始航迹Πinit后，筛选高质量节点放入集合Velite中（第7）行），使用一种多变量概率模型来描述抽样分布，参数α表示概率模型的均值和协方差矩阵（第8）行）。接着进入迭代优化阶段（第9）～17）行），基于近邻优化次数NOT（Optimized Times）来更新精英集和抽样分布（第12）～16）行），通过Matlab 的mvnrnd 函数生成采样点（第10）行），同RRT*一样对新扩展节点采取近邻优化，当节点数量超过限定值时，则删去树中低质量叶子节点（第11）行）。

3.1 搜索阶段

向光采样和背光采样依据当前节点总的扩展失败率οEFR(Expansion Failure Rate)来选择，它是扩展失败次数和搜索次数的比值，反映了当前树的生长情况。如果扩展失败率低，说明障碍物的影响较小，算法会趋向于在目标点附近的向光区域采样；反之，则说明障碍物影响较大，算法会在远离树中心的背光区域外采样，加强探索力度引导树逃离障碍区域。搜索阶段的采样和扩展策略如算法4和算法5所示。

3.1.1 向光区域均匀采样

向光区域是一个以目标点为中心的超球锥，对n维空间下的超球锥子集的均匀采样可以通过约束一个超球的极坐标来方便地实现。

图1 是在一个中心有障碍的简单二维环境下树的生长情况，方块为起始点，圆圈为目标点，黑色表示障碍物。一开始，向光区域为起始点和目标点间的连线，保证在自由空间里以最高效率向目标点生长；在与障碍物发生碰撞后，向光区域扩张为一个扇形，由于扩展失败率很低，向光区域可以继续引导树向目标点生长。

图1 向光区域采样Fig.1 Sampling in light area

3.1.2 背光区域

背光区域的形状是一个超球，超球的中心是当前树的中心xcenter，也就是所有节点坐标的均值，半径r为xcenter到扩展失败次数最多的节点x″的距离，在背光区域外的采样点xrand满足以下约束：

为了减少不必要的碰撞检测，引入一个基于碰撞增量的代价模型C，在计算最近的节点时，那些扩展失败的节点将被考虑额外的代价。

其中：n为单个节点的扩展失败次数；k的取值范围为0到1，目的是保证不会高估节点的额外代价。

图2表示的是一个规划难度较高的Bug trap 难题［10］，起始点在凹障碍物内，且与目标点无法直接相连。算法在向光区域多次尝试无果后转而在背光区域外采样，因此障碍物包围区域里的节点内疏外密，而且由于考虑了额外代价，在障碍物附近的节点出现了多分枝现象，相较于传统RRT 算法用了更少的扩展次数和碰撞检测次数。

图2 背光区域采样Fig.2 Sampling in dark area

3.2 优化阶段

3.2.1 节点筛选

基于降低航迹代价的原则筛选节点，定义节点x质量高低的指标函数为J，它与当前可行航迹Πcurrent有关，Πcurrent表示起始点到目标点的节点序列。

其中：J1代表了节点到当前航迹的最小距离，J2代表了节点到连接起始点和目标点间直线的距离；k1和k2表示权重系数。指标函数J越低则节点的质量越高。算法6为筛选节点流程。

3.2.2 机器学习模型

合适的机器学习模型可以更好地描述数据。高斯混合模型由多个单高斯分布组成，当分模型个数K选取合适时，可以用来逼近任意的抽样分布。

高斯混合模型被定义为：

现已有很多成熟的算法求解高斯混合模型的参数，本文采用最大期望化算法求解高斯混合模型参数，利用Matlab 的mvnrnd函数生成采样点。

3.2.3 分模型个数K

合适的K值选取是求解高斯混合模型的关键步骤，它需要合理地反映出当前航迹被障碍物分成了几个部分，也就是说K应该和航迹的有效节点数量是等同的。有效节点被定义为航迹节点序列Π经过裁剪后的新节点序列Πprune。

算法7 为裁剪函数的流程，它从起始点开始逐渐向目标点移动，遍历所有子节点直到找到最少代价的无碰撞航迹，裁剪过程结束后，航迹中不会再有可以直接连接的额外节点，图3 为一段航迹的裁剪示意图，实线为裁剪后的航迹，虚线为失效的航迹。

图3 航迹裁剪示意图Fig.3 Schematic diagram of pruning path

3.2.4 最大节点数量

优化阶段的扩展策略考虑近邻节点的优化，当节点数量达到最大值时，便删去树中最低质量的叶子节点，质量高低由3.2.1节定义的函数J判断，扩展流程见算法8，最大节点数量由式（18）定义：

其中：Nmax表示最大节点数量（Maximum Number of Nodes）；εi为地图尺寸；d为地图维度；λ为生长步长为初始航迹的节点数量。

3.2.5 更新周期

与其他通过找到新的可行航迹来调整采样策略的算法不同，本文根据近邻优化次数来周期性地更新抽样分布，具有稳定的更新频率。在算法更新了m次后的更新周期P被定义为：

其中：m为更新次数；P0为初始更新周期。

图4 中展示了抽样分布的变化过程，由于节点会不断被筛选更新，抽样分布也逐渐被学习更新到最优路径的附近区域。

图4 更新m次的抽样分布变化Fig.4 Sampling distribution change after m-times updating

4 算法性能分析

4.1 概率完备性

在找寻初始航迹的搜索阶段：理想情况下，向光区域采样策略可以引导树快速达到目标点；极坏情况下，那些扩展失败节点的额外代价将达到最大值，背光区域作用失效，同RRT一样对空间均匀采样。因此本文算法与RRT 一样具有概率完备性。

4.2 计算复杂度

5 仿真实验与结果分析

在Map1～4 等不同类型环境下仿真来验证本文算法的有效性，将其与RRT*算法、改进RRT*-FN 算法进行比较，从而来表现本文算法的优越性。考虑到算法本身具有的随机性和不同环境下的规划难度不同，对相同环境下的不同算法各进行30 次实验，不同环境下的仿真时间、起始点和目标点的位置见表1。算法的运行环境：64 位Windows10 操作系统；处理器Intel Core i5-8250U CPU；主频1.60 GHz；内存8 GB；计算软件Matlab 2017b。

图5 是不同环境下运行结果的比较。如图5（a）所示，Map1 是一个障碍物密度较大的环境场景，可以看出RRT*算法一成不变地对整个空间均匀采样，产生了大量那些远离航迹的采样，而这些节点对最终航迹并没有起到降低代价的作用；改进RRT*-FN 算法在一定程度上改善了RRT*算法对空间的盲目探索，在找到初始航迹后，便可以使用启发式采样向树中添加有意义的节点，达到集中采样的效果来优化航迹；相比之下，本文所提出的学习高质量节点的策略、自适应调整抽样分布的方法具有更好地识别航迹特征的能力，优化效果更为出色。Map2 是一个带有U 形障碍的特殊环境场景，从起始点到目标点需要穿过两个狭窄通道，规划难度较大，如图5（b）所示，由于最终航迹长度和起始点到目标点的直线距离相差很大，椭圆子集采样效果退化严重，大多节点都集中在了障碍物内，包裹航迹的附近节点分布不均匀，导致改进RRT*-FN算法的优化性能大大减弱。Map3 是一个由20 个随机半径、随机位置威胁球组成的三维环境场景，如图5（c）所示，在三维环境下本文算法依然可以灵活地调整采样空间，相较于另外两种算法也具有更好的优化效果。Map4 是一个带有两个狭窄通道的三维环境场景，如图5（d）所示，可以看出随着环境空间维度的提升，规划难度加大，算法的性能对比也变得越来越明显。

图5 Map1～4上的算法性能对比Fig.5 Performance comparison of three algorithms on Map1-4

表2 列出了30 次仿真实验的各算法性能指标的平均值。从表2 可以发现，从找寻初始航迹的消耗时间上看，近邻优化步骤使得RRT*算法比RRT算法要慢很多，本文方法根据树的生长情况去自适应调整采样策略，因此比RRT 算法用了更少的时间。从最终生成的航迹代价来看，在相同的仿真时间下，本文方法依据当前近邻优化次数周期性地学习树中高质量节点，新的抽样分布更能反映当前最优航迹的几何特征，相较于其他两种算法具有更高的优化效率，最终生成了更低代价的航迹。

表1 仿真参数设置Tab.1 Simulation parameter setting

图6 是不同环境下算法的收敛结果对比。由图6 可以看出，本文方法在搜索初始解时目的性强，优化阶段集中采样效果好，因此相较于另外两种算法具有更好的收敛性，而且在规划空间维度变高后差距更为明显。

表2 不同算法性能指标平均值Tab.2 Average performance indices of different algorithms

图6 时间与路径长度的关系对比Fig.6 Comparison of relationship between time and path length

6 结语

本文提出的AS-RRT*算法主要是在RRT*算法基础上对采样空间的建模进行改进，分为搜索和优化两阶段考虑、在向光和背光区域的有偏采样、学习高质量节点等策略都是为了降低随机采样带来的负面影响。最后的实验结果中也表明了本文方法的有效性。

本文只考虑了在静态环境下以无人机质点模型为载体的离线路径规划，未来方向可考虑任务目标的运动学约束、在动态环境下在线的路径规划。