CN112363532A - 一种基于quatre算法的多无人机同时起飞集结方法 - Google Patents

Abstract

本发明涉及一种基于QUATRE算法的多无人机同时起飞集结方法，采用模型预测控制模式结合长僚机模式实现无人机编队的飞行控制方法，将单架无人机在等待航线上起飞集结方法扩展成为多架无人机同时起飞集结方法。本发明采用多无人机同时起飞分组集结方式，该方法提高了起飞集结的速度，减少了无人机编队集结的时间和能耗。改进的QUATRE算法提高了算法的求解速度性能，为无人机编队实时性提供保障。

Description

一种基于QUATRE算法的多无人机同时起飞集结方法

技术领域

本发明涉及无人机编队控制领域，特别是涉及一种基于QUATRE算法的多无人机同时起飞集结方法。

背景技术

随着无人机编队技术的飞速发展，多无人机编队执行任务成为目前无人机应用的主流。然而，在实际环境下，无人机编队在执行特定任务时，所必须要做的事是从起飞场地起飞集结。现在无人机编队的起飞方式基本属于一架或者两架无人机同时起飞并且直线飞行，直到编队中所有无人机全部起飞后，执行集结功能，将无人机编队集结到一个动态的区域。

文献《舰载无人机编队控制技术研究》和设计出了一种分时转弯田径圆的等待航线完成无人机编队起飞集结。上述论文存在每次起飞单个无人机，整个编队全部起飞的时间较长。

文献《Round Tracking and Improved Differential Evolution Solution forRapid Prototyping of UAV Formation》设计了一种基于圆形轨迹的快速起飞集结算法。该算法将飞机集结的航线由直线形转变为圆形，缩小了无人机编队的集结时间。但是每次起飞单架无人机，仍然存在着起飞耗时较长的问题。

上述两篇文献都采用等待航线缩小单次起飞导致无人机间的距离，但是由于编队无人机数量较多，起飞方式又是单架起飞，这两种方法所消耗的时间和编队能源较多。在当下的应用中需要一种多无人机同时起飞的快速集结方法，该方法可以减少编队执行任务前在起飞集结上的时间消耗以及能源消耗。

发明内容

本发明解决的技术问题是：本发明的目的在于提供一种求解速度快、多无人机同时起飞的编队集结方法。

本发明的技术方案是：一种基于QUATRE算法的多无人机同时起飞集结方法，包括如下步骤：

步骤1，依据下述公式建立无人机数学模型，获取无人机的运动方程。

假设固定翼无人机的飞行过程中无侧滑及高度变化，速度由加速度和航迹方位角及航迹倾斜角控制，考虑在地面坐标系下无人机质心的运动方程，运用欧拉积分法对固定翼无人机的运动方程离散近似为：

式中,γ,χ分别为加速度、航迹方位角和航迹倾斜角；x,y,h分别为飞机质心位置在地面坐标系中的投影坐标；a,γ′,x′分别为加速度，航迹倾斜角速度和航迹方位角速度,dt 为采样周期；

取无人机i在k时刻的状态变量与控制变量为:X_i(k)＝[x_i(k) y_i(k) h_i(k)V(k)γ_i(k) _xi(k)]和U_i(k)＝[a_i(k)χ′_i(k)γ′_i(k)],此时无人机的运动方程即为：

X_i(k+1)＝f(X_i(k)，U_i(k)) (2)

步骤2:无人机小组组长起飞，无人机编队以起飞场地的跑道数为除数分成多个小组，使每个小组包含不大于跑道数的无人机数目；每个无人机小组分配一个组长，编队队长跟踪预设的圆形轨道，队长采用轨迹跟踪算法，逐渐向预设轨道移动并且最终跟随轨道；

步骤3:小组组员集结控制采用DMPC结构，依据QUATRE算法求解DMPC框架中的最优代价函数值，得到无人机起飞小组组员的集结控制量；

步骤4：依据步骤3得到的无人机起飞小组组员的集结控制量，无人机小组将在圆形轨道上完成对接，形成最终的无人机编队起飞集结队形；

本发明进一步的方案是：所述的步骤3小组组员集结控制采用DMPC结构的运动方程可以表示为：

