【SLAM】IMU预积分的理解、手把手推导（1/4）

在工程实践中我们常常使用视觉与IMU融合的解决方案，即视觉惯性里程计（VIO）来估计运动，而不是简单地使用视觉里程计（VO）。当使用到IMU时，预积分就是绕不开的一个话题。

由于篇幅设置，IMU预积分分为4篇完成：

• 【SLAM】IMU预积分的理解、手把手推导（1/4）：概要介绍
• 【SLAM】IMU预积分的理解、手把手推导（2/4）：噪声分离、分布形式、递推形式
• 【SLAM】IMU预积分的理解、手把手推导（3/4）：零偏更新后的速算
• 【SLAM】IMU预积分的理解、手把手推导（4/4）：残差的雅可比、总结

本文将就概要设计，即IMU预积分的原理和优势特点，进行详解。

为什么需要IMU

IMU简介

IMU，中文名叫惯性测量单元，英文名：Inertial measurement unit。它可以测量两个东西，加速度$a$是沿三个轴$a_x$、$a_y$、$a_z$方向的线加速度，而角速度$w$就是这三个方向的角速度$w_x$、$w_y$、$w_z$。

IMU的频率比较高，一般都在$100HZ$以上。在IMU内部，除了通常的白噪声，还有个特别的量零偏$bias$，它是传感器内部机械、温度等各种物理因素产生的传感器内部误差的综合参数。IMU的加速度计和陀螺仪的每个轴都用彼此相互独立的参数建模，每个时刻可实现自身主体加速度$a$和角速度$w$的测量，那么对其的观测模型可以如下建模：

假设IMU的加速度和角速度的真实值为$a$、$w$，测量值为$\tilde{a}$、$\tilde{w}$，那么有：

$${\tilde{w}}^b=w^b+b^g+{\eta }^g$$

$${\tilde{a}}^b=q_{bw}(a^w+g^w)+b^a+{\eta }^a$$

其中：

• 上下标的$g$表示gyro，即陀螺仪，$b^g$、${\eta }^g$就是陀螺仪的零偏$bias$、陀螺仪的白噪声
• 上下标的$a$表示acc，即加速度计，$b^a$、${\eta }^a$就是加速度计的零偏$bias$、陀螺仪的白噪声
• 上下标的$b$表示body，即IMU坐标系
• 上下标的$w$表示world，即世界坐标系
• $q_{bw}$表示从世界坐标系到IMU坐标系的转换关系
• $g$表示重力加速度

需要注意的是：加速度计的测量值是耦合了重力加速度$g$的，同时它的测量值是相对于IMU坐标系的。

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

IMU和视觉的互补

在纯视觉的SLAM或者VO中，由于图像的运动模糊、遮挡、快速运动、纯旋转、尺度不确定性的一系列问题，导致仅靠一个摄像头很难完成我们实际场景的应用需求。而IMU直接可以得到运动主体自身的角速度、加速度的测量数据，从而对运动有一个约束，或者说与视觉形成互补，可实现快速运动的定位和主体纯旋转的处理，从而进一步提高SLAM/VIO的可靠性。

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

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

IMU预积分的作用

在基于BA的纯视觉SLAM图优化算法中，各个节点、还有节点之间的边都可以是待优化的量。IMU预积分的初衷，希望将IMU的相对测量信息转换为帧与帧之间约束节点（位姿）的边，从而参与到优化框架中。

因此，IMU预积分的立足点就落在相邻两帧之间的IMU积分测量值计算及其动态更新上（IMU的频率很高），获得每周期PVQ（位置、速度和姿态）增量的测量值，对照其他通过非IMU方式获得的PVQ增量的估计值（纯视觉SLAM方案），进而获得PVQ增量的残差。然后以该残差构造代价函数对每个节点的PVQ进行迭代更新和优化。

在这个过程中通过对IMU测量噪声协方差的递推，获得了每周期PVQ增量测量值的噪声分布，再通过求协方差的逆的平方根，获得了信息矩阵，对残差中PVQ的比例进行调节，起到平衡权重的作用。

假设相邻两帧为第$i$、$j$帧，那么待优化变量就有：${\mathbf{R}}_i$、${\mathbf{p}}_i$、${\mathbf{v}}_i$、${\mathbf{R}}_j$、${\mathbf{p}}_j$、${\mathbf{v}}_j$、$\delta {\mathbf{b}}_i^g$、$\delta {\mathbf{b}}_i^a$。但是IMU预积分对应的残差主要是PVQ三个增量，因此信息矩阵要调节的比例也主要针对残差中的PVQ三项，而非全部待优化变量。

IMU预积分理论

IMU预积分的优化方法

优化的核心是构造代价函数，而代价函数的核心是构造残差，IMU预积分给出了具体的残差定义，即相邻两帧之间PVQ增量的估计值和测量值之差：

