CN103995539A - 一种无人机自主编队评价指标与mpc编队控制方法 - Google Patents

Abstract

本发明公开了一种无人机自主编队评价指标与模型预测控制(MPC)编队控制方法，属于飞行控制技术领域。本发明提供了关于无人机自主编队的相关定义、无人机自主编队避碰概率的计算方法和无人机自主编队评价指标，提出了适用于无人机MPC编队控制方法。本发明解决了在已知无人机自主编队的规模为M，编队中的无人机之间的平均期望间距为d_r，要求整个编队的碰撞概率小于P_c情况下，如何给出自主编队飞行控制***设计应遵循的避碰量化指标的问题。本发明方法为无人机自主编队控制***的工程设计提供了量化的指标依据，解决了无人机密集编队的避碰问题。

Description

一种无人机自主编队评价指标与MPC编队控制方法

技术领域

本发明属于飞行控制技术领域，涉及一种无人机自主编队评价指标的建立和MPC(ModelPredictive Control，模型预测控制)编队控制器的设计方法。

背景技术

在多架无人机进行密集的自主编队飞行的情况下，无人机与无人机之间的距离接近最小安全距离，并且对于编队中的某一特定无人机而言，因为其周围分布有其它无人机，缺乏***空间，这使得该无人机的避碰机动(例如规避邻近无人机由于各种随机干扰或某些突发情况或故障产生的非期望运动)比疏松编队情况下更加困难和复杂。如何判断自主编队的密集程度，以及如何设计和评价编队控制器的性能是无人机自主编队控制***设计需要重点考虑的问题之一。

发明内容

本发明为了解决现有技术中存在的问题，提供了一种无人机自主编队评价指标，并同时提供一种MPC编队控制方法。

本发明提供了一种无人机自主编队评价指标与MPC编队控制方法，其中，提供的无人机自主编队评价指标是：

$\mathrm{\σ}\left(t\right)=\sqrt{E[\mathrm{\Δ}{d}_f\left(t\right){]}^2-\{E\left[\mathrm{\Δ}{d}_f\left(t\right)\right]{\}}^2}=\sqrt{E[\mathrm{\Δ}{d}_f\left(t\right){]}^2-[\mathrm{\Δ}{d}_{\mathrm{Ef}}\left(t\right){]}^2}$

其中，Δd_f(t)表示t时刻编队的平均连接安全距离余量；σ(t)为t时刻无人机自主编队评价指标，是Δd_f(t)的均方差。

所述的MPC编队控制方法包括如下步骤：

第一步，建立无人机编队运动模型；

第二步，实现MPC编队控制，包括：预测模型、滚动优化和反馈校正。

所述第一步建立无人机编队运动模型，具体是：

设两个无人机W和L，速度分别为V_W和V_L，航迹偏角分别为和无人机W到无人机L的距离为d，x₀,y₀为距离d在无人机W航迹坐标系x,y轴上的分量；和V_Wc分别为无人机W的航迹偏角指令和速度指令，和V_Lc分别为无人机L的航迹偏角指令和速度指令，分别为无人机L和无人机W航迹偏角响应的时间常数，分别为无人机L和无人机W速度响应的时间常数，无人机L与无人机W的运动模型如下：

所述的第二步中的预测模型，具体是：

利用第一步的无人机编队运动模型作为预测模型，取状态可控输入为把节点L的航迹偏角和速度看作可测量干扰输出其中ΔV＝V_L–V_W，忽略D时，无人机编队运动模型的离散形式为：

