有人/无人机编队队形集结控制研究

本文主要是介绍有人/无人机编队队形集结控制研究，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

源自：系统工程与电子学报

作者：吴立尧苏析超王垒潘子双

注：若出现无法显示完全的情况，可搜索“人工智能技术与咨询”查看完整文章

摘要

针对有人/无人机(manned/unmanned aerial vehicle, MAV/UAV)编队队形集结控制问题，设计了一种基于航迹规划-跟踪的MAV/UAV编队集结控制策略。首先，考虑编队队形集结边界约束与防碰撞约束条件，设计了一种基于Dubins曲线与协同模拟退火粒子群优化相结合的编队集结航迹规划算法。然后, 以规划得到的集结航迹作为期望航迹，设计了一种双回路结构的非线性路径跟踪控制方法。最后, 利用Matlab软件对设计的航迹规划与跟踪控制方法进行了仿真对比实验，仿真结果验证了方法的有效性和优越性。

关键词

有人/无人机, 队形集结, 航迹规划, 航迹跟踪控制, Dubins-协同模拟退火粒子群优化算法, 非线性控制

引言

有人/无人机(manned/unmanned aerial vehicle, MAV/UAV)编队作战是MAV和UAV系统通过平台互操作和资源共享控制，按一定队形组成整体共同执行相同作战任务。同传统UAV编队相比，控制基站位于编队内MAV，飞行员作为绝对决策者指挥控制整个编队飞行，不仅发挥了人在回路的智慧与决策力，同时解决了传统UAV编队控制范围和通信延时的问题，大大提高了编队作战效能。在未来战争中，MAV/UAV编队作战将会扮演重要角色，因此MAV/UAV编队技术成为各军事强国的研究热点。

编队队形集结控制是MAV/UAV编队控制系统关键技术，是MAV和多UAV在起飞与空中飞行阶段形成期望队形的过程。国内外学者在编队集结问题研究上取得了一些成果，现有文献方法可分为两类：基于分布式算法和基于航迹规划-跟踪算法。文献[1]针对多UAV紧密编队提出了基于信息一致性的集结控制方案；文献[2]通过一阶一致性算法使UAV到达任务区域形成松散编队，利用二阶一致性算法将松散编队压缩成为紧密编队。文献[3]利用一致性算法解决了UAV编队集结，并考虑了编队内防撞问题；文献[4]利用集结点分配算法和一致性算法，解决了多UAV定点定时集结问题。文献[5]以攻击任务为背景，基于一致性算法设计了一种分散化控制策略，实现了多UAV同时到达；文献[6]改进了含有虚拟Learder的一致性算法以实现多UAV同时集结到目标位置；文献[7]以预期到达时间为变量通过一致性算法实现多机空中聚集及同时到达。该类方法通过控制UAV速度以改变飞行时间，有效解决多UAV同时到达集结点问题，这要求UAV速度具有较大的可变范围，但这类方法仅实现同时到达集结点，无法兼顾编队集结时各UAV的航向和速度，不能真正完成编队集结。

基于航迹规划-跟踪策略是预先规划出集结期望航迹，并设计期望航迹跟踪控制律，控制编队内各UAV沿预设航迹飞行。文献[8]采用勾股速端曲线作为备选路径，设计了分布式协同粒子群优化(particle swarm optimization, PSO)算法规划UAV编队集结航迹；文献[9]基于元胞遗传算法设计了时间协同和防碰撞的编队集结路径规划策略；文献[10]针对固定翼UAV协同作战时的编队集结问题, 提出了一种新的航迹规划和位置分配方法，实现了编队集结；文献[11]基于快速搜索随机树(rapid-exploration random tree, RRT)方法提出一种多UAV集结协同航迹规划策略，通过设定时间约束使各UAV同时到达；文献[12]运用A*算法规划得到编队集结航迹并通过B样条曲线对集结航迹进行平滑处理。这类方法通过规划最优集结航迹实现多UAV同时集结，是通过航迹的长短改变飞行时间，改善了控制律对于飞行速度的依赖，但无法兼顾编队集结时位置、航向等约束。

