固定翼无人机编队的启发分布式模型预测控制

陆浩然，王永峰，鞠兴龙

(1.国防科技大学空天科学学院, 长沙 410073; 2.32178部队科技创新研究中心, 北京 100012; 3.北京泊松技术有限公司, 北京 100089)

0 引言

随着无人机技术的发展，多无人机联合协同作战成为未来现代空战的发展趋势。无人机协同作战可提高无人机在复杂环境和大规模作战任务中的任务执行效率，同时增加任务的可靠性、扩大任务区域的覆盖范围和拓展任务执行能力[1]。作为无人机系统控制的前沿领域，无人机协同控制在编队控制[2-3]、任务分配[4]、轨迹规划[5]等方向都涌现出了较多的成果[6]。

无人机集群的编队飞行是无人机协同作战中的重点研究方向，通常指多架具有独立自主功能的无人机，基于任务需求以一定的队形保持飞行并协同完成任务的功能。通过编队飞行，无人机可以获得气动效率、油耗、打击和搜索任务成功率及鲁棒性等性能的提升。

常用的编队控制方法[7]包括基于行为[8]、虚拟结构[9]、领导者-跟随者[10]以及一致性编队控制等方法[11]，这些控制方法各有优缺点，且在实际应用中可相互组合。从行为模式角度看，无人机集群的编队飞行包括编队成型和编队保持两种模式。在实际的编队飞行过程中，两种行为模式会交替切换，集群中各无人机不仅需要尽可能满足队形约束、保持安全距离，还需要在靠近威胁区域时通过局部路径规划使各自绕行威胁区域，从而产生编队临时解散并再次成型的需求。因此，编队飞行控制实际上包括队形设计、路径规划、保持控制、重构和避障防撞等多个设计目标。目前的研究主要是针对不同的设计目标单独设计控制律[12-15]，并在应用中基于不同的任务场景进行功能切换。然而在实际应用中，通常希望可以采用单一的控制框架和尽量少的控制律设计来同时满足所有控制目标，从而减少控制算法之间的耦合和控制器的复杂度。

为此，本文以分布式模型预测控制(Distributed Model Predictive Control，DMPC)框架[16-17]为基础，将队形设计、路径规划、保持控制、重构和避障防撞等编队飞行控制目标转化为描述队形、安全距离、速度及威胁区域的代价函数和相容性约束进行联合优化，从而实现单一控制框架下的多控制目标一体化控制。

同时，针对分布式模型预测控制存在的长距离轨迹求解困难、邻居轨迹信息交互通信量大，导致预测时域短的问题，文本采用了分阶段的启发式轨迹规划算法，分别利用模型预测和多项式曲线进行短期和长期轨迹规划。基于动态规划的思想，利用长期轨迹规划结果对模型预测控制的结果进行优化；同时，利用多项式轨迹参数进行邻机轨迹信息交互，从而有效降低了轨迹交互所需的通信量，使控制系统具备更高的实用性。

1 分布式模型预测控制

如图1所示，分布式模型预测控制将整个系统分割成若干子系统，并为每个子系统分别设计模型预测控制器，各控制器之间存在一定的信息交换[18]。

图1 分布式模型预测控制框图[18]Fig.1 Schematic of distributed model predictive control

分布式模型预测控制的优化问题可以定义为：对于子系统i=1,2,…,Na

(1)

使得对于所有的l=0,1,2,…,N-1

xi(k+l+1|k)=Aixi(k+l|k)+Biui(k+l|k)

(2)

ck(xi(k+l|k),ui(k+l|k))≥0

(3)

gk(xi(k+l|k),ui(k+l|k),xj(k+l|k),uj(k+l|k))≥0

(4)

xi(k+l+1|k)∈X

(5)

ui(k+l|k)∈U

(6)

其中，k为当前时刻,l为预测时刻；k+l|k表示变量值为当前时刻对l步后状态的预测；Ji为本地子系统的代价函数；xi与ui分别为本地子系统状态和控制量；xj与uj分别为邻居子系统状态和控制量；ck为本地子系统约束；gk为本地子系统与邻居子系统的耦合约束；X为系统状态约束；U为系统输入约束。

