CN116908777A - 基于显式通信带标签伯努利的多机器人随机组网协同导航方法 - Google Patents

Abstract

本申请提供一种基于显式通信带标签伯努利的多机器人随机组网协同导航方法。本申请针对间歇性观测或无绝对观测环境多机器人系统的定位问题，通过协同定位数据自适应选择状态和状态协方差更新策略，在机器人信息源为里程计测量数据时、机器人信息源为相机观测数据时、机器人无信息源时分别搭建时间更新、观测更新和显示通信三种更新策略对机器人的状态和状态协方差更新；并根据系统采样时间周期对上述更新步骤进行迭代实现协同导航。本申请能够通过更新策略之间的协同，有效提高算法鲁棒性、一致性和定位精度，使协方差估计收敛性更好，同时定位精度更高。

Description

基于显式通信带标签伯努利的多机器人随机组网协同导航 方法

技术领域

本申请涉及协同导航技术领域，具体而言涉及一种基于显式通信带标签伯努利的多机器人随机组网协同导航方法。

背景技术

随着协同导航技术的发展，分布式协同导航逐渐成为一种主流的导航方式。区别于现有仅使用单一机器人机载测量值所进行的定位，分布式协同导航技术中同时协同使用本设备的测量值以及通信的共享数据，以分布式的架构协同定位，以提高多机器人系统在间歇性观测场景下导航的整体性能。

现有的分布式协同定位技术包括：

基于扩展卡尔曼EKF的定位方法。其已被广泛应用于分布式协同导航，该方法虽然能够满足分布式协同定位对实时性的要求，但存在重复计算问题。基于扩展卡尔曼EKF的定位方法，其将相关的姿态估计视为互不相关，并且会重复使用共享的观测值。由此，基于扩展卡尔曼EKF的定位方法会导致所有机器人的姿态估计不一致。

基于协方差交叉的分布式协同导航方法。该技术可用于数据融合，使每个机器人保持已分解组状态的估计，并将该估计与相邻机器人共享。基于协方差交叉的分布式协同导航方法，即使面对未知程度的估计间相关性，也能产生一致的估计。但是，其存在观测噪声方差膨胀的缺陷。观测噪声方差的膨胀，使得估计的位姿误差协方差矩阵过大，会导致估计精度较差。

发明内容

本申请针对现有技术的不足，提供一种基于显式通信带标签伯努利的多机器人随机组网协同导航方法，本申请根据协同定位数据自适应选择状态和状态协方差更新策略，能够在间歇测量或无绝对测量场景下，提供一种同时满足协同导航一致性要求和精度要求的随机组网协同导航技术。本申请具体采用如下技术方案。

首先，为实现上述目的，本申请提出一种基于显式通信带标签伯努利的多机器人随机组网协同导航方法，其步骤包括：

步骤1：对各机器人参数进行初始化，所述参数包括：机器人标签编号i、机器人地标编号j、机器人的初始状态变量s(x)、机器人的初始状态协方差

粒子权重

步骤2：接收机器人协同定位数据，并根据t时刻各机器人协同定位数据的信息源类型，自适应为各机器人选择步骤3中相应的状态和状态协方差更新策略；

步骤3：在t时刻机器人i的信息源为里程计测量数据时，采用扩展卡尔曼时间更新策略对机器人的状态和状态协方差进行预测，获得t+1时刻机器人i的状态矩阵以及t+1时刻机器人i的状态协方差矩阵/>

在t时刻机器人i的信息源为相机观测数据时，通过扩展卡尔曼EKF和粒子滤波PF算法对机器人状态二次预测，并对带标签多伯努利LMB滤波器初始化，基于置信度传播的LMB对机器人的状态和状态协方差进行最优估计，获得t+1时刻机器人i的状态矩阵s^l(x_t+1)，其中，l为随机有限集RFS的标签；在t时刻机器人i无信息源时，采用通信机制，利用友邻信息对无信息源机器人的状态和状态协方差进行更新，获得t+1时刻无信息源机器人i的状态矩阵t+1时刻无信息源机器人i的状态协方差矩阵/>

步骤4：在导航期间，按照步骤2至步骤3对机器人的状态矩阵进行迭代更新，获得各时刻下各机器人导航系统状态，直至导航结束。

可选的，如上任一所述的基于显式通信带标签伯努利的多机器人随机组网协同导航方法，其中，所述协同定位数据包括：

里程计测量得到的线速度和角速度/>相机观测得到的相对姿态φ和相对距离d，

其中，相机观测数据需通过坐标系转换模型将相对观测信息转换到二维平面坐标系下，转换模型为：转换模型中，o_ij表示机器人i二维平面绝对坐标系下的坐标信息，包括xx_i和yy_i，o_ij角标ij表示机器人i观测友邻或者地标j。

以上两种数据类型分别选择时间更新、观测更新更新策略，若机器人无信息源则选择显示通信的通信机制更新策略。

可选的，如上任一所述的基于显式通信带标签伯努利的多机器人随机组网协同导航方法，其中，扩展卡尔曼时间更新策略对机器人的状态和状态协方差进行预测时，

机器人i的状态时间更新模型为：

式中，表示时间更新后机器人i的状态矩阵，/>和/>分别表示t时刻绝对坐标系下的估计横纵坐标，/>表示t时刻机器人i的线速度，/>和/>分别表示t时刻机器人i的真实姿态和估计姿态，/>表示姿态估计误差，n_v表示线速度/>的零均值高斯噪声干扰，Δt表示系统采样时间。

可选的，如上任一所述的基于显式通信带标签伯努利的多机器人随机组网协同导航方法，其中，扩展卡尔曼时间更新策略对机器人的状态和状态协方差进行预测时，

机器人i的状态协方差更新模型为：

式中，表示时间更新后机器人i的状态协方差矩阵，/>表示t时刻的状态协方差，∑_qi表示其他关联机器人协方差通过对角化构成的矩阵，u₁至u_N表示其他关联机器人协方差的编号。

可选的，如上任一所述的基于显式通信带标签伯努利的多机器人随机组网协同导航方法，其中，通过扩展卡尔曼EKF和粒子滤波PF算法对机器人状态二次预测时所采用的模型中：

滤波增益为：

EKF的标定值计算为：

PF的先验值构造为：

权重更新为：其中，/>

重采样为：

重采样后得到新的样本记为：

