MSCKF VIO系统概述与架构设计
文章简介
本文是MSCKF VIO系列文章的第一篇，详细介绍MSCKF算法的核心思想、系统架构和状态表示方法。通过本文，读者可以对MSCKF VIO有一个全面的认识。
目录
文章目录
- [MSCKF VIO系统概述与架构设计](#MSCKF VIO系统概述与架构设计)
-
- 文章简介
- 目录
- [1. 算法背景](#1. 算法背景)
-
- [1.1 视觉-惯性里程计](#1.1 视觉-惯性里程计)
- [1.2 传统方法的局限](#1.2 传统方法的局限)
- [1.3 MSCKF的创新](#1.3 MSCKF的创新)
- [2. 系统架构](#2. 系统架构)
-
- [2.1 整体架构图](#2.1 整体架构图)
- [2.2 模块说明](#2.2 模块说明)
- [3. 状态表示](#3. 状态表示)
-
- [3.1 IMU状态向量（21维）](#3.1 IMU状态向量（21维）)
- [3.2 相机状态向量（6维/帧）](#3.2 相机状态向量（6维/帧）)
- [3.3 完整状态向量](#3.3 完整状态向量)
- [4. 坐标系定义](#4. 坐标系定义)
-
- [4.1 坐标系说明](#4.1 坐标系说明)
- [4.2 坐标变换关系](#4.2 坐标变换关系)
- [5. 算法流程](#5. 算法流程)
-
- [5.1 主循环流程](#5.1 主循环流程)
- [5.2 详细流程图](#5.2 详细流程图)
- [6. 关键数据结构](#6. 关键数据结构)
-
- [6.1 MapServer（特征地图）](#6.1 MapServer（特征地图）)
- [6.2 IMU数据缓冲](#6.2 IMU数据缓冲)
- [7. 参数配置](#7. 参数配置)
-
- [7.1 噪声参数](#7.1 噪声参数)
- [7.2 系统参数](#7.2 系统参数)
- [7.3 关键帧选择参数](#7.3 关键帧选择参数)
- [8. 性能特点](#8. 性能特点)
-
- [8.1 计算复杂度](#8.1 计算复杂度)
- [8.2 存储需求](#8.2 存储需求)
- [8.3 实时性能](#8.3 实时性能)
- [9. 与其他方法对比](#9. 与其他方法对比)
- [10. 总结](#10. 总结)
- 下一篇预告
- 参考文献
1. 算法背景
1.1 视觉-惯性里程计
视觉-惯性里程计（Visual-Inertial Odometry, VIO）是一种融合相机和IMU数据的状态估计方法。相比纯视觉SLAM，VIO具有以下优势：
- 尺度可观：IMU提供绝对尺度信息
- 运动模糊鲁棒：IMU在相机模糊时仍可工作
- 高频输出：IMU数据频率高，可提供平滑估计
1.2 传统方法的局限
EKF-SLAM方法：
- 将特征点加入状态向量
- 状态维度随特征数量增长
- 计算复杂度为 O ( n 2 ) O(n^2) O(n2)
视觉里程计方法：
- 仅使用相机数据
- 存在尺度漂移
- 快速运动下容易失败
1.3 MSCKF的创新
MSCKF（Multi-State Constraint Kalman Filter）的核心创新：
- 不将特征点加入状态向量：避免状态维度爆炸
- 维护多个相机状态：利用多帧约束
- 特征三角化后丢弃：控制计算复杂度
2. 系统架构
2.1 整体架构图
┌─────────────────────────────────────────────────────────────┐
│ MSCKF VIO 系统架构 │
├─────────────────────────────────────────────────────────────┤
│ │
│ ┌─────────────┐ ┌─────────────┐ ┌─────────────┐ │
│ │ IMU传感器 │ │ 立体相机 │ │ 特征跟踪器 │ │
│ └──────┬──────┘ └──────┬──────┘ └──────┬──────┘ │
│ │ │ │ │
│ ▼ ▼ ▼ │
│ ┌─────────────────────────────────────────────────────┐ │
│ │ 数据同步与缓冲 │ │
│ │ - IMU数据缓存 │ │
│ │ - 时间戳对齐 │ │
│ └─────────────────────┬───────────────────────────────┘ │
│ │ │
│ ┌──────────────┼──────────────┐ │
│ ▼ ▼ ▼ │
│ ┌─────────────┐ ┌─────────────┐ ┌─────────────┐ │
│ │ IMU传播 │ │ 状态增广 │ │ 测量更新 │ │
│ │ - 状态积分 │ │ - 添加新帧 │ │ - 特征处理 │ │
│ │ - 协方差传播│ │ - 协方差扩展│ │ - 卡尔曼增益│ │
│ └─────────────┘ └─────────────┘ └─────────────┘ │
│ │ │
│ ▼ │
│ ┌─────────────────────────────────────────────────────┐ │
│ │ 状态管理 │ │
│ │ - 相机状态剪枝 │ │
│ │ - 在线重置 │ │
│ └─────────────────────────────────────────────────────┘ │
│ │ │
│ ▼ │
│ ┌─────────────────────────────────────────────────────┐ │
│ │ 输出 │ │
│ │ - 位姿估计 │ │
│ │ - 速度估计 │ │
│ │ - 特征点云 │ │
│ └─────────────────────────────────────────────────────┘ │
│ │
└─────────────────────────────────────────────────────────────┘2.2 模块说明
| 模块 | 功能 | 输入 | 输出 |
|---|---|---|---|
| IMU传感器 | 采集惯性数据 | - | 角速度、加速度 |
| 立体相机 | 采集图像数据 | - | 左右图像 |
| 特征跟踪器 | 特征检测与匹配 | 图像 | 特征观测 |
| IMU传播 | 状态预测 | IMU数据 | 预测状态 |
| 状态增广 | 添加新相机帧 | 图像时间戳 | 扩展状态 |
| 测量更新 | 状态修正 | 特征观测 | 更新状态 |
| 状态管理 | 控制状态数量 | - | 剪枝后状态 |
3. 状态表示
3.1 IMU状态向量（21维）
IMU状态向量定义如下：
x I M U = θ b g v b a p δ q I U δ p I U \mathbf{x}_{IMU} = \begin{bmatrix} \boldsymbol{\theta} \\ \mathbf{b}g \\ \mathbf{v} \\ \mathbf{b}a \\ \mathbf{p} \\ \delta\mathbf{q}{IU} \\ \delta\mathbf{p}{IU} \end{bmatrix} xIMU= θbgvbapδqIUδpIU
各分量说明：
| 分量 | 符号 | 维度 | 说明 |
|---|---|---|---|
| 姿态误差角 | θ \boldsymbol{\theta} θ | 3 | 李代数表示的旋转误差 |
| 陀螺仪偏置 | b g \mathbf{b}_g bg | 3 | 陀螺仪零偏 |
| 速度 | v \mathbf{v} v | 3 | 世界坐标系下的速度 |
| 加速度计偏置 | b a \mathbf{b}_a ba | 3 | 加速度计零偏 |
| 位置 | p \mathbf{p} p | 3 | 世界坐标系下的位置 |
| 外参旋转 | δ q I U \delta\mathbf{q}_{IU} δqIU | 3 | IMU-相机旋转误差 |
| 外参平移 | δ p I U \delta\mathbf{p}_{IU} δpIU | 3 | IMU-相机平移误差 |
代码定义：
cpp
// imu_state.h
struct IMUState {
StateIDType id; // 状态ID
double time; // 时间戳
Eigen::Vector4d orientation; // 姿态四元数 [x,y,z,w]
Eigen::Vector3d position; // 位置
Eigen::Vector3d velocity; // 速度
Eigen::Vector3d gyro_bias; // 陀螺仪偏置
Eigen::Vector3d acc_bias; // 加速度计偏置
Eigen::Matrix3d R_imu_cam0; // IMU-相机旋转
Eigen::Vector3d t_cam0_imu; // IMU-相机平移
// 可观测性约束相关
Eigen::Vector4d orientation_null;
Eigen::Vector3d position_null;
Eigen::Vector3d velocity_null;
};3.2 相机状态向量（6维/帧）
相机状态向量定义：
x c a m = θ c p c \mathbf{x}_{cam} = \begin{bmatrix} \boldsymbol{\theta}_c \\ \mathbf{p}_c \end{bmatrix} xcam=θcpc
代码定义：
cpp
// cam_state.h
struct CAMState {
StateIDType id; // 状态ID
double time; // 时间戳
Eigen::Vector4d orientation; // 姿态四元数
Eigen::Vector3d position; // 位置
// 可观测性约束相关
Eigen::Vector4d orientation_null;
Eigen::Vector3d position_null;
// 立体相机外参
static Eigen::Isometry3d T_cam0_cam1;
};3.3 完整状态向量
当系统中有 N N N 个相机状态时，完整状态向量为：
x = x I M U x c a m 1 x c a m 2 ⋮ x c a m N \mathbf{x} = \begin{bmatrix} \mathbf{x}{IMU} \\ \mathbf{x}{cam_1} \\ \mathbf{x}{cam_2} \\ \vdots \\ \mathbf{x}{cam_N} \end{bmatrix} x= xIMUxcam1xcam2⋮xcamN
维度 ： 21 + 6 N 21 + 6N 21+6N
代码实现：
cpp
struct StateServer {
IMUState imu_state; // IMU状态
CamStateServer cam_states; // 相机状态集合
Eigen::MatrixXd state_cov; // 状态协方差矩阵
Eigen::Matrix<double, 12, 12> continuous_noise_cov; // 连续时间噪声
};4. 坐标系定义
4.1 坐标系说明
| 坐标系 | 符号 | 说明 |
|---|---|---|
| 世界坐标系 | W W W | 重力对齐的惯性系 |
| IMU坐标系 | I I I | IMU传感器坐标系 |
| 相机0坐标系 | C 0 C_0 C0 | 左相机坐标系 |
| 相机1坐标系 | C 1 C_1 C1 | 右相机坐标系 |
| 机体坐标系 | B B B | 机器人机体坐标系 |
4.2 坐标变换关系
世界坐标系 (W)
↓ T_WI (R_WI, t_I^W)
IMU坐标系 (I)
↓ T_IC (R_IC, t_C^I)
相机0坐标系 (C0)
↓ T_C0C1 (R_C0C1, t_C1^C0)
相机1坐标系 (C1)代码中的变换：
cpp
// IMU到相机的变换
state_server.imu_state.R_imu_cam0 = T_cam0_imu.linear().transpose();
state_server.imu_state.t_cam0_imu = T_cam0_imu.translation();
// 相机间变换
CAMState::T_cam0_cam1 = utils::getTransformEigen(nh, "T_cn_cnm1");
// IMU到机体的变换
IMUState::T_imu_body = utils::getTransformEigen(nh, "T_imu_body").inverse();5. 算法流程
5.1 主循环流程
cpp
void MsckfVio::featureCallback(const CameraMeasurementConstPtr& msg) {
// 1. IMU状态传播
batchImuProcessing(msg->header.stamp.toSec());
// 2. 状态增广
stateAugmentation(msg->header.stamp.toSec());
// 3. 添加特征观测
addFeatureObservations(msg);
// 4. 处理丢失的特征
removeLostFeatures();
// 5. 剪枝相机状态
pruneCamStateBuffer();
// 6. 发布结果
publish(msg->header.stamp);
// 7. 在线重置检查
onlineReset();
}5.2 详细流程图
┌──────────────────┐
│ 接收图像数据 │
└────────┬─────────┘
│
▼
┌──────────────────┐
│ IMU数据批量处理 │
│ (状态传播) │
└────────┬─────────┘
│
▼
┌──────────────────┐
│ 状态增广 │
│ (添加新相机帧) │
└────────┬─────────┘
│
▼
┌──────────────────┐
│ 特征观测更新 │
│ (跟踪/新特征) │
└────────┬─────────┘
│
▼
┌──────────────┴──────────────┐
│ │
▼ ▼
┌──────────────────┐ ┌──────────────────┐
│ 丢失特征处理 │ │ 继续跟踪 │
│ - 运动检测 │ │ (等待下一帧) │
│ - 位置初始化 │ └──────────────────┘
│ - 雅可比计算 │
│ - 零空间投影 │
│ - 测量更新 │
└────────┬─────────┘
│
▼
┌──────────────────┐
│ 相机状态剪枝 │
└────────┬─────────┘
│
▼
┌──────────────────┐
│ 发布结果 │
└────────┬─────────┘
│
▼
┌──────────────────┐
│ 在线重置检查 │
└──────────────────┘6. 关键数据结构
6.1 MapServer（特征地图）
cpp
// 存储所有正在跟踪的特征点
typedef std::map<FeatureIDType, Feature, ...> MapServer;
// 特征结构体
struct Feature {
FeatureIDType id; // 特征ID
Eigen::Vector3d position; // 3D位置（世界坐标系）
bool is_initialized; // 是否已初始化
std::map<StateIDType, Eigen::Vector4d> observations; // 观测数据
// 观测格式: [u0, v0, u1, v1]
// u0, v0: 左相机归一化坐标
// u1, v1: 右相机归一化坐标
};6.2 IMU数据缓冲
cpp
// 处理IMU和图像的时间不同步
std::vector<sensor_msgs::Imu> imu_msg_buffer;7. 参数配置
7.1 噪声参数
| 参数 | 默认值 | 说明 |
|---|---|---|
noise/gyro |
0.001 | 陀螺仪噪声标准差 (rad/s) |
noise/acc |
0.01 | 加速度计噪声标准差 (m/s²) |
noise/gyro_bias |
0.001 | 陀螺仪偏置随机游走 |
noise/acc_bias |
0.01 | 加速度计偏置随机游走 |
noise/feature |
0.01 | 特征观测噪声 (pixel) |
7.2 系统参数
| 参数 | 默认值 | 说明 |
|---|---|---|
max_cam_state_size |
30 | 最大相机状态数量 |
frame_rate |
40.0 | 图像帧率 (Hz) |
position_std_threshold |
8.0 | 位置不确定性阈值 (m) |
7.3 关键帧选择参数
| 参数 | 默认值 | 说明 |
|---|---|---|
rotation_threshold |
0.2618 | 旋转阈值 (rad) |
translation_threshold |
0.4 | 平移阈值 (m) |
tracking_rate_threshold |
0.5 | 跟踪率阈值 |
8. 性能特点
8.1 计算复杂度
| 操作 | 复杂度 | 说明 |
|---|---|---|
| 状态传播 | O ( 1 ) O(1) O(1) | 固定维度矩阵运算 |
| 状态增广 | O ( N ) O(N) O(N) | N N N为相机状态数 |
| 测量更新 | O ( m n 2 ) O(mn^2) O(mn2) | m m m为测量数， n n n为状态维度 |
| 特征处理 | O ( m ) O(m) O(m) | 线性复杂度 |
8.2 存储需求
- 状态向量 ： 21 + 6 N 21 + 6N 21+6N 个浮点数
- 协方差矩阵 ： ( 21 + 6 N ) 2 (21 + 6N)^2 (21+6N)2 个浮点数
- 特征存储 ：每个特征约 4 K + 12 4K + 12 4K+12 字节（ K K K为观测数）
8.3 实时性能
在现代硬件上，MSCKF VIO通常可以达到：
- 20-100 Hz的IMU处理频率
- 10-30 Hz的图像处理频率
- 总延迟 < 50ms
9. 与其他方法对比
| 方法 | 状态维度 | 计算复杂度 | 精度 | 实时性 |
|---|---|---|---|---|
| EKF-SLAM | 21 + 3 n 21 + 3n 21+3n | O ( n 2 ) O(n^2) O(n2) | 高 | 差 |
| MSCKF | 21 + 6 N 21 + 6N 21+6N | O ( N ) O(N) O(N) | 高 | 好 |
| 视觉里程计 | 6-12 | O ( 1 ) O(1) O(1) | 中 | 好 |
| 优化方法 | 21 + 6 N 21 + 6N 21+6N | O ( N 2 ) O(N^2) O(N2) | 最高 | 中 |
10. 总结
MSCKF VIO通过以下设计实现了高效精确的视觉-惯性里程计：
- 多状态约束：利用特征在多帧中的观测
- 状态管理：限制相机状态数量，及时剪枝
- 零空间投影：消除特征位置不确定性
- 可观测性约束：保证估计一致性
下一篇预告
下一篇文章将详细介绍IMU初始化过程，包括重力向量估计和陀螺仪偏置校准。
参考文献
-
Mourikis, A. I., & Roumeliotis, S. I. (2007). A multi-state constraint Kalman filter for vision-aided inertial navigation. In Proceedings 2007 IEEE International Conference on Robotics and Automation.
-
Sun, K., Mohta, K., Pfrommer, B., et al. (2018). Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight. IEEE Robotics and Automation Letters.
本文为原创文章，转载请注明出处。欢迎点赞、收藏、评论交流！