Description：VINS on Wheels 论文笔记 — 地面车辆 VINS 可观性退化分析（恒加速/无旋转多出不可观方向）、引入轮速里程计恢复尺度可观性、mVINS 利用平面运动软约束，Pioneer 3DX + Project Tango 真机验证
My Notion Note ID：K2E-B-W1-4
Created：2023-08-23
Updated：2026-06-11
License：转载欢迎：转载请注明作者 Yu Zhang 并附原文出处（yuzhang.io）

1. Summary

Title: VINS on Wheels Authors: Kejian J. Wu, Chao X. Guo, Georgios Georgiou, Stergios I. Roumeliotis (UMN MARS Lab) Paper: ICRA 2017（DOI 10.1109/ICRA.2017.7989603） Github: 无官方代码

本论文研究地面车辆上 VINS 的可观性问题。核心发现：VINS 在车辆受限运动（直线匀加速 / 圆弧匀速）下除标准 4 个不可观 DOF（3 平移 + yaw）外还会多出额外不可观方向，导致尺度估计失准。

论文提出两类修正：(1) 加入轮速里程计，使尺度方向恢复可观；(2) mVINS，把车辆近似平面运动表述为随机软约束纳入估计。Pioneer 3DX 机器人 + Project Tango 平板（MEMS IMU + 相机）真机实验验证了理论预测和精度提升。

圆弧运动实验：VINS（红色虚线）因尺度不可观偏离真实圆轨迹（蓝色实线），估计路径持续漂移扩散

2. Key Contributions

可观性分析：解析推导 VINS 在恒加速度 / 无旋转两种受限运动下的额外不可观方向（零空间），给出严格定理及证明
VINS + 里程计：融合低频轮速编码数据，证明尺度方向恢复可观（定理 3），并以鲁棒方式（2D 位移融合）处理噪声轮速
mVINS：把近似平面运动约束 $\mathbf{g}(\mathbf{x})=0$ 表述为随机（软）约束加入代价函数，比确定性约束更鲁棒（对路面颠簸有容错）
方法通用，与具体滤波 / 优化估计器无关

3. Method

3.1 VINS 状态与观测

状态向量（含 $N$ 个地标）：

\mathbf{x} = \bigl[{}^I_G\mathbf{q}^T,\;\mathbf{b}_g^T,\;{}^G\mathbf{v}_I^T,\;\mathbf{b}_a^T,\;{}^G\mathbf{p}_I^T \mid {}^C\mathbf{f}_1^T,\;\dots,\;{}^C\mathbf{f}_N^T\bigr]^T \tag{1}

${}^I_G\mathbf{q}$ ：global→IMU 四元数； $\mathbf{b}_g, \mathbf{b}_a$ ：bias； ${}^G\mathbf{v}_I, {}^G\mathbf{p}_I$ ：速度、位置； ${}^C\mathbf{f}_j$ ：地标。

IMU 测量（Eq 2）及相机观测（Eq 4）遵循标准 VINS 形式（见 VIO 预积分 + MSCKF 笔记）。

3.2 可观性退化：定理 1 — 恒加速度

恒定局部加速度条件：

{}^I\mathbf{a}(t) \triangleq \mathbf{C}({}^I_Gq(t))\,{}^G\mathbf{a}(t) \equiv {}^I\mathbf{a}, \quad \forall t \geq t_0 \tag{2}

定理 1：线性化 VINS 在条件 (2) 下，除 4 个标准不可观方向外，多出尺度方向（对应零空间列向量 $\mathbf{N}_s$ ）：

\mathbf{N}_s = \bigl[\mathbf{0}_{1\times3},\;\mathbf{0}_{1\times3},\;{}^G\mathbf{v}_{t_0}^T,\;-{}^I\mathbf{a}^T,\;{}^G\mathbf{p}_{t_0}^T,\;{}^C\mathbf{f}_1^T,\;\dots,\;{}^C\mathbf{f}_N^T\bigr]^T \tag{3}

物理意义：恒加速度时无法区分真实 body 加速度的大小与加速度计 bias 的幅值 → 尺度模糊。

3.3 可观性退化：定理 2 — 无旋转

