一种基于双目深度点云的无人机高精度相对定位方法

本发明涉及传感器信息融合，尤其涉及一种基于双目深度点云的无人机高精度相对定位方法。

背景技术：

1、无人机(uav)之间的相对定位技术是在多学科交叉的基础上发展起来的复杂领域，主要受到军事需求的推动。起初，这项技术被设计用于执行复杂的编队飞行和战术任务，以提高军事作战的效率和安全性。随着时间的推移，无人机在民用领域的应用日益增多，如灾害监控、交通管理、农业监测等，这进一步促进了相对定位技术的发展。相对定位技术使无人机能够在没有外部参考点的情况下，准确地掌握彼此的位置和运动状态，这对于执行精确的团队任务至关重要。

2、在现代战争场景中，无人机在执行超长时间侦察和超长距离打击任务中扮演着越来越重要的角色。然而，这些任务的执行受到无人机载荷和燃油容量的限制，为了克服这些限制，空中加油技术被使用，其目的是在不对无人机结构做重大修改的情况下，显著提升无人机的飞行范围、空中停留时间以及有效载荷。这一技术的应用，显著增强了无人机在长距离飞行、目标侦测和武器携带等任务中的性能。而在空中加油过程中，精确的相对定位技术显得尤为重要，因为加油机和无人机必须在飞行中保持严格的相对位置，以确保加油操作的安全和效率。这种技术的挑战在于，两架无人机需要在高速飞行中实现极高的定位精度和稳定性，同时还要考虑到空气动力学效应和潜在的环境干扰。总的来说，以无人机高精度相对定位为基础的空中加油技术的发展不仅提高了无人机在现代战争中的作战能力，也推动了无人机技术在自动导航和精确控制领域的进步。通过不断的技术创新，无人机的应用范围和效率将持续扩大，为现代军事行动提供更大的灵活性和更强的战略优势。

3、空中加油的对接阶段要求受油无人机在有限的自主操作权限内，能够自动解析传感器数据并生成相应的控制指令，以实现精确、高效和安全的加油操作。这不仅是一个飞行技术的挑战，也是对无人机自主导航和自动控制系统的考验。

4、首先，从受油机的姿态控制角度来看，确保对接的安全和稳定性至关重要。这需要无人机具备高度精准的飞行路径控制和速度匹配能力。受油无人机必须能够与加油机高度协调，并且能够实时反馈和调整，以应对动态环境中的任何变化。这不仅要求无人机具备高级的飞行控制系统，还需要它能实时处理大量的传感器数据，以实现精准的飞行动态调整。

5、同时，加油机锥套的精准检测与定位对于成功的空中加油至关重要。这需要无人机的传感器和导航系统能够精确识别加油机的位置和状态，并据此进行精确的飞行调整。无人机需要能够在多变的气象条件和可能的电子干扰下，准确地与加油机对接。此外，加油过程中可能发生的任何微小误差都必须被实时检测并纠正，以避免碰撞或其他危险情况的发生。

6、关于锥套的检测和定位，这是空中加油技术中的一个关键环节，但现有技术尚存在一定的局限性。虽然有多种方法被使用来解决这一挑战，但各自都有其限制。例如，惯性测量单元(imu)和全球定位系统(gps)导航系统在大部分情况下表现良好，但在某些复杂的飞行环境中，它们可能无法提供足够精确的导航信息。这主要是因为在高速飞行和复杂气象条件下，gps信号可能会受到干扰，而imu可能积累较大的误差。依赖于红外和激光雷达(lidar)的传感技术虽然能提供精确的距离和速度数据，但它们对环境光线的变化非常敏感，这可能导致在某些特定光照条件下性能不稳定，例如，在强烈的阳光或云层变化下，红外和激光传感器的效果可能会受到影响。并且为满足这些传感器在相对定位方面的特殊要求，可能需要对加油管和锥套进行显著的设计改进。这不仅增加了系统的实施难度，还提高了整个加油系统的复杂性和成本。

7、因此，为应对这些挑战，本专利采用基于模型预测控制(mpc)的方法作为一种创新且高效的解决方案。

