何 兵，朱晓宇，林浩申，李青勇，陈 琛

(火箭军工程大学 核工程学院，西安 710025)

无人飞行器(Unmanned Aircraft Vehicle,UAV)具有使用灵活、飞行自主等突出特点，在军事和民用领域应用广泛[1]。航迹规划是实现UAV自主飞行的重要环节，是指根据环境的已知信息和任务需求，考虑UAV飞行过程的相关约束，生成从起点到目标点的飞行航路。由于在对抗、遮挡等异常情况下卫星导航系统存在风险，通常UAV航路上设置一系列航路校正点降低惯性导航系统误差，以实现高精度导航避免碰撞风险。因此，航迹规划，不仅要考虑航路上的威胁、障碍、人文禁避飞区等规避约束，也要考虑UAV航迹校正点规划[2]。本文主要研究UAV的航迹校正点选择和优化问题。

当前航迹校正点选择和优化常用搜索算法有A*算法[3-5]，快速搜索随机树(Rapidly-exploring Random Tree,RRT)[8-9]算法，群智能优化算法等。A*算法通过选取估价函数对每个航迹搜索点进行评价[5]，启发式搜索在规避无效搜索和提高搜索效率上发挥了重要作用；群智能算法对目标函数和约束的表达比较宽松，可通过并行计算提高规划效率[6]，但容易陷入局部最优。作为典型的随机搜索算法，RRT算法在收敛速度上和总体的规划效率具有明显优势[10-14]。本文针对导航精度受限的UAV航迹校正点选择和优化问题，综合航迹长度和校正点约束，对RRT算法进行改进，实现航迹校正点的选择优化。与传统A*算法、RRT算法的对比仿真试验结果表明改进RRT算法的航迹校正点选择优化上具有很好的快速性和有效性。

1 问题描述

1.1 问题背景

本文的应用场景可以描述为：UAV在给定任务空间中执行任务，由于飞行过程中导航误差的存在，使UAV需要在航迹校正点集上进行误差校正，在规定的任务约束下规划出一条从起点A到目标点B的完整航迹。这类航迹规划问题可以转化为航迹校正点选择优化(Optimization of Path Correction points,OPC)研究，通过优化航迹校正点的搜索选取，综合考虑航迹长度代价和经过校正点个数，生成全局最优航迹。

不难发现，上述问题的寻优是在有向图g=(U,L,Δ)搜索出一条满足约束巡航路径。有向图中U表示任务空间内的校正点集，具体来说，空间内的垂直误差校正点集U⊥和水平误差校正点集U‖和任务的起点A、终点B共同构成有向图g的各个节点。L表示UAV在有效校正点间的飞行航迹的集合，校正点间的有效航迹上的误差增量不超过UAV正常工作导航精度的上限。Δ表示有效航迹上误差增量的集合，L和Δ之间存在对应关系。

U作为航迹校正点选择优化问题的输入，可以表示为

U={Ni|Ni,i=0,1,2,…,n+1}

(1)

其中，N0和Nn+1分别表示起点A和终点B。

(2)

(3)

1.2 约束条件和优化目标

航迹校正点的选择优化过程需要考虑到相邻校正点在当前导航精度约束下的可达性，即在当前应用场景中，UAV按照规划路径飞行指定目标点的前提是飞行误差的累积应始终维持在一个合理范围。本文设飞行过程中的垂直误差和水平误差始终小于θ个单位，才能按照规划路径飞行直至终点，则对于校正节点Nj校正前的导航误差为

此外还应考虑进行飞行误差校正需要达到相应的误差阈值条件，因此，对于航迹校正节点Nj在校正前的导航误差需满足：

(5)

综合式(4)和式(5)可得校正误差约束为

(6)

航迹校正点的选择优化过程选择的目标函数为校正次数代价和航迹长度代价，前者指的是确保飞行器经过的误差校正点总数l最小，后者指的是航迹(N0,Ni1),(Ni1,Ni2),…,(Nil,Nn+1)的总长度最小,则：

(7)

其中，dik,ik+1表示航迹节点ik到ik+1的距离。

同时考虑航迹不能同时满足两个优化目标，在指标函数(见式(8))中引入权重系数来衡量优化目标的重要程度：

(8)

其中，J1和J2为归一化系数，权重系数ω可根据飞行任务性质的不同人工调整。指标函数J越小说明航迹越优。

2 基于有效邻域的改进RRT算法

对于航迹校正点规模较大的OPC问题，全局搜索是最优解决方案，但近似O(n!)时间复杂度使其在实际应用中不可能采用，因此需要考虑采用随机搜索或启发式搜索等策略进行选择优化。同时传统航迹规划的解空间通常是无限且连续的，在搜索更新过程中，对搜索步长和搜索方向的限制较小，可以在任意步长和方向上进行搜索，而航迹校正点选择优化问题的解空间是有限且离散的，其搜索步长会受到当前节点累积误差信息影响，搜索方向受到先验节点位置信息的限制，因此，针对经典RRT算法进行改进，通过构建有效邻域在解空间中进行搜索，有效降低了获得可行解的复杂度。

2.1 经典RRT算法

设当前的解空间区域为R，起点和终点分别为A和B，二维空间内经典RRT算法[15]扩展过程如图1所示，其中，qnow为当前随机树的叶节点的集合，qrand表示在解空间子集R∩qnow内按一定的规则随机采样得到的叶节点，qnear表示在当前扩展树中与qrand最近的叶节点，ε为当前树向qrand方向的生长距离，qnew表示满足相关约束后随机树的新增节点。

图1 经典RRT算法扩展过程示意图

经典RRT[11-13]伪代码的描述，如下：

