一种高空无人机视觉融合IMU定位方法与流程

本发明涉及自动控制，尤其涉及一种高空无人机视觉融合imu定位方法。

背景技术：

1、目前，高空运动的无人机一般使用的定位方法都是基于雷达和gps信号的，但是在对抗场景及一些特殊环境中，无法接收gps信号和雷达使用受到干扰的情况下，如何定位无人机的位置是非常核心的重点。单纯使用相机进行图像匹配则计算需求量太大，而使用相机-imu数据融合定位则受到外界环境的干扰较大，无法得到精确的定位精度。因此如何在拒止环境下到无人机的精确位姿是现有技术中急需解决的技术问题。

技术实现思路

1、本发明的目的是提供一种高空无人机视觉融合imu定位方法，能够在拒止环境下对无人机进行定位，且定位精度高，受到环境影响小。

2、本发明采用的技术方案为：

3、一种高空无人机视觉融合imu定位方法，包括以下步骤：

4、步骤一：初始化视觉匹配系统和imu设备；所述的视觉匹配系统包括视觉采集系统和目标检测库，其中视觉采集系统包括工业单目相机和可处理图像的计算机设备；目标检测库是定位地区的图像数据集；

5、步骤二：提取图像特征：根据工业单目相机拍摄照片后在目标检测库中进行视觉匹配，以得到某一时刻的精确位置，并将此位置数据存储，待后续使用；

6、步骤三：相机和imu内、外参数标定：

7、首先使用imu_utils包校准imu的噪声密度和随机游走噪声；

8、然后使用kalibr工具和“棋盘格”对相机内部参数进行标定，改变相机发布频率；

9、然后再对相机、imu的外参进行标定，校准相机和imu之间的相对位姿及时间差；

10、步骤四：使用相机和imu进行数据融合：将相机和imu的原始测量数据，对视觉特征点进行检测和匹配，并对imu测量数据进行预积分，得到无人机在高空运动的轨迹；

11、步骤五：通过时间戳匹配和标志物的精确位置，对阶段性轨迹进行矫正，最终得到无人机的飞行路线以及标志物类型、位置、时间。

12、所述的步骤一中imu和相机进行初始化具体还包括陀螺仪偏置估计、重力、速度以及尺度估计以及重力方向修正。

13、所述的陀螺仪偏置估计具体包括如下步骤：

14、相邻图像帧之间的旋转变换矩阵有两种方式：一是根据单目视觉三维重建；二是对相邻帧图像之间的角速度数据进行预积分处理；无人机状态其中表示无人机的状态变量，分别表示未imu坐标系在世界坐标系下的方向向量、位移以及速度矢量，表示加速度计和陀螺仪的偏置。

15、本系统中单目相机与imu固联在一起，可将其视为刚性连接，故采用上述两种方式计算得到的相邻图像帧之间的旋转向量是相等的，则可将陀螺仪偏置问题转化为：

16、

17、式中，w代表世界坐标系，c代表相机坐标系，γ是预积分过程角度增量，由相机-imu外参标定确定，表示第k帧相机与世界坐标系之间的变换关系，表示的是k与k+1图像之间的imu预积分值，预积分值的偏置更新有

18、

19、在不考虑噪声的情况，相邻的图像帧之间的约束方程为：

20、

21、陀螺仪更新偏置一般情况下都是微小的量，则由式(2)和(3)可得：

22、

23、式中，表示雅可比矩阵，δ是积分项，[·]xyz表示取四元数的虚部向量；

24、系统的初始化过程比较短，因此可以认为n帧图像之间陀螺仪的偏置式保持恒定不变，即，

25、

26、由此可得解陀螺仪偏置为：

27、

28、

29、式中偏置更新量可以通过svd求得。

30、所述重力、速度以及尺度估计具体步骤如下：

31、当完成陀螺仪偏置估计后，通过偏置更新方程更新预积分因子会造成的误差较大和系统初始化发散不收敛的问题，因此，将估计的陀螺仪偏置带入预积分公式中重新计算：

