前言
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锚定位模块估计未知锚位置。