CN114637329A - 一种飞机多机密集编队队形重构方法及系统 - Google Patents

Abstract

本发明实施例公开了一种飞机多机密集编队队形重构方法及系统，包括以下步骤：将接收到的无人机激光雷达坐标系转换为大地坐标系，得到机群中无人机的大地直角坐标系数据；基于无人机在大地坐标系数据，计算无人机相对位置向量和无人机定位向量，比较两个向量之间模值、角度和方向，实现两个矢量的匹配，获得当前无人机机群内部各无人机的定位信息；获取当前无人机无人机机群内部各无人机的定位信息，根据初始编队队形和需要的编队队形，重新分配无人机的位置和航路。可以实现飞机在实际编队飞行过程中，对编队进行重新编排，编成新的队形，对飞机的位置控制更加精确，解决了现有技术中编队在重构中对环境和任务的适应性低的问题。

Description

一种飞机多机密集编队队形重构方法及系统

技术领域

本发明属于无人机飞行技术领域，涉及一种飞机多机密集编队队形重构方法及系统。

背景技术

多机编队队形重构要确定无人装备的间隔距离、位置、初始编队结构到最终结构转换的过程。无人装备编队重构包括队形切换及编队成员增加或缺少时新编队队形的重构。另外，在重构过程中，如何避免碰撞，还要以最小代价变换到新的队形，同时能够适应任务和环境的变化等都是无人机编队中需要考虑的问题。

发明内容

本发明的目的在于解决现有技术中的问题，提供一种飞机多机密集编队队形重构方法及系统。

为达到上述目的，本发明采用以下技术方案予以实现：

一种飞机多机密集编队队形重构方法，包括以下步骤：

S1：将接收到的无人机激光雷达坐标系转换为大地坐标系，得到机群中无人机的大地直角坐标系数据；

S2：基于无人机在大地坐标系数据，计算无人机相对位置向量和无人机定位向量，比较两个向量之间模值、角度和方向，实现两个矢量的匹配，获得当前无人机机群内部各无人机的定位信息；(获得无人机定位向量对应的无人机编号)

S3：获取当前无人机无人机机群内部各无人机的定位信息，根据初始编队队形和需要的编队队形，重新分配无人机的位置和航路。

本方法的进一步改进在于：

所述S1包括以下步骤：

计算各无人机的欧式距离，采用快速排序法进行排序，直至得到最小的欧氏距离，根据最小的欧氏距离确定自身无人机与激光雷达扫描到的无人机之间的最小距离的坐标，并将该坐标转成大地直角坐标系数据。

所述S1中包括以下步骤：

确定无人机与其他各无人机的欧氏距离：

将无人机通过激光雷达探测到周边无人机的探测结果记录为C_k＝(x_k,y_k,z_k)，其中(x_k,y_k,z_k)为点的位置坐标，该探测到无人机到携带激光雷达的固定翼无人机的欧氏距离为：

其中，k为激光雷达探测到的无人机的三维点云数据中某一个点的编号，j为携带激光雷达的无人机编号，Xrj为x方向上编号j号的携带激光雷达的无人机的位置；

将接收到的无人机激光雷达坐标系转换为大地坐标系：

(1)从大地坐标系O到大地直角坐标系G的转换：

其中，

e表示椭球体的第一偏心率；a表示椭球体的长半径；

(2)从大地直角坐标系G到地理坐标系N的转换

其中，(x₀,y₀,z₀)为N系坐标原点在G系中的坐标，L₀和B₀为N系坐标原点的经纬度；

(3)从地理坐标系N到载机坐标系B的转换绕X_n轴旋转φ的转换矩阵：

绕旋转后的Y_n绕旋转γ的转换矩阵：

绕旋转后的Z_n轴旋转θ的转换矩阵：

则从地理坐标系N到载机坐标系B的转换：

(4)从载机坐标系B到基座坐标系T的转换：

首先考虑载机坐标系和基座坐标系之间的安装误差，

绕Xb轴旋转Δφ_I的转换矩阵：

绕旋转后Yb轴旋转Δγ_I的转换矩阵：

