基于微粒群优化和遗传算法的多无人机三维编队重构方法

摘要

本发明公开了一种基于微粒群优化和遗传算法的多无人机三维编队重构方法，该方法在构建编队模型时不仅考虑了地面坐标系中的无人机位置，而且还考虑了无人机的速度、航迹角及航向角，将无人机中的各个飞行单元的控制输入进行分段线性化处理，并用近似的分段线性化控制输入代替连续的控制输入，然后用遗传算法进行粗搜索，随后用微粒群优化算法进行细搜索，在此基础上再用微粒群优化指导遗传算法搜索全局最优解，以求出分段线性化控制输入。与传统方法相比，本发明所提出的方法具有较好的实时性和快速性，该方法还可应用于解决复杂动态环境下多空间机器人的编队重构问题。

权利要求

1.一种基于微粒群优化和遗传算法的多无人机三维编队重构方法，其特征在于：

(一)无人机的数学模型

在对无人机分析的基础上，公式(1)-(6)给出了其数学模型，基于PSO和GA的无人机三维编队重构是以此模型来进行程序设计的，

$\stackrel{·}{v}=g[(T-D)/W-\sin \gamma ]---\left(1\right)$$\stackrel{·}{\gamma }=(g/v)(n\cos \phi -\cos \gamma )---\left(2\right)$$\stackrel{·}{\chi }=\left(\mathrm{gn}\sin \phi \right)/\left(v\cos \gamma \right)---\left(3\right)$$\stackrel{·}{x}=v\cos \gamma \cos \chi ---\left(4\right)$$\stackrel{·}{y}=v\cos \gamma \sin \chi ---\left(5\right)$$\stackrel{·}{z}=-v\sin \gamma ---\left(6\right)$式中：ν为无人机的速度，γ为飞行航迹角，χ为航向角，x，y，z表示在地面坐标系中无人机的位置，g为重力加速度，T为油门位置，D为气动阻力，W为无人机的重量，n为过载，φ为俯仰角；取状态变量为(ν，γ，χ，x，y，z)，控制输入为(T，n，φ)；

(二)三维编队重构最优时间控制的数学描述

假设某编队由N架无入机组成，控制向量作用初始时刻t＝0，终端时刻t＝T，定义编队内第i架无人机的控制输入为编队的控制输入向量则编队的连续控制输入向量U可进一步表述为定义编队内第i架无人机的状态变量xi＝(νi，γi，χi，xi，yi，zi)，因此，编队系统的状态变量定义为编队系统的运动方程可以表述为：$\stackrel{·}{X}\left(t\right)=f(t,X\left(t\right),U\left(t\right))---\left(7\right)$定义编队连续的控制输入U以及编队初始状态X(0)＝X0，则在t∈(0，T]任意时刻编队的状态均可由下式唯一确定：

$X\left(t\right)=X\left(0\right)+{\int }_0^{t^{-}}f(\tau ,X\left(\tau \right),U\left(\tau \right))\mathrm{d\tau }---\left(8\right)$如果给定了初始状态，则X(t)仅仅由U唯一确定，也可用X(t|U)表述；

通常，代价函数的标准形式可以表示为

$J\left(U\right)\stackrel{\Delta }{=}{\Phi }_0\left(X\left(T\right|U)\right)+{\int }_0^T{L}_0(t,X\left(t\right|U),U\left(t\right))\mathrm{dt}---\left(9\right)$约束条件可表述为：

$g_i\left(U\right)\stackrel{\Delta }{=}{\Phi }_i\left(X\left({\tau }_i\right|U)\right)+{\int }_0^{{\tau }_i}{L}_i(t,X\left(t\right|U),U\left(t\right))\mathrm{dt}\le 0\forall i\in \{1,...,M\}---\left(10\right)$对于编队系统最优时间控制问题可以表述为：寻找一个连续的控制输入U和终端时刻T使得编队系统代价函数J(U)最小，也即：

$\underset{u_1,T}{\min }...\underset{u_N,T}{\min }J\left(U\right)---\left(11\right)$编队系统代价函数J(U)可以表述为：

J(U)＝T                                 (12)

控制容许约束为：

Umin≤U(t)≤Umax，$\forall t\in [0,T),0<T---\left(13\right)$自由终端约束为：

$g_1(U,\mathrm{\Delta t})\stackrel{\Delta }{=}\underset{i=1}{\overset{N}{\Sigma }}\{{[(x_i\left(T\right)-x_m\left(T\right))-x_i^m]}^2+{[(y_i\left(T\right)-y_m\left(T\right))-y_i^m]}^2+{[(z_i\left(T\right)-z_m\left(T\right))-z_i^m]}^2\}---\left(14\right)$$=0$

式中：m∈{1，…，N}，定义第m架无人机作为编队的中心无人机；[xim，yim，zim]T为终端T时刻编队内第i架无人机相对于编号为m的中心无人机期望的相对坐标值；

定义任意两架无人机之间距离为di，j(xi(t)，xj(t))(其中，i，j∈{1，…，N})，其表达式为：

$d^{i,j}(x_i\left(t\right),x_j\left(t\right))\stackrel{\Delta }{=}\sqrt{{(x_i\left(t\right)-x_j\left(t\right))}^2+{(y_i\left(t\right)-y_j\left(t\right))}^2+{(z_i\left(t\right)-z_j\left(t\right))}^2}---\left(15\right)$为了防止无人机相撞，编队内任意两架无人机之间距离di，j(xi(t)，xj(t))必须大于安全防撞距离Dsafe：

