CN104331078A - 一种基于位置映射算法的多机器人协作定位方法 - Google Patents

Abstract

一种基于位置映射算法的多机器人协作定位方法，机器人编队中每个机器人至少完成：探测的步骤；机器人之间进行位置信息交换的步骤；利用位置映射算法机器人更新自身位置的步骤；机器人之间相互交换位置信息可以提高机器人定位的稳定性和快速性，机器人之间可以共享传感器信息和不同的传感器平台，所有机器人几乎都可以像装备高精度传感器的机，使得机器人能够完成更加复杂的任务。

Description

一种基于位置映射算法的多机器人协作定位方法

技术领域

本发明属于机器人定位技术领域，具体涉及一种基于位置映射算法的多机器人协作定位方法。

背景技术

随着机器人技术的发展，在一些面向应用的任务中，多机器人必须通过协作才能完成，如：机器人足球赛，机器人战场协同作战，多机器人协同作业等。然而有效的定位又是多机器人能进行有效协作的基础，在实际应用中一个机器人定位存在以下问题：单机器人只能通过自身传感器对周围的局部环境进行相对的定位，而且其获得的测量信息的误差会随着时间的推移变得越来越大；单机器人要实现精准定位，自身必须装备非常精确的传感器，其价格也会相对较高从而造成成本的增加；另外单个机器人如果定位出现误差，其自身不能很快的修正误差，造成误差累积导致定位失败。因此通过融合不同机器人的探测信息，可以使得机器人在实际环境中的定位更加准确和获得信息更加全面。但是现存的多机器人定位方法需要机器人之间同步的信息量很大，在机器人编队中机器人数量较多时，要实现机器人协同定位不仅需要同步大量的数据而且增加了机器人自身的处理器的负担。另外现有的多机器人协作定位技术多是单纯同步机器人间的几何关系，不仅需要传递的数据量大而且不能快速的扩散到整个机器人编队。

发明内容

针对现有技术存在的缺陷和不足，本发明提供一种基于位置映射算法的多机器人协作定位方法。

本发明的技术方案如下:

一种基于位置映射算法的多机器人协作定位方法，用于在多机器人构成的机器人编队中多机器人间协作定位，每个机器人至少完成：探测的步骤；机器人之间进行位置信息交换的步骤；利用位置映射算法机器人更新自身位置的步骤；

所述探测的步骤中，机器人感知到信息其中和均代表的是信息流并假设每一次测量都是独立的，为环境测量信息；为机器人间的探测信息，表示机器人与其探测到的其它K个机器人之间的几何关系信息，其中j＝1,2,…,K表示在机器人R_i探测到机器人R_j时，机器人R_i获得的其与机器人R_j之间的几何关系信息；

所述机器人之间进行位置信息交换的步骤中，机器人R_i探测到其他K个机器人，那么机器人R_i将向其他K个机器人传递其位置及探测信息，同时，机器人R_i还接收其他K个机器人分别发送的它们各自的位置及探测信息，所述位置信息包括机器人对自身位置进行预估的预估位置l_t和该预估位置的信度所述预估位置的信度指的是机器人对其位置预估的确信度；

所述利用位置映射算法机器人更新自身位置的步骤中，机器人R_i根据自身的位置和探测信息以及从其他K个机器人接收到的它们各自的位置和探测信息，机器人可以计算出其自身有K个可能位置，即机器人计算出的可能位置与其接收到的其他机器人的预估位置之间是一一映射的关系，表示为其中表示与所接收的其他某一个机器人的预估位置相对应的机器人的一个可能位置；机器人利用其计算出的K个可能位置，经过位置映射算法，在重新采样后，实现更新自身位置；所述位置映射算法，即在粒子滤波方法中，用这K个可能位置依次替换采样的加权的样本中拥有最小权值的样本，这些样本的权值由接收到的其他K个机器人的预估位置的信度依次相应代替；所述利用位置映射算法机器人更新自身位置，包括如下A、B两种情况：

A.只有一个机器人探测到对方的情况：即，机器人R_i可以探测到机器人R_j，而机器人R_j探测不到机器人R_i，则机器人R_i利用位置映射算法更新自身位置，包括如下步骤：

步骤一：机器人R_i感知到其与机器人R_j间的探测信息即机器人R_i与机器人R_j之间的几何关系信息，包括两者之间的间距d_i,j、由机器人R_i坐标连向机器人R_j坐标的连线与x轴正方向形成的夹角按下面公式进行计算：

