SALM文献之-VINS-Fusion: An optimization-based multi-sensor state estimator

---

VINS-Fusion算法原理详解

VINS-Fusion 是香港科技大学团队开发的基于优化多传感器融合框架，支持视觉（单目/双目）、IMU、GPS、激光雷达等多种传感器的实时状态估计与建图。其核心是在 VINS-Mono 基础上扩展多传感器紧耦合，通过滑动窗口非线性优化实现高精度、低漂移的定位。以下是其核心原理及公式推导的详细解析：

---

1. 系统架构与流程

VINS-Fusion 的流程分为 前端处理、滑动窗口优化 和 全局融合 三部分：

1. 前端处理：

   • 图像特征跟踪：提取Harris角点或FAST特征，使用KLT光流跟踪相邻帧特征。
   • IMU预积分：在滑动窗口内对IMU数据进行预积分，得到帧间相对运动。
   • 外参标定：在线估计相机与IMU之间的时空参数（$(T_{bc})$，包括旋转、平移和时间偏移。

2. 滑动窗口优化：

   • 维护一个固定大小的滑动窗口（通常10~20帧），将窗口内的 视觉重投影误差、IMU预积分误差 和 其他传感器（如GPS）约束 联合优化。

3. 全局融合：

   • 利用GPS或回环检测的全局观测，对局部滑动窗口的结果进行位姿图优化，抑制累积误差。

---

2. 状态向量与参数化

滑动窗口内的状态向量包括 位姿、速度、IMU偏差 及 外参：
$$[\mathcal{X}=\left[{\mathbf{x}}_0,{\mathbf{x}}_1,\dots ,{\mathbf{x}}_n,{\mathbf{T}}_{bc},{\lambda }_0,{\lambda }_1,\dots ,{\lambda }_m\right]]$$
其中：

• $({\mathbf{x}}_k=[{\mathbf{p}}_k,{\mathbf{v}}_k,{\mathbf{q}}_k,{\mathbf{b}}_a^k,{\mathbf{b}}_g^k])$ 表示第 $(k)$ 帧的IMU状态（位置、速度、姿态、加速度计/陀螺仪偏差）。
• $({\mathbf{T}}_{bc})$ 为IMU到相机的变换矩阵。
• $({\lambda }_i)$ 为第 $(i)$ 个特征点的逆深度（单目情况下）。

---

3. IMU预积分与误差模型

IMU动力学模型

IMU的角速度 $({\mathbf{\omega }}_t)$ 和加速度 $({\mathbf{a}}_t)$ 的测量模型为：
$$[\begin{array}{rl}{\mathbf{\omega }}_t& ={\mathbf{\omega }}_t^{\text{true}}+{\mathbf{b}}_g+{\mathbf{n}}_g,\\ {\mathbf{a}}_t& ={\mathbf{a}}_t^{\text{true}}+{\mathbf{b}}_a+{\mathbf{n}}_a+{\mathbf{R}}_{wb}^T\mathbf{g},\end{array}]$$
其中 $({\mathbf{n}}_g,{\mathbf{n}}_a)$ 为高斯白噪声，$(\mathbf{g})$ 为重力向量。

预积分量计算

在时间区间 $([t_k,t_{k+1}])$ 内，IMU预积分得到相对旋转 $(\Delta \mathbf{R})$、速度变化 $(\Delta \mathbf{v})$ 和位置变化 $(\Delta \mathbf{p})$：
$$[\begin{array}{rl}\Delta \mathbf{R}& =\prod _{k=1}^N\exp \left(({\mathbf{\omega }}_k-{\mathbf{b}}_g)\Delta t\right),\\ \Delta \mathbf{v}& =\sum _{k=1}^N\Delta {\mathbf{R}}_k({\mathbf{a}}_k-{\mathbf{b}}_a)\Delta t,\\ \Delta \mathbf{p}& =\sum _{k=1}^N\left(\Delta {\mathbf{v}}_k\Delta t+\frac{1}{2}\Delta {\mathbf{R}}_k({\mathbf{a}}_k-{\mathbf{b}}_a)(\Delta t{)}^2\right).\end{array}]$$
预积分量仅与IMU偏差相关，与全局状态解耦，避免重复积分。

预积分误差模型

预积分的残差定义为预测值与实际状态变化的差异：
$$[\begin{array}{rl}{\mathbf{r}}_{\Delta R}& =\log \left(\Delta {\mathbf{R}}^T{\mathbf{R}}_k^T{\mathbf{R}}_{k+1}\right),\\ {\mathbf{r}}_{\Delta v}& ={\mathbf{R}}_k^T({\mathbf{v}}_{k+1}-{\mathbf{v}}_k-\mathbf{g}\Delta t)-\Delta \mathbf{v},\\ {\mathbf{r}}_{\Delta p}& ={\mathbf{R}}_k^T({\mathbf{p}}_{k+1}-{\mathbf{p}}_k-{\mathbf{v}}_k\Delta t-\frac{1}{2}\mathbf{g}\Delta t^2)-\Delta \mathbf{p}.\end{array}]$$

