vslam论文1：Range-Focused Fusion of Camera-IMU-UWB for Accurate and Drift-Reduced Localization(RAL2021)

准确、低飘移定位的相机-IMU-UWB聚焦距离融合

摘要：

       在这项工作中，我们提出了一种紧耦合的单目摄像机、6自由度IMU和单个未知UWB锚融合方案，以实现精确和减少漂移的定位。具体地说，该文章聚焦于将UWB传感器整合到现有的最先进的视觉惯性系统。为实现这一目标，之前的工作使用单个最近的UWB距离数据来更新滑动窗口中的机器人位置(“聚焦位置”)，并展示了令人鼓舞的结果。然而，这些方法忽略了：1)UWB和相机传感器之间的时间偏移；2)两个连续关键帧之间的所有其他距离。我们的方法通过利用VIO通道中易获取的已传播信息，将视角转移到UWB测量(“聚焦距离”)。这使得以更有效的方式使用UWB数据：每个距离数据的时间偏差都被解决，并且所有获得的测量都能利用。实验结果表明，该方法无论在估计锚点位置还是减小长期轨迹的漂移都优于之前的方法。

结论：

       本文提出了一种新的“聚焦距离”的相机-IMU-UWB传感器融合方法。我们利用VIO系统容易获取的传播数据来补偿UWB和相机传感器之间的时间偏差，并使所有获得的UWB数据都能被使用。该想法被整合到两个UWB辅助组件：UWB锚点定位模块和基于优化的紧耦合视觉-惯性-距离数据，以在长期运行中提供准确的、减少漂移的里程计。对于这两个部件，现实生活和仿真实验结果都验证了所提出的系统的性能优于先前的“聚焦位置”方法。扩展到多机器人场景是未来的主要研究方向。具体地说，我们希望利用机器人之间的测距数据来不仅改善每个系统的里程计，而且还将不共享任何共同视觉环路闭合的独立地图组合在一起。

实验结果：

       该系统分别对每个模块进行了真实实验和仿真评估。主要是比较所提出的“聚焦距离“和之前的“聚焦位置”的方法，以VINS-Mono的结果为基线去进行改善。

A.UWB锚的定位

 图1：UWB锚点定位问题在两个锚点的真实实验中的收敛时间比较。

左：提出的“聚焦距离”方法(R)。             右图：之前的“聚焦位置”方法(P)。

图2：UWB锚点定位的仿真结果。

a)仿真设置。  b-c)具有不同时间偏移值和到锚定距离的先前的“聚焦位置”方法和提出的“聚焦距离”方法的结果。括号中显示了37.5毫秒情况下200多个仿真的平均误差和标准误差。d)在不同的UWB数据频率(相似的摄像头频率)和锚点距离的情况下，该方法的结果。da是运动中心到锚的距离，rb是运动半径，ep是最终锚位置估计误差。

B.视角-惯性-距离里程计

 表1：在EuRoC数据集上的ATE(M)的对比。

 图3：EUREC数据集中MH_03(左)和MH_04(右)序列的估计(提出系统)和地面真实轨迹的俯视图。

 表2：现实生活中实验的ATE(M)结果对比

图4：使用Loop_02作者收集的数据集进行提出方法的结果。

上：轨迹的俯视图。        下：位置误差的均方根。