其中DMPC框架中的无人机小组组员的整体代价即是组员数目架无人机的代价加权和：

本发明进一步的方案是：轨迹跟踪算法具体如下：

①通过设定的圆轨迹参数以及无人机位置，求得无人机在圆形轨迹上的投影点q；②偏移角度依据上述的投影点q和无人机位置在投影点q的偏移角度λ，求解得出此无人机在圆形轨迹上与投影点q偏移角度为λ的偏移采样点s；

③依据无人机方向速度与无人机到偏移采样点s求得无人机速度方向与无人机到偏移采样点s的夹角ψ，将ψ带入式(1)求得无人机下一刻的控制量u_i(k+1)：

u_i(k+1)＝[0，χ′(k+1)，0] (3)

④控制量u_i(k+1)为无人机下一步轨迹跟踪控制量。

本发明进一步的方案是：：所述步骤3中DMPC结构下的小组组员行为可以由每架UAV来联合描述，整个的小组组员运动方程可以表示为：

式中，

为无人机编队在k时刻的状态向量和控制向量。在控制输入量已知时(即

和

给定时),时刻k＝n*dt的编队状态可以根据式(5)递推出；

编队中各个无人机是相对独立进行控制，无人机编队的整体代价即是组员数目架无人机的代价加权和：

本发明进一步的方案是：：组员数目架无人机的代价加权和包括：集结队形代价函数、队内避免碰撞代价函数和整体性代价函数。

本发明进一步的方案是：：所述的集结队形代价函数：

J_p＝||X₁-X_i-M_1i||*m (8)

式中，X₁表示长机的位置信息，X_i表示小组内无人机i位置信息，M_1i表示长机与无人机组员i的相对队形信息，m为系数矩阵。

本发明进一步的方案是：：所述的避免碰撞代价函数：

为使无人机小组无安全事故，队内避免碰撞代价函数采取为下式：

J_li＝min(len(X_i，X_j))(i不是组长，j∈[1，n]&j≠i)

式中，X_i表示无人机i的位置信息，X_j表示无人机j的位置信息，d_ij表示无人机i 和无人机j间的距离，D为无人机碰撞半径，避免碰撞代价函数用于使无人机小组无安全事故，队内避免碰撞。

本发明进一步的方案是：：所述的整体性代价函数：

为了无人机小组起飞集结具有整体一致性，要求无人机组员状态与组长状态相似。取代价函数为下式：

J_z＝k1*|V_i-V₁|+k2*|χ_i-χ₁| (10)

式中，V_i表示无人机i的速度，V₁表示长机的速度，χ₁表示长机的航向角，χ_i表示无人机i的航向角。

其中，约束函数式(8)和(10)即为长-僚机模式中的僚机对于长机速度、航向角和位置信息的跟踪形式的融合。在模型预测控制中融入了长-僚机模式的队形可控。

(6)式中X_i(k)和U_i(k)分别为k时刻下的N步预测状态集和N步预测输入控制集，X₁(k)为组长的N步预测状态集。

发明效果

本发明的技术效果在于：

1.本发明采用模型预测控制模式结合长僚机模式实现无人机编队的飞行控制方法，将单架无人机在等待航线上起飞集结方法扩展成为多架无人机同时起飞集结方法。

2.本发明采用改进的QUATRE算法对DMPC中的代价函数进行优化求解，改进的QUATRE算法比现在常用的DE算法具有求解速度快、调节参数量少等优点。

本发明核心创新点是(1)多无人机同时起飞集结、(2)改进的QUATRE算法，与文献《舰载无人机编队控制技术研究》和文献《Round Tracking and Improved DifferentialEvolution Solution for Rapid Prototyping of UAV Formation》相比，在起飞方面有本质的不同，上述两篇文献中都采用的单无人机起飞等待航线集结方式，而本发明是采用多无人机同时起飞分组集结方式，该方法提高了起飞集结的速度，减少了无人机编队集结的时间和能耗。改进的QUATRE算法提高了算法的求解速度性能，为无人机编队实时性提供保障。

附图说明

图1为轨迹跟踪算法示意图，表明了轨迹跟踪算法中的各种参数；

