前言
Distributed Initialization for Visual-Inertial-Ranging Odometry with Position-Unknown UWB Network这篇论文是发表在ICRA 2023上的一篇文章，本文提出了一种基于位置未知UWB网络的一致性视觉惯性紧耦合优化测距算法( DC-VIRO )的分布式初始化方法。
对于位置未知的UWB锚节点，我们通过求解一个机器人辅助的分布式定位算法( Robot-aided Distributed Localization，RaDL )来初始化它们的位置。
对于机器人状态估计，我们将初始化锚点的测距测量值和视觉惯性测量值融合在一个一致滤波器中。将RaDL建模为基于一致性的优化问题，并采用分布式交替方向乘子法( Distributed Alternating Direction Method of Multipliers，D-ADM )算法进行求解。
为了识别不可观测的条件，我们提出了一种基于自包含Fisher信息矩阵( Fisher Information Matrix，FIM )的准则，该准则可以由每个锚节点直接使用本地保留的测距测量值进行评估。
我们使用协方差交集( Covariance Intersection，CI )来估计初始化锚点位置的协方差，以进行一致性数据融合。
一、问题背景
问题可以分为两个阶段：
1 .机器人辅助的UWB传感器网络定位问题；
2 .带有机载视觉惯性传感器和外部初始化UWB锚点的机器人状态估计问题
主要问题：
由于缺乏全局信息，VIO方法在长轨迹上存在定位漂移问题。
当UWB锚节点众多时，集中式方法容易导致整个网络严重的通信拥塞和中心节点的高计算复杂度。
在机器人辅助分布式定位( Radl )中存在两个主要的挑战：
1 .以分布式的方式求解优化问题；
现有的方法[ 22 ] [ 23 ]使用锚点的位置来评估FIM，这需要在评估FIM之前解决计算密集型的优化问题。
2 .识别不可观测条件，避免求解器发散。
由于与测距测量相对应的历史姿态被保存在状态的长窗口中[ 5 ]，状态向量及其协方差随着UWB锚点数量的增加而膨胀，导致在应用于大规模UWB网络时面临挑战。
二、主要假设
未知位置UWB网络
基于滤波器的估计器
三、理论流程
RaLD方法
UWB网络被设计成对等结构，其中UWB锚点在簇中充当节点( Opt。图2中的节点)。UWB网络中的每个节点利用其本地保存的测距测量值(图2中的局部测距)来估计Fisher信息矩阵( FIM )的行列式，然后决定是否准备初始化(图2中的FIM基判据)。我们使用集中式的Levenberg - Marquard算法进行单锚点初始化。
VIRO估计器
如图2所示，机器人平台在多状态约束卡尔曼滤波( MSCKF )框架(图2中FEJ - VIRO)中融合视觉惯性测距测量。算法2给出了机器人平台上运行的程序，可以看作是两部分：第2行9机器人状态估计，第10行13将初始化的UWB锚点增强为CI状态。如果对应的UWB锚点没有被增强到机器人状态(线3⋅7)，则在机器人平台上放弃测距测量。
基于FIM的判据
Fisher信息是一种度量一组测量携带的关于未知状态的信息量的方法。
高斯假设下的Fisher定义：
如( 5 )所示，如果我们用Ha估计det ( F )，锚点的位置Gpa需要估计，这需要求解一个计算密集型的优化问题。因此，我们利用Cauchy - Binet公式对det ( F )进行了几何解释，它具有如下形式：
其中( rj1 × rj2) · rj3等于如图3所示的平行六面体的体积.然后我们计算了四面体的体积| VOABC |，如图3 ( b )所示，其中( rj1 × rj2) · rj3是它的六倍。逆着图3 ( b )中{ rj1，rj2，rj3 }的方向，A，B，C三点对应图3 ( a )中的{ GpIj1，GpIj2，GpIj3 } .因此我们可以利用Heron型公式和Cayley - Menger行列式计算| VOABC |：
考虑到det( [ uj1 uj2 uj3 ]⊤) = 6 | VOABC |如图3 ( b )所示，我们可以用( 6 )计算det ( F )。此外，det ( F )可以随着新接收到的测距测量值进行增量更新。假设锚节点在tk时刻接收到一个新的测距量dk，用Υ k - 1计算det ( Fk - 1 )，其中Υ k - 1包含所有收集到的测距量，直到tk - 1。将dk和测距对{ di，dj }∈Υ k - 1以及对应的机器人位姿结合起来，利用式( 8 )计算det( [ uk uj ui ]⊤)2。然后将结果与det ( Fk-1 )相加，得到当前的FIM估计值det ( Fk )。由于det ( F )量化了已有的n个测距测量值所携带的信息，当det ( F ) > τ F时，我们可以设置一个阈值τ F来初始化UWB锚点的位置。
分布式优化
我们通过求解一个非线性优化问题来初始化UWB锚节点的位置，该问题是在基于一致性的优化框架中重新制定的。求解器的初始值由文献[ 27 ]给出，如文献[ 5 ]。
我们假设一个UWB锚点在时间步长tk (见图1中的Ready UWB Anchor)满足基于FIM的准则，并且存在n - 1个初始化的UWB锚点(见初始化UWB锚点图1)。对于这些n个UWB锚点，我们有det ( Fi ) > τF ( i = 1,2 , · · · , n)。优化问题：
在[ 19 ]的基础上，我们用分布式交替方向乘子法( D-ADMM )求解( 10 ) .在D - ADMM算法中，我们将一个对偶变量λ ij与每个约束xci = xcj( i , j = 1 , · · · , n)相关联。每个Opt。图2中的节点保留一个局部决策估计xci和一个对偶变量的向量λ ji，其中j < i .每个Opt。图2中的节点交替更新本地状态和对偶变量：
节点由UWB锚节点的ID进行排序。在每次迭代中，选择.节点更新分两步进行。首先，他们用利文贝格-马夸德( LM )求解( 12 )，以更新局部决策估计，并进一步用( 13 )更新对偶变量。其次，他们将新的决策估计值xci，k + 1和新的对偶变量λ k + 1ji发送给他们的后继者进行下一次迭代.
一致性VIRO滤波器
在这一部分中详细描述了在机器人上运行的基于滤波器的第一估计雅克比视觉惯性测距( FEJVIRO )，它在MSCKF框架中融合了UWB、相机和IMU的测量。我们只介绍[ 5 ]中的不同部分，读者可以参考我们先前的工作[ 5 ]来获得更多的细节。
[5]S. Jia, Y. Jiao, Z. Zhang, R. Xiong, and Y. Wang, "Fej-viro: A consistent first-estimate jacobian visual-inertial-ranging odometry," arXiv preprint arXiv:2207.08214, 2022.
向量情况：
距离测量及其雅可比：
带CI的协方差
尽管带CI的协方差恢复运行在新初始化的UWB锚点上，但我们在这个模块中描述它，因为它的目标是在机器人状态中增加初始化的锚点位置时保持VIRO滤波器的一致性。通过恢复锚点位置的协方差Paa的上界来保证一致性。
锚点协方差：
注意到Ω = { P1 · · · PM }是Pxx的6 × 6对角子矩阵。由( 24 )式，我们需要Pxx的上界来计算Paa的上界。根据协方差交集理论[ 25 ]，Pxx的上界由下式给出
通过上述两式可以得到锚点协方差的上界。如果{ G ( pai，( Paa ) }在机器人状态中没有被增广，我们只将其增广到状态和协方差。如果状态已经包含了它的估计位置Gpai，s和协方差Paa，s，我们用CI融合现有的值和新的估计值：
四、实验方案
基于OpenVINS [ 1 ]实现了Consistent VIRO Filter它是目前最先进的基于滤波器的视觉惯性估计器。基于机器人操作系统( Robot Operating System，ROS )实现了分布式UWB初始化方法。
仿真实验
真实环境实验
五、创新总结
方法：
第一阶段：
对于第一个挑战，我们通过共识形成( [ 18 ] )来重新表述集中式问题，并使用一种流行的称为分布式ADMM [ 19 ] [ 20 ]的对偶方法来解决。
对于第二个挑战，传感器位置的可观测性通常由Fisher信息矩阵( Fisher Information Matrix，FIM )进行分析。通过利用几何解释，我们提出了一种增量方法，直接利用锚点的局部测量来评估FIM的行列式，从而产生基于FIM的在线准则启动优化。
第二阶段：
在这项工作中，我们用协方差交叉( CI )恢复优化锚点的协方差[ 24 ] [ 25 ]，这是一致融合和协方差恢复的有用技术[ 26 ]。我们将机器人关键帧的位姿和小的边缘协方差广播到UWB网络中，然后用CI计算全协方差的上界。因此，每个锚点可以在优化后恢复其协方差，然后将位置和协方差传输给机器人以进行一致的状态增强。在机器人状态中增加一个锚点i后，机器人通过机器人与锚点i之间的距离测量来更新状态。
综上所述，本文的主要贡献如下：
·提出了一种基于位置未知UWB网络的视觉-惯性-测距一致性里程计分布式初始化方法DC - VIRO；
·将UWB初始化问题重新纳入一致性优化框架，并采用D - ADMM算法进行求解。我们还提出了一种基于FIM的准则来识别不可观测的情况，该准则是自容式的，并且只需要局部测距测量。
·我们使用CI技术有效地恢复了锚点位置的协方差。在机器人状态下，将恢复的协方差和优化的锚点位置进行增广，从而得到准确和一致的VIRO滤波器。
本文提出了一个分布式系统，它将来自相机、IMU和UWB的测量进行一致融合，以估计机器人的位姿和UWB锚点的位置。机器人平台和所有装有机载计算机的UWB锚点都是集群中的计算节点，它们之间只传输状态值和边缘协方差。机器人运行一个轻量级且一致的VIRO滤波器来估计机器人位姿。而UWB网络一旦满足基于FIM的准则，就用D - ADMM估计锚节点的位置。将估计的锚点位置发送给机器人，并与CI技术一致地增强到VIRO滤波器中。在未来的工作中，将尝试将DC - VIRO扩展到多机器人系统。