基于最小修正次数的飞行器修正路径规划

Aircraft Correction Path Planning Based on Minimum Correction Quantity

摘要: 本文研究了飞行器飞行过程中，所需经过校正点数量最少的路线规划问题。通过引入等效距离矩阵，使得终点约束条件线性化，以途经校正点数量最少为目标，建立了线性整数规划模型，通过标志矩阵设计隐枚举的过滤条件，从而提高运算效率，最终对求得的全局最优解进行成功飞行条件验证，验证通过。本模型准确，简单，算法效率高，问题中共计给出校正点588个，求出全局最优解用时4.3秒。

Abstract: This paper studies the route planning problem with the least number of calibration points. By introducing the equivalent distance matrix, the end constraint condition is linearized. A linear integer programming model is established to minimize the number of supply points. The filter conditions of implicit enumeration are designed by identification matrix, so that the operation efficiency is improved. Finally, the survival conditions of the global optimal solution are verified, and the verification is passed. This model is accurate, simple and efficient. A total of 588 supply points are given, and it takes 4.3 seconds to find the global optimal solution.

1. 背景和问题介绍

由于系统结构限制，飞行器的定位系统无法对自身进行精准定位，一旦定位误差积累到一定程度可能导致任务失败。因此，在飞行过程中对定位误差进行校正是智能飞行器航迹规划中一项重要任务。本文研究在某种误差修正方式下最优修正路线规划问题，该问题条件和背景可简化如下：假设飞行器的飞行区域如图1所示，出发点为A点，目的地为B点。飞行器在飞行过程中需要对定位误差进行校正。飞行区域中存在一些安全位置(称之为校正点)可用于误差校正，黄色的点为水平误差校正点，蓝色的点为垂直误差校正点。若垂直误差、水平误差都能得到及时校正，则飞行器可以按照预定航线飞行，通过若干个校正点进行误差校正后最终到达目的地。

假设飞行器在垂直误差校正点进行垂直误差校正后，水平误差保持不变。飞行器在水平误差校正点进行水平误差校正后，垂直误差保持不变。假设飞行器在飞行过程中对水平误差和垂直误差的影响是相似的，水平飞行，每飞行100 m产生水平误差和垂直误差均为−5；爬升飞行，每飞行100 m水平或垂直误差均为−6；下降飞行，每飞行100 m水平或垂直误差均为−4。由于修正点之间的间隔很近，可以近似的认为飞行器在两个校正点间飞行的距离为该两点间的直线距离。

若抵达目的地B前，飞行器的垂直误差或水平误差绝对值均超过5时，飞行器飞行失败，根据该飞行器飞行误差的特点，飞行器在飞行过程中，记录的数据比实际数据小，即误差总是负值，即飞行器的垂直误差或者水平误差均低于−5时飞行器飞行失败，且飞行器抵达目的地B点时，水平误差和垂直误差均不得低于−3。由于误差绝对值超过5，飞行器飞行失败，故在出发点A点，可以给定飞行器的垂直和水平误差初始值均为5。各个校正点的修正值均为给定值。误差修正时可由原本低于真实值修正为高于真实值，只需确保水平或垂直误差绝对值不会同时超过5都满足成功飞行条件。本论文研究在此问题背景下，如何找到飞行器从起点到终点的飞行路径，使得经过校正点的数量最少。

Figure 1. Schematic diagram of correction points

图1. 校正点示意图

飞行器在飞行过程中水平和垂直数值发生变化，误差与飞行状态有关，该问题可抽象为网络模型下有约束的路径规划问题，且该路径规划以途经节点个数最少为目标，而非总路径最短。对于有约束路径规划问题，常用的动态路径算法。最常见的动态路径搜索算法有A*算法 [1] [2] 及其延伸算法。A*算法是一种基于广度优先搜索的启发式搜索算法，问题规模较小时，算法效率高，搜索速度快，但该算法受问题规模制约较为明显，不适合用于较大规模或约束条件较多的问题。较为普遍的一种A*算法延伸算法是LAP*算法 [3]，LAP*算法的第一次搜索与A*相同，但后续的搜索利用了前面搜索相同的部分，文献 [3] 通过实验证明了该方法用于简单的路径计划任务的优势，尤其在移动机器人领域。另外一种A*算法的延伸算法是IDA*算法 [4]，该算法以牺牲搜索速度为代价，通过构建节点扩展方式防止大规模问题的路径搜索过程中的回溯搜索，对较大规模问题的求解优化A*算法，但求解速度比A*算法慢。这些算法虽然能够求解有约束路径搜索问题，但都有自身局限性，当问题规模较大时，仅能求得局部最优解，而无法实现全局最优解。本文将误差修正的约束条件线性化，对路径搜索中出现的回溯、分叉等情况进行约束，最终构造以途经校正点个数为目标的0~1线性整数规划问题，可求全局最优解。