上述式中，为时间更新状态误差协方差矩阵，/>为观测系数矩阵，R_t+1为观测噪声矩阵，/>表示估计后验状态量，/>表示时间更新状态量，/>表示t+1时刻机器人i的观测数据，/>表示修正后状态误差协方差矩阵，/>和/>分别表示状态先验和观测先验，randn表示均值为0标准差为0.1的白噪声，h(·)表示观测函数，/>表示粒子的观测模型，/>表示状态转移模型，/>为贝叶斯假设下的建议分布，N_p表示粒子个数，/>和/>分别表示t+1时刻机器人i的粒子状态和权重。

可选的，如上任一所述的基于显式通信带标签伯努利的多机器人随机组网协同导航方法，其中，对带标签多伯努利LMB滤波器初始化的具体步骤包括：

记包含多个机器人平台协同导航模型为：

其中，t表示时间，n表示机器人平台编号数，t时刻系统中所包含的n个机器人平台分别记为每个元素/>在空间/>上，其中/>和/>分别表示状态空间和带标签的离散空间，并且/>*表示不固定组网标签，每个观测元素/>其中/>表示一个向量域，n∈{1,...,M_t}表示n个机器人平台观测索引值；

首先，获取机器人与友邻、地标的位置和姿态信息，

然后，利用伯努利RFS以一种直观的方式表示关于单个对象存在的不确定性，以参数r表示机器人的存在概率，s(x)表示该机器人的状态变量，与前文一致可简写为s；记X为空集的概率为1-r，根据贝叶斯推理，确定单个机器人伯努利RFS的概率密度为：

其中：X是M个独立多伯努利随机有限集Xⁱ的并集，例如多伯努利RFS由参数集/>定义，通过参数集/>给出一个状态空间为X，标签空间为的LMB-RFS。

可选的，如上任一所述的基于显式通信带标签伯努利的多机器人随机组网协同导航方法，其中，基于置信度传播的LMB对机器人的状态和状态协方差进行最优估计的具体步骤中：

机器人平台的关联权重按照以下方式更新：

将存在概率更新为：

将粒子权重更新为：

将关联权重更新为：

式中，P_s表示每个机器人在当前时刻的生存概率，表示带标签l机器人当前时刻粒子权重，式中o_ij(1)和o_ij(2)分别表示机器人观测中的距离和方位信息；/>和/>分别表示进行时间更新后标签为l的机器人粒子状态与友邻的相对距离和相对方位，/>和/>分别为测距噪声和方位的方差；P_D表示检测概率，η^(l,m)表示带标签l的机器人与观测平台m的相关系数，ω_cul表示存在错误观测概率，N_p表示粒子个数。

可选的，如上任一所述的基于显式通信带标签伯努利的多机器人随机组网协同导航方法，其中，基于置信度传播的LMB对机器人的状态和状态协方差进行最优估计的具体步骤为：

首先，为描述机器人平台与观测量之间的关系，引入向量A_k，向量A_k所包含的元素记为其中/>表示t时刻新生标签集合，当时，表明当前机器人平台存在该组网集合中并且与地标建立观测量m有关；当/>时，表明当前机器人平台存在该组网集合中，但是当前机器人平台没有与地标之间建立观测量；当/>时，表明该机器人平台不在该组网集合中，对应的概率为/>式中t+1|t表示时间转移步；

然后，引入相关向量a_t，将权值解释为关联向量a_t的概率质量函数，将α_t的概率质量函数定义为：/>

根据贝叶斯原理，通过边缘乘积近似上式，获得边缘关联概率

式中：表示除去数组/>的相关向量a_t，

最后，利用上述步骤所得到的参数对机器人平台存在概率和状态进行最优估计，将存在概率更新为：

将机器人状态更新为：

式中，表示带标签l的机器人与观测量的关联状态空间概率密度函数。

可选的，如上任一所述的基于显式通信带标签伯努利的多机器人随机组网协同导航方法，其中，利用友邻信息对无信息源机器人的状态和状态协方差进行更新的具体步骤包括：

首先，在同一时刻存在若干友邻机器人平台能够与当前无信息源机器人进行通信时，将通信拓扑结构定义为：

Ω^*＝Ω∪{Δ}

式中，为t时刻机器人i的通信有向图，其边为/>表示t时刻机器人j将自身信息发送给机器人i，此外，通信信息源的索引集为/>

Ω^*表示通信范围内机器人Ω和地标Δ的集合；

然后，通过显示通信方式共享机器人信息，按照如下模型更新无信息源机器人i的状态和状态误差协方差矩阵：

式中：系数{c₀,c_j,j∈C_i,t}是非负且和为1，通过最小化或实现系数值分配。

有益效果

本申请提供一种基于显式通信带标签伯努利的多机器人随机组网协同导航方法。本申请针对间歇性观测或无绝对观测环境多机器人系统的定位问题，通过协同定位数据自适应选择状态和状态协方差更新策略，在机器人信息源为里程计测量数据时采用扩展卡尔曼EKF时间更新策略对机器人状态和状态协方差预测；在机器人信息源为相机观测数据时通过扩展卡尔曼EKF和粒子滤波PF算法对机器人状态二次预测，并对带标签多伯努利LMB滤波器初始化，基于置信度传播的LMB对状态和状态协方差进行最优估计；在机器人无信息源时采用通信机制，利用友邻信息对无信息源机器人的状态和状态协方差更新。由此，本申请通过分别搭建时间更新、观测更新和显示通信三种更新策略对机器人的状态和状态协方差更新，并根据系统采样时间周期对上述更新步骤进行迭代，能够有效提高算法鲁棒性、一致性和定位精度。本申请协方差估计收敛性更好，同时定位精度更高。

值得注意的是，以上步骤中的时间更新、观测更新和显示通信三种更新策略为相互独立更新策略，在解算过程中为选择关系。本发明在多机器人协同定位导航中能够提高精度定位，适用于较大规模、大范围网络以及卫星拒止、存在间歇测量等复杂环境，适合工程实际应用。

本发明不依赖信息源的完整性，采用通信机制共享友邻状态信息，实现无信息源机器人平台的状态更新，并设计了相对和绝对观测耦合的状态更新算法，利用机器人历史状态信息的关键帧对状态估计加以约束，使得协同导航算法一致性和精度提高，抑制机器人数据关联时重复计算冗余信息。

本申请的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本申请而了解。

附图说明