绕旋转后的Zb轴旋转Δ的转换矩阵：

从载机坐标系B到基座坐标系T的转换：

经过上述步骤，实现激光雷达定位数据坐标系到大地坐标系的坐标的转换。

所述S2包括以下步骤：

基于无人机在大地直角坐标系下的位置信息，设无人机实际位置与预定位置的偏差为R，则无人机实际位置在预设位置为圆心，位置偏差R为半径的圆内，基于此计算无人机在大地坐标系下的相对位置向量，同时激光雷达测量无人机之间的相对位置，得到激光雷达定位向量。

所述S2包括以下步骤：

设无人机航线预定位置坐标为(X2，Y2，Z2)；激光雷达定位的位置坐标为(X12，Y12，Z12)，无人机所处位置坐标为(X1，Y1，Z1)，

则航线定位矢量为(X2-X1，Y2-Y1，Z2-Z1)，激光雷达定位矢量(X12，Y12，Z12)，预设算法参数δ1为1.5，δ2为0.4；

(1)两矢量模值之差为：

矢量匹配需满足D<δ1；

(2)两矢量角度值之差为：

在X-Y平面上的投影：

在X-Z平面上的投影：

在Y-Z平面上的投影：

当α+β+γ<δ2；

(3)符号同值

矢量匹配需满足：

(X12，Y12，Z12)与(X2-X1，Y2-Y1，Z2-Z1)同号，即{X12*(X2-X1)>0&Y12*(Y2-Y1)>0&Z12*(Z2-Z1)>0}同时成立，则认为两矢量匹配，完成无人机识别。

所述S3包括以下步骤：

建立赋权矩阵D∈D_n*n，其中，n为编队中的无人机数量，D_ij表示由原编队位置F_i飞向新的编队队形P_i所需的代价；

然后在不同行不同列中找到n个D，使得这n个D_ij的和最小，即：

min∑D_ij (16)

其中，n个D_ij就指明了每架无人机在新的编队中的位置，根据不同任务的需求，改变D_ij的定义则可以得到基于不同优化条件的位置分配结果。

所述S3包括以下步骤：

利用蚁群搜索算法对具体位置进行分配操作：

将赋权矩阵中的每一个值D_ij作为一个单独的节点v_ij，得到一个无向完全图，图中的任意两点之间能够连通，将无人机编号和新队形中的位置的编号作为节点的二维坐标，表示第i个位置由第j个无人机占据，每个节点都与赋权矩阵中的元素一一对应；

进一步利用蚁群算法进行无人机位置分配，得到三维空间中的无人机重构时飞行轨迹相对于没有队形重构时飞行轨迹的变化矢量，并进一步得到匹配后的相对航路。

一种飞机多机密集编队队形重构系统，包括激光雷达数据标定预处理模块、无人机定位识别模块和无人机编队队形重构模块；

激光雷达数据标定预处理模块，用于将接收到的无人机激光雷达坐标系转换为大地坐标系，得到机群中无人机的大地直角坐标系数据；

无人机定位识别模块，基于无人机在大地坐标系数据，计算无人机相对位置向量和无人机定位向量，比较两个向量之间模值、角度和方向，实现两个矢量的匹配，获得当前无人机机群内部各无人机的定位信息；(获得无人机定位向量对应的无人机编号)

无人机编队队形重构模块，用于获取当前无人机无人机机群内部各无人机的定位信息，根据初始编队队形和需要的编队队形，重新分配无人机的位置和航路。

一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如权利要求1-7任一项所述方法的步骤。

一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1-7任一项所述方法的步骤。

与现有技术相比，本发明具有以下有益效果：

本发明公开了一种飞机多机密集编队队形重构方法及系统，可以实现飞机在实际编队飞行过程中，对编队进行重新编排，编成新的队形，提出了采用激光雷达测距的方法来确定每架飞机的实时位置，实现对飞机的位置控制更加精确，对环境和任务的适应性更强，本发明能够实现在多机队形重构前和队形重构后的队形稳定，适用于多种编队队形重构。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1为协同定位示例图；