$\left\{\begin{array}{c}X(k+1)=\mathrm{AX}\left(k\right)+\mathrm{Bu}\left(k\right)\\ Y\left(k\right)=\mathrm{CX}\left(k\right)\end{array}\right.$

其中，X(k)表示时刻k的状态，u(k)表示时刻k的可控输入，Y(k)表示时刻k的输出，A、B和C表示系数矩阵；设u(k+i|k)表示在时刻k假定的未来k+i时刻的可控输入值u；X(k+i|k)，Y(k+i|k)分别表示在时刻k，利用u(k+j|k),(j＝0,1,...,i-1)对X，Y作出的预测值；获得的状态X的预测方程为：

其中，P为预测时域，N为控制时域，u(k-1)表示k-1时刻的可控输入，差值Δu(k+j|k)＝u(k+j|k)-u(k+j-1|k)；

输出Y的预测方程为：

Y(k+j|k)＝CX(k+j|k),j＝1,2,...,P

所述的第二步中的滚动优化，具体是：

设编队飞行在时刻k的代价函数J为：

$J=\underset{l=1}{\overset{P}{\mathrm{\Σ}}}\left(\underset{m=1}{\overset{N}{\mathrm{\Σ}}}\right||{\mathrm{\ω}}_y(l,m)\·(Y_m(k+l|k)-{\mathrm{ref}}_m(k+l))|{|}^2+\underset{m=1}{\overset{N}{\mathrm{\Σ}}}\left|\right|{\mathrm{\ω}}_{\mathrm{\Δu}}(l,m)\·\mathrm{\Δ}{u}_m(k+l|k)\left|{|}^2\right)$

其中，ω_y为输出惩罚权重矩阵，ω_y(l,m)表示时域对(l,m)对应的输出惩罚权重；Y_m(k+l|k)为在控制时间m下、k时刻计算的k+l时刻的预测值；ref为间距保持指令，ref_m(k+l)为控制时间m下、k时刻计算的k+l时刻的间距保持指令；ω_Δu为输入变化率惩罚权重矩阵，ω_Δu(l,m)表示时域对(l,m)对应的输入变化率惩罚权重；Δu_m(k+l|k)为在控制时间m下的k时刻计算的k+l时刻输入变化率；令每个时域对(l,m)的权重相等，则ω_y和ω_Δu分别退化为一个行向量；

在时刻k的控制量u(k)为下面的约束优化问题：

Δu_opt＝arg minJ

其中，和分别为航迹偏角和速度指令限制；Δu_opt为最优序列的第一个控制量，u₁,u₂分别为和V_c；I_2×2表示2×2的单位矩阵，O_(N-1)×(N-1)表示(N-1)×(N-1)的0矩阵；

所述的第二步中的反馈校正，包括：(1)直接将X(k)作为***的初始状态；(2)把无人机L的航迹偏角和速度看作可测量干扰通过网络传递给无人机W并附加到预测模型的预测值。

本发明的优点和积极效果在于：

(1)本发明中提出的自主编队评价指标用于评价编队的疏密，并且本发明提出来了自主编队飞行控制***设计应遵循的避碰量化指标；

(2)本发明中提出的MPC编队控制方法解决了无人机密集编队的避碰问题；

(3)本发明所提出的这种针对自主编队控制***的避碰设计方法，为无人机自主编队控制***设计的工程实际应用提供了量化的指标参考。

附图说明

图1是本发明中无人机成员的安全距离示意图；

图2是本发明中无人机i与近邻无人机j的可能位置关系图；

图3是编队节点间的相对运动关系；

图4是模型预测控制框图；

图5是模型预测控制原理；

图6是本发明中PID编队控制器的仿真结果，a和b分别为无人机相对距离d在某无人机航迹坐标系x,y轴上的分量，c为无人机相对距离d的示意图；

图7是采用本发明MPC编队控制方法的仿真结果图，A和B分别为无人机相对距离d在某无人机航迹坐标系x,y轴上的分量，C为无人机相对距离d的示意图。

具体实施方式

下面结合附图和实施例对本发明进行详细说明。

本发明首先建立了无人机自主编队评价指标。

密集编队飞行的无人机之间的距离过近时发生碰撞的概率较高，通常无人机都会根据自身的实际情况确定一个安全距离d_s。d_s是这样一种距离，当某无人机和另一无人机或威胁的距离小于该距离的时候，该无人机必须采取相应措施。当两无人机间的距离远大于d_s时，无人机间的碰撞机会较小，这时编队可以认为是松散的，即对编队距离保持控制器的要求不高；当无人机间的距离接近d_s时，无人机随时需要规避可能的碰撞威胁，这时对编队距离保持控制器的要求较高，编队可以认为是紧密的。

本发明将解决如下问题：在要求自主编队具有的规模为M，编队中的无人机之间的平均期望间距为d_r，整个编队的碰撞概率小于P_c情况下，如何给出自主编队飞行控制***设计应遵循的避碰量化指标。

利用地形地貌掩护，在狭长的安全通道中以扁平的密集编队形式进行低空突防飞行，以高密集度突袭式的饱和攻击方式是无人机编队的主要作战样式，此时在这种扁平的密集编队中无人机之间、无人机与障碍或威胁之间发生碰撞的概率增大，在追求高突防概率原则下，对于编队协同制导控制器性能提出了很高的要求。扁平的密集编队形式相当于整个编队在垂直方向几乎没有避碰机动的自由空间，整个编队只能在水平方向进行避碰机动，这相当于在纵向是无限密集的、没有避碰机动的自由空间，是不能通过调整飞行高度的方法进行避碰机动的，因此在二维水平面内密集编队的避碰问题是最为复杂的基础性问题。对二维水平面内密集编队问题的解决方法和结论是可以方便地扩展到三维空间的编队问题。

无人机编队的疏密程度(简称疏密度)是一个描述编队中无人机成员之间距离大小特征的基本概念，它对于无人机自主编队协同制导控制***设计具有重要影响，下面将对无人机自主编队的疏密度进行分析。

为了描述无人机自主编队的疏密度，首先给出编队中无人机成员的安全距离的概念和定义。通常在一些适合于无人机编队进行饱和攻击任务中，需要利用地形地貌掩护，以编队形式低空突防飞行在狭窄的安全通道中，编队飞行的地理、气象环境复杂多变。如果编队中无人机之间、无人机与障碍之间的距离过近，那么发生碰撞的概率就会增大，从而威胁整个无人机编队的飞行安全，降低无人机编队的作战效能。

如何为编队飞行中的无人机之间确定合适的安全距离，是编队协同制导控制***设计的基本环节之一。通常根据无人机各自的实际情况确定一个安全距离d_si(t)，并利用它来控制编队中无人机成员之间的距离(简称机间距)d_ij(t)。这里，d_ij(t)＝d_ji(t)，且i≠j，表示无人机i和无人机j之间的距离，d_si(t)表示无人机i的安全距离。d_ij(t)的数目其中i,j＝1,2,…,M(t)，是编队中无人机成员的编号，第i个无人机成员也就是无人机i，M(t)是当前t时刻无人机总数。

定义1：无人机成员的安全距离d_si(t)是这样一种距离，当第i架无人机与编队中其他任何无人机j之间的机间距d_ij(t)小于它的时候，即d_ij(t)＜d_si(t)，则第i架无人机必须采取相应的避碰措施；当d_ij(t)＝d_si(t)时，第i架无人机则处于采取相应避碰措施的准备状态。

定义2：无人机成员的安全距离余量Δd_ij(t)，是指编队中无人机成员的机间距d_ij(t)与其安全距离d_si(t)的差。即Δd_ij(t)＝d_ij(t)-d_si(t)。

如图1所示，影响编队中无人机成员的安全距离d_si(t)的因素主要有三个：传感器测量误差分量、机动性能分量和网络诱导分量。无人机成员的传感器测量误差分量主要是由无人机自身的传感器技术状态决定的，如果两架无人机之间的距离小于传感器测量误差分量，则认为它们发生了碰撞。机动性能分量反映了无人机成员具有规避突发碰撞威胁的能力。在单架无人机单独飞行时，一般当安全距离小于传感器测量误差分量与机动性能分量之和时，无人机就要采取避碰措施的。网络诱导分量是由于编队支撑网络对信息传递的时延、丢包等因素对编队中无人机成员的安全距离的新增限制。

除了上述采用机间距d_ij(t)与安全距离d_si(t)大小关系的安全距离余量Δd_ij(t)来描述编队的疏密度之外，还应考察编队中无人机成员周围的自由空间情况，即考察编队的密集程度(简称密集度)。因为当第i架无人机成员与其他无人机之间的机间距d_ij(t)小于其安全距离d_si(t)，即安全距离余量Δd_ij(t)＜0时，该架无人机对于面临的碰撞威胁是需要进行避碰机动的，而这种规避机动常常是朝向其周围的自由空间的。此时如果第i架无人机周围又没有自由空间时，那么它该如何进行碰撞威胁规避机动呢？由此可见，除了需要使用安全距离d_si(t)概念来衡量编队的疏密度外，还需要引入一个描述编队中无人机成员周围的自由空间的变量，称为连接度的变量n_ci(t)，来反映编队到底可能有多“密”，即编队的密集度如何？

连接度n_ci(t)是指二维平面上距离无人机i接近d_si(t)的相邻无人机的个数。经分析发现，在二维平面内，最密集编队的第i架无人机成员周围距离接近d_si(t)的相邻成员数的平均值约为6到7左右，即编队中最密集处的连接度n_ci(t)约为6到7左右。为了描述整个编队的平均密集度水平，引入平均连接度因此，根据编队的平均连接度n_c(t)，可以来近似地判断编队的密集程度。

无人机成员间的避碰问题是高密集度的编队飞行需要重点解决的基本问题之一，它直接关系到无人机编队的自主性和协同性水平及其整体的作战效能。下面给出一个关于无人机成员和编队采取避碰机动的概率与疏密度和密集度之间的关系式。

编队中无人机成员的安全距离余量Δd_ij(t)＝d_ij(t)-d_si(t)为各态历经的随机过程，下面将与时间相关的变量中将时间省去进行简写，例如Δd_ij(t)简写为Δd_ij，其它也作此简写。根据中心极限定理，假设在各种互不显著的随机因素的综合作用下，无人机成员的安全距离余量Δd_ij近似服从以编队为其设定的安全距离余量Δd_rij为数学期望，以σ_ij ²为方差的正态分布，即当编队中无人机成员的机间距不大于其安全距离，即Δd_ij≤0时，无人机成员就需要或准备采取避碰机动措施，如图2所示。因此，无人机i与无人机j之间需要或准备采取避碰机动的概率p_b(i,j)为：

p_b(i,j)＝P{Δdi_j≤0}

$\begin{array}{c}={\∫}_{-\∞}^{\frac{-\mathrm{\Δ}{d}_{\mathrm{rij}}}{{\mathrm{\σ}}_{\mathrm{ij}}}}\frac{1}{\sqrt{2\mathrm{\π}}}\·e^{-\frac{{\mathrm{\τ}}^2}{2}\mathrm{d\τ}}\\ =\mathrm{\Φ}\left(\frac{-\mathrm{\Δ}{d}_{\mathrm{rij}}}{{\mathrm{\σ}}_{\mathrm{ij}}}\right)\\ =\mathrm{\Φ}(-{\mathrm{\λ}}_{\mathrm{rij}})\end{array}---\left(1\right)$

无人机i与无人机j之间无需采取避碰机动的概率q_b(i,j)为：

q_b(i,j)＝P{Δd_ij＞0}

$\begin{array}{c}=1-{\∫}_{-\∞}^{\frac{-\mathrm{\Δ}{d}_{\mathrm{rij}}}{{\mathrm{\σ}}_{\mathrm{ij}}}}\frac{1}{\sqrt{2\mathrm{\π}}}\·e^{-\frac{{\mathrm{\τ}}^2}{2}\mathrm{d\τ}}\\ =1-\mathrm{\Φ}\left(\frac{-\mathrm{\Δ}{d}_{\mathrm{rij}}}{{\mathrm{\σ}}_{\mathrm{ij}}}\right)\\ =1-\mathrm{\Φ}(-{\mathrm{\λ}}_{\mathrm{rij}})\end{array}---\left(2\right)$

式中，是安全距离余量比，为正态概率积分。

上式是仅仅考虑了疏密度因素的避碰机动概率公式，因为在无人机i的任何方向上均有可能发生碰撞危险，所以还应将密集度对避碰机动概率的影响反映出来，那么无人机i自己的避碰机动的概率p_b(i)为：

$\begin{array}{c}{p}_b\left(i\right)\underset{j=1}{\overset{n_{\mathrm{ci}}}{\mathrm{\Σ}}}{p}_b(i,j)[q_b(i,j){]}^{n_{\mathrm{ci}}-1}+\underset{j=1}{\overset{C_{n_{\mathrm{ci}}}^2}{\mathrm{\Σ}}}[p_b(i,j){]}^2[q_b(i,j){]}^{n_{\mathrm{ci}}-2}+\·\·\·\\ +\underset{j=1}{\overset{C_{n_{\mathrm{ci}}}^k}{\mathrm{\Σ}}}\left[p_b(i,j){]}^k\right[q_b(i,j){]}^{n_{\mathrm{ci}}-k}+\·\·\·+\underset{j=1}{\overset{n_{\mathrm{ci}}}{\mathrm{\Π}}}{p}_b(i,j)\\ =\underset{k=1}{\overset{n_{\mathrm{ci}}}{\mathrm{\Σ}}}\underset{j=1}{\overset{C_{n_{\mathrm{ci}}}^k}{\mathrm{\Σ}}}\left[p_b(i,j){]}^k\right[q_b(i,j){]}^{n_{\mathrm{ci}}-k}\\ =\underset{k=1}{\overset{n_{\mathrm{ci}}}{\mathrm{\Σ}}}\underset{j=1}{\overset{C_{n_{\mathrm{ci}}}^k}{\mathrm{\Σ}}}\left[\mathrm{\Φ}(-{\mathrm{\λ}}_{\mathrm{rij}}){]}^k\right[1-\mathrm{\Φ}(-{\mathrm{\λ}}_{\mathrm{rij}}){]}^{n_{\mathrm{ci}}-k},(n_{\mathrm{ci}}\≥2)\end{array}---\left(3\right)$

其中，n_ci为无人机i周围的近邻数，即连接度，为组合数。对于连接度n_ci＝1时，避碰机动的概率p_b(i)则采用式(1)来计算。

特别地，在编队设定的安全距离余量比λ_rij＝0，即将无人机成员i的安全距离作为设定的期望机间距情况下，当连接度n_ci＝2时，利用式(3)可以计算出无人机成员i的避碰机动概率p_b(i)＝0.75；但当连接度n_ci＝6时，p_b(i)＝0.984375。也就是说，如果设定的余量比λ_rij＝0，那么对于具有较高的疏密度和密集度的编队而言，无人机成员i几乎一定要采取避碰机动措施的。

整个编队采取避碰机动的概率p_b为：

$\begin{array}{c}{p}_b=\underset{i=1}{\overset{M}{\mathrm{\Σ}}}{p}_b\left(i\right)[1-p_b\left(i\right){]}^{M-1}+\underset{i=1}{\overset{C_M^2}{\mathrm{\Σ}}}[p_b\left(i\right){]}^2[1-p_b\left(i\right){]}^{M-2}+\·\·\·\\ +\underset{i=1}{\overset{C_M^k}{\mathrm{\Σ}}}\left[p_b\left(i\right){]}^k\right[1-p_b\left(i\right){]}^{M-k}+\·\·\·\underset{i=1}{\overset{M}{\mathrm{\Π}}}{p}_b\left(i\right)\\ =\underset{k=1}{\overset{M}{\mathrm{\Σ}}}\underset{i=1}{\overset{C_M^k}{\mathrm{\Σ}}}\left[p_b\left(i\right){]}^k\right[1-p_b\left(i\right){]}^{M-k}\\ =\underset{k=1}{\overset{M}{\mathrm{\Σ}}}\underset{i=1}{\overset{C_M^k}{\mathrm{\Σ}}}\left[p_b\left(i\right){]}^k\right[q_b\left(i\right){]}^{M-k}\end{array}---\left(4\right)$

式中，q_b(i)＝1-p_b(i)为无人机成员i无需采取避碰机动的概率，M为整个编队的节点数，k为整个编队中需要或准备采取避碰机动措施的节点数量。

由式(1)-式(4)可见，在安全距离的余量比λ_rij给定情况下，整个编队采取避碰机动的概率p_b是随着连接度n_ci与编队规模M的增大而增大的。

特别地，当p_b(i,j)＝α(0≤α≤1)为常值时，式(3)和式(4)分别变为：

$\begin{array}{c}{p}_b\left(i\right)=p=n_{\mathrm{ci}}\·\mathrm{\α}{\mathrm{\β}}^{n_{\mathrm{ci}}-1}+C_{n_{\mathrm{ci}}}^2{\mathrm{\α}}^2\·{\mathrm{\β}}^{n_{\mathrm{ci}}-2}+\·\·\·+C_{n_{\mathrm{ci}}}^k\·{\mathrm{\α}}^k{\mathrm{\β}}^{n_{\mathrm{ci}}-k}+\·\·\·+{\mathrm{\α}}^{n_{\mathrm{ci}}}\\ =\underset{k=1}{\overset{n_{\mathrm{ci}}}{\mathrm{\Σ}}}{C}_{n_{\mathrm{ci}}}^k\·{\mathrm{\α}}^k{\mathrm{\β}}^{n_{\mathrm{vi}}-k}\\ =1-{\mathrm{\β}}^{n_{\mathrm{ci}}}\end{array}---\left(5\right)$

$\begin{array}{c}{P}_b=M\·p{q}^{M-1}+C_M^2\·p^2{q}^{M-2}+\·\·\·+C_M^k\·p^k{q}^{M-k}+\·\·\·+p^M\\ =\underset{k=1}{\overset{M}{\mathrm{\Σ}}}{C}_M^k\·p^k{q}^{M-k}\\ =1-q^M\end{array}---\left(6\right)$

其中，β＝1-α，q＝1-p。

如果将无人机成员的安全距离d_si设定为其传感器测量误差分量，令设定的安全距离余量比λ_rij＝0，那么式(1)-(6)所计算的就是相应的无人机成员发生互相碰撞的概率以及编队发生碰撞的概率。

下面就利用对编队动态稳定特性有关键影响作用的避碰机动概率的概念，从安全距离余量和周围自由空间两方面对无人机自主编队进行分类定义。按照疏密度和密集度概念，可将无人机自主编队分为三种类型：疏松编队、紧密编队和密集编队。

定义3：疏松编队是指所有无人机成员的安全距离余量比均不小于3或者均介于2和3之间且平均连接度不大于3的无人机编队。即λ_rij≥3或2≤λ_rij＜3&n_c≤3，也就是说，所有无人机成员的安全距离余量均不小于其3倍的均方差或者均介于2倍和3倍的均方差之间且平均连接度不大于3，即Δd_rij≥3σ_ij或2σ_ij≤Δd_rij＜3σ_ij&n_c≤3。当无人机编队采用疏松编队飞行时，编队中无人机成员需要采取避碰机动的概率较低，无人机成员之间几乎不存在发生碰撞的机会，编队的疏密度和密集度均处于很低的状态，此情况下对于无人机编队的协同制导控制器的性能要求不高。

定义4：紧密编队是指所有无人机成员的安全距离余量比均介于2和3之间且平均连接度介于3和6之间的无人机编队。即2≤λ_rij＜3且3＜n_c＜6。也就是说，所有无人机成员的安全距离余量均介于其2倍和3倍的均方差之间且平均连接度介于3和6之间，即2σ_ij≤Δd_rij＜3σ_ij且3＜n_c＜6。从编队的疏密度来看，与疏松编队不同的是，当采用紧密编队飞行时，编队中无人机成员需要采取避碰机动的概率较大，无人机成员之间存在发生碰撞的机率较高；而从编队的密集度来看，由于编队的平均连接度小于6，这些无人机成员向周围规避机动所需要的自由空间是存在的，这些自由空间对于无人机成员向周围规避机动是适中的。与疏松编队情况相比，这时对无人机编队的协同制导控制器的性能要求较高。

定义5：密集编队是指所有无人机成员的安全距离余量比均小于2或者均介于2和3之间且平均连接度不小于6的无人机编队。即λ_rij＜2或2≤λ_rij＜3&n_c≥6。也就是说，所有无人机成员的安全距离余量均小于其两倍的均方差或者平均连接度不小于6，即Δd_rij＜2σ_ij或2σ_ij≤Δd_rij＜3σ_ij&n_c≥6。从编队的疏密度来看，密集编队与紧密编队没有太大区别，只是使得机间距d_ij更进一步地接近于其安全距离d_si，使得无人机成员需要采取避碰机动的概率比紧密编队要进一步增大。同时，由于编队的密集度提高，即编队的平均连接度不小于6，致使无人机成员几乎没有向周围规避机动所需要的自由空间，无人机成员之间存在发生碰撞的机率很高。因此，与紧密编队情况相比，密集编队飞行对无人机编队的协同制导控制器的性能要求更高。

定义6：编队的平均安全距离d_sf(t)，是指编队中所有无人机成员安全距离d_si(t)的平均值。即 $d_{\mathrm{sf}}\left(t\right)=\frac{1}{M\left(t\right)}\underset{i=1}{\overset{M\left(t\right)}{\mathrm{\Σ}}}{d}_{\mathrm{si}}\left(t\right).$

定义7：编队的平均连接机间距d_f(t)，是指编队中所有无人机成员与其邻近无人机的机间距d_ij(t)的平均值。即 $d_f\left(t\right)=\frac{1}{M\left(t\right)}\underset{i=1}{\overset{M\left(t\right)}{\mathrm{\Σ}}}\left\{\frac{1}{n_{\mathrm{ci}}\left(t\right)}\underset{j=1,i\≠j}{\overset{n_{\mathrm{ci}}\left(t\right)}{\mathrm{\Σ}}}{d}_{\mathrm{ij}}\left(t\right)\right\}.$

定义8：编队的平均连接安全距离余量Δd_f(t)，是指编队的平均连接机间距d_f(t)与平均安全距离d_sf(t)的差。即：

Δd_f(t)=d_f(t)-d_sf(t)

$\begin{array}{c}=\frac{1}{M\left(t\right)}\underset{i=1}{\overset{M\left(t\right)}{\mathrm{\Σ}}}\left\{\frac{1}{n_{\mathrm{ci}}\left(t\right)}\underset{j=1,i\≠j}{\overset{n_{\mathrm{ci}}\left(t\right)}{\mathrm{\Σ}}}{d}_{\mathrm{ij}}\left(t\right)\right\}-\frac{1}{M\left(t\right)}\underset{i=1}{\overset{M\left(t\right)}{\mathrm{\Σ}}}{d}_{\mathrm{si}}\left(t\right)\\ =\frac{1}{M\left(t\right)}\underset{i=1}{\overset{M\left(t\right)}{\mathrm{\Σ}}}\left\{\right[\frac{1}{n_{\mathrm{ci}}\left(t\right)}\underset{j=1,i\≠1}{\overset{n_{\mathrm{ci}}\left(t\right)}{\mathrm{\Σ}}}{d}_{\mathrm{ij}}\left(t\right)]-d_{\mathrm{si}}\left(t\right)\}\end{array}$

式(1)-(6)还可作为编队协同制导控制器性能的评价公式。对于具有同一种基本队形的编队，当无人机的安全距离d_si和编队的队形设定距离d_rij均相同，即安全距离的余量Δd_ij一定时，可用平均机间距的均方差的大小作为评价编队协同制导控制器性能指标，即指标σ：

$\mathrm{\σ}\left(t\right)=\sqrt{E[\mathrm{\Δ}{d}_f\left(t\right){]}^2-\{E\left[\mathrm{\Δ}{d}_f\left(t\right)\right]{\}}^2}=\sqrt{E[\mathrm{\Δ}{d}_f\left(t\right){]}^2-[\mathrm{\Δ}{d}_{\mathrm{Ef}}\left(t\right){]}^2}---\left(7\right)$

其中，Ex表示求取x的平均值，σ的值越小表示编队性能越好。

本发明提供的无人机的MPC编队控制方法，包括如下步骤：

第一步，建立无人机编队运动模型。

现阶段的飞行编队控制***，都是基于长机-僚机模式，即采用僚机跟随长机的编队模式，长机可以是真实长机也可以是虚拟长机。如图3，每个节点表示一个无人机，V_W和V_L分别为节点W和节点L的速度，和分别为节点W和节点L的航迹偏角，d为节点W到节点L的距离，节点W和节点L的航迹偏角的差值x和y为d在节点W速度方向上的正交分解。以节点W(图3中编号为N_W)的航迹坐标系(节点W的速度方向为x轴，与速度垂直向右的方向为y轴)为相对坐标系，地面坐标系为固定坐标系，利用理论力学中的绝对速度＝相对速度+牵连速度关系，建立节点L和节点W的运动学方程如下：

其中，分别表示节点L、节点W的速度向量，表示节点W到节点L的距离向量。

上式在节点W的航迹坐标系中分解为：

其中x,y为节点W相对节点L的距离d在节点W航迹坐标系x,y轴上的分量。采用小角度和小扰动假设对上式(9)线性化得：

x₀,y₀为某一扰动下距离d在无人机W航迹坐标系x,y轴上的分量。

节点L与节点W的运动模型：

其中，和V_Wc分别为节点W的航迹偏角指令和速度指令，和V_Lc分别为节点L的航迹偏角指令和速度指令，和V_Lc、V_Wc为外环的航迹偏角和速度控制量，作为指令作用于内环飞行控制***，分别为节点L和节点W航迹偏角响应的时间常数，分别为节点L和节点W速度响应的时间常数。

第二步，实现MPC编队控制，设计基于MPC编队控制器。

对于编队飞行控制，由式(11)可以看出，双无人机之间的运动机理清晰，模型简洁，因此很适合应用模型预测控制，而且随着电子计算机软硬件技术的迅猛发展，计算和存储成本越来越低，MPC方法越来越广泛地应用到飞行控制***这样的快速***中，因此本发明将根据编队飞行的特点设计基于状态空间MPC的编队控制器。

基于模型的MPC控制方法通常有三个主要部分：预测模型、滚动优化和反馈校正，如图4所示。当在网络诱导时延情况下，预测模型如步骤2.4所示获得。

步骤2.1：预测模型。

预测模型只注重模型的功能，而不注重模型的形式，只要具有预测功能的模型，如状态方程、传递函数这类传统模型，又如阶跃响应、脉冲响应这类非参数模型等均可作为预测模型使用。在本发明的编队控制中利用式(11)作为预测模型。

根据式(11)选取状态其中x，y为节点L与节点W的相对距离在节点W航迹坐标系中的投影；为节点W的航迹偏角；V_W为节点W的速度；可控输入为 其中和V_Wc分别为节点W的航迹偏角指令和速度指令。为减轻计算负担，这里把节点L的航迹偏角和速度看作可测量干扰即没有使用一阶模型对其进行预测，通过支撑网络直接传递节点L的航迹偏角和速度；输出其中 忽略D时(D随后会加在预测模型中，视为模型的校正)，编队运动模型的离散形式为：

$\left.\begin{array}{c}X(k+1)=\mathrm{AX}\left(k\right)+\mathrm{Bu}\left(k\right)\\ Y\left(k\right)=\mathrm{CX}\left(k\right)\end{array}\right\}---\left(12\right)$