附图用来提供对本申请的进一步理解，并且构成说明书的一部分，并与本申请的实施例一起，用于解释本申请，并不构成对本申请的限制。在附图中：

图1为本发明协同导航更新机制结构示意图；

图2为本发明基于显式通信带标签伯努利的多机器人随机组网协同导航方法流程图；

图3为本发明设计算法的定位误差和状态协方差跟踪示意图；

图4为本发明设计算法系统的相关分析图；

图3-1为基于Cen-EKF的集中式算法对比例的定位误差和状态协方差跟踪示意图；

图4-1为同为分布式协同导航机制GS-CI算法对比例的相关分析图；

图5为相对观测的三种情况；

图6为通信有向图示意；

图7为引入冗余关联向量的因子图模型；

图8为本申请所提供的融合算法的计算流程图

图9为算法精度对比

图10为协方差估计；

图11为第2实施例中实验场景和设备。

具体实施方式

为使本申请实施例的目的和技术方案更加清楚，下面将结合本申请实施例的附图，对本申请实施例的技术方案进行清楚、完整地描述。显然，所描述的实施例是本申请的一部分实施例，而不是全部的实施例。基于所描述的本申请的实施例，本领域普通技术人员在无需创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。

本技术领域技术人员可以理解，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)具有与本申请所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样定义，不会用理想化或过于正式的含义来解释。

本申请中所述的“和/或”的含义指的是各自单独存在或两者同时存在的情况均包括在内。

本申请中所述的“内、外”的含义指的是相对于系统本身而言，指向系统内部的方向为内，反之为外；而非对本申请的装置机构的特定限定。

本申请中所述的“连接”的含义可以是部件之间的直接连接也可以是部件间通过其它部件的间接连接。

本发明旨在提供一种基于显式通信带标签伯努利的多机器人随机组网协同导航方法，针对多机器人随机组网时平台个数不确定性，将平台状态建立随机有限集合，并构造LMB滤波器，将两种观测进行耦合并考虑机器人的历史观测信息，同时采用协方差交叉实现概率数据关联进行导航系统状态估计，保证了估计姿态严格一致，将EKF算法的标定值作为粒子滤波的先验值，构造LMB的随机有限集合，进而利用PF算法估计机器人的运动学参数，提高定位精度，对于无信息源机器人以显式通信拓扑来更新其状态和协方差，解决现有的协同定位方法在间歇测量或无绝对测量场景中算法一致性差和精度低的问题。

实施例1

在自动驾驶领域，分布式协同导航是很有前景的一种技术，其在全球卫星定位系统不可以用时，可基于机器人相对定位和通讯的定位方式，获得广泛应用。由于机器人通信受限存在间歇性观测，同时协同观测存在大量的冗余信息，如何避免交互信息冗余重复计算，实现估计一致性和高精度定位仍是分布式协同导航的关键问题。本发明以图1方式针对间歇性观测或无绝对观测环境多机器人系统，对多机器人运动平台搭建时间更新、观测更新和显示通信三种更新策略。本实施例所提供的基于显式通信带标签伯努利的多机器人随机组网协同导航算法(GS-EPF-LMB)，能够采用随机有限集对状态和观测建模，构造带标签多伯努利粒子，利用带标签的多伯努利耦合相对观测和绝对观测，通过粒子滤波器来优化带标签粒子状态，并利用历史信息对状态估计加以约束，同时采用CI实现概率数据关联进行导航系统状态估计；此外，每个机器人基于分层高斯模型，采用变分贝叶斯方法以分布式的方式同时估计姿态和观测噪声协方差矩阵，实现全局最优状态估计。

具体参考图2所示，本发明所述的基于显式通信带标签伯努利的多机器人随机组网协同导航方法的原理是：对多机器人运动平台三种更新策略，采用随机有限集对状态和观测建模，构造带标签多伯努利粒子，利用带标签的多伯努利耦合相对观测和绝对观测，通过粒子滤波器来优化带标签粒子状态，并利用历史信息对状态估计加以约束，同时采用协方差交叉实现概率数据关联进行导航系统状态估计，此外，每个机器人基于分层高斯模型，采用变分贝叶斯方法以分布式的方式同时估计姿态和观测噪声协方差矩阵，实现全局最优状态估计，提高多机器人导航系统精度。

本发明的具体实施方案如下：

1、初始化所有机器人相关参数，初始化内容主要包括：机器人标签和地标编号、初始状态变量和协方差、粒子权重等参数。

2、通过协同定位数据自适应选择状态和状态协方差更新策略，协同定位数据包括里程计测量得到的线速度和角速度/>相机观测得到的相对姿态φ观测和相对距离d，i为机器人的编号，t表示时刻，其中，相机观测数据需通过坐标系转换模型将相对观测信息转换到二维平面坐标系下，转换模型为：

式中，o_ij表示机器人i绝对坐标系下的坐标信息，包括xi和yi，o_ij角标ij表示机器人i观测友邻或者地标j；

以上两种数据类型分别选择步骤3和步骤4的更新策略，若机器人无信息源则选择步骤5所述的通信机制。

3、若t时刻机器人信息源为里程计测量数据，则采用EKF时间更新策略对机器人状态和状态协方差预测，其中，机器人i的状态时间更新模型为：

式中，表示时间更新后机器人i的状态矩阵，/>和/>分别表示t时刻绝对坐标系下的估计横纵坐标，/>表示t时刻机器人i的线速度，/>和/>分别表示t时刻机器人i的真实姿态和估计姿态，/>表示姿态估计误差，n_v表示线速度/>的零均值高斯噪声干扰，△t表示系统采样时间；

其中，还包括机器人i的状态协方差更新模型为：

式中，表示时间更新后机器人i的状态协方差矩阵，/>表示t时刻的状态协方差，∑_qi表示其他关联机器人协方差通过对角化构成的矩阵，u1至uN表示其他关联机器人协方差的编号。

4、若t时刻机器人信息源为相机观测数据，则通过扩展卡尔曼EKF和粒子滤波PF算法对机器人状态二次预测，并对带标签多伯努利LMB滤波器初始化，基于置信度传播的LMB对状态和状态协方差进行最优估计

(4.1)利用EKF和PF算法对机器人状态二次预测，模型为：

滤波增益：

计算EKF的标定值：

构造PF的先验值：

权重更新：其中，/>

重采样：

重采样后得到新的样本记为：

