跳转至

视觉惯性里程计（VIO）

概述

视觉惯性里程计（Visual-Inertial Odometry, VIO）是将相机和 IMU 数据融合进行实时位姿估计的技术。相机提供丰富的环境纹理信息但帧率有限，IMU 提供高频运动信息但存在漂移——两者互补融合，实现鲁棒的 6-DOF 位姿跟踪。

为什么需要 VIO

传感器 优势 局限
相机 纹理丰富、无漂移（回环检测） 帧率低（30Hz）、运动模糊、光照敏感
IMU 高频（200–1000Hz）、不受光照影响 长期漂移、需积分
VIO 融合 高频+高精度、鲁棒 系统复杂度增加

融合方式分类

graph TD
    A[VIO 融合方式] --> B[松耦合<br/>Loosely Coupled]
    A --> C[紧耦合<br/>Tightly Coupled]

    B --> B1[视觉里程计独立运行<br/>IMU 独立运行<br/>后端融合两者结果]

    C --> C1[视觉特征和 IMU 数据<br/>在同一优化框架内<br/>联合优化]

    A --> D[按后端分类]
    D --> D1[基于滤波<br/>EKF / MSCKF]
    D --> D2[基于优化<br/>Bundle Adjustment + IMU]

松耦合 vs 紧耦合

特性 松耦合 紧耦合
融合层级 结果级融合 原始数据级融合
精度 较低 更高
鲁棒性 单传感器失效时退化 更鲁棒（特征少时 IMU 补充）
实现复杂度 低 高
代表方案 - VINS-Mono, OKVIS, BASALT

紧耦合是主流

当前学术界和工业界的主流 VIO 方案都采用紧耦合方式。紧耦合能更好地处理退化场景（如纹理稀疏、快速运动），是更优的技术选择。

IMU 预积分理论

动机

在基于优化的 VIO 中，每次优化迭代更新状态估计后，需要重新积分 IMU 数据。IMU 预积分（Preintegration）将两个关键帧之间的 IMU 测量预先积分为相对运动约束，避免重复积分。

预积分量

两个关键帧 \(i\) 和 \(j\) 之间的预积分量：

旋转预积分：

\[ \Delta \mathbf{R}_{ij} = \prod_{k=i}^{j-1} \text{Exp}\left((\tilde{\boldsymbol{\omega}}_k - \mathbf{b}_g) \Delta t\right) \]

速度预积分：

\[ \Delta \mathbf{v}_{ij} = \sum_{k=i}^{j-1} \Delta \mathbf{R}_{ik} \cdot (\tilde{\mathbf{a}}_k - \mathbf{b}_a) \Delta t \]

位置预积分：

\[ \Delta \mathbf{p}_{ij} = \sum_{k=i}^{j-1} \left[ \Delta \mathbf{v}_{ik} \Delta t + \frac{1}{2} \Delta \mathbf{R}_{ik} \cdot (\tilde{\mathbf{a}}_k - \mathbf{b}_a) \Delta t^2 \right] \]

其中：

  • \(\tilde{\boldsymbol{\omega}}_k\) 为陀螺仪测量值
  • \(\tilde{\mathbf{a}}_k\) 为加速度计测量值
  • \(\mathbf{b}_g\), \(\mathbf{b}_a\) 为偏置
  • \(\text{Exp}(\cdot)\) 为 \(\mathfrak{so}(3) \to SO(3)\) 的指数映射

预积分的核心优势

当偏置估计 \(\mathbf{b}_g\), \(\mathbf{b}_a\) 变化较小时，可以通过一阶近似更新预积分量，而无需重新积分：

\[ \Delta \mathbf{R}_{ij}(\mathbf{b}_g + \delta \mathbf{b}_g) \approx \Delta \mathbf{R}_{ij}(\mathbf{b}_g) \cdot \text{Exp}\left(\frac{\partial \Delta \mathbf{R}_{ij}}{\partial \mathbf{b}_g} \delta \mathbf{b}_g\right) \]

这大幅减少了优化迭代中的计算量。

预积分协方差传播

预积分量的不确定性通过协方差矩阵传播：

\[ \boldsymbol{\Sigma}_{k+1} = \mathbf{A}_k \boldsymbol{\Sigma}_k \mathbf{A}_k^T + \mathbf{B}_k \mathbf{Q} \mathbf{B}_k^T \]

其中 \(\mathbf{A}_k\) 为状态转移矩阵，\(\mathbf{Q}\) 为 IMU 噪声协方差。

因子图公式化

VIO 问题可以自然地建模为因子图（Factor Graph）优化问题：

graph LR
    subgraph "状态变量"
    X0["X₀<br/>(p,R,v,b)"] 
    X1["X₁<br/>(p,R,v,b)"]
    X2["X₂<br/>(p,R,v,b)"]
    end

    subgraph "路标点"
    L1["L₁"] 
    L2["L₂"]
    L3["L₃"]
    end

    X0 --- |"IMU<br/>预积分"| X1
    X1 --- |"IMU<br/>预积分"| X2

    X0 --- |"视觉观测"| L1
    X0 --- |"视觉观测"| L2
    X1 --- |"视觉观测"| L1
    X1 --- |"视觉观测"| L3
    X2 --- |"视觉观测"| L2
    X2 --- |"视觉观测"| L3

    P["先验因子"] --- X0