A、B和C表示系数矩阵，此处时刻k也就是第k个采样周期，设X(k|k)＝X(k)表示在时刻k，根据Y(k)和u(k-1)获得的状态值，因为u(k)此时还没有计算出来；u(k+i|k)表示在时刻k假定的未来k+i时刻的输入值u；X(k+i|k)，Y(k+i|k)表示在时刻k，利用u(k+j|k),(j＝0,1,...,i-1)对X，Y作出的预测值；P为预测时域；N为控制时域，即u(k+i|k)＝u(k+N-1|k)N＜i＜P-1，通过迭代式(12)可得：

$\left.\begin{array}{c}X(k+1|k)=\mathrm{AX}\left(k\right)+\mathrm{Bu}\left(k\right|k)\\ X(k+2|k)=\mathrm{AX}(k+1|k)+\mathrm{Bu}(k+1|k)\\ =A^{2}X\left(k\right)+\mathrm{ABu}\left(k\right|k)+\mathrm{Bu}(k+1|k)\\ \·\\ \·\\ \·\\ X(k+P|k)=A^{P}X\left(k\right)+A^{P-1}\mathrm{Bu}\left(k\right|k)+...+\mathrm{Bu}(k+P-1|k)\end{array}\right\}---\left(13\right)$

即：

$X(k+j|k)=A^{j}X\left(k\right)+\left[\begin{array}{cccc}{A}^{j-1}& A^{j-2}& ...& I\end{array}\right]B\left[\begin{array}{c}u\left(k\right|k)\\ \·\\ \·\\ \·\\ u(k+j-1|k)\end{array}\right]j=\mathrm{1,2},...,P---\left(14\right)$