分布式模型预测控制具有易于建模、求解结果可直接用于控制等优点，然而该方法也存在一些固有缺陷。例如，模型预测的步长N受到无人机机载处理器算力限制，当N过大时模型求解时间会超过控制周期，因此在控制过程中预测出的轨迹只能实现在离无人机当前位置较近范围内的优化。此外，分布式模型预测控制需要基于部分邻机信息进行优化计算，例如为了满足基于安全距离的约束，控制器求解时需要获得邻机在未来一定时间段内的预期飞行轨迹。该问题通常可以在每个子系统解优化问题之后，将各规划的轨迹点通过通信链路传输给邻居来实现[19-20]。然而在实际应用中，无人机集群的通信容量是非常宝贵的资源，且在实战过程中极易受到干扰而导致通信中断，因此需要尽量减少编队控制过程中无人机之间的通信交互需求，从而提升算法的可靠性。

针对以上问题，本文采用启发型分布式模型预测算法，利用长距离多项式轨迹规划结果作为短距离模型预测控制的启发项，编队成员间以多项式系数的形式交互轨迹信息，并将邻机轨迹信息融入多项式轨迹规划求解时的代价函数，从而降低规划控制过程中的计算量与通信量。

2 无人机编队启发型分布式模型预测控制

如图2和图3所示，启发型分布式模型预测控制算法分为模型预测轨迹规划、多项式轨迹规划和轨迹组合筛选3个阶段。

图2 启发型模型预测轨迹规划Fig.2 Track planning based on heuristic distributed model predictive control

图3 规划过程各阶段Fig.3 Plannings tages

在模型预测轨迹规划阶段，控制器基于目标点信息、约束信息和本机状态信息，在轨迹可行域中采样多个模型预测轨迹终端点，并利用模型预测控制方法计算出N个时间步长内的控制量与到达终端点的轨迹。

在多项式轨迹规划阶段，依次以各模型预测轨迹终端为起点，并在目标点周围区域采样轨迹终点，利用多项式曲线平滑连接不同的模型预测轨迹终端与采样轨迹终点。利用获取到的邻机多项式轨迹参数生成对应的邻机轨迹，以规划轨迹的曲率、轨迹与邻机轨迹的距离、轨迹与障碍区域的距离作为代价函数对每条轨迹进行评价。

多项式轨迹规划结果本质上是对模型预测终端代价的修正，用于评价采样到的不同终端点，起到启发式规划的效果。

最后，分别对模型预测轨迹和多项式轨迹的代价在各自类别内进行归一化，再基于权重选取不同的模型预测轨迹和多项式轨迹形成轨迹组合，选取组合代价最小的轨迹作为最终规划轨迹，获取该轨迹中模型预测轨迹部分的第一个控制量作为下一个控制量进行控制，实现规划到控制的闭环。下面分别对算法各阶段进行具体介绍。

2.1 模型预测控制

本文研究的无人机编队控制场景限制在平面坐标系上，基于分布式模型预测算法框架，固定翼无人机编队控制问题可转化为以下几个部分。

2.1.1 离散运动模型

考虑到无人机自动驾驶仪可以生成加速度指令，无人机运动模型直接采用二阶积分器模型

(7)

其中，s、v、a分别为无人机位置、速度与加速度。将模型扩张到x-y平面并离散化，可得

zi(k+l+1|k)=Aizi(k+l|k)+Biui(k+l|k)

(8)

其中，zi=[six,vix,siy,viy]T,zi为无人机运动状态向量；six和siy分别为无人机i在平面坐标系x轴和y轴上的位置坐标；vix和viy分别为无人机i在平面坐标系x轴和y轴方向上的速度分量；Ai和Bi为无人机运动离散状态方程的系数；ui=[aix,aiy] 为无人机i在平面坐标系x轴和y轴方向上的加速度分量。

2.1.2 速度约束

固定翼无人机无法在空中悬停，其飞行速度受到最大与最小值限制，其约束可表示为

(9)

其中，vi(k+l|k)=[vix(k+l|k),viy(k+l|k)]为无人机i在k时刻对k+l时刻的速度矢量预测；Vmin和Vmax分别为无人机允许的最小、最大速度。

2.1.3 安全距离约束

安全距离为2架无人机之间允许的最小间距，可表示为

(10)

其中，si(k+l|k)和sj(k+l|k)分别为无人机i与无人机j在k时刻对k+l时刻的位置预测；R为单架无人机的安全距离。

2.1.4 避障约束

无人机飞行路线需始终与障碍区域保持一定的距离，为降低计算量，将障碍简化为以中心位置为圆心、具有一定半径的圆，该约束表示为

(11)

其中，si(k+l|k)为无人机i在k时刻对k+l时刻的位置预测；so,j为障碍区域j的圆心位置；Ro,j为障碍区域半径。