图2为模型预测控制图，通过多次采样和优化，得到的整个控制时域的最优控制序列；

图3为QUATRE算法种群和个体结构与1控制量之间的关系示意图，表明了无人机控制量如何转换成为个体以及种群；

图4为10架无人机从起点(0,0,0)起飞并且集结于以起点为中心的圆形轨道的整体轨迹仿真图；

图5为10架无人机起飞集结去除轨迹后部分时间节点仿真图；

图6为两组无人机起飞小组之间的距离图，反应了无人机编队对接完成的时间；

图7第10号无人机组员与队形位置的相对距离图，反应了起飞小组的队形收敛时间和稳定性；

图8为5号机采用各算法在集结飞行中的平均迭代次数对比图，表现了Pre-DMPC-QUATRE算法在迭代速率上的优势；

图9为10号无人机组员与自身对应位置的距离变化图，表现了 Pre-DMPC-QUATRE算法在无人机编队的收敛速度与其他算法相当。

具体实施方式

参见图1—图9，本发明的目的是这样实现的，一种基于QUATRE算法的多无人机同时起飞集结方法，其特征是：至少包括如下步骤：

步骤1，依据下式建立编队数学模型，将固定翼无人机的运动方程离散近似为：

取无人机i在k时刻的状态变量与控制变量为:X_i(k)＝[x_i(k) y_i(k) h_i(k) V(k)γ_i(k) x_i(k)]和U_i(k)＝[a_i(k)χ′_i(k)γ′_i(k)],此时无人机i的运动方程即为：

X_i(k+1)＝f(X_i(k)，U_i(k))；

步骤2，无人机小组组长UAV_z起飞，无人机编队以起飞场地的跑道数为除数分成多个小组(每个小组包含不大于跑道数的无人机数目)。每个无人机小组分配一个组长UAV_zi(状态信息x_zi,y_zi,h_zi,v_zi,γ_zi,χ_zi)，编队队长跟踪预设的圆形轨道 (C(x,y,h)＝R²)，组长采用轨迹跟踪算法，求得组长控制量 U_zi(k)＝[a_zi(k)χ'_zi(k)γ'_zi(k)]，该控制量驱动组长逐渐向预设轨道移动并且最终跟随轨道((x_zi,y_zi,h_zi)∈C(x,y,h))。