di，j(xi(t)，xj(t))≥Dsafe$\forall t\in [0,T],$${\forall }_{i\ne j}i,j\in \{1,...,N\}---\left(16\right)$为了确保编队内能正常实时通讯，实时更新作战态势，任意两架无人机之间距离di，j(xi(t)，xj(t))必须小于通讯保障距离Dcomm：

di，j(xi(t)，xj(t))≤Dcomm$\forall t\in [0,T],$${\forall }_{i\ne j}i,j\in \{1,...,N\}---\left(17\right)$综上，编队系统的最优时间控制问题的数学描述为：在满足约束条件(7)(13)(14)(16)(17)约束条件下，寻找一个连续的控制输入U和终端时刻T使得(11)(12)两式成立；

(三)基于PSO和GA的无人机三维编队重构程序设计

PSO和GA算法是一种智能化的全局寻优算法，利用PSO和GA算法解决优化问题不受目标函数是否为线性的限制，适合解决三维编队重构最优控制问题；然而编队内各个飞行单元的控制输入均为连续量，PSO和GA算法无法求解出连续的控制输入；因此，首先将编队内各个飞行单元的控制输入进行分段线性化处理，用近似的分段线性化控制输入代替连续的控制输入，然后采用PSO和GA算法进行寻优，求出分段线性化控制输入；

控制输入的分段线性化：控制输入的作用时间T被划分为np等分，对于编队内第i架无人机，定义一个ri×np维常数集合则在时间T内，第i架无人机的连续控制输入作用ui可以采用常量分段函数近似地表述成下式：

${\hat{u}}_i(t;n_p,{\Omega }_i)\stackrel{\Delta }{=}\underset{j=1}{\overset{n_p}{\Sigma }}{{\sigma }^i}_j{\chi }_j\left(t\right)\cong u_i\left(t\right)---\left(18\right)$上式中，χj(t)由下式给定：