技术实现思路

1、本发明的目的是提供一种基于双目深度点云的无人机高精度相对定位方法，使无人机在进行空中相对定位，快速且准确地识别目标物，以及实现精确的姿态控制。

2、为实现上述目的，本发明提供了一种基于双目深度点云的无人机高精度相对定位方法，包括以下步骤：

3、s1、使用双目相机捕捉目标物图像；

4、s2、基于步骤s1双目相机捕获的图像数据，利用双目立体视觉算法实时生成整个视野内的深度点云；

5、s3、对yolov7检测模型的主干网络进行优化；

6、s4、使用经过优化的yolov7模型对从双目相机获取的图像进行精确检测，并生成对应的2d检测框；

7、s5、根据2d检测框在深度点云中定位锥套目标的三维空间位置，生成相应的3d点云检测区域，优化点云稠密区域的计算，并应用均值滤波来提高点云数据的质量和准确性；

8、s6、对所确定的目标点进行持续追踪，并在连续图像中存储目标点的位置信息；

9、s7、通过计算位置信息，得出目标点的平均相对速度；

10、s8、基于模型预测控制算法，准确调整无人机的姿态，对无人机进行控制。

11、优选的，步骤s2实时生成整个视野内深度点云，包括图像的校正、匹配及深度计算，以获得空间中各点的三维坐标，具体如下所示：

12、s21、图像的校正，具体步骤如下：

13、s211、使用张氏标定法获得摄像头的畸变系数，结合摄像头厂家给出的内参，标定两个摄像头之间的相对位置和姿态，即外参包括旋转和平移；

14、s212、根据上述步骤s211获得的参数，计算立体校正所需的变换矩阵；这包括两个旋转矩阵，分别用于左右摄像头图像的校正，以及两个新的投影矩阵。

15、s213、利用上述旋转矩阵和投影矩阵，对原始图像进行重映射，如下所示：

16、p1'＝k1'r1r-1k1-1p1

17、p2'＝k2'r2(r-1k2-1p2+t)

18、其中，k1和k2分别为左右摄像头的内参矩阵，r和t分别为两个摄像头之间的旋转矩阵和平移向量，k1'、k2'以及r1和r2为需要重新计算的内参矩阵和旋转矩阵，p1'和p2'为校正后的像素位置，使用上述变换，可以将两个摄像头的图像转换到共同的视平面内，并使得同一物体点在两幅图像中的投影具有相同的y坐标；

19、s22、基于改进的代价函数，通过引入自适应高斯权重矩阵和归一化，对图像进行匹配，公式如下：

20、

21、其中，w为窗口区域，用于定义在每个像素周围考虑的区域大小；il,ir为两个待比较的图像；p,q分别为图像il,ir中的对应像素点，i,j为窗口区域内相对位置偏移量；w(i,j,p)为自适应高斯权重，(p+i,p+j)为偏移后的像素点；

22、s23、深度计算公式如下：

23、

24、其中，z是像素的深度，f为摄像头焦距；b是两个摄像头之间的基线距离，即两个摄像头的水平距离；d是视差值，即同一场景点在两个摄像头成像上的水平位置差；

25、s24、将2d图像坐标和深度值统一转换为3d空间坐标，公式如下：

26、

27、其中，(x,y)为图像平面中像素点的坐标，(x,y,z)为世界坐标系中像素点的坐标，(cx,cy)为图像的光心坐标。

28、优选的，在步骤s3对yolov7检测模型的主干网络进行优化中，具体步骤如下：

29、s31、设置卷积层权重阈值，并移除小于该阈值的权重，公式如下：

30、

31、其中，ωi,j'为剪枝后的权重矩阵，θ为预设的阈值，ωi,j为权重矩阵中第i行第j列的元素；

32、s32、使用cbam模块进行空间注意力加权，并调整cbam的权重分配方式，如下所示：

33、scone(x,y)＝σ(fcentre(i(x,y))+fdark(i(x,y)))

34、其中，σ为激活函数，fcentre和fdark分别对应加油锥套中心区域和深色色块的注意力权重；

