基于线性矩阵不等式的智能飞行器航迹规划方法

沈添天，袁思敏，吴芳，陈中祥，余果

( 1. 湖南师范大学 工程与设计学院, 长沙 410082；2. 中南大学 自动化学院, 长沙 410083 )

0 引 言

随着我国军事实力和航空事业的飞速发展，智能飞行器在军用和民用领域得到越来越广泛地使用. 在复杂的情况下，航迹规划已成为智能飞行器研究领域的一个重要课题. 然而智能飞行器的定位系统受到系统结构的限制，无法对自身进行精准的定位. 在飞行过程中会不断累积定位误差，定位误差累积到一定程度，就可能导致飞行任务失败. 因此，在飞行过程中对定位误差进行校正是智能飞行器航迹规划中一项重要任务. 航迹规划[1]是指在考虑多约束条件(如地形环境、自身性能等)下，为飞行器提供一条从出发点到目标点的求解简单、航迹较短、费时较少的飞行路径. 智能飞行器航迹规划通常被建模成一个带约束的最优化问题求解[2]. 本文将对定位精度受限情形下的多约束航迹规划问题展开研究.

航迹规划算法作为智能飞行器的核心研究内容，已有国内外诸多学者在这方面做了大量研究，例如Dijkstra算法[3-4]、A*算法[5]、粒子群优化(PSO)[6-7]，人工势场法[8]等. SEO等[9]利用最短路径A*算法建立硬件模型，通过代价函数更新规划路径. 但A*算法一般适用于二维平面的搜索，若应用在三维环境中，运算量庞大. CARSTEN等[10]则利用D*算法进行路径规划，并与立体视觉结合使用以实现三维环境中的动态路径. 另有遗传算法(GA)，因其具备较强的随机随即搜索能力而被广泛地应用于无人机3D航迹规划中[11]. KOU等[12]使用粒子群优化算法(ACO)解决了三维环境中带约束的无人机路径规划问题，但粒子群算法在复杂约束的路径规划问题中航迹精度不理想[13]. 在多种约束条件下，基于Dijkstra算法设计了路径规划方法的求解模型，通过校正策略优化和OD邻接矩阵处理，获得了满足约束条件的飞行路径[4].鉴于上述算法的优缺点，在三维环境及多个约束条件下，提出了一种基于线性矩阵不等式(LMI)的航迹规划方法.

LMI方法可将大量线性规划问题转化成几个线性规划问题，能更好地解决多个线性约束问题. LMI问题通常可以分为三类：可行解问题、线性目标函数优化问题和广义特征值优化问题. 文中主要研究目标函数的优化问题，以最小路径长度和最少校正点数为目标，通过两个目标的不同权重得到最优解. 以包含出发点、目标点及多个校正点的飞行区域为研究对象，通过线性矩阵不等式建立数学优化模型，并且通过两组数据的仿真结果，以及与Dijkstra算法(狄克斯特拉)、GA[14]及目标导引法的算法性能进行比较，从而验证了所提方法的有效性. 与其他方法相比，所提方法具有校正点少，航迹长度短的优点.

1 问题描述

1.1 问题引出与分析

在飞行过程中，飞行器会逐渐累积其定位误差，因此需要多个校正点对其进行误差校正. 其中，误差校正次数和航迹长度是两个重要的考虑因素. 文中主要研究在多个约束条件下的航迹规划，使得经过校正区域内的校正次数尽可能少并且航迹长度尽可能短.

飞行器航迹规划主要考虑如下几个约束条件：

1) 起点误差约束

在出发点，飞行器的垂直和水平误差均为0.

2) 误差增长约束

飞行器在飞行过程中由系统限制所产生的垂直误差和水平误差只与飞行距离线性相关，飞行器每飞行1 m，垂直误差和水平误差均增加 δ ，单位为m.

3) 误差校正点功能约束

在每个校正点上，垂直误差和水平误差不能同时校正. 在此前提下，当飞行器进行垂直误差校正后，垂直误差为0，水平误差不变. 同理，飞行器进行水平误差校正后，垂直误差不变，水平误差为0.

4) 误差校正点执行条件约束

为约束校正点执行校正功能的误差范围，设定飞行器的垂直误差校正的条件为累计垂直误差不大于α1， 累计水平误差不大于 α2；飞行器的水平误差校正条件为累计垂直误差不大于 β1，累计水平误差不大于β2，单位均为m.

5) 终点误差约束

为确保飞行器飞行任务能顺利完成，到达终点时，飞行器的垂直误差和水平误差均应小于 θ (m).