图2为第一轮计算；

图3为第二轮计算；

图4为迭代计算过程((a)为第三轮计算；(b)为第四轮计算；(c)为第五轮计算；(d)为第六轮计算)；

图5为两种编队队形示例((a)为第一种无人机编队队形；(b)为第二种无人机编队队形)；

图6是队形重构中的相对轨迹。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。

因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。

在本发明实施例的描述中，需要说明的是，若出现术语“上”、“下”、“水平”、“内”等指示的方位或位置关系为基于附图所示的方位或位置关系，或者是该发明产品使用时惯常摆放的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

此外，若出现术语“水平”，并不表示要求部件绝对水平，而是可以稍微倾斜。如“水平”仅仅是指其方向相对“竖直”而言更加水平，并不是表示该结构一定要完全水平，而是可以稍微倾斜。

在本发明实施例的描述中，还需要说明的是，除非另有明确的规定和限定，若出现术语“设置”、“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。

下面结合附图对本发明做进一步详细描述：

本发明实施例公开了一种飞机多机密集编队队形重构方法，包括以下步骤：

步骤1：激光雷达数据标定预处理；

首先：无人机编队中的每架无人机，通过激光雷达探测除自身外无人机编队中的其它无人机，确定其它无人机与各自的欧氏距离。具体表现为：

无人机听过自身携带的激光雷达探测周围的无人机，激光雷达的探测的结果是三维点云数据，探测到的每架无人机会产生多个点云数据，记为C_k＝(x_k,y_k,z_k)，其中(x_k,y_k,z_k)为点的位置坐标，激光雷达点云数据处理器根据点云数据计算无人机到每一个点的欧式距离，该探测到的无人机到携带激光雷达的固定翼无人机的欧氏距离为：

其中，k为探测到的无人机的三维点云数据中某一个点的编号，j为携带激光雷达的无人机编号，Xrj为x方向上编号j号的携带激光雷达的无人机的位置。

其次：根据无人机的激光雷达点云数据处理器计算得到的欧式距离，确定激光雷达探测到的各个无人机相对携带激光雷达的无人机的最小位置，取最小位置是确保机间距离足够大，保证编队中的无人机的飞行安全。

采用快速排序法进行排序，以获取每架无人机探测到的周围无人机距离自身的最小位置，快速排序法的具体过程为：

根据所有激光雷达扫描到的无人机的点云数据计算得到的与自身无人机的欧氏距离，得到距离数组{d₁₁,d₁₂,d_1j,…,d_kj,…}，随机选取距离数组中某一元素d_ab为基准元素，从前向后遍历数组，当遇到小于基准元素的元素时，把它放置在基准元素左侧，当遇到大于基准元素的元素时，它放置在基准元素右侧。

然后，将所有小于基准元素的元素作为一个新的距离数组，再次随机选取新的距离数组中某一元素为基准元素，再次遍历整个新的距离数组，将小于基准元素的放置在左侧，将大于基准元素的放置在右侧。

根据上述方法进行迭代，直至得到最小的欧氏距离，即为激光雷达扫描到的无人机与自身无人机之间的最小距离d_min，自身无人机与激光雷达扫描到的无人机之间的最小距离的点的坐标为P_obs＝(x_o,y_o,z_o)。

大地坐标系是以地心作为原点，Z_o轴为北极方向，X_o轴为地心指向格林尼治子午面与地球赤道的交点方向，Y_o轴与X_o、Z_o两轴相互垂直构成一个右手系。在大地坐标系中每一点的坐标可表示成：(B,L,H)，分别代表该点的纬度、经度和高程。纬度是指某点与地球球心的连线和地球赤道面所成的线面角，其数值在0°至90°之间；经度指某点与两极的连线与0°经线所在平面的夹角；大地高程是空间的点沿着参考椭球的法线方向到参考椭球面的距离。

大地直角坐标系与大地坐标系重合。大地直角坐标系中任一点的坐标可用该点在此坐标系各个坐标轴上的投影来表示：(x_g,y_g,z_g)。