1Initialization:xA,xB,N,eps2for i=1:N3 qrand←random()4 qnear←min(qknow,qrand),k=1:K5 qtemp←ε·qrandqnear→qrandqnear→6 ifqtempNOTsatisfy{condition1,…} continue7 qnew←qtemp8 qnow←{qnow,qnew}9 ifdistance(qnew,xB)

其中，N,K,eps为边界条件，N表示RRT构造的最大尝试次数,K表示当前RRT节点的规模,eps表示与终点接近程度。

2.2 基于有效邻域的改进RRT算法

2.2.1 有效邻域的构建

(9)

(10)

2.2.2 改进的RRT校正点选择算法

建立节点的邻域可达集后，迭代过程中最近邻节点从当前节点的邻域可达集中产生，新节点可以采用一定策略从近邻节点的Unear中选择，搜索域规模大大减小。进一步，叶节点状态矢量S用于存储节点状态信息，对相邻节点状态矢量建立链接关系，可以在反向溯源过程中快速给出可行航迹，具体步骤如下：

1) 叶节点状态表构建及其初始化。定义每个叶节点的状态矢量S为

S=(Norig,δ⊥,δ‖,Lpre,Lnow,dtotal,Ntotal)T

(11)

其中，Norig对应当前叶节点在U中的原始编号；δ⊥、δ‖为当前叶节点校正后的垂直误差和水平误差；Lpre为当前叶节点的前向叶节点的编号，Lnow为当前叶节点的编号，叶节点的编号由叶节点生长过程的先后顺序唯一确定；dtotal、Ntotal分别为截至当前节点的航程代价和累计校正次数。

同时可以通过改善叶子状态列表数据结构(图2)，提高计算更新速度。

图2 叶节点状态示意图

2) 选取目标点Lrand。设当前叶节点集合L={L0,L1,L2,…}，从当前随机树外的校正点集U-L中按如下策略选取一目标点Lrand：命中终点B的概率为p，命中其他叶节点的概率为1-p。其中，p为超参数，用于保证搜索的导向性，避免算法陷入局部最优解。

3) 搜索最近邻节点Lnear。在当前叶节点集合L中，搜索出同时满足Near法则和式(6)约束的节点Lnow作为最近邻节点Lnear。Near法则存在以下两种定义：

① 距离最近法则(LawA)：确保节点Lnear与节点Lrand的欧氏距离最小，如图3中的Lnear。

(12)

(13)

图3 最近邻节点选择法则示意图

基于有效邻域的快速扩展随机树(Rapidly Rapidly-exploring Random Tree based on effective Neighborhood,NRRT)算法流程如图4所示。

图4 NRRT算法流程框图

3 仿真算例与分析

3.1 仿真环境与参数配置

本文的仿真采用MATLAB R2018a (64-bit) 进行编程计算，硬件环境为：i7-8700 CPU，8G RAM，Win 10操作系统。随机树的最近邻产生采用距离最近法则，单位航程垂直和水平误差累积按照线性处理，其他参数设置如表1所示。

表1 仿真过程参数设置

其中，m为初始叶节点个数，p为随机选择B作为目标点的概率，[α1,α2],[β1,β2]为进行垂直误差和水平误差校正需要满足的误差阈值。

3.2 仿真结果

采用本文的基于有效邻域的RRT校正点选择优化算法(NRRT)对所给的数据集1[2]进行处理，综合考虑航迹长度约束和校正次数约束，分别规划了航程最优航迹和校正最优航迹，经过104次蒙特卡洛仿真规划结果如图5、图6所示，图中离散点是任务区域内的校正点集，最优航迹经过的校正点采用三角形标注。其航程最优航迹(图5)共经过9个校正点，航迹总长度为 104 279. 425 9 m，而校正最优航迹(图6)共经过 8个校正点，航迹总长度为 104 898.374 9 m。

图5 NRRT航程最优航迹

图6 NRRT校正最优航迹

算法复杂度方面，104次蒙特卡洛仿真总耗时540.509 3 s，最大计算耗时仅0.999 8 s，最小计算耗时0.011 9 s，94.24%的搜索耗时集中在[0,0.2 s]的区间内。

针对OPC问题，目前常用的方法是A*及其改进算法[15-16]，A*算法作为最有效的直接搜索算法，本文采用其与NRRT作横向比较，选取A*算法的代价函数为f(n)=g(n)+h(n)，航程最优航迹(图7)共经过8个校正点，航迹总长度为 105 057.363 6 m。算法复杂度方面，经过104次蒙特卡洛仿真，平均耗时稳定在1.021 4 s左右。

图7 A*航程最优航迹

NRRT算法的随机采样特性，极易通过单次搜索获得可行的次优解，但单次解的质量上要弱于 A*算法。而NRRT的搜索快速性，可以使其通过少量的蒙特卡洛仿真逼近最优解，达到提升解的质量的效果。NRRT算法和A*算法求解过程经过的校正点集、获得的最优航迹以及算法的平均耗时如表2所示。总体来说，面对复杂规划问题，NRRT要比A*算法的规划效率高，在处理OPC问题具有更大的优势。

表2 仿真结果

4 结论

针对航迹校正点的选择优化问题，设计了一种NRRT算法，一方面通过构建有效邻域大幅减小了传统RRT算法的搜索空间，间接提高其搜索的搜索效率，降低了算法的计算复杂度。另一方面设计了有效的叶节点列表存储结构，以查表代替运算，有效降低搜索和回溯的复杂度。两方面保证了算法的有效性，使其在实际操作过程中算法收敛速度很快，规划效率高，极易获得可行解，最后的仿真结果证明，在航迹校正点的选择优化中，NRRT在规划效率方面具有明显优势。