---

4. 视觉重投影误差

特征点参数化

对于单目相机，特征点使用逆深度参数化：$(\lambda =1/z)$，其观测模型为：
$$[{\mathbf{z}}_i^j=\pi \left({\mathbf{R}}_{bc}^T\left({\mathbf{R}}_{w{b}_j}^T\left({\mathbf{R}}_{w{b}_i}({\mathbf{p}}_{bc}+\frac{1}{{\lambda }_i}{\mathbf{f}}_i^c)+{\mathbf{p}}_{w{b}_i}-{\mathbf{p}}_{w{b}_j}\right)-{\mathbf{p}}_{bc}\right)\right)+{\mathbf{n}}_z,]$$
其中 $(\pi (\cdot ))$ 为相机投影函数，$({\mathbf{f}}_i^c)$ 为归一化相机坐标系下的特征点坐标。

重投影残差

定义第 $(i)$ 个特征点在第 $(j)$ 帧的重投影误差：
$$[{\mathbf{r}}_{\text{vis}}^j={\mathbf{z}}_i^j-{\hat{\mathbf{z}}}_i^j,]$$
其中 $({\hat{\mathbf{z}}}_i^j)$ 为预测的像素坐标。

---

5. 多传感器融合

GPS观测模型

GPS提供全局位置测量 $({\mathbf{p}}_{\text{GPS}})$，其残差为：
$$[{\mathbf{r}}_{\text{GPS}}={\mathbf{p}}_{\text{world}}-{\mathbf{p}}_{\text{GPS}},]$$
其中 $({\mathbf{p}}_{\text{world}})$ 为当前估计的全局位置。

激光雷达融合（可选）

若加入激光雷达，通过ICP或NDT匹配点云，构建点到面或点到线的残差，类似LOAM方法。

---

6. 滑动窗口非线性优化

目标函数联合最小化所有残差的马氏距离：
$$[\underset{\mathcal{X}}{\min }\left(\sum \Vert {\mathbf{r}}_{\text{vis}}{\Vert }_{{\Sigma }_{\text{vis}}}^2+\sum \Vert {\mathbf{r}}_{\text{IMU}}{\Vert }_{{\Sigma }_{\text{IMU}}}^2+\sum \Vert {\mathbf{r}}_{\text{GPS}}{\Vert }_{{\Sigma }_{\text{GPS}}}^2\right),]$$
其中 $(\Sigma )$ 为各传感器的协方差矩阵。使用 Ceres Solver 或 g2o 进行Levenberg-Marquardt优化。

---

7. 全局优化与回环检测

回环检测

采用DBoW2或NetVLAD进行视觉词袋匹配，检测到回环后，添加位姿约束：
$$[{\mathbf{r}}_{\text{loop}}=\log \left({\mathbf{T}}_{ij}^{-1}{\mathbf{T}}_i{\mathbf{T}}_j^{-1}\right),]$$
其中 $({\mathbf{T}}_{ij})$ 为通过PnP计算的相对位姿。

位姿图优化

将滑动窗口内的局部轨迹与GPS/回环的全局约束结合，构建位姿图并优化：
$$[\underset{{\mathbf{T}}_1,\dots ,{\mathbf{T}}_n}{\min }\left(\sum \Vert {\mathbf{r}}_{\text{odom}}{\Vert }^2+\sum \Vert {\mathbf{r}}_{\text{global}}{\Vert }^2\right).]$$

---

8. 关键技术与创新点

1. 紧耦合多传感器融合：IMU、视觉、GPS在优化层直接融合，而非松耦合的滤波架构。
2. 自适应滑动窗口：动态管理窗口大小，平衡计算量与精度。
3. 鲁棒的初始化：通过SfM和IMU对齐估计初始尺度、重力方向及IMU偏差。
4. 时间偏移标定：在线估计传感器间的时间延迟，提升异步数据融合精度。

---

9. 数学工具与理论

• 李群与李代数：姿态 $(\mathbf{q})$ 在 $(\text{SO(3)})$ 流形上优化，使用 $(\mathfrak{so}(3))$ 扰动模型计算雅可比。
• 协方差传播：IMU预积分的噪声通过一阶泰勒展开传递。
• 边缘化（Marginalization）：将滑出窗口的状态边缘化为先验信息，保留其约束。

---

10. 性能与应用场景

• 精度：室内环境下轨迹误差小于1%，室外结合GPS可达厘米级。
• 实时性：在i7 CPU上单线程运行频率超过20Hz。
• 适用场景：无人机、自动驾驶、AR/VR等需要鲁棒定位的场景。

---

总结

VINS-Fusion通过紧耦合多传感器优化，有效结合了视觉的高精度相对测量与IMU/GPS的绝对稳定性，其核心在于滑动窗口内的联合非线性优化与全局约束的引入。算法涵盖IMU预积分、视觉重投影误差、传感器融合及位姿图优化，是当前开源社区中性能领先的多模态SLAM解决方案。