式中，为时间更新状态误差协方差矩阵，/>为观测系数矩阵，R_t+1为观测噪声矩阵，/>表示估计后验状态量，/>表示时间更新状态量，/>表示t+1时刻机器人i的观测数据，/>表示修正后状态误差协方差矩阵，/>和/>分别表示状态先验和观测先验，randn表示均值为0标准差为0.1的白噪声，h(·)表示观测函数，/>表示粒子的观测模型，表示状态转移模型，/>为贝叶斯假设下的建议分布，Np表示粒子个数，/>和/>分别表示t+1时刻机器人i的粒子状态和权重。

(4.2)LMB滤波器初始化，具体步骤为：

首先，获取机器人与友邻、地标的位置和姿态信息，其中，包含多个机器人平台协同导航模型为：

式中，t表示时间，n表示机器人平台编号数，t时刻系统中所包含的n个机器人平台分别记为每个元素/>在空间/>上，其中/>和L分别表示状态空间和带标签的离散空间，并/>l∈L，*表示不固定组网标签，每个观测元素/>其/>表示一个向量域，n∈{1,...,M_t}表示n个机器人平台观测索引值；

其次，伯努利RFS以一种直观的方式表示关于单个对象存在的不确定性，参数r表示机器人的存在概率，s(x)表示该机器人的状态变量，与前文一致可简写为s；X为空集的概率为1-r，根据贝叶斯推理，单个机器人伯努利RFS的概率密度为：

式中：X是M个独立多伯努利随机有限集Xⁱ的并集，例如多伯努利RFS由参数集/>定义。通过参数集π＝{r^l,s^l}_l∈L给出了一个状态空间为X，标签空间为L的LMB-RFS。

(4.3)基于置信度传播的LMB对状态和状态协方差进行最优估计，其中，机器人平台的关联权重更新为：

存在概率更新：

粒子权重更新：

关联权重更新：

式中，Ps表示每个机器人在当前时刻的生存概率，表示带标签l机器人当前时刻粒子权重，式中o_ij(1)和o_ij(2)分别表示机器人观测中的距离和方位信息；/>和/>分别表示进行时间更新后标签为l的机器人粒子状态与友邻的相对距离和相对方位，/>和/>分别为测距噪声和方位的方差；PD表示检测概率，η^(l,m)表示带标签l的机器人与观测平台m的相关系数，ωcul表示存在错误观测概率，Np表示粒子个数。

(4.4)基于置信度传播的LMB对状态和状态协方差进行最优估计，具体步骤为：

首先，为了描述机器人平台与观测量之间的关系，引入向量Ak，其所包含的元素其中/>表示t时刻新生标签集合，当/>时，表明当前机器人平台存在该组网集合中并且与地标建立观测量m有关；当/>时，表明当前机器人平台存在该组网集合中，但是当前机器人平台没有与地标之间建立观测量；当/>时，表明该机器人平台不在该组网集合中，对应的概率为/>式中t+1|t表示时间转移步；

其次，引入相关向量a_t，我们可以将权值解释为关联向量a_t的概率质量函数，将a_t的概率质量函数定义为：

根据贝叶斯原理，通过边缘乘积近似上式，获得边缘关联概率

/>

式中：表示除去数组/>的相关向量a_t，

最后，利用以上步骤得到的参数对机器人平台存在概率和状态进行最优估计，方法如下：

存在概率更新：

机器人状态更新：

式中，表示带标签l的机器人与观测量的关联状态空间概率密度函数。

5、若t时刻机器人无信息源，则采用通信机制，利用友邻信息对无信息源机器人的状态和状态协方差更新，具体步骤为:

首先，在同一时刻存在若干友邻机器人平台可与当前无信息源机器人进行通信，定义通信拓扑结构为：

Ω^*＝Ω∪{△}

式中，为t时刻机器人i的通信有向图，其边为/>表示t时刻机器人j将自身信息发送给i，此外，通信信息源的索引集为/>Ω^*表示通信范围内机器人Ω和地标△的集合；

其次，通过显示通信方式共享机器人信息，更新无信息源机器人i的状态和状态误差协方差矩阵，模型如下：

式中：系数{c₀,c_j,j∈C_i,t}是非负且和为1，通过最小化或/>实现系数值分配。

至此所有机器人运动平台的协同导航过程完成。

下面介绍本发明的实施例，本实施例在一个室内二维平面场景进行验证，场景内含有3个机器人和9个地标，每个机器人都配备有一个里程计和一个单目相机，其中里程计用于观测每个机器人的线速度和角速度，单目相机获取机器人与地标或友邻机器人之间的距离和方位观测值。

为了验证本发明方法，机载在运行过程中存在间歇停机，保证数据的普适性，按照上述方法分别解算了60s,120s,180s,240s四个不同时段的数据，可得到3个机器人的均方根定位误差和协方差估计，结果如表1：

可以看出本发明提出的基于显式通信带标签伯努利的多机器人随机组网协同导航方法，解决了分布式协同导航算法的定位精度和算法一致性问题，定位精度保持在0.11m内，且协方差估计收敛性更好。

请参照图3和图4，本发明方法在120s时段内对位姿估计的偏差、协方差估计和相关分析。采用了本发明提出的方法，使得定位精度明显提高，且协同机器人之间状态协方差估计更加收敛，对于无信息源机器人以显式通信拓扑来更新其状态和协方差，解决现有的协同定位方法在间歇测量或无绝对测量场景中算法一致性差和精度低的问题，实现了协同导航算法中一致性和精度的平衡，适用于集群机器人协同定位算法在工程中的实际运用。

实验表明本实施例算法鲁棒性好，提高了估计一致性和定位精度，协方差估计收敛性更好，同时定位精度保持在0.11m内；较GS-CI算法定位状态协方差收敛性提高48.6％，精度提高11％。

实施例2

