论文阅读：UVIO: An UWB-Aided Visual-Inertial Odometry Framework with Bias-Compensated Anchors Initial

前言

今天再次仔细读了UVIO: An UWB-Aided Visual-Inertial Odometry Framework with Bias-Compensated Anchors Initialization这篇论文，这是23年8月份的论文，researchgate里说发表在2023IROS上，主要工作是修改openvins代码利用超宽带技术和视觉惯性里程计提供鲁棒低漂移定位，提出了一种多步初始化程序。

一、问题背景

卫星拒止环境下定位问题，UWB的引入解决VIO长期累积漂移的问题，对于UWB来说多个未知锚点位置确定是重要的初始化问题。UWB锚节点的初始化是通过求解线性最小二乘问题一步完成的，这会导致不准确或完全错误的初始化结果。迄今为止的所有研究都显示了将UWB技术用于定位的潜力，特别是在VIO导航系统中抑制漂移的潜力。然而，上述研究均未考虑锚节点初始化的求解高度依赖于各种因素，包括锚节点部署的几何形状以及收集初始化数据的无人机或移动机器人的运动轨迹，进而影响状态估计的精度。

过去在UWB锚点初始化上的工作成果如下所示：

在文献[ 11 ]中，作者提出了一种基于扩展卡尔曼滤波( EKF )的多传感器融合框架来联合估计锚和无人机的位置。UWB锚节点的初始化是通过求解线性最小二乘问题一步完成的，这会导致不准确或完全错误的初始化结果。

[ 12 ]提出了一种利用低成本IMU的测距测量和读数进行UWB锚自定位的精确且易于使用的方法。通过自由移动标签来估计锚点的位置，使用紧耦合的误差状态卡尔曼滤波器( ESKF )来融合UWB和惯性测量。在不考虑测量偏差的情况下，首先通过迭代最小二乘法初始化锚点。

在文献[ 13 ]中，提出了一种基于主动锚节点的超宽带定位系统的自校准算法。该算法使用迭代梯度下降和卷积神经网络的误差检测来估计固定锚节点的位置，即使在非视距( NLOS )条件下也具有较高的精度，但需要锚节点之间交换距离信息。

在[ 14 ]中的研究通过将单个UWB锚点纳入定位过程，解决了VIO在室内定位中的漂移问题。锚点的初始化是通过求解一个非线性最小二乘问题来完成的，该问题需要对必须提供的锚点位置进行初始猜测，并且先验已知

Nguyen等人在[ 15 ]和[ 16 ]中的研究提出了两种紧耦合的里程计框架，将单目视觉特征观测与单个UWB锚点提供的距离测量相结合。第一种框架使用利文贝格-马夸特非线性优化算法的变体来同时估计尺度因子和锚点位置，而第二种框架通过解决每个距离数据的时间偏移并利用所有可用的测量值来更有效地利用UWB测量值。随后，在[ 17 ]和[ 18 ]中，作者提出了一种基于图优化的无人机多传感器同时定位与地图构建( Simultaneous Localization and Mapping，SLAM )方法。该系统融合了多种传感器的数据，包括IMU，相机，激光雷达和UWB距离测量来进行估计。

最近，Jia等人[ 19 ]提出了一种视觉惯性测距( VIRO )方法，通过结合至少3个锚点的UWB测距测量来减少VIO系统中的定位漂移。该方法利用长短时间窗口结构来初始化锚点的位置和协方差。论文还分析了UWB锚节点位置未知时VIRO系统的可观测性，指出理想情况下存在4个不可观测方向，并通过多状态约束卡尔曼滤波( Multi-State Constraint Kalman Filter，MSCKF )融合UWB量测数据降低了不可观测方向。

在最近的一项研究[ 20 ]中，作者应用FIM的原理来确定指定航班量内的最优点，以收集用于初始化单个UWB锚的位置及其测量偏差的数据。该方法包括使用随机车辆位置的粗三角剖分，然后使用FIM优化的点进行精细三角剖分，为单个锚提供最大的信息。

[11] K. Hausman, S. Weiss, R. Brockers, L. Matthies, and G. S. Sukhatme, “Self-calibrating multi-sensor fusion with probabilistic measurement validation for seamless sensor switching on a uav,” in 2016 IEEE International Conference on Robotics and Automation (ICRA), 2016, pp. 4289–4296.

[12] Q. Shi, S. Zhao, X. Cui, M. Lu, and M. Jia, “Anchor self-localization algorithm based on uwb ranging and inertial measurements,” Tsinghua Science and Technology, vol. 24, no. 6, pp. 728–737, 2019.