本文以MAV/UAV编队执行空中任务为背景，为实现编队安全集结，借鉴航迹规划-跟踪控制框架，综合考虑集结首末端队形约束、运动学约束和碰撞约束问题，设计了一种航迹规划-跟踪的MAV/UAV编队集结控制策略。首先，采用Dubins曲线作为航迹规划方法，设计了一种协同模拟退火PSO(cooperative simulated annealing PSO, CSAPSO)算法求解各僚机最优集结点位置，得到规划的编队集结航迹。然后, 以规划的协同集结航迹作为期望航迹，设计了一种双回路结构的非线性路径跟踪控制方法。最后，利用Matlab软件对本文设计的编队集结航迹规划和跟踪控制方法的有效性和优越性进行仿真验证。

1 编队集结控制问题

编队集结控制问题可分为队形集结航迹规划和期望航迹跟踪控制两个子问题，如图 1所示。MAV/UAV编队队形集结任务想定为：长机MAV从基地或者空中出发，按预先规划的航迹飞行，各僚机UAV分布在空间任意位置。飞行员下达编队集结指令后，编队指挥控制系统进行顶层航迹规划，规划得到的集结航迹信息传递到航迹跟踪控制模块，生成的航迹跟踪控制律传输到各UAV的底层控制系统中，从而实现了由任务需求到指令下达再到编队飞行航迹的映射。

图1 编队集结控制问题分解

1.1 编队集结控制框架

为实现编队由初始任意状态到期望队形的集结过程，本节设计了MAV/UAV编队集结控制策略框架，如图 2所示。

图2 编队集结控制结构图

集结控制包括5个层次，第1层是编队协同任务层，属于人机交互模块，由飞行员选择编队集结任务，并下达集结队形信息指令；第2层是中央计算处理模块，通过接收各UAV状态和战场态势，实时计算出编队内相对距离、与障碍物的距离以及各MAV/UAV飞行时间等，输入下层模块；第3层是航迹规划模块，利用设计的航迹规划算法生成最优的协同集结航迹；第4层是航迹跟踪模块，将上一层生成的协同集结航迹作为期望航迹，利用设计的航迹跟踪控制方法生成姿态制导律；第5层是UAV控制系统，上层姿态制导控制律输入控制系统中实现相应UAV飞行动作。前4层模块部署于MAV指挥控制系统中，第5层部署于UAV控制系统中。为简化研究问题，做出如下假设：

(1) 忽略气动力对于MAV和UAV的影响，只考虑质点运动方程；
(2) 不考虑编队内通信延时和丢包问题；
(3) 为保证编队以相同速度进入编队保持阶段，假设MAV和UAV在预定高度以固定速度匀速飞行；
(4) 当UAV到达集结点处时，编队采取长机-僚机控制策略实现队形保持。

1.2 长僚机运动学模型

本文研究重点在于设计顶层MAV/UAV编队集结策略，而非具体的底层控制器，故将MAV/UAV视为质点。长机MAV由飞行员操纵舵机实现控制到飞行航迹的映射，为简化问题，假定攻角和侧滑角均为0，MAV运动学模型可表示为

(1)

(1) 式中：xl, yl, zl表示长机MAV的位置坐标；θl, φl分别表示MAV的航迹俯仰角和航向角；g为重力加速度; vl为长机MAV的飞行速度; Dl, Wl分别表示MAV的飞行阻力和自身重力; Tl, nl, ϕl 分别表示MAV的发动机推力、法向过载和滚转角，由飞行员控制油门杆、升降舵和副翼舵实现MAV的运动。

同MAV不同，僚机UAV由自动驾驶仪实现控制律到飞行航迹的映射，则UAV运动学模型如下所示：

(2)

式中：i∈{1, 2, …, N－1};xi, yi, zi, θi, φi表示第i架UAV的位置坐标、航迹俯仰角和航向角；

分别表示第i架UAV的俯仰角速度控制律和航向角速度控制律，由僚机UAV控制系统实现。

1.3 约束条件

考虑N架MAV/UAV编队起飞集结问题，设任务编队内包含一架MAV和N-1架UAV，I={l, 1, …, N－1}为编队序号集合。

(1) 边界条件约束

已知编队内MAV和UAV初始位姿，初始时刻状态可表示为[xi, yi, φi, vi]。为满足编队集结条件，僚机对应长机的终端状态约束为

(3)