无旋转条件： ${}^{I_t}_G\mathbf{C} \equiv {}^{I_{t_0}}_G\mathbf{C}$ ， $\forall t \geq t_0$ 。

定理 2：VINS 在无旋转下多出与全部 3 个 orientation DOF 相关的额外不可观方向（对应零空间 $\mathbf{N}_o$ ），不只是 yaw。

物理意义：无旋转时无法区分局部重力方向和加速度计 bias → roll/pitch 也退化（全部 orientation 不可观）。

3.4 VINS with Odometer

轮速编码器提供左右轮角速度 $w_l, w_r$ ，换算为线速度 $v$ 和角速度 $w$ ：

v = \frac{r_l w_l + r_r w_r}{2}, \quad w = \frac{r_r w_r - r_l w_l}{a} \tag{4}

$r_l, r_r$ ：左右轮半径； $a$ ：轴距（均离线批量最小二乘标定）。

定理 3：给定里程计测量 (4)，(3) 中的尺度方向变为可观。

实现：把原始轮速积分为相邻 odometer 帧间的 2D 位移。设里程计在帧 $k$ 到 $k+1$ 之间给出旋转增量 $\Delta\theta_O$ （偏航角，Eq 15 残差形式）和平移增量 $\Delta\mathbf{p}_O$ （地面平面 2D 位移，Eq 17 残差形式）。

Eq 15（旋转残差）： $r_{\Delta\theta} = \Delta\theta_O - {}^O_I\Lambda\bigl(\hat{\mathbf{q}}_{k+1}^{-1}\otimes\hat{\mathbf{q}}_k\bigr)$ （里程计偏航增量 vs IMU 积分偏航增量之差，投影到里程计系 yaw 分量）。
Eq 17（平移残差）： $r_{\Delta p} = \Delta\mathbf{p}_O - {}^O_I\Lambda\bigl({}^G\hat{\mathbf{p}}_{k+1} - {}^G\hat{\mathbf{p}}_k\bigr)$ （里程计 2D 平移增量 vs IMU 积分位移差投影到地面平面之差）。

两个残差通过滤波器更新对状态施加约束，恢复尺度可观性；odometer-IMU 外参离线标定，相机-IMU 外参在线估计。

3.5 mVINS — 流形约束

平面运动约束（ $\{G\}$ 系坐标轴与平面法向）参数化为 2-DOF 四元数 ${}^\pi\mathbf{q}_G$ + 标量 ${}^\pi z_G$ ，约束函数：

\mathbf{g}(\mathbf{x}) = \begin{bmatrix}\Lambda^O_I\mathbf{C}^{I_k}_G\hat{\mathbf{C}}^{T}\mathbf{e}_3 \\ {}^\pi z_G + \mathbf{e}_3^T\hat{\mathbf{C}}\bigl({}^O\mathbf{p}_{I_k} - {}^O_G\mathbf{C}^{TO}\mathbf{p}_I\bigr)\end{bmatrix} = \mathbf{0} \tag{5}

符号： $\{O\}$ ：里程计坐标系（地面平面系，z 轴朝上）； $\Lambda^O_I$ ：从 IMU 坐标系 $\{I\}$ 到里程计系 $\{O\}$ 的旋转矩阵（离线标定外参的一部分），取前 2 行，将 3D 向量投影到地面平面； $\mathbf{e}_3 = [0,0,1]^T$ ； $\hat{\mathbf{C}}$ 为当前重力方向对应的旋转矩阵估计； ${}^O\mathbf{p}_{I_k}$ 为 IMU 在里程计坐标系 $\{O\}$ 中的位置（由里程计 2D 位移积分）； ${}^O_G\mathbf{C}$ 为从全局坐标系 $\{G\}$ 到里程计系 $\{O\}$ 的旋转矩阵（离线标定）。第 1 块（ $2\times1$ ）= roll / pitch 为零约束；第 2 块（标量）= z 轴位移为零约束。

随机（软）约束版本（VOS）：把 $\mathbf{g}(\mathbf{x})$ 建模为零均值高斯过程，加入代价函数额外项 $\|\mathbf{g}(\mathbf{x})\|^2_\mathbf{R}$ ，用 Mahalanobis 距离检测并临时去掉不满足约束的时刻（如过减速带）。比确定性约束（VOD）更鲁棒。