2. 校正点删选

由问题背景可知，飞行器水平误差和垂直误差修正是相互独立的，互相之间不存在影响关系，行进过程对两者误差产生是相同的，但水平校正点只能修正水平误差，垂直校正点只能修正垂直误差，两者可以单独计算。为确保经过的路径长度最短，飞行器路线选择会尽可能沿着从起点到终点的直线方向走，虽然同样长度的路段，爬升、水平、下降飞行产生的误差不同，但是飞的越多误差越大，尽量少绕路。

由问题背景可知，飞行器水平误差和垂直误差修正是相互独立的，互相之间不存在影响关系，行进过程对两者误差产生是相同的，但水平校正点只能修正水平误差，垂直校正点只能修正垂直误差，两者可以单独计算。为确保经过的校正点数量最少，飞行器路线选择会采取如下策略：

1) 飞行器要在邻近失败的极限状态进行校正，即飞尽可能远的路再进行校正。

2) 尽可能沿着从起点到终点的直线方向飞，虽然同样长度的路段，爬升、水平、下降飞行产生的误差不同，但是飞的越多误差越大，尽量少绕路。

将校正点数据可视化，如图2所示。在xoy平面绘制校正点位置，其中红色圆圈表示水平校正点，蓝色五角星表示垂直校正点，红色三角形表示起点，黑色三角形表示终点。校正点的投影分布如图1所示。从图像可以看出，在连接起点到终点的直线附近，均有数量充足的水平和垂直校正点。根据上述修正策略，校正点应当在该直线附近选择。

设第i个校正点坐标为 $\left(x_i,y_i,z_i\right)$，在xoy面上的投影坐标为 $\left(x_i,y_i\right)$。起点投影坐标 $\left(x_0,y_0\right)$，终点投影坐标 $\left(x_t,y_t\right)$，令

$k=\frac{y_t-y_0}{x_t-x_0},$

连接起点和终点的投影直线方程为

$y=kx-k{x}_0+y_0,$

为刻画第i个补给点与投影直线的位置关系，计算

$L_i=\left|y_i+kx+k{x}_0-y_0\right|,$

给定范围阈值l，当满足 $L_i\le l$ 时，第i个校正点为备选校正点。最终从588个校正点中确定水平或垂直校正点的个数为n。

Figure 2. Horizontal or vertical correction point on xoy plane

图2. 水平或垂直校正点在xoy面的投影分布

3. 途经最少校正点模型的建立

以删选出来的n个校正点为节点，构建网络模型，要解决的问题即在该网络中寻找从起点到终点途经节点个数最少，能够满足飞行条件的路经。

引入校正点类别标志向量 $F=\left(f_1,f_2,\cdots ,f_n\right)$ 和误差修正量向量 $T=\left(t_1,t_2,\cdots ,t_n\right)$，其中 $t_i$ 表示第i个校正点的误差修正量，