在地理坐标系中，原点是载机中心在某一时刻所处的位置，Z_n指向正北方向，X_n垂直于地表指向天空，Y_n与Z_n、X_n相互垂直构成一个右手系，指向正东，如图1所示。地理坐标系中的每一个点的坐标可以表示为(x_n,y_n,z_n)。

载机坐标系的原点为载机导航系统的中心，Y_b代表载机横轴，Z_b代表载机纵轴，X_b由机腹指向机背。Φ载机航向角，载机俯仰角γ，载机横滚角θ代表该坐标系相对地理坐标系的三个姿态角。当姿态角均为零时，载机坐标系的三轴指向与地理坐标系的三轴指向重合。在载机坐标系中任一点的坐标表示为：(x_b,y_b,z_b)。

由于存在初始安装误差，即载机坐标系和基座坐标系不完全重合，同时由于光电侦察平台基座与载机之间是用减振器联结，在工作时减振器振动会产生误差。将基座坐标系相对载机坐标系的三个安装误差角定义为Δφ_I，Δγ_I，Δθ_I，基座坐标系相对载机坐标系的三个振动误差角定义为Δφ_v，Δγ_V，Δθ_V。

本专利需要将激光雷达在基座坐标系下的坐标转换为大地坐标系，转换步骤如下：

(1)从大地坐标系O到大地直角坐标系G的转换

其中，

e表示椭球体的第一偏心率；a表示椭球体的长半径；

(2)从大地直角坐标系G到地理坐标系N的转换

其中，(x₀,y₀,z₀)为N系坐标原点在G系中的坐标，L₀、B₀为N系坐标原点的经纬度。

(3)从地理坐标系N到载机坐标系B的转换

绕X_n轴旋转φ的转换矩阵

绕旋转后的Y_n绕旋转γ的转换矩阵：

绕旋转后的Z_n轴旋转θ的转换矩阵：

则从地理坐标系N到载机坐标系B的转换：

(4)从载机坐标系B到基座坐标系T的转换

首先考虑载机坐标系和基座坐标系之间的安装误差

绕Xb轴旋转Δφ_I的转换矩阵：

绕旋转后Yb轴旋转Δγ_I的转换矩阵：

绕旋转后的Zb轴旋转Δ的转换矩阵：

载机坐标系和基座坐标系之间的振动误差与安装误差的转换类似，从载机坐标系B到基座坐标系T的转换：

经过上述4个计算步骤，完成激光雷达定位数据坐标系到大地坐标系的坐标转换。

步骤2：飞机的定位识别；

经过数据预处理和滤波后得到激光雷达测量数据的格式如下，(x,y,z,reflectivity),其中x,y,z分别为基座坐标系下的坐标，reflectivity为反射率值，取值范围为0～255。

激光雷达可以测量无人机之间的位置，但是无法得知所测无人机的编号，由于激光雷达可以感知物体表面颜色，物体颜色不同，激光雷达测量数据的反射率也会不同，具体的，如物体表面为黑色，反射率值在70以下，如果物体表面为白色，反射率值大小在180以上。

本发明实施例中长机，僚机分别采用黑色和白色两种颜色，这样在编队飞行过程中，无人机所搭载的激光雷达可以将反射率返回值小于70的定位看作是长机，将反射率大于180的返回值看作是僚机定位数据。由此，可先一步识别出长机，进一步地，通过所提出的向量匹配算法区分不同的僚机数据。

无人机协同定位—匹配算法：通过预先设定的编队飞行路线，可知无人机每个时刻在大地坐标系下的位置信息，由于飞行过程中误差存在，无人机实际位置与预定位置会存在偏差，设位置偏差为R，无人机实际位置即在以预设位置为圆心，位置偏差R为半径的圆内，由此可以计算无人机在大地坐标系下的相对位置向量(由搭载激光雷达无人机的位置指向被激光雷达探测到无人机的位置)；另一方面，激光雷达可以测得无人机之间的相对位置，得到激光雷达定位向量，通过两个向量之间模值、角度、方向的比较，选择出最为匹配的一对向量，即可知该激光雷达定位向量所对应的无人机编号。