其中θ_i是机器人R_i的朝向与x轴正方向形成的夹角；α_i,j代表机器人R_i的朝向与机器人R_i、R_j的坐标连线之间的夹角；

则由器人R_i提供的其与机器人R_j之间的探测信息为

步骤二：机器人R_i与机器人R_j之间相互交换各自的预估位置及预估位置的信度，且机器人R_i与机器人R_j之间还将共享探测信息并将机器人R_i的预估位置定义为所有样本的期望值，将机器人R_i的预估位置的信度定义为所有样本权值的最大值，如式(4)和式(5)所示：

$l_t^{R_i}=E\left(L_t^{R_i}\right)---\left(4\right)$

${\widehat{\mathrm{\ω}}}_t^{R_i}=\max \left({\mathrm{\ω}}_t^{R_i}\right)---\left(5\right)$

式(4)中，机器人R_i的预估位置是机器人R_i采样的加权的样本集合的期望值；式(5)中，为机器人R_i预估位置的信度；

机器人R_i将其预估位置预估位置的信度探测信息传递给机器人R_j，如式(6)所示：

机器人R_j按照相同的计算方法，因为机器人R_j并没有探测到机器人R_i，所以它只向机器人R_i传送自己的预估位置及预估位置的信度如式(7)和式(8)所示：

$l_t^{R_j}=E\left(L_t^{R_j}\right)---\left(7\right)$

${\widehat{\mathrm{\ω}}}_t^{R_j}=\max \left({\mathrm{\ω}}_t^{R_j}\right)---\left(8\right)$

式(7)中，机器人R_j的预估位置是机器人R_j采样的加权的样本集合的期望值；

式(8)中，为机器人R_j预估位置的信度；

机器人R_j将其预估位置预估位置的信度传递给机器人R_i，如式(9)所示：

步骤三：机器人R_i更新自身位置；

首先，机器人R_i根据机器人R_j的预估位置和该预估位置的信度计算一个可能位置；然后，机器人R_i采样的加权的样本中，拥有最小权值的样本将被这个计算出的可能位置替换，此样本的权值将被接收到机器人R_j的预估位置信度所替换，此过程如以下公式所示：

${\mathrm{\ω}}_t^{R_{i,\min }}={\widehat{\mathrm{\ω}}}_t^{R_j}---\left(11\right)$

其中，分别为机器人R_i的拥有最小权值的样本及其权值；

最后，在重新采样后，机器人R_i的位置更新。

B.两个机器人相互探测到对方的情况，则方法为：

步骤一：机器人R_i感知到其与机器人R_j间的探测信息同时，机器人R_j也感知到其与机器人R_i间的探测信息其中d_j,i为两者之间的间距、为由机器人R_j坐标连向机器人R_i坐标的连线与x轴正方向形成的夹角；

步骤二：机器人R_i与机器人R_j之间相互交换各自的预估位置及预估位置的信度，且机器人R_i与机器人R_j之间还共享探测信息和探测信息机器人R_i将其预估位置预估位置的信度探测信息传递给机器人R_j，如式(6)所示，同时，机器人R_j将其预估位置预估位置的信度探测信息传递给机器人R_i，如式(12)所示，

步骤三：机器人更新自身位置；

首先，根据最优探测信息选择规则，机器人R_i和机器人R_j分别从机器人R_i的探测信息和机器人R_j的探测信息中选择出最优的探测信息；所述最优探测信息选择规则：

1)如果探测信息来自于不同的传感器，机器人将选择精确度高的传感器所获得的信息；

2)如果探测信息来自于相同的传感器，机器人将选择自身传感器所获得的探测信息；

接下来，机器人R_i和机器人R_j更新自身位置。

其中，机器人R_i按照情况A中步骤三的方法更新自身位置。

机器人R_j更新自己的位置方法与机器人R_i相同，机器人R_j根据机器人R_i的预估位置和该预估位置的信度计算一个可能位置；机器人R_j采样的加权的样本中，拥有最小权值的样本将被这个计算出的可能位置替换，此样本的权值将被接收到机器人R_i的预估位置信度所替换，此过程如以下公式所示：

${\mathrm{\ω}}_t^{R_{j,\min }}={\widehat{\mathrm{\ω}}}_t^{R_j}---\left(14\right)$

其中，分别为机器人R_j的拥有最小权值的样本及其权值；