步骤3，小组组员集结，无人机小组的控制系统可以看作多个小的独立的无人机控制子系统组成的大系统，其控制结构可以采用DMPC(运动方程组为

代价函数为

小组的集结问题即被简化为单机规模的小组组员数目个局部优化问题(u＝argmin_u(J_i(x_i,u_i)))。优化的目标即是小组组员集结代价函数(J(x,u))，用改进的QUATRE算法求解DMPC问题，得到无人机起飞小组组员的集结控制量

该集结控制量驱动小组组员与小组组长形成以组长为顶点的三角形(M矩阵为小组队形矩阵)。

步骤4，无人机小组对接，当所有无人机小组都成功起飞集结在空中的预设轨道时，无人机小组将在圆形轨道上完成对接，形成最终的无人机编队起飞集结队形。

作为优选，步骤3中所述在DMPC结构下的小组组员的行为可以由每架组员 UAV来联合描述，整个的小组组员运动方程可以表示为：

无人机小组组员的整体代价即是组员数目架无人机的代价加权和：

无人机起飞小组的整体集结问题可以简化为单机规模的组员数目个局部优化问题：

作为优选，步骤3中所述小组集结代价函数分为集结队形代价函数、队内避免碰撞代价函数和整体性代价函数三个个部分，为了无人机编队能够以较高的效率达到集结队形，选取代价函数为：

J_p＝||X₁-X_i-M_1i||*m

为使无人机小组无安全事故，队内避免碰撞代价函数采取为下式：

J_li＝min(len(X_i，X_j))(i不是组长，j∈[1，n]&j≠i)

为了使无人机小组整体保持一致，整体性代价函数采取为下式：

J_z＝k1*|V_i-V₁|+k2*|χ_i-χ₁|

作为优选，步骤3中所述改进的QUATRE算法求解DMPC问题，标准QUATRE 算法主要包括仿射变换操作和选择操作。QUATRE算法将DMPC方法N步预测所得的预测值作为进化的种群个体，每三个基因数据一组对应控制输入 U_i(k)＝[a(k)，x(k)，γ(k)]的三个分量。在QUATRE算法过程中，种群通过放射变换操作，对个体所带有的每组基因进行交叉变异，得到变异种群。变异种群通过选择操作留下对代价函数适应度更好地个体，从而得到下一代种群。QUATRE 算法迭代速度慢的原因之一是每次迭代的种群都是随机生成的，没有继承上一轮迭代的任何成果。对于模型预测控制而言，N步的预测计算采纳第一步，其后的 N-1步对于下一轮的N-1步而言已经是优化过的控制参数，对于代价函数的适应程度远高于初始种群。因而在初始化种群中插入上一轮的控制输入量后N-1步所形成的新的N步控制输入，人工添加优良基因。t轮迭代后最佳个体为

模型预测控制将会采用

所表示的三个分量控制无人机完成一次采样周期的飞行。插入优化基因即保留t轮中的

为其添加一个随机控制输入

即t+1轮的种群中添加优化个体

这样，改进的QUATRE算法在初始解空间中就有着最优个体附近方向，并且在高纬度下QUATRE算法的寻求解时各个方向上的概率是相等的，那么基于DMPC框架的QUATRE改进算法(Pre-DMPC-QUATRE)丢失最优解方向而落入局部优解的可能性被减弱。优化的个体代价值远远低于其他随机个体代价，通过种群竞争的关系加快种群的优化，减少算法迭代。

表1步骤中各个符号及其含义

为使本发明的目的、技术方案和优点更加清楚，下面结合附表1定义的符号内容说明本发明的具体实施过程：一种基于改进QUATRE算法的无人机编队起飞集结方法，至少包含以下步骤：

步骤1，依据下述公式建立无人机数学模型，获取无人机的运动方程。

假设固定翼无人机的飞行过程中无侧滑及高度变化，速度由加速度和航迹方位角及航迹倾斜角控制，考虑在地面坐标系下无人机质心的运动方程，运用欧拉积分法对固定翼无人机的运动方程离散近似为：

式中,γ,χ分别为加速度、航迹方位角和航迹倾斜角；x,y,h分别为飞机质心位置在地面坐标系中的投影坐标；a,γ′,x′分别为加速度，航迹倾斜角速度和航迹方位角速度,dt为采样周期。

取无人机i在k时刻的状态变量与控制变量为:X_i(k)＝[x_i(k) y_i(k) h_i(k) V(k)γ_i(k) x_i(k)]和U_i(k)＝[a_i(k)χ′_i(k)γ′_i(k)],此时无人机的运动方程即为：

X_i(k+1)＝f(X_i(k)，U_i(k)) (2)

步骤2：无人机小组组长起飞。

无人机编队以起飞场地的跑道数为除数分成多个小组(每个小组包含不大于跑道数的无人机数目)。每个无人机小组分配一个组长UAV_zi(状态信息 x_zi,y_zi,h_zi,v_zi,γ_zi,χ_zi)，编队队长跟踪预设的圆形轨道(C(x,y,h)＝R²)，组长采用轨迹跟踪算法(RPTS)，求得组长控制量U_zi(k)＝[a_zi(k)χ'_zi(k)γ'_zi(k)]，该控制量驱动组长逐渐向预设轨道移动并且最终跟随轨道。

轨迹跟踪算法具体如下：

①通过设定的圆轨迹参数以及无人机位置，求得无人机在圆形轨迹上的投影点q；

②偏移角度依据上述的投影点q和无人机位置在投影点q的偏移角度λ，求解得出此无人机在圆形轨迹上与投影点q偏移角度为λ的偏移采样点s；如在图1中，一架无人机位于圆形轨迹之外，将无人机位置在投影点q的偏移角度λ设定为0.2，投影点q结合参数λ，求解得出此无人机在圆形轨迹上与投影点q偏移角度为λ的偏移采样点s；③依据无人机方向速度与无人机到偏移采样点s求得无人机速度方向与无人机到偏移采样点s的夹角ψ，将ψ带入式(1)求得无人机下一刻的控制量u_i(k+1)：

u_i(k+1)＝[0，χ′(k+1)，0] (3)

④控制量u_i(k+1)为无人机下一步轨迹跟踪控制量。

步骤3:采用DMPC结构小组组员集结控制，依据QUATRE算法求解DMPC框架中的最优代价函数值，得到无人机起飞小组组员的集结控制量。

模型预测控制(model predictive control,MPC)又被称为滚动时域控制，其核心是在线滚动优化。滚动优化的每一次优化都会得到当前时刻的优化控制域的第一部分作用于系统，直到下一时刻。如图2所示，通过多次采样和优化，得到整个控制时域的优化控制作用。

小组起飞集结问题分析

无人机起飞小组都为两架以及以上的无人机所组成，随着无人机成员的增加，求解的变量也在增加。无人机编队在起飞集结的过程中，每架无人机之间的动力学特性是解耦的，通过长僚机模型的约束以及其他约束条件的作用下，不同的无人机组员相对独立的控制。此外，在无人机小组起飞集结的过程中，通过通信可以交换信息，协同动作，这种无人机小组的控制系统可以看作多个小的独立的无人机控制子系统组成的大系统，其控制结构可以采用DMPC模型。

在步骤3中DMPC结构下的小组组员行为可以由每架UAV来联合描述，整个的小组组员运动方程可以表示为：

式中，

为无人机编队在k时刻的状态向量和控制向量。在控制输入量已知时(即

和

给定时),时刻k＝n*dt的编队状态可以根据式 (4)递推出。

编队中各个无人机是相对独立进行控制，无人机编队的整体代价即是组员数目架无人机的代价加权和。

(5)式中X_i(k)和U_i(k)分别为k时刻下的N步预测状态集和N步预测输入控制集，X₁(k)为组长的N步预测状态集。

小组起飞集结问题可以简化为单机规模的N个局部优化问题，如下所示：

由(7)式可以看出，在DMPC的结构下，每次预测时域内的优化仅与第i架无人机自身状态、输入控制集U_i(k)以及从通信网络获得的领航的无人机预测状态集X₁(K)相关，优化目标得以缩小。

组员数目架无人机的代价加权和包括：集结队形代价函数、队内避免碰撞代价函数和整体性代价函数。

1)集结队形代价函数

