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

作者 | LZ紫色大智  编辑 | 汽车人

原文链接：https://zhuanlan.zhihu.com/p/90213963

点击下方卡片，关注“自动驾驶之心”公众号

ADAS巨卷干货，即可获取

点击进入→自动驾驶之心【SLAM】技术交流群

本文只做学术分享，如有侵权，联系删文

为什么工程实践中我们使用视觉与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）中写出的公式形式相同，读者可以直接对应代码中的部分来对照。如果有时间，我会再更新以上公式的推导过程。

以上就是预积分的简要介绍，在VIO开源框架（如VINS-MONO）中，IMU预积分部分属于前端部分，是在采集了IMU数据之后就立刻进行的运算。在完整的VIO系统中除前端外还有很多其他的部分，包括：初始化部分（即IMU数据与相机数据的对齐）以及最重要的后端优化求解部分。而后端优化求解还包括很多内容，比如整体的目标函数如何构建，求解优化的过程中运用了滑动窗口的方法来保证状态量的数量从而控制计算量等，随着我个人学习进度的推进还会进行总结和整理。

投稿作者为『自动驾驶之心知识星球』特邀嘉宾，欢迎加入交流！

① 全网独家视频课程

BEV感知、毫米波雷达视觉融合、多传感器标定、多传感器融合、多模态3D目标检测、车道线检测、轨迹预测、在线高精地图、世界模型、点云3D目标检测、目标跟踪、Occupancy、cuda与TensorRT模型部署、大模型与自动驾驶、Nerf、语义分割、自动驾驶仿真、传感器部署、决策规划、轨迹预测等多个方向学习视频（扫码即可学习）

视频官网：www.zdjszx.com

② 国内首个自动驾驶学习社区

近2400人的交流社区，涉及30+自动驾驶技术栈学习路线，想要了解更多自动驾驶感知（2D检测、分割、2D/3D车道线、BEV感知、3D目标检测、Occupancy、多传感器融合、多传感器标定、目标跟踪、光流估计）、自动驾驶定位建图（SLAM、高精地图、局部在线地图）、自动驾驶规划控制/轨迹预测等领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频，期待交流！

③【自动驾驶之心】技术交流群

自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多模态感知、Occupancy、多传感器融合、transformer、大模型、点云处理、端到端自动驾驶、SLAM、光流估计、深度估计、轨迹预测、高精地图、NeRF、规划控制、模型部署落地、自动驾驶仿真测试、产品经理、硬件配置、AI求职交流等方向。扫码添加汽车人助理微信邀请入群，备注：学校/公司+方向+昵称（快速入群方式）

④【自动驾驶之心】平台矩阵，欢迎联系我们！