一种视觉惯性里程计中的加权欧拉预积分方法与流程

本发明涉及一种视觉惯性里程计中的加权欧拉预积分方法，属于同时定位与地图构建(slam)技术领域。

背景技术

视觉惯性里程计(vio)是融合视觉和惯性传感器的融合slam系统，视觉传感器和惯性传感器成本都很低，包含大量信息，在许多方面两者具有互补性。vio在许多领域都有潜在的及广泛的应用，比如机器人领域、增强现实以及无人驾驶等。

单目vio中的一种典型高效的融合方式是紧耦合这种方法将视觉与惯性的待优化量融合为一个参量来整体估计。惯性测量单元(imu)通常获取测量数据的速率远高于视觉传感器。当前vio的解决方案能通过非线性优化来获得较高精度的状态估计。imu的预积分技术在整个vio处理流程中很重要的数据预处理部分。提高预积分的精度对于后续的联合初始化、运动估计及优化具有重要意义。

lupton在2012年首次提出了预积分的概念，他们将两个关键帧中的惯性测量值结合为一个相对运动的约束。forster提出了在流行结构上能适用的旋转群的预积分理论。tongqing通过增加imu偏置更正部分来扩展了imu预积分。已有的预积分技术通常使用欧拉积分及中值积分的数值积分方式，虽然便于计算，却损失了部分精度，对最终定位结果的精度有影响。

技术实现要素：

发明目的：针对上述现有技术的问题，本发明目的在于提出一种视觉惯性里程计中的加权欧拉预积分方法，充分利用imu测量数据，在不增加运算难度的前提下，相较欧拉预积分方法，提升定位精度，且估计的偏置更接近于实际情况。

技术方案：为实现上述发明目的，本发明采用如下技术方案：

一种视觉惯性里程计中的加权欧拉预积分方法，其特征在于：该方法首先在积分运动模型上使用加权欧拉预积分，然后将两关键帧间的imu测量值通过迭代求和处理，得到两关键帧之间的运动约束，再通过公式处理进一步得到相对运动增量模型以及预积分测量模型。

进一步地，将积分运动模型迭代公式使用加权欧拉预积分处理后表示为：

其中δt是两次imu测量值之间的时间间隔；r,v,p分别代表旋转、速度和平移，w,a分别代表角速度和加速度；rwb表示从body坐标系到世界坐标系的旋转；下标w和b分别表示世界坐标系和body坐标系，()^表示将向量转化为反对称矩阵；上标t表示转置；

在数值积分中，两个iimu测量时刻间的角速度和加速度计算公式为：

wb(t,t+δt)＝c1(t)wb(t)+c2(t)wb(t+δt)

ab(t,t+δt)＝c3(t)ab(t)+c4(t)ab(t+δt)

其中，c1-c4为权值系数。

进一步地，通过对两个连续视觉关键帧间的imu测量值迭代求和，得到两个相邻关键帧i,j之间的约束为：

其中，r,v,p分别代表旋转、速度和平移；g为重力加速度；为加权后的imu测量的角速度和加速度，b^g、b^a为对应的偏置，η^g,η^a为对应的噪声。

进一步地，所述相对运动增量模型表示为：

其中，r,v,p分别代表旋转、速度和平移；g为重力加速度；上标t表示转置；为加权后的imu测量的角速度和加速度，b^g、b^a为对应的偏置，η^g,η^a为对应的噪声。δrij、δvij、δpij分别表示关键帧i,j之间相对旋转、速度和平移。

进一步地，所述预积分测量模型表示为：

其中，分别表示含有噪声项的关键帧i,j之间相对旋转、速度和平移，δvij，δpij分别表示相应的噪声项，表示j-1时刻的右雅可比。

进一步地，还包括根据如下公式获得协方差矩阵σij：

其中，和代表高斯噪声的协方差矩阵，

进一步地，还包括根据如下公式获得雅可比矩阵jj：

其中，分别表示偏置发生改变之前关键帧i,j之间的相对旋转、速度和平移的预积分值。