为了无人机编队能够以较高的效率达到集结队形，选取代价函数为：

J_p＝||X₁-X_i-M_1i||*m (8)

式中，X₁表示长机的位置信息，X_i表示小组内无人机i位置信息，M_1i表示长机与无人机组员i的相对队形信息，m为系数矩阵。

2)避免碰撞代价函数

为使无人机小组无安全事故，队内避免碰撞代价函数采取为下式：

J_li＝min(len(X_i，X_j))(i不是组长，j∈[1，n]&j≠i)

式中，X_i表示无人机i的位置信息，X_j表示无人机j的位置信息，d_ij表示无人机i 和无人机j间的距离，D为无人机碰撞半径。

3)整体性代价函数

为了无人机小组起飞集结具有整体一致性，要求无人机组员状态与组长状态相似。取代价函数为下式：

J_z＝k1*|V_i-V₁|+k2*|χ_i-χ₁| (10)

式中，V_i表示无人机i的速度，V₁表示长机的速度，χ₁表示长机的航向角，χ_i表示无人机i的航向角。

其中，约束函数式(8)和(10)即为长-僚机模式中的僚机对于长机速度、航向角和位置信息的跟踪形式的融合。在模型预测控制中融入了长-僚机模式的队形可控。

综上所述，三个部分的合并即为无人机人机小组起飞集结过程的代价函数，如下式：

J_i(k)＝m*||X₁-X_i-M_1i||+min(len(X_i，X_j))+k₁*|V_i-V₁|+k₂*|χ_i-χ₁| (11)

改进的QUATRE算法求解DMPC问题

1.标准QUATRE算法