本实施例针对多机器人系统位姿估计中的交互信息冗余、一致性、精度低差问题，提出一种基于显示通信带标签伯努利的多机器人随机组网协同导航算法(Global StateExtended Kalman-based Particle Filter on Labeled muti-Bernoulli,GS-EPF-LMB)，该方法同时估计机器人位姿、绝对和相对观测噪声协方差矩阵。对于每个机器人，在构造的分层高斯模型下，其位姿和观测噪声协方差矩阵的后验概率密度函数相互耦合，因此采用变分贝叶斯(Variable Bayesian,VB)方法获得近似的联合估计。针对多机器人随机组网时平台个数不确定性，将平台状态建立随机有限集合(Random Finite Set,RFS)，并构造带有标签的多伯努利(Labeled muti-Bernoulli,LMB)滤波器，将两种观测进行耦合并考虑机器人的历史观测信息，同时采用CI实现概率数据关联进行导航系统状态估计，减少交互信息的冗余并保证了估计姿态严格一致；将EKF算法的标定值作为粒子滤波(particle filter,PF)的先验值，进而利用PF算法估计机器人的运动学参数，构造LMB的随机有限集合，实现最优估计，提高定位精度；此外，本实施例提出的基于显示通信带标签伯努利的多机器人随机组网协同导航算法通信模式与GS-CI方法保持一致，使得观测和通信两种模型可以独立更新进行定位估计。

1多机器人系统模型

本实施例考虑了一群在二维空间移动的同构/异构的多机器人和已知位置信息的地标系统，通过多机器人自身信息自适应选择导航方式，其中包括时间更新、绝对观测、相对观测和通信四个相互独立的导航机制，如图1显示通信带标签伯努利的多机器人随机组网协同导航机制所示。将机器人群索引定义为地标定义为Δ，并满足Ω^*＝Ω∪{Δ}。每个移动机器人都安装本体传感器(里程计或惯性导航单元)和外部传感器(相机、雷达或UWB)，其中本体传感器用于敏感机器人自身运动，外部传感器主要用于对地标的绝对观测和其他机器人的相对观测，其状态更新采用基于EKF策略。将t时刻对于机器人i的状态信息定义为/>则整个系统的机器人状态表示为此外，在系统对机器人状态更新中，每个机器人i位姿估计值，用表示/>其协方差表示为/>

1.1运动传播模型

运动模型描述了里程计敏感机器人的空间位移。虽然该框架并不局限于任何特定的模型，本实施例中里程计敏感机器人线速度和角速度/>设Δt为两个连续观测点之间的时间间隔，[x_i,y_i]^T表示机器人位置，θ_i表示机器人的姿态，机器人i的非线性离散时间运动模型可由式(1)表达。

在状态更新时，若假设每个机器人的运动模型中速度不相关，则运动传播的协方差更新为：

式中：Σ_ui表示机器人i的协方差，其中线速度v_i受零均值高斯随机变量噪声n_v干扰，且协方差为机器人t时刻的姿态为/>对运动模型线性化，在估计状态中先不考虑 表示机器人的绝对坐标系位置。对式(1)线性化处理，机器人i运动方程简化为式(3)。

其中姿态估计的误差模型是由一个零均值的高斯随机变量构成，方差/>的边界值为。协方差矩阵Σ_ui的增量由式(4)表示。

其中C表示状态转移矩阵，将在下文给出说明。当速度的精确值未知时，将输入本身建模为一个高斯随机变量v_j其方差由最大输入值v_max和/>决定。当速度的精确值未知时，可以通过式(5)得到协方差增量的上限。/>

其中I_n是n×n的单位矩阵。

1.2观测更新模型

观测信息源有绝对观测和相对观测两种，其中相对位置观测包括相对姿态(φ)观测和相对距离(d)观测。由此，机器人i观测到机器人j(相对观测)、机器人i观测到地标(绝对观测)，其相对位置都可由式(6)表示。

式中：是包含角度参数θ的旋转矩阵。

在大多数情况下，相对位置通过相机、雷达、UWB等外部传感器观测得到，观测通常用相对距离(d_ij)和相对姿态(φ_ij)关系表示二维坐标系的相对位置。因此，相对位置由式(7)给出。

观测估计误差定义为由此将式(6)可近似为：

为了区分位置估计误差姿态估计误差/>以及观测噪声/>其中

根据机器人的观测信息计算H阵，系统可分为三种相对观测情况，如图5所示。

当机器人i观测到机器人j即观测对象为机器人j时，H_ij由式(9)计算。

同理，当机器人j观测到机器人i即观测对象是机器人i时，H阵为：

当观测目标为地标Δ时，H_iΔ通过式(11)计算。

/>

通过式(7)计算传感器的测距和测角，观测噪声可以表示为式(12)。

其方差表示为那么观测估计误差/>为其中，/>

在实际工程中，机器人可以同时观察多个目标，因此需要将观测量关联起来。定义一个观测拓扑结构，在同一观察更新过程中同时处理多个观测量。首先将观测拓扑定义为一个有向图，其边/>角标j表示机器人i在t时刻观测到的对象。为更直观表达，将机器人i所观察到的对象的索引集表示为并满足/>综合观测结果可以由式(13)叠加向量表示。

通过分离状态误差和其余项，观测误差的方差表示为其中/>同理相对观测时，观测协方差表示为/>其中/>

综上，机器人i的运动模型和观测模型可表示为以下一般形式：

式中f(·)和h(·)分别是系统状态函数和观测函数，其余符号定义与上文保持一致。

1.3通信模块

在本体传感器和外部传感器都失效时，机器人没有信息源，所以无法通过运动或观测来更新自身的位姿，这是就需要借助通信模型来更新没有信息源机器人的位姿，通信模式下，消息传递情况如图6通信有向图所示。

图6中绿色三角表示在某时刻没有信息源的机器人i，通过通信半径R判断可以通信的成员j。机器人i可以通过当机器人j发送估计信息来更新自身位置，其中机器人j发送自身的状态和协方差/>

但在实际中，机器人的状态估计和/>的相关性是未知的，因此融合估计是困难的。如果忽视了两者融合估计之间的相关性，那么将导致过度收敛问题。对此，本实施例在假设两者估计之间的相关性的情况下采用CI融合估计，并保证估计的一致性。一致估计可以被看作是另一种保持均值但协方差矩阵保守的估计，设估计z是一个均值为/>和方差为Σ_z的高斯随机向量。如果满足/>和/>那么这对估计/>被称为一致估计。

同观测更新来定义通信拓扑，作为一个有向图，其边为表示t时刻机器人j将自身信息发送给i。此外，通信信息源的索引集为采用CI可以得到了机器人i在接收j∈C_i,t信息时的更新方程，向机器人i发送信息的机器人的索引集，通过式(15)更新自身位姿和协方差。