$$残{差}_{ij}=PVQ增量估计{值}_{ij}-PVQ增量测量{值}_{ij}$$

其中：PVQ增量估计值通常需要通过非IMU的方式获得，例如纯视觉SLAM方案中帧与帧中关键点的匹配结算，这也是IMU预积分与外部的唯一界面，或者说是IMU预积分对外部的唯一依赖，只要有了这个估计值，IMU预积分就可以独立运作了。PVQ增量测量值就来自IMU预积分。

在图优化的过程中，要进行局部甚至全局的反复优化，随着优化的推进，IMU的测量零偏（bias）也会变化，此时残差中的PVQ增量测量值就需要重新计算，IMU预积分就提供了一个近似的测量值修正方法，免去了积分的重新计算，是预积分降低计算量的关键。

这个残差公式可以直接添加到诸如Ceres的优化框架中，用以实现一个代价函数，通过多轮迭代实现对每个节点精确位姿的解算。

因此IMU预积分的主线就是：求解相邻两帧之间PVQ增量的测量值，试图通过在既有IMU预积分测量值上添加一个近似修正量的方式来避免重新积分。

IMU预积分测量值的速算

前面提到，之所以PVQ增量测量值需要重新积分，关键在于角速度和加速度的测量零偏（bias）在优化过程中会发生变化。

为了避免重新积分，IMU预积分的解决方案是假设：

$$PVQ增量测量值（新）=PVQ增量测量值（旧）+PVQ增量测量值更新量$$

那么，只要获得PVQ增量测量值更新量就可以避免重新积分了。

既然是由于bias引起的，那么就把PVQ增量测量值当做bias的函数，求该函数对bias的导数，那么bias更新量与函数导数的乘积就是bias发生变化后PVQ测量值的更新量（近似值）。这样一来每当bias发生变化就能够通过线性运算直接获得新的PVQ增量测量值，而不需要重新积分。

$$PVQ增量测量值（新）=PVQ增量测量值（旧）+\frac{\partial PVC增量测量值}{\partial 测量零偏bias}\times 零偏更新量$$

以上就是IMU预积分避免重新积分，降低运算量的关键。

IMU预积分优化的残差比例调节

IMU预积分实现了测量噪声（即协方差）的递推计算，并将协方差转换成信息矩阵，应用于优化过程，调节残差的比例，起到平衡权重的作用。

回忆卡尔曼滤波，也有一个协方差矩阵$P$，迭代更新，在更新步骤，$P$被用于计算增益$K$，增益$K$实际上是估计值的不确定性在总的系统不确定性中所占的比值，然后增益$K$被用于调节残差的比例。

在IMU预积分中协方差的作用和卡尔曼滤波器中协方差$P$的作用完全相同，也是需要迭代更新，也是用于调节残差的比例。在IMU预积分中，首先将测量噪声从PVQ增量测量值中分离出来，证明其服从高斯分布，然后推导出协方差的递推公式。

所谓信息矩阵其实就是用于调节残差比例的权重，首先要取协方差矩阵的逆矩阵，相当于取了协方差的倒数，方差越大权重越小，反之权重越大，然后再将逆矩阵转成信息矩阵。

重力加速度和绝对姿态求解

传统的SLAM算法很少使用加速度来计算速度和位置，原因是加速度计的测量值是包含反向重力的比力，而不是纯加速度。这使得一旦姿态不准确，重力投影误差将对速度和位置积分产生严重影响。

IMU预积分直接使用加速度的测量值进行速度和位置的估计，没有使用诸如编码器、毫米波雷达等测速装置。那么IMU预积分是如何解算出重力加速的方向，从而获得绝对位姿的呢？

IMU预积分仍然是通过优化的方法获得重力加速度的方向的。PVQ增量测量值是通过IMU预积分获得的测量值，其中耦合了重力加速度，特别是P和V，并不是通常意义上的位移和速度，而是根据IMU加速度计的测量值计算出来的所谓的位移和速度，耦合了重力加速度之后在垂直方向上位移和速度会产生巨大的分量，P和V的这个垂直分量在真实世界是不存在的，存在的唯一目的是协助找出重力加速度的真正方向。

而PVQ增量估计值是通过非IMU方式获得的，原本是没有重力加速度分量在其中的，P和V在垂直方向也就没有对应的分量，那么我们根据对重力加速度方向的估计，添加一个重力加速度的分量进去（即待求解的重力加速度及其方向），使P和V的估计值在垂直方向获得对应的分量。再通过对残差的迭代优化，使估计值和测量值相匹配，就能够获得重力加速度的真实方向。

注意：如果上面的预积分理论没怎么看懂，可以先结合后续的的具体推导过程和最后的总结部分。预积分部分的理论比较复杂，想要一次阅读就能够完全理解，还是很难很难。

相关阅读

• IMU预积分的理解和推导
• 简明预积分推导
• 如何理解IMU以及其预积分