一种融合IMU陀螺仪数据降噪的VIO定位增强方法

本发明提供一种融合imu陀螺仪数据降噪的vio定位增强方法，主要解决视觉惯性里程计(visual inertial odometry,vio)中惯性测量单元(inertia measurement unit,imu)的陀螺仪噪声与零偏难以进行准确建模，从而容易造成后续积分过程的发散导致最终姿态估计不准确的问题，该技术属于机器人/无人车定位导航领域。

背景技术：

1、vio是"visual inertial odometry"的缩写，通常称为视觉惯性里程计。它是一种结合了视觉信息和惯性测量单元(inertia measurement unit,imu)数据的传感器融合技术，用于估计机器人或无人驾驶车辆在未知环境中的位姿变化。其主要依靠摄像头捕捉环境图像，通过图像序列中的特征点匹配来估计相机的运动。而imu中的加速度计与陀螺仪可以分别提供高频的加速度和角速度信息，通过积分可以获得载体的速度、位置与姿态，从而帮助估计机器人或车辆的动态状态。将这两种传感器的数据结合起来，可以互补两个传感器的优势，提高定位的准确性和鲁棒性，尤其是在视觉信息受限或imu数据存在噪声的情况下。

2、受成本所限，通常vio使用的imu为微机电系统(micro-electro-mechanicalsystems，mems)imu，但mems惯性测量单元的数据由于自身特性的影响，是包含噪声与零偏的，因此通常将imu的测量值建模为：测量值＝真实值+零偏+噪声。为获取位姿信息，需要将imu测量的加速度与角速度信息分别进行两次以及一次积分，在长时间积分过程中微小的零偏和噪声会带来较为明显的位姿发散。为解决上述问题目前的vio方案有两种做法，一种是在使用之前通常会对imu进行标定，如利用kalibr库离线计算imu零偏与噪声以及与相机之间的外参。另一种是在线标定，在使用过程中动态估计零偏与噪声。但目前这两种方法也存在如下问题：

3、一、离线标定无法适应长时间运行以及更加复杂多样的环境：

4、mems惯性测量单元的数据由于受到坐标轴对准误差、尺度参数、温度变化、时变补偿等的影响，其在不同环境与使用场景下的零偏与噪声是不同的，这意味着在环境发生变化时使用前都需要对imu进行离线标定。除此之外，在运行过程中若环境发生较大变化，零偏与噪声也会动态地进行改变，因此离线标定在面对长时间运行以及更加复杂多变的使用环境时仍显得捉襟见肘。

5、二、在线标定计算过程复杂，更新过程会带来重复积分

6、在线标定通常出现在基于优化框架的vio方案中，它在后端优化的过程中把零偏也视为优化变量，计算预积分残差对零偏的雅可比，构建最小二乘问题优化。因此，零偏在整个vio的使用过程中是动态更新的，一定程度解决了离线标定在长时间运行过程中零偏变化的问题，但此方案在计算雅可比以及求解最小二乘问题是的过程更加复杂，且零偏更新后整个积分过程需要重复计算，虽然可以对积分量进行一阶泰勒展开，将零偏的更新乘上雅可比补偿在原来的积分量中，但也存在着有效线性区间的问题。

技术实现思路

1、针对目前vio系统中惯性测量单元零偏与噪声估计的局限性：1.离线标定无法适应长时间运行以及更加复杂多样的环境。2.在线标定计算过程复杂且会带来重复积分。考虑到如今深度学习在视觉惯性定位的广泛应用，可以通过前期数据的训练而获得更加准确的imu数据标定。除此之外在vio系统处于曲线运动时，通常伴随着偏航角的变化，而稳定可靠的姿态估计又是在曲线行驶以及后续行驶时提高定位精度的关键。因此本发明使用扩张卷积神经网络对imu中的陀螺仪数据进行降噪处理，剔除测量角速度数据中包含的零偏与噪声，而后再输入vio系统中进行位姿估计。通过神经网络隐式的学习其内部噪声，既解决了离线标定无法适应复杂场景，又避免了在线标定零偏估计中复杂的计算过程，使得imu的角速度数据更加精准，积分后提供更加准确的姿态估计，进而提高整体vio的位姿估计精度。