式中：下标f和l分别表示僚机和长机状态；Δx和Δy分别表示x方向和y方向长僚机间距满足期望队形参数。

(2) 转弯性能约束

固定翼MAV与UAV在实际飞行中通过滚转运动实现航向偏转。UAV的转弯半径R应满足如下转弯半径约束：

(4)

式中：g为重力加速度；ϕmax 为满足UAV机动性能的最大滚转角。

(3) 编队内防撞约束

为避免编队集结过程中内部MAV与UAV之间发生碰撞，需满足以下约束条件:

(5)

式中：xi(t)、yi(t)分别表示t时刻第i架飞行器质心坐标；L为最小安全距离。

(4) 编队外避障约束

同时为避免编队外部与障碍物、禁飞区等发生碰撞，需满足以下约束条件：

(6)

式中：xo、yo表示障碍物质心坐标；Ro为障碍物最大威胁半径。

2 协同航迹规划算法设计

N架MAV/UAV编队集结航迹规划问题可等价于为每架UAV确定一个集结位置，使UAV与MAV满足速度、航向角和队形约束，同时所有航迹的总体评价指标趋于最优或次优。为更好地解决MAV/UAV编队协同集结航迹规划问题，本节设计了并行合作的模拟退火粒子群算法确定最优编队集结点，并利用Dubins曲线生成编队集结航迹。Dubins曲线是曲率限制下两个位姿点之间的最短路径[13]，被广泛应用在UAV的航迹规划中[14-16]。

PSO算法是一种基于群体智能的随机寻优算法[17]，同其他进化算法相比，PSO算法以收敛速度快，算法简单，避免了复杂的遗传操作等优点被广泛应用在工程计算中。标准PSO算法是假设在D维目标搜索空间中，有M个粒子组成群体，第j个粒子的位置和速度可分别表示为Xj=(xj1, xj2, …, xjD)和Vj=(vj1, vj2, …, vjD)，通过将粒子位置代入到适应度函数中可确定对应的适应度值。第i个粒子搜索到的最优位置称为个体极值pbestj=(pj1, pj2, …, pjD)，整个粒子群搜索到的最优位置称为全局极值gbestj=(gj1, gj2, …, gjD)。每个粒子的位置和速度更新公式如下：

(7)

式中：vj(k)为粒子的当前速度; xj(k)为粒子的当前位置; c1和c2为学习因子; r1和r2为[0, 1]均匀分布的随机数；pj为粒子到目前为止的最优位置；pg为所有粒子到目前为止的最优位置；λ为压缩因子，如下式所示：

(8)

模拟退火(simulated annealing, SA)算法是一种基于Monte-Carlo迭代求解的随机寻优算法[17]。SA算法从较高的初始温度开始，随着温度参数不断下降，通过一定的概率在解空间突跳随机寻找使目标函数最优的全局最优解，即可实现在局部最优解中概率性的跳出得到全局最优解。Metropolis准则定义了某一温度下系统从状态i变化到状态j的能量概率为

(9)

式中: Ei和Ej为固体在状态i和j状态下的能量；Ej－Ei为能量改变量；K为波尔兹曼常数；T为当前温度。SA算法以一种概率接受“变差的”解，这种行为看似不合理，实则在一定程度上提高了算法的灵活性，扩大了算法的搜索范围，增加了粒子的多样性。这种模拟退火的策略使算法避免在迭代搜索过程中因陷入局部最优解的弊端，提高了搜索到全局最优解的可能性和可靠性。但主要不足是收敛速度太慢，尤其是问题的规模增大时，算法的效率更低。

MAV作为长机按预先设计的航迹γl(m)={xl(m), yl(m), φl(m)}飞行，γl(m)包含MAV位置和航向角信息，可采用离散点集合γl(m)={xl(m), yl(m), φl(m)}表示，其中m表示离散点位置，m∈{0, 1, 2, …, K}。这里将N-1架UAV航迹的集结点位置集合视为N-1个子种群，各子种群按并行按标准粒子群算法进行速度和位置更新。为了求解各UAV最优的集结位置，确定子种群的适应度函数尤为关键。

以期望集结队形为楔形编队为例，如图 3所示。

图3 楔形编队示意图

