IMU Preintegration#
0. 为什么需要 IMU 预积分#
要解释 IMU 预积分的话,首先需要先了解正常情况下 IMU 积分与所谓的 IMU 预积分的区别。这里直接给出 IMU 积分过程(其中涉及到的 IMU 测量模型与运动学模型先不展开)。IMU 积分是指根据 IMU 传感器获取得到的加速度 和角速度 ,与给定 IMU bias ,从第 时刻的 基础上不断积分得到第 时刻的 。如下图所示,

IMU 积分公式如下:
观察积分公式我们发现 中的积分项依赖于初值 。 如果时刻 的状态发生改变,时刻 的状态必须重新计算。在基于优化的 Visual Inertial Odometry 算法中,状态每次迭代都会改变,如果每次迭代都重新积分,那么计算量会非常大。而预积分的目的就是想将这部分预先计算,且不受每次迭代更新重新计算,由此,我们需要想方法去掉基于 World 坐标系下的依赖项 。
IMU 预积分技术最早由 T Lupton 于 12 年提出,以欧拉角为基础。C Forster 于 15 年将其进一步拓展到李代数上,形成了一套优雅的理论体系。这套 IMU 预积分理论在开源因子图优化库 GTSAM 中进行实现,且被广泛应用于图优化框架的 VIO 中。其中包括 SVO2、ORB-SLAM3、VINS-Mono/Fusion、ICE-BA 等。由于这套理论的概念是在李群流行上的推导的,因此其中涉及到许多李群李代方面的知识,这部分知识可参考(因此下文中使用到的一些公式、定义将直接给出,不再加以说明)。这套理论的后续发展有,Guoquan Huang 等,在欧拉积分/二阶龙格库塔积分(Runge-Kutta 即,中值积分)基础上引入群空间闭式 (closed-form) 积分。
1. IMU 器件测量模型与运动学模型#
IMU 通常包括一个三轴加速度计和一个三轴陀螺仪,能够测量传感器相对于惯性系的旋转速率和加速度。测量模型定义如下:
其中 分别为陀螺仪与加速度计的测量值, 为随时间缓慢变化的 bias, 是白噪声。该模型利用了 Static World Assumption,由于 SLAM 的运行场景通常比较小(由此重力变化不大),且使用的 IMU 器件一般为 MEMS 器件(精度较低,陀螺仪静置时无法敏感到地球自转)。因此在上式 测量模型中忽略了地球自转、重力矢量变化等影响因素,且假设运行区域水平面是一个平面。在此假设下的 VINS 中,世界坐标系 被认为是一个惯性系,由此,下面的运动模型都是基于该假设进行推导。因此有运动模型的微分方程形式如下:
根据上式, 时刻的状态可以通过积分得:
当假设在时间 内 和 是恒定的,使用欧拉积分,可以得到上式的离散形式如下:
为了公式书写方便与符号简明,下面省略一些上下标记号,约定如下 。将测量模型(式 )带入离散运动模式(式 )易得:
上述公式中,我们注意到噪声采用的 ,其中下标 表示为 discrete ,即使用了离散时间噪声,它们与连续噪声项 是不同的,离散噪声与连续噪声的协方差有如下关系(Nikolas Trawny and Stergios I. Roumeliotis. Indirect Kalman Filter for 3D Attitude Estimation 2.1 有详细推导过程):
2. IMU 预积分#
下面我们推导如何将两个关键帧 和 之间的所有 IMU 测量数据进行积分,由 时刻的 直接更新得到 时刻的 。过程如下图:

假设 IMU 的采样频率不变,即时间间隔 恒定,每个离散时刻由 表示,由此将式 简化符号如下:
在关键帧 和 之间迭代所有 时间间隔的 IMU 积分有:
其中
为了避免每次更新初始的 , 和 都需要从头积分求解 , 和 。由此引出相对运动的预积分项(理想值)以避免依赖于 , 和 :
其中
需要注意的是虽然所称为 ‘‘delta’’ 量,但与 相比, 和 都不对应于速度和位置的真实物理变化,而是以一种使式 的右侧独立于时刻 的状态量和重力效应的方式定义的。使得我们能从两个关键帧之间的惯性测量直接计算式 的右侧。上述三个预积分公式中,前两个式子的推导是显而易见的。而对于 的推导如下(为了节省篇幅,令 ),
利用等差数列求和等式左右两边同时左乘
我们注意到,即使采用式 进行预积分计算,但是它依旧依赖于 bias 的估计。为了决绝这个问题,预积分算法将之分解与两个步骤:1. 首先假设两个关键帧之间的 bias 是一致的,即 先进行预积分 IMU 测量值([2.1 章节](#2.1 预积分 IMU 测量值));2. 当 bias 更新时,采用其他方式避免重复积分地更新预积分值([2.3 章节](#2.3 伴随 bias 迭代更新的预积分测量更新))。
2.1 预积分 IMU 测量值#
式 将关键帧 和 的状态(等式左侧)与测量值(等式右侧)联系起来。在这个意义上,可以被当初一个测量模型。不幸的是,它对测量噪声的依赖相当复杂,这使得 MAP 估计的直接应用也变得复杂。为了使得求解容易,需要将噪声项从惯性测量量中进行分离,使得预积分项变成 “测量值理想值白噪声” 的形式。
2.1.1 项#
利用且令利用将噪声项移至最后
其中 即为预积分旋转测量值,它由陀螺仪测量值和对陀螺仪 bias 的估计计算得到。而 为其测量噪声。
2.1.2 项#
将式 ,即 代入 的公式中,有
利用忽略高阶小项利用
其中 即为预积分速度测量值,它由 IMU 测量值和对 bias 的估计计算得到。而 为其测量噪声。
2.1.3 项#
将式 和式 ,即 和 代入 的公式中,有
利用且忽略高阶小项利用
其中 即为预积分位置测量值,它由 IMU 测量值和对 bias 的估计计算得到。而 为其测量噪声。
2.1.4 IMU 预积分测量模型#
通过 2.1.1~2.1.3 我们有 、 和 将之代入式 既能获取 IMU 预积分测量模型,即
2.2 预积分项的噪声传递#
根据式 ,将 IMU 预积分测量模型的噪声定义为 。通常我们将噪声向量近似为零均的正态分布,但是准确地建模噪声的协方差是至关重要的。由于在优化过程用的是马氏距离,需要用到协方差的逆对优化各项进行加权。由此需要对 IMU 预积分测量模型的噪声进行分析(目的是给出协方差的计算表达式)。假设 IMU 预积分测量模型的噪声满足如下:
为了证实该假设,首先我们来看 IMU 预积分测量旋转噪声 ,由式 可得
对两边去对数得令由于是小量于是有则有利用
由此易得 (一阶近似)满足零均高斯分布,因为它可以通过零均高斯噪声 的线性组合构成。
接下来我们来看 IMU 预积分测量速度噪声 和测量位置噪声 ,由式 和式 易得:
通过上式易看出 和 由加速度噪声 和预积分测量旋转噪声 线性组合而成,由于其两者都是零均的高斯噪声( 在式 已证明),所以 和 满足零均高斯分布。
通过式 和式 可知 IMU 预积分测量噪声 可由 IMU 测量噪声 线性组合构成。由于 IMU 测量噪声 的协方差已知(可以通过标定或查阅数据手册),因此只需通过一系列线性组合就可以 IMU 预积分测量噪声 的协方差 了。为了满足实时性避免重复计算,下面推导预积分测量噪声协方差的递推形式。
2.2.1 项的递推形式#
根据式 有
2.2.2 项的递推形式#
根据式 有
2.2.3 项的递推形式#
根据式 有
2.2.4 总结#
整合式 ,可以得到 的递推形式如下:
既有 ,由此根据协方差定义可得:
2.3 伴随 bias 迭代更新的预积分测量更新#
在此之前的预积分都是假设了在两个关键帧 和 之间 IMU 的 bias 是恒定的基础上推导的。然而大多数情况下,IMU 的 bias 在优化过程中将伴随一个微小的增量 进行变化。如果当 bias 变化时重新进行预积分 IMU 测量值,这将耗费巨大的计算量。因此,为了决绝这个问题,提出了利用线性化来进行 bias 变化时预积分测量项的一阶近似更新方式():
其中 为旧 bias (原始假定) , 为新 bias (迭代更新所得) ,而 是微小的增量。为了简化符号,假设已经在给定的偏差估计下的预积分变量为 。为了执行 的更新,需要计算其中的雅克比项。
2.3.1 项关于 bias 的雅克比#
根据式 有:
利用且令利用将噪声项移至最后利用当也是小量时有由于为小量,因此也是小量令
于是根据 的定义有
其中
2.3.2 项关于 bias 的雅克比#
根据式 有:
结合式利用忽略高阶小项利用
于是根据 的定义有
2.3.3 项关于 bias 的雅克比#
根据式 有:
忽略高阶小项利用
于是根据 的定义有
通过 2.3.1~2.3.3 我们可知,当接收到新的测量数据时这些雅克比可以通过递增方式及时计算。
3. IMU 预积分残差#
通过预积分测量模型 (式 ) 和测量噪声在一阶 (式 ) 上协方差为 的零均高斯噪声,因此可以得到关于 IMU 预积分因子的残差 ,有:
其中包含前面推导的伴随 bias 迭代更新的预积分测量如式 ,这种近似的修正方式避免了积分的重新计算,是预积分技术降低计算量的关键。
首先,需要先理解 IMU 预积分因子中的残差指的是预计预积分计算值 (由非 IMU 的其他方式估计的预积分值) 与 IMU 预积分测量值的差。在进行优化时,我们通常采用 “lift-solve-retract” 的方式,而每一次迭代过程将利用
重新进行参数化式 ,通过增量来更新状态进而 retract 估计量,而 solve 步骤需要围绕当前估计的结果成本进行线性化,而线性化展开的目的是便于解析计算残差的雅克比行列表达式。这个过程通常被称之为 lifting ,它使得残差成为定义在易于计算雅克比行列式的向量空间上的函数。因此我们接下来将推导残差关于向量 的雅克比行列式。
3.1 残差 的雅克比#
由于 中不含 ,因此 关于这些状态增量的雅克比都为 0 。在 3.1.1~3.1.3 的推导中采用 来进行速记。
3.1.1 残差 关于 的雅克比#
利用利用
3.1.2 残差 关于 的雅克比#
利用
3.1.3 残差 关于 的雅克比#
利用令利用利用
3.1.4 总结#
根据上述的推导,残差 关于 的雅克比为:
3.2 残差 的雅克比#
由于 中不含 ,因此 关于这些状态增量的雅克比都为 0 。
3.2.1 残差 关于 的雅克比#
利用利用
3.2.3 残差 关于 和 的雅克比#
3.2.3 残差 关于 和 的雅克比#
由于 关于 和 是线性的,因此 关于 和 的雅克比可以直接依据线性系数得到,依赖于式 易得:
3.2.4 总结#
根据上述的推导,残差 关于 的雅克比为:
3.3 残差 的雅克比#
由于 中不含 ,因此 关于这些状态增量的雅克比都为 0 。
3.3.1 残差 关于 的雅克比#
利用利用
3.3.2 残差 关于 的雅克比#
3.2.3 残差 关于 和 的雅克比#
3.3.4 残差 关于 和 的雅克比#
由于 关于 和 是线性的,因此 关于 和 的雅克比可以直接依据线性系数得到,依赖于式 易得:
3.3.5 总结#
根据上述的推导,残差 关于 的雅克比为:
4. Bias 模型#
在介绍 IMU 模型时,式 ,我们说 biases 式随时间缓慢变化的量。因此,采用 “布朗运动 (Brownian motion)” 进行建模(补充:物理学中的布朗运动与数学中的 “维纳过程 (Wiener process)” 密切相关)。biases 白噪运动学的微分方程形式如下:
根据上式,在两个连续关键帧 i 和 j 之间的时间间隔 上积分得:
其中,上式采用例如 进行速记,且定义带有零均值和协方差分别为 和 的离散噪声 和 。根据式 可以简单被包含到因子图优化中,作为 IMU 预积分因子的附加项。
5. IMU 噪声的形式与单位#
当我们在使用 IMU 的时候,离不开对 IMU Allan 方差的标定。这时候,我们通常会采用 kalibr_allan 或 allan_variance_ros 工具进行标定。根据式 和第 4 节的内容,可知 IMU 的噪声形式分为连续形式和离散形式。而不同形式下其单位不同。结合前面的内容与对应的参考文献,下面直接给出不同形式下名称与单位。
名称 | 符号 | 单位 |
---|
gyroscope white noise (or gyro noise density) | | |
gyroscope random work (or gyro’s bias instability) | | |
accelerator white noise (or accel noise density) | | |
accelerator random work (or accel’s bias instability) | | |
discrete gyroscope white noise | | |
discrete gyroscope random work | | |
discrete accelerator white noise | | |
discrete accelerator random work | | |
Reference#