[13] M. Ridolfi, J. Fontaine, B. Van Herbruggen, W. Joseph, J. Hoebeke, and E. De Poorter, “Uwb anchor nodes self-calibration in nlos conditions: a machine learning and adaptive phy error correction approach,” Wireless Networks, vol. 27, 05 2021.

[14] B. Gao, B. Lian, D. Wang, and C. Tang, “Low drift visual inertial odometry with uwb aided for indoor localization,” IET Communications, vol. 16, no. 10, pp. 1083–1093, 2022.

[15] T. H. Nguyen, T.-M. Nguyen, and L. Xie, “Tightly-coupled singleanchor ultra-wideband-aided monocular visual odometry system,” in 2020 IEEE International Conference on Robotics and Automation (ICRA), 2020, pp. 665–671.

[16] ——, “Range-focused fusion of camera-imu-uwb for accurate and drift-reduced localization,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 1678–1685, 2021.

[17] T.-M. Nguyen, S. Yuan, M. Cao, T. H. Nguyen, and L. Xie, “Viral slam: Tightly coupled camera-imu-uwb-lidar slam,” 2021.

[18] T.-M. Nguyen, M. Cao, S. Yuan, Y. Lyu, T. H. Nguyen, and L. Xie, “Viral-fusion: A visual-inertial-ranging-lidar sensor fusion approach,” IEEE Transactions on Robotics, vol. 38, no. 2, pp. 958–977, 2022.

[19] S. Jia, Y. Jiao, Z. Zhang, R. Xiong, and Y. Wang, “Fej-viro: A consistent first-estimate jacobian visual-inertial-ranging odometry,” in 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2022, pp. 1336–1343.

[20] J. Blueml, A. Fornasier, and S. Weiss, “Bias compensated uwb anchor initialization using information-theoretic supported triangulation points,” in 2021 IEEE International Conference on Robotics and Automation (ICRA), 2021, pp. 5490–5496.

二、主要假设

固定静止锚点假设。

假设机器人的姿态(通过VIO)，以及UWB的距离测量值对系统是可用的。

三、理论流程

1、UWB辅助的视觉惯性里程计滤波器设计

（1）状态向量

状态向量如下

依次为

a、IMU坐标系在全局坐标系中的位置速度方向以及零偏

b、复制的不同时刻的IMU位姿

c、包括N个特征在全局框架中的位置

d、包括每个相机presen的相机内参和相机外参

e、包括UWB外参数，即UWB标签相对于IMU框架的位置，M个锚点在全局框架中的位置，以及每个锚点的偏置参数。

（2）锚点固定策略

在所提出的滤波器中，每个UWB锚点可以视为固定的，即它们的值保持不变，或者可以在线修正，从而添加到UWB状态中。通过固定两个锚点，我们可以设置一个无漂移的全局框架(事实上,非线性可观测性分析表明,将一个锚固定在位置上,朝向另一个锚就足够了;为了简化结果部分的实现,将2个锚固定在位置上)。

（3）UWB测距模型与雅可比

UWB测距模型：

分别是一个乘性标量，充当一个距离依赖的偏置项，和一个常数偏置项。

距离测量的雅可比为：

（4）时间对齐

与[ 19 ]不同的是，我们没有插值距离测量以使其与相机测量对齐，但是，为了避免插值引入额外的误差源，我们执行了我们称之为延迟更新的方法。如图所示，这种更新策略包括在时间tn的最近一次更新和时间tk的下一次相机图像之间收集距离测量。在tk时刻接收到新的相机图像后，进行一系列从tn时刻到tk时刻的链式传播更新。

2、UWB锚点初始化

首先，从一个短的随机或标准轨迹中收集数据，通过求解线性最小二乘问题得到一个粗略的初始解。随后，在相同数据上建立一个非线性优化问题，并对其进行求解，以细化第一层粗定位。然后，利用得到的锚点位置估计计算一组路点，优化我们提出的GDOP度量，以最大化所有参与的UWB锚点的三角定位信息。然后将这些最优航路点的数据用于一个非线性优化问题，以同时优化所有UWB锚点的位置。

（1）粗锚定位：最优双法

最小二乘建模实际中经常为病态的，在这方面，[ 11 ]介绍了一个测量包含检验，其中测量被添加到最小二乘问题中只有当问题的条件数通过添加这样的测量而减少。在这项工作中，我们用所谓的最优二重方法重新表述了[ 20 ]中描述的最小二乘问题，旨在增加最小二乘问题的条件数和最小化解的不确定性。