4. Experiments & Results

平台：Pioneer 3 DX 机器人（轮速 10 Hz）+ Project Tango 平板（MEMS IMU 100 Hz，灰度相机 640×480 @ 30 Hz）；NVIDIA Tegra K1 CPU（2.3 GHz 四核）+ 4 GB RAM。

前端：200 FAST 角点 + KLT 光流 + 2-pt RANSAC。

后端：SR-ISWF（Square-Root Inverse Sliding Window Filter，平方根逆形式滑动窗口滤波器，用平方根信息矩阵代替协方差矩阵以提升数值稳定性），滑窗 15 poses，~7 Hz。

运动影响验证（定理 1/2 实验证明）

尺度比 $SR = d_\text{est}/d_\text{gt} - 1$ （越接近 0 越好）：

运动类型	SR 均值	SR 标准差	位置 RMSE
Pioneer 圆弧运动（恒加速，符合定理 1）	0.16	0.08	81 cm
手持通用运动（无受限运动）	0.003	0.03	14 cm

圆弧运动 SR 持续漂移，手持运动 SR 在零附近振荡 → 验证定理 1。

尺度比随时间变化：圆弧（蓝色，均值 0.16）持续增大；手持（红虚线，均值 0.003）稳定近零

系统性能（5 个数据集，各约 1 km 室内建筑）

四种配置比较（Table I，RMSE in meters for x-y / z / total，+ % of distance）：

VINS（仅 VINS，无里程计无平面约束）：最弱基线，大漂移
VO（VINS + 里程计，无平面约束）：x-y 改善明显，尺度恢复可观
VOD（VINS + 里程计 + 确定性平面约束）：z 轴有约束；但确定性约束在路面颠簸时反而有负面影响
VOS（VINS + 里程计 + 随机平面约束）：全方位最优，x-y 和 z 均显著改善

效率：整个 VINS pipeline 68 ms / 关键帧（其中 SR-ISWF 36 ms）；里程计 + 平面约束各 < 1 ms；总 CPU ~50%（7 Hz 运行）。

5. Ablation & Discussion

定性 vs 随机约束（VOD vs VOS）：确定性约束（ $\mathbf{g}(\mathbf{x})=0$ 硬约束）在完全平坦路面最优，但路面颠簸时强制约束引入错误信息 → z 轴反而变差。VOS 软约束通过 Mahalanobis 检测自动跳过不满足时刻，更鲁棒
里程计内参标定：论文做离线批量最小二乘（融合视觉 + 惯性 + 里程计），运行时当已知量固定；相机-IMU 外参在线估计。标定精度对里程计融合效果影响显著
运动依赖性：特殊运动（直线/圆弧）在实际中总是"近似"满足——只要车辆稍微加速变化就会周期性地信息恢复，但在"近似满足"期间 Hessian 矩阵严重病态，仍会显著降级精度

6. Strengths / Limitations / Future Work

优点：

理论严格：给出可观性退化的解析证明和精确零空间表达式
方法通用（滤波 / 优化估计器均适用）
随机平面约束对路面不平坦有显式容错机制

局限：

里程计需离线标定（轮半径 + 轴距），换轮胎或磨损后需重新标定
mVINS 平面假设不适用于阶梯 / 多楼层场景
无回环（纯里程计）；轮子打滑时里程计测量退化（论文 future work 提出补偿方案）

后续工作（论文展望）：

扩展 mVINS 到多平面间移动（上楼梯）
补偿轮胎打滑（wheel slippage compensation）

References

类别	链接
论文	Wu K.J., Guo C.X., Georgiou G., Roumeliotis S.I. (2017). VINS on Wheels. ICRA. PDF
可观性基础	Hesch J.A. et al. (2014). Camera-IMU-Based Localization: Observability Analysis and Consistency Improvement. IJRR.
相关	可观性 / FEJ 见 VIO 滑窗优化；轮速融合见 K2E-B-W Wheel Odometry 系列

Table of Contents