VINS-Mono:把 IMU 塞进滑窗
论文:VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator(HKUST 沈劭劼组,2018)。VINS-Mono 想说的事情不复杂:单目相机拿不出尺度,IMU 短时间内又准得离谱,两者 信息互补。所以不如把它们塞进同一个滑动窗口,让一个非线性优化把位姿、速度、Bias、地图点、外参、时间戳全部一起解出来。
1. 为什么单目 VIO 要滑窗 + 优化
先说清楚 VIO 之前的两条路线,才能理解为什么滑窗优化在 2017 前后突然成了主流。
滤波路线(MSCKF / ROVIO) 用 EKF 递推:IMU 做预测,视觉做更新,状态只保留当前一时刻加上有限帧的相机克隆。优点是常数时间、无需回溯;缺点是线性化点定下来就改不了,非线性重的场景(快速旋转、初始化)容易发散。
全局 BA 路线(ORB-SLAM 的 mono 版本) 把所有帧和点堆进一个大优化问题,效果最好但代价是 O(帧数³),实时性顶不住。
滑窗优化是折中:只保留最近 N 帧(VINS-Mono 用 N=10),窗口里的所有变量一起优化,窗口外的旧信息通过边缘化变成先验塞回来。这样既能反复重新线性化(比滤波精),又不用维护整个历史(比全局 BA 快)。VINS-Mono 是把这个思路做到工程可用的第一个开源单目 VIO 系统。
为什么单目非要配 IMU?单目相机的尺度不可观——一张图看不出物体是 10cm 还是 10m,纯视觉 SLAM 拿到的地图是一个未知比例的东 西。IMU 的加速度计天然带尺度(),只要走个几米就能把尺度锁死。反过来,IMU 单独用会因 Bias 漂移积分爆掉(几秒钟位置误差就到米级),必须靠视觉不断把 Bias 校准回来。
2. 系统概览
VINS-Mono 分成四个模块,前后串成一条流水线:
图像 ─┬─► 前端(KLT 光流跟踪 + 关键帧选择)
│
IMU ─┴─► 预积分(帧间 IMU 增量)
│
▼
初始化(vision-only SfM → 对齐 IMU)
│
▼
滑窗紧耦合优化(视觉重投影 + IMU 预积分 + 边缘化先验)
│
▼
回环检测(DBoW2 + 4-DoF 位姿图优化)
每一步都单独可以拆开跑(作者也在代码里做了模块化),但真正让 VINS-Mono 成为参考实现的是它把这四块同时做好了,之前的开源系统总在某一环节掉链子。
3. IMU 预积分
预积分是 VIO 里最重要的一个 trick,源自 Forster 等人的 IMU Preintegration on Manifold(2015)。要理解为什么必须预积分,先看不预积分会发生什么。
IMU 一般 100–1000 Hz,视觉 30 Hz。两个视觉帧之间可能夹着几十次 IMU 采样。朴素做法是:每次优化时把两帧之间的 IMU 从头积分一遍。问题是——积分依赖当前帧的位姿和 Bias 猜测值,只要这两个变量在优化中变一点(这正是优化在干的事),就得重新积分一遍。一次优化几百次迭代乘上几十次 IMU 积分,算力立刻爆炸。
预积分的解决方案:把 IMU 积分拆成两部分——一部分只跟原始测量和 Bias 有关(可以离线算),另一部分只跟当前帧位姿有关(改也没关系,因为不涉及 IMU 采样)。数学上定义帧 到 之间的预积分量:
这三个量只依赖原始 IMU 测量 和 Bias ,与全局位姿无关。Bias 变化时用一阶泰勒近似修正,不用重新积分。优化时残差写成:
预积分等于把"两帧之间发生了什么"压缩成三个数(加两个 Bias 随机游走项)。窗口一挪,新帧预积分算一次,扔进优化循环里被反复使用,边际成本几乎为零。
4. 初始化
单目 VIO 最难的一环是冷启动。相机不动 IMU 观测不到重力方向;相机纯旋转视觉解不出深度;加速度不够大 Bias 和重力方向解不开。VINS-Mono 用松耦合初始化:先纯视觉解结构,再把 IMU 对齐上去。
步骤:
- 纯视觉 SfM:前 20 帧滑窗做小规模 BA,恢复一组尺度未知的位姿和地图点
- 陀螺 Bias 估计:预积分的旋转量应该等于视觉解出的相对旋转,最小化两者之差求
- 重力 + 尺度 + 速度联合线性方程:预积分的位置增量应该等于视觉位置增量乘尺度,加上重力和速度积分。写成 一次解出来
- 重力细化:重力大小已知(9.81),把 3-DoF 的向量约束到 2-DoF 球面上,再优化一次
整个过程需要 5–10 秒的充分激励运动(走走停停、变向)。如果起始阶段机器人静止或匀速直线,尺度和 Bias 会不可观,初始化直接失败。这个限制到现在都没根本解决,是所有单目 VIO 的通病。
5. 滑窗优化
初始化之后进入正常运行状态,滑窗里保留最近 10 个关键帧。所有变量堆成一个向量:
x = [p_0, v_0, q_0, b_a_0, b_g_0, # 关键帧 0
p_1, v_1, q_1, b_a_1, b_g_1, # 关键帧 1
...
p_9, v_9, q_9, b_a_9, b_g_9, # 关键帧 9
T_bc, # 相机-IMU 外参
t_d, # 时间戳偏差
λ_0, λ_1, ..., λ_M] # M 个地图点逆深度
目标函数是三项残差的加权和:
- :边缘化留下的先验项
- :预积分残差
- :视觉重投影残差,套一层 Huber 核 抑制外点
用 Ceres Solver 上 Dogleg/LM 迭代求解,5–15 次迭代收敛。地图点用逆深度参数化(一个数而不是 三个数),好处是远点不会因为深度接近无穷让 Jacobian 病态。
6. 边缘化
窗口满了要挪,新关键帧进来,最老的一帧得出去。但那一帧观测过的地图点和 IMU 约束不能直接扔掉,否则等于丢信息。边缘化用 Schur complement 把老变量的信息压进先验项:
对老变量 消元后得到 上的等价方程 ,其中 。这个 就是下一轮优化的先验信息矩阵。
VINS-Mono 有两种边缘化策略:
- 最老帧是关键帧:老关键帧和它独有的地图点全部边缘化,形成先验
- 最老帧不是关键帧:直接丢掉视觉观测,只保留 IMU 预积分把两侧连起来
这个二选一策略的实际意义:关键帧稀疏放置在场景变化处,非关键帧只做 IMU 桥接。既保证窗口里视野覆盖广,又不浪费计算。
边缘化会引入一个后遗症——先验的线性化点冻结。 是在老变量当前估计处线性化算出来的,后续优化不能改变这些点的线性化位置,否则可观性错误会累积。这是 First Estimate Jacobian(FEJ)问题的另一面,VINS-Mono 没做严格的 FEJ 处理,靠工程调参压住。
7. 回环
前端用 DBoW2 词袋检索候选回环帧,几何验证用 PnP + RANSAC。发现回环之后不是直接改滑窗里的位姿——那样会引起大跳变。VINS-Mono 做的是:把回环约束加进滑窗(当前帧和历史帧之间加一个视觉约束),然后离线做一次 4-DoF 位姿图优化(只优化 3 维位置 + yaw,roll/pitch 因为重力约束是可观的不用优化)。
为什么 4-DoF?IMU 观测重力后 roll/pitch 全局可观,也就是说滑窗漂移只在 xyz + yaw 四个维度上累积。这个观察让位姿图规模缩小近一半,跑得更快。
8. 实验结果
在 EuRoC MAV 数据集上(无人机室内飞行):
| 序列 | OKVIS RMSE (m) | VINS-Mono RMSE (m) |
|---|---|---|
| MH_01_easy | 0.34 | 0.15 |
| MH_03_medium | 0.20 | 0.16 |
| MH_05_difficult | 0.47 | 0.29 |
| V1_02_medium | 0.13 | 0.11 |
| V2_03_difficult | 0.29 | 0.27 |
VINS-Mono 在大多数序列上略优于 OKVIS(也是滑窗优化,但视觉更保守)。真正让 VINS-Mono 出圈的不是精度而是鲁棒性:在快速运动、光照变化、部分遮挡下不容易崩,代码开源易读,成了后续几乎所有单目 VIO 论文的基线。
9. 局限
- 单目尺度依赖 IMU 激励:静止或匀速时尺度会漂
- KLT 光流对纹理依赖强:白墙、玻璃、夜间失效
- 预积分假设 IMU 白噪声:低端 MEMS IMU 有色噪声会让协方差估错
- 滑窗大小固定 10 帧:长走廊场景下 10 帧覆盖太短,回环没触发前累积误差就已经出问题
- 4-DoF 位姿图:假设重力方向已知且不变,不适合火箭、潜水器这种重力方向可变的场景
10. VINS-Mono 在 VIO 家谱里的位置
往前看,VINS-Mono 站在 OKVIS(滑窗 + 双目)、MSCKF(EKF + null space)、ORB-SLAM(关键帧 BA)三个流派交汇处,把它们的核心 idea 揉进单目场景。往后看,它直接催生了: