VINS-Fusion学习笔记

前言

VINS-Fusion是一套定位数据融合算法，基于优化的方法对多传感器得到的状态量进行融合估计得到准确的位置信息。

传感器分类

所谓多传感器主要分为局部估计传感器以及全局估计传感器

局部估计

只能获取无人机瞬时状态变化，通过对瞬时变化的积分求解整体变化情况，存在误差累积以及积分漂移等问题，如IMU、视觉里程计等均属于局部估计。

全局估计

任何时刻，可以获得无人机的绝对状态信息，如磁力计、气压计、GPS等，该类状态估计方式不存在上述问题，但会存在精度误差、噪声以及更新频率等问题。

局部估计

该算法局部估计使用VIO算法，算法中使用滑动窗口对关键帧进行保存，并实时对窗口进行更新。更新后，结合窗口中每一帧位姿、速度以及bias并添加边缘化约束、IMU预积分约束、视觉重投影约束以及回环检测约束得到优化函数，然后调用ceres-solver进行优化。

• 预积分：通过中值积分分离出优化变量，降低后续计算量，即对帧间的IMU数据进行积分，获取先验值
• 边缘化：根据次新帧是否为关键帧将次新帧或最旧帧进行边缘化，即推出滑动窗口，退出时需要将该帧的IMU数据保留，以保证IMU积分的连贯性。
• 回环检测与ORB_SLAM算法中的原理类似
  

通过基于非线性优化的方法实现VIO的局部状态估计

全局估计

问题描述

结合VIO得到的局部位姿估计以及全局传感器得到的具有噪声的低频的全局状态信息进行状态量的全局准确估计。称该问题为位姿的图优化，可由下图表示。

这里假设所有估计的估计误差均满足高斯分布，则可以将该优化问题定义为极大似然问题’

其中z为传感器测量值，h为传感器的模型，h(X)则理解为重投影，故该式表示为求解X使得各传感器的重投影误差（也理解为残差）之和最小。这里的范数使用马氏范数

传感器因子（Sensor Factors）

分别定义局部因子、GPS因子、磁力计因子、气压计因子

局部因子

GPS因子

其余全局因子

同GPS因子定义类似

代码部分

对GPS因子部分


利用GPS传感器的残差值对数据进行优化
得到更加准确的GPS信号
对VIO仅有优化时，以两帧间GPS的增量作为标准值，对两针见VIO的增量进行优化。
如残差定义中所示

这里以GPS的增量减去两帧位姿变化产生的增量。代码中仅对残差进行了定义，具体优化过程中由ceres执行。
添加factor后进行ceres求解，更新GPS与VIO坐标系转换的参数，从而更新位姿。

总结

VINS-Fusion相比于传统的基于滤波的数据融合算法实现了传感器数据的解耦，对时间同步没有严格要求，定位精度较基于滤波的算法也更高。但该方法所需要的计算时间更长。