引言 

        对于许多机器人应用来说，可靠和全局一致的定位仍然是一个开放的研究问题。近年来，由于相机和IMU传感器的互补性，视觉惯性里程计(VIO)或同时定位和建图(VI-SLAM)是实现这一目的的流行方法。尽管最先进的方法，如[1]-[3]可以实现非常准确和高速率的位姿和速度估计，但传感器噪声和计算误差使系统容易随着时间的推移而累积漂移。这个问题的流行解决方案是包括一个额外的全局传感器，如GPS[4]、[5]。对于无法使用GPS的情况(室内、隧道、走廊等)，UWB是适合小规模作业的替代方案[6]、[7]。

        已经针对各种应用和场景提出了融合UWB数据和VIO的方法[8]-[11]。这些方法以松耦合的方式工作，这意味着首先在独立的定位系统中计算UWB距离和相机-IMU数据，然后将UWB和相机-IMU子系统获得的位置估计对准并融合在一起。虽然这些方法可以直接构建，但我们相信，如果立即融合所有传感器数据，利用可用信息之间的相关性，结果可能会得到改善。此外，它们需要使用多个已知UWB锚点进行基于距离的定位，这可能成本高昂，并抑制了在许多空间受限场景中的应用，例如室内、隧道、走廊等。

        最近，引入了仅使用单个位置未知UWB锚点的方法[12]-[15]。这样的系统既有用于精确定位的无漂移距离测量的优点，也有便于实际应用，因为不需要设置时间来校准锚位置。结果表明，通过将UWB、相机和/或IMU的测量紧密耦合在一个联合优化问题中，可以实现更准确和鲁棒的定位。然而，这些方法以一种类似于模拟的方式处理UWB数据：每个相机位置与一个距离测量匹配，并且不考虑两个连续相机帧之间的任何其他距离。这种观点不能反映真实的传感器系统，原因有很多：真实的UWB传感器独立于相机/IMU传感器，因此距离/图像信息之间总是存在时间偏差；UWB测距速率不符合标准相机或IMU速率，UWB数据速率通常比相机高几倍；UWB数据速率在实际操作过程中可能会因视线丢失而变化，这意味着可用的UWB数据量也可能不同。

        为了解决这些问题，在这篇文章中，我们提出了一种更有效的方法来融合视觉、惯性和UWB测量。本质上，我们利用VIO通道中的现有状态传播过程来有效地为每个距离数据制定UWB误差项。 我们的主要贡献包括：

（1）通过利用VIO通道计算的传播数据来处理UWB测量的所谓“聚焦距离”的观点，充分解决了UWB相机传感器之间的时间偏差，并允许使用所有可用的UWB数据；

（2）单目相机、6自由度IMU和单个UWB锚的紧耦合方案，以提供减少漂移的里程计，用内置UWB锚定位模块来估计未知的锚位置；

（3）广泛的实验结果，以验证提出的UWB辅助系统中每个部件的性能。

系统概述

 图5：整个系统概述。基于最先进的VIO系统VINS-Mono，我们的贡献被嵌入到UWB辅助组件中。

        图5说明了所提议的系统的概述。移动机器人配备了一个单目摄像头、一个6自由度IMU和一个固定在车身框架上的UWB传感器，所有的内外参数都得到了校准。可对放置在未知位置的单个UWB锚进行距离测量。在这项工作中，采用了双向飞行时间(TW-ToF)超宽带传感器，因为它不需要传感器之间的时钟同步，更适合于许多应用场景。

        该系统分两个阶段运行：

1)基于VIO的UWB锚点定位：最初，仅使用相机和IMU来提供准确的短期里程计，并结合距离测量来估计UWB锚点位置。一旦不确定性低于某一阈值，锚位估计就被认为是固定的。

2)视角惯性距离里程计：一旦找到UWB锚定位置，后续的距离测量将与视觉和惯性数据紧密融合在一起，进行基于关键帧的联合优化，以获得准确且减少漂移的长期里程计。

         理论上，这两个阶段可以合并为一个阶段(即，利用UWB锚点位置扩展VIO优化的状态向量)。然而，由于多种原因，这样的系统在实践中不太可能奏效：

（1）VIO的滑动窗口被设计成包含短时间跨度的数据，这不利于对三维空间多样化位置数据的强烈依赖的UWB锚点定位任务。扩大窗口大小是不可行的，因为由于视觉测量的指数增长，将对计算机施加显著的计算负担。

（2）如果没有提供对锚点位置的良好初始猜测，优化将很难产生任何令人满意的结果。由于此初始猜测通常是手动测量的，并且每次操作都可能发生变化，因此容易受到人为错误的影响，应该避免。