有益效果：本发明提出一种视觉惯性里程计中的加权欧拉预积分方法，充分利用imu测量数据，在不增加运算难度的前提下，相较于传统欧拉预积分方法，定位精度约提升了40％，且估计的偏置更接近于实际情况。随着无人驾驶技术、增强现实技术、机器人技术的飞速发展和普及以及相关行业的兴起，大众对于位置服务的定位精度提出来更高的要求，因此视觉惯性里程计中的加权欧拉预积分方法具有重大的意义和市场价值。

附图说明

图1是含加权预积分的预积分流程图。

图2是eurocmav数据集使用的无人机。

图3是轨迹图(实线)与真实轨迹(虚线)。

图4是原始vio系统、用加权欧拉预积分方法的vio系统和groundtruth之间的加速度计偏置在不同方向上的对比图。

图5是原始vio系统、用加权欧拉预积分方法的vio系统和groundtruth之间的陀螺仪偏置在不同方向上的对比图。

具体实施方式

下面结合附图和具体实施例，对本发明技术方案作进一步说明。

本发明实施例公开的一种视觉惯性里程计中的加权欧拉预积分方法，该方法首先在积分运动模型上使用加权欧拉预积分，然后将两关键帧间的imu测量值通过迭代求和处理，得到两关键帧之间的运动约束，再通过公式处理进一步得到相对运动增量模型以及预积分测量模型。最后给出了噪声传播和偏置更新的方法，也为vio的后续处理提供了协方差矩阵和雅可比矩阵。

下面对本实施例的视觉惯性里程计中的加权欧拉预积分方法的具体理论基础和实施过程做详细介绍：

(1)imu测量值加权理论

imu预积分旨在获得imu测量值和其协方差矩阵，并为相机和惯性器件联合初始化提供初值，此外还为后端优化提供imu约束关系。

imu测量值包括运动主体的陀螺仪和加速度计的测量值。这些测量值通常被传感器噪声和偏置所影响。本发明假设加速度和陀螺仪的偏置都hi随机游走模型。imu测量值如下所示：

本发明定义为imu世界坐标系下的角速度和加速度，wb(t)和aw(t)代表不受噪声和偏置影响的角速度和加速度的真值.η^g(t)和η^a(t)代表陀螺仪和加速度计的高斯噪声。偏置的导数和噪声η^g,η^a都符合高斯分布。

本发明假设两个iimu测量时刻间的角速度和加速度都是常数，并且由两个连续的测量值加权表示。

wb(t,t+δt)＝c1(t)wb(t)+c2(t)wb(t+δt)(3)

ab(t,t+δt)＝c3(t)ab(t)+c4(t)ab(t+δt)(4)

系数的计算：

c1(t)＝wb(t)/(wb(t)+wb(t+δt))

c2(t)＝wb(t+δt)/(wb(t)+wb(t+δt))

c3(t)＝ab(t)/(ab(t)+ab(t+δt))

c4(t)＝ab(t+δt)/(ab(t)+ab(t+δt))

其中：wb(t,t+δt),ab(t,t+δt)代表处于每个imu采样周期中的角速度和加速度值。为了使表达更简洁，本发明定义加权后的imu测量值为在后续部分中，这种加权的思想将用于加权欧拉数值积分，以及旋转的更新。

(2)相对运动增量模型

从由描述imu测量值的运动模型可知，在两个imu测量时刻之间的运动状态可以由下积分运动模型公式迭代得到：

其中δt是两次imu测量值之间的时间间隔。r,v,p分别代表旋转、速度和平移。从公式可以看出，当t时刻的运动状态改变时，公式(5)需要重复计算。为了避免这种重复计算，本发明采用了预积分算法，预处理如下所示：

为了使迭代公式能用于离散测量值，本发明使用提出的加权欧拉积分方法。在这种数值积分中，使用了(3)和(4)中的加权imu测量值。

加权思想也用于了旋转矩阵的更新中。