在上述条件下，为飞行器规划出一条从出发点到目标点的航迹. 在本文中实现的是双优化目标航迹规划，即在实现最短航迹长度的同时经过最少校正点数. 在航迹规划中存在大量无序的误差校正点，这些校正点的坐标和校正类型已知. 在满足约束条件的前提下，我们需要从中选出几个校正点来确定最优航迹规划模型.

1.2 符号说明

本文中R为实数集.VT和 ‖V‖分 别代表向量V的转置和欧几里得范数. 其余符号所表示的含义列在下表1中予以说明.

表1 常用符号含义

2 多约束优化模型的建立与求解

本节通过引入LMI方法，经过凸优化后，构建既能满足多个约束条件，又能实现校正次数最少和航迹长度最短的航迹规划模型. 首先通过设计一个0～1决策变量数组来描述一个可变航迹，然后将基于航迹的决策矩阵描述提出所有约束条件，最后将这些约束条件转化成矩阵形式，从而促进了LMI优化.

2.1 描述飞行轨迹的变量矩阵

由于飞行器在飞行过程中会产生垂直方向和水平方向上的累积误差，因此一定间隔的定位校正对于获得满足条件的航迹至关重要. 假设航迹是由笛卡尔空间中一系列引导飞行器从起始位置(记为A点)开始向目标位置(记为B点)飞行的校正点连接而成的.

一般来说，每次校正时，定位误差都会在水平方向或垂直方向上重新校正. 为便于后续描述，把校正点分别命名为水平校正点和垂直校正点来表示它们对飞行器定位的影响. 图1中在大量的冗余校正点中，它们的选择将直接决定在多重约束下以及目标满足度方面的飞行性能.

图1 起始点A和目标点B周围的冗余校正点

在筛选过后，对被选中的校正点采用面向目标的方式进行重新排列，以便后续变量矩阵的定义. 具体方式以图2所示的筛出的一小组校正点为例来解释这种以目标为导引的重新排列.

图2 筛选出的校正点以目标为导引的方式重新排列(以一小组选中校正点为例)

A点和B点之间的直线轨迹被视为不可能发生的参考轨迹，因此被视为理想情况. 如图2(a)所示，假设有6个筛出的校正点，根据其投影在参考轨迹上的分布重新排序. 这些校正点的次序是根据它们的投影到A点 的距离进行重新排列的. 我们把它们和A点、B点一起记作Ck，k=1,···,n, 其中C1表 示点A，Cn表示点B，C2和Cn-1分别表示在参考轨迹上的投影距A点最近和最远的校正点. 在6个被选中的校正点所形成的具体例子中，它们被重新排序为C1,C2,C3,C4,C5,C6,C7,C8，如图2(b)所示. 至此，校正点的预处理完成，并生成一组经过筛选和重新排序的校正点用于后续的变量定义和选择最优化. 这种预处理确保了校正点以目标为导引的选择.

基于校正点ck，k=1,···,n, 的重新排列，我们建立了一个特定的变量矩阵，记作X∈Rn×n，以此来描述不同的航迹. 该矩阵的元素值为0或1，第i行和第j列的元素记作xi,j，其中i∈[1,n] ，j∈[1,n] .

当xi,j=1 时，表示航迹的一部分是从第i个校正点出发到第j个校正点结束. 其中x1,k=1 表示第一部分轨迹，从A点(第一个校正点)开始到第k个校正点结束. 航迹至少要经过一个校正点，而且变量矩阵X的第一行有且只能有一项被赋值为1，表示航迹的第一部分是从点A出发到第一个被选中的校正点. 因此对于第一行引出的结论为

该校正点作为一个新的起点只能被选择一次，且下一个校正点必须要比前一个更接近终点B，即Cn.

因此，式(3)中的变量矩阵X实际上是一个三角矩阵. 进一步分析可以得到，由于飞行器在每一个校正点上只能经过一次，所以1的值限定了该矩阵每一行及每一列的元素和的最大值. 这些条件可以被表述为：

为确保飞机最终能到达终点B，矩阵X的最后一列中至少有一项要设为1. 由此可以得到

我们以图2的8个点为例来描述最优航迹和决策矩阵之间的关系. 如图3所示，假设最优航迹为A→C3→C5→B，校正次数最少且飞行距离最短，决策矩阵如表2所示. 由图3和表2可知，飞行器的航迹是从A到C3，然后到C5，最后到B；它们对应的变量为x1,3=1,x3,5=1,x5,8=1 .

图3 经过规划后的轨迹

表2 图3所示规划后轨迹的变量矩阵

2.2 约束条件

1)初始校正约束点

由于一般环境(风、GPS不稳定等因素)造成的飞行器漂移，会导致飞行器在飞行过程中产生垂直和水平误差. 根据误差校正点执行条件约束，第一个被选中的校正点必须满足以下关系式：

