个人在国外网站做电商,改版网站会影响百度,合肥网站排名优化公司,东莞app软件开发解决方案前言
Distributed Initialization for Visual-Inertial-Ranging Odometry with Position-Unknown UWB Network这篇论文是发表在ICRA 2023上的一篇文章#xff0c;本文提出了一种基于位置未知UWB网络的一致性视觉惯性紧耦合优化测距算法( DC-VIRO )的分布式初始化方法。
对于…前言
Distributed Initialization for Visual-Inertial-Ranging Odometry with Position-Unknown UWB Network这篇论文是发表在ICRA 2023上的一篇文章本文提出了一种基于位置未知UWB网络的一致性视觉惯性紧耦合优化测距算法( DC-VIRO )的分布式初始化方法。
对于位置未知的UWB锚节点我们通过求解一个机器人辅助的分布式定位算法( Robot-aided Distributed LocalizationRaDL )来初始化它们的位置。
对于机器人状态估计我们将初始化锚点的测距测量值和视觉惯性测量值融合在一个一致滤波器中。将RaDL建模为基于一致性的优化问题并采用分布式交替方向乘子法( Distributed Alternating Direction Method of MultipliersD-ADM )算法进行求解。
为了识别不可观测的条件我们提出了一种基于自包含Fisher信息矩阵( Fisher Information MatrixFIM )的准则该准则可以由每个锚节点直接使用本地保留的测距测量值进行评估。
我们使用协方差交集( Covariance IntersectionCI )来估计初始化锚点位置的协方差以进行一致性数据融合。
一、问题背景
问题可以分为两个阶段
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 )中{ rj1rj2rj3 }的方向ABC三点对应图3 ( a )中的{ GpIj1GpIj2GpIj3 } .因此我们可以利用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和测距对{ didj }∈Υ 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 )更新对偶变量。其次他们将新的决策估计值xcik 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 ) }在机器人状态中没有被增广我们只将其增广到状态和协方差。如果状态已经包含了它的估计位置Gpais和协方差Paas我们用CI融合现有的值和新的估计值 四、实验方案
基于OpenVINS [ 1 ]实现了Consistent VIRO Filter它是目前最先进的基于滤波器的视觉惯性估计器。基于机器人操作系统( Robot Operating SystemROS )实现了分布式UWB初始化方法。
仿真实验 真实环境实验 五、创新总结
方法
第一阶段
对于第一个挑战我们通过共识形成( [ 18 ] )来重新表述集中式问题并使用一种流行的称为分布式ADMM [ 19 ] [ 20 ]的对偶方法来解决。
对于第二个挑战传感器位置的可观测性通常由Fisher信息矩阵( Fisher Information MatrixFIM )进行分析。通过利用几何解释我们提出了一种增量方法直接利用锚点的局部测量来评估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扩展到多机器人系统。