最后，在重新采样后，机器人R_i和机器人R_j的位置更新。

有益效果：本发明的基于位置映射算法的多机器人协作定位方法主要有以下几个方面的优点：

一、机器人之间相互交换位置信息可以提高机器人定位的稳定性和快速性，如果其中一个机器人被准确的定位，其他机器人之间通过信息交换可以根据此机器人的位置信息更新自己的位置信息从而准确定位；

二、机器人之间可以共享传感器信息，机器人编队中可能有的机器人装备价格昂贵的高精度传感器，有的机器人装备价格相对便宜的传感器，通过共享传感器信息使得编队中所有机器人几乎都可以像装备高精度传感器的机器人一样精确定位；

三、机器人可以共享不同的传感器平台，在机器人编队中有的机器人可能装备视觉传感器，而有的机器人可能装备距离传感器，通过共享传感器平台可以使得机器人能够完成更加复杂的任务。

附图说明

图1为本发明具体实施方式的基于位置映射算法的多机器人协作定位方法流程图；

图2为本发明具体实施方式的三个机器人之间进行信息交换的示意图；

图3为本发明具体实施方式的两个机器人间几何关系的示意图；

图4为本发明具体实施方式的只有一个机器人R_i探测到对方的情况中机器人R_i利用位置映射算法更新自身位置的方法流程图；

图5为本发明具体实施方式的两个机器人相互探测到对方的情况中机器人R_i利用位置映射算法更新自身位置的方法流程图；

图6(a)为本发明实施例中机器人R₁的实际轨迹和样本分布轨迹图(b)为本发明实施例中机器人R₂的实际轨迹和样本分布轨迹图；

图7(a)为本发明实施例中机器人R₁分别在时间T＝30s、T＝58.5s、T＝59.4s、T＝126s定位的样本分布情况示意图；(b)为本发明实施例中机器人R₂分别在时间T＝30s、T＝58.5s、T＝59.4s、T＝126s定位的样本分布情况示意图。

具体实施方式

下面结合附图对本发明的具体实施方式做详细说明。

本实施方式的基于位置映射算法的多机器人协作定位方法，用于在多机器人构成的机器人编队中多机器人间协作定位，如图1所示，每个机器人至少完成：探测的步骤；机器人之间进行位置信息交换的步骤；利用位置映射算法机器人更新自身位置的步骤；

所述探测的步骤中，机器人感知到信息其中t为探测时间，和均代表的是信息流并假设每一次测量都是独立的，为环境测量信息；为机器人间的探测信息，表示机器人与其探测到的机器人之间的几何关系信息，例如，若机器人R_i探测到其他K个机器人，则其中j＝1,2,…,K代表在机器人R_i探测到机器人R_j时，机器人R_i获得的相关的几何关系信息；假设环境测量信息和机器人间的探测信息是独立的，则机器人的姿态概率p(s_t|Z_t)按公式(1)进行计算：

$p\left(s_t\right|Z_t)=p\left(s_t\right|Z_t^e,Z_t^d)=p\left(s_t\right|Z_t^e)\·p\left(s_t\right|Z_t^e)---\left(1\right)$

其中，s_t为机器人的姿态；可以由贝叶斯滤波(Bayes fi lters)计算出来；通过位置映射算法得出，其取值为本发明位置映射算法中的机器人预估位置的信度。

所述机器人之间进行位置信息交换的步骤中，机器人R_i探测到其他K个机器人，那么机器人R_i将向其他K个机器人传递其位置及探测信息，同时，机器人R_i还接收其他K个机器人分别发送的它们各自的位置及探测信息，所述位置信息包括机器人对自身位置进行预估的预估位置l_t、该预估位置的信度所述预估位置的信度指的是机器人对其位置预估的确信度；例如，将机器人R₁、R₂、R₃间的协作简化为3组一对一的协作关系后，机器人R₁、R₂、R₃之间的位置信息交换关系如图2的示意图所示。

所述利用位置映射算法机器人更新自身位置的步骤中，机器人R_i根据自身的位置信息及从其他K个机器人接收到的位置信息，机器人可以计算出其自身有K个可能位置，即机器人计算出的可能位置与其接收到的其他机器人的预估位置之间是一一映射的关系，即其中表示机器人计算出的其与所接收的其他某一个机器人的预估位置相对应的一个可能位置；机器人利用其计算出的K个可能位置，经过位置映射算法，在重新采样后，实现更新自身位置；所述利用位置映射算法机器人更新自身位置，包括如下A、B两种情况：

