论文阅读：Bias Compensated UWB Anchor Initialization using Information-Theoretic Supported Triangulation

前言

Bias Compensated UWB Anchor Initialization using Information-Theoretic Supported Triangulation Points是发表在2021ICRA上的一篇文章，主要是UWB和IMU的融合，提出一种可以进行精确的UWB锚点位置估计的方法。

一、问题背景

但在没有GNSS信号可用的区域，如森林或室内位置，需要有其他形式的定位提供者。类似于GNSS卫星对移动机器人的精确估计，UWB锚点的位置必须尽可能精确已知。为了减小定位误差，锚节点的位置必须尽可能精确已知。通常通过测量UWB锚点的精确位置来手动进行校准，但由于这是一个容易出错且耗时的过程，在某些场景中也不适合，我们希望尽可能避免这种情况。本文目标是在一个房间内随机放置锚点，移动机器人，在我们的案例中，无人机，自动初始化锚点位置。此外，针对UWB模块的距离依赖误差，计算了带有恒定偏移项的线性偏置模型。

关于UWB锚点校准(或初始化)定位系统方面有大量的工作。

Hol等人[ 5 ]提出了一种用于室内定位的UWB接收机校准方法。首先将多个UWB接收机放置在静止的地方。在接收器附近放置相同数量的发射机。他们获得了这个配置的数据集。在这个数据集上进行了非线性优化。然后在接收器周围移动一个发射机，并记录另一个数据集。以第一次优化得到的位置作为初始值，在第二个数据集上进行第二次非线性优化

[6]目标是提供一种动态锚点设置的初始化方法。在移动标签和锚节点之间只进行距离测量。所提出的方法显然对多路径传播非常鲁棒，因为在位置候选项被Unscented卡尔曼滤波器( UKF )进一步精化之前使用了基于RANSAC的离群点剔除。

另一种自动校准UWB锚点的方法是利用接收机的距离信息来估计锚点的位置。

[5] Jeroen D Hol, Thomas B Sch ̈ on, and Fredrik Gustafsson. Ultrawideband calibration for indoor positioning. In Ultra-Wideband (ICUWB), 2010 IEEE International Conference on, volume 2, pages 1–4. IEEE, 2010.

[6] Byungjae Park and Sejin Lee. Robust range-only beacon mapping in multipath environments. ETRI Journal, 42(1):108–117, 2020.

[7] Roberta Cardinali, Luca De Nardis, Maria Gabriella Di Benedetto, and Pierfrancesco Lombarde. Uwb ranging accuracy in high- and low-data-rate applications. IEEE Transactions on Microwave Theory and Techniques, 54(4):1865–1875, 4 2006.

[8] David Moreno-Salinas, Antonio M Pascoal, and Joaquin Aranda. Optimal sensor placement for multiple target positioning with range-only measurements in two-dimensional scenarios. Sensors, 13(8):1067410710, 2013.

二、主要假设

多个固定锚点随机分布在室内。

载体为无人机。

UWB和IMU进行组合。

在整个初始化过程中，计算UWB锚点在3D参考帧中的位置，我们假设移动机器人，在我们的情况下，无人机，可以通过其他传感器模式(例如,基于视觉的,带有GNSS信号的,激光等)在3D参考帧中估计自己的位姿。

三、理论流程

主要是Uwb anchor初始化过程。

1、粗略的初始位置计算

对于这种粗略的初始化，将无人机飞行到随机位置，同时收集从车辆上的UWB节点到想要初始化的锚点的距离测量值。无人机上的节点到锚节点的距离可以表示为：

p是载体UWB接收机三维全局位置，q是待估计UWB锚点三维全局位置，z为载体UWB接收机到UWB锚点的距离，dp是载体UWB接收机在全局坐标系中距离原点距离，dq是UWB锚点在全局坐标系中距离原点距离。

假设已知载体节点的位置和在[ 1 ]中做的没有偏差，对于每个节点和锚节点之间的距离测量，我们可以制定一个修改的最小二乘问题为

它是一组线性方程，其形式为Ax = b，其中A的行是时刻ti的测量值。

尽管UWB传感器被认为对多路径问题具有相当的鲁棒性，但它们在实际中表现出不可忽略的距离依赖偏差和依赖于制造商的恒定偏移量。为了提高三角剖分结果的精度，我们对上述的距离模型进行了扩展。用一个与距离有关的偏差β和一个恒定的偏移量γ来更好地反映实际测量的距离：

沿用文献[ 1 ]的思路，我们设计了两个额外的辅助元素和γ，并对上式中的距离项进行了修正。

解此Ax = b形式的线性方程组，即可解出锚点位置q和两个偏差项β和γ。式中基于随机选择的无人机位置。在实际应用中，这个方程组通常不是适定的，会产生很差的解。尽管如此，粗略的方向和距离可以作为一个初始猜测，以应用我们的信息论方法在下面的细化步骤中进行最佳无人机位置选择。