${\chi }_j\left(t\right)=\left\{\begin{array}{cc}1& (j-1)·\Delta t_p\le t\le j·\Delta t_p\\ 0& \mathrm{otherwise}\end{array}\right.---\left(19\right)$定义编队的分段线性化常系数集合为编队系统的近似控制输入集合为寻找最优控制输入集合使代价指标函数最小的问题就转化为寻找最优常系数集合Ω的问题；

近似参数化：控制输入经过近似处理后，寻找最优控制输入集合U和T使代价指标函数最小的问题近似地等价于寻找最优常数参数集合Ω和Δtp；因此，三维编队重构最优控制的代价函数可近似表述为：

$J\cong \underset{\Omega ,\Delta t_p}{\min }(n_p·\Delta t_p)---\left(20\right)$控制容许约束可近似表述为：

(umin)i≤σij≤(umax)i$\forall i\in \{1,...,N\},$$\forall j\in \{1,...,n_p\},0<\Delta t_p---\left(21\right)$自由终端约束可近似表述为：

${\hat{g}}_1(\Omega ,\mathrm{\Delta t})\stackrel{\Delta }{=}\underset{i=1}{\overset{N}{\Sigma }}\{{[(x_i\left(T\right)-x_m\left(T\right))-x_i^m]}^2+{[(y_i\left(T\right)-y_m\left(T\right))-y_i^m]}^2+{[(z_i\left(T\right)-z_m\left(T\right))-z_i^m]}^2\}=0---\left(22\right)$系统状态方程近似表述为：

$\stackrel{·}{X}\left(t\right)\cong f(t,X\left(t\right),\hat{U}(t;n_p,\Omega ))---\left(23\right)$其他约束条件表达式不变；

分段线性化控制输入U以后，即可采用PSO和GA算法解决三维编队重构最优控制问题；

将编队的控制输入常数集合(其中ri为第i架无人机控制输入的维数)与分段区间Δtp组合，只要确定了这些参数，就可解出编队控制输入；这样，无人机三维编队重构实际上转化成了在N×np×ri+1维上寻找使代价函数最优的问题；定义三维编队重构最优时间控制的扩展代价函数为：$J_{\mathrm{extend}}\stackrel{\Delta }{=}\underset{\Omega ,\Delta t_p}{\min }\{(n_p·\Delta t_p)+{\sigma }^{*}·{\hat{g}}_1(\Omega ,\mathrm{\Delta t})+\underset{i=1}{\overset{N-1}{\Sigma }}\underset{j=i+1}{\overset{N}{\Sigma }}[{\sigma }_{\mathrm{ij}}·\max (0,D_{\mathrm{safe}}-d^{i,j}(x_i\left(t\right),x_j\left(t\right)))---\left(24\right)$$+{\sigma }_{\mathrm{ij}}^{\prime }·\max (0,d^{i,j}\left(x_i\left(t\right)x_j\left(t\right)\right)-D_{\mathrm{comm}})\left]\right\}$式中：σij和σ′ij分别为安全防撞距离约束和通讯保障距离约束的惩罚常系数；σ*为终端约束(22)的惩罚常系数；为(22)式左端的表达形式，即终端T时刻编队内各无人机状态与期望状态的误差的平方和；

实际应用中GA的代价函数取为1/Jextend，PSO代价函数取为Jextend；

基于以上说明，就可以用PSO和GA算法求解无人机三维编队重构问题；GA算法进行粗搜索，PSO算法精度较高，用它进行细搜索；再用PSO指导GA搜索全局最优解；任意给定初始状态，指定终端时刻的相对状态，基于本发明提出的算法，可找到最优控制输入，驱动各无人机达到指定编队队形。

2.一种基于微粒群优化和遗传算法的多无人机三维编队重构方法，其特征在于：该方法的具体步骤为：

步骤1：初始化数目为M的微粒群，任意给定编队内各无人机的初始状态，指定终端时刻编队无人机的相对状态；给定微粒群算法的参数c1，c2，w；c1，c2称为学习因子，w为惯性权重；设置遗传算法参数Pc，Mute；Pc为交叉概率，取值范围为〔0.7，0.9〕；Mute为变异概率，取值范围为〔0，0.1〕；

步骤2：计算微粒代价函数并保留最优微粒的位置和代价函数；

步骤3：用混合概率P将微粒群分为两个子群；一个子群为粒子群，另一个子群为染色体种群；

步骤4：对M*P子群使用PSO算法；任意给定初始解xi，初始速度vi，历史最优位置pbesti，全局最优位置gbest，计算出相应的代价函数；由下式更新粒子的速度和位置信息：

$\left\{\begin{array}{c}{v}_i={\mathrm{wv}}_i+c_1·r_1·({\mathrm{pbest}}_i-x_i)+c_2·r_2·(\mathrm{gbest}-x_i)\\ x_i=x_i+v_i\end{array}\right.---\left(25\right)$其中，r1，r2为随机数；把新位置得到的解代入代价函数，求出在新位置处代价函数的值，若新位置处代价函数小于历史最优位置处代价函数，则历史最优位置更新为新位置，否则不做任何修改；若新位置处代价函数小于全局最优位置处代价函数，则全局最优位置更新为新位置，否则，不做任何修改；

步骤5：对剩余的子群，即染色体使用GA算法；随机产生初始解，并计算出初始适应度；遗传算法包含三个重要算子，分别为选择算子、交叉算子和变异算子：

5.1选择算子

选择采用数学轮盘赌的方法按每个染色体的适应度进行，这种方法确保了染色体被选择的概率与其适应度成正比；

5.2交叉算子

由于采用的是浮点数编码方式，所以将使用以算术交叉为基础的交叉算子；交叉算子为：

$P_1^{\mathrm{new}}=\omega ·P_1+(1-\omega )·P_2$(26)$P_2^{\mathrm{new}}=\omega ·P_2+(1-\omega )·P_1$式中：P1和P2为从种群中随机选择的两个父个体，P1new、P2new为通过交叉运算子运算后产生的子代对应新个体；ω为参数，ω∈[0，1]；

5.3变异算子

采用自适应加速变异算子对交叉算子作用后的群体的染色体进行变异操作，算法如下：

$P_i^j(k+1)=P_i^j\left(k\right)+\beta ·\Delta P_i^j\left(k\right)+\rho ·{\mathrm{sP}}_i^j\left(k\right)$$\Delta P_i^j\left(k\right)=(P_i^{\mathrm{best}}\left(k\right)-P_i^j\left(k\right))·\left|N\left(\mathrm{0,1}\right)\right|---\left(27\right)$${\mathrm{sP}}_i^j(k+1)=\beta ·{\mathrm{acc}}^j\left(k\right)·\Delta P_i^j\left(k\right)+\rho ·{\mathrm{sP}}_i^j\left(k\right)$式中：Pij(k)为第k代中第j个染色体的第i个分量，Pbest(k)为第k代中最好的个体，ρ和β分别为学习速率和惯量常数，N(0，1)为正态随机分布函数，sPij(k)为进化趋势，accj(k)定义为：

步骤6：比较PSO算法得到的最优解与GA算法得到的最优解优劣，若PSO算法的解优于GA算法的最优解，则当前最优解为PSO算法的最优解，并把GA算法的最优解替换为PSO算法的最优解；否则，当前最优解为GA算法的最优解，并把PSO算法的最优解替换为GA算法的最优解；

步骤7：步骤2～步骤6重复执行，直到满足结束条件。

说明书

(一)技术领域

本发明涉及一种基于微粒群优化(Particle Swarm Optimization，以下简称PSO)遗传算法(Genetic Algorithm，以下简称GA)的多无人机三维编队重构技术，属于航空科学技术领域。

(二)背景技术

无人机最早出现在1913年，二战期间，出现了一种用无线电指令来遥控的军用无人机，它是无人机的鼻祖。大战末期，德国人研制成功了带有战斗部的V-1，V-2无人机，也是最早的巡航导弹，从此无人机开始了实战使用。到了四五十年代，无人机作为靶机使用。六十年代以后出现了应用于战场侦察的无人机，无人机已经表现出十分明显的军用价值。已经问世的无人机靶机、诱饵机和侦察机等在过去的使用中都显示出各自的威力。

现代无人机的完整定义：它是一种有动力，采用无线电遥控+自主飞控系统的、能携带多种任务设备、执行多种任务、可机动飞行、多次使用的无人驾驶飞行器。

现代无人机主要执行战场侦察与监视、目标截获、火力校正、毁损评估以及电子诱骗与电子干扰等任务。进入21世纪后，随着无人机技术日趋成熟，性能不断完善，它能够承担的任务范围进一步扩大，任务级别由战术级逐步扩大到战役、战略级。侦察型无人机的任务已由战术侦察向战略侦察范围扩展，所具有的全天候、大纵深监视能力将成为卫星侦察、有人侦察的重要补充与增强手段。无论在战略或者战术侦察范围，无人机都将成为应用非常广泛的低风险、高效费比的战场感知平台。

杀伤型无人机的任务由当前的电子干扰、反雷达攻击向执行多种精确打击和空战任务发展。无人侦察机技术的发展和成熟，为攻击型无人机的诞生创造了条件。攻击型无人机是一个新崛起的机种，目前已是战争中一支重要战术辅助力量和战斗力增强因子。它可携带不同的武器，执行不同的任务，可多次回收使用；它既有有人战斗机和导弹的优点，又和导弹一样没有人员的伤亡和被俘的危险；而且因不存在人的生理限制，可超长时间续航，也可以超机动飞行；还因其目标特征小，具有很强的突防能力和生存力；同时它又和有人机一样，可多次使用，活动空间大，可执行多种任务，而与有人机相比无人机成本低了很多。

在不久的将来，无人机将逐步担当航空航天控制和航空航天对地攻击任务，成为实施空中精确打击的一种手段。无人机的用途由执行侦察任务扩展到执行多种打击任务后，必将演变成一种高效费比、攻防兼备的全新武器概念，并将引起军队作战思想、作战样式和组织编制的一系列改革。

无人机编队飞行，就是将多架无人机按照一定的队形进行排列，并使其在整个飞行过程中保持队形不变。当某架无人机因敌人的通信干扰或者受到攻击而掉队失踪，其余的无人机就应该能立即填补它留下的空缺。编队飞行的无人机能通过信息共享在飞行中改变原有队形，自主地对突发事件做出反应。机群中的所有无人机都要在执行任务时根据面临的具体情况来分担各自的任务，这就涉及到三维编队重构技术。当飞行中的机群受到电子干扰或者在战斗中损坏时，可以用机群中别的无人机来完成三维编队重构，从而保持编队队形稳定。三维编队重构研究的核心就是怎样在飞行中，实时规划机群中无人机的飞行轨迹，使得性能良好的无人机能在飞行中完成对编队队形的重构，使得编队队形不变或者达到最优队形。

(三)发明内容

本发明的目的在于提供一种基于微粒群优化和遗传算法的多无人机三维编队重构方法，以解决现有技术中三维编队重构的最小能量控制、最短时间与最小能量综合控制等问题，以及多编队重构、多机协同等复杂系统集中控制的优化问题。

本发明涉及一种基于PSO和GA的多无人机三维编队重构方法，发明的内容具体如下：

1、无人机的数学模型

建立无人机的数学模型是实现三维编队重构的必要前提。在对无人机分析的基础上，公式(1)-(6)给出了其数学模型。基于PSO和GA的无人机三维编队重构是以此模型来进行程序设计的。

$\stackrel{·}{v}=g[(T-D)/W-\sin \gamma ]---\left(1\right)$

$\stackrel{·}{\gamma }=(g/v)(n\cos \phi -\cos \gamma )---\left(2\right)$

$\stackrel{·}{\chi }=\left(\mathrm{gn}\sin \phi \right)/\left(v\cos \gamma \right)---\left(3\right)$

$\stackrel{·}{x}=v\cos \gamma \cos \chi ---\left(4\right)$

$\stackrel{·}{y}=v\cos \gamma \sin \chi ---\left(5\right)$

$\stackrel{·}{z}=-v\sin \gamma ---\left(6\right)$

式中：v为无人机的速度，γ为飞行航迹角，χ为航向角，x，y，z表示在地面坐标系中无人机的位置，g为重力加速度，T为油门位置，D为气动阻力，W为无人机的重量，n为过载，φ为俯仰角。取状态变量为(v，γ，χ，x，y，z)，控制输入为(T，n，φ)。

2、三维编队重构最优时间控制的数学描述

假设某编队由N架无人机组成，控制向量作用初始时刻t＝0，终端时刻t＝T，定义编队内第i架无人机的控制输入为(油门、过载、俯仰角，仿真过程中把油门位置转换成推力)编队的控制输入向量则编队的连续控制输入向量U可进一步表述为定义编队内第i架无人机的状态变量xi＝(vi，γi，χi，xi，yi，zi)。因此，编队系统的状态变量定义为编队系统的运动方程可以表述为：

$\stackrel{·}{X}\left(t\right)=f(t,X\left(t\right),U\left(t\right))---\left(7\right)$

定编队连续的控制输入U以及编队初始状态X(0)＝X0，则在t∈(0，T]任意时刻编队的状态均可由下式唯一确定：

$X\left(t\right)=X\left(0\right)+{\int }_0^{t^{-}}f(\tau ,X\left(\tau \right),U\left(\tau \right))\mathrm{d\tau }---\left(8\right)$

如果给定了初始状态，则X(t)仅仅由U唯一确定，也可用X(t|U)表述。

通常，代价函数的标准形式可以表示为

约束条件可表述为：

对于编队系统最优时间控制问题可以表述为：寻找一个连续的控制输入U和终端时刻T使得编队系统代价函数J(U)最小，也即：

$\underset{u_1,T}{\min }···\underset{u_N,T}{\min }J\left(U\right)---\left(11\right)$

编队系统代价函数J(U)可以表述为：

J(U)＝T                                                         (12)

控制容许约束为：

$U_{\min }\le U\left(t\right)\le U_{\max },\forall t\in [0,T),0<T---\left(13\right)$

自由终端约束为：

$=0$

式中：m∈{1，…，N}，定义第m架无人机作为编队的中心无人机(参考无人机)；[xim，yim，zim]T为终端T时刻编队内第i架无人机相对于编号为m的中心无人机期望的相对坐标值。

定义任意两架无人机之间距离为di，j(xi(t)，xj(t))(其中，i，j∈{1，…，N})，其表达式为：

为了防止无人机相撞，编队内任意两架无人机之间距离di，j(xi(t)，xj(t))必须大于安全防撞距离Dsafe：

$d^{i,j}(x_i\left(t\right),x_j\left(t\right))\ge D_{\mathrm{safe}},\forall t\in [0,T],{\forall }_{i\ne j}i,j\in \{1,···,N\}---\left(16\right)$

为了确保编队内能正常实时通讯，实时更新作战态势，任意两架无人机之间距离di，j(xi(t)，xj(t))必须小于通讯保障距离Dcomm：

$d^{i,j}(x_i\left(t\right),x_j\left(t\right))\le D_{\mathrm{comm}},\forall t\in [0,T],{\forall }_{i\ne j}i,j\in \{1,···,N\}---\left(17\right)$

综上，编队系统的最优时间控制问题的数学描述为：在满足约束条件(7)(13)(14)(16)(17)约束条件下，寻找一个连续的控制输入U和终端时刻T使得(11)(12)两式成立。

3、基于PSO和GA的无人机三维编队重构程序设计原理

PSO和GA算法是一种智能化的全局寻优算法，利用PSO和GA算法解决优化问题不受目标函数是否为线性的限制，适合解决三维编队重构最优控制问题。然而编队内各个飞行单元的控制输入均为连续量，PSO和GA算法无法求解出连续的控制输入。因此，首先将编队内各个飞行单元的控制输入进行分段线性化处理，用近似的分段线性化控制输入代替连续的控制输入，然后采用PSO和GA算法进行寻优，求出分段线性化控制输入。

控制输入的分段线性化：控制输入的作用时间T被划分为np等分，对于编队内第i架无人机，定义一个ri×np维常数集合则在时间T内，第i架无人机的连续控制输入作用ui可以采用常量分段函数近似地表述成下式：

上式中，χj(t)由下式给定：

${\chi }_j\left(t\right)=\left\{\begin{array}{cc}1& (j-1)·\Delta t_p\le t\le j·\Delta t_p\\ 0& \mathrm{otherwise}\end{array}\right.---\left(19\right)$

定义编队的分段线性化常系数集合为编队系统的近似控制输入集合为寻找最优控制输入集合使代价指标函数最小的问题就转化为寻找最优常系数集合Ω的问题。

近似参数化：控制输入经过近似处理后，寻找最优控制输入集合U和T使代价指标函数最小的问题近似地等价于寻找最优常数参数集合Ω和Δtp。因此，三维编队重构最优控制的代价函数可近似表述为：

$J\cong \underset{\Omega ,\Delta t_p}{\min }(n_p·\Delta t_p)---\left(20\right)$

控制容许约束可近似表述为：

${\left(u_{\min }\right)}_i\le {\sigma }_j^i\le {\left(u_{\max }\right)}_i,\forall i\in \{1,···,N\},\forall j\in \{1,···,n_p\},0<\Delta t_p---\left(21\right)$

自由终端约束可近似表述为：

系统状态方程近似表述为：

$\stackrel{·}{X}\left(t\right)\cong f(t,X\left(t\right),\hat{U}(t;n_p,\Omega ))---\left(23\right)$

其他约束条件表达式不变。

分段线性化控制输入U以后，即可采用PSO和GA算法解决三维编队重构最优控制问题。

将编队的控制输入常数集合(其中ri为第i架无人机控制输入的维数)与分段区间Δtp组合，只要确定了这些参数，就可解出编队控制输入。这样，无人机三维编队重构实际上转化成了在N×np×ri+1维上寻找使代价函数最优的问题。定义三维编队重构最优时间控制的扩展代价函数为：

$+{\sigma }_{\mathrm{ij}}^{\prime }·\max (0,d^{i,j}(x_i\left(t\right),x_j\left(t\right))-D_{\mathrm{comm}})\left]\right\}$

式中：σij和σij′分别为安全防撞距离约束和通讯保障距离约束的惩罚常系数；σ*为终端约束(22)的惩罚常系数；为(22)式左端的表达形式，即终端T时刻编队内各无人机状态与期望状态的误差的平方和。

实际应用中GA的代价函数取为1/Jextend，PSO代价函数取为Jextend。

基于以上说明，就可以用PSO和GA算法求解无人机三维编队重构问题。GA算法进行粗搜索，PSO算法精度较高，用它进行细搜索。再用PSO指导GA搜索全局最优解。任意给定初始状态，指定终端时刻的相对状态，基于本发明提出的算法，可找到最优控制输入，驱动各无人机达到指定编队队形。具体步骤如下：

步骤1：初始化数目为M的微粒群，任意给定编队内各无人机的初始状态，指定终端时刻编队无人机的相对状态。给定微粒群算法的参数c1，c2，w。c1，c2称为学习因子，w为惯性权重。设置遗传算法参数Pc，Mute。Pc为交叉概率，取值范围为〔0.7，0.9〕；Mute为变异概率，取值范围为〔0，0.1〕。

步骤2：计算微粒代价函数并保留最优微粒的位置和代价函数。

步骤3：用混合概率P(小于1)将微粒群分为两个子群。一个子群为粒子群，另一个子群为染色体种群。

步骤4：对M*P子群使用PSO算法。任意给定初始解xi，初始速度vi，历史最优位置pbesti，全局最优位置gbest，计算出相应的代价函数。由下式更新粒子的速度和位置信息：

$\left\{\begin{array}{c}{v}_i={\mathrm{wv}}_i+c_1·r_1·({\mathrm{pbest}}_i-x_i)+c_2·r_2·(\mathrm{gbest}-x_i)\\ x_i=x_i+v_i\end{array}\right.---\left(25\right)$

其中，r1，r2为随机数。把新位置得到的解代入代价函数，求出在新位置处代价函数的值，若新位置处代价函数小于历史最优位置处代价函数，则历史最优位置更新为新位置，否则不做任何修改。若新位置处代价函数小于全局最优位置处代价函数，则全局最优位置更新为新位置，否则，不做任何修改。

步骤5：对剩余的子群(染色体)使用GA算法。随机产生初始解，并计算出初始适应度。遗传算法包含三个重要算子，分别为选择算子、交叉算子和变异算子，分别介绍如下。

5.1选择算子

选择采用数学轮盘赌的方法按每个染色体的适应度进行，这种方法确保了染色体被选择的概率与其适应度成正比。

5.2交叉算子

由于采用的是浮点数编码方式，所以将使用以算术交叉为基础的交叉算子。交叉算子为：

$P_1^{\mathrm{new}}=\omega ·P_1+(1-\omega )·P_2$

$P_2^{\mathrm{new}}=\omega ·P_2+(1-\omega )·P_1---\left(26\right)$

式中：P1和P2为从种群中随机选择的两个父个体，P1new、P2new为通过交叉运算子运算后产生的子代对应新个体。ω为参数，ω∈[0，1]。

5.3变异算子

采用自适应加速变异算子对交叉算子作用后的群体的染色体进行变异操作，算法如下：

$P_i^j(k+1)=P_i^j\left(k\right)+\beta ·\Delta P_i^j\left(k\right)+\rho ·{\mathrm{sP}}_i^j\left(k\right)$

$\Delta P_i^j\left(k\right)=(P_i^{\mathrm{best}}\left(k\right)-P_i^j\left(k\right))·\left|N\left(\mathrm{0,1}\right)\right|---\left(27\right)$

${\mathrm{sP}}_i^j(k+1)=\beta ·{\mathrm{acc}}^j\left(k\right)·\Delta P_i^j\left(k\right)+\rho ·{\mathrm{sP}}_i^j\left(k\right)$

式中：Pij(k)为第k代中第j个染色体的第i个分量，Pbest(k)为第k代中最好的个体，ρ和β分别为学习速率和惯量常数，N(0，1)为正态随机分布函数，sPij(k)为进化趋势，accj(k)定义为：

步骤6：比较PSO算法得到的最优解与GA算法得到的最优解优劣，若PSO算法的解优于GA算法的最优解，则当前最优解为PSO算法的最优解，并把GA算法的最优解替换为PSO算法的最优解。否则，当前最优解为GA算法的最优解，并把PSO算法的最优解替换为GA算法的最优解。

步骤7：步骤(2)～步骤(6)重复执行，直到满足结束条件。

本发明提出了一种基于PSO和GA的多无人机三维编队重构方法，其优点及功效在于：与传统的多无人机三维编队重构方法相比，该发明所提出的方法具有较好的实时性和快速性。该方法不仅可以解决单编队重构的最优时间控制问题，还可以解决单编队重构的最小能量控制、最短时间与最小能量综合控制等问题，也可以解决多编队重构、多机协同等复杂系统集中控制的优化问题。本发明是解决复杂动态环境下多无人机三维编队重构的有效技术途径，同时，本发明也可应用于复杂环境下的空间机器人三维编队重构等航天技术领域。

(四)附图说明

图1代价函数随迭代次数的关系

图2三维编队重构最优控制轨迹图

图3三维编队重构水平面运动轨迹图

图4三维编队重构高度变化曲线

图5三维编队重构过程无人机间距变化曲线

图6三维编队重构过程无人机油门作用曲线

图7三维编队重构过程无人机过载变化曲线

图8三维编队重构过程无人机俯仰角作用曲线

图中标号及符号说明如下：

“o”——表示初始时刻无人机的位置

“*”——表示终端时刻无人机的位置。

J——代价函数

Itertation——迭代次数

T——推力

t——时间

d——意两架无人机的间距

n——过载

Φ——俯仰角

(五)具体实施方式

下面结合附图和实施例，对本发明的技术方案做进一步的说明。

一种基于PSO和GA的多无人机三维编队重构方法，内容具体如下：

1、无人机的数学模型

建立无人机的数学模型是实现三维编队重构的必要前提。在对无人机分析的基础上，公式(1)-(6)给出了其数学模型。基于PSO和GA的无人机三维编队重构是以此模型来进行程序设计的。

$\stackrel{·}{v}=g[(T-D)/W-\sin \gamma ]---\left(1\right)$

$\stackrel{·}{\gamma }=(g/v)(n\cos \phi -\cos \gamma )---\left(2\right)$

$\stackrel{·}{\chi }=\left(\mathrm{gn}\sin \phi \right)/\left(v\cos \gamma \right)---\left(3\right)$

$\stackrel{·}{x}=v\cos \gamma \cos \chi ---\left(4\right)$

$\stackrel{·}{y}=v\cos \gamma \sin \chi ---\left(5\right)$

$\stackrel{·}{z}=-v\sin \gamma ---\left(6\right)$

式中：v为无人机的速度，γ为飞行航迹角，χ为航向角，x，y，z表示在地面坐标系中无人机的位置，g为重力加速度，T为油门位置，D为气动阻力，W为无人机的重量，n为过载，φ为俯仰角。取状态变量为(v，γ，χ，x，y，z)，控制输入为(T，n，φ)。

2、三维编队重构最优时间控制的数学描述

假设某编队由N架无人机组成，控制向量作用初始时刻t＝0，终端时刻t＝T，定义编队内第i架无人机的控制输入为(油门、过载、俯仰角，仿真过程中把油门位置转换成推力)编队的控制输入向量则编队的连续控制输入向量U可进一步表述为定义编队内第i架无人机的状态变量xi＝(vi，γi，χi，xi，yi，zi)。因此，编队系统的状态变量定义为编队系统的运动方程可以表述为

$\stackrel{·}{X}\left(t\right)=f(t,X\left(t\right),U\left(t\right))---\left(7\right)$

定编队连续的控制输入U以及编队初始状态X(0)＝X0，则在t∈(0，T]任意时刻编队的状态均可由下式唯一确定：

$X\left(t\right)=X\left(0\right)+{\int }_0^{t^{-}}f(\tau ,X\left(\tau \right),U\left(\tau \right))\mathrm{d\tau }---\left(8\right)$

如果给定了初始状态，则X(t)仅仅由U唯一确定，也可用X(t|U)表述。

通常，代价函数的标准形式可以表示为

约束条件可表述为：

对于编队系统最优时间控制问题可以表述为：寻找一个连续的控制输入U和终端时刻T使得编队系统代价函数J(U)最小，也即：

$\underset{u_1,T}{\min }···\underset{u_N,T}{\min }J\left(U\right)---\left(11\right)$

编队系统代价函数J(U)可以表述为：

J(U)＝T                                           (12)

控制容许约束为：

$U_{\min }\le U\left(t\right)\le U_{\max },\forall t\in [0,T),0<T---\left(13\right)$

自由终端约束为：

$=0$

式中：m∈{1，…，N}，定义第m架无人机作为编队的中心无人机(参考无人机)；[xim，yim，zim]T为终端T时刻编队内第i架无人机相对于编号为m的中心无人机期望的相对坐标值。

定义任意两架无人机之间距离为di，j(xi(t)，xj(t))(其中，i，j∈{1，…，N})，其表达式为：

为了防止无人机相撞，编队内任意两架无人机之间距离di，j(xi(t)，xj(t))必须大于安全防撞距离Dsafe：

$d^{i,j}(x_i\left(t\right),x_j\left(t\right))\ge D_{\mathrm{safe}},\forall t\in [0,T],{\forall }_{i\ne j}i,j\in \{1,···,N\}---\left(16\right)$

为了确保编队内能正常实时通讯，实时更新作战态势，任意两架无人机之间距离di，j(xi(t)，xj(t))必须小于通讯保障距离Dcomm：

$d^{i,j}(x_i\left(t\right),x_j\left(t\right))\le D_{\mathrm{comm}},\forall t\in [0,T],{\forall }_{i\ne j}i,j\in \{1,···,N\}---\left(17\right)$

综上，编队系统的最优时间控制问题的数学描述为：在满足约束条件(7)(13)(14)(16)(17)约束条件下，寻找一个连续的控制输入U和终端时刻T使得(11)(12)两式成立。

3、基于PSO和GA的无人机三维编队重构程序设计原理

PSO和GA算法是一种智能化的全局寻优算法，利用PSO和GA算法解决优化问题不受目标函数是否为线性的限制，适合解决三维编队重构最优控制问题。然而编队内各个飞行单元的控制输入均为连续量，PSO和GA算法无法求解出连续的控制输入。因此，首先将编队内各个飞行单元的控制输入进行分段线性化处理，用近似的分段线性化控制输入代替连续的控制输入，然后采用PSO和GA算法进行寻优，求出分段线性化控制输入。

控制输入的分段线性化：控制输入的作用时间T被划分为np等分，对于编队内第i架无人机，定义一个ri×np维常数集台则在时间T内，第i架无人机的连续控制输入作用ui可以采用常量分段函数近似地表述成下式：

上式中，χj(t)由下式给定：

${\chi }_j\left(t\right)=\left\{\begin{array}{cc}1& (j-1)·\Delta t_p\le t\le j·\Delta t_p\\ 0& \mathrm{otherwise}\end{array}\right.---\left(19\right)$

定义编队的分段线性化常系数集合为编队系统的近似控制输入集合为寻找最优控制输入集合使代价指标函数最小的问题就转化为寻找最优常系数集合Ω的问题。

近似参数化：控制输入经过近似处理后，寻找最优控制输入集合U和T使代价指标函数最小的问题近似地等价于寻找最优常数参数集合Ω和Δtp。因此，三维编队重构最优控制的代价函数可近似表述为：

$J\cong \underset{\Omega ,\Delta t_p}{\min }(n_p·\Delta t_p)---\left(20\right)$

控制容许约束可近似表述为：

${\left(u_{\min }\right)}_i\le {\sigma }_j^i\le {\left(u_{\max }\right)}_i,\forall i\in \{1,···,N\},\forall j\in \{1,···,n_p\},0<\Delta t_p---\left(21\right)$

自由终端约束可近似表述为：

系统状态方程近似表述为：

$\stackrel{·}{X}\left(t\right)\cong f(t,X\left(t\right),\hat{U}(t;n_p,\Omega ))---\left(23\right)$

其他约束条件表达式不变。

分段线性化控制输入U以后，即可采用PSO和GA算法解决三维编队重构最优控制问题。

将编队的控制输入常数集合(其中ri为第i架无人机控制输入的维数)与分段区间Δtp组合，只要确定了这些参数，就可解出编队控制输入。这样，无人机三维编队重构实际上转化成了在N×np×ri+1维上寻找使代价函数最优的问题。定义三维编队重构最优时间控制的扩展代价函数为：

$+{\sigma }_{\mathrm{ij}}^{\prime }·\max (0,d^{i,j}(x_i\left(t\right),x_j\left(t\right))-D_{\mathrm{comm}})\left]\right\}$

式中：σij和σij′分别为安全防撞距离约束和通讯保障距离约束的惩罚常系数；σ*为终端约束(22)的惩罚常系数；为(22)式左端的表达形式，即终端T时刻编队内各无人机状态与期望状态的误差的平方和。

实际应用中GA的代价函数取为1/Jextend，PSO代价函数取为Jextend。

基于以上说明，就可以用PSO和GA算法求解无人机三维编队重构问题。GA算法进行粗搜索，PSO算法精度较高，用它进行细搜索。再用PSO指导GA搜索全局最优解。任意给定初始状态，指定终端时刻的相对状态，基于本发明提出的算法，可找到最优控制输入，驱动各无人机达到指定编队队形。

下面以某多无人机编队为例，任意给定编队的初始状态和终端时刻编队内各无人机的相对状态，指定PSO算法和GA算法参数，运用本发明提出的方法总能找到一组最优解，满足代价函数要求及编队内系统的各种约束条件，实现三维编队重构。具体步骤如下：

步骤1：初始化：给定编队内各无人机的初始状态为，指定终端时刻编队无人机的相对状态。微粒群数目M＝240，微粒群算法的参数c1＝2，c2＝2，w＝0.9。设置遗传算法参数Pc＝0.9，Mute＝0.1。最大迭代次数Ncmax＝500。

步骤2：计算微粒代价函数并保留最优微粒的位置和代价函数。

步骤3：用混合概率P(P＝0.5)将微粒群分为两个子群。一个子群为粒子群，另一个子群为染色体种群。

步骤4：对子群数目为120的粒子使用PSO算法。任意给定初始解xi，初始速度vi，历史最优位置pbesti，全局最优位置gbest，计算出相应的代价函数。由下式更新粒子的速度和位置信息：

$\left\{\begin{array}{c}{v}_i=0.8v_i+2·r_1·({\mathrm{pbest}}_i-x_i)+2·r_2·(\mathrm{gbest}-x_i)\\ x_i=x_i+v_i\end{array}\right.$

把新位置得到的解代入代价函数，求出在新位置处代价函数的值，若新位置处代价函数小于历史最优位置处代价函数，则历史最优位置更新为新位置，否则不做任何修改。若新位置处代价函数小于全局最优位置处代价函数，则全局最优位置更新为新位置，否则，不做任何修改。

步骤5：对剩余的子群(染色体)使用GA算法。随机产生初始解，并计算出初始适应度。遗传算法包含三个重要算子，分别为选择算子、交叉算子和变异算子，分别介绍如下。

5.1选择算子

选择采用数学轮盘赌的方法按每个染色体的适应度进行，这种方法确保了染色体被选择的概率与其适应度成正比。

5.2交叉算子

由于采用的是浮点数编码方式，所以将使用以算术交叉为基础的交叉算子。交叉算子为：

$P_1^{\mathrm{new}}=0.8·P_1+(1-0.8)·P_2$

$P_2^{\mathrm{new}}=0.8·P_2+(1-0.8)·P_1$

5.3变异算子

采用自适应加速变异算子对交叉算子作用后的群体的染色体进行变异操作，算法如下：

$P_i^j(k+1)=P_i^j\left(k\right)+0.85·\Delta P_i^j\left(k\right)+1.45·{\mathrm{sP}}_i^j\left(k\right)$

$\Delta P_i^j\left(k\right)=(P_i^{\mathrm{best}}\left(k\right)-P_i^j\left(k\right))·\left|N\left(\mathrm{0,1}\right)\right|$

${\mathrm{sP}}_i^j(k+1)=0.85·{\mathrm{acc}}^j\left(k\right)·\Delta P_i^j\left(k\right)+2·{\mathrm{sP}}_i^j\left(k\right)$

步骤6：比较PSO算法得到的最优解与GA算法得到的最优解优劣，若PSO算法的解优于GA算法的最优解，则当前最优解为PSO算法的最优解，并把GA算法的最优解替换为PSO算法的最优解。否则，当前最优解为GA算法的最优解，并把PSO算法的最优解替换为GA算法的最优解。

步骤7：步骤(2)～(6)重复执行，直到满足最大迭代次数。

本例指定终端时刻队形为“＞”，图1-图8给出了用本发明提出的方法的仿真结果。

图1棕色曲线表示用PSO和GA算法代价函数曲线，青色表示PSO代价函数曲线，可见运用PSO和GA算法要远远好于单独使用PSO算法。图2给出了无人机三维编队重构轨迹，由图可见无人机满足要求。图3给出了水平方向无人机的运动轨迹。图4给出了各无人机的高度变化曲线。图5给出了三维编队重构过程中无人机的间距变化曲线。图6三维编队重构过程中油门作用曲线。图7给出了三维编队重构过程中过载作用曲线。图8给出了三维编队重构过程中俯仰角作用曲线。