基于旋转平移解耦框架的视觉惯性初始化方法

来源：3D视觉工坊

1. 导读

精确和鲁棒的初始化对于视觉惯性里程计(VIO)至关重要，因为不良的初始化会严重降低姿态精度。在初始化期间，估计诸如加速度计偏差、陀螺仪偏差、初始速度和重力等参数是至关重要的。IMU传感器需要精确估计陀螺仪偏差，因为陀螺仪偏差会影响旋转、速度和位置。现有的VIO初始化方法大多采用运动结构法来解决陀螺漂移问题。然而，SfM在快速运动或退化场景中不够稳定和有效。为了克服这些限制，我们通过添加新的不确定性参数和优化模块来扩展旋转-平移-解耦框架。首先，我们采用一种陀螺仪偏置优化器，它结合了概率法向极线约束。其次，我们融合IMU和视觉测量来有效地求解速度、重力和比例。最后，我们设计了一个额外的细化模块，有效地减少重力和比例误差。在EuRoC数据集上的大量初始化测试表明，我们的方法将陀螺仪偏差和旋转估计误差分别平均降低了16%和4%。它还显著降低了重力误差，平均降低了29%。

2. 引言

视觉惯性里程计（VIO）旨在在各种未知环境中估算相机的位置。它通过融合来自相机的图像信息和来自惯性测量单元（IMU）传感器的IMU测量数据来实现这一目标。相机可以估计视觉地图并减少姿态漂移。IMU传感器为相机运动提供度量尺度，并具备短期鲁棒性。VIO具有诸多优势，如体积小、成本低、功耗低。这些特性使得VIO在虚拟现实、增强现实和自动化机器人等领域的应用日益广泛。

为了有效运行VIO系统，必须在初始化阶段准确估计一组参数。这些参数包括尺度、重力方向、初始速度和加速度计及陀螺仪的偏差。初始化不正确将导致收敛性差和系统其他参数估计不准确。另一方面，快速初始化也很重要，因为VIO系统必须在IMU正确初始化后才能工作。

基本上，以往的VIO初始化工作可分为紧耦合和松耦合方法。紧耦合方法假设相机的姿态可以通过IMU测量进行近似。它们将视觉观测与IMU积分相融合，并通过封闭形式的解来估计初始化参数，这可能会增加计算成本。此外，这些方法通常忽略陀螺仪偏差，这可能会影响准确性。松耦合方法假设通过视觉结构从运动恢复（SfM）获得的相机轨迹非常准确。首先，它们解决视觉SfM问题，并根据得出的相机姿态初始化惯性参数。因此，这些方法的准确性在很大程度上依赖于视觉SfM的性能。然而，在相机快速运动或共同特征点太少的情况下，视觉SfM可能会变得不稳定。

总体而言，紧耦合和松耦合方法都未能充分利用相机和IMU之间的互补信息。具体而言，紧耦合方法未利用视觉观测来估计陀螺仪偏差，这可能会导致数值稳定性问题和准确性降低。松耦合方法未使用IMU测量来增强视觉SfM的稳定性，从而在具有挑战性的运动场景中导致准确性低或初始化失败。受图像观测可直接用于优化图像帧之间旋转的启发[12]，提出了一种旋转-平移解耦的VIO初始化方法。该方法增强了视觉观测与IMU测量之间的联系。然而，这种方法忽略了图像特征匹配的质量。在最终结果中，每个匹配都被赋予相同的权重。尽管已从特征匹配中去除异常值，但2D特征对应点的误差分布随图像内容和具体匹配技术而变化。因此，考虑2D特征匹配的不确定性至关重要。

3. 效果展示

MH02数据集和MH03数据集的角速度和标度误差可视化。第一行图像是MH02，第二行图像是MH03。列(a)显示了基于角速度着色的相应数据集的轨迹。列(b)和(c)分别显示了我们的方法和DRT-t方法基于相应数据集上着色的尺度误差的轨迹。刻度误差在0和1之间。颜色越浅，误差越小。

4. 主要贡献

为了克服SfM的局限性并提高初始化准确性和鲁棒性，我们在考虑概率法正态极线约束（PNEC）的情况下估计陀螺仪偏差。该方法通过考虑特征点的异性和非均匀不确定性来提高陀螺仪偏差估计的准确性。基于旋转-平移解耦框架，我们首先解决旋转问题。然后，我们使用线性全局平移约束（LiGT）解决平移问题。最后，我们通过求解最小二乘问题来估计速度、尺度和重力。为了进一步提高尺度和重力的准确性，我们还引入了一个改进的尺度和重力细化模块。推荐课程：彻底剖析激光-视觉-IMU-GPS融合SLAM算法：理论推导、代码讲解和实战。

5. 方法

陀螺仪偏差的准确估计在提高VIO系统轨迹准确性方面起着核心作用。偏差会影响旋转，进而影响平移和速度的积分。在本节中，我们提出了一种能够准确求解初始化参数的方法，这些参数包括陀螺仪偏差、速度、重力和尺度。初始化过程分为以下四个步骤：（1）陀螺仪偏差估计，（2）旋转和平移估计，（3）尺度、速度和重力估计，以及（4）尺度和重力细化。

正态极线约束（NEC）的几何特性以及陀螺仪偏差与NEC之间的关系。法向量n1和n2垂直于包含f1i（f2i）和f1j（f2j）（红色和绿色）的极平面，且所有法向量位于同一平面（黄色）内，形成一个可用于求解旋转Rcicj（橙色）的约束。通过将外部参数Rbc（蓝色）引入，求解Rcicj的问题转化为求解陀螺仪偏差（粉色）的问题。

6. 实验结果

7. 总结 & 未来工作

我们提出了一种基于旋转-平移解耦框架的鲁棒且精确的视觉-惯性初始化方法。

通过建立概率正态极线约束，我们推导出了一个新的陀螺仪偏置估计公式，该公式直接利用视觉观测结果并计算特征点位置的不确定性。然后，基于惯性测量单元（IMU）和视觉测量值建立最小二乘问题，以估计速度、尺度和重力。最后，我们引入了一个改进的尺度-重力优化模块。

大量的初始化实验表明，我们的方法在保持高计算效率的同时，提高了准确性和鲁棒性。该方法的一个局限性在于没有考虑加速度计偏置对速度和尺度的影响。未来，我们将研究一个涵盖加速度计偏置的尺度-重力优化模块。