[1] 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), pages 4289–4296, May 2016.

2、基于FIM的最优点计算

目标是找到无人机放置在有限体积中的最佳位置，以便在全局坐标系中对一个固定的UWB锚进行最佳三角剖分。在这个锚点被三角化之前，我们假设无人机的位置在一个有界体积内已知。

（1）FIM理论基础

为了找到最优的传感器布局，相应的克拉美罗下界( CramerRao Lower Bound，CRLB )或FIM被考虑[ 9 ]。CRLB表示了一个确定性参数的估计量的方差的一个下界。通过实现这个界，无偏估计量被称为(完全)有效的。另一方面，FIM从获得的未知参数的测量数据中捕捉信息量，并对其进行估计。在正则性条件下，任何无偏估计量的方差至少高达FIM的逆，并且下面的不等式成立：

其中，θ为估计问题的变量，且

Cov { ( θ ) }对应于估计参数的协方差矩阵。下面定义FIM ( θ ) (简写为FIM)为:

其中，表示对数似然函数关于未知参数θ的梯度。通过选择合适的估计量，使CRLB最小化或FIM最大化，可以减少估计参数时的不确定性。

（2）UWB锚点初始化的FIM

引入偏置和噪声后的距离测量模型：公式（9）

为了得到Fisher信息矩阵，必须进行计算：

对于一般的高斯噪声，也有Fisher信息矩阵的一般表达式[ 10 ]。对于UWB模块的估计，该表达式为：

用指数k和l分别表示x，y，z三个坐标轴。值得注意的是，随着我们扩展到使用一个远程依赖的偏差项，每个测量的协方差矩阵Ci依赖于锚点位置q。因此，方程中的第二项需要考虑为非零项。

（3）最优性准则

费希尔信息矩阵有几个最优性准则来最大化收集到的信息。其中一些在[ 11 ]中描述：

本文选用D-optimum，它使给定模型的待估参数的多维不确定性椭球体积最小。

A-optimum使CRLB的迹最小，从而使估计值的平均方差最小。

E-optimum最大化Fisher信息矩阵的最小特征值，这意味着不确定性椭球的最大轴的长度得到最小化。

D-optimum的主要优点是在参数上具有尺度不变性，对线性变换也具有不变性D-optimum的缺点是如果没有找到全局最优，得到的D-optimum可能是错误的。这是由于不确定性椭球在一个维度上可以达到最小化，而在另一个维度上我们根本没有信息。也就是说，不确定度椭球在正方向上很小，而在其他方向上很大。然而，由于我们对无人机的计算限制以及D-optimum不需要计算矩阵逆的优点，它仍然是我们最喜欢的选择。

在一定的假设条件下，FIM行列式的最大化问题可以解析求解。[12][13]

作为演示我们方法功能的一个实例，在图2中，我们假设无人机只允许在1 × 1 × 1m的体积中移动，并且我们希望通过将无人机飞行到五个位置来实现最佳的UWB锚点位置初始化。此外，我们假设协方差矩阵是距离依赖的。待估计UWB锚节点的真实位置设置为[ 1 . 5,1 . 0]T m。直观地说，无人机飞行到允许体积内的最佳位置是在距离锚点最近的立方体的角落。

这个例子还强调了体积中最优点的选择对UWB锚位置的低敏感性：锚相对于体积的初始方向和距离已经足够收敛到图2中所描述的结果。或者换句话说，要使最优点被放置在图2所描绘的不同位置，真正的锚点位置需要大幅度地改变。此外，在距离无关的协方差矩阵上，[ 14 ]提出将FIM变换到球坐标系下，强调FIM依赖于距离矢量之间的夹角。加入距离依赖元素实质上又增加了"越近越好"的要求- -不需要非常精确的锚点初始位置信息。这种关于锚点的最佳点位置的低敏感性有利于我们的粗初始化仍然足够精确，以生成用于后续锚点位置精化的信息点。

3、距离相关和位置相关协方差

在实际中，考虑到利用无人机位置对锚点位置进行后期三边测量需要基线，距离越近的测量点相关性越强。这必须在FIM的协方差矩阵中加以考虑。对于相关协方差矩阵，采用平方指数协方差。按元素定义如下：

长度尺度L表示函数的光滑性。大的长度尺度值表征的是变化缓慢的函数，小的长度尺度值表征的是变化迅速的函数。通过结合基于距离的协方差矩阵和相关协方差矩阵得到

4、细化锚点定位和偏差计算

利用所提出的D-optimumFIM优化器中协方差矩阵的这一定义，我们将无人机位置相关性考虑在内，并可以确保在定义的体积内有很好的测量点间隔。一旦在体积中定义了最佳位置，我们将重新求解公式5，用于锚点位置的精化，同时求解偏差项。之后将锚点位置和偏置项用于无人机的基于UWB -惯性的闭环紧耦合控制中。

