一种多无人机协同定位与目标分配方法

本发明属于飞机，尤其涉及无人机，具体为一种多无人机协同定位与目标分配方法。

背景技术：

1、随着小型无人机技术的快速发展，低空无人机活动日益频繁。在多无人机协同任务与管控场景中，需要对多个空中目标快速完成定位并合理分配任务资源。现有技术方案存在以下主要问题：

2、（1）多机全配置方案成本高昂：现有协同定位方案通常要求编队中每架无人机均配置激光测距仪或高精度惯性导航设备，硬件成本高，重量大，不利于小型无人机平台搭载。

3、（2）单机定位精度受限：仅依赖gps或单目图像进行目标定位时，距离估计误差大，尤其在高度方向上误差显著，难以满足实际任务精度需求。

4、（3）目标分配缺乏最优化：现有方案多采用就近原则或预设规则进行目标分配，未能从系统层面最小化整体代价（如总飞行距离或时间），导致任务效率低下。

5、（4）信息共享机制缺失：在多机协同场景中，各机独立定位而缺乏有效的目标坐标广播机制，造成重复定位资源浪费。

6、因此，亟需一种低成本、高精度、可工程实现的多无人机协同定位与目标分配方法，以解决上述技术问题。

技术实现思路

1、为克服现有多无人机编队方法存在定位精度差、成本高等技术问题，本发明提供了一种多无人机协同定位与目标分配方法。

2、本发明提供了一种多无人机协同定位与目标分配方法，包括以下步骤：

3、s1、系统初始化与角色设定：设无人机编队中无人机的总数量为n，需要对m个目标进行分配，将无人机编队中其中一架作为主控无人机，剩余均作为协同无人机，其中主控无人机配置有带激光测距功能的双光吊舱，其用于目标定位与任务分配；协同无人机配置有数据链通信设备，其用于接收广播信息；

4、s2、主控无人机在其编队视野中选定一个参考目标进行激光测距，获取主控无人机与参考目标之间的径向距离，同时通过其双光吊舱采集包含参考目标及编队视野内其他目标的当前帧图像，记录该激光测距时刻双光吊舱的吊舱姿态传感器输出的横滚角、俯仰角、偏航角以及主控无人机的gps位置坐标（,,），得到参考目标在enu坐标系下的三维坐标向量；

5、s3、在所述当前帧图像中，利用目标检测算法获取各目标的像素边界框，设参考目标的像素边界框面积为，第个目标的像素边界框面积为，则参考目标对应的像素尺寸以及其余目标对应的像素尺寸分别为：

6、，

7、，

8、根据针孔相机模型，目标在当前帧图像中的成像尺寸与目标实际物理尺寸及目标距离之间满足以下比例关系：

9、，

10、其中为针孔相机的焦距；

11、则，参考目标以及其余目标对应的成像尺寸分别为：

12、，

13、，

14、假设所有目标的物理尺寸相同，且的情况下，得到：

15、，

16、则主控无人机与第个目标的径向距离估算公式为：

17、，

18、其中，，和分别为参考目标的边界框像素宽度和高度；为第个目标在当前帧图像中的像素尺寸，，和分别为第个目标的边界框像素宽度和高度；

19、基于，得到第个目标在enu坐标系下的三维坐标向量为：

20、，

21、其中，为主控无人机的双光吊舱在对准第个目标时的相机坐标系单位方向向量，通过第个目标在当前帧图像中的像素坐标与相机参数反算得到，r为当前帧图像对应的吊舱姿态旋转矩阵；

22、s4、将第个目标在enu坐标系下的三维坐标向量转换为ecef坐标，再将ecef坐标转换为wgs-84大地坐标；

23、s5、将个目标分配给个无人机，其子步骤为：

24、s51、代价矩阵元素计算：

25、代价矩阵的元素表示第架无人机飞往第个目标的欧氏距离代价，其中，，，由以下公式计算：

26、，

27、其中，为第架无人机当前时刻的ecef坐标，其由各协同无人机通过gps实时获取并上报，再通过数据链汇总至主控无人机，为第j个目标的ecef坐标，其由s2～s4步骤计算得出；

28、s52、目标函数与约束条件：

29、以最小化编队总飞行距离为目标函数，其公式为：

30、，

31、其中，为分配决策变量，表示将目标分配给无人机，约束条件为：

32、，

33、，

34、；

35、s53、利用匈牙利算法进行求解，其子步骤为：

36、s531、将代价矩阵中的每行减去该行最小值，使每行至少有一个零元素；

37、s532、将s531处理后的代价矩阵每列减去该列最小值，使每列至少有一个零元素；

38、s533、寻找独立零元素集合，即互不同行同列，若找到n个独立零，则得到最优分配；

