论文阅读：Range-Focused Fusion of Camera-IMU-UWB for Accurate and Drift-Reduced Localization

前言

Range-Focused Fusion of Camera-IMU-UWB for Accurate and Drift-Reduced Localization这篇文章发表于2021年的RAL上，基于VINS-mono进行改进，是基于优化紧耦合VIO融合UWB较早的成果，并且具有估计未知锚点的模块。 

一、问题背景

VIO存在累计漂移误差问题，室内等场景GPS无法作为全局传感器，VIO和UWB松组合的融合方式有机会提升，多个已知UWB锚点限制了应用。

之前的方法以类似模拟的方式处理 UWB 数据：每个相机位置都与一个范围测量配对，并且不考虑两个连续相机帧之间的任何其他范围。由于多种原因，这种观点并不能反映现实生活中的传感器系统：真实的 UWB 传感器独立于相机/IMU 传感器，因此范围/图像消息之间始终存在时间偏移； UWB测距速率不符合标准相机或IMU速率，UWB数据速率往往比相机高几倍；由于视线损失，UWB 数据速率在实际操作过程中可能会有所不同，这意味着可用的 UWB 数据量也会有所不同。

过去的锚点估计大多只是以部分优化的方式。在这项工作中探索了相机-IMU 和 UWB 的组合，在未知位置仅使用单个锚点。这种设置将结合 VIO 管道的优点，以实现准确的短期里程计和最灵活的 UWB 锚点配置。

过去VIO融合UWB的方法使用 UWB 数据有相同的基本原理：从残差在状态向量中的位置的角度来表述。(the residual is formulated from the point of view of the position in the state vector.)这种视图会导致以下问题：1) 一个位置与一个最近的 UWB 测量配对，忽略相机帧和范围数据之间的时间偏移，2) 两个连续相机帧之间的所有其他范围都被丢弃。相比之下，所提出的系统根据距离测量的时间戳制定 UWB 残差，从而允许以传感器的精度使用距离数据。

二、主要假设

1、环境中存在 单个未知UWB锚点

2、VINS-mono的基本假设

3、移动机器人配备一个单目相机、一个6 - DoF IMU和一个与身体框架刚性连接的UWB传感器，并校准所有的内在和外在参数。对放置在未知位置的单个UWB锚的距离测量是可用的。在这项工作中，采用了双向飞行时间( TW-ToF ) UWB传感器，因为它不需要传感器之间的时钟同步，因此更适合许多应用场景。

三、理论流程

1、系统整体介绍

主要分为两个阶段：UWB锚点估计初始化（利用短期VIO里程计估计未知锚点位置）、UWB-相机-惯导融合定位里程计（找到UWB锚点位置后将UWB测量融合到VIO系统中限制累计漂移误差）。理论上，这两个阶段可以合并为一个(即用UWB锚点位置扩展VIO优化的状态向量)。然而，由于多种原因，这种系统不太可能在实际中工作：（1）VIO的滑动窗口设计问题。（2）锚点位置的初始猜测问题。

VIO管道采用光束法平差公式的最小化成本函数：

IMU预积分状态传播：

2、位置聚焦和范围聚焦的UWB残差分析

聚焦位置的UWB残差：

聚焦距离的UWB残差：

两个公式在（1）UWB和相机同步（2）两个相机帧之间的所有其他额外距离测量都被忽略时相同。

3、基于VIO的UWB锚点估计初始化

优化问题建立：

j时刻载体位置估计（计算残差用）：

利用IMU或相机速率进行更新计算。

为保证优化的性能，检查以下条件以启动或跳过优化过程：

检查机器人是否在运动，因为如果机器人是静态的，新的数据将是相同的，优化结果不会改善。

检查每个轴的位置的样本方差是否足够大，即机器人的运动覆盖所有方向。

终止准则：一旦启动，代价函数( 10 )可以使用标准的Levenberg-Marquaurdt算法和Ceres求解器[进行优化。由于系统是在线执行的，因此引入终止准则来确定解的不确定性：

σmax为协方差矩阵的最大奇异值，σp为给定的阈值。

4、基于关键帧的视觉-惯性-范围融合里程计

基于滑动窗口在关键帧之间建立优化问题：

聚焦距离的UWB因子：通过IMU和运动模型进行计算：

四、实验方案

1、UWB锚点估计效果

2、里程计定位效果实验

（1）EuRoc数据集仿真实验

（2）真实数据集实验

五、创新总结

这篇文章通过利用VIO管道计算的传播数据来处理UWB测量，这种所谓的"距离聚焦"视角充分解决了UWB相机传感器之间的时间偏移，并允许所有可用的UWB数据被消耗；采用单目相机、6 - DoF IMU和单个UWB锚的紧耦合融合方案，提供减少漂移的里程计，并内置UWB锚定位模块估计未知锚位置。