A.只有一个机器人探测到对方的情况：即，机器人R_i可以探测到机器人R_j，而机器人R_j探测不到机器人R_i，机器人R_i利用位置映射算法更新自身位置的过程，这种情况可能发生在机器人R_j没有能力探测到其它机器人时，或者其它机器人超出了机器人R_j的探测范围。在这种情况下，机器人R_i利用位置映射算法更新自身位置，如图4所示，包括如下步骤：

步骤一：机器人R_i感知到其与机器人R_j间的探测信息即机器人R_i与机器人R_j之间的几何关系信息，包括两者之间的间距d_i,j、由机器人R_i坐标连向机器人R_j坐标的连线与x轴正方向形成的夹角如图3所示，按下面公式进行计算：

其中θ_i是机器人R_i的朝向与x轴正方向形成的夹角，可以由测量或者方向传感器获得；α_i,j表示机器人R_i的朝向与机器人R_i、R_j的坐标连线之间的夹角，可以由器人R_i的传感器直接获得；

则由器人R_i提供的其与机器人R_j之间的探测信息为

步骤二：机器人R_i与机器人R_j之间相互交换各自的预估位置及预估位置的信度，且机器人R_i与机器人R_j之间还将共享探测信息并将预估位置定义为所有样本的期望值，将预估位置的信度定义为所有样本权值的最大值，如式(4)和式(5)所示：

$l_t^{R_i}=E\left(L_t^{R_i}\right)---\left(4\right)$

${\widehat{\mathrm{\ω}}}_t^{R_i}=\max \left({\mathrm{\ω}}_t^{R_i}\right)---\left(5\right)$

式(4)中，机器人R_i的预估位置是机器人R_i采样的加权的样本集合的期望值；式(5)中，为机器人R_i预估位置的信度；

机器人R_i将其预估位置预估位置的信度、探测信息传递给机器人R_j，如式(6)所示：

机器人R_j按照相同的计算方法，因为机器人R_j并没有探测到机器人R_i，所以它只向机器人R_i传送自己的预估位置及预估位置的信度如式(7)和式(8)所示：

$l_t^{R_j}=E\left(L_t^{R_j}\right)---\left(7\right)$

${\widehat{\mathrm{\ω}}}_t^{R_j}=\max \left({\mathrm{\ω}}_t^{R_j}\right)---\left(8\right)$

式(7)中，机器人R_j的预估位置是机器人R_j采样的加权的样本集合的期望值；

式(8)中，为机器人R_j预估位置的信度；

机器人R_j将其预估位置预估位置的信度传递给机器人R_j，如式(9)所示：

步骤三：机器人更新自身位置；

首先，机器人R_i根据机器人R_j的预估位置和该预估位置的信度计算一个可能位置；然后，机器人R_i采样的加权的样本中，拥有最小权值的样本将被这个计算出的可能位置替换，此样本的权值将被接收到机器人R_j的预估位置信度所替换，此过程如以下公式所示：

${\mathrm{\ω}}_t^{R_{i,\min }}={\widehat{\mathrm{\ω}}}_t^{R_j}---\left(11\right)$

其中，分别为机器人R_i的拥有最小权值的样本及其权值；

最后，在重新采样后，机器人R_i的位置更新。机器人位置更新将存在以下三种可能：

1)当机器人R_i采样样本权值的最大值时，机器人R_j比机器人R_i更加确定自己的位置。这种情况发生在机器人R_j的定位精度高于机器人R_i时，此时机器人R_i更新自己的位置。

2)当机器人R_i采样样本权值的最大值时，机器人R_i比机器人R_j更加确定自己的位置。此时从机器人R_j接收到的预估位置的信度对机器人R_i的位置更新没有任何帮助。

3)当机器人R_i采样样本权值的最大值时，机器人R_j和机器人R_i对其估计位置有相似的信度，此时机器人位置更新的意义不大。

B.两个机器人相互探测到对方的情况，在此情况下，两个机器人都有能力探测到对方，并向对方传递探测信息。在这种情况下，机器人R_i和机器人R_i利用位置映射算法更新自身位置，如图5所示，包括如下步骤：

步骤一：机器人R_i感知到其与机器人R_j间的探测信息同时，机器人R_j也感知到其与机器人R_i间的探测信息其中d_j,i为两者之间的间距、为由机器人R_j坐标连向机器人R_i坐标的连线与x轴正方向形成的夹角；

