【论文】Range-Focused Fusion of Camera-IMU-UWB for Accurate and Drift-Reduced Localization

Abstract

I. INTRODUCTION

为什么需要添加UWB？因为传统的VIO会由于传感器的噪声和计算误差产生累计偏移。所以需要GPS或者是UWB来做修正。UWB则是专注于室内。

以前对于UWB的使用一般都是松耦合，本文相信紧耦合可以更好好的将UWB数据协同定位得到更加好的效果。

由于UWB的频率是大于camera的，所以在以往的工作中，都是直接使用camera对齐的UWB数据，中间的连续的距离数据会被丢弃，这不能很好得体现一个现实系统。

本文一共做出了三个贡献：

1）提出了一个“range-force”的方法解决了UWB和camera之间的频率差问题，让所有的UWB数据可以被使用上。
2）一个单目相机+6自由度的IMU+UWB的紧耦合优化，同时一开始是不知道UWBanchor的位置的。
3）做了大规模的实验。

II. LITERATURE REVIEW

A. UWB-Aided Localization and Mapping

在常规中使用UWB现实3D定位，需要两个条件中的一个：
1）最少4个UWB基站，同时需要直到他们在哪里。
2）三个已知的基站和机器人的高度数据。

在本文中，我们只需要一个UWB基站同时还不需要知道基站的位置，来实现融合定位。

B. Visual-Inertial-Range Localization and Mapping

提出了很多的使用UWB来定位的方法，所有的方法的核心观点都是从状态向量中的位置的观点来用公式表示残差。同时都会丢弃掉UWB中间的数据。
本文中根据IMU可以通过各个部分的递推融合进入到VIO中，那么UWB残差也是可以将时间差中的各个数据递推融合，以此来使用上所有的数据。

III. PRELIMINARIES

A. System Overview

本文中使用的是two-way-time-ok-flight UWB，这中UWB的布置不需要在多个传感器之间进行时间同步。

这个系统分为两个部分执行：
1）通过VIO来确定UWB的基站位置：【具体后面讲】这个基站的位置估计一旦测试出来是低于某个阈值之后，就会被认为是固定的了。
2）视觉惯性距离里程计：上面一步初始化成功之后，就会就会开始视觉惯性UWB紧耦合的融合优化。

然而这个系统会由于下面的原因不能够在现实中使用：
1）VIO的滑动窗口太小了。UWB基站的定位任务是依赖于很强的3D位置信息的。但是拓宽滑动窗口的大小会加大计算的负担（动态滑动窗口可以？）
2）这个优化能否得出令人满意的数据是很依赖于初始的估计的。因为这个初始化估计是典型的手动测量，可能会被任何的操作所修改。

C. Optimization-Based Monocular Visual-Inertial Odometry

和VINS-mono一样

D. IMU State Propagation

也是和VINS-MONO一样

IV. RANGE-FOCUSED FUSION OF CAMERA-IMU-UWB

A. Position-Focused vs. Range-Focused UWB Residual

1）Position-Focused：

计算误差值强制应于滑动窗口中的每一个关键位姿。k是关键帧。

2）Range-Focused UWB Residual

计算残差强制对应于每一个UWB帧，i是UWB帧。

上面的两种情况只有在UWB和camera频率相同的情况或者是不管两帧之间的UWB的情况才会相同。
由于UWB的频率是大于camera的，以前的方法都是丢弃部分的UWB帧，所以这意味着UWB的信息没有被充分利用。

B. UWB Anchor Localization Based on VIO Data

1) Problem Formulation:

为了需要估计UWB的基站位置，我们需要对这个情况进行建模。

由于IMU的频率是大于UWB的，所以可以用imu预计分得到的位置数据于UWB相匹配。基于此，可以得到下面的代价函数：
这个代价函数起始是和上面Range-Focused UWB Residual的残差是一样的，通过最小二乘来得到UWB基站的坐标。

2) Sufficient Conditions:

在实验中，估计结果将取决于覆盖所有3D轴的轨迹以及锚相对于移动半径有多远。为了让效果更加好，下面这些情况将会在优化中被开始或者跳过：
1）UAV的移动速度必须要大于某个阈值，检查飞机是否是在移动当中。
2）距离的平方需要大于某个阈值，确保每一个轴的都需要足够大。

通过执行上面这些条件，我们发现这个估计实现得非常有效果。

3) Termination Criterion:

上面的公式是使用LM算法和ceres来实现的。

max是协方差矩阵的最大特征值，p是给出的阈值。如果符合上面的条件的话，表面UWB基站的位置就确定了。由于计算特征值的和雅可比矩阵的逆非常耗时间，所以这个步骤将会在一个独立线程中执行。

C. Keyframe-Based Visual-Inertial-Range Odometry

1) Problem Formulation:

一但UWB基站的位置被确定了之后，本文就会将VIO和UWB进行融合。
方法是将VIO的残差和range的残差进行相加得到新的代价函数。其中对于UWB的数据，会对他进行加权处理$y_r$。

2) Range-Focused UWB Factor:

使用的是Range-Focused的方法，但是他的j时刻的位置信息不好处理。一般来说有上面两种方式。上面的公式可能会导致过拟合UWB和IMU的噪声数据，下面一条是用线性来替代非线性，如果对于𝜟t太长的话，会加大误差。

所以采用折中的方法。（看来外国人也很喜欢折中）

V. EXPERIMENTAL RESULTS

效果不错。