我们对锚点粗初始化问题的最小二乘形式的条件化进行改进的第一种选择是在测量模型中考虑一个已知的β = 1。因此，对于锚点A的距离测量z，有

在忽略测量噪声的情况下，我们可以将之前的方程平方化，并将其改写为如下形式：

应用双方法[ 21 ]，可以得到关于Gp A和γ的线性关系。更准确地说，考虑zi和zj是来自同一锚点A的两个不同的(不一定连续)范围测量，而GpU i和GpU j是采集测量时的UWB标签位置。将两式相减可得：

最小二乘建模为：

为了识别对zi和zj，应该确定一个从任何其他测量中减去的最佳枢轴测量，以使解决方案的不确定性最小化。为此，令nk⋅N 0，σ2n，为与锚点A的第k个距离测量相关的噪声，εk⋅N ( 0 , Σ ε)为与UWB标签的第k个位置相关的噪声。在接下来的分析中，我们只考虑向量b是一个随机实体，而将矩阵A视为确定性的。尽管这是一个近似，但它使分析变得更容易，从而为最佳支点的选择提供了一个有意义的标准。

最小二乘的信息矩阵为：

（2）非线性细化

我们的锚点初始化程序的下一步是基于利文贝格-马夸特( LM )算法的第一个粗解的非线性精化，在第一个随机轨迹期间收集的相同数据上执行。特别地，我们寻求最小化下面的成本函数：

与线性求解只估计每个锚点的常值偏差γ不同，在非线性优化问题中，我们包含了每个锚点的距离依赖偏差β。距离偏差的初始猜测值设定为1。

最终解的估计参数协方差矩阵为：

（3）最优航路点

尽管上面介绍了最优的双重方法，但使用在随机轨迹期间收集的测距测量来估计UWB锚点的位置具有明显的局限性。锚点初始化的轨迹关于信息的最优性可以基于不同的测度进行评估。在[ 20 ]中，锚的位置的第一个估计是通过收集通过一组通过Fisher信息矩阵最大化计算的航路点的数据来精化的。在这项研究中，GDOP [ 21 ]被用作同时估计多个锚点位置的最优性度量。最小化UWB映射不确定性的轨迹合成如下。令H为非线性测距量余弦方向的雅克比

分别为航路点位置相对于锚点的方位角和俯仰角。

现在考虑至少四个目标位置，我们可以计算如下的GDOP：

最大化式( 8 )等价于最小化估计不确定椭球的体积，该体积仅是航路点相对于UWB锚点位置的方位角和俯仰角的函数，因此将其作为优化参数。尽管如此，还是要考虑多重锚。此外，对于航路点的操作搜索卷可以有任意形状的( UWB锚的位置可能在无人机的业务量之外)。因此，航路点的确定成为一个具有挑战性的优化问题，特别是在非结构化环境中。此外，与[ 20 ]中对每个锚点单独优化多条轨迹不同，我们感兴趣的是生成一条同时最小化所有UWB锚点估计精度的最优轨迹。为此，我们不考虑角度，而是直接将航路点位置作为下面优化问题的参数：

为了解决这个问题，我们使用[ 23 ]中应用的基于网格的进化算法。该算法首先将无人机的操作体积看作一个高度为h，宽度为w，长度为l的正方体，np个初始航路点(即q ( 1 , -)，..，q( np , -) ) (见图2)。立方体体积被划分为np = nhp × nlp × nwp小立方体，其中nhp、nlp和nwp分别表示沿Z (高度)、X (长度)和Y (宽度)轴的坐标变化数。每个小立方体是一个3D网格，用G1 ..表示。Gnp，它定义了对应路点的所有可能位置。进化Alg . 1将每条染色体定义为一个3 × np的矩阵，该矩阵中的基因沿样本格点的3个坐标轴表示对应的航路点。每条染色体表示离散空间(见图3)中路点集合的一个特定配置。

[23] S. Yang, M. Li, X. Liu, and J. Zheng, “A grid-based evolutionary algorithm for many-objective optimization,” IEEE Transactions on Evolutionary Computation, vol. 17, no. 5, pp. 721–736, 2013.

在此，我们报告了应用基于网格的进化算法的结果。如下图所示。

在获得最优航路点集合后，无人机可以飞过这些航路点并收集数据，然后在Sec中解释的非线性求精中再次使用这些数据。在最后一步中，我们最好利用前面计算出的最优航路点所包含的信息，这样就可以同时对距离内的所有UWB锚点进行精确初始化。

四、实验方案