式中：系数{c₀,c_j,j∈C_i,t}是非负且和为1，通过最小化或实现系数值分配。在实际操作过程中，由于c₀和c_j的参数是经验值设定，对于多机器人协同系统来说，组合参数的逆较大，导致结果过保守；此外，更新策略只涉及上一时刻状态信息，估计结果难以避免奇异值，使得系统缺乏泛化性能。本实施例采用带标签的伯努利耦合观测更新，耦合目标-观测关联历史记录，使得协方差收敛性更好，避免奇异值。

2带标签伯努利耦合观测更新

本实施例针对多机器人协同导航系统的状态变量和观测信息采用采用带标签的多伯努利随机有限集建模，耦合相对观测和绝对观测并考虑机器人的历史观测信息。每个LMB-RFS分量用存在概率表示目标-观测关联历史记录，将相对观测和绝对观测融合，采取粒子形式实现状态估计和机器人本体的存在概率估计更新，使得LMB滤波器精确且以高计算效率的逼近最优状态。

2.1带标签的随机有限集(L-RFS)

RFS是一个随机变量有限值映射集合，其每个元素表示机器人的状态估计。引入标记RFS类，以便在RFS框架内估计物体的状态，对于多机器人运动平台建立RFS如式(16)。

式中：每个元素在空间/>上，其中/>和L分别表示状态空间和带标签的离散空间，并/>l∈L；同理，对于多机器人平台的绝对观测和相对观测有限集合为：

式中：*表示不固定组网标签，每个观测元素其/>表示一个向量域，

n∈{1,...,M_t}表示观测索引值。

2.2带标签的多伯努利随机有限集

伯努利RFS以一种直观的方式表示关于单个对象存在的不确定性。参数r表示机器人的存在概率，s(x)表示该机器人的状态变量，与前文一致简写为s；X为空集的概率为1-r。因此，伯努利RFS的概率密度由式(18)给出。

式中：X是M个独立多伯努利随机有限集Xⁱ的并集，例如多伯努利RFS由参数集/>定义。通过参数集π＝{r^l,s^l}_l∈L给出了一个状态空间为X,标签空间为L的LMB-RFS。

2.3粒子滤波

粒子滤波(Particles Filter,PF)可以很好地解决非高斯噪声和高度非线性的问题，基于此，本实施例提出LMB对机器人状态进行粒子化，将EKF算法的标定值作为PF算法的先验值，进而利用PF算法估计机器人的状态为LMB提供可靠的先验值，其算法步骤如下：

步骤1初始化：t＝0。从初始先验分布p(s₀)中进行N_p个粒子的抽样，即根据EKF的标定值来生成先验粒子和观测，如式(19)。

步骤2加权粒子的预测、采样：t＝1,2,…,n。利用扩展卡尔曼滤波的标定值对粒子进行更新，如式(20)。

步骤3粒子权值计算。利用式(21)对N_p个粒子进行对应权值计算，并通过式(22)对N_p个粒子相应的权值进行归一化，最后将t+1时刻粒子和权值记为

步骤4通过重采样，计算重采样的粒子和权值利用式(23)计算状态变量的优化估计/>和每个粒子对应的方差矩阵/>

步骤5将步骤4重采样后的粒子和计算的方差矩阵代入步骤2进行迭代计算，最终得到优化的状态估计和协方差估计。优化后的状态降低了位置估计的不确定性，同时，优化后的方差为LMB关联概率提供可靠参数依据，增强了相对和绝对观测的耦合程度。2.4LMB预测和关联权重更新预测

首先，通过相机检测范围设定概率阈值PG，筛选出新加入组网的机器人，并提出冗余历史信息。

Pnon>PG (24)

式中：Pnon表示当前机器人的观测与已组网机器人无关联的概率。

随后结合机器人的观测，通过似然函数将LMB-RFS运动平台状态进行粒子化处理，生成多运动平台的粒子状态、粒子权重以及运动平台组网概率等相关参数，具体为：

式中：N_p表示粒子个数；x^l表示粒子状态；K(.)表示似然函数；表示粒子的权重；r^l表示运动平台的组网概率；μ_B表示新增组网平台概率均值；M表示观测个数。

值得注意的是，新生成对象的标签空间和现有对象的标签空间是互补，满足B∩L＝φ。非固定群组分为已组网运动平台多伯努利RFS(以S作为标记)和新增组网平台多伯努利RFS(以B作为标记)，即：

式中：上标S表示t时刻组网机器人群，且下一时刻t+1也在组网群中的组员；上标B表示t时刻未在组网群，下一时刻t+1在组网群中的组员，即新添组员，已存在的成员保留了历史关键信息，则其对应的标签集合域遵循如下规定：

根据式(18)，LMB-RFS的概率密度函数(pdf)为：

式中：l′表示未参与组网标记的标签，为包含函数，当l∈L^*时为1，否则为0；L(X^*)表示LMB-RFS的机器人标签集合；δ表示克罗内克函数，由于标记的RFS基数分布与未标记RFS的基数分布相同，因此/>的角标|X^*|＝(|L(X^*)|)表示同一标签，当X^*中的标签相互独立时△(X^*)＝1，否则为0。

新组网平台进行粒子滤波的时间更新：

式中：W_t ^*表示t时刻组网机器人的噪声集合，其包括测距噪声和方位噪声，随后通过参数集π＝{r^l,s^l}_l∈L对存在概率、权重和关联权重更新。

式中：P_s(·)表示每个机器人在当前时刻的生存概率，式中o_ij(1)和o_ij(2)分别表示机器人观测中的距离和方位信息；和/>分别表示进行时间更新后标签为l的机器人粒子状态与友邻的相对距离和相对方位，/>和/>分别为测距噪声和方位的方差；P_D表示检测概率，ω_cul表示存在错误观测概率，N_p表示粒子个数。

2.5基于置信度传播的概率数据关联观测更新

采用基于因子图的置信传播算法计算边缘关联概率，在关联权重更新后引入冗余变量，其引入冗余关联向量的因子图模型如图7所示。

为了关联概率快速边缘化，引入关联向量a_t+1对应的相关变量b_t+1，其中包含元素当/>时，表示存在与移动机器人相关的观测m；当时，表示不存在与移动机器人相关的观测m。联合关联概率质量函数为：

当或者/>参数否则为1，表示运动平台与观测集合完成匹配。引入冗余变量后的LMB协同导航系统的消息传递为：

/>

式中：表示机器人平台间的关联概率，/>表示当前粒子状态对应的观测与运动平台无关联的概率，为下一步迭代筛选冗余历史信息。

