一种多传感器融合的自然保护区无人机三维空间定位方法与流程

本发明涉及一种无人机定位方法，尤其涉及一种多传感器融合的自然保护区无人机三维空间定位方法。

背景技术：

1、对自然保护区进行监测往往需要使用无人机采集数据。为了知晓监测区域的精确位置或对监测区域进行三维建模，需要得到无人机在三维空间中的位姿。一般的无人机三维空间定位方法中，在辅以其他传感器的同时，主要利用图像匹配或者点云配准方法计算无人机在三维空间中的相对位姿。但是用于自然保护区的无人机采集的图像数据往往具有类似的特征和颜色分布，这使得图像间的匹配难以有效进行。同时激光雷达采集的三维点云分辨率太低，会严重影响无人机定位的精度。

2、imu英文为 inertial measurement unit，中文为惯性传感器，是测量物体三轴姿态角（或角速率）及加速度的装置。陀螺仪和加速度计是惯性导航系统的核心装置。借助内置的加速度传感器和陀螺仪，imu可测量来自三个方向的线性加速度和旋转角速率，通过解算可获得载体的姿态、速度和位移等信息。

技术实现思路

1、本发明的目的就在于提供一种解决上述问题，因图像数据有类似的特征和颜色分布，导致图像间的匹配难以有效进行，从而无法准确得到无人机在三维空间中的精确位姿的，一种多传感器融合的自然保护区无人机三维空间定位方法。

2、为了实现上述目的，本发明采用的技术方案是这样的：一种多传感器融合的自然保护区无人机三维空间定位方法，包括以下步骤：

3、s1，训练一高分辨率的深度图获取模型，包括以下步骤s11~s12：

4、s11构造训练数据集：

5、选取一rgb-d数据集，包括多个rgb-d数据，每个rgb-d数据包括一彩色图像i和第一深度图像；

6、获取rgb-d数据集对应区域的点云数据，投影到图像平面生成与rgb-d数据一一对应的第二深度图像d，其中分辨率大于d；

7、将对应的i、、d构成一个训练样本，所有训练样本构成训练数据集；

8、s12，用训练样本训练得到一高分辨率的深度图获取模型，用于输入i、d，并以为期望输出；

9、s2，在无人机上搭载相机、激光雷达、imu，分别用于采集自然保护区的彩色图像、点云数据、imu数据，所述imu数据包括三维加速度数据和三维角速度数据，且对于采样频率，相机等于激光、远远小于imu；

10、s3，初始化；无人机起飞前先静置，根据静置期间的imu数据，得到无人机的初始旋转矩阵 r0、imu的加速计偏置 b a、陀螺仪偏置 b ω；并预设此时无人机速度 v=0，初始位姿为 p0，，其中，t为转置操作；

11、s4，无人机起飞，相机、激光雷达、imu工作，将相机和激光雷达采集到一次彩色图像和一个点云数据的时刻作为一次更新时刻；

12、s5，在每个更新时刻更新一次点云数据，与上一时刻的点云数据配准得到无人机位姿，其中第 i个更新时刻 t i得到无人机位姿 p i的方法包括s51~s53；

13、s51，得到无人机在 t i时当前预估位姿，包括a1~a2；

14、a1，获取 t i时刻无人机飞控对无人机的预估位姿 p c、imu对无人机的预估位姿 p i，其中， p c为无人机飞控对无人机位姿进行预测得到， p i为imu采用预积分方式估计得到；

15、a2，对、，分别转换为对偶四元数进行加权平均，再将加权平均后得到的对偶四元数归一化并转换为位姿矩阵，获得当前预估位姿；

16、s52，提升点云数据分辨率，生成当前高密度点云数据，包括b1~b2；

17、b1， t i时刻相机采集到彩色图像 i i、激光雷达采集到点云数据 q i；

18、b2，将 q i投影到图像平面生成当前深度图 d i，将 i i、 d i一起送入高分辨率的深度图获取模型，输出一高分辨率深度图，再反投影到三维空间得到一点云数据，作为当前高密度点云数据；

19、s53，配准；

20、将作为初始位姿，用迭代最近点算法将与配准，输出一位姿数据，作为第 i个更新时刻的无人机位姿 p i，所述为第 i-1个更新时刻的当前高密度点云数据；

21、s6，重复s5直到完成飞行，得到每个更新时刻的无人机位姿。

22、作为优选，s12具体为：

23、s12-1，构造一生成网络：

24、包括第一自编码器、第二自编码器和扩散模型；

25、所述第一自编码器用于对i、d编码，得到编码彩色图像i’和编码第二深度图像d’；

26、所述第二自编码器用于对编码，得到编码第二深度图像并能对解码；

27、s12-2，训练生成网络；