状态向量

每个关键帧的状态：

\[ \mathbf{x}_k = \begin{bmatrix} \mathbf{p}_k \\ \mathbf{R}_k \\ \mathbf{v}_k \\ \mathbf{b}_{a,k} \\ \mathbf{b}_{g,k} \end{bmatrix} \in \mathbb{R}^3 \times SO(3) \times \mathbb{R}^3 \times \mathbb{R}^3 \times \mathbb{R}^3 \]

优化目标

\[ \mathbf{x}^* = \arg\min_{\mathbf{x}} \left\{ \| \mathbf{r}_p \|^2 + \sum_{k} \| \mathbf{r}_{\text{IMU}}^k \|_{\Sigma_k}^2 + \sum_{(i,j)} \| \mathbf{r}_{\text{vis}}^{ij} \|_{\Sigma_{ij}}^2 \right\} \]

其中：

  • \(\mathbf{r}_p\) 为先验残差
  • \(\mathbf{r}_{\text{IMU}}^k\) 为 IMU 预积分残差
  • \(\mathbf{r}_{\text{vis}}^{ij}\) 为视觉重投影残差
  • \(\| \cdot \|_{\Sigma}^2 = (\cdot)^T \Sigma^{-1} (\cdot)\) 为马氏距离

经典 VIO 系统

VINS-Mono

港科大开源的经典紧耦合单目 VIO 系统，是学习 VIO 的标准参考。

系统架构：

graph TD
    A[IMU 数据] --> B[预积分]
    C[图像] --> D[特征跟踪<br/>KLT 光流]

    B --> E[初始化]
    D --> E

    E --> F[非线性优化<br/>滑动窗口]
    F --> G[边缘化]

    F --> H[回环检测<br/>DBoW2]
    H --> I[全局位姿图优化]

    F --> J[输出位姿]
    I --> J

关键技术：

模块 方法
前端 KLT 光流特征跟踪
初始化 视觉 SfM + IMU 对齐
后端 滑动窗口 + 边缘化
回环 DBoW2 词袋检测 + 位姿图优化
优化器 Ceres Solver

OKVIS

ETH 开源，基于关键帧的紧耦合 VIO，支持双目。

MSCKF

基于 EKF 的高效 VIO，通过 Multi-State Constraint 避免维护特征点状态：

\[ \mathbf{x} = \begin{bmatrix} \mathbf{x}_{\text{IMU}} \\ \mathbf{x}_{C_1} \\ \mathbf{x}_{C_2} \\ \vdots \end{bmatrix} \]

特征点被观测到后不加入状态，而是在跟踪丢失时构建多状态约束。

BASALT

TUM 开源的高精度 VIO，使用双目 + IMU，特点是高效的非线性优化和光流前端。

系统对比

系统 融合方式 后端 相机配置 特色
VINS-Mono 紧耦合 优化 单目 完整系统、回环
VINS-Fusion 紧耦合 优化 单目/双目/多目 多传感器
OKVIS 紧耦合 优化 双目 关键帧管理
MSCKF 紧耦合 EKF 双目 计算高效
BASALT 紧耦合 优化 双目 高精度
ORB-SLAM3 紧耦合 优化 单目/双目/RGB-D 地图复用

实际部署注意事项

相机-IMU 标定

相机和 IMU 之间的外参 \(\mathbf{T}_{CI}\)（相对位姿）需要精确标定：

# 使用 Kalibr 工具
rosrun kalibr kalibr_calibrate_imu_camera \
    --bag calibration.bag \
    --cam cam_config.yaml \
    --imu imu_config.yaml \
    --target april_grid.yaml

时间同步

相机和 IMU 之间的时间偏移 \(t_d\) 会严重影响精度：

\[ t_{\text{cam}} = t_{\text{IMU}} + t_d \]

VINS-Mono 支持在线估计 \(t_d\)，但硬件级时间同步（如触发信号）更可靠。

常见失败场景

场景 原因 缓解方案
纹理缺失 白墙、天空 依赖 IMU 短时预测
快速旋转 运动模糊 高帧率相机、全局快门
光照剧变 特征跟踪失败 HDR 相机、鲁棒特征
长走廊 退化运动 多传感器融合
纯旋转 单目尺度不可观 双目或 IMU 约束

更多 SLAM 相关内容请参考 SLAM 专题

参考资料

  • Qin et al., "VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator", IEEE Trans. Robotics, 2018
  • Forster et al., "On-Manifold Preintegration for Real-Time Visual-Inertial Odometry", IEEE Trans. Robotics, 2017
  • Mourikis & Roumeliotis, "A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation", ICRA 2007
  • Sola, "Quaternion kinematics for the error-state Kalman filter", 2017
  • Kalibr 标定工具：https://github.com/ethz-asl/kalibr

评论 #