式中：d1,j表示A点到第j点的欧氏距离；Nj为指示变量；当Nj=1 ， 表示第j点为垂直校正点；当Nj=0 ，表示第j点为水平校正点. 从A点到第j点的飞行航迹会导致定位误差. 根据误差校正点功能约束，无人机经过校正点Cj误差校正后的水平误差 ΔX1,j和垂直误差 ΔY1,j为：

2)中间校正点约束

飞行器从第j点(j≠1 ) 飞到第k点(k≠1 )，根据误差校正点执行条件约束可以求得满足条件的水平误差和垂直误差如下：

考虑到飞行中的累积误差，分别用 ΔXj,k和 ΔYj,k来表示在第k点校正后的水平误差和垂直误差. 根据误

差校正点功能约束，对于 ΔXj,k和 ΔYj,k有如下的表述：

3)最终校正点约束

在最终校正点约束中，飞行器从第j点(j＜k)出发到第k点(k=n)，根据终点误差约束，水平误差和垂直误差必须满足以下关系式：

4)目标函数的建立

本文针对航迹规划模型设定了双优化目标.Obj1是为了获得最小飞行距离， O bj2是为了使经过的校正点数最少，

2.3 基于LMI的轨迹优化

在2.2节中可以选择611个校正点时，线性约束条件达300 000以上. 在本节中，这些约束通过线性矩阵不等式被转换为若干个约束.

我们定义一个所有元素值都为1的辅助矩阵v

根据每行、每列之和的上界，可以将约束(5)～(6)转化为如下所示的线性矩阵不等式：

建 立b1=[1,01×(n-1)]T和b2=[01×(n-1),1]T为 两 个 辅助 矩阵，其 中 01×(n-1)表 示1 ×(n-1) 的 零 向 量. 由式(4)和式(8)可得出第一行和最后一列的下界如下：

根据2.1中校正点的顺序，距离矩阵D被描述为

根据误差校正点功能约束，所选的第一个校正点必须满足

式中：b1X表示式(3)中变量矩阵的第一行；N=[N1,N2,···,Nj,···,Nn]为校正点类型向量.为了区分其他校正点和两个端点，设N1=-1，Nn=-1.根据Nj在2.2节2)中的描述，N=[-1,N2,···,Nj,···,-1].

基于式(3)中的决策变量矩阵X，我们设计了一个特殊的矩阵M. 将第i行和第j列的元素记作xi,jdi,j，其中i∈[1,n]，j∈[1,n] .

由式(13)～(14)可将水平误差约束和垂直误差约束等效化为：

最后根据约束式(17)～(18)，当飞行器从选定的第j个校正点到达终点B(k=n)时，垂直误差和水平误差必须满足表达式

式中：v为 式(21)中的矩阵；为矩阵

双优化目标函数式(19)和式(20)可表示成：

总之，2.2节中数量超过300 000的约束通过2.3节中的线性矩阵不等式可以转换成若干个约束，当可以选择的校正点数量增加时，只增加了变量数量而不增加约束条件，从而大大减少了计算量，简化了内存空间.

3 数据验证结果

本节通过两组仿真验证了所提方法的有效性. 具体来说，首先通过图1中绘制的一系列校正点将提出的方法与许多现有的方法进行比较，然后根据另一组较小规模的数据进行轨迹规划，来证明所提方法的通用性. 使用的电脑配置为标准PC (Intel i5-3 470 CPU，8.00 GB内存，Windows 10操作系统)，使用MATLAB和Gurobi求解器.

3.1 和现有方法的比较

图1绘制的合成场景中包含给定的611个校正点和两个端点(出发点A和目标点B)，校正点的坐标及类型由MATLAB中随机函数生成. 将这611个校正点表示为Ck,k=1,···,613 ，其中有306个水平校正点和305个垂直校正点. 误差约束参数给定为α1=25m， α2=15m ， β1=20m ， β2=25m ， θ =30m ，δ=0.001m. 应用提出的基于LMI的航迹规划方法来实现式 (38) ～ (39) 中的双重目标，生成的最优规划三维航迹图如图4所示. 具体飞行轨迹如下：A→C504→C295→C92→C608→C90→C12→C404→C595→C502→B.

图4 最优规划三维航迹图