具体识别定位过程为：

根据激光雷达和航线预定位置得到的位置矢量的模值和方位角度来进行匹配:

参见图1，可知两矢量分别为航线定位矢量(X2-X1,Y2-Y1,Z2-Z1)与激光雷达定位矢量(X12,Y12,Z12),预设算法参数δ1为1.5,δ2为0.4。

(1)两矢量模值之差：

矢量匹配应满足D<δ1

(2)两矢量角度值之差

在X-Y平面上的投影：

在X-Z平面上的投影：

在Y-Z平面上的投影：

当α+β+γ<δ2；

(3)符号同值

安装时候设置激光雷达大地坐标系轴角方向相同，如矢量匹配，应满足：(X12,Y12,Z12)与(X2-X1,Y2-Y1,Z2-Z1)同号，

即{X12*(X2-X1)>0&Y12*(Y2-Y1)>0&Z12*(Z2-Z1)>0}同时成立。

当模值之差，角度之差，符号同值均满足条件时，则认为这两矢量匹配,完成识别工作。

具体的举例，比如无人机所携带的激光雷达，可以测到2组点，记为d₁(a₁₂,a₁₃)，其他49架无人机可以得到49组航线定位参考数据：pos₀,pos₂,pos₃,…,pos₅₀

将这j组点，分别与这50组点进行匹配，最多进行2*50＝100次运算，即可得出位置矢量的匹配对,即a12与pos2匹配，a13与pos3匹配。

对于50架无人机，由航线规划可以获得各自在大地坐标系下的位置pos₁,pos₂,pos₃,…,pos₅₀

其中有：pos_i(x_i,y_i,z_i)

50架无人机携带的激光雷达由于距离和遮挡限制，会测量出多个位于其周边的无人机的相对位置信息，表示为：d₁,d₂,d₃,...,d₅₀

其中d_k是定义的第k架无人机周边的j架无人机相对其的位置信息：

d_k(a_1k,a_2k,a_3k,…,a_jk) (16)

其中有a_km表示第K架无人机机载激光雷达所采集到第m架无人机位置的相对坐标：

a_km(x_km,y_km,z_km) (17)

定义距离匹配函数为：

定义角度匹配函数：

定义符号对比函数：

在实际计算过程中，pos坐标及其对应的飞机编号是已知信息，雷达测量数据已知，k是已确定位置和编号的无人机(起始为长机0号无人机)，即需要确定距离a_mk中m对应的实际飞机编号。

当满足条件：

D(a_km,pos_i)＜δ₁ (23)

φ₁(a_km,pos_i)+φ₂(a_km,pos_i)+φ₃(a_km,pos_i)＜δ₂ (24)

此时，定义q＝1其中δ₁和δ₂为极小的阈值。

可认为编号为i的无人机位于相对于k号无人机的距离为a_km处，也就是说编号为k的无人机的距离a_km处的无人机是编号为i的僚机。对于多架无人机，在无人机编队飞行中，将僚机无人机与激光雷达扫描到的距离进行匹配的算法：

无人机搭载激光雷达可以扫描一定范围内的无人机，超过一定范围的无人机无法识别，另外位于同一个扫描角内的无人机由于遮挡，也无法识别，或者由于部分遮挡导致只能采集到少量点云数据，将少量点云数据的部分丢弃。

识别步骤：

参见图2，第一轮计算，以0号无人机为长机，0号无人机携带的激光雷达只能确定在P1、P2和P3处共有三台无人机，参见图2，并给出这三个点相对于0号无人机的距离信息，并以P_0-1(x1，y1，z1)、P_0-2(x2，y2，z2)、P_0-3(x3，y3，z3)坐标表示，但是不能确定这3个位置的无人机的编号。所有的无人机都会根据航线规划可以获得自己的大地坐标信息，从而获得所有无人机相对于0号无人机的相对坐标，通过匹配算法可以计算P1,P2,P3处是几号无人机。

