一种固定翼无人机队形变换方法和系统与流程

1.本发明属于无人机技术领域，涉及固定翼无人机编队队形控制及变换技术，更具体地，涉及一种固定翼无人机队形变换方法和系统。


背景技术：

2.随着无人机相关技术的发展，无人机在各方面尤其是军事方面发挥着越来越重要的作用，用在侦察、打击的各环节作战中，无人机的出现不断推动作战方式和模式的变革。无人机应用在军事领域中具有以下优点：作战零伤亡、造价相对低、体积小、环境适应性强、灵活、隐蔽性好。
3.近几十年来，无人机的相关技术已经较为成熟，但随之而来的是，应用场景更加多元，任务更加复杂，单架无人机效率低、任务完成率差，无人机集群作战便应运而生。无人机集群作战中，编队是基础，基于执行任务的目标及环境，通过变换不同队形以最小代价有效完成作战任务，赋予无人机更强的作战能力。
4.但是在当前较为常用的的编队队形变换方法中，无人机的建模相对简单，无法满足三维空间的队形变换，灵活性较低；常用的长机-僚机方式仅对僚机进行控制，未控制长机，协同性较差；现有飞行控制算法如神经网络、h∞控制设计的控制器阶次较高，运行时间长，难以满足作战中实时性的要求，时效性不足。


技术实现要素：