由于u(k)在k时刻是未知的，而u(k-1)是已知的，设差值

Δu(k+j|k)＝u(k+j|k)-u(k+j-1|k)   (15)

有：

$\left.\begin{array}{c}u\left(k\right|k)=\mathrm{\Δu}\left(k\right|k)+u(k-1)\\ u(k+1|k)=\mathrm{\Δu}(k+1|k)+\mathrm{\Δu}\left(k\right|k)+u(k-1)\\ \·\\ \·\\ \·\\ u(k+N-1|k)=\mathrm{\Δu}(k+N-1|k)+\·\·\·\mathrm{\Δu}\left(k\right|k)+u(k-1)\end{array}\right\}---\left(16\right)$

则对于1≤j≤N有：

$\left.\begin{array}{c}X(k+1|k)=\mathrm{AX}\left(k\right)+B[\mathrm{\Δu}\left(k\right|k)+u(k-1)]\\ X(k+2|k)=A^{2}X\left(k\right)+\mathrm{AB}[\mathrm{\Δu}\left(k\right|k)+u(k-1|k)]\\ +B[\mathrm{\Δu}(k+1|k)+\mathrm{\Δu}\left(k\right|k)+u(k-1)]\\ =A^{2}X\left(k\right)+(A+I)\mathrm{B\Δu}\left(k\right|k)+\mathrm{B\Δu}(k+1|k)\\ +(A+I)\mathrm{Bu}(k-1)\\ \·\\ \·\\ \·\\ X(k+N|k)=A^{N}X\left(k\right)+(A^{N-1}+\·\·\·+A+I)\mathrm{Bu\Δ}\left(k\right|k)+...\\ +\mathrm{B\Δu}(k+N-1|k)+(A^{N-1}+\·\·\·+A+I)\mathrm{Bu}(k-1)\end{array}\right\}---\left(17\right)$

式中I是单位矩阵，进一步得到：

$X(k+j|k)=A^{j}X\left(k\right)+\left[\begin{array}{ccc}{\mathrm{\Σ}}_{i=0}^{j-1}{A}^{i}B& \·\·\·& B\end{array}\right]\left[\begin{array}{c}\mathrm{\Δu}\left(k\right|k)\\ \·\\ \·\\ \·\\ \mathrm{\Δu}(k+j-1|k)\end{array}\right]+{\mathrm{\Σ}}_{i=0}^{j-1}{A}^i\mathrm{Bu}(k-1)---\left(18\right)$

对于N≤j≤P有：

$\left.\begin{array}{c}X(k+N1|k)=A^{N+1}X\left(k\right)+(A^N+\·\·\·+A+I)\mathrm{Bu\Δ}\left(k\right|k)+...\\ +(A+I)\mathrm{B\Δu}(k+N-1|k)\\ +(A^N+\·\·\·+A+I)\mathrm{Bu}(k-1)\\ \·\\ \·\\ \·\\ X(k+P|k)=A^{P}X\left(k\right)+(A^{P-1}+\·\·\·A+I)\mathrm{Bu\Δu}\left(k\right|k)+...\\ +(A^{P-N}+\·\·\·+A+I)\mathrm{B\Δu}(k+N-1|k)\\ +(A^{P-1}+\·\·\·+A+I)\mathrm{Bu}(k+1)\end{array}\right\}---\left(19\right)$

