Range-Focused Fusion of Camera-IMU-UWB for Accurate and Drift-Reduced Localization

作者：Thien Hoang Nguyen; Thien-Minh Nguyen; Lihua Xie

论文地址：https://ieeexplore.ieee.org/abstract/document/9350155

使用了单目camera、imu、UWB来构建一种紧耦合坐标系，其中UWB是一种用于近距离定位的设备，有发送端、接收端构成，测量结果可以直接得到对应的距离和方位，类似于GPS的作用。

摘要： 在这项工作中，我们提出了单目camera、6-DoF IMU 和单个未知UWB锚点的紧耦合融合方案，以实现准确且减少漂移的定位。具体而言，本文着重于将 UWB 传感器整合到现有的最先进的视觉惯性系统中。以前实现这一目标的工作使用单个最近的 UWB 测距数据来更新滑动窗口中的机器人位置（“以位置为中心”），并且已经证明了令人鼓舞的结果。然而，这些方法忽略了 1) UWB 和相机传感器之间的时间偏移，以及 2) 两个连续关键帧之间的所有其他距离。我们的方法通过利用从视觉惯性里程计方案中随时可用的传播信息，将视角转移到 UWB 测量（“关注距离”）。这允许以更有效的方式使用 UWB 数据：解决每个距离数据的时间偏移，并且可以利用所有可用的测量。实验结果表明，所提出的方法在估计锚点位置和减少长期轨迹漂移方面始终优于以前的方法。

1 引言

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

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

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

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

通过利用 VIO 方案计算的传播数据处理 UWB 测量的所谓“以距离为中心”的观点，充分解决了 UWB 相机传感器之间的时间偏移问题，并允许消耗所有可用的 UWB 数据；

单目相机、6-DoF IMU 和单个 UWB 锚点的紧耦合融合方案，以提供减少漂移的里程计，内置 UWB 锚点定位模块来估计未知的锚点位置；

广泛的实验结果来验证每个提议的 UWB 辅助系统组件的性能。

本文的结构如下：首先，第二部分介绍了与该主题相关的文献。然后，第三部分概述了系统以及 VIO 的相关概念。第四部分从处理 UWB 测量的所谓“以距离为中心”的观点开始，然后将其用于 UWB 锚定定位和用于里程计的相机-IMU-UWB 传感器的紧耦合融合这两个任务。接下来，第五部分提供了现实生活和模拟结果，并与最先进的方法进行了比较。第六节为总结。

2 文献论述

A. UWB-Aided Localization and Mapping

已经提出了将 UWB 结合到现有定位系统中的不同方法 [16]。UWB 距离可用于独立的定位方法，然后与：单目相机 [17]-[18][19]、IMU [20]、[21]、RGB-D 相机 [22]、IMU 和 RGB- D [9]、LiDAR [23] 等，以提高 SLAM 系统的准确性和鲁棒性。为仅 3D 范围定位获得独特的解决方案需要：1) 最少四个具有已知位置的 UWB 锚点，或 2) 三个已知的锚点和机器人的高度数据 [24]、[25]。这种假设限制了系统的适用场景，因为操作区域需要适应 UWB 锚点设置，并且每个新环境都需要额外的时间和精力来校准锚点的位置。为了减轻这种要求，鉴于机器人可以使用具有附加锚间范围的公制里程计 [23]，或仅使用公制里程计 [15]、[22]，最近的方法尝试在操作期间估计锚点的地图]，或甚至只有最大规模的里程计 [17]，[18]。然而，这些解决方案仍然以次优方式处理 UWB 数据，这在第 II-B 节中进行了解释。在这项工作中，我们探索了相机 IMU 和 UWB 的组合，在未知位置只有一个锚点。这种设置将结合使用 VIO 方案进行准确的短期里程计和最灵活的 UWB 锚点配置的好处。此外，视觉信息对于深度学习时代的许多高水平研究和应用至关重要。

B. Visual-Inertial-Range Localization and Mapping

与本文最相关的是使用相机-IMU-UWB 传感器进行定位和建图任务的文献。虽然大多数方法使用 VIO 进行在线定位，并单独使用 UWB 进行基于距离的相对定位 [26]-[27][28][29]，但最近的工作表明，可以同时融合视觉、惯性和 UWB 数据，获得锚位置估计并改进姿势估计，[15] 提出了 EKF 解决方案，[14] 采用了姿势图优化坐标系。虽然最终目标和融合方法不同，但这些方法都使用 UWB 数据的基本原理：从状态向量中的位置角度确定残差。此视角导致以下问题：1) 一个位置与一个最近的 UWB 测量配对，忽略相机帧和距离数据之间的时间偏移，2) 丢弃两个连续相机帧之间的所有其他范围。相比之下，所提出的系统根据距离测量的时间戳来制定 UWB 残差，这允许以传感器的精度使用距离数据。通过利用 VIO 方案中 IMU 状态传播过程的结果，可以为每个距离测量导出 UWB 残差，从而解决时间偏移问题并可以利用所有可用距离。