5.为了解决以上问题，本发明提出了一种固定翼无人机队形变换方法和系统。该方法针对现有的无人机编队队形变换中存在的灵活性较低、协同性较差、时效性不足等问题，基于信息一致性进行固定翼无人机的队形建模，利用匈牙利算法进行目标点的指派，基于一致性控制设计编队控制律并进行控制，将队形变换扩展至三维空间，模型更加贴近实际，改善了无人机编队的灵活性，对所有无人机实现了控制，改进了编队的协同能力，此外一致性算法运行代价低，实时性好，提升了队形变换的时效性。本发明涉及的多无人机一致性建模、编队队形表示、队形目标点分配、多无人机一致性控制等技术，针对固定翼无人机编队队形控制和变换任务，提升了变换过程的灵活性、协同能力以及效率。
6.根据本发明技术方案的第一方面，提供一种固定翼无人机队形变换方法，具体包括：
7.s1：多无人机建模步骤，选取状态变量和操纵变量，建立多无人机运动状态模型；
8.s2:队形矩阵表示及多目标点生成步骤，确定多无人机队形参考点坐标，基于多目标点与参考点的距离和夹角计算多目标点坐标，实现队形矩阵表示；
9.s3：目标点分配步骤，构建当前时刻距离矩阵，对所述距离矩阵进行行归约，实现多无人机与多目标点分配；
10.s4:多无人机一致性控制步骤，确定一致性控制律，生成机间避撞策略，通过一致性控制完成多无人机队形变换。
11.进一步地，所述步骤s1中，无人机i在t时刻的无人机运动状态模型表示如下：
[0012][0013]
其中，状态变量为ri＝[v
xit
,v
yit
,v
zit
,a
xit
,a
yit
,a
zit
,x
it
,y
it
,z
it
]
t
,v
xit
,v
yit
,v
zit
分别代表t时刻无人机i在x、y、z轴上的分速度，a
xit
,a
yit
,a
zit
代表t时刻无人机i在x、y、z轴上的分加速度，x
it
,y
it
,z
it
代表t时刻无人机i在三维坐标系下的坐标，操纵变量为ui＝[a
xit
,a
yit
,a
zit
]
t
，即通过三个方向的加速度来控制无人机i的运动过程。
[0014]
进一步地，所述步骤s2中，所述多无人机队形参考点坐标的计算公式为：
[0015][0016][0017][0018]
其中，x0，y0，z0为多无人机队形参考点在x、y、z轴上的坐标，x
it
,y
it
,x
it
代表t时刻无人机i在三维坐标系下的坐标，n为编队中无人机的数目。
[0019]
进一步地，所述步骤s2中，所述多目标点坐标，即队形矩阵表示为：
[0020]
xj＝x0+lj*cosθj*cosψj
[0021]
yj＝y0+lj*cosθj*sinψj
[0022]
zj＝z0+lj*sinθj
[0023]
其中，xj，yj，zj为目标点j坐标，xn，y0，z0为多无人机队形参考点在x、y、z轴上的坐标，θj和ψj分别是目标点j与参考点在竖直方向和水平方向上的偏向角，lj为目标点j与参考点的距离。
[0024]
进一步地，所述步骤s3具体包括：
[0025]
通过匈牙利算法，构建当前时刻某无人机与多目标点的距离矩阵；
[0026]
对所述距离矩阵中每一行进行归约，保证归约后每一行都存在唯一的0元素并映射为1，其余元素映射为0，则所述距离矩阵转换为0、1矩阵；
[0027]
在该0、1矩阵中，逐行查找1元素，若当前1元素所在列不存在第二个1 元素时，将当前无人机与该目标点进行绑定，否则对所有的1元素映射为实际距离，选择距离最小值进行指派；
[0028]
完成当前无人机的指派后，对剩余未指派的无人机与目标点重新计算指派的 0、1矩阵，重复指派操作，直至完成所有无人机的目标点指派。
[0029]
进一步地，所述步骤s4中，所述一致性控制律为：
[0030]axi,t+1
＝kp1*(v
xdj,t-v
xi,t
+k1*δx
t
)
[0031]ayi,t+1
＝kp2*(v
ydj,t-v
yi,t
+k2*δy
t
)
[0032]azi,t+1
＝kp3*(v
zdj,t-v
zi,t
+k3*δz
t
)
[0033]
其中，a
xi,t+1
,a
yi,t+1
,a
zi,t+1
分别为t+1时刻无人机i在三维空间x、y、z 三个方向的加速度，ko1，kp2，kp3，k1，k2，k3为常数系数，v
xdj,t
，v
ydj,t
,v
zdj,t
为t时刻目标点j在三个轴上的
分速度，v
xi,t
，v
yi,t
,v
zi,t
为t时刻无人机i在三个轴的分速度，δx
t
，δy
t
,δz
t
则是t时刻无人机i与目标点j之间的距离。
[0034]
进一步地，所述步骤s4中，所述机间避撞策略为：
[0035]
实时计算机间距离，
[0036]
当两架无人机的机间距离大于等于最大安全距离时，停止避撞；当机间距离小于最大安全距离且在不断缩小时，启动二维规避；
[0037]
启动二维规避后，当机间距离大于等于安全距离时，停止避撞；当机间距离小于安全距离时，启动三维规避，同时对避撞之后的队形进行高度方向还原。
[0038]
进一步地，所述二维规避具体包括：
[0039]
机间距离小于最大距离且大于安全距离，并且机间距离在不断缩小，判断无人机与目标点的连线是否存在交叉；
[0040]
若存在交叉，则将两架无人机的目标点对换并记录，跳入下一步；若不存在交叉，则直接进入下一步；
[0041]
根据两架无人机的相对位置，向着拉开距离的方向调整各自的偏向角，即：相对位置偏上的无人机向上扭转方向，相对位置偏下的无人机向下扭转方向。
[0042]
进一步地，所述三维规避具体包括：
[0043]
将无人机运行高度根据最小安全距离进行划分，得到十二个高度区间，将高度区间进行编号后，记录其使用状态；
[0044]
判断机间距离是否小于安全距离且大于最小安全距离，若不满足则停止避撞；若满足，遍历高度区间状态，并将找到的前两个未使用的高度区间分配给可能发生碰撞的无人机；
[0045]
更改可能发生碰撞的无人机的目标点高度，基于控制律进一步将两架无人机拉开距离。
[0046]
进一步地，所述高度还原具体操作如下：
[0047]
分别计算无人机与对于目标点平面坐标(x、y方向)二维距离方差、三维坐标下的三维距离方差；
[0048]
判断三维距离方差与二维距离方差差值是否大于第一阈值，如果小于第一阈值则无需进行高度还原，否则进入下一步；
[0049]
判断二维距离方差是否小于第二阈值，如果小于第二阈值，进行下一步；
[0050]
查询初始队形中同一个目标点的高度信息，根据该高度信息结合一致性控制律进行无人机运动控制，更改当前无人机所在高度区间的使用状态为未使用，最终实现目标队形的变换。
[0051]
根据本发明技术方案的第二方面，提供一种固定翼无人机队形变换系统，具体包括：
[0052]
多无人机建模单元，用于选取状态变量和操纵变量，建立无人机运动状态模型；
[0053]
队形矩阵表示及目标点生成单元，用于确定多无人机队形参考点坐标，基于多目标点与参考点的距离和夹角计算多目标点坐标，实现队形矩阵表示；
[0054]
目标点分配单元，用于构建当前时刻距离矩阵，对所述距离矩阵进行行归约，实现多无人机与多目标点分配；
[0055]
多无人机一致性控制单元，用于确定一致性控制律，生成机间避撞策略，通过一致性控制完成多无人机队形变换。
[0056]
本发明相比现有技术有如下优点：
[0057]
通过一致性建模，将队形及运动扩展至三维空间，以队形目标点与无人机匹配的方式，减少了任务过程中必要的通信，最小化单机失效对编队的影响，提升了现有方法模型的灵活性，更加具有实际意义。
[0058]
基于匈牙利算法进行目标点指派，既实现了编队变换过程的代价最小化，又降低了机间碰撞的可能性，提升了算法的有效性。
[0059]
基于一致性控制的控制律计算简单，仅仅需要进行二阶控制，与现有的多阶控制方法相比，控制过程运行代价小，时间短，同时理论上最终会收敛至目标状态，更加快速有效地实现了无人机的控制。
[0060]
与较为常用的基于人工势场的机间避撞方法相比，基于规则的机间避撞策略更加有效，并且简单易懂，计算量小，运行速度快。
附图说明
[0061]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。
[0062]
图1为根据本发明实施例的目标点计算效果图；
[0063]
图2为根据本发明实施例的常用无人机编队队形图；
[0064]
图3为根据本发明实施例的匈牙利算法流程图；
[0065]
图4为根据本发明实施例的机间避撞算法流程图；
[0066]
图5为根据本发明实施例的一字形编队仿真效果图；
[0067]
图6为根据本发明实施例的正方形编队仿真效果图；
[0068]
图7为根据本发明实施例的三角形编队仿真效果图；
[0069]
图8为根据本发明实施例的圆形编队仿真效果图；
[0070]
图9为根据本发明实施例的两条平行一字形编队仿真效果图。
[0071]
本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
[0072]
这里将详细地对示例性实施例进行说明，其示例表示在附图中。下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本公开相一致的所有实施方式。相反，它们仅是与如所附权利要求书中所详述的、本公开的一些方面相一致的装置和方法的例子。
[0073]
本公开的说明书和权利要求书中的术语“第一”、“第二”等是用于区别类似的对象而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本公开的实施例例如能够以除了在这里图示或描述的那些以外的顺序实施。
[0074]
此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
[0075]
多个，包括两个或者两个以上。
[0076]
和/或，应当理解，对于本公开中使用的术语“和/或”，其仅仅是一种描述关联对象的关联关系，表示可以存在三种关系。例如，a和/或b，可以表示：单独存在a，同时存在a和b，单独存在b这三种情况。
[0077]
本发明技术方案涉及一种固定翼无人机队形变换方法和系统，该方法包括：
[0078]
多无人机建模，包括无人机的状态量、控制量的选取及相应的计算；
[0079]
队形表示及目标点生成，包括队形参数的设计及目标点坐标的计算；
[0080]
目标点分配，主要是将无人机与目标点以最小代价进行指派；
[0081]
多无人机编队变换，主要涉及多无人机一致性控制律的设计及机间避撞。
[0082]
实施例
[0083]
参照本实施例，本发明技术方案利用一致性理论对无人机编队进行数学建模，通过l-ψ-θ方法进行目标队形描述以及目标点生成，使用匈牙利算法完成无人机与目标点的指派任务，利用一致性控制律进行编队队形控制，此外，通过基于规则的策略完成机间避撞任务，最终实现了多机多队形快速、无碰撞变换的仿真验证。本发明的编队队形变换过程主要如下：多无人机一致性建模—编队队形表示及目标点生成—队形目标点分配—多无人机一致性控制，下面将分别对每一步进行介绍。
[0084]
多无人机一致性建模
[0085]
在队形变换过程中，多无人机编队系统的数学建模是基础，系统的数学模型主要给出了状态量与控制量的对应关系。实际应用中，无人机的运动状态通过一个非线性的微分方程来进行描述，该公式表示如下：
[0086][0087]
其中，状态变量为ri＝[v
xit
,v
yit
,v
zit
,a
xit
,a
yit
,a
zit
,x
it
,y
it
,z
it
]
t
,v
xit
,v
yit
,v
zit
分别代表t时刻无人机i在x、y、z轴上的分速度，a
xit
,a
yit
,a
zit
代表t时刻无人机i在x、y、z轴上的分加速度，x
it
,y
it
,z
it
代表t时刻无人机i在三维坐标系下的坐标，操纵变量为ui＝[a
xit
,a
yit
,a
zit
]
t
，即通过三个方向的加速度来控制无人机i的运动过程。
[0088]
另外，对fm(ri,t)、gm(ri,t)作如下说明(分别针对x，y，z轴方向)：
[0089]
对于位置坐标而言，只与fm(ri,t)相关：
[0090]
x
it+1
＝x
it
+v
xit
*δt；
[0091]yit+1
＝y
it
+v
yit
*δt；
[0092]zit+1
＝z
it
+v
zit
*δt；
[0093]
其中，x
it+
，y
it+1
，z
it+1
分别表示t+1时刻无人机i在三维坐标系下的位置坐标，x
it
，y
it
，z
it
分别表示t时刻无人机i在x、y、z轴上的位置坐标；v
xit
，v
yit
，v
zit
分别表示t时刻无人机i在x、y、z轴上的分速度，δt表示时间间隔。
[0094]
对于速度而言，同时涉及到fm(ri,t)、gm(ri,t)：
[0095]vxit+1
＝v
xit
+a
xit
*δt；
[0096]vyit+1
＝v
yit
+a
yit
*δt；
[0097]vzit+1
＝v
zit
+a
zit
*δt；
[0098]
其中，v
xit+1
，v
yit+
，v
zit+1
分别表示t+1时刻无人机i在x、y、z轴上的分速度，v
xit
，v
yit
，v
zit
分别表示t时刻无人机i在x、y、z轴上的分速度；a
xit
， a
yit
，a
zit
分别表示t时刻无人机i在x、y、z轴上的分加速度，δt表示时间间隔。
[0099]
对于加速度而言,只与gm(ri,t)u相关：
[0100]axit+1
＝a
xi,t+1
；
[0101]ayit+
＝a
yi,t+1
；
[0102]azit+1
＝a
zi,t+1
；
[0103]
其中，a
xit+1
，a
yit+1
，a
zit+1
以及a
xi,t+1
、a
yi,t+1
、a
zi,t+1
都分别表示t+1时刻无人机i在x、y、z轴上的分加速度，a
xi,t+1
、a
yi,t+1
、a
zi,t+1
的计算详见下文。
[0104]
编队队形表示及目标点生成
[0105]
对于队形变换而言，首先需要明确目标队形中各个目标点与参考点的位置关系，这就需要借助队形表示来完成。当前较为常用的表示方法有：l-ψ方法和l-l 方法，前者通过与参考点的距离和夹角给出特定队形的表示，后者则是借助与目标点各个轴上的距离来描述。在此，首先给出参考点坐标的计算公式：
[0106][0107][0108][0109]
其中，x0，y0，z0为多无人机队形参考点在x、y、z轴上的坐标，x
it
,y
it
,z
it
代表t时刻无人机i在三维坐标系下的坐标，n为编队中无人机的数目。
[0110]
为了满足三维队形变换的需求，借助垂直方向夹角θ将l-ψ方法扩展至三维空间，最终实现了队形矩阵的表示，即：
[0111]
xj＝x0+lj*cosθj*cosψj
ꢀꢀ
(5)
[0112]
yj＝y0+lj*cosθj*sinψj
ꢀꢀ
(6)
[0113]
zj＝z0+lj*sinθj
ꢀꢀ
(7)
[0114]
其中，xj，yj，zj为目标点j坐标，x0，y0，z0为多无人机队形参考点坐标，θj和ψj分别是目标点j与参考点在竖直方向和水平方向上的偏向角，lj为目标点j与参考点的距离。
[0115]
目标点生成的效果如图1所示，此处以矩形队形为例进行说明：一列竖向圆圈表示当前无人机的位置，中间圆圈表示当前队形的几何中心即参考点位置，两行横向圆圈表示最终生成的目标点。
[0116]
在本发明中主要涉及的常用无人机编队队形有一字形、正方形、三角形、圆形、两条平行一字形，具体的队形效果如图2。
[0117]
队形目标点分配
[0118]
在生成队形中的各个目标点之后，为了实现由当前队形向目标队形的转换，需要以最优情况将无人机与目标点进行分配，该分配问题是一个标准的匈牙利算法任务分配问题，分配算法的流程图详见图3。
[0119]
基于当前各个无人机的位置和目标点位置构建当前的距离矩阵，在此基础上不断进行行归约，对归约得到的独立0元素进行映射和标记，根据距离矩阵中的标记元素的分布，最终选择出最优分配方案。
[0120]
多无人机一致性控制
[0121]
完成目标点与无人机的指派任务后，需要通过多无人机一致性控制最终完成编队队形变换，其中主要包括一致性控制律的设计和机间避撞策略的生成。
[0122]
首先，基于一致性控制理论对固定翼无人机的控制律进行设计，基于多无人机一致性建模中控制量的设定，分别在x、y、z三个轴计算下一时刻无人机的加速度，并以此作为飞机的控制量进行控制，相应的控制律如下：
[0123]axi,t+1
＝kp1*(v
xdj,t-v
xi,t
+k1*δx
t
)
ꢀꢀ
(8)
[0124]ayi,t+1
＝kp2*(v
ydj,t-v
yi,t
+k2*δy
t
)
ꢀꢀ
(9)
[0125]azi,t+1
＝kp3*(v
zdj,t-v
zi,t
+k3*δz
t
)
ꢀꢀ
(10)
[0126]
其中，a
xi,t+1
,a
yi,t+1
,a
zi,t+1
分别为t+1时刻时无人机i在三维空间x、y、z 三个方向的加速度，kp1，kp2，kp3，k1，k2，k3为常数系数，v
xdj,t
，v
ydj,t
,v
zdj,t
为t时刻时目标点j在三个轴上的分速度，v
xi,t
，v
yi,t
,v
zi,t
为t时刻时无人机i在三个轴的分速度，δxy，δy
t
,δz
t
则是t时刻时当前无人机i与目标点j之间的距离。
[0127]
针对无人机的机间避撞问题，首先给出以下三个距离定义：
[0128]
最小安全距离：实际无人机的安全距离+无人机的尺寸，当机间距离小于该值时，无人机将发生碰撞。
[0129]
安全距离：最小安全距离+改变高度避撞所需最小距离，当机间距离小于该值时，需要及时进行高度的拉伸，否则将发生碰撞。
[0130]
最大安全距离：需要进行二维规避的最大距离，机间距离小于该值时，无法确定是否会小于最小安全距离，所以需要进行机间避撞。
[0131]
基于上述三个距离定义，通过分析仿真过程中的机间碰撞问题，人工生成避撞规则，采用碰撞检测-二维规避-三维避撞-高度还原的步骤，具体的机间避撞算法流程如图4。
[0132]
下面针对固定翼无人机编队队形变换过程中的编队队形表示及目标点生成、队形目标点分配、多无人机一致性控制三部分，结合附图对本发明进行进一步的介绍。
[0133]
首先，在编队队形表示及目标点生成部分，对输入的当前无人机队形结合式-2、式-3、式-4进行参考点的计算，再结合目标点的l-ψ-θ信息，以参考点的坐标为基准，基于式-5、式-6、式-7分别计算出各个目标点的位置，计算的效果见附图1。
[0134]
之后，在队形目标点分配过程中采取匈牙利算法完成指派任务，算法的流程见图3。首先完成无人机与目标点的距离矩阵的构造，对距离矩阵中每一行进行归约，保证归约后每一行都存在唯一的0元素并映射为1，其余元素映射为0，则距离矩阵转换为0、1矩阵。在该0、1矩阵中，逐行查找1元素，若当前1 元素所在列不存在第二个1元素时，将对应的无人机与目标点进行绑定，否则对所有的1元素映射为实际距离，选择距离最小值进行指派。完成当前节点的指派后，对剩余未指派的无人机与目标点重新计算指派的0、1矩阵，重复指派操作，直至完成所有无人机的目标点指派。
[0135]
下面举例说明通过匈牙利算法解决目标点指派的过程，以三架无人机指派为例，假设初始距离矩阵为:
[0136][0137]
通过归约计算可得指派矩阵为：
[0138][0139]
由于当前m中1元素均在第一列，则先将m中的元素映射至距离矩阵d中，通过比较，选择第一列中距离最小的无人机1与目标点1进行指派。指派后重复归约可得：
[0140][0141]
在此指派矩阵中可看出，可将无人机2与目标点3进行绑定，将目标点2 指派给无人机3，至此便完成了三无人机的目标点指派任务。
[0142]
基于目标点生成和目标点指派两个步骤的结果，进行多无人机一致性控制。在此过程中，主要需要考虑各无人机的运动量控制和队形的机间碰撞问题。
[0143]
首先对各个无人机的运动控制律进行设计。对于无人机集群的运动以较小的时间间隔进行划分，在每个时间间隔内，无人机的运动可以近似为匀速直线运动，根据一致性理论所设计的无人机的控制律可以得到无人机的控制量公式如式-8、式-9、式-10，根据匀速直线运动距离与速度的关系及速度与加速度的关系可知(y、 z轴同理)：
[0144]
x(t+1)＝x(t)+vx(t)*δt
ꢀꢀ
(11)
[0145]
vx(t+1)＝vx(t)+ax*δt
ꢀꢀ
(12)
[0146]
此处在计算无人机的速度与加速度时，依据控制律所得结果应以实际飞机参数为标准，对于超出物理参数界限的速度、加速度应进行越界处理，从而保证仿真过程中固定翼无人机的运动符合实际运动规律。
[0147]
至于编队队形变换过程中可能存在的机间碰撞问题，本发明采用碰撞检测
‑ꢀ
二维规避-三维避撞-高度还原的步骤解决。
[0148]
首先，在碰撞检测部分，基于机间距离和速度方向可以确定是否存在发生碰撞的风险，并以此为依据分情况讨论。经过多次仿真，基于一致性控制的控制律控制下的固定翼无人机队形变换可能产生碰撞的条件有：
[0149]
距离小于最大安全距离且在不断缩小；
[0150]
无人机与目标点的连接线存在交叉；
[0151]
连接线不存在交叉，但是运行轨迹最小距离小于最小安全距离；
[0152]
当前距离小于安全距离。
[0153]
经过碰撞检测后，针对可能发生碰撞的情况，首先采取二维规避方式进行机间避撞，当两架无人机当前距离小于最大安全距离且在不断缩小时，便需要进行二维规避操作。二维规避的主要思路如下：
[0154]
机间距离小于最大距离且大于安全距离，并且机间距离在不断缩小，判断无人机与目标点的连线是否存在交叉；
[0155]
若存在交叉，则将两架无人机的目标点对换并记录，跳入下一步；若不存在交叉，则直接进入下一步；
[0156]
根据两架无人机的相对位置，向着拉开距离的方向调整各自的偏向角，即：相对位置偏上的无人机向上扭转方向，相对位置偏下的无人机向下扭转方向。
[0157]
当二维规避无法在限定时间内完成有效避撞时，两架无人机的间距可能达到安全距离这一临界值，此时需要在此基础上通过改变纵向高度，加大可能发生碰撞的无人机间距拉开速度。三维避撞的主要思路如下：
[0158]
将无人机运行高度根据最小安全距离进行划分，得到十二个高度区间，将高度区间进行编号后，记录其使用状态；
[0159]
判断机间距离是否小于安全距离且大于最小安全距离，如果不满足则直接结束该过程；
[0160]
若满足，遍历高度区间状态，并将找到的前两个未使用的高度区间分配给可能发生碰撞的无人机；
[0161]
更改可能发生碰撞的无人机的目标点高度，基于控制律进一步将两架无人机拉开距离。
[0162]
一旦进入三维避撞操作，势必将导致无人机与目标节点存在高度上的偏差，为了实现完整的队形变换，便需要对避撞之后的队形进行高度方向的还原。为了保证在高度还原过程中不会发生碰撞，需要根据平面坐标下无人机与目标点的偏差决定是否进入高度还原状态。具体操作如下：
[0163]
分别计算无人机与目标点平面坐标(x、y方向)距离方差、三维坐标下的距离方差
[0164]
判断三维方差与二维方差差值是否大于阈值，如果小于阈值则无需进行高度还原，否则进入下一步
[0165]
判断二维方差是否小于阈值，如果小于阈值，进行下一步
[0166]
查询初始队形中同一个目标点的高度信息，基于此高度信息，结合一致性控制律进行无人机运动控制，更改当前无人机所在高度区间的使用状态为未使用，最终实现目标队形的变换。
[0167]
最终，通过仿真实现固定翼无人机编队的五种常用队形的互相变换过程，且所有变换均未发生碰撞，仿真效率有一定提升，五种队形仿真效果见图5-图9。
[0168]
以上对本技术实施例所提供的一种基于一致性控制及匈牙利算法的固定翼无人机队形变换算法，进行了详细介绍。以上实施例的说明只是用于帮助理解本技术的方法及其核心思想；同时，对于本领域的一般技术人员，依据本技术的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本技术的限制。
[0169]
上述说明示出并描述了本技术的若干优选实施例，但如前所述，应当理解本技术并非局限于本文所披露的形式，不应看作是对其他实施例的排除，而可用于各种其他组合、修改和环境，并能够在本文所述申请构想范围内，通过上述教导或相关领域的技术或知识进行改动。而本领域人员所进行的改动和变化不脱离本技术的精神和范围，则都应在本技术所附权利要求书的保护范围内。