2.6观测更新

基于消息传递的关联权重更新后，对相关的观测更新，即对组网机器人的组网概率和空间概率密度函数更新。

式中：表示观测与运动平台相关权重；/>表示更新后的粒子权重，j表示粒子；/>表示存在概率或组网概率。

2.7剪枝、重采样和状态估计

设定剪枝概率并筛选出具有高组网概率的粒子。

由于粒子滤波存在退化现象，故对更新后的粒子状态进行重采样，最后对所有运动平台进行状态更新。

式中：表示经过状态预测、粒子筛选、剪枝、重采样等操作的粒子状态，运动平台的状态空间分布/>

3 算法融合

3.1 分布式算法整体设计

本实施例融合算法总流程图如图8所示，前文耦合了绝对和相对观测，保留时间更新、观测更新、通信更新三个独立的更新机制，增强系统的泛化性能；机器人获得观测信息后通过协同数据类型自适应选择更新方式，算法将同时更新机器人状态和协方差。

3.2算法评价指标

本实施例对算法的评价指标采用协同导航常用的两个指标，分别为定位误差和状态协方差估计误差。

定位误差，即状态均方根估计误差，表示估计位置与Vcion相机给的真值的误差，定义为：

式中：和s分别表示算法估计值和真实值。状态协方差跟踪反映机器人对其估计位置的不确定性，即协方差矩阵的收敛性，定义为：

4实验验证

为验证本实施例所提出的基于显示通信的自适应协同导航算法GS-EPF-LMB在真实物理世界中的算法性能，在本节，利用多伦多大学航空航天研究所(UTIAS)Leung提供的一个开放的多机器人协同定位和映射数据集来测试本实施例所提出的GS-EPF-LMB算法的性能。数据集是一个由9个独立数据组成的二维室内数据集合，每个数据集包含多个机器人的里程计和观测数据，以及由Vicon相机提供的位置地面真实数据。

每个机器人都配备有一个里程计和一个单目相机，其中里程计用于观测每个机器人的线速度和角速度，输出频率为67(Hz)，单目相机获取机器人与地标或友邻机器人之间的距离(range)和方位观测值(bearing)。所有移动机器人的地标参考位置和参考姿态均由一个为100(Hz)的10个摄像头的Vicon运动捕捉系统提供，其位置精度约为1(mm)，实验环境和设备情况如图11所示：

实验采用3台机器人数据集，将改进的融合算法与同为分布式协同导航机制GS-CI算法进行对比；同时与基于Cen-EKF的集中式算法来验证分布式较集中式算法在间歇测量环境下分布式架构的明显优势。实验针对物理数据集分别解算了60s,120s,180s,240s四个不同时段的数据，为了避免算法性能的偶然性，对每个周期都进行5次解算求其平均。

如图3-1、图4-1和图3、图4，分别为三种算法在120s时段内对位姿估计的偏差、协方差估计和相关分析。图3、图4为本实施例中GS-EPF-LMB的算法性能。

表1算法在不同时段下的定位精度

由表1可知本实施例设计的GS-EPF-LMB算法定位偏差保持在0.11m内，较GS-CI分布式算法定位精度提高10.5％；较集中式Cen-EKF算法精度提高11.0％，由于本实施例测试环境存在间歇测量情况，集中式算法存在单点故障问题，导致算法整体容错性差、精度低。算法对比效果如图9所示。

算法协方差跟踪

表2分布式下协方差估计

表2所示为协方差估计的收敛性，其反映了在对机器人估计位置的不确定性，通过图10不难发现本实施例算法GS-EPF-LMB较GS-CI算法在收敛性上更好，提高48.6％。

5结论

针对间歇观测或无绝对观测情况，提出了一种基于显示通信带标签伯努利的多机器人随机组网协同导航算法(GS-EPF-LMB)，建立时间更新、观测更新和显示通信互相独立的三个更新模块，通过LMB随机组网模型，对相对观测和绝对观测建立耦合关系，置信度传播数据，估计最优的状态，解决信息冗余的同时，提高算法的一致性；此外，以EKF算法的标定值作为PF算法的先验值为基础，进而利用PF算法估计机器人的运动学参数来提高算法精度。实验结果表明：本实施例提出的算法精度和一致性较GS-CI等分布式算法有较大提升，有效提高了系统的鲁棒性，具有很好的使用价值和应用前景。

以上仅为本申请的实施方式，其描述较为具体和详细，但并不能因此而理解为对本申请专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些均属于本申请的保护范围。

Claims (9)

1.一种基于显式通信带标签伯努利的多机器人随机组网协同导航方法，其特征在于，步骤包括：

步骤1：对各机器人参数进行初始化，所述参数包括：机器人标签编号i、机器人地标编号j、机器人的初始状态变量s(x)、机器人的初始状态协方差粒子权重/>

步骤2：接收机器人协同定位数据，并根据t时刻各机器人协同定位数据的信息源类型，自适应为各机器人选择步骤3中相应的状态和状态协方差更新策略；

步骤3：在t时刻机器人i的信息源为里程计测量数据时，采用扩展卡尔曼时间更新策略对机器人的状态和状态协方差进行预测，获得t+1时刻机器人i的状态矩阵以及t+1时刻机器人i的状态协方差矩阵/>

在t时刻机器人i的信息源为相机观测数据时，通过扩展卡尔曼EKF和粒子滤波PF算法对机器人状态二次预测，并对带标签多伯努利LMB滤波器初始化，基于置信度传播的LMB对机器人的状态和状态协方差进行最优估计，获得t+1时刻机器人i的状态矩阵s^l(x_t+1)，其中，l为随机有限集RFS的标签；在t时刻机器人i无信息源时，采用通信机制，利用友邻信息对无信息源机器人的状态和状态协方差进行更新，获得t+1时刻无信息源机器人i的状态矩阵t+1时刻无信息源机器人i的状态协方差矩阵/>

步骤4：在导航期间，按照步骤2至步骤3对机器人的状态矩阵进行迭代更新，获得各时刻下各机器人导航系统状态，直至导航结束。

2.如权利要求1所述的基于显式通信带标签伯努利的多机器人随机组网协同导航方法，其特征在于，所述协同定位数据包括：

里程计测量得到的线速度和角速度/>相机观测得到的相对姿态φ和相对距离d，