MAV作为编队长机，各UAV作为MAV的僚机分别跟踪长机预设航迹实现队形集结。已知长机以初始状态和固定速度vl沿预设航迹γl飞行，僚机则以初始状态qfi(0)={xfi(0), yfi(0), φfi(0)}和固定速度vf沿规划航迹飞行，其中僚机集结点集合可表示为γfi(m)={xfi(m), yfi(m), φfi(m)}，下脚标fi表示第i架僚机UAV。假设在第i架僚机的最优集结点qgi处完成与长机的编队集结，可得qgi、γl和γfi之间的关系如下：

(10)

式中：di={Δxi, Δyi, 0}为第i架僚机相对于长机的期望队形参数；Xi为第i架UAV相对于MAV的最优集结点位置。为成功实现编队集结成期望构型，长机和僚机需要同时到达，即各自到达集结点的飞行时间差值为0。实际上飞行时间差|Te| 应满足以下关系式：

(11)

(12)

(13)

式中：Tf和Tl分别表示僚机和长机的集结时间；vf和vl分别表示僚机和长机的飞行速度；t(., .)表示初始状态与集结状态之间的飞行时间；L(., .)表示两点状态之间的航迹长度，可由Dubins曲线理论中已知两点位姿间的长度公式求得[11]。ε为阈值，qgi集结点的确定对于|Te|起决定性作用。这里定义第i架UAV的适应度函数为

(14)

为得到安全、最优的编队集结航迹，本节将编队视为一个种群，编队中各UAV视为子种群。在整个大种群中，为避免各子种群之间发生碰撞以及与外界障碍物等发生碰撞，各子种群在计算各自适应度函数之后将进行子种群间的协作评价，将当前各子种群得到的个体极值共享构成新的团体，通过修正编队种群的协同适应度函数，更新个体极值和群体极值，确定各子种群得到的最优值是否满足编队集结约束，从而找到满足编队协作下的最优(或次优)值。

为了规划出安全、可飞的UAV航迹，将防撞约束和避障约束以惩罚函数加入到适应度函数中，定义编队协同适应度函数如下：

(15)

式中：kinner和kouter分别表示编队内外防碰撞的惩罚因子；H为障碍物个数。当编队内外发生碰撞时，此时J1或J2大于0，惩罚项起到约束作用；当编队避开障碍物或编队内未发生碰撞，J1或J2等于0，此时各粒子群最优群体极值即为编队各UAV安全可飞航迹的集结点位置。

当集结过程中编队内MAV/UAV之间或UAV与障碍物发生碰撞时，此时协同适应度函数J明显是一个很大的数，不符合协同适应度函数的精度，种群将重新对各子种群重新进行更新。为提高算法寻优效率，避免发生碰撞的两架UAV同时改变航迹，本文为编队MAV/UAV设立优先级，即优先级高的飞机可保持原规划航迹，低优先级的飞机在碰撞基础上重新规划，原集结点不符合安全集结要求，需要重新计算符合安全的集结点。整个MAV/UAV编队中，MAV级别最高，所有UAV听从飞行员指挥，其次根据UAV种类、油量等因素确定UAV级别，执行任务重的UAV级别高，油量相对少的UAV级别高，以此依次确定UAV1、UAV2、UAV3和UAV4。

综合上述分析，给出Dubins-CSAPSO航迹规划算法的求解步骤如下：

步骤1 给定MAV长机预设航迹γl, 离散化表示为γl(m)={xl(m), yl(m), φl(m)}，m∈{0, 1, 2, …, K}，根据队形参数计算各UAV集结点集合γfi(m)={xfi(m), yfi(m), φfi(m)}；

步骤2 对各子种群进行初始化。设置各子种群的规模为M，初始温度Ti，结束温度Tf，当前迭代次数k=1和最大迭代次数TT；初始化各子种群中所有粒子的速度vij和位置xij，i=1, 2, …, N-1表示第i个子种群，j=1, 2, …, M表示子种群中的第j个粒子；

步骤3 根据式(7)更新各子种群中粒子的速度和位置，利用Dubins曲线理论计算qfi(0)和qfi(xij)两状态点之间距离，并根据式(11)、式(12)和式(14)计算第i个子种群的目标适应度函数值Ji，通过比较适应度函数值，存储局部最优pij和全局最优pig；