39、s534、若独立零少于n个，通过最少直线覆盖所有零元素，更新矩阵，即未覆盖元素减去最小未覆盖值，双覆盖元素加回，重复步骤s533，直至找到n个独立零；

40、匈牙利算法输出最优分配矩阵，其中，表示将目标最优分配给无人机，最优总代价为：

41、；

42、s6、主控无人机通过数据链以单向广播形式，将包含各协同无人机对应的分配目标信息的广播数据包发送至编队所有成员，其中广播数据包包括目标编号、目标wgs-84纬度、目标wgs-84经度、目标大地高以及对应分配无人机编号，各协同无人机收到广播后，按照分配到的wgs-84大地坐标独立规划并执行飞行任务；协同无人机无需回复确认，主控无人机默认广播成功。

43、本发明中，编队中仅主控无人机配备高精度的激光测距仪和双光吊舱，其余大量协同无人机仅配备数据链通信设备和低成本可见光吊舱，打破了每架无人机都必须配备激光测距仪的传统高成本模式，协同无人机无需昂贵测距设备，仅靠视觉和广播指令即可执行任务，降低了整个无人机编队的硬件造价和维护成本。

44、本发明中，将激光的绝对精确性与视觉的相对比例性融合，仅用一次激光测距即可获取所有目标的三维坐标。利用同一帧图像规避了多目标分时测量导致的运动误差，确保了同一时刻目标空间位置的相对一致性。

45、不动点迭代方法确保了在大地高计算中的高收敛精度（达弧度），保障了远距离飞行时坐标的绝对准确。

46、本发明将分配问题转化为线性指派问题，构建欧氏距离代价矩阵，并采用匈牙利算法进行全局最优求解。确保编队整体飞行总航程最短，节约了编队整体燃油/电量消耗，提升了单次任务的可覆盖范围。每机一个目标，每个目标仅被分配一次，杜绝了资源冲突和任务遗漏，使任务执行井然有序。

47、优选的，步骤s2的子步骤为：

48、s21、吊舱光轴在相机坐标系下的单位方向向量为：

49、；

50、s22、由吊舱姿态传感器实时采集的横滚角、俯仰角和偏航角，按zyx欧拉角顺序构造吊舱姿态旋转矩阵r，将相机坐标系向量转换至enu坐标系，得到：

51、，

52、其中各基本旋转矩阵为：

53、，

54、，

55、，

56、s23、以主控无人机的当前位置为原点，计算参考目标在enu坐标系下的三维坐标向量为：

57、，

58、其中，为姿态旋转矩阵，为吊舱光轴在相机坐标系下的单位方向向量，为主控无人机与参考目标之间的径向距离，的形式为[,,]t，为参考目标相对主控无人机的东向偏移量，为参考目标相对主控无人机的北向偏移量，为参考目标相对主控无人机的天向偏移量。

59、优选的，步骤s4的子步骤为：

60、s41、以主控无人机的gps位置（,,）为enu原点，将其转换为ecef坐标（,,），wgs-84大地坐标的椭球参数分别为：长半轴，扁率倒数，第一偏心率，则主控无人机当前位置的ecef坐标计算公式为：

61、，

62、其中，，

63、，

64、，

65、第个目标的ecef坐标由主控无人机当前位置的ecef坐标叠加enu偏移量得到：

66、，

67、其中，为转换矩阵，

68、，

69、s42、由第个目标的ecef坐标计算wgs-84大地坐标，采用不动点迭代方法，其子步骤为：

70、s421、计算大地经度：

71、，

72、s422、计算赤道投影距离：

73、，

74、s423、迭代计算大地纬度，迭代初始值设为，并按照以下公式进行迭代更新，直至相邻两次迭代结果之差小于弧度：

75、，

76、，

77、s424、计算大地高度：

78、，

79、，

80、最终得到第个目标的wgs-84大地坐标（,,），即得到纬度、经度和大地高度。

81、本发明提供的技术方案与现有技术相比具有如下技术效果：

82、一、大幅降低硬件成本：无人机编队中仅需一台具有激光测距功能的双光吊舱（配置于主控无人机），其余协同无人机配置数据链通信设备，系统整体硬件成本降低约60%～75%。

83、二、实现多目标高精度定位：对激光直接测距目标的定位误差优于2m（enu综合，90%分位），对等尺寸假设目标的定位误差在当前噪声水平下均值约41～60m，可满足无人机集群管控的任务精度需求。

84、三、全局最优任务分配：匈牙利算法保证最小化编队总飞行距离，相较就近原则平均降低总代价约15%～30%。

85、四、工程实现简单、实时性好：算法核心由矩阵运算和线性规划组成，计算量小，可在嵌入式飞控计算机上实时运行，满足任务型无人机的快速响应需求。

86、五、可扩展性强：本发明所述方法适用于n架无人机对m个目标的通用场景（），通过扩展代价矩阵维度即可直接适配不同编队规模。