步骤二：机器人R_i与机器人R_j之间相互交换各自的预估位置及预估位置的信度，且机器人R_i与机器人R_j之间还将共享探测信息和探测信息机器人R_i将其预估位置预估位置的信度、探测信息传递给机器人R_j，如式(6)所示，同时，机器人R_j将其预估位置预估位置的信度、探测信息传递给机器人R_i，如式(12)所示，

步骤三：机器人更新自身位置；

首先，首先，根据最优探测信息选择规则，机器人R_i和机器人R_j分别从机器人R_i的探测信息和机器人R_j的探测信息中选择出最优的探测信息；所述最优探测信息选择规则：

1)如果探测信息来自于不同的传感器，机器人将选择精确度高的传感器所获得的信息；则该项原则可以保证机器人编队共享准确的传感器信息。

2)如果探测信息来自于相同的传感器，机器人将选择自身传感器所获得的探测信息；则该项原则可以保证避免给机器人编队引入误差。

接下来，机器人R_i按照情况A中步骤三的方法更新自身位置。

机器人R_j更新自己的位置方法与机器人R_i相同，机器人R_j根据机器人R_i的预估位置和该预估位置的信度计算一个可能位置；机器人R_j采样的加权的样本中，拥有最小权值的样本将被这个计算出的可能位置替换，此样本的权值将被接收到机器人R_i的预估位置信度所替换，此过程如以下公式所示：

${\mathrm{\ω}}_t^{R_{j,\min }}={\widehat{\mathrm{\ω}}}_t^{R_i}---\left(14\right)$

其中，分别为机器人R_j的拥有最小权值的样本及其权值；

最后，在重新采样后，机器人R_i和机器人R_j的位置更新。

实施例

针对两个机器人构成的机器人编队进行基于位置映射算法的多机器人协作定位仿真实验。实验选取的环境为近似矩形建筑物的楼道中，其形状如图6和图7所示的相似对称的方形轮廓所示。在试验中，设定对环境感知能力较弱的机器人R₁和对环境感知能力相对较强的机器人R₂，为了验证位置映射算法的有效性，设定机器人R₁在定位开始就处于“绑架(Kidnapped robot problem)”状态。在实验开始时，机器人R₁的初始位置位于坐标(0,8)处，但是机器人R₁认为自己位于坐标(0,0)的位置。由于机器人R₁和R₂都处于近似对称结构的环境中，机器人对自己被“绑架”的敏感度低，所以机器人R₁并未察觉自己定位失败，直到探测到一直精确定位的机器人R₂。当机器人R₂进入机器人R₁的探测范围时，机器人R₂将自己的预估位置及预估位置的信度发送给机器人R₁，此时机器人R₁再结合自身的探测信息由位置映射算法更新自己的位置，在“绑架”中恢复定位。

仿真结果如图6所示，其中，图(a)为机器人R₁的实际轨迹和样本分布轨迹图，图(b)为机器人R₂的实际轨迹和样本分布轨迹图，图中虚线B为样本分布变化的轨迹图，实线A为机器人实际运动的轨迹图。当机器人R₁和机器人R₂在T＝58.5s，分别处于(8.5,8.1)和(7.2,3.1)时，机器人R₂进入机器人R₁的探测范围，两机器人交换位置信息。如图6中图(a)所示，机器人R₁在定位失败中恢复时，其定位轨迹由位置1迅速变换到准确的位置2。

此仿真过程包含了发明所述的两种情况：只有一个机器人探测到对方的情况和两个机器人相互探测到对方的情况。当仿真开始时，两个机器人相互都探测不到对方，随后机器人R₂进入机器人R₁的探测范围，此时为只有一个机器人探测到对方的情况。随后两机器人以相同的方向运动一段时间，此时两机器人距离较近，为两个机器人相互探测到对方的情况，由图6可见，此种情况两机器人的样本分布轨迹和实际运动轨迹非常吻合，说明定位都非常准确。在随后的时间里，两机器人朝不同的方向运动，两机器人相互探测不到对方，由图6(a)可见，机器人R₁在此阶段出现了定位偏差，样本分布轨迹和实际运动轨迹出现了偏离。

