本发明涉及无人集群定位领域，具体涉及一种基于毫米波雷达的集群无人机定位方法、系统、存储介质和电子设备。
背景技术：
1、近年来，无人机已逐渐走向实用，但受自身性能限制，无人机个体面对复杂环境和任务时颇显局限，因此，无人机技术发展呈现集群协同化的趋势。无人机集群是由一定数量的无人机组成，利用信息交互与反馈，激励与响应，实现相互间行为协同，可动态适应环境，共同完成特定任务的自主式智能系统。无人机集群以其远高于个体的群体智能优势，在军事和民用领域成为研究热点，可形成极高的军事、社会与经济效益。
2、协同编队是无人机集群执行任务的技术基础，多架无人机为适应任务要求而进行航迹规划，保持特定的几何队形排列飞行，可以降低无人机飞行能量需求，更高效地完成作业任务。目前无人机广泛采用卫星导航定位技术，但在拒止场景下，存在卫星导航定位稳定性差、不可靠等问题，在这种情况下，可以通过相对定位技术来测量集群个体间相对位置关系。在测量手段方面，现有无人机集群主要采用无线定位技术来实现相对定位：
3、例如，基于连续波信号的无线定位技术，如蓝牙、wifi(wireless fidelity)、紫蜂(zigbee)等，通常利用信号强度指示信息(rssi)或信道状态信息(csi)来估算距离，但存在覆盖范围小，受噪声干扰较大，定位不稳定且精度较低等问题。
4、再如，超宽带(uwb)定位技术基于接收到的多个窄脉冲信号的到达时间，使用tdoa(time difference of arrival)和tof(time of fight)算法来实现定位，其定位精度可以达到厘米级。但定位精度受到无人机机间时间同步精度、参与定位的无人机数量的影响，实现复杂度高。
技术实现思路
1、(一)解决的技术问题
2、针对现有技术的不足，本发明提供了一种基于毫米波雷达的集群无人机定位方法、系统、存储介质和电子设备，解决了传统无线定位技术测量精度低的技术问题。
3、(二)技术方案
4、为实现以上目的，本发明通过以下技术方案予以实现：
5、一种基于毫米波雷达的集群无人机定位方法，基于长机-僚机编队模式，该方法由长机执行，包括：
6、s1、根据所规划航迹的关键路径点集合和集群僚机运动模型，估计集群编队在未来的测量时刻t的几何拓扑结构，以得到集群僚机相对位置的期望值；其中每一架无人机的规划航迹是指根据目标任务协同规划出从起飞点到任务目标的飞行航迹；
7、s2、截止测量时刻t之前，结合导航失效前的历史位置坐标以及惯性测量单元的测量信息，实时计算得到wgs84坐标系下的长机自身位置坐标；
8、s3、到达测量时刻t，利用毫米波雷达对集群僚机进行测量，得到僚机相对于长机的位置，以得到集群僚机相对位置的测量值；
9、s4、对集群僚机相对位置的期望值和测量值进行关联处理，建立僚机编号与其相对位置测量值的对应关系；
10、s5、向每架僚机发送其相对位置测量值和长机自身位置坐标，以在僚机处计算得到wgs84坐标系下的僚机自身位置坐标。
11、优选的，发生卫星导航拒止时，长机向僚机广播导航失效消息，集群切换到相对定位模式，以执行s1～s5；否则保持卫星导航定位模式。
12、优选的，所述s1包括：
13、s101、进行wgs84坐标系到地心空间直角坐标系的坐标转换，转换完成后，得到测量时刻t长机和所有僚机在地心空间直角坐标系的位置坐标集合；其中转换表达式如下：
14、
15、其中，(l,b,h)分别表示wgs84坐标下，关键路径点的大地经度、大地维度、大地高度，(x,y,z)为关键路径点在地心空间直角坐标系的直角坐标；
16、s102、进行地心空间直角坐标系到载机地理坐标系的坐标转换，转换完成后，得到测量时刻t长机和所有僚机在地理坐标系下的坐标集合；
17、以长机为载机地理坐标系原点，令其wgs84坐标为(l0,b0,h0)，地心空间直角坐标为(x0,y0,z0)，则地心空间直角坐标为(xw,yw,zw)的僚机，在此地理坐标系中的位置坐标计算式如下：
18、
19、其中，坐标变换矩阵
20、s103、根据所述坐标集合得到各僚机相对长机的距离、方位和俯仰，以得到测量时刻t集群僚机相对位置的期望值。
21、优选的，所述s4包括：
22、采用数据关联算法对集群僚机的相对位置期望值和测量值进行关联；当发生部分僚机无法关联到相对位置测量值的情况时，重新执行s3进行测量；若达到连续测量的最大次数后，仍存在有僚机不能与相对位置测量值相关联，则将此僚机的相对位置期望值作为其测量值。
23、优选的，所述s5包括：
24、s501、进行相对位置到载机坐标系的转换；
25、令长机测量的僚机相对位置为(r,a,θ)，僚机在长机的载机坐标系中用直角坐标(xp,yp,zp)表示为下式：
26、
27、其中，r、a、θ分别表示测量的距离、俯仰和方位；
28、s502、进行载机坐标系到载机地理坐标系的坐标转换；
29、僚机在长机的载机地理坐标系中的直角坐标(xl,yl,zl)表示为：
30、
31、其中，β、ε、γ分别为载机的偏航角、俯仰角、横滚角；
32、s503、进行载机地理坐标系到地心空间直角坐标系的坐标转换；
33、已知长机在测量时刻t的位置坐标载机地理坐标系到地心坐标系的坐标旋转矩阵lba表示为下式：
34、
35、其中，ld、le分别为载机地理坐标系与地心空间直角坐标系的d、e轴旋转矩阵；
36、经过坐标旋转平移后，僚机在地心空间直角坐标系的空间坐标(x,y,z)表示为：
37、
38、s504、进行地心空间直角坐标系到wgs84坐标系的坐标转换，转换式如下：
39、
40、其中，由于变量b和h相互嵌套，使用递归法或近似直接法求解，以最终得到wgs84坐标系下的僚机自身位置坐标。
41、一种基于毫米波雷达的集群无人机定位系统，基于长机-僚机编队模式，该系统由长机执行，包括：
42、估计模块，用于根据所规划航迹的关键路径点集合和集群僚机运动模型，估计集群编队在未来的测量时刻t的几何拓扑结构，以得到集群僚机相对位置的期望值；其中每一架无人机的规划航迹是指根据目标任务协同规划出从起飞点到任务目标的飞行航迹；
43、第一计算模块，用于截止测量时刻t之前，结合导航失效前的历史位置坐标以及惯性测量单元的测量信息，实时计算得到wgs84坐标系下的长机自身位置坐标；
44、测量模块，用于到达测量时刻t，利用毫米波雷达对集群僚机进行测量，得到僚机相对于长机的位置，以得到集群僚机相对位置的测量值；
45、关联模块，用于对集群僚机相对位置的期望值和测量值进行关联处理，建立僚机编号与其相对位置测量值的对应关系；
46、第二计算模块，用于向每架僚机发送其相对位置测量值和长机自身位置坐标，以在僚机处计算得到wgs84坐标系下的僚机自身位置坐标。
47、一种存储介质，其存储有用于基于毫米波雷达的集群无人机定位的计算机程序，其中，所述计算机程序使得计算机执行如上所述的集群无人机定位方法。
48、一种电子设备，包括：
49、一个或多个处理器；存储器；以及一个或多个程序，其中所述一个或多个程序被存储在所述存储器中，并且被配置成由所述一个或多个处理器执行，所述程序包括用于执行如上所述的集群无人机定位方法。
50、(三)有益效果
51、本发明提供了一种基于毫米波雷达的集群无人机定位方法、系统、存储介质和电子设备。与现有技术相比，具备以下有益效果：
52、本发明首次将毫米波雷达技术用于解决无人机集群定位问题，能够实现高精度的距离、方位、俯仰、速度测量。相比于现有的无线定位技术，测量信息更丰富，测量精度更高。与此同时，本发明提出了解决卫星导航定位和集群相对定位这两种定位方式的坐标统一问题，便于编队控制器的设计，以保证集群在特定的位置保持特定的编队几何结构。
1.一种基于毫米波雷达的集群无人机定位方法，其特征在于，基于长机-僚机编队模式，该方法由长机执行，包括：
2.如权利要求1所述的集群无人机定位方法，其特征在于，
3.如权利要求1所述的集群无人机定位方法，其特征在于，所述s1包括：
4.如权利要求1所述的集群无人机定位方法，其特征在于，所述s4包括：
5.如权利要求1所述的集群无人机定位方法，其特征在于，所述s5包括：
6.一种基于毫米波雷达的集群无人机定位系统，其特征在于，基于长机-僚机编队模式，该系统由长机执行，包括：
7.一种存储介质，其特征在于，其存储有用于基于毫米波雷达的集群无人机定位的计算机程序，其中，所述计算机程序使得计算机执行如权利要求1～5任一项所述的集群无人机定位方法。
8.一种电子设备，其特征在于，包括：
