双目视觉-惯性SLAM系统的闭环标记：理解漂移和延迟对跟踪精度的影响
双目视觉-惯性SLAM系统的闭环标记：理解漂移和延迟对跟踪精度的影响
论文名称：Closed-Loop Benchmarking of Stereo Visual-Inertial SLAM Systems: Understanding the Impact of Drift and Latency on Tracking Accuracy
作者单位：Georgia Institute of Technology
论文下载链接：https://arxiv.org/pdf/2003.01317
摘要
在GPS拒绝的环境下，视觉惯性SLAM对于机器人导航至关重要，例如：室内，地下。一贯以来，视觉惯性SLAM通过开环分析评估其系统性能，重点关注SLAM系统的漂移级别。这篇论文中，我们提出的问题是视觉估计延迟在闭环导航任务中的重要性，例如准确轨迹追踪任务。为了理解视觉惯性SLAM系统中漂移和延迟的影响，进行闭环基准测试模拟，使用视觉惯性估计的反馈来命令机器人遵循期望的轨迹。通过额外评估目前具有先进代表性的视觉惯性SLAM系统进行轨迹追踪的性能，我们揭示了这些系统中，延迟减少对于视觉估计模块的重要性。这些发现表明了视觉惯性SLAM将来发展的方向。
1. 介绍
基于视觉的状态估计技术，例如视觉里程计（VO）和视觉同步定位和建图（VSLAM），是机器人在未建图场景下自主导航的关键技术。VO会经常忘记感知过的世界结构，但是VSLAM保留了所遍历世界的长期地图。由于缺失绝对的位置信号，例如来自于GPS信号，VO/VSLAM补充了传统基于车轮/惯性的里程计。
对于仅仅依赖视觉传感器的VO/VSLAM，视觉惯性SLAM（VI-SLAM）使用两种互补的数据流，来获得更好的准确性和鲁棒性，以及更高的状态估计频率。视觉传感器提供准确的，但是对于环境中的绝对地标是稀疏而延迟的测量。减少估计漂移，是通过在一个长期而潜在断续测量的过程中，进行观测和匹配地标。惯性传感器是对机器人进行高频率，几乎是实时的，但是有漂移的测量。惯性测量弥补了短期的视觉特征损失（例如在纹理缺失的情境下）。对于一个VI-SLAM系统的姿态估计信息，能够作为一个高质量的反馈信号发送给一个控制器，用来支持在某个环境中移动机器人的轨迹追踪。
尽管VI-SLAM在机器人学中最终用途是闭环导航，但是VI-SLAM的一般基准是采用开环分析，即，SLAM的输出不影响实际机器人的驱动和将来传感器的输入。虽然反映了VI-SLAM估计漂移级别，开环估计不能完全解决在闭环操作中，由导航和VI-SLAM估计所带来的双重影响。对于有针对性的闭环导航，很难从获得到的开环基准得分中，分析到VI-SLAM的独特见解。为了解决这样的基准差距，对于闭环VI-SLAM估计，我们提出了一个开源的、可再现的基准模拟，以及用它评估几种VI-SLAM方法的结果。可再现、闭环的基准应该服务于指导移动机器人将来的VI-SLAM研究。
尽管VI-SLAM漂移是影响闭环导航性能的关键因素，视觉估计的延迟同样在闭环中产生重要影响。如图1所示，对于视觉过程子系统的延迟减少，由于累计IMU偏差的二项性（实时），能够提高融合的视觉惯性状态估计的准确性。因此，这篇论文研究了在闭环基准模拟下的VI-SLAM系统的漂移和视觉估计延迟的影响，通过实现和测试具有不同运行属性的公开VI-SLAM系统。闭环基准结果表明，VI-SLAM系统必须平衡漂移和延迟。
图1：延迟对于误差的影响。
2. 相关工作
这段首先回顾了视觉惯性状态估计在闭环导航中的现状。名称VI-SLAM会被用来表示视觉惯性里程计（VIO）和视觉惯性SLAM。之后，本章结合对于闭环轨迹追踪基准的讨论，回顾了VI-SLAM的评估方法。
A. 闭环导航中的VI-SLAM
对于移动机器人，将过滤器使用在视觉惯性状态估计有很久远的历史（例如：EKF；MSCKF）。稀疏光流（例如：KLT）和MSCKF的结合已经被认为是一种很有效的VI-SLAM方法。大部分基于过滤的方法有一个缺点是构图质量较差，从而影响长期的重定位导航。
VI-SLAM通过运行集束优化（BA）来保持一个明确的地图，从而提高状态估计的准确性和长期的鲁棒性。为了降低BA的立方计算耗费，基于BA的VI-SLAM一般用历史信息（关键帧和地标）的一个子集进行运算，这个子集数据通过一个滑动窗口或一个可见度图来提取。目前代表性基于BA的VI-SLAM包括基于特征的OKVIS，基于KLT的VINS-Fusion和Kimera。使用OKVIS进行闭环导航，已经在地面和空中机器人被证明有效。VINS-Fusion已经在微型飞行器上演示了完整的导航。Kimera实时估计3D网格，这有益于导航。
近来，直接VI-SLAM系统被提出，这些系统不需要明确的特征提取和匹配。直接系统通过使用原始读取的图像数据进行对象函数优化，从而同步完成数据关联和状态估计。直接VI-SLAM系统，例如SVO和ROVIO，已经被集成到闭环导航系统中。尽管KLT和直接VI-SLAM都比基于特征的VI-SLAM计算简单，但是它们也对基于导航的条件更加敏感，例如，它们需要准确的姿态预测（来自惯性）和最小的光源条件变化。此外，KLT和直接方法都是以短基线特征匹配为特点。在另一方面，特征描述子匹配可以提供可靠的长期基线匹配，从而提高状态估计效果。（如图2所示）
图2：3种常用系统特征匹配分布。
B. 闭环轨迹追踪评估
不同VI-SLAM方法的开环估计已经在论文中被广泛实施，例如，在多种数据集上，在多种计算设备上和多种拟合环境中。在导航任务中，不同VI-SLAM方法的闭环估计却很好被研究。机载估计倾向于被用来作为单独实现的报道。闭环估计的一个挑战就是，闭环导航不仅仅是软件层面的问题。整个系统的性能受到所选择传感器、计算资源、系统动力和目标环境的影响。所以这些因素都要通过实验控制，从而达到对于使用VI-SLAM进行闭环导航的系统性能的全面评估。实现全面而重复的闭环评估的一个途径就是通过模拟。有几种模拟器常被机器人领域所使用。Gazebo是最流行模拟器之一，而RotorS则带有特定飞行器的扩展内容。AirSim是另外一种选择，通过循环引擎对于数据进行逼真的渲染。而一个更新的发展是，将硬件集成到循环中。实时捕获机器人轨迹的方法，同时在远处工作站上渲染虚拟视觉数据，来收集真实物理情况下的实际数据和来自易于扩展的渲染器的虚拟数据。但是，所获取的真值依赖于MoCap设备，它很难调整到超过房间大小的环境中。为了在闭环导航中正确基准化VI-SLAM，基准框架需要被重新配置不来涵盖不同的因素，例如，传感器配置，计算/机器人平台，和目标环境。此外，在整个导航过程中，真值都需要被报道。这项工作的目的是为了填补一个开源、闭环基准框架的差异，用来支持上述不同配置的需要；并基于闭环评估的结果，为不同具有代表性的VI-SLAM系统提供不同的性能见解。
3. 闭环系统概览
闭环轨迹追踪系统包含两个主要子系统，如图3所示。，包括：1）一个VI-SLAM系统输入视觉/惯性数据来生成高频的状态估计和低频的地图更新；2）一个使用来自VI-SLAM姿态最终模块的高频输出信息的控制器，用以生成实际的命令。尽管这篇论文仅关注双目惯性传感输入，但是该系统支持其他常见的数据传感器，例如单目和RGB-D相机。
图3：闭环轨迹追踪系统概要。
虽然，构图和闭合回环对于准确和鲁棒的状态估计很重要，但是这两个模块需要很高的计算量，并且它们的计算的频率比姿态追踪的低（经常相差一个数量级）。因此，需要在反馈控制中使用的高频位姿估计信息，被位姿追踪模块所搜集。这篇论文，探讨了被用来作为反馈控制的状态追踪模块的有效性和准确性。大量的VI-SLAM系统被集成进闭环系统中，涉及代表性的涉及选项，例如松/紧耦合的视觉惯性融合，基于直接/特征数据的关联，以及基于过滤/BA的后端。
这项研究的关注的是，被使用在闭环中的VI-SLAM系统的轨迹追踪性能。通过计算在整个导航过程中理想和实际轨迹之间的累计姿态误差，来反映追踪性能。这里，我们计算了理想和实际轨迹之间平移误差的均方根，称为追踪RMSE，作为性能指标。追踪RMSE与ATE公式相匹配，一般用在开环估计，但是却使用实际机器人轨迹。它直接测量轨迹追踪系统的最终性能，从而捕获姿态追踪漂移和延迟的联合影响。其他捕获方向误差的指标被在线报道。
模拟实验中使用的移动机器人是差速驾驶TurtleBot2，安装在机器人上的是一个30fps的双目相机，其基线为11cm，并且在其底座安置了IMU装置。来自于双目相机和IMU装置的数据流输入到VI-SLAM系统中，从而获得\(SE(3)\)的状态估计输出。轨迹追踪控制器使用\(SE(3)\)中的子集\(SE(2)\)估计信息数据，来追踪目标轨迹。下一章将陈述所实现的VI-SLAM系统、轨迹追踪控制器和Gazebo/ROS中模拟配置信息。
A. 视觉惯性SLAM系统
几个可获得的公开双目惯性SLAM系统，被选来集成在闭环基准系统中。下面陈述具体5个实现：
- MSC： MSCKF-VIO + MSF。MSCKF-VIO是一个紧耦合的VIO系统，使用基于KLT的前端和MSCKF的后端。基于EKF的传感器融合MSF在将其发送到控制器之前，先对MSCKF-VIO的低速率估计输出进行了压缩，并且不包含闭环部分。
- VINS：VINS-Fusion是一个紧耦合的VI-SLAM系统，使用基于KLT的前端和基于BA的后端。VINS具有很大的延迟，是由于使用BA。它提供一个低延迟、高速率的IMU传播信号，这个信号被发送给控制器。VINS带有闭环功能，这样就适用在长期重访的场景中。执行圆周运动来初始化VINS先验知识，从而开启SLAM的估计过程。
- SVO：SVO + MSF。一个有效的、松耦合的VIO系统，包含直接SVO和融合MSF，并且没有闭合回环模块。SVO上述方法中最低的姿态追踪延迟。
- ORB：ORB-SLAM + MSF。ORB-SLAM有一个基于特征的前端和一个基于BA的后端。由于需要大量计算的特征提取和匹配，ORB-SLAM具有很大的视觉估计延迟。类似于MSC和SVO，MSF被集成进ORB来生成高频的估计信号。
- GF：懒GF-ORB-SLAM + MSF。一个送耦合ORB的变化版本，带有两方面高效的调整：良好特征和懒双目匹配。良好的特征匹配在特征匹配上限的预算下执行目标地图到图像帧的匹配。懒双目调整将双目ORB-SLAM计算划分为即时姿态估计所需要的内容和哪些组成将来姿态估计计算的内容。前者被有限运行，因此，能够更快地输出位姿估计数据。这两方面的调整降低了延迟，同时对于姿态估计的准确性没带来显著的影响。
如果初始化方法没有被陈述，那么默认方法，是在开始闭环/开环之前将机器人静置10秒钟。
B. 反馈控制
理想轨迹\(d^*(t) \in \Bbb{R}^2\)是通过使用样条线的一系列指定航路点构建的。希拉雷式机器人的指数稳定轨迹跟踪控制器产生了机器人可遵循的运动学上可行的轨迹。下面讨论中，对于加速度和速度的限制阐述地很清楚，尽管它们存在于实际地实现中。
机器人姿态作为一个时间地函数\(g(t) \in SE(2)\)服从下面地控制等式：
\(\dot{g} = g \cdot \left[ \begin{matrix} v \\ 0 \\ w \end{matrix} \right]\) and \(\dot{v}=u^1 \\ \dot{w}=u^2\) (1)
公式1中，\(v\)是前向速度，\(w\)是角速度，这两者都存在实体坐标系中。信号\(u=(u^1,u^2)^T\)坐标是前向加速度和角加速度（在实体坐标系中）。
所使用地控制器依赖于机器人移动地差分平整度，从而获得一个机器人前面虚拟点地指数稳定化数据（通过一个距离\(\lambda\)）。下面定义\(\lambda\)调整选择矩阵和角速度矩阵：
\(R_{\lambda}=R \cdot diag(1, \lambda)\) and \(\hat{w}(\lambda, \dot{\lambda})=\left[ \begin{matrix} 0 & -\lambda w \\ \frac{1}{\lambda}w & \frac{\dot{\lambda}}{\lambda} \end{matrix} \right]\) (2)
公式2中，\(R\)是\(g\)中方向所提供的选择矩阵。对于世界坐标系下\(\hat{x}\)向量的单位向量\(e_1\)，轨迹追踪控制律如下：
\(u=c_pR_{\lambda}^{-1}(d^*-d-\lambda * Re_1)+c_d(R_{\lambda}^{-1}\dot{d^*}-V)-c_d\dot{\lambda}e_1-\hat{w}(\lambda,\dot{\lambda})V-(\hat{w}(\lambda,\dot{\lambda})-c_{\lambda}I)\dot{\lambda}e_1\) (3)
公式3中，\(c_p\)，\(c_d\)，\(c_{\lambda}\)是反馈增益，并且\(V=[v|w]^T\)。额外的偏移动力学是：
\(\dot{\lambda}=-c_{\lambda}(\lambda-\epsilon)\)，其中，\(\lambda(0)>\epsilon > 0, c_{\lambda} > 0\). (4)
动态系统由公式1-4所表示，获得了一个机器人姿态姿态\(g*(t)\)的参考轨迹和主要的速度成分\(V*(t)\)，用来追踪理想的轨迹\(d*(t)\)。偏移变量\(\lambda*(t)\)可以被忽略。
实时轨迹控制器驱使机器人去追踪基于机器人所估计状态（也就是\(SE(3)\)的一个状态子集\(SE(2)\)）的反馈的参考轨迹。这些控制命令是：
\(v_{cmd}=k_x*\widetilde{x}+v^*\) (5)
\(w_{cmd}=k_{\theta}*\widetilde{\theta}+k_{y}*\widetilde{y}+w^*\)
其中，\([\widetilde{x}, \widetilde{y}, \widetilde{\theta}]^T \approx g^{-1}g^*\)是在物体坐标系下目前状态\(g\)和理想状态\(g^*\)之间的相对位姿误差。在误差缺失的情况下，控制信号是\(V^*(t)\)。
C. 模拟配置
这一章描述模拟Gazebo环境，用来测试使用VI-SLAM系统的闭环轨迹追踪。所创建测的场景用来机器人导航的是一个虚拟办公室世界（如图4）。这个虚拟世界是基于一个实际办公室的平面图，同时带有纹理地图的表面。由于碰撞检查和路径规划不在本文讨论范围之内，因此墙要放置在离地面1米的位置。引入碰撞避免会增加另一个耦合因素到闭合系统中，这也就会在确定追踪错误源（即，是否用来避免碰撞或由于错误的估计）中引入不需要的复杂度。
图4：模拟地图。
六个测试轨迹被创建用来闭环导航实验，每个都有不同的特定（如图5所示）。前两个轨迹相对短（不超过50米），并且具有很少的重访区域。第三和第四条轨迹都是中等长度（不超过120米），其中第三条有很多的重访区域，因为它回溯了上次轨迹；相比，第四条轨迹交叉得较早轨迹端，并且是反向或垂直于之前轨迹。最后两条是长轨迹（不超过240米），其中第五条回溯轨迹段，相比第六条没有沿着相反方向回溯。所有的轨迹对于机器人来说都是相同的出发点，即都是世界坐标下的原点。测试了三种理想的线性速度：0.5m/s，1.0m/s，和1.5m/s。基于这些速度，模拟中的导航线路从30秒持续的480秒。
4. 实验结果
这章描述了两个主要实验的结果。前一个涉及双目VI-SLAM方法的开环估计，其中控制器输入正确的姿态而不是VI-SLAM的估计。开环估计主要有两个目标：1）它表明，与视频录制的开环基准测试（例如EuRoC）相比，该方法在模拟世界中的排名大致得以保留；2）它描述了相对基准的仿真环境的功能范围。第二个实验实施闭环追踪测试，其中控制器输入VI-SLAM的估计数据。闭环基准的目标是确定VI-SLAM的哪些属性（即，漂移，延迟），对轨迹追踪性能由影响。
所有5个VI-SLAM系统的参数配置都是通过参数扫描发现的。对于每个测试配置（理想轨迹，理想线性速度，VI-SLAM方法和IMU），基准的运行被重复了五次，以至于随机参数，例如多线程数和随机传感器噪声，都能被正确反映。模拟了两个常被使用的IMU：一个高端ADIS16448和一个低端的MPU6000。开环基准运行在i7-4770（单线程通过得分：2228）。闭环基准运行在一个双Intel Xeon E5-2680内核的工作站（单线程通过得分：1661）。作为参考，大部分开源闭环导航系统使用一个Intel NUC，它们的CPUs得分在1900-2300之间（单线程）。整个方法模块，包括模拟器，集成VI-SLAM系统和轨迹追踪控制器都被公开了。
A. 开环结果与分析
鉴于模拟和记录的开环基准数据可能不一致，这章实施一次对比来验证模拟的应用领域。比较表明，模拟场景与现有基准有一些重叠，尽管它们并不跨越整个域。基于相似性，当在等效的实际环境中部署闭环系统时，闭环实现应具有预测能力。
一个EuRoC的子集和模拟的开环序列特征如表1所示。持续时间配置项以描述[2, 10]分钟间隔的媒介定义；Duration属性分类为低（0.5m/s）到中等（1.0m/s）再到高（1.5m/s）；Revisit频率是跟随的轨迹以及在时间上相距遥远的轨迹段上出现特征的共同可见性的函数；捕获的另一个统计数据是使用ORB（最后一列）每帧跟踪的平均特征数（Feats）。模拟序列比EuRoC序列具有较少的纹理信息。这两个基准的特征有足够的重叠，模拟反映了稍微不同的情况。 定性地，相对于EuRoC序列，模拟序列是可比较的或更难的基准测试案例。
表1：模拟数据序列特征。
为了进一步比较，我们针对地面真实性进行了开环基准测试，以了解VI-SLAM算法的姿态估计属性以及两个基准测试集在相对顺序方面是否一致。 表2和表3汇总了中等运动曲线的5次重复的平均结果，其中省略了磁迹丢失的情况（破折号）。 根据这些表，对于EuRoC序列，VI-SLAM估计和地面真实性之间的ATE通常较低。 相对于所有方法中的主流值，SVO和VINS在EuRoC中均表现出异常，其中SVO为1，而VINS为3。 它们出现在MH序列中，这表明这些通常对于VINS和SVO而言更成问题。 但是，对于模拟情况，SVO和VINS也有一个磁道故障。 总的来说，结果与先前的说法一致，即模拟序列与EuRoC相当或更难。 如果将轨道故障作为对模拟性能结果的惩罚而添加，那么算法的等级排序在EuRoC和模拟之间是一致的。 GF通常具有最低的ATE，而VINS具有最高的ATE。 此外，不同VI-SLAM的相对延迟时间顺序也一致：SVO最低，而VINS最高（MSC是唯一的，因为不匹配）。这些比较支持使用仿真对VISLAM进行基准测试，并具有针对特定部署条件的有效性。
表2和3：运动曲线5次重复的平均结果。
B. 闭环结果与分析
轨迹跟踪性能在表4和表5中进行了量化。所需轨迹和实际轨迹之间的跟踪RMSE反映了5次重复的平均值。 平均RMSE超过10m的情况被认为是导航失败，因此省略（破折号）。 每个VI-SLAM中视觉估计的平均等待时间在每个表的底部行中报告（算法按升序排序）。
表4和5：量化后的轨迹追踪性能。
根据表4和表5，在多种配置下，VINS和ORB均失败。 与ORB相比，GF的成功率和RMS显着提高。视觉估计等待时间的减少有助于改善，因为ORB和GF的开环结果在漂移方面相似，但在等待时间方面却大不相同。 结果表明，满足标准帧速率潜伏期水平（约30ms）是最好的，而且对于良好的闭环轨迹跟踪性能来说，可能是必不可少的。基于过滤器的MSC受IMU数据质量的影响很大，因为它无法导航多个低端IMU情况和较高的速度。 结果表明，姿态估计过度依赖IMU，图2支持了MSC，其中MSC对于检测到的特征的长期数据关联性较差。 通过与世界上已知的静态点链接并改善绝对位置估计，能够重新关联到丢失的轨道可以改善性能。否则，诸如MSC之类的系统将依赖于可观察性较差的综合估计。
最后两种检查方法是SVO和GF，它们都成功地跟踪了除一个序列以外的所有序列的摄像机姿态。 这是两种性能最强的方法。 有趣的是，它们具有不同的运行时属性。 对于开环和闭环评估，SVO的等待时间最低，但漂移最高，而GF相反。 从开环到闭环，它们的相对性能保持不变，只是对一小部分序列取模。 看起来低延迟可以容忍较高的漂移，而较低的漂移则允许较高的延迟。 但是，总的来说，似乎等待时间足够低，相对于闭环轨迹跟踪的等待时间增强，针对精度增强更好（对于大多数静态的，具有足够特征的地面车辆）。比较两种IMU类型的SVO和GF可以看出，高端IMU提供了最佳的地面速度误差缩放比例，而GF随着速度的增加而更加一致。
这些定量结果可以在图6和7中定性地看到，它们跟踪了不同VI-SLAM方法的闭环机器人轨迹。 VINS超出了许多运行范围。 着眼于SVO和GF的痕迹，很明显，对于给定序列，SVO在运行中具有更高的估计方差，而GF轨迹更紧密地聚集。 该属性与IMU类型无关。 总体而言，GF似乎是表现最好的。 作为ORB的一种修改，它试图减少姿势估计的等待时间，同时保留ORB的有益特性。 本文的研究结果表明，在争取实现足够小的延迟的同时优先考虑精度是一种有效的手段，可为自主移动机器人应用识别出高性能的VISLAM。 解决GF的异常情况仍然需要做一些工作。
图6和7：定性结果。
5. 总结
本文研究了几种立体VI-SLAM方法，以了解其闭环轨迹跟踪特性。 这项研究得到了模拟Gazebo环境的支持，该环境表明可以代表一组特定的基准条件。 对结果的分析表明，潜伏期和漂移都在实现精确的轨迹跟踪中起着重要作用。 建立在ORB-SLAM上的VI-SLAM系统（用GF表示）提供了最准确的轨迹跟踪结果，这与其开环性能一致。 其他方法不一致。 SVO在闭环中具有较高的性能，但在开环中具有较差的性能，而ORB则相反。 未来的工作将通过其他移动机器人，渲染选项以及视觉环境或设置来扩展基准测试环境。 重要的是，与实际的防撞系统集成以及环境障碍对SLAM的影响将改善基准的任务真实性。