仿射变换(QUATRE算法)在几何上是从一个仿射空间到另一个仿射空间的变换过程。如果X和Y表示两个仿射空间，则变换函数(f:X→Y)通常采用以下形式B→MB+W。QUATRE算法将原始仿射变换形式扩展为新的。QUATRE算法中的精确演化范式使用式(12)生成粒子候选。

表示逐分量乘法(对应矩阵元素的乘法，与MATLAB中的“.*”运算相同)。 M是QUATRE算法的交叉选择矩阵，矩阵M的所有元素均为0或1。

表示M的矩阵元素的二进制逆运算，非零元素的逆值为零，而零值为逆值是1。式(13)表示出了二进制逆运算的示例。

选择矩阵M是自动生成的矩阵，是从矩阵M_temp一步一步变换而来。我们以人口规模ps＝D为例来描述这一生成过程。M_temp是通过正交本征矢量矩阵P和对角本征值矩阵Λ相乘而生成的(M_temp＝P^TΛP)。Λ＝diag(d₁,d₂,...,d_D)是对角特征值矩阵，功能在于放大粒子演化。为简单起见，M_temp由元素等于1的下三角矩阵初始化。等式4-3给出了显示从M_temp到M转换的示例。转换有两个步骤，第一步是在 M_temp中随机排列每个D维行向量的元素，第二步是随机交换矩阵M_temp中的行向量。这两个步骤做完后，选择矩阵M就得到了。

QUATRE算法变异种群的变异式为下式(14):

式中，B_r1,G和B_r2,G是种群B的第G代的随机矩阵，它们是通过随机排列第G代种群X的行向量生成的B_gbest,G表示具重复的全局最佳个体矩阵，其每一个行向量的值都是b_gbest向量(b_gbest＝opt{b₁,b₂,...,b_ps}，B_gbest是种群B的第G代最佳个体，也是前G代的最佳个体)。

QUATRE算法流程如下：

①求解种群中所有个体的代价值

②生成选择矩阵M和初始种群B

③根据公式(14)生成差分矩阵W

④根据公式(12)生成变异种群B’

⑤对比代价函数适应度，选择变异种群和当代种群中的适应度较高的个体，组成新一代种群

⑥种群中最优个体满足条件否，是，输出最优个体，否，返回③

2.改进QUATRE算法

针对传统QUATRE算法求解速度慢迭代次数多的问题，将在初始化种群加入人工优化基因个体，采用人工添加优良基因，提高算法的效率。

1)种群个体结构

QUATRE算法将DMPC方法N步预测所得的预测控制量作为进化的种群个体。在三个数据一组对应控制输入U_i(k)＝[a(k),χ(k),γ(k)]的三个分量。由于在DMPC模型下， N步预测控制量

为下式

无人机单步控制量U_i(k)又是由U_i(k)＝{a(k),χ′(k),γ′(k)}所构成的，种群中个体的每三个基因数据一组对应控制输入U_i(k)＝{a(k),χ′(k),γ′(k)}的三个分量。种群和个体与控制量示意图如图3所示。

2)初始化种群的插入优化基因

QUATRE算法迭代速度慢的原因之一是每次迭代的种群都是随机生成的，没有继承上一轮迭代的任何成果。分布式模型预测控制将约束函数求解得到的最优控制指令集U^N(K)的前三个控制指令应用于无人机飞行控制上，舍弃剩下的 3*(N-1)个控制指令，而对于N步预测控制模型而言，剩下的3*(N-1)个指令对具有N步预测的约束代价函数在下一个采样时刻的适应度高。因此我们在每一次迭代求解完成后，将这一时刻求得的最佳控制指令集分级处理，第一部分用于无人机的飞行控制，第二部分则存储起来，将其基因转入下一时刻的模型求解的初始种群中。这样，改进的QUATRE算法在初始解空间中就有着最优个体附近方向，并且在高纬度下QUATRE算法的寻求解时各个方向上的概率是相等的，改进 QUATRE算法丢失最优解方向而落入局部优解的可能性被减弱。

QUATRE算法优化的具体过程为：从机体存储空间获得上一时刻的最优控制集X_gbest,G，采用公式(15)求得此时刻下的优化个体X_opt，用优化个体X_opt为初始种群做优化操作。将初始优化种群代入QUATRE算法流程中第2步中。