参见图3，第二轮计算时，由于已经确定了P1处的无人机的编号，以P1处的无人机为准，P1处的无人机携带的激光雷达可以扫描到P0、P2、P3和P4处有无人机，并且可以获得P_1-0、P_1-2、P_1-3、P_1-4四个距离坐标信息，通过步骤2中确定的P_0-1坐标，根据位置矢量计算方法与本轮扫描到的4个点到坐标信息，可以计算出4个相对于0号无人机的距离信息，分别是P_0-0、P_0-2、P_0-3、P_0-4，其中P_0-0代表的是0号本身，P_0-2、P_0-3在步骤2和3中已经确定了P2、P3处的无人机编号。所以本轮只需要确定P_0-4代表的无人机编号，确定未知的无人机的实际编号由匹配算法决定。

参见图4(图4a-图4d)第三轮计算时，由于P2处的无人机编号已经确定了，重复第二轮计算过程的方法。可以确定P6处的无人机编号。后续过程重复步骤4中的第二轮计算的方法，可以计算出编队中所有无人机的位置以及所对应的无人机编号。经过上述步骤，获得当前无人机机群内部各架无人机的定位信息，送入飞行控制器进行编队控制。

步骤3：飞机多机编队新队形位置确定：

(1)编队队形重构中的位置分配

参见图5，其中图5a中，F₁～F₄表示的是重构前的四架无人机组成的无人机编队队形；图5b中，P₁～P₄表示的是需要转换成的编队队形。在编队重构问题中，首先需要解决的是为原编队队形中的每架无人机F_i分配一个新编队队形中的位置P_i。

首先，建立赋权矩阵D∈D_n*n，n为编队中的无人机数量，D_ij表示由原编队位置F_i飞向新的编队队形P_i所需的代价。然后在不同行不同列中找到n个D，使得这n个D_ij的和最小，即：

min∑D_ij (25)

这n个D_ij就指明了每架无人机在新的编队中的位置。考虑到不同任务需求，改变D_ij的定义就可以得到基于不同优化条件的位置分配结果。

当编队中的无人机数量增加时，位置指派问题的规模将显著增加，编队重构的最优方法就要要通过大量的计算才能得到。如果还希望考虑到防碰撞等问题，就更需要更为快速的算法。对此，本专利采用了采用蚁群搜索算法，对最优解进行搜索来进行具体的位置分配操作。

首先将赋权矩阵中的每一个值D_ij都作为一个单独的节点v_ij，得到一个无向完全图，图中的任意两点之间都连通。将无人机编号和新队形中的位置的编号作为节点的二维坐标，表示第i个位置由第j个无人机占据，每个节点都与赋权矩阵中的元素一一对应。

然后利用蚁群算法进行具体的位置分配操作，包括初始化、策略选择、信息素更新、局部搜索、求全局最优解等过程。

(2)编队队形重构中的指令计算

基于蚁群算法的位置分配最终得到形如F₁→P₄、F₂→P₁等匹配对。这些匹配对就代表了三维空间中的无人机重构时飞行轨迹相对与没有队形重构时飞行轨迹的变化矢量，参见图6，直线T₁T₂代表没有队形重构时，无人机在T₁时刻到T₂时刻内的飞行轨迹，由于发生了队形重构，使得实际轨迹成为直线T₁T₂′，无人机的实际轨迹产生了直线T₁T₁′所示的相对变化。而T₁T₁′就是匹配对所代表的航路，为了表述方便，将这些航路定义为相对航路。

本专利提出的控制策略就是将决策得到的相对航路与原先航路相叠加得到新的航路，分析队形变换的约束条件，计算出给出合适的指令，使多架无人机按照新的航路飞行，在要求的时刻到达新的位置。

对于包含n架无人机的编队，存在n条这样的直线。无人机队形重构的一个基本原则是所有的无人机都在时刻T₁开始构型重构，并在T₂时刻同时完成重构。

本发明实施例公开了一种飞机多机密集编队队形重构系统，包括激光雷达数据标定预处理模块、无人机定位识别模块和无人机编队队形重构模块，

激光雷达数据标定预处理模块，用于将接收到的无人机激光雷达坐标系转换为大地坐标系，得到机群中无人机的大地直角坐标系数据；