2、一种融合imu陀螺仪数据降噪的vio定位增强方法，分别获取相机采集的图像帧以及imu中加速度与陀螺仪测量的加速度与角速度，在前端分别进行特征点提取追踪和imu的数据积分，而后在后端构建视觉重投影残差与imu预积分残差，一同构建最小二乘优化问题求解，使得速度、位置、姿态的状态估计最优。

3、针对imu的角速度测量数据进行降噪处理：imu陀螺仪测量的角速度数据首先输入给训练后的扩张卷积神经网络模型，在降噪处理后输出给前端的imu数据积分环节。

4、具体的技术细节如下详述。

5、(1)、用于imu陀螺仪数据降噪的扩张卷积神经网络结构

6、在此定义的神经网络结构，由5个扩张卷积层叠加而成，核维度、膨胀间隙、通道宽度；其接收时间窗口为n的imu原始数据，经过5层卷积后输出当前陀螺仪角速度的补偿量。推断的过程描述为：

7、

8、表示n时刻的imu原始测量数据，其是一个6维向量，包含加速度计测量的三轴加速度与陀螺仪测量的三轴角速度，f(.)即由神经网络建立的imu原始数据到陀螺仪校正量的映射关系，该网络在时间多尺度上提取信息并计算平滑的校正量。输入网络的数据包含加速度计以及陀螺仪的imu的测量值。

9、(2)用于imu陀螺仪数据降噪的扩张卷积神经网络训练过程

10、在网络模型的训练过程中，根据输入的imu数据回归当前角速度的修正量，对原始的imu数据进行补偿，对角速度数据进行开环积分，获取到当前的姿态数据，并以此与姿态真值构建损失函数，引导神经网络迭代优化。

11、首先基于姿态增量建立动力学模型。通过对姿态增量(即imu的陀螺仪的测量数据)进行积分，得到刚性平台的三维姿态：

12、rn＝rn-1exp(ωndt)

13、这其中旋转矩阵rn∈so(3)代表着n时刻由imu坐标系到世界系下的映射关系，角速度是在dt期间的平均值，并且由exp(.)指数映射建立起到李群的关系。上述式子通过连续的开环积分过程会逐渐传递并累计误差。

14、下面进行陀螺仪的降噪过程建模：

15、

16、是经过降噪过后的角速度，是imu直接输出的角速度测量值；是陀螺仪关于尺度以及坐标轴对准误差的内参；陀螺仪的校正量包含了其零偏：其中是时变参数，是静态零偏。神经网络利用附近大小为n的本地窗口中的陀螺仪原始数据，计算出陀螺仪的校正量陀螺仪内参矩阵在最开始时设置为单位矩阵，而后在训练过程中进行优化更新。

17、先用补偿后角速度与角速度真值ωn分别利用上式在时间窗口为j的区间进行积分：

18、

19、而后构建如下损失函数：

20、

21、log(.)是李群的对数映射，ρ(.)是huber核函数，设定huber损失参数为0.005，并且将最终的损失函数定义为：

22、

23、本发明通过设计扩张卷积深度神经网络对imu陀螺仪的原始数据进行校正。通过数据驱动构建陀螺仪原始数据与降噪后数据的映射关系，既解决了离线标定无法适应复杂场景，又避免了在线标定的零偏估计中复杂的计算过程，使得imu的角速度数据更加精准，减少了姿态积分过程中的误差累积，提供了更加准确的姿态估计，改善了vio系统在曲线运动状态下的姿态估计不准的问题，从而实现vio系统的定位增强，提高整体vio的位姿估计精度。