多约束条件下无人机航迹规划

李昱奇，刘志乾，程凝怡，王莹莹，朱春丽

1.中国石油大学（北京）安全与海洋工程学院，北京 102249

2.中国石油大学（北京）机械与储运工程学院，北京 102249

3.中海油研究总院有限责任公司，北京 100027

无人机航迹规划是全局优化问题，在复杂的动态场环境下，基于各种约束条件，寻求最优飞行路径[1]。无人机只有依靠导航系统提供的信号，准确地到达预定位置，才能保证任务顺利完成[2-3]。目前远程长航时无人机配备的导航系统是以惯性导航系统为主，以卫星导航、天文导航、多普勒导航等为辅助手段的组合导航系统[4]，但由于其精密程度高、结构复杂、系统存在限制等原因，在飞行过程中会产生定位误差，若单纯依靠惯性导航，定位误差会随时间累积，当误差累积到一定程度将无法实现精确定位，影响任务执行[5-7]。为此，无人机须前往相应误差校正点进行误差校正，由此引发的约束问题，称为导航精度类约束；同理，若遇威胁因素，需要躲避而引发的约束称为规避类约束。

大部分学者的研究点专注于如何提高导航精度[8-10]。如张楠[11]对高空长航时无人机组合导航系统的方案、模型和算法进行了研究，并分析了方案的可行性，但并未与航迹规划相结合。而针对航迹规划问题，主要研究集中在规避类约束方面[12-17]。如2016 年，尹高扬等人[18]采用改进的快速扩展随机树法解决三维空间下无人机航迹规划问题，该算法在搜索最优路径的同时，可以有效躲避威胁和障碍。同年，Calik[19]针对雷达、山地等威胁障碍类约束，以无人机最小飞行时间为目标函数，利用多智能体结构蚁群算法（Ant Colony Optimization with a Multiagent Structure）求解无人机全局最优路径规划问题，该算法可以快速有效规避障碍，并形成最优路径。但针对导航精度约束下航迹规划问题研究较少，目前有2015 年刘世一等[20]基于惯性导航系统误差模型和光学侦察载荷校正模型，分析了导航精度对长航时无人机航迹规划的影响，但校正点设立较少，约束条件较为简单。

综上所述，以复杂类导航精度为约束的无人机航迹规划问题研究尚浅。鉴于此，本文将基于Dijkstra 算法对导航精度约束下无人机航迹规划问题进行研究。

1 导航精度约束下无人机航迹规划模型建立

无人机在飞行过程中会产生两个方向的定位误差——垂直误差和水平误差。假设无人机每飞行1 m，垂直误差和水平误差将各增加δ个单位，到达目的地时垂直误差和水平误差均应小于θ个单位才能确保任务完成。

图1 无人机航迹规划区域示意图

无人机飞行区域如图1所示，A点为起点，B点为终点。在此飞行区域内分布有N个误差校正点（不包含A、B），蓝点为垂直误差校正点，只将垂直误差校正为0，黄点为水平误差校正点，只将水平误差校正为0。当无人机垂直误差不大于α1个单位且水平误差不大于α2个单位时，才能在垂直误差校正点进行垂直误差校正。同理，当无人机垂直误差不大于β1个单位且水平误差不大于β2个单位时，才能在水平误差校正点进行水平误差校正。在满足误差要求前提下，通过规划无人机航迹，使航迹长度最短且经过的校正点数量最少。

1.1 决策变量

为保证无人机航迹的实时位置信息，将所有校正点是否执行定位误差校正功能Pb,i,j作为决策变量，其中b表示误差校正点编号，为1,2,…,N-1,N；i表示实际航迹中行驶的第i个误差校正点；j表示误差校正点类型判断标志，j=1 为垂直校正点，j=0 为水平误差校正点。例如b=125,i=3,j=1 时，P125,3,1表示编号125的点为垂直误差校正点，且在所有航迹校正点中为第3个校正点。Pb,i,j=1 代表编号为b的校正点执行误差校正，Pb,i,j=0 代表不执行误差校正。

1.2 目标函数

为保证无人机顺利完成任务，飞行过程中应综合考虑两个优化目标：