28、将s11中训练样本送入生成网络，对每个训练样本，将i、d送入第一自编码器，得到i’、d’，将i’、d’送入扩散模型中，以为期望输出，训练扩散模型，最后将扩散模型的输出送入第二自编码器中解码，输出一图像作为高分辨率深度图，得到高分辨率的深度图获取模型。

29、作为优选，步骤s3中，得到初始旋转矩阵、imu的加速计偏置、陀螺仪偏置具体为：

30、s31，在静置期间采集imu数据，并计算静置期间的三维加速度平均值、三维角速度平均值；

31、s32，将的方向设为z轴方向，用施密特正交化构建初始的三维空间坐标系，得到无人机的初始旋转矩阵 r0、并根据下式计算加速计偏置 b a和陀螺仪偏置 b ω；

32、                   （1），

33、                          （2），

34、式（1）中，为自然保护区的重力加速度。

35、作为优选，步骤b1中， p i为imu采用预积分方式估计得到，imu采样频率大于相机和激光雷达，则在 t i-1到 t i间，imu按采样间隔∆ t进行了多次采样，其迭代方式根据下式计算：

36、           （3），

37、        （4），

38、    （5），

39、        （6），

40、式（3）中， t为 t i-1到 ti间imu的采样时刻，∆ t为imu的采样间隔，exp(.)为exp函数，为imu数据中的三维角速度数据， b ω为陀螺仪偏置， r( t)、 r( t+∆ t)分别为 t时刻、 t+∆t时刻旋转矩阵；

41、式（4）中，为imu数据中的三维加速度数据， v( t)、 v( t+∆ t)分别为t时刻、 t+∆t时刻的无人机速度，为自然保护区的重力加速度， b a为加速计偏置；

42、式（5）中， p( t)、 p( t+∆ t)分别为t时刻、 t+∆t时刻的无人机三维坐标。

43、本发明思路为：

44、一、先训练一高分辨率的深度图获取模型：用高分辨率的rgb-d数据与激光雷达的点云数据训练网络。其中rgb-d数据中的深度图像分辨率高于三维点云转换成的深度图像的分辨率。训练时，输入有两个，一个是rgb-d数据中的彩色图像i，一个是点云数据投影生成的分辨率较低的第二深度图像d，模型的期望输出为rgb-d数据中的第一深度图像。这是因为分辨率高于d，通过训练，输入彩色图像i和对应的低分辨率的深度图像，就能输出接近于rgb-d数据集中高分辨率的深度图像。在构造高分辨率的深度图获取模型中，会用到扩散模型，扩散模型生成的潜空间模拟深度图像的数据分布。训练时，将i和d组成低分辨率的rgb-d图像数据，利用vision transformer编码，再用神经网络学习该编码与在潜空间中的采样点之间的匹配关系。最后利用迁移学习对模型进行微调，使之适合该应用的需要。

45、二、获取更精确的位姿数据：本发明结合无人机飞控对无人机的预估位姿 p c和imu对无人机的预估位姿 p i生成一预估位姿，用于后续点云配准中。 p c可采用pid控制算法等方法，对无人机位姿变化的实时预测进行积分， p i采用预积分方式，由于imu采样频率更高，所以在第 i-1次更新时刻和第i次更新时刻之间，无人机飞控对无人机的预估位姿、imu对无人机的预估位姿一直在实时预测，精度也更精确。

46、三、提升点云数据分辨率：所述点云数据为三维点云数据。无人机飞行时无需搭载深度相机，仅采用普通相机采集彩色图像、激光雷达获取低密度的点云数据即可。由于对设备要求不高，可以提高数据采集的速度。将彩色图像、低密度的点云数据，用高分辨率的深度图获取模型提升分辨率，得到高密度的点云数据。

47、四、精确点云配准：用上述第二点得到的预估位姿作为迭代最近点算法的初始位置，对第三点得到的高密度的点云数据进行配准，配准结果为一更精准的位姿数据，从而提升了无人机在三维空间中定位的精度。

48、与现有技术相比，本发明的优点在于：

49、1、提出了一种新的多传感器融合的自然保护区无人机三维空间定位方法。该方法利用图像数据、点云数据和高分辨率的深度图获取模型提升点云数据的分辨率，并结合无人机飞控对无人机的预估位姿、imu对无人机的预估位姿得到一预估位姿，用于点云配准。本发明能提升三维点云分辨率、以及点云配准的精度，最终使得无人机在三维空间中定位的精度提升。

50、2、本发明利用深度图像提升点云空间密度，能提升算法效率，减少计算资源占用，保证无人机位姿能快速甚至实时更新。

51、3、本发明尤其适用于彩色图像颜色分布接近，局部特征不明显、区分度不高、纹理分布均匀的场景，如自然保护区，这些地区使用彩色图像进行匹配或配准效果较差，但采用本发明的点云数据进行配准可以提升无人机位姿计算精度。