即：

$\begin{array}{c}X(k+j|k)=A^{j}X\left(k\right)+\left[\begin{array}{ccc}{\mathrm{\Σ}}_{i=0}^{j-1}{A}^{i}B& \·\·\·& {\mathrm{\Σ}}_{i=0}^{j-N}\end{array},A^{i}B\right]\left[\begin{array}{c}\mathrm{\Δu}\left(k\right|k)\\ \·\\ \·\\ \·\\ \mathrm{\Δu}(k+\mathrm{aN}-1|k)\end{array}\right]---\left(20\right)\\ +{\mathrm{\Σ}}_{i=0}^{j-1}{A}^i\mathrm{Bu}(k-1)\end{array}$

综上可得总的状态X的预测方程为：

最后，输出Y的预测方程为：

Y(k+j|k)＝CX(k+j|k),j＝1,2,...,P   (22)

步骤2.2：滚动优化。

滚动优化是MPC的最主要特征，与通常的最优控制算法(LQR)最大的差别主要表现在滚动优化的代价函数不是一成不变的全局函数，而是在每一采样时刻，代价函数只涉及从现在到未来的一个有限时段(预测时域和控制时域)，而到下一采样时刻，预测时域向前推移如图5所示。因此MPC在每一时刻有一个相对于该时刻的代价函数，不同时刻代价函数的相对形式是相同的，但其绝对形式(即所包含的时间区域，权重等)则是不同的，即优化不是一次离线完成，而是反复在线进行，这就是滚动优化的含义，也是MPC区别于传统最优控制的根本特点。MPC采用这种有限时段优化能顾及由于模型失配、时变、干扰等引起的不确定性，及时弥补这些因素造成的影响，并始终把新的优化建立在实际过程的基础上，对于导机编队控制，模型的平衡点，长机的干扰在不断的变化，因此，建立在有限时段上的滚动优化策略更加符合编队飞行的特点。

设编队飞行在时刻k的代价函数J为：

$J=\underset{l=1}{\overset{P}{\mathrm{\Σ}}}\left(\underset{m=1}{\overset{N}{\mathrm{\Σ}}}\right||{\mathrm{\ω}}_y(l,m)\·(Y_m(k+l|k)-{\mathrm{ref}}_m(k+l))|{|}^2+\underset{m=1}{\overset{N}{\mathrm{\Σ}}}\left|\right|{\mathrm{\ω}}_{\mathrm{\Δu}}(l,m)\·\mathrm{\Δ}{u}_m(k+l|k)\left|{|}^2\right)---\left(23\right)$

其中，P为预测时域；N为控制时域；Y_m(k+l|k)为在控制时间m下、k时刻计算的k+l时刻的预测值；ref为间距保持指令，ref_m(k+l)为控制时间m下、k时刻计算的k+l时刻的间距保持指令；ω_y为输出惩罚权重矩阵，反映了节点W与L间距的保持力度和航迹偏角、速度的匹配力度，是对距离保持最为重要的权重，ω_y(l,m)表示时域对(l,m)对应的输出惩罚权重；ω_Δu为输入变化率惩罚权重矩阵，反映了指令变化率的大小，具有积分的性质，使***实现无差控制，ω_Δu(l,m)表示时域对(l,m)对应的输入变化率惩罚权重；Δu_m(k+l|k)为在控制时间m下的k时刻计算的k+l时刻输入变化率。为简化二次规划(Quadratic programming,QP)的求解，这里令每个时域对(l,m)的权重相等，则两个权重矩阵分别退化为行向量 和ω_x,ω_y表示节点W与L间距的保持力度，ω_ΔV表示节点W的航迹偏角、速度的匹配力度，ω_ΔVc表示可控输入变化率的大小。可见在不加终端约束的情况下，权重的物理意义十分明确，但为了保证***的稳定，这里采取了较大的预测时域，经仿真设计取P＝50；N＝40；ω_y＝(200,250,200,50)；ω_Δu＝(5,0.1)。

最后，MPC在时刻k的控制量u(k)为下面的约束优化问题：

其中，为航迹偏角限制，分别表示最小、最大偏航角，为速度指令限制，V_min、V_max分别表示最小速度和最大速度。Δu_opt为最优序列的第一个控制量,u1,u₂分别为和V_c。I_2×2表示2×2的单位矩阵，O_(N-1)×(N-1)表示(N-1)×(N-1)的0矩阵。

形如式(24)的代价函数可以化为QP标准问题(二次规划问题)，在权重为正的时候是严格凸的，具体过程采用较成熟的KWIK算法求解，此处不在赘述。KWIK算法参考文件：Schmid,C.,L.T.Biegler.Quadratic programming methods for reduced hessian SQP[J].Computers&ChemicalEngineering,18(9):817832,1994。

步骤2.3：反馈校正。

实际***中存在的非线性、时变、模型失配、干扰等因素，基于不变模型的预测不可能和实际情况完全相符，因此，反馈策略是不可少的。滚动优化只有建立在反馈校正的基础上，才能体现出它的优越性。因此，MPC在通过优化确定了一系列未来的控制作用后，为了防止模型失配或环境干扰引起控制对理想状态的偏离，并不是把这些控制作用逐一全部实施，而只是应用当前时刻的控制作用，即求得的最优控制序列的第一个控制量，到下一采样时刻，首先监测对象的实际输出，并通过各种反馈策略修正预测模型或加以补偿，然后再进行新的优化。

综上所述，MPC综合利用历史信息和模型信息，对代价函数不断进行滚动优化，并根据实际测得的对象输出修正或补偿预测模型。

在每一个计算周期通过支撑网络获得***的当前状态X(k)，可能会与状态的预测值X(k|k-1)不符，但状态空间MPC方法直接将X(k)作为***的初始状态，这起到了类似反馈校正的作用；另外如前所述，把节点L的航迹偏角和速度看作可测量干扰通过网络传递给节点W并按式(11)附加到预测模型的预测值上，也起到了校正的作用，而典型的PID控制器没有对节点L的干扰输入做处理。

步骤2.4：存在网络诱导时延情况下的预测模型。

当网络诱导时延不可忽略时，线性运动模型的离散化需要考虑时延的影响，考察线性时不变连续***的一般形式：

$\left.\begin{array}{c}\frac{\mathrm{dX}\left(t\right)}{\mathrm{dt}}\mathrm{AX}\left(t\right)+\mathrm{Bu}\left(t\right)\\ Y=\mathrm{CX}\left(t\right)\end{array}\right\}---\left(25\right)$

其时域解为：

$X\left(t\right)=e^{A(t-t_0)}X\left(t_0\right)+{\∫}_{t_0}^t{e}^{A(t-\mathrm{\μ})}\mathrm{Bu}\left(\mathrm{\μ}\right)\mathrm{d\μ}---\left(26\right)$

t₀表示***的初始时间，当存在网络诱导时延τ时，因为MPC的输入u是离散的，设u的采样周期为T_s，当新的状态没有到达的时候u将等于上一时刻的输入，所以有

$\frac{\mathrm{dX}\left(t\right)}{\mathrm{dt}}=\begin{array}{cc}\mathrm{AX}\left(t\right)+\mathrm{Bu}\left((k-1)T_s\right),& {\mathrm{kT}}_s\&le;t\&le;{\mathrm{kT}}_s+\mathrm{\&tau;}\\ \mathrm{AX}\left(t\right)+\mathrm{Bu}\left(k{T}_s\right),& k{T}_s+\mathrm{\&tau;}\&le;t<(k+1)T_s\end{array}---\left(27\right)$

当kT_s≤t＜kT_s+τ时，考虑状态X从kT_s转移到kT_s+τ，有：

$\begin{array}{c}X(k{T}_s+\mathrm{\&tau;})=e^{\mathrm{A\&tau;}}X\left(k{T}_s\right)+{\&Integral;}_{k{T}_s}^{{\mathrm{kT}}_s+\mathrm{\&tau;}}{e}^{A(k{T}_s+\mathrm{\&tau;}-\mathrm{\&mu;})}\mathrm{Bu}\left((k-1)T_s\right)\mathrm{d\&mu;}\\ =e^{\mathrm{A\&tau;}}X\left(k{T}_s\right)+{\&Integral;}_0^{\mathrm{\&tau;}}{e}^{\mathrm{A\&mu;}}\mathrm{d\&mu;}\&CenterDot;\mathrm{Bu}\left((k-1)T_s\right)\end{array}---\left(28\right)$

当kT_s+τ≤t＜(k+1)T_s时，考虑状态X从kT_s+τ转移到(k+1)T_s，有：

