2. 视觉惯性导航基础

视觉惯性导航（Visual-Inertial Navigation）是移动机器人、无人机、AR/VR 设备实现自主定位与姿态估计的核心技术路线之一。它把相机提供的环境视觉信息与惯性测量单元（IMU）提供的高频运动信息融合起来，在未知环境中估计载体的位置、速度、姿态，并同步构建环境地图。理解 VIO 的定义、传感器组成、坐标系约定、时间同步机制以及典型系统，是后续学习预积分与状态估计的必备基础。

一、VIO 的定义与核心思想

VIO 全称 Visual-Inertial Odometry，即视觉惯性里程计。它通过相机观测环境中的特征点或直接利用像素亮度，结合 IMU 测量的角速度与加速度，递推估计载体的运动状态。其核心思想可以概括为“互补融合”：

• 视觉：提供长期稳定的环境约束，能抑制 IMU 的漂移，但输出频率低（通常 10～60 Hz），且在纹理缺失、快速运动、光照突变时容易失效。
• IMU：提供高频（通常 100～1000 Hz）的角速度和加速度，短时精度高、动态响应快，但存在零偏和噪声，积分后误差随时间累积。
• 融合：用 IMU 做高频递推和运动先验，用视觉做低频观测校正，从而在精度、鲁棒性和频率之间取得平衡。

二、传感器组成与数据特性

典型 VIO 系统由以下传感器构成：

传感器	典型输出	频率	主要作用	主要误差
单目/双目/鱼眼相机	图像、特征点、光流	10～60 Hz	环境观测、尺度恢复（双目）、回环检测	纹理缺失、运动模糊、光照变化
MEMS IMU	角速度、加速度	100～1000 Hz	高频递推、运动先验、重力方向估计	零偏、随机游走、温漂
轮速计/气压计（可选）	速度、高度	50～200 Hz	辅助尺度、抑制高度漂移	打滑、气压波动

其中，相机与 IMU 之间的外参（相对位姿）和 IMU 内参（零偏、噪声密度）是 VIO 标定的关键参数。工程中通常先离线标定相机内参、相机-IMU 外参和时间偏移，再在线估计 IMU 零偏。

三、坐标系与变换关系

VIO 涉及多个坐标系，必须明确其定义与变换方向：

• 世界坐标系 W：惯性系或局部重力对齐系，用于表达全局位姿。
• 机体坐标系 B：与 IMU 固连，通常以 IMU 中心为原点。
• 相机坐标系 C：以相机光心为原点，Z 轴指向光轴前方。
• IMU 坐标系 I：与机体坐标系一致或存在固定外参。

核心变换包括：世界到机体的位姿 \(T_{WB}\)、相机到 IMU 的外参 \(T_{CI}\)、以及像素坐标与相机坐标的投影关系。VIO 的状态量通常包含位置、速度、姿态、IMU 零偏，有时还包括相机-IMU 时间偏移。

四、时间同步：VIO 精度的隐形门槛

相机与 IMU 的采样时刻往往不同步，若忽略时间偏移，融合结果会出现明显漂移甚至发散。时间同步通常分三步：

1. 硬件同步：使用触发信号让相机曝光与 IMU 采样在同一时钟源下对齐，精度可达微秒级。
2. 时间戳对齐：为每帧图像和每个 IMU 数据打上统一时钟的时间戳，常用 ROS 的 sensor_msgs/Image 与 sensor_msgs/Imu。
3. 在线估计时间偏移：将相机-IMU 时间差 \(t_d\) 作为状态量，在优化中联合估计。

# 典型 ROS 时间同步检查
rosrun topic_tools relay /camera/image_raw /cam/image
rosbag info vio_dataset.bag
# 查看图像与 IMU 时间戳偏差
python3 check_time_offset.py --bag vio_dataset.bag --cam /cam/image --imu /imu/data

五、应用场景与典型系统

VIO 已广泛应用于以下场景：

• 无人机：室内外自主飞行、避障、定点悬停。
• 移动机器人：仓储 AGV、配送机器人、扫地机的定位与建图。
• AR/VR：头显与手机的六自由度跟踪、手势交互。
• 自动驾驶：隧道、城市峡谷等 GNSS 失效场景的航位推算。

典型开源系统包括：

系统	融合方式	特点
VINS-Mono / VINS-Fusion	滑动窗口优化	单目/双目/IMU，支持回环，工程成熟
OKVIS	关键帧优化	双目+IMU，精度高，适合研究
MSCKF	滤波	计算量低，适合嵌入式实时运行
ORB-SLAM3	优化+回环	支持视觉、视觉惯性、多地图

六、工程复盘要点

在实际部署 VIO 时，建议按以下清单逐项检查：

• 相机与 IMU 外参是否标定准确，重投影误差是否在 0.5 像素以内。
• IMU 零偏是否在线估计，静止时角速度输出是否接近零。
• 时间戳是否统一时钟源，图像与 IMU 时间偏移是否小于 1 ms。
• 图像曝光时间是否与 IMU 采样匹配，避免运动模糊。
• 特征点数量与分布是否充足，纹理缺失时是否有降级策略。
• 回环检测与全局优化是否开启，长期漂移是否可接受。