（1）无人机从起点A到终点B的航迹长度尽可能小；

（2）经过校正区域进行校正的次数尽可能小。

因此目标函数可表示为：

式（1）中：n表示无人机从起点A出发经过的所有校正点数量；L表示无人机航迹长度，表示第i-1 个校正点和第i个校正点之间的距离。

1.3 约束条件

（1）误差增长约束

无人机在空间飞行时由系统限制所产生的垂直误差和水平误差只与飞行距离线性相关，无人机每飞行1 m，垂直误差和水平误差将各增加δ个单位。因为误差校正会影响无人机在之前飞行过程中所积累的误差，所以取无人机在两个相邻校正点之间所产生的垂直误差ei,v和水平误差ei,h作为最小误差计量单位，则定位误差与两个校正点之间距离di的关系如式（2）所示。

式（2）中：ei,v为第i-1 个误差校正点与第i个误差校正点之间的垂直误差；ei,h为第i-1 个误差校正点与第i个误差校正点之间的水平误差。

（2）终点误差约束

为保证飞行任务的顺利完成，到达终点时定位误差应小于θ个单位，因此，无人机所产生的垂直定位误差和水平定位误差应满足式（3）约束。

式（3）中：Ei,v表示无人机从起点出发经过第i个校正点时所累积的总垂直误差，EB,v表示终点B处的垂直误差；Ei,h表示无人机从起点出发经过第i个校正点时所累积的总水平误差，EB,h表示终点B处的水平误差。

（3）起点误差约束

在起点A处，无人机的垂直和水平误差均为0，则该约束可表示为：

（4）误差校正点功能约束

当无人机经过垂直校正点并且执行误差校正时，其垂直误差将变为0，水平误差保持不变，该约束可表示为：

同理，当无人机经过水平校正点并且执行误差校正时，其水平误差将变为0，垂直误差保持不变，该约束可表示为：

（5）误差校正点执行条件约束

无人机的垂直误差不大于α1个单位，水平误差不大于α2个单位时才能进行垂直误差校正；无人机的垂直误差不大于β1个单位，水平误差不大于β2个单位时才能进行水平误差校正。若使路径中第k个校正点能够执行垂直误差校正功能，即Pb,k,1=1，需要满足式（7）所示的约束条件。

若使路径中第k个校正点能够执行水平误差校正功能，即Pb,k,0=1，需要满足式（8）所示的约束条件。

2 基于Dijkstra算法的航迹规划法

2.1 传统Dijkstra算法

Dijkstra 算法[21]作为路径规划经典算法可从全局出发寻找最优解，较高的搜索精度为其优势所在，其搜索过程如下：

（1）初始化，令d(v1)=0,d(vj)=w1j(j=2,3,…,n),S={v1}，R=VS={v2,v3,…,vp}。

（2）在R中寻找一个顶点vk，使得：

置S=S∪{vk},R=VS。若(vk,vp)∈E，即点vk与vp间距wkp满足终点处的误差约束，则算法终止，否则转（3）。

（3）修正d(vj)，对R中每个vj，令d(vj)=min{d(vj),d(vk)+wkj},转（2）。

但传统算法将所有可能路径都加入其中，计算量较大，效率低。为此通过校正策略优选、校正方案优选和O-D邻接矩阵处理方式，简化搜索路径，降低计算量，提高搜索效率。

2.2 校正策略优选

无人机在某一时刻到达特定编号的校正点Pk时，根据校正点功能只能进行垂直误差校正或水平误差校正。因此在整个航迹规划过程中会产生两种误差校正策略——完全交替校正和不完全交替校正。如图2 所示，完全交替校正表示任意相邻两次误差校正均不同，即垂直误差校正与水平误差校正交替进行（以图中实线表示，Pk与Pk+1为不同类型校正点）；不完全交替校正表示存在相邻两次误差校正之间为同类型的误差校正情况（以图中虚线表示，Pk与为相同类型校正点）。

图2 误差校正策略示意图

针对上述两种误差校正策略，分别计算并比较其校正效率，进而确定模型求解过程中两种策略的优先选择级，两种校正策略的航迹长度具体计算步骤如下。

（1）完全交替校正策略