2.1.5 代价函数

模型预测控制的代价函数包括阶段代价函数和终端代价函数。终端代价函数以及与之相关的终端集和终端控制器组成了终端成分三要素，这三要素的设计关系到模型预测控制的稳定性与收敛性，对其进行计算和证明是一个比较复杂的问题。在本文采用的算法框架下，通过在可行域中采样获得模型预测控制的终端状态，利用不同的终端状态和终端代价函数引入启发性和随机性，以产生不同的轨迹，最后利用筛选机制去除不合理的轨迹，这样可以避免对终端问题三要素的设计过程，增强控制器的通用性。

本文采用的代价函数为

Lif(xi(k+N|k),xref)

(12)

其中，阶段代价函数定义为

(13)

终端代价函数定义为

(14)

其中，wS和wT分别为阶段代价函数和终端代价函数的权重系数；N为最大预测长度；sref为无人机的参考轨迹。

本文采用虚拟结构法进行队形控制，所有无人机保持预设虚拟结构，跟随虚拟结构中心飞行。每架无人机的参考轨迹定义为由虚拟结构中心运动轨迹和虚拟结构确定的一系列路径坐标点

sref=[sref(k),sref(k+1)，…，sref(k+N)]

(15)

2.1.6 终端点采样

如图4所示，设N为最大预测长度，dt为两次预测点之间的时间间隔，无人机的最大、最小速度分别为Vmax和Vmin；以无人机当前位置为起点，分别以VmaxNdt和VminNdt为半径，在无人机当前航向的一定偏差内划定弧形的采样区域，并在采样区域中的障碍区域外以一定位置间隔进行终端点采样。采样的终端点引导了预测控制的轨迹方向，并且是下一步多项式规划的起点，这样增加了预测的多样性，从而实现了启发式规划。

图4 模型预测轨迹终端点采样Fig.4 End points sampling of MPC track

2.2 多项式规划

2.2.1 多项式曲线

分别以各模型预测终端采样点的位置速度状态为起点，各轨迹终端采样点的位置速度状态为终点，求解轨迹在x轴和y轴上的分量随时间变化的曲线。为保证轨迹加速度连续，采用5次多项式曲线拟合

(16)

曲线约束为

(17)

其中，smpc(N)和vmpc(N)分别为模型预测轨迹终端点的位置和速度状态，作为多项式曲线的起点状态约束；spoly(T)和vpoly(T)分别为多项式轨迹终端采样点的位置和速度状态，作为多项式曲线的终点状态约束。

利用多项式曲线进行轨迹规划的步骤，本质上是用运算量较小的方法对无人机在较长距离上的飞行轨迹进行多次规划采样。通过评估每条采样轨迹的代价，并选取代价最优的轨迹作为无人机在长距离上的期望运动轨迹。

由于每条多项式曲线的起点是各模型预测轨迹终端，从动态规划的角度看，基于不同起点的最优多项式曲线的代价估计值可以视为对相应模型预测轨迹终端状态价值的评估。这样通过采样长期状态价值对短期的模型预测轨迹价值进行修正，使得对模型预测轨迹的评估和选择更为准确。

无人机集群轨迹规划时，需要将各成员的规划轨迹信息发送给邻居以避免在规划过程中的轨迹聚集，实现防撞功能。传统的多智能体轨迹信息交互方法需要将规划出的轨迹坐标点依次发送给周围的邻居，这样发送的信息量与轨迹长度成正相关，并且接收到邻居轨迹坐标点的成员无法基于自己的控制周期对邻居轨迹点进行修正。多项式曲线作为无人机在长距离上的期望轨迹，可以表示无人机未来的飞行意图，因此可以作为轨迹交互信息发送给邻机。使用多项式轨迹进行信息交互时，无论轨迹长度，只需要传输a0,x～a5,x和a0,y～a5,y这12个多项式系数，可以显著减少邻居无人机之间交换轨迹信息时传输的数据量。接收到邻居多项式轨迹系数的成员可以自行产生不同时刻或尺度的邻居轨迹点进行评估，有效增加了规划上的便利性。

2.2.2 代价函数

轨迹规划过程中需要降低的代价包括障碍代价、加速度代价以及邻居最小间距代价，其中障碍代价和邻居最小间距代价可以用log障碍函数的形式表示

(18)