$\begin{array}{c}X\left((k+1)T_s\right)=e^{A(T_s-\mathrm{\&tau;})}X({\mathrm{kT}}_s+\mathrm{\&tau;})+{\&Integral;}_{{\mathrm{kT}}_s+\mathrm{\&tau;}}^{(k+1)T_s}{e}^{A((k+1)T_s-\mathrm{\&mu;})}\mathrm{Bu}\left({\mathrm{kT}}_s\right)\mathrm{d\&mu;}\\ =e^{A(T_s-\mathrm{\&tau;})}[e^{\mathrm{A\&tau;}}X\left({\mathrm{kT}}_s\right)+{\&Integral;}_0^{\mathrm{\&tau;}}{e}^{\mathrm{A\&mu;}}\mathrm{d\&mu;}\&CenterDot;\mathrm{Bu}\left((k-1)T_s\right)]+{\&Integral;}_0^{T_s-\mathrm{\&tau;}}{e}^{\mathrm{A\&mu;}}\mathrm{d\&mu;}\&CenterDot;\mathrm{Bu}\left(k{T}_s\right)\\ =e^{A{T}_s}X\left(k{T}_s\right)+e^{A(T_s-\mathrm{\&tau;})}{\&Integral;}_0^{\mathrm{\&tau;}}{e}^{\mathrm{A\&mu;}}\mathrm{d\&mu;B}\&CenterDot;u\left((k-1)T_s\right)+{\&Integral;}_0^{T_s-\mathrm{\&tau;}}{e}^{\mathrm{A\&mu;}}\mathrm{d\&mu;B}\&CenterDot;u\left(k{T}_s\right)\\ =A_{1}X\left(k{T}_s\right)+B_{1}u\left((k-1)T_s\right)+B_{2}u\left(k{T}_s\right)\end{array}---\left(29\right)$

其中：

$\begin{array}{ccc}{A}_1=e^{A{T}_s},& B_1=e^{A(T_s-\mathrm{\&tau;})}{\&Integral;}_0^{\mathrm{\&tau;}}{e}^{\mathrm{A\&mu;}}\mathrm{d\&mu;B},& B_2=\end{array},{\&Integral;}_0^{T_s-\mathrm{\&tau;}}{e}^{\mathrm{A\&mu;}}\mathrm{d\&mu;B}---\left(30\right)$

在不发生歧义的情况下，k表示第k个采样周期，如有Z(k)＝Z(kT_s)，设

$Z\left(k\right)=\left(\begin{array}{c}X\left(k\right)\\ u(k-1)\end{array}\right)---\left(31\right)$

则存在网络诱导时延的情况下，MPC的离散化模型为：

$\left.\begin{array}{c}Z(k+1)=A_{z}Z\left(k\right)+B_{z}u\left(k\right)\\ Y\left(k\right)=C_{z}Z\left(k\right)\end{array}\right\}---\left(32\right)$

其中

$A_z=\left(\begin{array}{cc}{A}_1& B_1\\ 0& 0\end{array}\right),B_z=\left(\begin{array}{c}{B}_2\\ I\end{array}\right),C_z=\left(\begin{array}{cc}C& 0\end{array}\right)---\left(33\right)$

然后可求出预测模型。

下面对本发明的MPC编队控制方法进行验证和评价。

首先，通过安全距离的余量比λ_ri和来计算编队采取避碰机动的概率。假设无人机的安全距离d_si＝200m，均方差σ_ij＝10m，编队的队形设定距离d_rij＝230m，其中i,j＝1,2,…,7。安全距离的余量比按照定义3，此编队属于疏松编队。无人机成员i与j之间需要采取避碰机动的概率p_b(i,j)＝α＝Φ(-3)＝0.00135。如果由7架无人机组成基本队形为多边形的编队，即N＝7，n_c＝6，由式(5)和式(6)计算出无人机成员采取避碰机动的概率p_b(i)＝0.008073＜1％，编队采取避碰机动的概率p_b＝0.05516＜6％。如果其它假设条件不变，只将编队的队形设定距离更接近于安全距离d_si(t)，改为d_rij(t)＝210m，λ_rij＝1，按照定义5，此编队属于密集编队，则p_b(i,j)＝α＝Φ(-1)＝0.1587，在此情况下，无人机成员采取避碰机动的概率p_b(i)＝0.64543＞60％，编队采取避碰机动的概率p_b＝0.999295≈100％。也就是说，将安全距离的余量比由3减小到1(即安全距离的余量由3σ减小到1σ)，则编队需要采取避碰机动的概率就从小于6％增大到接近于100％，编队类型也由疏松编队变为密集编队。编队需要采取避碰机动的概率接近于1，编队几乎一定要采取避碰机动措施的。可见，安全距离的余量比λ_ri不仅是划分编队类型的标志性参数，而且是影响编队的动态稳定特性的变量，是判定编队的疏密度和密集度的重要指标，也是考核编队协同制导控制器性能的重要参数。

假设有N＝32的无人机编队，平均连接度n_c(t)＝7，无人机的安全距离d_si＝200m，编队的队形设定距离d_rij＝230m，其中i,j＝1,2,…,32。

无人机成员的安全距离余量比如果要求编队的碰撞概率在统计意义上不大于30％，即p_b≤30％。根据式(1)-式(6)可以计算出按照定义5，此编队属于密集编队，拟设计的编队协同制导控制器对32架无人机控制精度的均方差σ_ij应不大于10.17m。

然后，为了说明无人机自主编队评价指标对于编队控制器设计的指导作用，选择典型PID编队控制器与本发明所提出的MPC编队控制器进行对比仿真验证。

双机编队测试航迹场景：最小速度50m/s；最大速度200m/s；初速100m/s；队形保持距离x＝100m；y＝-173.2m；d＝200m；t＝5s时航迹偏转负60度；t＝30s时再偏转正60度。

(1)PID编队控制器仿真结果，如图6的a、b和c所示，c为无人机相对距离d的示意图，a和b分别为无人机距离d在某无人机航迹坐标系x,y轴上的分量；

(2)MPC编队控制器仿真结果，如图7的A、B和C所示，C为无人机相对距离d的示意图，A和B分别为无人机相对距离d在某无人机航迹坐标系x,y轴上的分量。

在测试航迹70s的仿真时间里，采用PID编队控制器的间距标准差σ＝32.7089m；采用本发明MPC编队控制器的间距标准差σ＝0.8393m。仿真结果表明，PID编队控制器在编队保持的过程中与期望间距最多相差超过了60m，而MPC编队控制器与期望间距维持在2.75m之内，验证了本发明提供的MPC编队控制方法在编队飞行控制中的优势。

Claims (5)

1.一种无人机自主编队评价指标与模型预测控制(MPC)编队控制方法，其特征在于：

所述的无人机自主编队评价指标如下：

$\mathrm{\&sigma;}\left(t\right)=\sqrt{E[\mathrm{\&Delta;}{d}_f\left(t\right){]}^2-\{E\left[\mathrm{\&Delta;}{d}_f\left(t\right)\right]{\}}^2}=\sqrt{E[\mathrm{\&Delta;}{d}_f\left(t\right){]}^2-[\mathrm{\&Delta;}{d}_{\mathrm{Ef}}\left(t\right){]}^2}---\left(1\right)$

其中，Δd_f(t)表示t时刻编队的平均连接安全距离余量；σ(t)为t时刻无人机自主编队评价指标，是Δd_f(t)的均方差；参数Δd_Ef(t)＝E[Δd_f(t)]；

所述的MPC编队控制方法，具体实现步骤如下：

第一步，建立无人机编队运动模型：

设两个无人机W和L，速度分别为V_W和V_L，航迹偏角分别为和无人机W到无人机L的距离为d，x₀,y₀为距离d在无人机W航迹坐标系x,y轴上的分量；无人机L与无人机W的运动模型如下：

和V_Wc分别为无人机W的航迹偏角指令和速度指令，和V_Lc分别为无人机L的航迹偏角指令和速度指令，分别为无人机L和无人机W航迹偏角响应的时间常数，分别为无人机L和无人机W速度响应的时间常数；

第二步，实现MPC编队控制，包括：预测模型、滚动优化和反馈校正；

步骤2.1：预测模型；

利用式(2)作为预测模型，取状态可控输入为把节点L的航迹偏角和速度看作可测量干扰输出其中 忽略D时，无人机编队运动模型的离散形式为：