设定第k-1、k、k+1 点为相邻3 个误差校正点，分别记作Pk-1、Pk和Pk+1，且3 点的误差校正类型依次为垂直误差校正、水平误差校正和垂直误差校正。

①设定无人机在第k-1 个校正点Pk-1进行垂直误差校正之后的误差为：

②无人机在第k个校正点Pk进行误差校正之前的垂直误差和水平误差分别为：

③无人机在第k个校正点Pk进行误差校正之后的垂直误差和水平误差分别为：

④无人机在第k+1 个校正点Pk+1进行误差校正之前的垂直误差和水平误差分别为：

⑤无人机在第k+1 个校正点Pk+1进行误差校正之后的垂直误差和水平误差分别为：

联立式（9）～式（13）可求解无人机由校正点Pk-1到校正点Pk+1之间的最大航迹距离为max(dk+dk+1)。

（2）不完全交替校正策略

如图2所示，设定第k-1、k、k+1 点为相邻3个误差校正点，分别记作Pk-1、Pk和P′k+1，且3点的误差校正类型依次为垂直误差校正、水平误差校正和水平误差校正。无人机在第k-1 个校正点Pk-1进行垂直误差校正之后的误差与完全交替校正策略的误差相同。

根据假设条件推理可知，无人机在第k+1 个校正点P′k+1进行误差校正之前的误差变化和航迹距离约束方程可由式（9）～式（11）表示。

由于垂直误差校正点与水平误差校正点执行校正功能的误差约束范围ε（包括误差约束α1、α2、β1、β2）不同，即无人机航迹距离变量d′k+1的约束条件发生变化，故无人机在第k+1 个校正点P′k+1进行误差校正之前的垂直误差和水平误差分别为：

无人机在第k+1 个校正点P′k+1进行误差校正之后的垂直误差和水平误差分别为：

联立可求解无人机由校正点Pk-1到校正点P′k+1之间的最大航迹距离为max(d′k+d′k+1)。

（3）最优校正策略

对比两种误差校正策略可知，无人机飞行最大航迹距离越大，则越快达到终点，且经过校正点较少，即若max(dk+dk+1)＜max(d′k+d′k+1)则采取完全交替校正，反之采取不完全交替校正。若max(dk+dk+1)=max(d′k+d′k+1)，可以看出，当无人机飞行距离相同时，完全交替校正策略所用的校正次数最少，此时选用完全交替校正策略。

2.3 校正方案优选

基于完全交替校正策略，存在两种误差校正顺序：（1）无人机从起点A出发后先进行垂直误差校正，继而与水平误差校正交替进行（VHV校正）；（2）无人机从起点A出发后先进行水平校正，继而与垂直误差校正交替进行（HVH校正）。

根据误差约束范围ε中α1、α2、β1、β2及δ这 5 个误差校正参数的限制，可得到每步误差校正时的距离约束，并将其与每一个误差校正点处的误差进行比较，进而将误差约束转化成对距离的约束。

当无人机采用VHV 校正时，无人机从A点出发，起始为A-V 校正，过程为VHV 校正循环，最终到达B点，总体循环模式可借助向量表达，如式（16）所示：

VHV 校正循环中误差变化满足上述约束，联立式（2）、式（4）～式（8），得到执行VHV循环中各阶段距离约束dA-V、dV-H、dH-V和累计距离约束dVHV，其中dVHV=dV-H+dH-V。同理，在无人机采用HVH校正时，联立上式得到执行HVH循环中各阶段距离约束dA-H、d′H-V、d′V-H和累计距离约束dHVH，其中dHVH=d′H-V+d′V-H。

通过比较dVHV与dHVH大小，当dVHV＞dHVH时，采用VHV校正策略，当dVHV＜dHVH时，采用HVH校正策略，当dVHV=dHVH时，根据dA-V与dA-H数值大小，确定校正策略。

2.4 O-D距离邻接矩阵及其化简

根据校正点(x,y,z)坐标和两种误差校正点类型，建立所有校正点间初始的O-D距离邻接矩阵[22]，命名为O0。在此基础上，根据校正策略和校正方案对矩阵O0进行化简，实现流程如下：