其中，s、so和ro分别为当前无人机位置矢量、障碍圆心位置矢量和障碍区域半径。当s与so之间的距离大于ro时，代价函数为0，当s→so时，CB→∞。在每条轨迹上采样N个点，以所有采样点的障碍代价函数之和作为针对每个障碍的障碍代价，则总的障碍代价函数可以表示为

(19)

其中，M为障碍区域总数；N为每条轨迹的采样点总数；si为无人机在第i个采样点的位置[sx,i,sy,i]；so,j为第j个障碍区域的中心位置[so,j,so,j]；ro,j为第j个障碍区域的半径。

与障碍代价类似，邻机最小间距代价可以表示为

(20)

其中，L为邻机总数；N为每条轨迹的采样点总数；si为无人机在第i个采样点的位置[sx,i,sy,i]；snbr,i,k为第k个邻居在第i个采样时刻的预测位置；rnbr,k为与第k个邻居之间的安全距离。

为了优化轨迹曲率，设置加速度代价函数

(21)

其中，ai为无人机在第i个采样点的加速度矢量[ax,i,ay,i]。

此外，定义多项式轨迹终端点的代价函数，当T时刻终端采样点越接近T时刻参考轨迹点，代价越低

(22)

综合以上各式，每条多项式轨迹总的代价函数可以表示为

Cpoly=wnbrCnbr+woCo+waCacc+wTECT,j

(23)

其中，wnbr、wo、wa和wTE分别为各代价的权重。

2.2.3 终端点采样

如图5所示，多项式规划的终端点采样方法是以T+Ndt时刻的虚拟结构的参考位置为基准，在其周围以一定位置间隔进行采样。虚拟结构的参考轨迹有可能穿越障碍区域，因此对多项式规划的终端进行采样有助于在不同方向上找到有效规避障碍区域的路径。

图5 多项式轨迹终端点采样Fig.5 End points sampling of polynomial track

2.3 轨迹组合筛选

由于模型预测轨迹和多项式轨迹在求解时采用的代价函数与权重不同，因此两种曲线的代价值并不等价。为了综合评价组合而成的轨迹，需要先对两种轨迹分别按类归一化，再按类分配权重后，利用动态规划选取综合代价值最低的组合轨迹。

曲线代价值的归一化表达式为

(24)

如图6所示，轨迹组合选择问题可以等价为从本机当前位置出发，经过模型预测终端采样点，最终到达轨迹终端采样点的小规模动态规划问题。轨迹初始点到模型预测轨迹终端采样点连线的权重为

wmpc,l=wmpcCnorm,l,mpc

(25)

图6 轨迹组合筛选Fig.6 Track combination selection

模型预测轨迹终端采样点到多项式轨迹终端采样点连线的权重为

wpoly,l=wpolyCnorm,l,poly

(26)

其中，Cnorm,l,mpc表示第l条模型预测轨迹的归一化代价函数；wmpc为模型预测轨迹的权重；Cnorm,l,poly表示第l条多项式轨迹的归一化代价函数；wpoly为多项式轨迹的权重。通过求解该动态规划问题即可得到最优的组合曲线。

需要注意的是，与通常的轨迹规划不同，该组合曲线的多项式部分仅作为模型预测控制的启发项，用于增大模型预测控制的预测范围，所以并不需要其必须满足约束条件。在实际应用中，飞控系统在每个控制周期只使用模型预测控制规划出的下一时刻控制量，所以模型预测轨迹部分也并非是在未来时刻必须跟随飞行的指定轨迹，而是在每个控制周期都会基于当前飞行状态发生相应变化。

此外，在进行队形控制时，各无人机的控制目标是跟随本机与虚拟结构队形所对应的参考轨迹。在分布式控制的架构下，各无人机都基于自己的参考轨迹独立规划本机飞行轨迹，相邻无人机的轨迹信息会影响到本机轨迹规划过程中对采样轨迹的评价和选择。因此，若部分成员无人机因故障持续输出与实际不符的轨迹信息，正常成员可能会因为选择了避让轨迹，产生轨迹上的短期扰动，但不会影响正常成员对虚拟结构整体的跟随，也不会导致整个控制系统崩溃。

2.4 算法流程

综上所述，基于启发型分布式模型预测算法的固定翼无人机编队控制流程如下。

(1)离线阶段：

给定无人机允许的速度范围[Vmin,Vmax]，模型预测控制预测长度N, 多项式轨迹时长T，各障碍区域中心与范围半径ro，邻机之间安全距离rnbr，以及各代价函数的权重wS、wT、wnbr、wo、wa、wTE和两类轨迹的权重wmpc、wpoly。