步骤4 各子种群选择各自的全局最优作为代表，并与其他子种群以协作的方式组成团体，根据式(15)计算当前协同适应度函数值；

步骤5 根据式(11)、式(12)和式(14)更新各子种群的目标适应度函数值Ji，得到前后状态的能量改变量，若改变量小于0，则更新值作为新的当前解，否则以概率式(9)接受更新值为当前解，然后进行退温操作，T=αT；

步骤6 检查终止条件(达到最大迭代次数TT、降到终止温度Tf或协同适应值函数值J满足阈值ε)，若没有达到条件，令k=k+1，返回步骤3；若满足条件，则输出协同的Dubins曲线。

综合上述编队集结策略的设计和分析，在已知长机MAV航迹和初始位姿的条件下，得到的航迹曲线是编队最优的集结航迹，既满足编队集结精度又保证了编队飞行安全。

3 航迹跟踪控制

为实现UAV准确跟踪期望航迹，路径跟踪控制器的设计是对于任意给定几何路线, 包括直线、圆和曲线，设计航迹跟踪制导律和姿态跟踪控制律，使UAV位置和姿态跟踪到期望航迹上并继续跟随期望航迹运动，如图 4所示。本节以单架UAV作为研究对象，以规划得到的集结航迹作为期望航迹，航迹跟踪误差运动学方程[18]为

图4 UAV航迹跟踪误差模型

(16)

式中：ex, ey分别代表纵向和侧偏距误差；χd表示期望航迹的航向角；κ(s) 是期望航迹上虚拟目标的曲率；

为UAV航向角与期望航迹航向角的差值；v, ω分别表示UAV的空速和航向速率控制指令；vw, φw分别表示定常风场的风速和方位角；

表示期望航迹上虚拟目标速度。

航迹跟踪控制模块是UAV控制系统中重要组成部分，本节设计了双回路结构的UAV非线性航迹跟踪控制器。外回路以虚拟目标速度和航向速率作为控制变量，基于矢量场方法设计UAV航迹跟踪制导律；内回路以外回路制导律作为控制输入，设计滚转角控制律跟踪外回路期望航向角，从而实现了期望航迹的跟踪。

3.1 矢量场方法

矢量场(vector field, VF)方法是一种简单且鲁棒性好的控制方法，通过在期望航迹周围构建向量，驱使UAV以期望航向角运动，实现跟踪误差收敛到零[19-23]。为了将UAV引导到期望航迹上，在期望航迹附近构建了渐近稳定的矢量场如下：

(17)

式中：kd>0是增益参数，用以控制ev收敛速度；χ∞∈(0, π/2]是一个常值角度，对任意ev满足：

当侧偏距误差和方位角误差同时等于0时，便实现了期望航迹的跟踪。在构建的矢量场中，UAV的期望航向角由χd(ey)决定，由式(17)可知，当UAV距离期望航迹较远时，UAV的方位差以χ∞跟踪期望航迹，即UAV以垂直期望路径方向运动；而随着侧偏距的逐渐变小，UAV逐渐发生偏转直至与期望航迹切线方向平行。

为设计航迹跟踪控制律，根据航迹跟踪误差模型式(16)定义李雅普诺夫函数如下：

(18)

对式(18)求导可得

(19)

式中：

根据式(19)设置虚拟目标速度

和UAV偏航速率ω的控制律为

(20)

(21)

将式(20)和式(21)代入式(19)中可得到

(22)

当ey≤0时，0≤χd(ey)≤π/2，此时eyvsinχd(ey)⩽0；当ey≥0时，－π/2≤ χd(ey)≤0，此时eyvsin χd(ey)≤0。所以对于任意ey，始终满足eyvsin χd(ey)≤0，即满足V1≥0；

。综上所述，路径跟踪误差模型式(16)在控制律式(20)和式(21)的作用下是全局渐近稳定的，即ex, ey,

渐近趋于0。

3.2 姿态跟踪

本节基于非线性方法设计内回路姿态跟踪控制器，使UAV的滚转角跟踪外回路得到的期望航向速率。UAV作水平转弯机动时，通常需要借助滚转姿态控制系统，通过调整副翼使UAV倾斜产生侧力，实现UAV的航向偏转，可表示为

(23)