图7中(a)图为机器人R₁在时间T＝30s、T＝58.5s、T＝59.4s、T＝126s定位的样本分布情况，图(b)为机器人在时间T＝30s、T＝58.5s、T＝59.4s、T＝126s定位的样本分布情况，为显示了两机器人在各个时间段定位的样本分布情况，图中的黑色麻点堆区域为样本的分布区域，代表着机器人的可能位置。图7(a)图中，在T＝30s时机器人R₁由于被“绑架”，其样本(图上所示的黑色麻点堆)和机器人的实际位置(图上所示的黑色箭头)分布在地图的不同位置，而机器人R₂一直准确定位，样本分布相对集中，如图7(b)所示。在T＝58.5s时，机器人R₂进入机器人R₁的探测范围，机器人R₁通过接收机器人R₂的位置信息结合自己的探测信息更新自己的位置。在T＝59.4s时，机器人R₁的样本分布到机器人的实际位置周围，完成位置更新，在被“绑架”中恢复准确定位。整个仿真过程中机器人R₂一直精确定位，仿真结果说明了机器人间的协同定位可以帮助定位能力相对较差的机器人实现快速准确的定位。

Claims (2)

1.一种基于位置映射算法的多机器人协作定位方法，用于在多机器人构成的机器人编队中多机器人间协作定位，其特征在于：每个机器人至少完成：探测的步骤；机器人之间进行位置信息交换的步骤；利用位置映射算法机器人更新自身位置的步骤；

所述探测的步骤中，机器人感知到信息其中和均代表的是信息流并假设每一次测量都是独立的， $Z_t^e=\{z_t^e,z_{t-1}^e,z_{t-2}^e,\·\·\·,z_0^e\}$ 为环境测量信息； $Z_t^d=\{z_t^{R_{i,1}},z_t^{R_{i,2}},\·\·\·,z_t^{R_{i,K}}\}$ 为机器人间的探测信息，表示机器人与其探测到的其它K个机器人之间的几何关系信息，其中j＝1,2,…,K表示在机器人R_i探测到机器人R_j时，机器人R_i获得的其与机器人R_j之间的几何关系信息；

所述机器人之间进行位置信息交换的步骤中，机器人R_i探测到其他K个机器人，那么机器人R_i将向其他K个机器人传递其位置及探测信息，同时，机器人R_i还接收其他K个机器人分别发送的它们各自的位置及探测信息，所述位置信息包括机器人对自身位置进行预估的预估位置l_t和该预估位置的信度所述预估位置的信度指的是机器人对其位置预估的确信度；

所述利用位置映射算法机器人更新自身位置的步骤中，机器人R_i根据自身的位置和探测信息以及从其他K个机器人接收到的它们各自的位置和探测信息，机器人可以计算出其自身有K个可能位置，即机器人计算出的可能位置与其接收到的其他机器人的预估位置之间是一一映射的关系，表示为其中表示与所接收的其他某一个机器人的预估位置相对应的机器人的一个可能位置；机器人利用其计算出的K个可能位置，经过位置映射算法，在重新采样后，实现更新自身位置；所述位置映射算法，即在粒子滤波方法中，用这K个可能位置依次替换采样的加权的样本中拥有最小权值的样本，这些样本的权值由接收到的其他K个机器人的预估位置的信度依次相应代替。

2.根据权利要求1所述的基于位置映射算法的多机器人协作定位方法，其特征在于：所述利用位置映射算法机器人更新自身位置，包括如下A、B两种情况：

A.只有一个机器人探测到对方的情况：即，机器人R_i可以探测到机器人R_j，而机器人R_j探测不到机器人R_i，则机器人R_i利用位置映射算法更新自身位置，包括如下步骤：

步骤一：机器人R_i感知到其与机器人R_j间的探测信息即机器人R_i与机器人R_j之间的几何关系信息，包括两者之间的间距d_i,j、由机器人R_i坐标连向机器人R_j坐标的连线与x轴正方向形成的夹角按下面公式进行计算：

其中θ_i是机器人R_i的朝向与x轴正方向形成的夹角；α_i,j代表机器人R_i的朝向与机器人R_i、R_j的坐标连线之间的夹角；

则由器人R_i提供的其与机器人R_j之间的探测信息为

步骤二：机器人R_i与机器人R_j之间相互交换各自的预估位置及预估位置的信度，且机器人R_i与机器人R_j之间还将共享探测信息并将机器人R_i的预估位置定义为所有样本的期望值，将机器人R_i的预估位置的信度定义为所有样本权值的最大值，如式(4)和式(5)所示：

