作者：泡泡机器人
标题：Tightly Coupled Optimization-based GPS-Visual-Inertial Odometry with Online Calibration and Initialization
作者：Shihao Han, Feiyang Deng, Tao Li, and Hailong Pei
机构：
来源：arXiv 2022
编译：介来拉石
审核: 朱虎
摘要
大家好，今天为大家带来的文章是 Tightly Coupled Optimization-based GPS-Visual-Inertial Odometry with Online Calibration and Initialization
本文提出了一种基于紧耦合优化的GPS视觉惯性里程计系统，以解决视觉惯性里程计在长期运行中的轨迹漂移问题。视觉重投影残差、IMU残差和GPS测量残差在局部束平差中联合最小化，其中我们将GPS测量和IMU预积分应用于IMU残差，以形成新的GPS残差。为了提高系统的效率和鲁棒性，我们提出了一种快速参考系初始化方法和GPS-IMU外部和时间偏移的在线校准方法。此外，我们还进一步测试了在线校准方法的性能和收敛性。在EuRoC数据集上的实验结果表明，我们的方法始终优于其他紧耦合和松耦合方法。同时，该系统已经在KAIST数据集上进行了验证，证明了我们的系统在视觉或GPS故障的情况下可以正常工作。
主要工作与贡献
1. 我们利用GPS测量和IMU预积分（与用于IMU残差的方法相同）制定了一种新的GPS残差。由于GPS残差和视觉都可以纠正IMU偏差，因此我们的方法在视觉或者GPS故障的情况下维持系统的正常工作。
2. 我们提出了一种非线性优化框架下，对GPS-IMU外参和时间偏移的在线校准方法，并进一步测试了我们的GPS-IMU外参和时间偏移在线较准方法的性能和收敛性。
3. 我们提出了一种快速初始化GPS坐标系和VIO坐标系转换的方法，提高了系统对GPS的有效利用率。
4. 通过假设相同的噪声大小，我们将我们提出的方法与EuRoC数据集上的另一种紧耦合方法[18]进行了比较，这证明我们的系统的定位精度优于[18]。同时，与其他紧密耦合方法相比，我们的系统是第一个在公共数据集KAIST上进行测试的系统。此外，由于没有开源紧耦合GPS-VIO系统，我们将我们的系统与开源松耦合方法VINS-Fusion在KAIST和我们的数据集上进行了比较。
算法流程
A. 框架和符号设定
传感器框架如图2所示，右上角标为b、c、g的依次表示IMU帧、相机帧以及GPS。(∙)bk、(∙)ck和(∙)gk分别表示tk时刻三帧的参数。文章使用(∙)enu表示enu框架和(∙)w表示世界帧，其中ENU帧的z轴和局部世界帧的z轴均平行于重力方向。其余相关符号表示可以具体在论文中查看。
B. 基于紧耦合优化的GPS-VIO
公式(1)是状态向量集合，公式(2)是在确定的时间间隔内的惯性残差定义，其中ΔRi，i+1，Δvi，i+1和Δpi，i+1表示预积分旋转、速度和位置测量。
(3)是视觉残差公式；(4)则是视觉惯性的非线性优化问题。
在本文中，我们提出了一种新的GPS残差，如上图3所示，用于校正视觉惯性估计的误差。该残差由GPS数据、IMU预积分、IMU姿态、IMU速度、加速度计偏差以及陀螺仪偏差组成。该残差在时间ti 的IMU状态、偏差和预积分之间具有相对约束。为了联合最小化这些误差和残差，我们提出了一种基于优化的状态估计方法。这里，非线性优化问题可以表述为：（其中rgk表示GPS残差）
C. GPS-VIO初始化
初始化过程分为两步：首先进行VIO初始化，得到良好的初始惯性变量，通常耗时15~30s。第二部则是进行GPS-VIO初始化，但这一步需要先消耗一定的时间来收集GPS观测和与之对齐的VIO位姿来进行对齐轨迹。为了解决这一部分的耗时问题，我们提出了一种新的GPS-VIO快速初始化方法，就是能在执行第一步时，同时能收集和处理第二部所需的数据。方法细节如下：
在VIO初始化期间（即第一步），我们可以通过立体视觉里程计获得每一个IMU帧相对于第一个IMU帧的位置，和通过GPS测量得到各个对应对齐IMU帧时间戳的每个GPS位置（通过插值实现），也可以通过公式(6)转换得到相对于第一个IMU帧的各个GPS位置，其中Pgb是GPS-IMU外参。
由此，我们就得到了两条轨迹，一条是视觉里程计中的，一条是对应在ENU坐标系下插值的GPS轨迹。然后将这两条轨迹通过公式(7)得到最小平方估计问题。最后还可以通过公式(8)计算得到外参。通过这种方法，VIO和GPS-VIO的初始化几乎同时完成，与[9]和[18]相比提高了效率。
D. GPS残差和校准
为了减少视觉惯性估计中的累计误差，并要求系统能在GPS或视觉故障的情况下保持正常工作，我们提出了一种利用GPS测量和IMU预积分的新残差估计方法。首先，利用公式(9)中的第一个将时间tkϵ(ti,ti+1)处的GPS测量值转换为时间ti+1处的IMU帧，以便有效地使用IMU预积分Δpi+1 和ΔRi+1，其中td是GPS和IMU时钟之间的时间偏移。
然后根据以下公式构造GPS残差：
如果想要进一步研究论文，请阅读原论文，这里翻译的可能比较笼统，下面有论文下载链接。
实验与结果分析
A. EuRoC MAV
表一比较了我们使用GPS立体IMU传感器配置的系统与最先进的算法ORB-SLAM3、松耦合方法VINS Fusion和紧耦合方法[18]的性能。我们的结果是10次处决后的中位数。值得一提的是，我们在[18]中采用了n=1的结果，因为我们只融合了一个GPS数据。如表所示，通过紧密耦合的融合GPS和视觉惯性里程计，我们的系统比ORB-SLAM3和VINS Fusion获得更精确的结果。
B. KAIST数据集
B. 户外实验
[18] G. Cioffi and D. Scaramuzza, "Tightly-coupled Fusion of Global Positional Measurements in Optimization-based Visual-Inertial Odometry," 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2020, pp. 5089-5095
如果你对本文感兴趣，想要下载完整文章进行阅读，可以关注【3D视觉工坊】公众号。点击阅读原文， 即可获取本文下载链接。
本文仅做学术分享，如有侵权，请联系删文。