32、设未知的单目尺度因子为s，imu在世界坐标系下的位置为：

33、

34、将式(7)代入到imu位移和速度的运动方程中，得到：

35、

36、

37、由式(8)可以计算出邻帧之间视觉观测的imu预积分量，将其与相邻帧之间的imu数据积分得到的预积分因子构成残差：

38、

39、为方便计算，将相对世界坐标系下的imu速度矢量变换到自身所在的imu坐标下，于是有定义待估计的变量为在不考虑测量噪声和偏置时，式(9)结果为0，于是有：

40、

41、在有连续4个图像帧的条件下，同样可以采用svd方式对式(10)进行求解。

42、所述对重力方向修正具体包括如下步骤：

43、通常认为重力加速度的模长保持不变，因此重力加速度减少一个自由度；由式(9)求解出的在世界坐标系下的重力方向于是将重力向量中的两个自由度按其切平面进行分解可得：

44、

45、式中，矩阵η＝[l1 l2]t，和分别代表切平面上两个相互正交的分量，分别等于：

46、

47、将公式(11)代入公式(9)中，观测方程变为：

48、

49、式中,

50、

51、i3是单位矩阵

52、对式(13)采用最小二乘法求得出η，由下式对重力方向进行修正：

53、

54、将式(15)的结果重新带回式(9)中重复上述操作，多次迭代求解后得到精确的重力方向、尺度因子s和速度。初始化完成后将视觉约束、imu约束和闭环约束放在一个大的目标函数中进行非线性优化，求解滑窗内所有帧的pvq、bias等。利用dbow进行闭环检测，当检测成功后进行重定位，最后对整个相机轨迹进行闭环优化。这样就能得到无人机的精确位置信息。

55、本发明通过结合基于深度学习的视觉匹配和相机-imu数据融合方法的轨迹跟踪，得到无人机的位姿，整个过程由于采用的视觉拍摄和imu的定位数据，从而受到环境的影响小，且定位精度高。本发明设计的拒止环境下高空无人机定位方法，具有一定的实际意义。

技术特征：

1.一种高空无人机视觉融合imu定位方法，其特征在于：包括以下步骤：

2.根据权利要求1所述的高空无人机视觉融合imu定位方法，其特征在于：所述的步骤一中imu和相机进行初始化具体还包括陀螺仪偏置估计、重力、速度以及尺度估计以及重力方向修正。

3.根据权利要求2所述的高空无人机视觉融合imu定位方法，其特征在于：所述的陀螺仪偏置估计具体包括如下步骤：

4.根据权利要求2所述的高空无人机视觉融合imu定位方法，其特征在于：本系统中单目相机与imu固联在一起，可将其视为刚性连接，故采用上述两种方式计算得到的相邻图像帧之间的旋转向量是相等的，则可将陀螺仪偏置问题转化为：

5.根据权利要求2所述的高空无人机视觉融合imu定位方法，其特征在于：所述重力、速度以及尺度估计具体步骤如下：

6.根据权利要求2所述的高空无人机视觉融合imu定位方法，其特征在于：所述对重力方向修正具体包括如下步骤：

技术总结
本发明公开了一种高空无人机视觉融合IMU定位方法，包括以下步骤：初始化视觉匹配系统和IMU设备；提取图像特征；相机和IMU内、外参数标定；使用相机和IMU进行数据融合；通过时间戳匹配和标志物的精确位置，对阶段性轨迹进行矫正，最终得到无人机的飞行路线以及标志物类型、位置、时间。本发明通过结合基于深度学习的视觉匹配和相机‑IMU数据融合方法的轨迹跟踪，得到无人机的位姿，整个过程由于采用的视觉拍摄和IMU的定位数据，从而受到环境的影响小，且定位精度高。本发明设计的拒止环境下高空无人机定位方法，具有一定的实际意义。

技术研发人员：姜波,王帮民,陈启明,马书磊,倪凯歌,唐晓燕,宋玙薇,雷文静
受保护的技术使用者：中国电子科技集团公司第二十七研究所
技术研发日：
技术公布日：2024/1/16