$l_t^{R_i}=E\left(L_t^{R_i}\right)---\left(4\right)$

${\widehat{\mathrm{\ω}}}_t^{R_i}=\max \left({\mathrm{\ω}}_t^{R_i}\right)---\left(5\right)$

式(4)中，机器人R_i的预估位置是机器人R_i采样的加权的样本集合的期望值；式(5)中，为机器人R_i预估位置的信度；

机器人R_i将其预估位置预估位置的信度探测信息传递给机器人R_j，如式(6)所示：

$R_i\stackrel{l_t^{R_i},{\widehat{\mathrm{\ω}}}_t^{R_i},z_t^{R_{i,j}}}{\→}{R}_j---\left(6\right)$

机器人R_j按照相同的计算方法，因为机器人R_j并没有探测到机器人R_i，所以它只向机器人R_i传送自己的预估位置及预估位置的信度如式(7)和式(8)所示：

$l_t^{R_j}=E\left(L_t^{R_j}\right)---\left(7\right)$

${\widehat{\mathrm{\ω}}}_t^{R_j}=\max \left({\mathrm{\ω}}_t^{R_j}\right)---\left(8\right)$

式(7)中，机器人R_j的预估位置是机器人R_j采样的加权的样本集合的期望值；式(8)中，为机器人R_j预估位置的信度；

机器人R_j将其预估位置预估位置的信度传递给机器人R_j，如式(9)所示：

$R_j\stackrel{l_t^{R_j},{\widehat{\mathrm{\ω}}}_t^{R_j}}{\→}{R}_i---\left(9\right)$

步骤三：机器人R_i更新自身位置；

首先，机器人R_i根据机器人R_j的预估位置和该预估位置的信度计算一个可能位置；然后，机器人R_i采样的加权的样本中，拥有最小权值的样本将被这个计算出的可能位置替换，此样本的权值将被接收到机器人R_j的预估位置信度所替换，此过程如以下公式所示：

${\mathrm{\ω}}_t^{R_{i,\min }}={\widehat{\mathrm{\ω}}}_t^{R_j}---\left(11\right)$

其中，分别为机器人R_i的拥有最小权值的样本及其权值；

最后，在重新采样后，机器人R_i的位置更新；

B.两个机器人相互探测到对方的情况，则方法为：

步骤一：机器人R_i感知到其与机器人R_j间的探测信息同时，机器人R_j也感知到其与机器人R_i间的探测信息其中d_j,i为两者之间的间距、为由机器人R_j坐标连向机器人R_i坐标的连线与x轴正方向形成的夹角；

步骤二：机器人R_i与机器人R_j之间相互交换各自的预估位置及预估位置的信度，且机器人R_i与机器人R_j之间还共享探测信息和探测信息机器人R_i将其预估位置预估位置的信度探测信息传递给机器人R_j，如式(6)所示，同时，机器人R_j将其预估位置预估位置的信度探测信息传递给机器人R_i，如式(12)所示，

$R_j\stackrel{l_t^{R_j},{\widehat{\mathrm{\ω}}}_t^{R_j},z_t^{R_{j,i}}}{\→}{R}_i---\left(12\right)$

步骤三：机器人更新自身位置；

首先，根据最优探测信息选择规则，机器人R_i和机器人R_j分别从机器人R_i的探测信息和机器人R_j的探测信息中选择出最优的探测信息；所述最优探测信息选择规则：

1)如果探测信息来自于不同的传感器，机器人将选择精确度高的传感器所获得的信息；

2)如果探测信息来自于相同的传感器，机器人将选择自身传感器所获得的探测信息；

接下来，机器人R_i按照情况A中步骤三的方法更新自身位置；机器人R_j更新自己的位置方法与机器人R_i相同，机器人R_j根据机器人R_i的预估位置和该预估位置的信度计算一个可能位置；机器人R_j采样的加权的样本中，拥有最小权值的样本将被这个计算出的可能位置替换，此样本的权值将被接收到机器人R_i的预估位置信度所替换，此过程如以下公式所示：

${\mathrm{\ω}}_t^{R_{j,\min }}={\widehat{\mathrm{\ω}}}_t^{R_i}---\left(14\right)$

其中，分别为机器人R_j的拥有最小权值的样本及其权值；

最后，在重新采样后，机器人R_i和机器人R_j的位置更新。