算法改进1：双目扩展与外参在线标定

感谢： 紫薯萝卜 https://zhuanlan.zhihu.com/p/77975605

1. 改进思路

接下来的章节中会介绍MSCKF的各种改进方法，目的都是为了提高VIO的精度、稳定性、鲁棒性，其中大部分是为了提高精度。VIO的核心是视觉和IMU的融合，所以不论是滤波方法（MSCKF）还是优化方法（VINS），改进的思路都可以从IMU、视觉和融合这三方面着手：

• 提升IMU
  • 更高精度的IMU
  • 更准的IMU标定模型
  • 更高的采样频率（200HZ, 500HZ, 1000HZ）
  • 更高精度的IMU积分方法：Euler, Midpoint, RK4
• 提升视觉
  • 提升图像质量：高动态范围相机，ISP，前端直方图均衡化
  • 增加图像观测：加大FOV，扩展双目或多目
  • 使用性能更好的特征：SURF, SIFT
  • 全局快门

 

• 提升融合算法

• 数据同步：传感器数据同步对VIO精度影响非常大，对数据做硬件同步、软件同步，还可以将同步误差 加到算法中实时估计，进一步降低同步误差的影响。
• 标定参数估计：包括相机内参标定、IMU与相机之间外参标定、双目外参标定，标定参数不准也会影响算法精度，可以将标定参数加入到算法中，以标定结果作为初始值，在VIO过程中实时估计内外参。
• 视觉观测模型扩展：视觉观测约束越多，状态估计精度越高。一方面可以扩展双目或多目，观测模型中加入更多的残差项；另一方面可以引入直线等特征，在环境纹理不足时提高精度。
• 能观性改进：理论上VIO位姿有4个维度是不能观的，包括3DoF全局位置和1DoF绕重力轴的旋转（Yaw角），而MSCKF中Yaw角被错误的能观了，这就产生了Inconsistency，使得算法精度变差。通过合理调整EKF系统的线性化点（FEJ）来改变系统的能观性秩，从而提升算法精度。

 

2. 双目扩展

MSCKF从单目扩展到双目只需要做非常少的改动：

1. 前端只需要对左目的特征点跟踪其右目观测，将跟踪特征观测的维度从2维 扩展到4维
2. 后端框架上只把左目相机加入到状态向量中，根据双目外参可以算出相机状态对应的右目相机位姿，然后特征残差模型从单目的2维残差变成双目的4维残差。

双目的残差定义如下：

其中，为特征 在相机 的双目观测。

单特征对单相机残差模型：

残差模型共4行，上面两行是左目残差对左目相机状态的残差模型，Jacobian与单目一致（见之前Measurement Update章节），下面两行是右目残差对左目相机的残差模型，Jacobian如下：

式中各导数项在之前Measurement Update章节都已推导，右目残差的Jacobian比左目残差多一项

 

3. 外参在线标定

在原始MSCKF中，相机外参被当做已知量，需要事先标定。如果标定参数不准，势必会导致算法精度下降。因此，可以将标定参数加入到状态向量中进行估计，以标定结果作为初值，在VIO运动过程中实时修正外参状态。

3.1 相机与IMU之间的外参估计

相机与IMU之间的外参为 ，将其加入到状态向量后，状态向量变为：

相机与IMU之间的外参只在State Augmentation时被用到，所以加入到状态估计后，唯一的改动是State Augmentation的Jacobian中要加上新增相机状态对外参的Jacobian，如下：

3.2 双目外参估计

左右目之间的外参为 ，同样扩展到状态向量中，唯一的改动是在残差模型的 中需要加入残差对外参状态的Jacobian，而且只有右目残差有，左目残差与外参无关。

右目残差对外参的Jacobian如下：

推导过程：

 

3.3 测试效果

为了凸显外参估计的效果，对外参的平移量加了0.02的误差，测试估计值能否收敛到真实值附近。测试发现双目外参 X轴（基线方向）能比较好的收敛， Z轴（深度方向）次之， Y轴收敛性最差，相机与IMU之间的外参收敛性不好，一开始以为是初始化协方差给太小了，调大初始化协方差后，估计量波动幅度会变大，但仍未收敛到真值。

                                                                       相机与IMU外参在线标定结果

 

                                                                                                    双目外参在线标定结果

在EuRoC数据集上进行对比测试，测试使用固定外参，在标定外参平移量 x 轴手动加0.02,0.05的误差，以及加入外参估计等情况下的RMSE，结果如下：

测试结论：

• 随着外参误差不断变大，VIO误差也不断变大，加入外参估计以后，会减少VIO误差，但仍比使用真值要差。
• VIO对相机与IMU之间偏移量的敏感度较低，就算加入很大误差，VIO精度也没有明显变差；而VIO对双目外参（基线）的敏感度很高，基线变大，VIO轨迹明显尺度变大，加入外参估计后，效果有明显改善。

个人观点：VIO运动过程中，有些维度约束比较强，所以能够比较准确的估计，而有些维度约束较弱，难以估计，而相机与IMU之间的偏移量就属于约束较弱的项。

 

3.4 状态向量扩展

扩展状态向量其实是一种通用思路，对于不准的参数量都可以加入到状态向量中估计，只需要在MSCKF中加入该状态的模型，一般来说，要么修改State Augmentation中的Jacobian，要么修改观测残差模型中的Jacobian。这里只介绍了两种外参的估计，其实相机内参、后面将介绍的同步误差 也都可加入到状态向量中。

那是不是估的状态越多越好呢？答案是否定的。首先，状态越多计算量越大，算法也越复杂；再者，状态向量维度越高，系统自由度越大，状态估计精度也越差。一般如果某些量足够准确，那就没必要对其进行估计。内外参估计，一方面是因为标定本身存在误差，另一方面是因为传感器长时间使用外参也会发生改变，为了避免频繁标定，才将其加入到状态估计中，而且一般会将内外参状态的初始协方差设得比较小，让其缓慢地修正，避免内外参波动太大造成VIO精度下降。

 

参考文献

1. Sun K, Mohta K, Pfrommer B, et al. Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight[J]. IEEE Robotics & Automation Letters, 2018, 3(2):965-972.
2. Li M , Mourikis A I . High-precision, consistent EKF-based visual-inertial odometry[J]. The International Journal of Robotics Research, 2013, 32(6):690-711.