多无人机位姿跟踪方法、装置及设备

本技术涉及无人机跟踪，特别是涉及一种多无人机位姿跟踪方法、装置及设备。

背景技术：

1、6d位姿跟踪在增强现实，机器人操作和无人机编队等领域逐渐得到广泛的应用，其目的是充分利用序列图像提供信息以提高位姿解算精度和解决遮挡等难题。在无人机编队中，受到编队队形和多无人机形状相似影响，跟踪长机的僚机视场会出现形状相似的多无人机互相遮挡情况。为了让僚机持续跟踪编队长机，我们提出基于一种基于光流位姿相似性和粒子滤波的鲁棒的多无人机6d位姿跟踪方法。

2、目前主流的6d位姿跟踪方法有基于关键点、基于边缘和基于区域三类方法。这些方法需要手工设计特征和细致调整超参数，缺乏泛化能力。近年来一些端到端方法提出来提升6d位姿估计的精度，但其回归精度有限。为了解决对称物体的位姿预测问题，poserbpf将rao–blackwellized粒子滤波和学习自动编码器网络结合，有效跟踪任意种类的对称物体的6d位姿。微创机器人采用椭圆匹配观测模型作为粒子滤波的观测模型，结合机械臂输出的运动信息，对无标记的手术缝合针进行6d位姿跟踪。但这些方法都无法直接运用到编队多无人机跟踪场景，主要在于其无法解决形状形似无人机互相遮挡等问题。

技术实现思路

1、基于此，有必要针对上述技术问题，提供一种能够有效区分编队中的多无人机，并且能够持续进行跟踪的多无人机位姿跟踪方法、装置及设备。

2、一种多无人机位姿跟踪方法，所述方法包括：

3、获取无人机初始位姿，然后基于所述无人机初始位姿，初始化粒子滤波，得到无人机粒子分布；以及

4、基于所述无人机初始位姿，通过人体迁移网络模型进行预测，得到无人机骨架与部件检测；

5、根据所述无人机骨架与所述部件检测，对两时刻之间的无人机粒子分布进行相似度判断，然后采用贪婪算法进行匹配，得到检测结果；其中，当通过前若干时刻的无人机粒子分布进行相似度判断时检测到某轨迹无人机，而当前时刻没有检测到该无人机，则通过前若干时刻能检测到的该无人机骨架进行传播到当前时刻进行补充；

6、获取前一时刻各无人机的位姿分布，将前一时刻各无人机的位姿分布输入匀速运动模型进行姿态分布预测，得到当前时刻各无人机的姿态分布；

7、基于所述检测结果对当前时刻各无人机的姿态分布进行权重更新，得到位姿跟踪结果。

8、其中一个实施例中，获取无人机初始位姿，然后基于所述无人机初始位姿，初始化粒子滤波，得到无人机粒子分布，包括：

9、估算视场范围内各无人机的初始6d位姿，确定初始化方差；

10、根据所述初始6d位姿与所述初始化方差，随机抽取若干粒子作为初始粒子，然后计算各粒子的初始权重；

11、通过对各粒子的初始权重进行处理，得到无人机粒子分布。

12、其中一个实施例中，所述初始权重表达式为：

13、

14、式中，x是m×6维度向量代表初始粒子；μ是均值向量以初始位姿表示；∑＝covpose表示初始化方差矩阵。

15、其中一个实施例中，所述部件检测表示为：

16、

17、式中，表示无人机中心位置为(bx,by)的具有等宽高(bw＝bh)的关键点边界框；conf表示置信度；classk,k＝1...k+1表示部件类别与无人机类别；

18、所述无人机骨架包括目标边界框预测与骨架端点，其中：

19、目标边界框预测表示为：

20、pi＝[px,py,pw,ph,conf,class1,class2,...,classk+1,(x1,y1),...,(xk,yk)]；

21、骨架端点表示为：

22、

23、式中，pi表示无人机预测中心位置为(px,py)的具有宽高分为(pw,ph)的无人机边界框；i＝1,2,...,n表示无人机数量；(xk,yk)表示骨架端点在图像上坐标位置。

24、其中一个实施例中，根据所述无人机骨架与所述部件检测，对两时刻之间的无人机粒子分布进行相似度判断，然后采用贪婪算法进行匹配，得到检测结果，包括：