四、实验方案

1、模拟实验

我们利用公式( 9 )中的距离依赖偏差模型对不同位置的UWB距离测量进行了仿真。图3显示了结果。我们注意到，我们的偏差补偿显著改善了结果。

图4展示了完整的初始化过程，显示了锚点的真实位置(红色三角形)，随机选择的初始三角点(绿色' x ')以及使用这些位置得到的粗略的初始锚点估计，利用公式（5）。 (绿色三角形)，在随后选择的体积内选择信息论最优三角剖分位置(蓝色' x ')，并基于这些最优位置再次使用公式（5）进行锚点位置的精化估计。 (蓝色三角形)。作为对我们建议的偏差补偿效果的比较和演示，该图还显示了使用公式（3）不建模偏差(黑色三角形)的最优点而不是公式（5）的三角锚位置。

2、真实环境实验

我们进一步进行了一系列真实实验，以证明我们的方法在真实硬件上的使用，甚至用于后续的无人机的UWB -惯性闭环控制。对于所有的真实实验，我们使用了Asctec蜂鸟四旋翼飞行器(图1 )，配备了飞行计算机( Odroid XU4 )和UWB模块( DecaWave TREK1000 )。此外，环境中任意放置3个UWB模块( Deca Wave TREK1000 )。UWB测距的标准差为0.09 m。我们使用Optitrack运动捕捉系统来获取我们所有过程步骤的无人机位置。我们将我们的真实世界结果与文献[ 1 ]中的结果进行了比较，在文献[ 1 ]中，作者在随机轨迹上移动一架无人机，以增加距离测量，只要他们改进了方程中矩阵的条件数。3由先前的测量组成。将新的测量值累加至最大测量次数或直到矩阵条件数达到一定质量为止。

在第一个实验中，我们按照文献[ 1 ]的方法进行了120次初始化。图5展示了我们的方法在考虑偏差情况下的初始化结果。

此外，还进行了两个实验，一个是悬停实验，另一个是基于本文方法初始化的锚点位置的紧耦合UWB -惯性EKF的轨迹跟踪实验。

地面真值由我们的Optitrack系统获取。在18个航点飞行20次的情况下，轨迹跟随的平均跟踪误差为0.19 m，标准差为0.0997 m (图6 )。

在悬停测试中，无人机被送到1m的高度，并在那里悬停60秒。我们在无人机上使用了五种不同的位姿估计器进行闭环控制。表1表示所有设置的均方根误差。对于[ 1 ]中提出的方法，利用我们的最佳调谐知识，我们仍然得到了0.713 m的RMSE。不幸的是，文献[ 1 ]中的作者在他们的工作中没有报告基于UWB -惯性估计的导航性能。有趣的是，尽管我们的方法改进了UWB定位和偏差补偿，但所有其他基于UWB的装置都表现出类似的性能。即使使用Optitrack测量，RMSE也超过1cm，我们假设这是由于无人机阴影估计精度的低控制器性能造成的。

五、创新总结

所提出的初始化是一个两阶段过程。首先，无人机导航到空间中的一些随机点，并在每个点记录一个对UWB锚点的测量值，并初始化UWB锚点的位置。在达到足够的点后，执行位置的初始猜测。我们利用和扩展了文献[ 1 ]中提出的方法，用一种改进的最小二乘法来包含偏差项。计算的位置和相应的协方差矩阵用于计算合适的边界体积，该体积用于使用Fisher信息矩阵( FIM )构建最优点。在这些最优点处，最大化距离相关UWB测量的可用信息，以获得最佳的三边测量结果。利用移动机器人在这些最优点处获得的信息，采用与随机点三边测量法相同的修正最小二乘算法计算UWB锚点位置的最终估计值，包括线性距离相关偏差和常值偏移。

相对于现有技术，我们改进了锚点在3D中的位置初始化，并包括信号偏置项，以进一步改进使用UWB锚点作为定位系统的移动系统的后续状态估计器。特别地，我们的贡献如下：

• 从2D到3D空间的扩展和基于FIM / CRLB的传感器优化布置方法的翻转[ 8 ]用于距离传感模块。
• FIM / CRLB定义问题集的扩展协方差模型，包括距离依赖、偏置项和测量位置之间的相关性。
• 扩展以较低的计算复杂度实时初始化几个UWB锚点，并在没有任何先验知识的情况下改进模型，包括距离依赖的偏置项和偏置项。
• 基于验证的仿真和真实的真实实验进行详细评估，包括与现有方法的比较(和改进)；
• 评估3个UWB锚用于无人机机载实时UWB -惯性定位控制时，锚的初始化精度对导航精度的影响。

在本文中，我们解决了在没有先验知识的情况下准确的UWB锚点初始化问题，使用FIM进行信息优化的三角定位选择，并使用与距离相关的偏差模型对UWB测量数据进行三角定位，以提高最终的三角定位精度。