b_opt＝b_gbest，G*A

B＝[b_opt，b₂，...，b_ps]^T

步骤4：无人机小组对接

无人机小组对接，当所有无人机小组都成功起飞集结在空中的预设轨道时，无人机小组将在圆形轨道上形成最终的无人机编队起飞集结对接队形。

3.实验仿真

共有10架无人机的无人机编队起飞集结，无人机编队期望飞行速度为30m/s,最高速度为40m/s，加速度为5m/s²。10架无人机在有5条跑道的机场起飞，分成两组起飞，组长采用圆形轨迹集结算法，队员在DMPC框架下跟随组长，形成预设队形。每组间隔5s，最终在机场上空成圆形盘旋，形成双“人”字队形。此外分别采用改进的 QUATRE算法(Pre-DMPC-QUATRE)、QUATRE算法、PSO算法、GA算法以及GLPSO 算法，进行编队集结起飞，对比所有算法的性能。

图4为10架无人机的整体轨迹仿真图，无人机编队分成了两组依次从起点(0,0,0)起飞，集结于空中预设的圆形轨道处。

如图5，可以看出在30s左右时无人机已经完成集结队形，在36s处无人机形成的双“人”字形不嵌套的原因是：无人机小组组长处于不同的航向角，因而队员的相对位置有所偏差。

图6为无人机起飞小组间的距离图，可以明显发现两组无人机小组处于稳定距离状态，完成了两组无人机的队形对接。

图7为未10号组员与集结队形位置的相对距离。可以看出在28s往后，10号无人机已经处于正确的队形范围里了，无人机小组集结队形已经稳定。

上述仿真图表现了Pre-DMPC-QUATRE算法的稳定性能。

图8为5号机采用各算法在集结飞行中的平均迭代次数。从图中可以看出 Pre-DMPC-QUATRE算法对比其他算法具有迭代速度快的能力。

图9为10号无人机与自身对应位置的距离，图中仿真结果对比表现了 Pre-DMPC-QUATRE算法在无人机编队的收敛速度与其他算法相当。图8和图9的联合对比反映出了，Pre-DMPC-QUATRE算法保证了无人机编队收敛速度的同时，极大地减少了算法的计算量以及提高了算法的寻优能力。

本发明采用模型预测控制模式结合长僚机模式实现无人机编队的飞行控制，长僚机模式保证了无人机编队队形稳定。模型预测控制模式保证了无人机编队队员对环境的适应度高，面对环境的变化快速做出相对应的控制指令的变化。本发明通过采用改进的QUATRE算法对DMPC中的代价函数进行优化求解，改进的 QUATRE算法能够快速有效的实现无人机小组的集结，使得无人机编队起飞集结难度小、复杂度低、快速有效。

以上所述仅为本发明的验证实施例，并不用以限制本发明，凡在本发明技术思想下所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种基于QUATRE算法的多无人机同时起飞集结方法，其特征是：包括如下步骤：

步骤1，依据下述公式建立无人机数学模型，获取无人机的运动方程。

假设固定翼无人机的飞行过程中无侧滑及高度变化，速度由加速度和航迹方位角及航迹倾斜角控制，考虑在地面坐标系下无人机质心的运动方程，运用欧拉积分法对固定翼无人机的运动方程离散近似为：

式中，γ，χ分别为加速度、航迹方位角和航迹倾斜角；x，y，h分别为飞机质心位置在地面坐标系中的投影坐标；a，γ′，x′分别为加速度，航迹倾斜角速度和航迹方位角速度，dt为采样周期；

取无人机i在k时刻的状态变量与控制变量为：X_i(k)＝[x_i(k) y_i(k) h_i(k) γ(k) γ_i(k) x_i(k)]和U_i(k)＝[a_i(k)χ′_i(k)γ′_i(k)]，此时无人机的运动方程即为：

X_i(k+1)＝f(X_i(k)，U_i(k)) (2)

步骤2：无人机小组组长起飞，无人机编队以起飞场地的跑道数为除数分成多个小组，使每个小组包含不大于跑道数的无人机数目；每个无人机小组分配一个组长，编队队长跟踪预设的圆形轨道，队长采用轨迹跟踪算法，逐渐向预设轨道移动并且最终跟随轨道；