误差校正点数和校正前误差的航迹规划结果如表3所示，校正点类型取1表示该点为垂直误差校正点，取0表示该点为水平误差校正点. 由表3可知，第一个选定的校正点C504是垂直校正点，到达C504时的累积误差均为13.39 m. 因此，在C504处进行垂直校正之前定位误差满足施加的约束条件(垂直误差上限为α1=25m ，水平误差上限为 α2=15m )，C92等其他垂直校正点也满足这个约束条件. 从表中可以明显看出，飞行器到达所有选定的水平校正点时累积误差也满足执行水平校正的约束条件(即垂直误差上限为β1=20m ，水平误差上限为 β2=25m ). 在目标点B处的最终垂直误差和水平误差分别为8.49 m和19.69 m，它们都小于到达终点的误差上限 θ = 3 0m .飞行器的定位误差随选取校正点顺序的变化情况如图5所示. 图中校正点序号代表选定的9个校正点和目标点B. 结合航迹规划结果表3，由折线图5可以直观地看出，选取的校正点交替出现，且误差都在约束条件之内尽可能大，能够在误差即将超出约束条件时及时进行校正. 最优航迹长度为104 890.55 m，经过的校正点个数为9.

表3 航迹规划结果 m

图5 校正点序号对定位误差的影响

为验证上述LMI方法的优越性，将上述结果与一些现有的航迹规划方法在相同的611个校正点区域场景所得结果进行对比：

1) Dijkstra算法：Dijkstra算法计算从一个节点到其他节点的最短路径，进而得到最短路径树. 它使用广度优先搜索来解决单源最短路径问题，所以空间复杂度和时间复杂度都比较高.

2) 遗传算法：遗传算法通过模仿自然选择和遗传学的机制寻找最优解，并根据适应度值或交叉操作循环执行新一代操作，直到达到优化目标. 数据量较大时，遗传算法容易陷入局部最优.

3) 目标引导法：目标导引法通过最小化与目标点和前一个通过的校正点的距离来一个一个地选择校正点，产生下一个相邻的校正点，将轨迹引导到目标点. 算法逻辑简单，但每次选择一个校正点，都必须遍历所有校正点.

对图1的数据采用Dijkstra算法、GA和目标导引法分别进行处理，算法性能比较如表4所示，从表中可以看出4种算法的各数据点误差均满足约束条件，其中Dijkstra算法的航迹长度最优，但是校正点数量最多，表明Dijkstra算法不适用于双目标优化问题. 除Diijkstra算法外，其他3种算法的校正点数量相同. 而采用LMI方法的飞行轨迹长度比GA短1 947.42 m，比目标导引法短6 395.948 m，说明LMI方法优于GA和目标导引法. 本文提出的LMI方法的有效性在于它可以将大量约束转换为几个约束. 当校正点数变化时，约束条件不变，只需要调整变量矩阵的大小. 它可以应用于通过调整变量矩阵解决较大尺度校正点问题的轨迹，因此适用于解决多约束下的航行轨迹问题.

表4 算法性能指标

3.2 较小规模数据的仿真

上节的大规模数据用于验证算法的有效性，本节的较小规模数据主要用于验证算法的正确性. 对包含325个校正点的合成场景进行航迹规划，校正点数几乎是第一组数量的一半. 误差约束参数给定为α1=20m， α2=10m ， β1=15m ， β2=20m ， θ =20m ，δ=0.001m. 基于上述提出的LMI方法得到的最优规划三维航迹图如图6所示. 具体航迹规划如下：A→C163→C114→C8→C309→C305→C123→C45→C160→C92→C93→C61→C292→B.

图6 最优规划三维航迹图

表5显示了飞行轨迹经过的校正点及进行校正前的误差，由表5数据可知，用于执行垂直改正的α1=20m (累积垂直误差)和 α2=10m (累积水平误差)的上边界条件在所有垂直校正点均得到满足. 飞行器到达所有选定的水平校正点时累积误差也满足执行水平校正的约束条件(即垂直误差上限为β1=15m ，水平误差上限为 β2=20m ). 飞行器到达目标点B的垂直误差和水平误差均小于 θ =20m 的上限. 最优飞行轨迹的行程长度约为109 342.28 m，校正点个数为12.

表5 航迹规划结果 m

4 结 论

本文对工作在复杂环境下的智能飞行器提出了一种基于LMI的航迹规划方法. 首先通过设计一个0～1三角变量矩阵来表示飞行航迹，该航迹以目标为导引，不重复的经过一系列校正点. 对于航迹相关矩阵变量项施加了一些强制性的约束条件，如最终定位误差的上限、进行校正的条件等. 所有这些约束条件作为一个整体之后被转换并施加到先前定义的变量矩阵，然后采用基于线性矩阵不等式的优化方法实现了最小校正次数和最小行程长度的双重目标. 仿真结果验证了LMI方法的有效性，并且证明了该方法具有较少的校正次数和较短的航迹长度. 在未来的研究中，我们将进一步研究具有不确定性的航迹规划.