(2)在线阶段，在K时刻：

1)获取本机位置和当前接收到的邻机轨迹参数；

2)对模型预测控制预测长度N和多项式轨迹时长T分别进行模型预测轨迹终端点和多项式轨迹终端点采样；

3)基于邻机轨迹参数拟合所有邻机轨迹；

4)求解所有当前位置到模型预测终端采样点的轨迹；

5)求解所有模型预测终端采样点到多项式终端采样点的轨迹；

6)利用轨迹组合筛选方法选取从当前位置到多项式终端采样点的轨迹；

7)在选取的组合轨迹中，将模型预测轨迹的第一步控制量u(k+1|k)施加于被控对象实施控制。

3 仿真实验及分析

考虑由9架无人机组成的固定翼无人机集群，利用本文提出的方法，仿真集群从无队形的状态，绕过多个障碍区域，最终形成直线编队的过程。仿真参数如表1所示。

表1 仿真参数

图7 编队成员飞行轨迹Fig.7 Flight paths of the UAVs

图8 编队成员收敛误差变化Fig.8 Convergence errors of the UAVs

图7所示为集群成员的飞行轨迹，图8所示为集群在收敛为直线编队过程中每架无人机与本机的虚拟结构期望位置误差的变化。9架无人机在起始区域不同位置、不同速度方向的初始状态下起飞后，依次绕过5个障碍区域，最终形成横向直线编队飞行，并实现了编队队形的快速收敛。由于模型预测轨迹的代价函数只考虑了对期望轨迹的跟踪，并将与障碍中心的距离视为约束条件，因此每架无人机在绕过障碍时都以尽量贴近障碍区域的轨迹运行,从而实现代价函数的最小化。

图9所示为编队中的一架无人机在飞行过程中单次规划的结果，基于本文提出的算法，无人机规划出了多条模型预测轨迹和多项式轨迹，并从中筛选出了符合要求的组合轨迹。求解多项式轨迹时，约束条件作为代价函数可以有效降低问题的复杂性，加快求解速度，同时避免求解失败，但也会导致规划出的多条多项式轨迹都穿越了障碍区域，然而在之后的组合轨迹选择过程中，违反约束的轨迹会因为代价较高被排除，从而增强算法的可靠性。

图9 单次规划轨迹Fig.9 Planned track of a single step

图10展示了无人机在飞行过程中单次轨迹规划时，邻机轨迹信息对规划结果的影响。无人机在未收到邻机轨迹信息的情况下，首先选择了一条曲率较小的组合路径。无人机在收到周围2架无人机的多项式轨迹系数后，为2架邻机各生成了100个点的轨迹曲线，并将邻居轨迹点加入本机的多项式轨迹代价函数重新评估各轨迹代价。由于原轨迹与邻机轨迹过于接近，算法重新选择了一条更为安全但弯曲的轨迹。若按照传统按点传输的轨迹信息交互方法，在一个规划周期内各智能体互相传输100个点的期望轨迹会对通信系统提出非常高的要求，而采用多项式系数进行期望轨迹传输可以在不增加通信负担的情况下，以任意尺度完整地生成邻居期望轨迹并进行评估。

图10 邻居轨迹信息对单步规划的影响Fig.10 Impact of neighbor track on single step planning

4 结论

本文针对固定翼无人机编队飞行控制过程中形成的多目标优化问题特点，提出了基于分布式模型预测控制算法框架的一体化解决方案，该方法将队形约束、安全距离、速度以及威胁区域等约束转化为代价函数与相容性约束，并采用优化求解器同时优化，从而有效减少了编队控制算法直接的耦合和控制器的复杂度。在算法应用时，本文充分结合模型预测轨迹规划与多项式轨迹规划的特点，利用采样等方法，将多项式轨迹规划的结果作为短距离模型预测控制的启发项，同时与邻机以多项式系数的形式交互轨迹信息，并将邻机轨迹信息融入多项式轨迹规划求解时的代价函数，在增大了模型预测控制预测范围的同时，降低了规划控制过程中的计算量与通信量，并通过仿真证明了方法的可行性。

在分布式模型预测控制框架下，未来的研究还可以对代价函数的结构和参数设计进行优化，以实现大规模集群多约束状态下的快速有效收敛；同时，采用机器视觉、激光雷达主动探测并结合机器学习的方法对邻居轨迹进行更精确的估计，从而实现完全无通信的集群编队控制。