步骤3：小组组员集结控制采用DMPC结构，依据QUATRE算法求解DMPC框架中的最优代价函数值，得到无人机起飞小组组员的集结控制量；

步骤4：依据步骤3得到的无人机起飞小组组员的集结控制量，无人机小组将在圆形轨道上完成对接，形成最终的无人机编队起飞集结队形。

2.如权利要求1所述的一种基于QUATRE算法的无人机编队起飞集结方法，其特征是：所述的步骤3小组组员集结控制采用DMPC结构的运动方程可以表示为：

其中DMPC框架中的无人机小组组员的整体代价即是组员数目架无人机的代价加权和：

3.如权利要求1所述的一种基于QUATRE算法的无人机编队起飞集结方法，其特征在于：轨迹跟踪算法具体如下：

①通过设定的圆轨迹参数以及无人机位置，求得无人机在圆形轨迹上的投影点q；

②偏移角度依据上述的投影点q和无人机位置在投影点q的偏移角度λ，求解得出此无人机在圆形轨迹上与投影点q偏移角度为λ的偏移采样点s；

③依据无人机方向速度与无人机到偏移采样点s求得无人机速度方向与无人机到偏移采样点s的夹角ψ，将ψ带入式(1)求得无人机下一刻的控制量u_i(k+1)：

u_i(k+1)＝[0，χ′(k+1)，0] (3)

④控制量u_i(k+1)为无人机下一步轨迹跟踪控制量。

4.如权利要求1所述的一种基于QUATRE算法的无人机编队起飞集结方法，其特征在于：所述步骤3中DMPC结构下的小组组员行为可以由每架UAV来联合描述，整个的小组组员运动方程可以表示为：

式中，

为无人机编队在k时刻的状态向量和控制向量。在控制输入量已知时(即

……和

给定时)，时刻k＝n*dt的编队状态可以根据式(5)递推出；

编队中各个无人机是相对独立进行控制，无人机编队的整体代价即是组员数目架无人机的代价加权和：

5.如权利要求2所述的一种基于QUATRE算法的无人机编队起飞集结方法，其特征在于：组员数目架无人机的代价加权和包括：集结队形代价函数、队内避免碰撞代价函数和整体性代价函数。

6.如权利要求5所述的一种基于QUATRE算法的无人机编队起飞集结方法，其特征是：所述的集结队形代价函数：

J_p＝||X₁-X_i-M_1i||*m (8)

式中，X₁表示长机的位置信息，X_i表示小组内无人机i位置信息，M_1i表示长机与无人机组员i的相对队形信息，m为系数矩阵。

7.权利要求5所述的一种基于QUATRE算法的无人机编队起飞集结方法，其特征是：所述的避免碰撞代价函数：

为使无人机小组无安全事故，队内避免碰撞代价函数采取为下式：

J_li＝min(len(X_i，X_j))(i不是组长，j∈[1，n]&j≠i)

式中，X_i表示无人机i的位置信息，X_j表示无人机j的位置信息，d_ij表示无人机i和无人机j间的距离，D为无人机碰撞半径，避免碰撞代价函数用于使无人机小组无安全事故，队内避免碰撞。

8.权利要求1所述的一种基于QUATRE算法的无人机编队起飞集结方法，其特征是：所述的整体性代价函数：

为了无人机小组起飞集结具有整体一致性，要求无人机组员状态与组长状态相似。取代价函数为下式：

J_z＝k1*|V_i-V₁|+k2*|χ_i-χ₁| (10)

式中，V_i表示无人机i的速度，V₁表示长机的速度，χ₁表示长机的航向角，χ_i表示无人机i的航向角。

其中，约束函数式(8)和(10)即为长-僚机模式中的僚机对于长机速度、航向角和位置信息的跟踪形式的融合。在模型预测控制中融入了长-僚机模式的队形可控。

(6)式中X_i(k)和U_i(k)分别为k时刻下的N步预测状态集和N步预测输入控制集，X₁(k)为组长的N步预测状态集。

Images