式中：ϕ 为UAV滚转角。考虑UAV控制输入约束，则无人机滚转角满足以下条件：

为设计滚转角跟踪控制律，引入辅助控制输入

，综合式(16)和式(23)，得到路径跟踪误差系统如下:

(24)

式中：p为滚转角速度; u为滚转角控制变量。

结合式(23)，定义滚转角状态误差如下：

(25)

对式(25)求导可得

(26)

式中：

由式(21)得到。

为得到合适的滚转角控制律u，设计李雅普诺夫函数如下:

(27)

式中：λ>0，q>1为自选参数。

对V2求导可得

(28)

设计滚转角控制律为

(29)

式中：kp, kc为正实数，将式(29)代入式(28)中可得到：

(30)

当参数满足

时可实现

。综上，控制律式(29)的设计能够实现V2≥0，

，即实现状态变量

。

4 仿真实验及分析

为验证本文设计的编队集结控制策略的有效性，本文分别对航迹规划算法和航迹跟踪控制方法进行了Matlab仿真。仿真实验环境为Inter Core i7，2.30GHz处理器，Windows10操作系统，仿真软件为Matlab R2016b。

4.1 航迹规划算法仿真实验

在仿真过程中，仿真算法基础参数如表 1所示。本节想定两种编队集结情形：海上编队起飞集结和空中编队不同初始状态集结。实验1为起飞集结空域中无障碍物，并考虑MAV/UAV上升和航母舰面保障因素，最后在集结区域形成楔形编队。实验2为MAV/UAV在有禁飞区的空域中，以不同的初始状态完成编队集结，形成楔形编队。实验1和实验2仿真相关参数如表 2和表 3所示。

表1 仿真基础参数

表2 实验1仿真参数

表3 实验2仿真初始位置参数

图 5和图 6为实验1的三维和二维航迹规划图。如图 5所示，MAV首先从航母起飞，经历速度加速到理想速度、航迹倾角由0逐渐升至最大航迹倾角并保持再逐渐减至0后，MAV到达集结高度，并以理想速度按预设航迹飞行；舰面保障阶段，航母继续向前行进，UAV1~UAV4依次起飞后与MAV相同规律到达集结高度，同时依次与MAV完成集结并保持队形继续飞行，当UAV4与MAV到达集结点后，整个编队安全完成整个集结过程。

图5 MAV/UAV编队起飞集结三维规划图

图6 MAV/UAV编队起飞集结二维规划图

图 7为MAV与UAV1~UAV4距离随时间变化曲线图，其中以UAV1到达集结高度作为时间起点，MAV与UAV之间的距离随着时间不断变化。91s时，UAV1到达集结点，MAV-UAV1的距离为队形期望距离，并一直保持。在这个过程中，MAV-UAV1距离最小为53 m，超过编队内防撞距离。同理，136.3 s、183.4 s和233.5 s时，UAV2、UAV3和UAV4分别到达集结点，即相对距离为期望距离。综合起飞时间、舰面保障时间和平飞集结时间，整个编队的集结时间为302 s。如图 8为实验2的航迹规划图。初始阶段，MAV和4架UAV分别处于不同位置，此时MAV飞行员下达编队集结指令，4架UAV根据MAV预设飞行航迹规划集结航迹。图中编队在本文设计的算法能够避开禁飞区，且编队内各飞行器间始终保持安全距离，即编队实现了安全编队集结，整个编队集结时间为126.5 s。综合两次仿真规划的航迹可以看出，本文设计的算法能够有效的为整个编队规划出集结航迹，不仅避免编队内MAV/UAV之间的碰撞，而且防止UAV与障碍物之间的撞击，同时满足了编队内MAV和UAVs之间的速度、航向和队形要求，实现了安全、有效的集结。

图7 MAV与UAV间相对距离变化曲线

图8 不同位置编队集结规划图

为验证本文设计的航迹规划算法的优越性，将本文算法与合作遗传算法(cooperative genetic algorithm, CGA)和文献[14]进行了对比分析。表 4为本文算法与合作遗传算法作用下各UAV与MAV的集结时间差。由表 4明显看出，本文设计算法满足时间阈值要求，由于UAV飞行速度很快，最大误差仅为v·Δt=0.4 m，可忽略不计。但CGA算法下，虽然时间也很小，但是MAV与UAV间相对距离最大达到了v·Δt=48 m，误差大，编队无法形成队形，不能实现编队集结。图 9为本文算法与启发式直接搜索算法[14]的对比图，由图可以明显看出，利用启发式直接搜索算法确定集结点，其中UAV1~UAV3生成的规划航迹同本文算法相同，但UAV4穿过禁飞区，无法安全飞行，即这次规划是失败的。

