聊聊视觉惯性里程计的IMU预积分模型.docxVIP

为什么工程实践中我们使用视觉与IMU融合的解决方案即视觉惯性里程计（VIO）来估计运动而不是简单地使用视觉里程计（VO）。视觉惯性里程计的传感器主要包括相机和惯性测量单元（IMU）两种传感器各有优缺点，VIO的优势就在于IMU与相机的互补性。

视觉传感器在大多数纹理丰富的场景中效果很好，但是遇到玻璃或白墙这样特征少的场景就很难有效工作了。尽管如此相机数据的优点在于数据基本不会有漂移。如果将相机放在原地固定，那么估计的位姿也是固定不动的。

IMU传感器本身也是有自身缺点的，比如IMU长时间使用就会有非常大的累积误差。但是在短时间内，其相对位移数据又有很高的精度，所以当视觉传感器失效时，融合IMU数据，能够提高定位的精度。IMU虽然可以测得角速度和加速度，但这些量都存在明显的漂移，使得积分两次得到的数据就会存在很大误差。如果将IMU固定不动，其数据也会因为漂移的影响使得积分得到的位姿飘走。但是对于短时间内的快速运动，IMU能够提供相对准确的估计，这又刚好弥补了相机的缺点。相机运动过快时会出现模糊或者两帧之间重叠区域太少无法匹配。

IMU和相机就这样相辅相成，因此现在很多SLAM算法的研究都是融合相机和IMU数据进行位姿估计。

IMU误差部分

一提到VIO，相信很多人都会立刻想到一个名词——预积分。这也是希望接触VIO的研究者们最长看到的词汇。那么究竟预积分是怎么回事，为什么预积分在VIO中这么重要，就是接下来讨论的重点了。

从上面的公式可以看出，我们读的数据都不是客观事实，是在客观事实的基础上叠加上传感器的误差，也就是偏置和噪声。

a和g分别表示acc和gyro，w表示在世界坐标系，b表示IMU体坐标系。在后面的公式中会有很多下标，很容易混淆。

位移，速度和姿态（position,velocity,quaternion,i.e.PVQ）对时间的导数可以写成

前两个公式没问题，就是三个运动量之间的积分微分。第三个关于四元数微分，在这里稍微推导一下，方便大家后续阅读。

中间穿插了一段四元数推导，现在继续。

从第i时刻的PVQ对IMU的测量值进行积分得到第j时刻的PVQ：

因为IMU的采样频率高，通常为100Hz-1000Hz，数据量非常大，在做优化的时候，不可能将如此多的数据都放到状态变量中，因此通常的做法是每隔一段时间提取一个数据，比如每隔1秒提取一个。也就是上式中假如i是第一秒提取的IMU数据，j是第二秒提取的IMU数据。基本过程就是：已知第i秒的PVQ；第i秒和第j秒中间所有数据（如100个）以及我们已知的运动学知识积分，从第i秒一点一点积分得到第j秒的PVQ。但是这样在做后端优化的过程中，当我们进行迭代求解计算来更新和调整PVQ的值时，一旦（比如第1秒）的PVQ进行了调整，每一个中间过程以及后面所有的轨迹都要重新再积分算一遍，如果是100Hz，两秒之间有100个采集数据，就要计算100次积分。有没有什么方法是可以不用如此牵一发而动全身，摆脱冗繁的积分过程？预积分的目的就是尝试将这100次积分过程变成只有1次积分，或者说用1个值来代替100个值，通过预积分模型的应用可以大大节省了计算量。

一个简单的公式就可以将积分模型转为预积分模型。

改叫?IMU预积分。分别对应了位置，速度和姿态。

预积分量仅和IMU测量值有关，它将一段时间内的IMU数据直接积分起来就得到了预积分量。

下图为IMU和视觉传感器的模型，可以在一定程度上帮助理解。

IMU相机模型

预积分的离散形式

IMU的预积分误差

一段时间内IMU构建的预积分量作为测量值，对两时刻之间的状态量进行约束。

误差中的位移，速度和偏置都是直接相减得到的，也就是测量值减估计值。第二行是四元数的旋转误差。

通过将积分模型转化为预积分模型有效地减小了计算量，但是我们同时丢失了一些东西。当我们用1个结果代替（如100个）数据点的时候，我们就不知道这1个结果的不确定度了。在转化之前，这100个数据点每一个数据点的不确定度我们是知道的（因为IMU数据作为测量值的噪声方差我们能够标定），但是这100个数据积分形成的预积分量的方差是多少呢？这就需要我们在得到IMU预积分的结果之后，还要推导预积分量的协方差，需要知道IMU噪声和预积分量之间的线性递推关系。

假设已知了相邻时刻误差的线性传递方程：

即误差的传递分为两部分：当前时刻的误差传递给下一时刻，当前时刻测量噪声传递给下一时刻。

协方差矩阵可以通过递推计算得到：

其中

以上结果的推导利用了李群李代数的知识，过程过于繁复，因此没有在这里展开推导过程，以上结果与主流VIO开源代码（VINS-MONO）中写出的公式形式相同，读者可以直接对应代码中的部分来对照。如果有时间，我会再更新以上