$\left\{\begin{array}{c}X(k+1)=\mathrm{AX}\left(k\right)+\mathrm{Bu}\left(k\right)\\ Y\left(k\right)=\mathrm{CX}\left(k\right)\end{array}\right.---\left(3\right)$

其中，X(k)表示时刻k的状态，u(k)表示时刻k的可控输入，Y(k)表示时刻k的输出，A、B和C表示系数矩阵；设u(k+i|k)表示在时刻k假定的未来k+i时刻的可控输入值u，X(k+i|k)，Y(k+i|k)分别表示在时刻k，利用u(k+j|k),(j＝0,1,...,i-1)对X，Y作出的预测值；获得的状态X的预测方程为：

其中，P为预测时域，N为控制时域，u(k-1)表示k-1时刻的可控输入，差值Δu(k+j|k)＝u(k+j|k)-u(k+j-1|k)；

输出Y的预测方程为：Y(k+j|k)＝CX(k+j|k),j＝1,2,...,P；

步骤2.2：滚动优化；

设编队飞行在时刻k的代价函数J为：

$J=\underset{l=1}{\overset{P}{\mathrm{\&Sigma;}}}\left(\underset{m=1}{\overset{N}{\mathrm{\&Sigma;}}}\right||{\mathrm{\&omega;}}_y(l,m)\&CenterDot;(Y_m(k+l|k)-{\mathrm{ref}}_m(k+l))|{|}^2+\underset{m=1}{\overset{N}{\mathrm{\&Sigma;}}}\left|\right|{\mathrm{\&omega;}}_{\mathrm{\&Delta;u}}(l,m)\&CenterDot;\mathrm{\&Delta;}{u}_m(k+l|k)\left|{|}^2\right)---\left(4\right)$

其中，ω_y为输出惩罚权重矩阵，ω_y(l,m)表示时域对(l,m)对应的输出惩罚权重；Y_m(k+l|k)为在控制时间m下、k时刻计算的k+l时刻的预测值；ref为间距保持指令，ref_m(k+l)为控制时间m下、k时刻计算的k+l时刻的间距保持指令；ω_Δu为输入变化率惩罚权重矩阵，ω_Δu(l,m)表示时域对(l,m)对应的输入变化率惩罚权重；Δu_m(k+l|k)为在控制时间m下的k时刻计算的k+l时刻输入变化率；令每个时域对(l,m)的权重相等，则ω_y和ω_Δu分别退化为一个行向量；

在时刻k的控制量u(k)为下面的约束优化问题：

Δu_opt＝arg minJ

其中，和分别为航迹偏角和速度指令限制；Δu_opt为最优序列的第一个控制量，u₁u₂分别为和V_c；I_2×2表示2×2的单位矩阵，O_(N-1)×(N-1)表示(N-1)×(N-1)的0矩阵；

步骤2.3：反馈校正，包括：(1)直接将X(k)作为***的初始状态；(2)把无人机L的航迹偏角和速度看作可测量干扰通过网络传递给无人机W并附加到预测模型的预测值。

2.根据权利要求1所述的无人机自主编队评价指标与MPC编队控制方法，其特征在于：所述的t时刻编队的平均连接安全距离余量Δd_f(t)的获取方法是：

Δd_f(t)＝d_f(t)-d_sf(t)

$\begin{array}{c}=\frac{1}{M\left(t\right)}\underset{i=1}{\overset{M\left(t\right)}{\mathrm{\&Sigma;}}}\left\{\frac{1}{n_{\mathrm{ci}}\left(t\right)}\underset{j=1,i\&NotEqual;j}{\overset{n_{\mathrm{ci}}\left(t\right)}{\mathrm{\&Sigma;}}}{d}_{\mathrm{ij}}\left(t\right)\right\}-\frac{1}{M\left(t\right)}\underset{i=1}{\overset{M\left(t\right)}{\mathrm{\&Sigma;}}}{d}_{\mathrm{si}}\left(t\right)\\ =\frac{1}{M\left(t\right)}\underset{i=1}{\overset{M\left(t\right)}{\mathrm{\&Sigma;}}}\left\{\right[\frac{1}{n_{\mathrm{ci}}\left(t\right)}\underset{j=1,i\&NotEqual;1}{\overset{n_{\mathrm{ci}}\left(t\right)}{\mathrm{\&Sigma;}}}{d}_{\mathrm{ij}}\left(t\right)]-d_{\mathrm{si}}\left(t\right)\}\end{array}---\left(5\right)$

其中，d_f(t)表示t时刻编队的平均连接机间距，d_sf(t)表示t时刻编队的平均安全距离，M(t)是t时刻无人机总数，d_ij(t)表示无人机i和无人机j之间的距离，d_si(t)表示无人机i的安全距离，n_ci(t)表示t时刻无人机i的连接度。

3.根据权利要求1或2所述的无人机自主编队评价指标与MPC编队控制方法，其特征在于：所述的无人机自主编队评价，通过下面式(6)-(11)作为评价公式，具体是：

设d_ij(t)表示无人机i和无人机j之间的距离，d_si(t)表示无人机i的安全距离，则无人机i和无人机j的安全距离余量Δd_ij(t)＝d_ij(t)-d_si(t)，下面将与时间t相关的变量中的时间t省去进行简写；

设无人机成员的安全距离余量Δd_ij服从正态分布Δd_rij为设定的安全距离余量，σ_ij ²为方差；当Δd_ij≤0时，无人机成员就需要或准备采取避碰机动措施，无人机i与无人机j之间需要或准备采取避碰机动的概率p_b(i,j)为：

p_b(i,j)＝P{Δd_ij≤0}

$\begin{array}{c}={\&Integral;}_{-\&infin;}^{\frac{-\mathrm{\&Delta;}{d}_{\mathrm{rij}}}{{\mathrm{\&sigma;}}_{\mathrm{ij}}}}\frac{1}{\sqrt{2\mathrm{\&pi;}}}\&CenterDot;e^{-\frac{{\mathrm{\&tau;}}^2}{2}\mathrm{d\&tau;}}\\ =\mathrm{\&Phi;}\left(\frac{-\mathrm{\&Delta;}{d}_{\mathrm{rij}}}{{\mathrm{\&sigma;}}_{\mathrm{ij}}}\right)\\ =\mathrm{\&Phi;}(-{\mathrm{\&lambda;}}_{\mathrm{rij}})\end{array}---\left(6\right)$

无人机i与无人机j之间无需采取避碰机动的概率q_b(i,j)为：

q_b(i,j)＝P{Δd_ij＞0}

$\begin{array}{c}=1-{\&Integral;}_{-\&infin;}^{\frac{-\mathrm{\&Delta;}{d}_{\mathrm{rij}}}{{\mathrm{\&sigma;}}_{\mathrm{ij}}}}\frac{1}{\sqrt{2\mathrm{\&pi;}}}\&CenterDot;e^{-\frac{{\mathrm{\&tau;}}^2}{2}\mathrm{d\&tau;}}\\ =1-\mathrm{\&Phi;}\left(\frac{-\mathrm{\&Delta;}{d}_{\mathrm{rij}}}{{\mathrm{\&sigma;}}_{\mathrm{ij}}}\right)\\ =1-\mathrm{\&Phi;}(-{\mathrm{\&lambda;}}_{\mathrm{rij}})\end{array}---\left(7\right)$

式中，是安全距离余量比，为正态概率积分；

将密集度对避碰机动概率的影响反映出来，无人机i自己的避碰机动的概率p_b(i)为：

$\begin{array}{c}{p}_b\left(i\right)\underset{j=1}{\overset{n_{\mathrm{ci}}}{\mathrm{\&Sigma;}}}{p}_b(i,j)[q_b(i,j){]}^{n_{\mathrm{ci}}-1}+\underset{j=1}{\overset{C_{n_{\mathrm{ci}}}^2}{\mathrm{\&Sigma;}}}[p_b(i,j){]}^2[q_b(i,j){]}^{n_{\mathrm{ci}}-2}+\&CenterDot;\&CenterDot;\&CenterDot;\\ +\underset{j=1}{\overset{C_{n_{\mathrm{ci}}}^k}{\mathrm{\&Sigma;}}}\left[p_b(i,j){]}^k\right[q_b(i,j){]}^{n_{\mathrm{ci}}-k}+\&CenterDot;\&CenterDot;\&CenterDot;+\underset{j=1}{\overset{n_{\mathrm{ci}}}{\mathrm{\&Pi;}}}{p}_b(i,j)\\ =\underset{k=1}{\overset{n_{\mathrm{ci}}}{\mathrm{\&Sigma;}}}\underset{j=1}{\overset{C_{n_{\mathrm{ci}}}^k}{\mathrm{\&Sigma;}}}\left[p_b(i,j){]}^k\right[q_b(i,j){]}^{n_{\mathrm{ci}}-k}\\ =\underset{k=1}{\overset{n_{\mathrm{ci}}}{\mathrm{\&Sigma;}}}\underset{j=1}{\overset{C_{n_{\mathrm{ci}}}^k}{\mathrm{\&Sigma;}}}\left[\mathrm{\&Phi;}(-{\mathrm{\&lambda;}}_{\mathrm{rij}}){]}^k\right[1-\mathrm{\&Phi;}(-{\mathrm{\&lambda;}}_{\mathrm{rij}}){]}^{n_{\mathrm{ci}}-k},(n_{\mathrm{ci}}\&GreaterEqual;2)\end{array}---\left(8\right)$

其中，n_ci为无人机i的连接度，为组合数；对于连接度n_ci＝1时，避碰机动的概率p_b(i)采用式(6)来计算；

整个编队采取避碰机动的概率p_b为：

$\begin{array}{c}{p}_b=\underset{i=1}{\overset{M}{\mathrm{\&Sigma;}}}{p}_b\left(i\right)[1-p_b\left(i\right){]}^{M-1}+\underset{i=1}{\overset{C_M^2}{\mathrm{\&Sigma;}}}[p_b\left(i\right){]}^2[1-p_b\left(i\right){]}^{M-2}+\&CenterDot;\&CenterDot;\&CenterDot;\\ +\underset{i=1}{\overset{C_M^k}{\mathrm{\&Sigma;}}}\left[p_b\left(i\right){]}^k\right[1-p_b\left(i\right){]}^{M-k}+\&CenterDot;\&CenterDot;\&CenterDot;\underset{i=1}{\overset{M}{\mathrm{\&Pi;}}}{p}_b\left(i\right)\\ =\underset{k=1}{\overset{M}{\mathrm{\&Sigma;}}}\underset{i=1}{\overset{C_M^k}{\mathrm{\&Sigma;}}}\left[p_b\left(i\right){]}^k\right[1-p_b\left(i\right){]}^{M-k}\\ =\underset{k=1}{\overset{M}{\mathrm{\&Sigma;}}}\underset{i=1}{\overset{C_M^k}{\mathrm{\&Sigma;}}}\left[p_b\left(i\right){]}^k\right[q_b\left(i\right){]}^{M-k}\end{array}---\left(9\right)$

式中，q_b(i)＝1-p_b(i)为无人机成员i无需采取避碰机动的概率，M为整个编队的无人机个数；

特别地，当p_b(i,j)＝α(0≤α≤1)为常值时，式(8)和式(9)分别变为式(10)和式(11)：

$\begin{array}{c}{p}_b\left(i\right)=p=n_{\mathrm{ci}}\&CenterDot;\mathrm{\&alpha;}{\mathrm{\&beta;}}^{n_{\mathrm{ci}}-1}+C_{n_{\mathrm{ci}}}^2{\mathrm{\&alpha;}}^2\&CenterDot;{\mathrm{\&beta;}}^{n_{\mathrm{ci}}-2}+\&CenterDot;\&CenterDot;\&CenterDot;+C_{n_{\mathrm{ci}}}^k\&CenterDot;{\mathrm{\&alpha;}}^k{\mathrm{\&beta;}}^{n_{\mathrm{ci}}-k}+\&CenterDot;\&CenterDot;\&CenterDot;+{\mathrm{\&alpha;}}^{n_{\mathrm{ci}}}\\ =\underset{k=1}{\overset{n_{\mathrm{ci}}}{\mathrm{\&Sigma;}}}{C}_{n_{\mathrm{ci}}}^k\&CenterDot;{\mathrm{\&alpha;}}^k{\mathrm{\&beta;}}^{n_{\mathrm{vi}}-k}\\ =1-{\mathrm{\&beta;}}^{n_{\mathrm{ci}}}\end{array}---\left(10\right)$

$\begin{array}{c}{P}_b=M\&CenterDot;p{q}^{M-1}+C_M^2\&CenterDot;p^2{q}^{M-2}+\&CenterDot;\&CenterDot;\&CenterDot;+C_M^k\&CenterDot;p^k{q}^{M-k}+\&CenterDot;\&CenterDot;\&CenterDot;+p^M\\ =\underset{k=1}{\overset{M}{\mathrm{\&Sigma;}}}{C}_M^k\&CenterDot;p^k{q}^{M-k}\\ =1-q^M\end{array}---\left(11\right)$

其中，β＝1-α，q＝1-p。

4.根据权利要求1所述的无人机自主编队评价指标与MPC编队控制方法，其特征在于：所述的第二步中的预测模型，在存在网络诱导时延情况下，具体获取方法是：

当网络诱导时延不能忽略时，线性运动模型的离散化需要考虑时延的影响，考察线性时不变连续***的一般形式为：

$\left.\begin{array}{c}\frac{\mathrm{dX}\left(t\right)}{\mathrm{dt}}\mathrm{AX}\left(t\right)+\mathrm{Bu}\left(t\right)\\ Y=\mathrm{CX}\left(t\right)\end{array}\right\}---\left(12\right)$

