跳到主要内容

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 的加速度计天然带尺度(m/s2m/s^2),只要走个几米就能把尺度锁死。反过来,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 采样)。数学上定义帧 ii 到 jj 之间的预积分量:

αij=∫∫Rik(ak−ba) dt2,βij=∫Rik(ak−ba) dt,γij=∫(ωk−bg) dt\alpha_{ij} = \int\int R_{ik}(a_k - b_a) \, dt^2, \quad \beta_{ij} = \int R_{ik}(a_k - b_a) \, dt, \quad \gamma_{ij} = \int (\omega_k - b_g) \, dt

这三个量只依赖原始 IMU 测量 (ak,ωk)(a_k, \omega_k) 和 Bias (ba,bg)(b_a, b_g),与全局位姿无关。Bias 变化时用一阶泰勒近似修正,不用重新积分。优化时残差写成:

rIMU=[RiT(pj−pi−viΔt−12gΔt2)−αijRiT(vj−vi−gΔt)−βij2[qi−1⊗qj⊗γij−1]xyzbaj−baibgj−bgi]r_{IMU} = \begin{bmatrix} R_i^T(p_j - p_i - v_i \Delta t - \frac{1}{2}g\Delta t^2) - \alpha_{ij} \\ R_i^T(v_j - v_i - g\Delta t) - \beta_{ij} \\ 2[q_i^{-1} \otimes q_j \otimes \gamma_{ij}^{-1}]_{xyz} \\ b_a^j - b_a^i \\ b_g^j - b_g^i \end{bmatrix}

预积分等于把"两帧之间发生了什么"压缩成三个数(加两个 Bias 随机游走项)。窗口一挪,新帧预积分算一次,扔进优化循环里被反复使用,边际成本几乎为零。

4. 初始化​

单目 VIO 最难的一环是冷启动。相机不动 IMU 观测不到重力方向;相机纯旋转视觉解不出深度;加速度不够大 Bias 和重力方向解不开。VINS-Mono 用松耦合初始化:先纯视觉解结构,再把 IMU 对齐上去。

步骤:

  1. 纯视觉 SfM:前 20 帧滑窗做小规模 BA,恢复一组尺度未知的位姿和地图点
  2. 陀螺 Bias 估计:预积分的旋转量应该等于视觉解出的相对旋转,最小化两者之差求 bgb_g
  3. 重力 + 尺度 + 速度联合线性方程:预积分的位置增量应该等于视觉位置增量乘尺度,加上重力和速度积分。写成 Ax=b\mathbf{A}\mathbf{x} = \mathbf{b} 一次解出来
  4. 重力细化:重力大小已知(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 个地图点逆深度

目标函数是三项残差的加权和:

min⁡x{∥rprior∥2+∑k∈B∥rIMU(zk,k+1,x)∥Pk2+∑(l,j)∈Cρ(∥rcam(zlj,x)∥Pl2)}\min_x \left\{ \|r_{prior}\|^2 + \sum_{k \in B} \|r_{IMU}(z_{k,k+1}, x)\|^2_{P_k} + \sum_{(l,j) \in C} \rho(\|r_{cam}(z_l^j, x)\|^2_{P_l}) \right\}
  • rpriorr_{prior}:边缘化留下的先验项
  • rIMUr_{IMU}:预积分残差
  • rcamr_{cam}:视觉重投影残差,套一层 Huber 核 ρ\rho 抑制外点

用 Ceres Solver 上 Dogleg/LM 迭代求解,5–15 次迭代收敛。地图点用逆深度参数化(一个数而不是 (x,y,z)(x,y,z) 三个数),好处是远点不会因为深度接近无穷让 Jacobian 病态。

6. 边缘化​

窗口满了要挪,新关键帧进来,最老的一帧得出去。但那一帧观测过的地图点和 IMU 约束不能直接扔掉,否则等于丢信息。边缘化用 Schur complement 把老变量的信息压进先验项:

[HmmHmrHrmHrr][ΔxmΔxr]=[bmbr]\begin{bmatrix} H_{mm} & H_{mr} \\ H_{rm} & H_{rr} \end{bmatrix} \begin{bmatrix} \Delta x_m \\ \Delta x_r \end{bmatrix} = \begin{bmatrix} b_m \\ b_r \end{bmatrix}

对老变量 xmx_m 消元后得到 xrx_r 上的等价方程 H′Δxr=b′H' \Delta x_r = b',其中 H′=Hrr−HrmHmm−1HmrH' = H_{rr} - H_{rm} H_{mm}^{-1} H_{mr}。这个 H′H' 就是下一轮优化的先验信息矩阵。

VINS-Mono 有两种边缘化策略:

  • 最老帧是关键帧:老关键帧和它独有的地图点全部边缘化,形成先验
  • 最老帧不是关键帧:直接丢掉视觉观测,只保留 IMU 预积分把两侧连起来

这个二选一策略的实际意义:关键帧稀疏放置在场景变化处,非关键帧只做 IMU 桥接。既保证窗口里视野覆盖广,又不浪费计算。

边缘化会引入一个后遗症——先验的线性化点冻结。H′H' 是在老变量当前估计处线性化算出来的,后续优化不能改变这些点的线性化位置,否则可观性错误会累积。这是 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_easy0.340.15
MH_03_medium0.200.16
MH_05_difficult0.470.29
V1_02_medium0.130.11
V2_03_difficult0.290.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 揉进单目场景。往后看,它直接催生了:

  • VINS-Fusion:作者自己的双目/GPS 扩展
  • VINS-Mobile:手机端 ARM 版
  • ORB-SLAM3:滑窗 + Atlas 多地图,思路上接近 VINS-Mono 但工程更完善
  • Kimera:VINS-Mono + 语义层
  • DM-VIO:直接法 + 延迟边缘化,精度反超 VINS-Mono

在 DROID-SLAM 这类端到端学习方法出现之前,VINS-Mono 是单目 VIO 的事实标准,也是 SLAM 课上最常被作为参考实现拆解的一份代码。