其中，相机观测数据需通过坐标系转换模型将相对观测信息转换到二维平面坐标系下，转换模型为：转换模型中，o_ij表示机器人i二维平面绝对坐标系下的坐标信息，包括xx_i和yy_i，o_ij角标ij表示机器人i观测友邻或者地标j。

3.如权利要求2所述的基于显式通信带标签伯努利的多机器人随机组网协同导航方法，其特征在于，扩展卡尔曼时间更新策略对机器人的状态和状态协方差进行预测时，

机器人i的状态时间更新模型为：

式中，表示时间更新后机器人i的状态矩阵，/>和/>分别表示t时刻绝对坐标系下的估计横纵坐标，/>表示t时刻机器人i的线速度，/>和/>分别表示t时刻机器人i的真实姿态和估计姿态，/>表示姿态估计误差，n_v表示线速度/>的零均值高斯噪声干扰，Δt表示系统采样时间。

4.如权利要求3所述的基于显式通信带标签伯努利的多机器人随机组网协同导航方法，其特征在于，扩展卡尔曼时间更新策略对机器人的状态和状态协方差进行预测时，

机器人i的状态协方差更新模型为：

式中，表示时间更新后机器人i的状态协方差矩阵，/>表示t时刻的状态协方差，∑_qi表示其他关联机器人协方差通过对角化构成的矩阵，u₁至u_N表示其他关联机器人协方差的编号。

5.如权利要求2所述的基于显式通信带标签伯努利的多机器人随机组网协同导航方法，其特征在于，通过扩展卡尔曼EKF和粒子滤波PF算法对机器人状态二次预测时所采用的模型中：

滤波增益为：

EKF的标定值计算为：

PF的先验值构造为：

权重更新为：其中，/>

重采样为：

重采样后得到新的样本记为：

上述式中，为时间更新状态误差协方差矩阵，/>为观测系数矩阵，R_t+1为观测噪声矩阵，/>表示估计后验状态量，/>表示时间更新状态量，/>表示t+1时刻机器人i的观测数据，/>表示修正后状态误差协方差矩阵，/>和/>分别表示状态先验和观测先验，randn表示均值为0标准差为0.1的白噪声，h(·)表示观测函数，/>表示粒子的观测模型，/>表示状态转移模型，/>为贝叶斯假设下的建议分布，N_p表示粒子个数，/>和/>分别表示t+1时刻机器人i的粒子状态和权重。

6.如权利要求5所述的基于显式通信带标签伯努利的多机器人随机组网协同导航方法，其特征在于，对带标签多伯努利LMB滤波器初始化的具体步骤包括：记包含多个机器人平台协同导航模型为：

其中，t表示时间，n表示机器人平台编号数，t时刻系统中所包含的n个机器人平台分别记为每个元素/>在空间/>上，其中/>和/>分别表示状态空间和带标签的离散空间，并且/>*表示不固定组网标签，每个观测元素/>其中表示一个向量域，n∈{1,...,M_t}表示n个机器人平台观测索引值；

首先，获取机器人与友邻、地标的位置和姿态信息，

然后，利用伯努利RFS以一种直观的方式表示关于单个对象存在的不确定性，以参数r表示机器人的存在概率，s(x)表示该机器人的状态变量；记X为空集的概率为1-r，根据贝叶斯推理，确定单个机器人伯努利RFS的概率密度为：

其中：X是M个独立多伯努利随机有限集Xⁱ的并集，多伯努利RFS由参数集定义，通过参数集/>给出一个状态空间为X，标签空间为/>的LMB-RFS。

7.如权利要求6所述的基于显式通信带标签伯努利的多机器人随机组网协同导航方法，其特征在于，基于置信度传播的LMB对机器人的状态和状态协方差进行最优估计的具体步骤中：

机器人平台的关联权重按照以下方式更新：

将存在概率更新为：

将粒子权重更新为：

将关联权重更新为：

式中，P_s表示每个机器人在当前时刻的生存概率，表示带标签l机器人当前时刻粒子权重，式中o_ij(1)和o_ij(2)分别表示机器人观测中的距离和方位信息；/>和/>分别表示进行时间更新后标签为l的机器人粒子状态与友邻的相对距离和相对方位，/>和/>分别为测距噪声和方位的方差；P_D表示检测概率，η^(l,m)表示带标签l的机器人与观测平台m的相关系数，ω_cul表示存在错误观测概率，N_p表示粒子个数。

8.如权利要求7所述的基于显式通信带标签伯努利的多机器人随机组网协同导航方法，其特征在于，基于置信度传播的LMB对机器人的状态和状态协方差进行最优估计的具体步骤为：

首先，为描述机器人平台与观测量之间的关系，引入向量A_k，向量A_k所包含的元素记为其中/>表示t时刻新生标签集合，当/>时，表明当前机器人平台存在该组网集合中并且与地标建立观测量m有关；当/>时，表明当前机器人平台存在该组网集合中，但是当前机器人平台没有与地标之间建立观测量；当/>时，表明该机器人平台不在该组网集合中，对应的概率为/>式中t+1|t表示时间转移步；

然后，引入相关向量a_t，将权值解释为关联向量a_t的概率质量函数，将α_t的概率质量函数定义为：/>

根据贝叶斯原理，通过边缘乘积近似上式，获得边缘关联概率

式中：表示除去数组/>的相关向量a_t，

最后，利用上述步骤所得到的参数对机器人平台存在概率和状态进行最优估计，将存在概率更新为：

将机器人状态更新为：

式中，表示带标签l的机器人与观测量的关联状态空间概率密度函数。

9.如权利要求2所述的基于显式通信带标签伯努利的多机器人随机组网协同导航方法，其特征在于，利用友邻信息对无信息源机器人的状态和状态协方差进行更新的具体步骤包括：

首先，在同一时刻存在若干友邻机器人平台能够与当前无信息源机器人进行通信时，将通信拓扑结构定义为：

Ω^*＝Ω∪{Δ}

式中，为t时刻机器人i的通信有向图，其边为/>表示t时刻机器人j将自身信息发送给机器人i，此外，通信信息源的索引集为/> Ω^*表示通信范围内机器人Ω和地标Δ的集合；

然后，通过显示通信方式共享机器人信息，按照如下模型更新无信息源机器人i的状态和状态误差协方差矩阵：

式中：系数{c₀,c_j,j∈C_i,t}是非负且和为1，通过最小化或/>实现系数值分配。