其时域解为：

$X\left(t\right)=e^{A(t-t_0)}X\left(t_0\right)+{\&Integral;}_{t_0}^t{e}^{A(t-\mathrm{\&mu;})}\mathrm{Bu}\left(\mathrm{\&mu;}\right)\mathrm{d\&mu;}---\left(13\right)$

t₀表示***的初始时间，当存在网络诱导时延τ时，因为MPC的输入u是离散的，设u的采样周期为T_s，当新的状态没有到达的时候u将等于上一时刻的输入，所以有：

$\frac{\mathrm{dX}\left(t\right)}{\mathrm{dt}}=\begin{array}{cc}\mathrm{AX}\left(t\right)+\mathrm{Bu}\left((k-1)T_s\right),& {\mathrm{kT}}_s\&le;t\&le;{\mathrm{kT}}_s+\mathrm{\&tau;}\\ \mathrm{AX}\left(t\right)+\mathrm{Bu}\left(k{T}_s\right),& k{T}_s+\mathrm{\&tau;}\&le;t<(k+1)T_s\end{array}---\left(14\right)$

当kT_s≤t＜kT_s+τ时，考虑状态X从kT_s转移到kT_s+τ，有：

$\begin{array}{c}X(k{T}_s+\mathrm{\&tau;})=e^{\mathrm{A\&tau;}}X\left(k{T}_s\right)+{\&Integral;}_{k{T}_s}^{{\mathrm{kT}}_s+\mathrm{\&tau;}}{e}^{A(k{T}_s+\mathrm{\&tau;}-\mathrm{\&mu;})}\mathrm{Bu}\left((k-1)T_s\right)\mathrm{d\&mu;}\\ =e^{\mathrm{A\&tau;}}X\left(k{T}_s\right)+{\&Integral;}_0^{\mathrm{\&tau;}}{e}^{\mathrm{A\&mu;}}\mathrm{d\&mu;}\&CenterDot;\mathrm{Bu}\left((k-1)T_s\right)\end{array}---\left(15\right)$

当kT_s+τ≤t＜(k+1)T_s时，考虑状态X从kT_s+τ转移到(k+1)T_s，有

$\begin{array}{c}X\left((k+1)T_s\right)=e^{A(T_s-\mathrm{\&tau;})}X({\mathrm{kT}}_s+\mathrm{\&tau;})+{\&Integral;}_{{\mathrm{kT}}_s+\mathrm{\&tau;}}^{(k+1)T_s}{e}^{A((k+1)T_s-\mathrm{\&mu;})}\mathrm{Bu}\left({\mathrm{kT}}_s\right)\mathrm{d\&mu;}\\ =e^{A(T_s-\mathrm{\&tau;})}[e^{\mathrm{A\&tau;}}X\left({\mathrm{kT}}_s\right)+{\&Integral;}_0^{\mathrm{\&tau;}}{e}^{\mathrm{A\&mu;}}\mathrm{d\&mu;}\&CenterDot;\mathrm{Bu}\left((k-1)T_s\right)]+{\&Integral;}_0^{T_s-\mathrm{\&tau;}}{e}^{\mathrm{A\&mu;}}\mathrm{d\&mu;}\&CenterDot;\mathrm{Bu}\left(k{T}_s\right)\\ =e^{A{T}_s}X\left(k{T}_s\right)+e^{A(T_s-\mathrm{\&tau;})}{\&Integral;}_0^{\mathrm{\&tau;}}{e}^{\mathrm{A\&mu;}}\mathrm{d\&mu;B}\&CenterDot;u\left((k-1)T_s\right)+{\&Integral;}_0^{T_s-\mathrm{\&tau;}}{e}^{\mathrm{A\&mu;}}\mathrm{d\&mu;B}\&CenterDot;u\left(k{T}_s\right)\\ =A_{1}X\left(k{T}_s\right)+B_{1}u\left((k-1)T_s\right)+B_{2}u\left(k{T}_s\right)\end{array}---\left(16\right)$

其中参数：

$\begin{array}{ccc}{A}_1=e^{A{T}_s},& B_1=e^{A(T_s-\mathrm{\&tau;})}{\&Integral;}_0^{\mathrm{\&tau;}}{e}^{\mathrm{A\&mu;}}\mathrm{d\&mu;B},& B_2=\end{array},{\&Integral;}_0^{T_s-\mathrm{\&tau;}}{e}^{\mathrm{A\&mu;}}\mathrm{d\&mu;B}---\left(17\right)$

k表示第k个采样周期，则Z(k)＝Z(kT_s)，设

$Z\left(k\right)=\left(\begin{array}{c}X\left(k\right)\\ u(k-1)\end{array}\right)---\left(18\right)$

则存在网络诱导时延的情况下，MPC的离散化模型为：

$\left.\begin{array}{c}Z(k+1)=A_{z}Z\left(k\right)+B_{z}u\left(k\right)\\ Y\left(k\right)=C_{z}Z\left(k\right)\end{array}\right\}---\left(19\right)$

其中，

$A_z=\left(\begin{array}{cc}{A}_1& B_1\\ 0& 0\end{array}\right),B_z=\left(\begin{array}{c}{B}_2\\ I\end{array}\right),C_z=\left(\begin{array}{cc}C& 0\end{array}\right)---\left(20\right)$

最后根据式(19)和(20)的离散化模型求出预测模型。

5.根据权利要求1或4所述的无人机自主编队评价指标与MPC编队控制方法，其特征在于：所述的预测时域P＝50；所述的控制时域N＝40；所述的输出惩罚权重ω_y＝(200,250,200,50)，所述的输入变化率惩罚权重ω_Δu＝(5,0.1)。