3 概述

我们将提供系统概述，描述将在第 IV-B 和 IV-C 节中使用的 VIO 的符号和最相关的概念。由于篇幅限制，对于视觉惯性组件，感兴趣的读者可以参考 VINS-Mono [1] 了解更多详细信息。

A. System Overview

图 2提出系统的概述。一个移动机器人配备了一个单目相机、一个 6-DoF IMU 和一个刚性连接到身体坐标系的 UWB 传感器，所有内参和外参都经过校准。可以对放置在未知位置的单个 UWB 锚进行距离测量。在这项工作中，采用双向飞行时间 (TW-ToF) UWB 传感器，因为它不需要传感器之间的时钟同步，因此更适合许多应用场景。

系统运行分两个阶段：

基于 VIO 的 UWB 锚点定位（第 IV-B 部分）：最初仅使用相机和 IMU 来提供准确的短期里程计，将结合距离测量来估计 UWB 锚点位置。一旦不确定性下降到某个阈值以下，锚点位置估计就被认为是固定的。

Visual-Inertial-Range 里程计（第 IV-C 部分）：一旦找到 UWB 锚点位置，随后的距离测量将在基于联合关键帧的优化中与视觉和惯性数据紧融合在一起，以获得准确且减少漂移的长期里程计。

理论上，这两个阶段可以合二为一（即使用 UWB 锚点位置扩展 VIO 优化的状态向量）。然而，由于多种原因，这种系统不太可能在实践中工作：

VIO 的滑动窗口被设计为在短时间内包含数据，这不利于 UWB 锚点定位任务，该任务强烈依赖于 3D 中的空间多样化位置数据。扩展窗口大小是不可行的，因为视觉测量值呈指数级增长，会给计算机带来巨大的计算负担。如果没有提供锚点位置的良好初始猜测，优化将难以产生任何令人满意的结果。由于这个初始猜测通常是手动测量的，并且可以随每次操作而改变，因此它容易出现人为错误，应该避免。

B. Notation

t_c、t_i 和 t_j 分别表示为相机、IMU、UWB 范围测量的时间戳。tk 是滑动窗口中关键帧之一的时间戳，在不失一般性的情况下设置为 tk=:tc。应该注意的是，只有在满足某些标准的情况下，相机帧才会被选为关键帧 [1]。这可能导致两个连续的关键帧被多个相机帧分隔。因此，两个关键帧之间可用的 UWB 测量数量通常在整个滑动窗口中并不统一。图 1(b) 展示了这种观察的一个例子。

图1 (a) 参考系和测量值的说明。(b) 我们的公式中传感器测量和关键帧的时间；(c-d) 分别是先前（“以位置为中心”）和建议（“以距离为中心”）方法的因子图。请注意，只有在满足某些条件时，才会选择相机帧作为关键帧。

C. Optimization-Based Monocular Visual-Inertial Odometry

D. IMU State Propagation

4 关注距离的Camera-IMU-UWB融合

间的所有其他额外距离测量。

事实上，仿真（第 V-A 部分）表明这两种方案在这些条件下具有相似的结果。然而，实际上相机和 UWB 传感器总是相互独立工作，因此时间偏移问题是固有的，不应被忽视。此外，标准的 UWB 数据速率通常比相机的数据速率高很多倍。因此，以前的方法总是会丢弃大部分可用距离数据，这意味着 UWB 传感器仍未得到充分利用。

B. UWB Anchor Localization Based on VIO Data

1) Problem Formulation

图 3 该系统的 UWB 锚定位组件概述。

2) Sufficient Conditions

问题的可观察性已在 [29] 中建立，其中指出机器人不应直接向锚点移动。在实践中，估计结果将取决于覆盖所有 3D 轴的轨迹以及锚点相对于移动半径的距离。将 x 轴上位置的样本方差表示为 S2x（对于 y 和 z 轴类似），当机器人移动时，它会针对每个新位置数据递归更新。为确保优化的性能，检查以下条件以启动或跳过优化过程：

3) Termination Criterion

启动后，可以使用标准 Levenberg-Marquardt 算法 [34] 和 Ceres 求解器 [35] 优化成本函数 (10)。由于系统是在线执行的，因此引入终止准则来确定解的不确定性：

C. Keyframe-Based Visual-Inertial-Range Odometry

1) Problem Formulation