$f_i=\{\begin{array}{l}0,第i个校正点为水平校正点,\\ 1,第i个校正点为垂直校正点.\end{array}$

计算任意两个校正点之间的距离矩阵 $D={\left(d_{ij}\right)}_{n\times n}$，其中

$d_{ij}=\{\begin{array}{l}\sqrt{{\left(x_i-x_j\right)}^2+{\left(y_i-y_j\right)}^2+{\left(z_i-z_j\right)}^2},i\ne j,\\ 0,i=j.\end{array}$

由于爬升、下降和水平飞行产生误差分别为 ${\gamma }_1,{\gamma }_2,{\gamma }_3$，可以将实际飞行折算为水平飞行距离，得到任意两个校正点间的等效水平飞行距离矩阵 $S={\left(s_{ij}\right)}_{n\times n}$，其中

$s_{ij}=\{\begin{array}{l}\frac{{\gamma }_1{d}_{ij}}{{\gamma }_2},z_j>z_i,\\ d_{ij},z_j>z_i,\\ \frac{{\gamma }_3{d}_{ij}}{{\gamma }_2},z_j<z_i.\end{array}$

基于水平飞行，得到误差产生矩阵 $W={\left(w_{ij}\right)}_{n\times n}$，有

$W={\gamma }_{2}H.$

构造任意两个节点间的误差修正矩阵 $E={\left(e_{ij}\right)}_{n\times n}$，其中

$e_{ij}=\{\begin{array}{l}{t}_i,i\ne j,\\ 0,i=j.\end{array}$

元素 $e_{ij}$ 表示飞行器在第i个校正点获得校正后，继续行进到第j个校正点，这个过程中如果产生误差大于修正值，飞行条件容易不满足，当飞行器的水平误差和垂直误差同时低于 $\theta$ 时，飞行任务失败。构造标志矩阵 $C={\left(c_{ij}\right)}_{n\times n}$，其中

$c_{ij}=\{\begin{array}{l}1,e_{ij}+w_{ij}>\alpha ,i\ne j,\\ 0,其他.\end{array}$

若飞行器到达校正点时，水平误差和垂直误差同时达到 $\theta$ 及以下，则飞行任务失败，标志为0的边，校正量与误差量的差距超过 $\alpha \ge \theta$，为了确保飞行任务成功，求解路径时，这样的边将不会选择。阈值 $\alpha$ 取值越大，任务完成的可能性越大，取值小，标志矩阵1元素数量越多，求解复杂度越高。这实际上是隐枚举的过滤条件。

引入0~1变量

$u_{ij}=\{\begin{array}{l}1,选择从第i个校正点走到第j个校正点,\\ 0,否则.\end{array}$

其中 $i,j=1,\cdots ,n$。

要求从起点到终点所经过的校正点数目最少的路，即经过的边的个数最少，即

$\min {\displaystyle \underset{i=1}{\overset{n}{\sum }}{\displaystyle \underset{j=1}{\overset{n}{\sum }}{u}_{ij}}.}$

约束条件构造如下：

1) 为了确保飞行任务成功的条件，只能在标志矩阵为1的元素对应的边中选择，于是有约束条件

$u_{ij}\le c_{ij},i,j=1,\cdots ,n.$

2) 终点成功条件

飞行器在行进过程中，对水平误差和垂直误差的产生是相同的，到达终点时，飞行器的水平误差和垂直误差均不得低于 $\mu$，在起点飞行器的水平误差和垂直误差均为 $\sigma$，故在从起点到终点的过程中，水平误差和垂直误差的修正总量应满足约束

水平误差： $\sigma +{\displaystyle \underset{i=1}{\overset{n}{\sum }}{\displaystyle \underset{j=1}{\overset{n}{\sum }}\left(1-f_i\right)e_{ij}{u}_{ij}}}-{\displaystyle \underset{i=1}{\overset{n}{\sum }}{\displaystyle \underset{j=1}{\overset{n}{\sum }}{w}_{ij}{u}_{ij}}}\ge \mu$，

垂直误差： $\sigma +{\displaystyle \underset{i=1}{\overset{n}{\sum }}{\displaystyle \underset{j=1}{\overset{n}{\sum }}{f}_i{e}_{ij}{u}_{ij}}}-{\displaystyle \underset{i=1}{\overset{n}{\sum }}{\displaystyle \underset{j=1}{\overset{n}{\sum }}{w}_{ij}{u}_{ij}}}\ge \mu$。

3) 路径约束条件

对于路的起点和终点 [5]，约束如下：

$\{\begin{array}{l}{\displaystyle \underset{j=1}{\overset{n}{\sum }}{u}_{1j}=1,}\\ {\displaystyle \underset{j=1}{\overset{n}{\sum }}{u}_{j1}}=0,\\ {\displaystyle \underset{j=1}{\overset{n}{\sum }}{u}_{jn}=1}.\end{array}$

由于要求途经节点数量最少的路，不会出现绕圈的情况，即校正不会到达2次，对于路径的中间校正点，约束如下：

$\{\begin{array}{ll}{\displaystyle \underset{j=1}{\overset{n}{\sum }}{u}_{ij}}={\displaystyle \underset{j=1}{\overset{n}{\sum }}{u}_{ji}},\hfill & i=2,\cdots ,n-1,i\ne j,\hfill \\ {\displaystyle \underset{j=1}{\overset{n}{\sum }}{u}_{ij}}\le 1,\hfill & i=2,\cdots ,n-1,\hfill \\ {\displaystyle \underset{j=1}{\overset{n}{\sum }}{u}_{ji}}\le 1,\hfill & i=2,\cdots ,n-1.\hfill \end{array}$

由于增加了飞行成功条件，满足上述路径约束未必能形成一条准确的路径，可能会出现如图3所示的情况，增加约束条件

$u_{ij}+u_{ji}\le 1,i,j=1,\cdots ,n,i\ne j.$

Figure 3. Error path sketch map

图3. 错误路径示意图

于是得到如下的线性整数规划模型：

$\begin{array}{l}\min {\displaystyle \underset{i=1}{\overset{n}{\sum }}{\displaystyle \underset{j=1}{\overset{n}{\sum }}{u}_{ij}},}\\ \text{s}\text{.t}.\{\begin{array}{l}{u}_{ij}\le c_{ij},i,j=1,\cdots ,n,\\ \sigma +{\displaystyle \underset{i=1}{\overset{n}{\sum }}{\displaystyle \underset{j=1}{\overset{n}{\sum }}\left(1-f_i\right)e_{ij}{u}_{ij}}}-{\displaystyle \underset{i=1}{\overset{n}{\sum }}{\displaystyle \underset{j=1}{\overset{n}{\sum }}{w}_{ij}{u}_{ij}}}\ge \mu ,\\ \sigma +{\displaystyle \underset{i=1}{\overset{n}{\sum }}{\displaystyle \underset{j=1}{\overset{n}{\sum }}{f}_i{e}_{ij}{u}_{ij}}}-{\displaystyle \underset{i=1}{\overset{n}{\sum }}{\displaystyle \underset{j=1}{\overset{n}{\sum }}{w}_{ij}{u}_{ij}}}\ge \mu ,\\ {\displaystyle \underset{j=1}{\overset{n}{\sum }}{u}_{1j}=1,}{\displaystyle \underset{j=1}{\overset{n}{\sum }}{u}_{j1}}=0,{\displaystyle \underset{j=1}{\overset{n}{\sum }}{u}_{jn}=1},\\ {\displaystyle \underset{j=1}{\overset{n}{\sum }}{u}_{ij}}={\displaystyle \underset{j=1}{\overset{n}{\sum }}{u}_{ji}},i=2,\cdots ,n-1,i\ne j,\\ {\displaystyle \underset{j=1}{\overset{n}{\sum }}{u}_{ij}}\le 1,i=2,\cdots ,n-1,\\ {\displaystyle \underset{j=1}{\overset{n}{\sum }}{u}_{ji}}\le 1,i=2,\cdots ,n-1,\\ u_{ij}+u_{ji}\le 1,i,j=1,\cdots ,n,i\ne j,\\ u_{ij}=0或1.\end{array}\end{array}$

4. 模型求解、验证及结论

取 $\theta =-5$， $\sigma =5$， $\mu =-3$，当阈值 $\alpha =-5$ 时，本模型可求得全局最优解，采用 $N=600$ 个校正点进行计算用时56.2秒，经过校正点数量最少的路径，除了起点和终点，中间共经过28个校正点，其中15个水平校正点，13个垂直校正点，飞行路径总长度为1174.5米。其飞行路线图见图4所示，飞行路线图在xoy面投影见图5。可以计算飞行器到达每个校正点还未进行校正时的水平误差和垂直误差，见表1，可见飞行器沿着该路径行走，抵达任何一个校正点时，水平误差和垂直误差均未同时小于 $\theta$，在抵达终点时水平误差和垂直误差均同时大于 $\mu$，满足飞行条件。

当 $\alpha \le -5$ 时，最优解不发生变化，但运算时间会增长。参数 $\alpha =-4$ 或−3时，路径发生变化，路径总长度1176.6米，但途经水平和垂直校正点个数不变，满足飞行条件，终点条件有所提升。参数 $\alpha \ge -2$ 时，已无法找到满足飞行条件的路径，说明若途经校正点个数少于28，则无法满足飞行条件。

Table 1. Horizontal error and vertical error of the aircraft at each calibration point

表1. 飞行器在各个校正点校正前的水平误差和垂直误差

Figure 4. The path with the least number of correction points

图4. 经过校正点个数最少的路径

Figure 5. Path projection with the least number of correction points

图5. 经过校正点个数最少的路径投影

5. 模型评价和扩展

本文将飞行器爬升、下降的飞行距离等效为水平飞行距离，从而构建了误差产生矩阵，使得终点飞行条件可以线性化，基于此建立了线性整数规划问题，可以求得全局最优解。在考虑途经校正点的成功飞行条件时，将水平误差和垂直误差至少应有一个大于 $\theta$ 的条件进行放松，转化为标志矩阵，相当于设置了0~1整数规划问题求解隐枚举的过滤条件，极大提高了算法的运行效率。该模型简洁易求解，稳定性高，并可以通过模型参数调整过滤隐枚举条件的范围，极大提高了算法效率。本文在求解时仅保留了满足所有约束条件的一组最优解，实际上可能存在多组最优解。本文建立的模型基于水平误差修正和垂直误差修正过程是相互独立，且校正点要么满足水平误差修正要么满足垂直误差修正，若校正点可以同时进行两种误差修正，或者存在多个飞行器，或者增加水平误差和垂直误差修正条件，可在此模型基础上继续改进研究。若校正点对水平误差和垂直误差的校正和飞行器当前飞行状态有关，为动态校正，则本模型不适用。

参考文献