无人机定位识别模块，基于无人机在大地坐标系数据，计算无人机相对位置向量和无人机定位向量，比较两个向量之间模值、角度和方向，实现两个矢量的匹配，获得当前无人机机群内部各无人机的定位信息；(获得无人机定位向量对应的无人机编号)

无人机编队队形重构模块，用于获取当前无人机无人机机群内部各无人机的定位信息，根据初始编队队形和需要的编队队形，重新分配无人机的位置和航路。

本发明一实施例提供的终端设备的示意图。该实施例的终端设备包括：处理器、存储器以及存储在所述存储器中并可在所述处理器上运行的计算机程序。所述处理器执行所述计算机程序时实现上述各个方法实施例中的步骤。或者，所述处理器执行所述计算机程序时实现上述各装置实施例中各模块/单元的功能。

所述计算机程序可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器中，并由所述处理器执行，以完成本发明。

所述终端设备可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述终端设备可包括，但不仅限于，处理器、存储器。

所述处理器可以是中央处理单元(CentralProcessingUnit，CPU)，还可以是其他通用处理器、数字信号处理器(DigitalSignalProcessor，DSP)、专用集成电路(ApplicationSpecificIntegratedCircuit，ASIC)、现成可编程门阵列(Field-ProgrammableGateArray，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。

所述存储器可用于存储所述计算机程序和/或模块，所述处理器通过运行或执行存储在所述存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现所述终端设备的各种功能。

所述终端设备集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，RandomAccessMemory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。

以上仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种飞机多机密集编队队形重构方法，其特征在于，包括以下步骤：

S1：将接收到的无人机激光雷达坐标系转换为大地坐标系，得到机群中无人机的大地直角坐标系数据；

S2：基于无人机在大地坐标系数据，计算无人机相对位置向量和无人机定位向量，比较两个向量之间模值、角度和方向，实现两个矢量的匹配，获得当前无人机机群内部各无人机的定位信息；(获得无人机定位向量对应的无人机编号)

S3：获取当前无人机无人机机群内部各无人机的定位信息，根据初始编队队形和需要的编队队形，重新分配无人机的位置和航路。

2.根据权利要求1所述的一种飞机多机密集编队队形重构方法，其特征在于，所述S1包括以下步骤：

计算各无人机的欧式距离，采用快速排序法进行排序，直至得到最小的欧氏距离，根据最小的欧氏距离确定自身无人机与激光雷达扫描到的无人机之间的最小距离的坐标，并将该坐标转成大地直角坐标系数据。

3.根据权利要求2所述的一种飞机多机密集编队队形重构方法，其特征在于，所述S1中包括以下步骤：

确定无人机与其他各无人机的欧氏距离：

将无人机通过激光雷达探测到周边无人机的探测结果记录为C_k＝(x_k,y_k,z_k)，其中(x_k,y_k,z_k)为点的位置坐标，该探测到无人机到携带激光雷达的固定翼无人机的欧氏距离为：

其中，k为激光雷达探测到的无人机的三维点云数据中某一个点的编号，j为携带激光雷达的无人机编号，Xrj为x方向上编号j号的携带激光雷达的无人机的位置；

将接收到的无人机激光雷达坐标系转换为大地坐标系：

(1)从大地坐标系O到大地直角坐标系G的转换：

其中，

e表示椭球体的第一偏心率；a表示椭球体的长半径；

(2)从大地直角坐标系G到地理坐标系N的转换

其中，(x₀,y₀,z₀)为N系坐标原点在G系中的坐标，L₀和B₀为N系坐标原点的经纬度；

(3)从地理坐标系N到载机坐标系B的转换

绕X_n轴旋转φ的转换矩阵：

绕旋转后的Y_n绕旋转γ的转换矩阵：

绕旋转后的Z_n轴旋转θ的转换矩阵：

则从地理坐标系N到载机坐标系B的转换：

(4)从载机坐标系B到基座坐标系T的转换：

首先考虑载机坐标系和基座坐标系之间的安装误差，

绕Xb轴旋转Δφ_I的转换矩阵：

绕旋转后Yb轴旋转Δγ_I的转换矩阵：

绕旋转后的Zb轴旋转Δ的转换矩阵：

从载机坐标系B到基座坐标系T的转换：

经过上述步骤，实现激光雷达定位数据坐标系到大地坐标系的坐标的转换。

4.根据权利要求1所述的一种飞机多机密集编队队形重构方法，其特征在于，所述S2包括以下步骤：

基于无人机在大地直角坐标系下的位置信息，设无人机实际位置与预定位置的偏差为R，则无人机实际位置在预设位置为圆心，位置偏差R为半径的圆内，基于此计算无人机在大地坐标系下的相对位置向量，同时激光雷达测量无人机之间的相对位置，得到激光雷达定位向量。

5.根据权利要求4所述的一种飞机多机密集编队队形重构方法，其特征在于，所述S2包括以下步骤：

设无人机航线预定位置坐标为(X2，Y2，Z2)；激光雷达定位的位置坐标为(X12，Y12，Z12)，无人机所处位置坐标为(X1，Y1，Z1)，

则航线定位矢量为(X2-X1，Y2-Y1，Z2-Z1)，激光雷达定位矢量(X12，Y12，Z12)，预设算法参数δ1为1.5，δ2为0.4；

(1)两矢量模值之差为：

矢量匹配需满足D<δ1；

(2)两矢量角度值之差为：

在X-Y平面上的投影：

在X-Z平面上的投影：

在Y-Z平面上的投影：

当α+β+γ<δ2；

(3)符号同值

矢量匹配需满足：

(X12，Y12，Z12)与(X2-X1，Y2-Y1，Z2-Z1)同号，即{X12*(X2-X1)>0&Y12*(Y2-Y1)>0&Z12*(Z2-Z1)>0}同时成立，则认为两矢量匹配，完成无人机识别。

6.根据权利要求1所述的一种飞机多机密集编队队形重构方法，其特征在于，所述S3包括以下步骤：

建立赋权矩阵D∈D_n*n，其中，n为编队中的无人机数量，D_ij表示由原编队位置F_i飞向新的编队队形P_i所需的代价；

然后在不同行不同列中找到n个D，使得这n个D_ij的和最小，即：

min∑D_ij (16)

其中，n个D_ij就指明了每架无人机在新的编队中的位置，根据不同任务的需求，改变D_ij的定义则可以得到基于不同优化条件的位置分配结果。

7.根据权利要求6所述的一种飞机多机密集编队队形重构方法，其特征在于，所述S3包括以下步骤：

利用蚁群搜索算法对具体位置进行分配操作：

将赋权矩阵中的每一个值D_ij作为一个单独的节点v_ij，得到一个无向完全图，图中的任意两点之间能够连通，将无人机编号和新队形中的位置的编号作为节点的二维坐标，表示第i个位置由第j个无人机占据，每个节点都与赋权矩阵中的元素一一对应；

进一步利用蚁群算法进行无人机位置分配，得到三维空间中的无人机重构时飞行轨迹相对于没有队形重构时飞行轨迹的变化矢量，并进一步得到匹配后的相对航路。

8.根据权利要求1所述的一种飞机多机密集编队队形重构系统，其特征在于，包括激光雷达数据标定预处理模块、无人机定位识别模块和无人机编队队形重构模块；

激光雷达数据标定预处理模块，用于将接收到的无人机激光雷达坐标系转换为大地坐标系，得到机群中无人机的大地直角坐标系数据；

无人机定位识别模块，基于无人机在大地坐标系数据，计算无人机相对位置向量和无人机定位向量，比较两个向量之间模值、角度和方向，实现两个矢量的匹配，获得当前无人机机群内部各无人机的定位信息；(获得无人机定位向量对应的无人机编号)

无人机编队队形重构模块，用于获取当前无人机无人机机群内部各无人机的定位信息，根据初始编队队形和需要的编队队形，重新分配无人机的位置和航路。

9.一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1-7任一项所述方法的步骤。

10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-7任一项所述方法的步骤。

Images