（1）将所有校正点信息转化为初始O-D邻接距离矩阵O0，容易得到O0={dmn}，且m,n∈[0,N-1]，m,n∈[0,N+1]，则dmn表示无人机由第m个校正点飞到第n个的校正点所走过的直线距离。

（2）基于校正策略优选，若选择完全交替校正策略，则对矩阵O0中任一元素dmn，若jm=jn，则令dmn的值为无穷大，以dmn=inf 表示。若选择不完全交替校正策略，则此步骤可忽略，执行（3）。

（3）基于校正方案优选，若选择VHV 校正，则需要将矩阵O0中第一行数据中大于dA-V的值改为inf，对矩阵O0其余各行数据中任一元素dmn，若jm=1，且dmn ＞dV-H,则dmn=inf；若jm=0,且dmn ＞dH-V,则dmn=inf ；若选择HVH校正，则需要将矩阵O0中第一行数据中大于dA-H的值改为inf，对矩阵O0其余各行数据中任一元素dmn，若jm=1，且dmn ＞d′V-H，则dmn=inf；若jm=0 ，且dmn ＞d′H-V，则dmn=inf。

通过上述步骤简化搜索路径，并将约束条件融入传统算法，最终得到基于Dijkstra算法的航迹规划法。

3 仿真验证

基于上述模型和算法，设置仿真参数，α1=25 ，α2=15，β1=20，β2=25，θ=30，δ=0.001，出发点、终点及校正点坐标可根据Matlab 中随机函数创建得到。同时，为得到可行解，校正点数量和三维空间大小应根据仿真参数α1、α2、β1、β2、θ和δ的取值进行合理分配。此处列出某次求解过程中对应的参数值：起点坐标A（0，50 000，5 000），终点坐标B（10 000，59 652.34，5 022），单位m，校正点数量共611个，部分参数如表1所示。

表1 空中点迹坐标位置关系图

结合仿真参数，利用Matlab 编程实现上述算法，最终确定校正策略为完全交替校正，校正方案为VHV 校正，无人机航迹点为：

飞行轨迹三维视图如图3所示，图中黄色点为水平误差校正点，蓝色点为垂直误差校正点，无人机从起点出发经过的误差校正点编号及校正前后误差如表2所示。

图3 航迹规划三维路线图

表2 误差校正点编号及校正前后误差

无人机经过校正点数目为10，所走路线长度为105 581.895 m。如表2所示，每一数据点处的误差都满足约束要求，这表明，基于Dijkstra算法的航迹规划法在解决导航精度类约束下无人机航迹规划问题方面具备正确性和有效性。

同时，为验证算法优越性[23]，将上述结果与传统Dijkstra算法和遗传算法所得结果分别进行对比。仿真实验涉及校正点数量611 个，数据量较大，遗传算法容易陷入局部最优[24]，为此，针对上述参数共进行10 次仿真，取其中最优结果。算法性能比较如表3 所示，可以看出各数据点误差均满足要求，三种算法皆具备正确性和有效性。传统Dijkstra 算法的校正点数量、总路线长度和算法耗时均优于遗传算法结果，表明Dijkstra 算法更适用于解决导航精度约束问题，在计算精度与传统算法仅相差691.343 m，即0.66%情况下，改进的算法能够利用更少的时间得到最优解，在解决导航精度约束下无人机航迹规划问题方面具备一定的先进性。

表3 算法性能比较

4 结束语

以多约束下无人机航迹规划问题为切入点，重点分析了导航精度约束下无人机航迹规划问题。将误差校正这一概念按照导航精度约束处理，建立导航精度约束下无人机航迹规划模型，并设计基于Dijkstra 算法的航迹规划法求解该模型。最后通过仿真实验验证该算法的正确性和有效性，并与传统算法和遗传算法进行对比来验证算法先进性，结果表明无人机在实际航行中可以完成导航精度约束问题，能够快速实现最短航迹规划。同时，校正点为随机选取，路径规划可随校正点位置信息改变而发生变化，算法适应性较强。但研究中忽略了无人机转弯半径问题，存在一定误差，鉴于此，可尝试通过曲线方程修正模型，并根据转弯半径完善算法，从而使研究内容更贴合实际。