35、s33、将空间注意力图scone使用逐元素乘法应用于原始特征图f，如下所示：

36、

37、s34、采用深度可分离卷积来更加高效地提取加油锥套的特征，使用中等尺度的特征图来进行检测，公式如下：

38、pcone＝fmid

39、其中，pcone是用于检测加油锥套的特征图，fmid是网络中尺度适中的特征图。

40、优选的，在步骤s5中，具体步骤如下：

41、s51、根据2d检测框在深度点云中定位锥套目标的三维空间位置，并生成相应的3d点云检测区域，剔除远离摄像机的物体或过于接近的物体，如下所示：

42、

43、其中，droi(x,y)表示检测区域的点云深度值；

44、s52、优化点云稠密区域的计算，公式如下：

45、

46、其中，l为指示函数，为给定的条件生成一个二进制输出，如果条件为真，它返回1，否则返回0；

47、s53、对有效点云进行均值滤波，公式如下：

48、

49、

50、优选的，在步骤s6存储目标点的位置信息中，计算稠密点云质心位置作为目标点，公式如下：

51、

52、

53、

54、其中，(xcenter,ycenter,zcenter)是3d空间中的相对目标点。

55、优选的，在步骤s7计算目标点的平均速度，如下所示：

56、

57、其中，pi＝(xi,yi,zi)为第i帧图像中目标点的位置坐标。

58、优选的，在步骤s8构建无人机的状态和控制输入建模中，基于模型预测控制算法，无人机的状态标量包括位置(x,y,z)、偏航角θ、俯仰角ψ；

59、控制标量包括：速度v、偏航速率yaw、和滚转速率pitch；则无人机的动力学模型可以表示为：

60、

61、优选的，基于无人机的动力学模型，针对空中对接加油场景，目标函数如下所示：

62、s811、首先，计算最小化无人机与目标点之间的距离，公式如下：

63、

64、其中，(xcenter,k,ycenter,k,zcenter,k)为当前目标点相对于无人机的位置，ωpos是位置误差的权重；

65、s812、其次，最小化无人机的飞行速度变化率，公式如下：

66、

67、其中ωctrl是控制输入变化的权重；

68、s813、最后，目标函数如下所示：

69、

70、优选的，基于无人机的动力学模型，针对多无人机相对定位场景，目标函数如下所示：

71、s821、通过计算无人机当前位置和目标位置之间的欧几里得距离来实现跟踪误差，如下所示：

72、

73、其中，xk是无人机第k个时间步的位置，ω1是权重因子；

74、s822、视场角修正，如下所示：

75、

76、其中，ω2是权重因子，ellipse是视场角和无人机位置的函数；此函数可以根据无人机的位置、方向以及视场角度来计算；

77、s823、在3d空间中，将视场范围视作一个椭圆体，目标位于该椭圆体内部，椭圆函数公式如下：

78、

79、其中，(x,y,z)是无人机的位置，(xu,yu,zu)是目标点的位置。

80、因此，本发明采用上述一种基于双目深度点云的无人机高精度相对定位方法，有益效果如下：

81、1、使用检测框内的点云稠密区域的质心代替检测框的中心点作为目标点。在实际的三维空间中，物体的形状和姿态可能导致其表面的一部分更接近相机，并且目标不一定可以充满检测框，使用质心可以更准确地表示物体的实际位置，同时避免检测框抖动带来的检测抖动。

82、2、在目标移动或旋转时，质心的位置变化比检测框中心的位置变化更平滑，从而提供更稳定的目标追踪。

83、3、通过考虑未来几个时间步(离散时间间隔)内的无人机状态和环境变化，实现先进的路径规划和无人机对接，通过不断更新目标位置，提出的mpc方案使无人机能够动态跟踪移动目标，适用于空中加油的场景，同时对搜索与救援、监视等应用也尤为重要。

84、4、特别考虑了有限视野的可见性约束。这确保了无人机在执行任务跟踪时，目标始终保持在相机视野内，mpc控制器能够以较高频率生成控制指令，这在多数嵌入式计算平台上是可行的，表明该方案适合实时应用。

85、下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。