实验部分在上一节最有航路点中有一些模拟仿真实验，此外还有如下。

1、仿真实验

在MATLAB中对初始化过程进行了仿真和广泛的测试，并对不同锚配置、无人机轨迹、参数值和噪声水平的多个场景进行了全面的探索。特别地，在图6中，我们比较了以下每一步初始化后的解：初始粗最小二乘解( LS )，相同初始化数据上的非线性最小二乘解( NLS )，通过随机路径点对非线性最小二乘解进行精化( Rnd-WPS )，以及通过最优路径点对非线性最小二乘解进行精化( Opt-WPS )。需要强调的是，相对于前两种方法，随机和最优航路点精化是在"新"数据上进行的。为了比较的公平性，我们考虑所有方法的样本量相同。在所有情况下，与其他测试方法相比，通过最优路径点的优化过程表现出优异的性能，显著改善了解的方差和平均误差的最小化。这一结果强调了结合信息论支持的基于优化的方法来改进锚映射的初始解的有效性，即使在具有挑战性的锚配置的情况下，也提高了初始化过程的可靠性。

2、真实场景实验

扩展OpenVINS编写UVIO。采用文献[ 24 ]中介绍的系统自治框架，使无人机能够在给定的飞行体积内通过航路点自主飞行。

对于第一组实验，我们将无人机部署在飞行体积的中心，并执行Sec描述的初始化过程。Ⅳ.为了评估所提出方法的准确性，我们使用无人机的地面真实位置来执行初始化。我们对锚点进行了两种配置下的3次实验，对应的位置误差如表所示。与仿真实验相比，误差的增加是由UWB测量模型的非理想性解释的，该模型在初始化阶段假设偏差为常值，而在现实中干扰、反射和其他不需要的物理现象可能会改变它们的值。此外，实验结果表明，所提出的方法的准确性受锚相对于飞行体积的位置的影响。特别地，位于体积之外和角落的锚点会导致更高的初始化误差。另一方面，放置在有利位置的锚点，例如靠近飞行体积的中心，会产生更低的初始化误差。与文献[ 20 ]的结果比较表明，所提出的方法是优越的，因为它同时考虑了多个锚点，并且能够对单个锚点实现可比的甚至更低的初始化误差，例如图7中的锚点19333。

为了评估UVIO的实时性能，进行了第二组实验。此时，初始化是考虑VIO框架估计的位姿进行的，并随着无人机在位悬停而停止。随后进行了持续时间大致相同的手动飞行，与通过航路点的自动飞行相比，允许更灵活的机动。对于轨迹的评估，我们选择了绝对轨迹误差( ATE )作为评估UVIO性能的指标[ 25 ]。在所有的实验中，我们注意到使用UVIO比只使用VIO在定位性能上有整体的提高。这体现在ATE均方根误差( RMSE )的降低(图8 )。为了进一步测试和验证我们的框架的可靠性，与纯VIO相比，我们模拟了严重的帧率下降和相机的完全丢失(图9 )。这些实验的结果证明了UVIO在视觉条件差或相机故障的情况下也是有效的，在这些情况下，纯VIO会失效。

五、创新总结

我们的方法扩展和改进了[ 20 ]中关于信息驱动的UWB锚初始化和使用的想法，通过开发一种完全自动化的初始化方法来同时初始化和随后使用具有测量偏差的多个初始未知的UWB锚。我们利用GDOP作为一种新的度量标准来指导可扩展UWB锚点集的数据收集、初始化和不确定性降低的优化。更确切地说，我们的贡献是：

在视觉- UWB支持的估计方法中，完全自动化地检测、初始化和使用初始未知的UWB锚点。

新的最小二乘公式，被称为最佳双方法( Sec。Ⅳ- A)，增加了矩阵条件数，以改进粗锚点初始化。

基于GDOP度量在任意形状的区域中生成最佳航路点，以实现精确和同时初始化多个UWB锚点的最佳数据收集。

通过一个自主飞行的无人机(和代码的开源)，在仿真和真实实验中验证了所提方法的有效性。

在真实世界中验证了我们的方法对相机丢失的弹性。

这篇论文解决的也是UWB-VIO系统的初始化问题，重点在精准估计UWB锚点上，随机的轨迹进行UWB锚点估计之前很多成果分析过观测性问题，最优化出一条精准轨迹可以避免这个问题并且提高估计的精度。本文没有卷UWB-VIO的定位精度，采用滤波方法进行后端优化更注重效率，重点是本文提出的未知锚点初始化方法是创新之处，是核心的地方。