2) Range-Focused UWB Factor

可以通过以下方法之一将时间 tj 的位置与状态 xk 相关联：

遵循 VINS-Mono [1] 的边缘化策略，每当该关键帧被边缘化时，连接到第一个关键帧的 UWB 因子与视觉和惯性因子一起转换为线性化先验。

5 实验结果

A. UWB Anchor Localization

图 5 说明了同时估计两个锚点的结果。两种方法都达到相同的小于 0.1 m 的最终误差，但很明显，所提出的方法提供了更快的收敛时间。原因是我们的“以距离为中心”的方法为每个新的距离测量改进了解决方案，而“以位置为中心”的对应方法在收集到足够的数据以取得突破之前陷入了局部最小值。

图 6(a) 显示了模拟设置。位置和速度数据以 20 Hz 的频率生成，一个 UWB 锚提供 20 Hz 的距离数据。所有数据都被高斯噪声 η∼N(0,0.02) 破坏。对于每次模拟，执行估计直到满足 Levenberg-Marquardt 算法的停止标准。如果满足条件（12），我们停止模拟并获得锚点位置估计以及位置误差。否则，估计会继续下一个数据点，并使用最终估计来测量误差。

B. Visual-Inertial-Range Odometry

1) Simulation With EuRoC Dataset

表 I ATE (m) 在 EuroC 数据集上的比较。

图 7 EuRoC 数据集中的 MH_03（左）和 MH_04（右）序列的估计（来自提议的系统）和真值轨迹的俯视图。

2) Real-Life Experiment

硬件系统由一个配备有前面介绍的传感器的平台组成 (V-A)。这些实验旨在测试里程计的漂移：“循环”测试包括使用 Vicon 运动捕捉系统在 6 m × 6 m 区域内进行几分钟的连续运动以获取地面实况，而“开放”测试包括各种轨迹 在 30 m × 10 m 的室外区域，由 Leica MS60 激光跟踪系统提供地面实况。在所有的实验中，一个 UWB 锚点被放置在一个未知的位置，并在每次新的测试中重新定位。锚点位置在操作过程中在线估计。

表 II 报告了 ATE 结果。在 T265 VI-SLAM（立体相机 + IMU）、VINS-Mono（单相机 + IMU）、我们对“关注位置”系统的实现和建议的“关注距离”方法（单相机 + IMU）之间进行了比较 + 一个 UWB 锚点）。图 8 显示了 Loop_02 数据集中位置误差的轨迹和均方根 (RMS) 的概览。对于仅视觉惯性的方法（VINS-Mono 和 T265 VI-SLAM），累积漂移不会随着时间的推移而得到纠正，从而导致更大的误差。当 UWB 数据涉及到“以位置为中心”的方法时，性能得到显着提高。尽管如此，所提出的解决方案显然在所有实验中都表现出色。

表 II 真实实验中 ATE (m) 的比较。最佳结果以粗体突出显示

图 8 使用 Loop_02 作者收集的数据集进行visual-inertial-range里程计的结果。顶部：轨迹的俯视图。底部：位置误差的 RMS。

6 结论

本文提出了一种用于融合相机-IMU-UWB 传感器的新“关注距离”方法。我们利用 VIO 系统中随时可用的传播数据来补偿 UWB 和相机传感器之间的时间偏移，并允许使用所有可用的 UWB 数据。这个想法被整合到两个 UWB 辅助组件中：一个 UWB 锚定位模块和一个基于紧耦合优化的visual-inertial-range数据融合，以在长期操作中提供准确和减少漂移的里程计。对于这两个组件，现实生活和模拟实验结果验证了所提出的系统优于以前的“以位置为中心”的方法。扩展到多机器人场景是未来的主要研究方向。具体来说，我们希望利用机器人之间的测距数据不仅改进每个系统的里程计，而且还结合不共享任何公共视觉闭环的各个地图。

备注：作者也是我们「3D视觉从入门到精通」特邀嘉宾：一个超干货的3D视觉学习社区

原创征稿

初衷
3D视觉工坊是基于优质原创文章的自媒体平台，创始人和合伙人致力于发布3D视觉领域最干货的文章，然而少数人的力量毕竟有限，知识盲区和领域漏洞依然存在。为了能够更好地展示领域知识，现向全体粉丝以及阅读者征稿，如果您的文章是3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、硬件选型、求职分享等方向，欢迎砸稿过来~文章内容可以为paper reading、资源总结、项目实战总结等形式，公众号将会对每一个投稿者提供相应的稿费，我们支持知识有价！

投稿方式

邮箱：vision3d@yeah.net 或者加下方的小助理微信，另请注明原创投稿。

▲长按加微信联系

▲长按关注公众号