25、基于所述无人机骨架与所述部件检测，通过光流位姿相似度对两时刻之间的无人机粒子分布进行相似度判断，得到匹配结果；

26、然后通过贪婪算法分别计算各匹配结果从属于各类别无人机的相似度，根据相似度赋予无人机类别，从而得到检测结果。

27、其中一个实施例中，所述光流位姿相似度表达式为：

28、

29、式中，表示使用光流场将第ir时刻传递到il时刻的无人机实例；oks代表计算两架无人机的对象关键点相似度。

30、其中一个实施例中，所述匀速运动模型表示为：

31、

32、式中，(vxk,vyk)表示第k个骨架端点的二维点；intr表示相机内参矩阵；pk表示3d点；r表示无人的旋转矩阵；t表示位移矩阵。

33、其中一个实施例中，基于所述检测结果对当前时刻各无人机的姿态分布进行权重更新，得到位姿跟踪结果，包括：

34、基于所述检测结果，通过权重更新矩阵对当前时刻各无人机的姿态分布进行权重更新，得到最优位姿跟踪结果；

35、所述权重更新矩阵表示为：

36、

37、位姿跟踪结果表示为：

38、

39、式中，是指t-1时刻的第i架无人机的第m枚粒子的权重；是指第t时刻第i架无人机的第m枚粒子的权重；表示第i架无人机第m枚粒子骨架端点与网络预测骨架端点在图像上的欧氏距离；表示第t-1时刻第i架无人机的第m枚粒子的位姿；表示第t时刻第i架无人机第m枚粒子的位姿。

40、一种多无人机位姿跟踪装置，所述装置包括：

41、初始化模块，用于获取无人机初始位姿，然后基于所述无人机初始位姿，初始化粒子滤波，得到无人机粒子分布；以及

42、骨架预测模块，用于基于所述无人机初始位姿，通过人体迁移网络模型进行预测，得到无人机骨架与部件检测；

43、相似性匹配模块，用于根据所述无人机骨架与所述部件检测，对两时刻之间的无人机粒子分布进行相似度判断，然后采用贪婪算法进行匹配，得到检测结果；其中，当通过前若干时刻的无人机粒子分布进行相似度判断时检测到某轨迹无人机，而当前时刻没有检测到该无人机，则通过前若干时刻能检测到的该无人机骨架进行传播到当前时刻进行补充；

44、位姿跟踪模块，用于获取前一时刻各无人机的位姿分布，将前一时刻各无人机的位姿分布输入匀速运动模型进行姿态分布预测，得到当前时刻各无人机的姿态分布；基于所述检测结果对当前时刻各无人机的姿态分布进行权重更新，得到位姿跟踪结果。

45、一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时上述任一项所述方法的步骤。

46、上述多无人机位姿跟踪方法、装置及设备，通过获取无人机初始位姿，然后基于无人机初始位姿，初始化粒子滤波，得到无人机粒子分布；以及基于无人机初始位姿，通过人体迁移网络模型进行预测，得到无人机骨架与部件检测；根据无人机骨架与部件检测，对两时刻之间的无人机粒子分布进行相似度判断，然后采用贪婪算法进行匹配，得到检测结果；其中，当通过前若干时刻的无人机粒子分布进行相似度判断时检测到某轨迹无人机，而当前时刻没有检测到该无人机，则通过前若干时刻能检测到的该无人机骨架进行传播到当前时刻进行补充；获取前一时刻各无人机的位姿分布，将前一时刻各无人机的位姿分布输入匀速运动模型进行姿态分布预测，得到当前时刻各无人机的姿态分布；基于检测结果对当前时刻各无人机的姿态分布进行权重更新，得到位姿跟踪结果。

47、本发明通过人体迁移网络模型提供无人机骨架检测结果，同时追溯前几时刻进行相似性匹配以及对网络检测的缺失进行补充，然后将检测结果作为粒子滤波的观测模型，并提出匀速运动模型作为粒子滤波的预测模型，将预测的前时刻各无人机的姿态分布和检测结果作为更新的粒子权重，得到最优的位姿跟踪结果，避免了形状相似的无人机互相遮挡而无法识别的问题，实现无人机的6d位姿跟踪，鲁棒性好，能够持续进行跟踪。