与传统的欧拉积分相比，加权欧拉积分法能充分利用角速度和加速度测量值，并且能够更加真实地反映速度和角度的变化趋势。在数值积分处理过程中，本发明假设在两次imu测量期间的角速度、加速度和方向都为常值，由于imu的测量频率很高，所以这种假设是合理的。

通过对两个连续视觉关键帧间的imu测量值进行求和，可以得到两个相邻关键帧i,j之间的迭代模型。

其中，本发明省略了代表不同坐标系的下标，并使用下标代表ti和tj的函数，这些处理公式表达更简洁。

公式(8)已经给出了ti和tj时刻的运动约束。本发明选择预积分与加速度和角速度相关的部分。因此相对运动增量独立于ti时刻的位姿和速度。相对运动增量模型可以定义如下：

实际上，公式(9)的等式右端可以预先通过两个关键帧间的imu测量值计算。然而，等式右端值还受高斯噪声和随机游走偏置的影响。本发明假设在两个连续关键帧间的偏置为常值，再讨论迭代过程中的高斯噪声的影响。

(3)预积分测量模型及噪声传播公式

相对运动增量模型能够从噪声传播方面进一步分析。通过李群计算和一阶展开的处理，旋转增量能被分为两个部分，其中一部分包含imu测量值,另一部分包含高斯噪声：

速度和平移的相对增量也能分离出噪声项：

注意初始状态δrii＝i3×3,δαii＝03×3，因此基本的预积分测量模型如下所示：

公式(10)表示了基本的预积分测量值由高斯噪声项和关键帧i,j的状态组成。将各个噪声项组合并定义为一个噪声向量同时，在公式(10)到公式(12)中，每个噪声项都能具体表示为：

矩阵形式为：

(4)偏置更新方法

高斯噪声迭代公式(15)是基于两个连续关键帧之间的传感器噪声不变的假设的。实际传感器偏置都是随着时间缓慢变化的。此部分将建立偏置更新模型。

从公式(9)可知，预积分测量值近似服从高斯分布。因此预积分测量值的协方差可以由以下公式迭代计算：

其中，和代表高斯噪声的协方差矩阵，通常可以在imu手册中查阅。另外，初始状态值设为σii＝09×9。同时，γij相对于γii的一阶雅可比矩阵也能通过初始雅可比矩阵迭代计算。

其中，

使用迭代公式(16)(17)能获得协方差矩阵σij和雅可比矩阵jj。因此相对于已知的偏置δrij,δvij和δpij的一阶近似为：

其中，是中对应位置上的子矩阵，同样的方法能够获得子矩阵和当偏置缓慢变化时，用公式(18)来近似更新预积分结果而不再重复计算。

实例：实验数据来源于公开可用的eurocmav数据集。七个测试数据集分别为v1-01-easy，v1-02-mediunm，v2-01-easy，v2-02-medium，mh-01-easy，mh-03-medium和mh-05-difficult。这些数据集展示了配备imu和立体相机的无人机的动态运动，使用的无人机如附图2所示。vio系统是基于visua-inertialorb-slam2。

首先比较了这五个数据集的估计轨迹与地面真实轨迹。图3显示估计的轨迹几乎跟踪地面的真实情况一致其中存在的虚线，是由于使用的vio方案是基于关键帧方法。如果选取的两个关键帧之间的持续时间持续很长，则这两个关键帧之间的轨迹将成为虚线。但总体估计轨迹符合实际情况。

其次将估计的陀螺仪和加速度计的偏置与实际偏置进行比较。结果表明，使用加权欧拉预积分的vio估计的加速度计偏置比原始vio估计的加速度计偏置更为平滑，并且更接近地面真实偏置，而对陀螺仪偏置的影响不大。图4和图5显示了mh-01-easy数据集的加速度计和陀螺仪偏置。

最后，表1给出了vio系统的定位精度的rmse对比，对比结果显示，使用加权欧拉预积分方法的vio定位结果精度比原始vio系统的定位结果提高约40％。

表1定位精度的rmse对比