图9 两种算法规划航迹对比图

表4 各UAV与MAV的集结时间差

4.2 航迹跟踪控制仿真实验

首先对本文设计的航迹跟踪控制算法的有效性进行验证。本节综合直线、圆和Clothoid曲线作为期望航迹。仿真时间为110 s，UAV期望飞行速度v=20 m/s，UAV的初始位置为p1=[50, 100]，初始航向角为φ1=0°，初始滚转角为ϕ1=0∘，定常风场风速为vw=4 m/s，风场方位角φw=0°。控制参数选择为kd=0.1，kω=0.1，kx=0.5，χ∞=π/2，kp=0.5，kc=0.1，UAV的控制输入约束ω∈[－0.49 rad/s, 0.49 rad/s]和ϕ∈[－π/4 rad, π/4 rad]。

图 10为UAV跟踪期望航迹的仿真结果，图 11为UAV实际飞行航迹与期望航迹跟踪误差曲线，图 12和图 13分别为控制变量虚拟目标速度、偏航速率变化曲线，图 14为UAV滚转角变化曲线，图 15为本文设计方法与传统跟踪控制方法的对比图。

图10 UAV期望航迹跟踪结果图

图11 UAV航迹跟踪误差图

图12 虚拟目标速度

图13 航向速率变化曲线

图14 滚转角变化曲线

图15 航迹跟踪对比图

从图 10和图 11能够看出，本文设计的路径跟踪控制律在定常风场和UAV运动学约束条件下能够精准跟踪期望航迹曲线，跟踪模型误差保持在0.2 m内；由图 12看出期望航迹速度在控制器的作用下收敛到UAV速度设定值20 m/s；图 13中航向速率在跟踪期望曲线初始阶段变化明显，根据UAV初始位置和航向角以最大和最小航向速率跟踪期望航迹，12.5 s后收敛到期望值。图 14为内回路滚转角跟踪结果，UAV由滚转角决定航向速率，反之跟踪航向速率可得到滚转角变化曲线。初始阶段，UAV滚转角快速由0°变化到ϕmin，随后在控制律的作用下实现了滚转角的姿态跟踪。

为了证明本文设计算法的优越性，在无风和控制约束饱和的条件下，对本文设计跟踪算法与传统跟踪控制方法进行了对比分析，如图 15所示。在定常风场条件下，风场风速为vw=4 m/s，风场方位角φw=0°。由图能够明显看出，4种方法均能实现对期望路径的跟踪，但本文设计算法的控制性能明显优于反步法、比例-积分-微分(proportional integral differential, PID)、线性二次调节器(linear quadratic regulator, LQR)等传统跟踪方法。

进一步, 以实验2中规划得到的队形集结航迹作为期望航迹，对UAV非线性航迹跟踪控制器的实用性进行了仿真验证。仿真中，MAV和UAV的飞行速度为200 m/s，定常风场风速为vw=10 m/s，风场方位角φw=0°。相较于航迹规划部分中各UAV的初始位姿，本节假设各UAV的初始状态存在初始误差，具体MAV和UAV初始状态参数如表 5所示。MAV/UAV编队集结航迹跟踪效果图如图 16和图 17所示。图中黑色实线表示已规划好的编队集结航迹，红色实线为实际跟踪航迹，在航迹跟踪控制器的作用下，各UAV能够准确跟踪到期望航迹。但由于UAV飞行速度过高，从图中看出两种曲线重合在一起，为更佳清晰直观对比期望航迹与跟踪航迹，图 17给出了初始阶段各UAV的局部放大图，由图 17可以看出，在存在初始误差和定常风场干扰情况下，各UAV渐近趋向于期望航迹，并准确、稳定地跟踪期望航迹，实现了集结任务下达到编队航迹规划，再到集结航迹跟踪控制的过程，验证了非线性航迹跟踪控制器的实用性。