CN103135117A - 一种分散式多机器人协同定位算法 - Google Patents

Abstract

本发明公开了一种分散式多机器人协同定位算法，针对安装有定位装置、可以相互通信的n个机器人（分别标识为R1,R2,…，Rn），包括k＝1时刻的协同定位计算和k+1(k＝1,2,…)时刻的协同定位计算等步骤。该方法是一种分散式贝叶斯估计算法，在不对系统模型进行解耦近似的前提下，机器人的运动测量信息、单机器人对外观测信息、机器人之间的相对观测信息等三类定位传感器数据在机器人本地融合；机器人之间的通信为点对点通信，包含两部分：一是发生相对观测的两个机器人之间相对观测信息的通信，二是所有机器人组成一个通信链，相邻机器人之间通信的内容为发送者融合了本地定位传感器数据后的机器人联合状态估计。该方法具有模块化、良好的可扩展性和好的容错性等优点。

Description

一种分散式多机器人协同定位算法

技术领域

本发明属于多机器人的定位算法，涉及一种分散式多机器人协同定位算法。

背景技术

随着科技发展，多机器人（包括室内机器人、室外机器人、无人机、水下潜航器等）系统正在科研、农业、医疗、军事等领域获得广泛应用。机器人定位（即确定机器人的位置、方位角等状态）往往是保证机器人正常工作的一项基本要求。单个机器人通过码盘、惯性测量单元等测量自身的运动信息，可以推算自身的状态。这种推算方式的定位误差会随着时间而累积。机器人可以通过对外观测来抑制这种定位误差的增长，如GPS观测、对已知地标的观测等。

多机器人协同定位（cooperative localization）利用机器人之间的相对观测（距离、相对方位等），通过一定的信息交换实现机器人之间定位资源共享，从而能够获得比各自定位更优的性能。例如，在一个机器人群中，某些机器人配备了高精度惯导系统和GPS（全球卫星定位系统），其他机器人则配备了码盘，如果机器人之间可以测量彼此之间的距离或相对方位，那么部分机器人的高精度导航信息可以在机器人之间共享，每个机器人的定位精度相比于独立定位时都会有所提高。又如，在进行协同定位的水下潜航器群中，某一个潜航器浮出水面进行GPS定位可以提高群中多个潜航器的定位精度，在减少机动能耗、增强隐蔽性的同时提高潜航器群的定位精度。

数据融合算法从结构上可以分为集中式（Centralised Data Fusion）、分层式（HierarchicalData Fusion）和分散式（Decentralised Data Fusion）三种基本形式。①集中式数据融合算法中，相关传感器的原始测量数据被传送到一个处理中心，由处理中心单独完成数据融合。②分层式数据融合算法中，传感器的原始测量数据在本地进行一定的处理，然后逐级上传，在每一级这些信息都进行一定的融合，最后在一个处理中心得到全局估计。③分散式数据融合算法中，每个节点处理本地的传感器数据，节点与节点之间可以进行点对点的通信。节点之间的地位是对等的，既不依赖于单一的处理中心，也没有等级之分。

分散式数据融合结构满足以下要求：没有中心节点；不要求有公共通信信道用以广播；单个节点不需要全局信息作为先验信息。采用分散式数据融合结构的系统应该具有以下优势：模块化；良好的可扩展性；好的容错性。在上述优势的吸引下，分散式协同定位算法成为机器人领域的研究热点。按照分散式数据融合的实现途径，现有的分散式协同定位算法可以分为复制型、解耦型和分布计算型。在复制型分散式协同定位算法中，每个机器人在功能上都相当于一个处理中心，对全体机器人进行定位。复制型分散式协同定位算法具有模块化和良好的容错性，但是可扩展性差。在解耦型分散式协同定位算法中，将协同定位问题解耦为多个相互独立的子问题，每个机器人求解一个或多个子问题。由于协同定位问题中各机器人状态两两相关，必须对这种相关进行近似才能实现解耦，因此解耦型算法会带来精度损失。分布计算型分散式协同定位算法是将某种集中式算法的运算拆分到各个平台上。现有分布计算型分散式协同定位算法对平台间的同步要求严格，并且往往需要在平台间广播信息，从而直接影响了算法的容错性和可扩展性。因此，有必要探求一种能够提升整体性能的分散式协同定位算法。

发明内容

针对现有分散式多机器人协同定位算法存在的技术问题，本发明的目的是提出一种分散式多机器人协同定位算法，该算法在不对系统模型进行解耦近似的前提下，能够充分实现分散式数据融合结构的三点优势：模块化、良好的可扩展性和好的容错性。

本发明提出的分散式多机器人协同定位算法，针对安装有定位装置、可以相互通信的n个机器人（分别标识为R1,R2,…,Rn），包括以下步骤：

步骤一、k＝1时刻的协同定位计算即估计

其中

表示机器人Ri（i∈{1，…,n}）在k=1时刻的状态，k=1时刻的协同定位计算由全体机器人在时间段[k,k+1)(k=1)内共同完成，具体包含以下通信和计算：

1）在k(k＝1)时刻，如果机器人Ri(i＝1,…,n)对机器人Rj(j＝1,…,n，j≠i)进行了相对观测

那么机器人Ri把相对观测信息发送给机器人Rj；

2）在时间段[k,k+1)(k=1)内，全体机器人满足链式通信拓扑结构；从通信链的一端开始，按照在通信链上的位置将机器人依次记为{1}₁,{2}₁，…,{n}₁；

机器人{i}₁(i＝1,…,n)根据公式(1)进行状态预测计算：

$p\left(x_1^{{\left\{i\right\}}_1}\right)=\∫p\left(x_0^{{\left\{i\right\}}_1}\right)\·p\left(x_1^{{\left\{i\right\}}_1}\right|x_0^{{\left\{i\right\}}_1})d{x}_0^{{\left\{i\right\}}_1}---\left(1\right)$

式中

表示机器人{i}₁的初始状态，p(·)表示随机变量的概率密度函数，p(·|·)表示条件概率密度函数；

如果机器人{i}₁(i＝1,…,n)进行了单机器人对外观测

则根据公式(2)进行对外观测更新计算：

$p\left(x_1^{{\left\{i\right\}}_1}\right|z_1^{{\left\{i\right\}}_1})=\frac{p\left(x_1^{{\left\{i\right\}}_1}\right)p\left(z_1^{{\left\{i\right\}}_1}\right|x_1^{{\left\{i\right\}}_1})}{\∫p\left(x_1^{{\left\{i\right\}}_1}\right)p\left(z_1^{{\left\{i\right\}}_1}\right|x_1^{{\left\{i\right\}}_1})d{x}_1^{{\left\{i\right\}}_1}}---\left(2\right)$

将对外观测

的数值代入上式左边条件概率

得到进行对外观测更新后的状态估计，记为

在语义明确的情况下可以略去观测量，简记为

3）用表示[k，k+1)(k=1)时间段内通信链上前i-1个机器人{{1}₁,…,{i-1}₁}所融合的观测量集合，机器人{i}₁(i＝2,…,n)接收机器人{i-1}₁发来的消息

然后根据公式(3)计算联合状态分布：

$p(x_1^{{\left\{1\right\}}_1},\·\·\·,x_1^{{\left\{i\right\}}_1})=p\left(x_1^{{\left\{i\right\}}_1}\right)\·(p_1^{{\left\{1\right\}}_1},\·\·\·,x_1^{{\{i-1\}}_1})---\left(3\right)$

所得估计结果为 $p(x_1^{{\left\{1\right\}}_1},\·\·\·,x_1^{{\left\{i\right\}}_1})|(z_1^{{\left\{i\right\}}_1}\∪Z_1^{i-1});$

如果存在涉及机器人{i}₁(i＝2,…,n)的相对观测

或者

则机器人{i}₁(i＝2,…,n)根据公式(4)进行以下相对观测更新计算：

$p(x_1^{{\left\{1\right\}}_1},\&CenterDot;\&CenterDot;\&CenterDot;,x_1^{{\left\{i\right\}}_1}|Z_1^{i\&CenterDot;})=p(x_1^{{\left\{1\right\}}_1},\&CenterDot;\&CenterDot;\&CenterDot;,x_1^{{\left\{i\right\}}_1})\&CenterDot;\frac{\underset{j<i}{\mathrm{\&Pi;}}p\left(z_1^{{\left\{i\right\}}_1{\left\{j\right\}}_1}\right|x_1^{{\left\{i\right\}}_1},x_1^{{\left\{j\right\}}_1})p\left(z_1^{{\left\{j\right\}}_1{\left\{i\right\}}_1}\right|x_1^{{\left\{i\right\}}_1},x_1^{{\left\{j\right\}}_1})}{\underset{j<i}{\mathrm{\&Pi;}}p\left(z_1^{{\left\{i\right\}}_1{\left\{j\right\}}_1}\right)p\left(z_1^{{\left\{j\right\}}_1{\left\{i\right\}}_1}\right)}---\left(4\right)$

式中 $p\left(z_1^{{\left\{i\right\}}_1{\left\{j\right\}}_1}\right)=\&Integral;\&CenterDot;\&CenterDot;\&CenterDot;\&Integral;p(x_1^{{\left\{1\right\}}_1},\&CenterDot;\&CenterDot;\&CenterDot;,x_1^{{\left\{i\right\}}_1})p\left(z_1^{{\left\{i\right\}}_1{\left\{j\right\}}_1}\right|x_1^{{\left\{i\right\}}_1},x_1^{\left\{j\right\}}1)d{x}_1^{{\left\{1\right\}}_1}\&CenterDot;\&CenterDot;\&CenterDot;{\mathrm{dx}}_1^{{\left\{i\right\}}_1},$

$p\left(z_1^{{\left\{j\right\}}_1{\left\{i\right\}}_1}\right)=\&Integral;\&CenterDot;\&CenterDot;\&CenterDot;\&Integral;p(x_1^{{\left\{1\right\}}_1},\&CenterDot;\&CenterDot;\&CenterDot;,x_1^{{\left\{i\right\}}_1})p\left(z_1^{{\left\{j\right\}}_1{\left\{i\right\}}_1}\right|x_1^{{\left\{i\right\}}_1},x_1^{{\left\{j\right\}}_1})d{x}_1^{{\left\{1\right\}}_1}\&CenterDot;\&CenterDot;\&CenterDot;d{x}_1^{{\left\{i\right\}}_1};$ 表示机器人{i}₁所融合的k＝1时刻的机器人之间相对观测量的集合，即 $Z_1^{i\&CenterDot;}=\left\{z_1^{{\left\{i\right\}}_1{\left\{j\right\}}_1}\right|j<i\}\&cup;\{z_1^{{\left\{j\right\}}_1{\left\{i\right\}}_1}|j<i\};$ 记 $Z_1^i=Z_1^{i\&CenterDot;}\&cup;z_1^{{\left\{i\right\}}_1}\&cup;Z_1^{i-1},$ 则进行相对观测更新后的联合状态估计为简记为

4）机器人{i}₁(i＝1,…,n-1)将估计结果

发送给机器人{i+1}₁，当机器人{n}₁按照公式(1)-(4)完成计算后，得到k=1时刻全体机器人协同定位的结果

其中

是全体机器人在k＝1时刻的观测量集合，包括单个机器人的对外观测和机器人之间的相对观测；

步骤二、k+1(k=1,2,…)时刻的协同定位计算即估计其中

表示机器人Ri（i∈{1，…,n}）在k+1时刻的状态，k+1(k=1,2,…)时刻的协同定位计算由全体机器人在时间段[k+1,k+2)(k=1,2,…)内共同完成，具体包含以下通信和计算：

1）在k+1(k＝1,2,…)时刻，如果机器人Ri(i＝1,…,n)对机器人Rj(j＝1,…,n,j≠i)进行了相对观测

那么机器人Ri把相对观测信息发送给机器人Rj；

2）在时间段[k+1,k+2)(k=1,2,…)内，以k时刻协同定位的完成机器人{n}_k为起点，n个机器人建立一条新的通信链，按照在通信链上的位置将机器人依次记为{1}_k+1，{2}_k+1,…,{n}_k+1；

与k＝1时刻类似，用

表示时间段[k+1,k+2)(k=1,2,…)内通信链上前i个机器人{{1}_k+1，…,{i}_k+1}所融合的观测量集合，k时刻协同定位的完成机器人{n}_k获得全体机器人协同定位的结果

其中

是全体机器人截至k（k＝1,2,…）时刻历史观测量的集合，即

机器人{1}_k+1，也即{n}_k，对联合状态估计中的状态顺序进行调整，得到

k+1(k＝1,2,…)时刻的协同定位计算由机器人{1}_k+1启动，机器人{i}_k+1(i＝2,…,n)在接收到机器人{i-1}_k+1发来的消息

之后，启动本地计算；

机器人{i}_k+1(i＝1,…,n)根据公式(5)进行状态预测计算：

$p(x_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_k^{{\left\{n\right\}}_{k+1}})=\&Integral;p(x_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_{k+1}^{{\{i-1\}}_{k+1}},x_k^{{\left\{i\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_k^{{\left\{n\right\}}_{k+1}})\&CenterDot;p\left(x_{k+1}^{{\left\{i\right\}}_{k+1}}\right|x_k^{{\left\{i\right\}}_{k+1}})d{x}_k^{{\left\{i\right\}}_{k+1}}---\left(5\right)$

如果机器人{i}_k+1(i＝1,…,n)进行了单机器人对外观测则根据公式(6)进行对外观测更新计算：

$p(x_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_k^{{\left\{n\right\}}_{k+1}}|z_{k+1}^{{\left\{i\right\}}_{k+1}})=\frac{p(x_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_k^{{\left\{n\right\}}_{k+1}})p\left(z_{k+1}^{{\left\{i\right\}}_{k+1}}\right|x_{k+1}^{{\left\{i\right\}}_{k+1}})}{p\left(z_{k+1}^{{\left\{i\right\}}_{k+1}}\right)}---\left(6\right)$

式中， $p\left(z_{k+1}^{{\left\{i\right\}}_{k+1}}\right)=\&Integral;\&CenterDot;\&CenterDot;\&CenterDot;\&Integral;p(x_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_k^{{\left\{n\right\}}_{k+1}})p\left(z_{k+1}^{{\left\{i\right\}}_{k+1}}\right|x_{k+1}^{{\left\{i\right\}}_{k+1}})d{x}_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,d{x}_{k+1}^{{\left\{i\right\}}_{k+1}},d{x}_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,d{x}_k^{{\left\{n\right\}}_{k+1}},$ 所得估计结果为

简记为 $p(x_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_k^{{\left\{n\right\}}_{k+1}});$

如果存在涉及机器人{i}_k+1(i＝2,…,n)的相对观测

或者

则机器人{i}_k+1(i＝2,…,n)根据公式(7)进行相对观测更新计算：

$p(x_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_k^{{\left\{n\right\}}_{k+1}}|Z_{k+1}^{i\&CenterDot;})=$

$\frac{p(x_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_k^{{\left\{n\right\}}_{k+1}})\underset{j<i}{\mathrm{\&Pi;}}p\left(z_{k+1}^{{\left\{i\right\}}_{k+1}{\left\{j\right\}}_{k+1}}\right|x_{k+1}^{{\left\{i\right\}}_{k+1}},x_{k+1}^{{\left\{j\right\}}_{k+1}})p\left(z_{k+1}^{{\left\{j\right\}}_{k+1}{\left\{i\right\}}_{k+1}}\right|x_{k+1}^{{\left\{i\right\}}_{k+1}},x_{k+1}^{{\left\{j\right\}}_{k+1}})}{\underset{j<i}{\mathrm{\&Pi;}}p\left(z_{k+1}^{{\left\{i\right\}}_{k+1}{\left\{j\right\}}_{k+1}}\right)p\left(z_{k+1}^{{\left\{j\right\}}_{k+1}{\left\{i\right\}}_{k+1}}\right)}---\left(7\right)$

式中， $p\left(z_{k+1}^{{\left\{i\right\}}_{k+1}{\left\{j\right\}}_{k+1}}\right)=\&Integral;\&CenterDot;\&CenterDot;\&CenterDot;\&Integral;p(x_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_k^{{\left\{n\right\}}_{k+1}})p\left(z_{k+1}^{{\left\{i\right\}}_{k+1}{\left\{j\right\}}_{k+1}}\right|x_{k+1}^{{\left\{i\right\}}_{k+1}},x_{k+1}^{{\left\{j\right\}}_{k+1}})d{x}_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,{\mathrm{dx}}_k^{{\left\{i\right\}}_{k+1}},{\mathrm{dx}}_k^{{\{i+1\}}_{k+1}}\&CenterDot;\&CenterDot;\&CenterDot;,{\mathrm{dx}}_k^{{\left\{n\right\}}_{k+1}},$ $p\left(z_{k+1}^{{\left\{j\right\}}_{k+1}{\left\{i\right\}}_{k+1}}\right)=\&Integral;\&CenterDot;\&CenterDot;\&CenterDot;\&Integral;p(x_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_k^{{\left\{n\right\}}_{k+1}})p\left(z_{k+1}^{{\left\{j\right\}}_{k+1}{\left\{i\right\}}_{k+1}}\right|x_{k+1}^{{\left\{i\right\}}_{k+1}},x_{k+1}^{{\left\{j\right\}}_{k+1}})d{x}_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,{\mathrm{dx}}_k^{{\left\{i\right\}}_{k+1}},{\mathrm{dx}}_k^{{\{i+1\}}_{k+1}}\&CenterDot;\&CenterDot;\&CenterDot;,{\mathrm{dx}}_k^{{\left\{n\right\}}_{k+1}};$ 式中

表示机器人{i}_k+1所融合的k+1时刻的机器人之间相对观测量的集合，即 $Z_{k+1}^{i\&CenterDot;}=\left\{z_{k+1}^{{\left\{i\right\}}_{k+1}{\left\{j\right\}}_{k+1}}\right|j<i\}\&cup;\{z_{k+1}^{{\left\{j\right\}}_{k+1}{\left\{i\right\}}_{k+1}}|j<i\};$ 所得估计结果为其中 $Z_{k+1}^i=Z_{k+1}^{i\&CenterDot;}\&cup;z_{k+1}^{{\left\{i\right\}}_{k+1}}\&cup;Z_{k+1}^{i-1};$

3）机器人{i}_k+1(i＝1,…,n-1)将联合状态估计

发送给机器人{i+1}_k+1；

当机器人{n}_k+1完成计算后，得到k+1(k＝1,2,…)时刻全体机器人协同定位的结果

其中

与现有技术相比，本发明的优点在于：

（1）精度：本发明没有对系统模型进行解耦近似，避免了相应的精度损失；

（2）可扩展性：机器人之间所有的通信都是点对点通信，不需要广播；机器人之间的通信链可以动态调整，降低了通信对机器人空间位置的约束；

（3）模块化：每个机器人的工作模式相同，接收邻居机器人发送的消息（启动机器人除外），融合本地定位传感器数据，将融合结果发送给另一个邻居机器人（完成机器人除外）；

（4）容错性：当有机器人发生故障时，非故障机器人可以自组织起来，继续进行协同定位。

附图说明

图1是本发明k(k=1,2,…)时刻，机器人Ri(i∈{1，…,n})对机器人Rj(j∈{1，…,n},j≠i)进行相对测量时，机器人Ri(i∈{1，…,n})向机器人Rj(j∈{1，…,n},j≠i)发送的信息的示意图。

图2是本发明k=1时刻的协同定位计算中，机器人{i}₁(i＝2,…,n-1)执行的计算及与通信链上相邻机器人之间的通信内容示意图。

图3是本发明k+1(k=1,2,…)时刻的协同定位计算中，机器人{i}_k+1(i＝2,…,n-1)执行的计算及与通信链上相邻机器人之间的通信内容示意图。

具体实施方式

下面以三个机器人的协同定位说明本发明的方法。

三个机器人R1、R2、R3分别装备了码盘用以测量自身运动信息；机器人R1安装有GPS接收机，可以测得自身的位置；机器人R2安装有激光雷达，可以测量到其他机器人的距离；三个机器人通过无线网络进行通信。

公式(1)-(7)给出的是一般计算形式，具体计算形式与所涉及的概率分布有关。对于高斯分布，公式(1)-(7)中的计算结果具有解析表达式；对于非高斯分布，公式(1)-(7)中的计算结果往往没有解析表达式，可以采取Monte-Carlo仿真的方法求解。对于非高斯系统，可以通过线性化手段和高斯假设建立近似的高斯系统模型，从而进行解析求解。

下面结合实施例说明公式(1)-(7)中变量及概率密度函数的具体含义。设机器人Ri(i＝1,2,3)的运动方程为

$x_{k+1}^{\mathrm{Ri}}=f_k^{\mathrm{Ri}}(x_k^{\mathrm{Ri}},u_k^{\mathrm{Ri}})+w_k^{\mathrm{Ri}},$ k=0,1,2…，    (8)这里，机器人的状态x=[x y θ]^T，其中(x,y)是机器人的位置坐标，θ是机器人的方位角；码盘测量信息u=[V ω]^T，其中V是机器人平移的线速度，ω是机器人转动的角速度；设噪声

为高斯白噪声，记作

这里用N(m,q)表示均值为m、方差为q的高斯分布。

当运动方程(8)中

是非线性函数时，在

的估计均值

处对(8)进行一阶泰勒展开，得到近似的线性方程，

$x_{k+1}^{\mathrm{Ri}}=\&dtri;f_k^{\mathrm{Ri}}\&CenterDot;x_k^{\mathrm{Ri}}-\&dtri;f_k^{\mathrm{Ri}}\&CenterDot;{\hat{x}}_k^{\mathrm{Ri}}+f_k^{\mathrm{Ri}}({\hat{x}}_k^{\mathrm{Ri}},u_k^{\mathrm{Ri}})+w_k^{\mathrm{Ri}},---\left(9\right)$

式中

式(9)定义了高斯条件分布 $p\left(x_{k+1}^{\mathrm{Ri}}\right|x_k^{\mathrm{Ri}})=N(\&dtri;f_k^{\mathrm{Ri}}\&CenterDot;x_k^{\mathrm{Ri}}-\&dtri;f_k^{\mathrm{Ri}}\&CenterDot;{\hat{x}}_k^{\mathrm{Ri}}+f_k^{\mathrm{Ri}}({\hat{x}}_k^{\mathrm{Ri}},u_k^{\mathrm{Ri}}),Q_k^{\mathrm{Ri}}).$

k(k=1,2,…)时刻机器人R1获得的GPS观测为自身的位置，记作

观测方程为

$z_k^{R1}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right]x_k^{R1}+v_k^{R1},---\left(10\right)$

$\stackrel{\mathrm{\&Delta;}}{=}H\&CenterDot;x_k^{R1}+v_k^{R1}$

设

则上式定义了高斯条件分布 $p\left(z_k^{R1}\right|x_k^{R1})=N(H\&CenterDot;x_k^{R1},W_k^{R1}).$

k(k=1,2,…)时刻机器人R2用激光雷达测得到机器人Rj(j∈{1,3})的距离，记作

观测方程为

$z_k^{R2\mathrm{Rj}}=\sqrt{{(x_k^{R2}\left(1\right)-x_k^{\mathrm{Rj}}\left(1\right))}^2+{(x_k^{R2}\left(2\right)-x_k^{\mathrm{Rj}}\left(2\right))}^2}+v_k^{R2\mathrm{Rj}},$ (11)

$\stackrel{\mathrm{\&Delta;}}{=}h(x_k^{R2},x_k^{\mathrm{Rj}})+v_k^{R2\mathrm{Rj}}$

式中

对上式进行线性化，得到

$z_k^{R2\mathrm{Rj}}=\&dtri;h_k^{R2}\&CenterDot;x_k^{R2}+\&dtri;h_k^{\mathrm{Rj}}\&CenterDot;x_k^{\mathrm{Rj}}+h({\hat{x}}_k^{R2},{\hat{x}}_k^{\mathrm{Rj}})-\&dtri;h_k^{R2}\&CenterDot;{\hat{x}}_k^{R1}-\&dtri;h_k^{\mathrm{Rj}}\&CenterDot;{\hat{x}}_k^{\mathrm{Rj}}+v_k^{R2\mathrm{Rj}},---\left(12\right)$

式中， $\&dtri;h_k^{R2}=\frac{\&PartialD;h}{\&PartialD;x_k^{R2}}{|}_{x_k^{R2}={\hat{x}}_k^{R2}}=\left[\begin{array}{ccc}\frac{{\hat{x}}_k^{R2}\left(1\right)-{\hat{x}}_k^{\mathrm{Rj}}\left(1\right)}{h({\hat{x}}_k^{R2},{\hat{x}}_k^{\mathrm{Rj}})}& \frac{{\hat{x}}_k^{R2}\left(2\right)-{\hat{x}}_k^{\mathrm{Rj}}\left(2\right)}{h({\hat{x}}_k^{R2},{\hat{x}}_k^{\mathrm{Rj}})}& 0\end{array}\right],$ $\&dtri;h_k^{\mathrm{Rj}}=\frac{\&PartialD;h}{\&PartialD;x_k^{\mathrm{Rj}}}{|}_{x_k^{\mathrm{Rj}}={\hat{x}}_k^{\mathrm{Rj}}}=-\&dtri;h_k^{R2}.$

上式定义了高斯条件分布 $p\left(z_k^{R2\mathrm{Rj}}\right|x_k^{R2},x_k^{\mathrm{Rj}})=N(\&dtri;h_k^{R2}\&CenterDot;x_k^{R2}+\&dtri;h_k^{\mathrm{Rj}}\&CenterDot;x_k^{\mathrm{Rj}}+h({\hat{x}}_k^{R2},{\hat{x}}_k^{\mathrm{Rj}})-\&dtri;h_k^{R2}\&CenterDot;{\hat{x}}_k^{R2}-\&dtri;h_k^{\mathrm{Rj}}\&CenterDot;{\hat{x}}_k^{\mathrm{Rj}},W_k^{R2\mathrm{Rj}}).$

采用本发明的方法，三个机器人进行协同定位的过程如下：

（1）k＝1时刻的协同定位计算即估计

其中

表示机器人Ri（i∈{1,2,3}）在k=1时刻的状态。k=1时刻的协同定位计算由3个机器人在时间段[k，k+1)(k=1)内共同完成，具体包含以下通信和计算：

①在k(k=1)时刻，机器人Ri(i＝2)对机器人Rj(j=1,3)进行了相对观测机器人Ri(i＝2)把相对观测信息发送给机器人Rj(j=1,3)；

②在时间段[k,k+1)(k=1)内，全体机器人满足链式通信拓扑结构；从通信链的一端开始，按照在通信链上的位置将机器人依次记为{1}₁,{2}₁,{3}₁，分别对应机器人R2、R1和R3；

机器人{i}₁(i＝1,2,3)按照公式（1）进行状态预测：

$p\left(x_1^{{\left\{i\right\}}_1}\right)=\&Integral;p\left(x_0^{{\left\{i\right\}}_1}\right)\&CenterDot;p\left(x_1^{{\left\{i\right\}}_1}\right|x_0^{{\left\{i\right\}}_1})d{x}_0^{{\left\{i\right\}}_1}---\left(1\right)$

机器人{1}₁对应机器人R2，机器人{2}₁对应机器人R1，机器人{3}₁对应机器人R3，设机器人Ri(i＝1,2,3)的初始状态

服从高斯分布，

的均值和方差分别为

由公式(9)定义， $p\left(x_1^{\mathrm{Ri}}\right|x_0^{\mathrm{Ri}})=N({\&dtri;f}_0^{\mathrm{Ri}}\&CenterDot;x_0^{\mathrm{Ri}}-{\&dtri;f}_0^{\mathrm{Ri}}\&CenterDot;{\hat{x}}_0^{\mathrm{Ri}}+f_0^{\mathrm{Ri}}({\hat{x}}_0^{\mathrm{Ri}},u_0^{\mathrm{Ri}}),Q_0^{\mathrm{Ri}}),$ 代入公式（1）得

$p\left(x_1^{\mathrm{Ri}}\right)=N(f_0^{\mathrm{Ri}}({\hat{x}}_0^{\mathrm{Ri}},u_0^{\mathrm{Ri}}),{\&dtri;f}_0^{\mathrm{Ri}}{P}_0^{\mathrm{Ri}}{\left({\&dtri;f}_0^{\mathrm{Ri}}\right)}^T+Q_0^{\mathrm{Ri}})$ (13)

$\stackrel{\mathrm{\&Delta;}}{=}N(m_i,q_i)$

机器人R1获得GPS观测

按照公式（2）进行对外观测更新，有

$p\left(x_1^{R1}\right|z_1^{R1})=\frac{p\left(x_1^{R1}\right)p\left(z_1^{R1}\right|x_1^{R1})}{\&Integral;p\left(x_1^{R1}\right)p\left(z_1^{R1}\right|x_1^{R1}){\mathrm{dx}}_1^{R1}}---\left(14\right)$

式中

由公式(10)定义， $p\left(z_1^{R1}\right|x_1^{R1})=N(H\&CenterDot;x_1^{R1},W_1^{R1}),$ 将GPS观测值代入上式左边条件概率得到进行对外观测更新后的状态估计，记为

$p\left(x_1^{R1}\right)|z_1^{R1}=N(m_1+K_1(z_1^{R1}-{\mathrm{Hm}}_1),q_1-K_1{\mathrm{Hq}}_1)---\left(15\right)$ $\stackrel{\mathrm{\&Delta;}}{=}N(m_1^{\&prime;},q_1^{\&prime;})$

式中，

在语义明确的情况下可以略去观测量，简记为

机器人R2和机器人R3没有进行对外观测，无需进行对外观测更新；

③用

表示[k，k+1)(k=1)时间段内通信链上前i-1个机器人{{1}₁,…,{i-1}₁}所融合的观测量集合。机器人{i}₁(i＝2,3)接收机器人{i-1}₁发来的消息

然后按照公式（3）计算联合状态分布：

$p(x_1^{{\left\{1\right\}}_1},...,x_1^{{\left\{i\right\}}_1})=p\left(x_1^{{\left\{i\right\}}_1}\right)\&CenterDot;p(x_1^{{\left\{1\right\}}_1},...,x_1^{{\{i-1\}}_1})---\left(3\right)$

所得估计结果为 $p(x_1^{{\left\{1\right\}}_1},\&CenterDot;\&CenterDot;\&CenterDot;,x_1^{{\left\{i\right\}}_1})|(z_1^{{\left\{i\right\}}_1}\&cup;Z_1^{i-1});$

如果存在涉及机器人{i}₁(i＝2,3)的相对观测

或者

则机器人{i}₁(i＝2,3)按照公式（4）进行以下相对观测更新计算：

$p(x_1^{{\left\{1\right\}}_1},...,x_1^{{\left\{i\right\}}_1}|z_1^{i\&CenterDot;})=p(x_1^{{\left\{1\right\}}_1},...,x_1^{{\left\{i\right\}}_1})\&CenterDot;\frac{\underset{j<i}{\mathrm{\&Pi;}}p\left(z_1^{{\left\{i\right\}}_1{\left\{j\right\}}_1}\right|x_1^{{\left\{i\right\}}_1},x_1^{{\left\{j\right\}}_1})p\left(z_1^{{\left\{j\right\}}_1{\left\{i\right\}}_1}\right|x_1^{{\left\{i\right\}}_1},x_1^{{\left\{j\right\}}_1})}{\underset{j<i}{\mathrm{\&Pi;}}p\left(z_1^{{\left\{i\right\}}_1{\left\{j\right\}}_1}\right)p\left(z_1^{{\left\{j\right\}}_1{\left\{i\right\}}_1}\right)}---\left(4\right)$

式中 $p\left(z_1^{{\left\{i\right\}}_1{\left\{j\right\}}_1}\right)=\&Integral;...\&Integral;p(x_1^{{\left\{1\right\}}_1},...,x_1^{{\left\{i\right\}}_1})p\left(z_1^{{\left\{i\right\}}_1{\left\{j\right\}}_1}\right|x_1^{{\left\{i\right\}}_1},x_1^{{\left\{j\right\}}_1}){\mathrm{dx}}_1^{{\left\{1\right\}}_1}...{\mathrm{dx}}_1^{{\left\{i\right\}}_1},$

$p\left(z_1^{{\left\{j\right\}}_1{\left\{i\right\}}_1}\right)=\&Integral;...\&Integral;p(x_1^{{\left\{1\right\}}_1},...,x_1^{{\left\{i\right\}}_1})p\left(z_1^{{\left\{j\right\}}_1{\left\{i\right\}}_1}\right|x_1^{{\left\{i\right\}}_1},x_1^{{\left\{j\right\}}_1}){\mathrm{dx}}_1^{{\left\{1\right\}}_1}...{\mathrm{dx}}_1^{{\left\{i\right\}}_1};$ 表示机器人{i}₁所融合的k＝1时刻的机器人之间相对观测量的集合，即 $Z_1^{i\&CenterDot;}=\left\{z_1^{{\left\{i\right\}}_1{\left\{j\right\}}_1}\right|j<i\}\&cup;\{z_1^{{\left\{j\right\}}_1{\left\{i\right\}}_1}|j<i\};$ 记 $Z_1^i=Z_1^{i\&CenterDot;}\&cup;z_1^{{\left\{i\right\}}_1}\&cup;Z_1^{i-1},$ 则进行相对观测更新后的联合状态估计为

简记为

对于机器人{2}₁，接收机器人{1}₁发来的信息

也即

然后按照公式(3)求得联合分布

即

其均值和方差分别为

$\left(\begin{array}{c}{m}_2\\ m_1^{\&prime;}\end{array}\right),\left[\begin{array}{cc}{q}_2& 0\\ 0& q_1^{\&prime;}\end{array}\right],---\left(16\right)$

存在涉及机器人{i}₁（i＝2）的相对观测 $z_1^{{\left\{i\right\}}_1{\left\{j\right\}}_1}(j=1,j<i),$ 即

机器人{2}₁按照公式(4)进行距离观测更新，得

其均值和方差分别为

$\left(\begin{array}{c}{m}_2^{\&prime;}\\ m_1^{\&prime;\&prime;}\end{array}\right)=\left(\begin{array}{c}{m}_2\\ m_1^{\&prime;}\end{array}\right)+K_2(z_1^{R2R1}-{\&dtri;h}_1^{R1}{m}_1^{\&prime;}-{\&dtri;h}_1^{R2}{m}_2)---\left(17\right)$

$\left[\begin{array}{cc}{q}_2^{\&prime;}& q_{21}\\ q_{21}^T& q_1^{\&prime;\&prime;}\end{array}\right]=\left[\begin{array}{cc}{q}_2& 0\\ 0& q_1^{\&prime;}\end{array}\right]-K_2\left[\begin{array}{cc}{\&dtri;h}_1^{R2}& {\&dtri;h}_1^{R1}\end{array}\right]\left[\begin{array}{cc}{q}_2& 0\\ 0& q_1^{\&prime;}\end{array}\right]---\left(18\right)$

式中 $K_2=\frac{1}{{\&dtri;h}_1^{R1}{q}_1^{\&prime;}{\left({\&dtri;h}_1^{R1}\right)}^T+{\&dtri;h}_1^{R2}{q}_2{\left({\&dtri;h}_1^{R2}\right)}^T+W_1^{R2R1}}\left[\begin{array}{c}{q}_2{\left({\&dtri;h}_1^{R2}\right)}^T\\ q_1^{\&prime;}{\left({\&dtri;h}_1^{R1}\right)}^T\end{array}\right];$

对于机器人{3}₁，接收机器人{2}₁发送的信息

然后按照公式(3)求得联合分布

其均值和方差分别为

$\left(\begin{array}{c}{m}_2^{\&prime;}\\ m_1^{\&prime;\&prime;}\\ m_3\end{array}\right),\left[\begin{array}{ccc}{q}_2^{\&prime;}& q_{21}& 0\\ q_{21}^T& q_1^{\&prime;\&prime;}& 0\\ 0& 0& q_3\end{array}\right]---\left(19\right)$

存在涉及机器人{i}₁（i＝3）的相对观测 $z_1^{{\left\{j\right\}}_1{\left\{i\right\}}_1}(j=2,j<i),$ 即

机器人{3}₁按照公式(4)进行距离观测更新，得

其均值和方差分别为

$\left(\begin{array}{c}{m}_2^{\&prime;\&prime;}\\ m_1^{\&prime;\&prime;}\\ m_3^{\&prime;}\end{array}\right)+\left(\begin{array}{c}{m}_2^{\&prime;}\\ m_1^{\&prime;\&prime;}\\ m_3\end{array}\right)+K_3(z_1^{R2R3}-{\&dtri;h}_1^{R2}{m}_2^{\&prime;}-{\&dtri;h}_1^{R3}{m}_3)---\left(20\right)$

$\left[\begin{array}{ccc}{q}_2^{\&prime;\&prime;}& q_{21}^{\&prime;}& q_{23}\\ {\left(q_{21}^{\&prime;}\right)}^T& q_1^{\&prime;\&prime;\&prime;}& q_{13}\\ q_{23}^T& q_{13}^T& q_3^{\&prime;}\end{array}\right]=\left[\begin{array}{ccc}{q}_2^{\&prime;}& q_{21}& 0\\ q_{21}^T& q_1^{\&prime;\&prime;}& 0\\ 0& 0& q_3\end{array}\right]-K_3\left[\begin{array}{ccc}{\&dtri;h}_1^{R2}& 0& {\&dtri;h}_1^{R3}\end{array}\right]\left[\begin{array}{ccc}{q}_2^{\&prime;}& q_{21}& 0\\ q_{21}^T& q_1^{\&prime;\&prime;}& 0\\ 0& 0& q_3\end{array}\right]---\left(21\right)$

式中 $K_3=\frac{1}{{\&dtri;h}_1^{R2}{q}_2^{\&prime;}{\left({\&dtri;h}_1^{R2}\right)}^T+{\&dtri;h}_1^{R3}{q}_3{\left({\&dtri;h}_1^{R3}\right)}^T+W_1^{R2R3}}\left[\begin{array}{c}{q}_2^{\&prime;}{\left({\&dtri;h}_1^{R2}\right)}^T\\ q_{21}^T{\left({\&dtri;h}_1^{R2}\right)}^T\\ q_3{\left({\&dtri;h}_1^{R3}\right)}^T\end{array}\right];$

④机器人{i}₁(i＝1,2)将估计结果发送给机器人{i+1}₁。当机器人{3}₁按照公式(1)-(4)完成计算后，得到k=1时刻全体机器人协同定位的结果

其中

是3个机器人在k＝1时刻的观测量集合，包括单个机器人的对外观测和机器人之间的相对观测。

（2）k+1(k=1,2,…)时刻的协同定位计算即估计

其中

表示机器人Ri（i∈{1,2,3}）在k+1时刻的状态。k+1(k=1,2,…)时刻的协同定位计算由全体机器人在时间段[k+1,k+2)(k=1,2,…)内共同完成，具体包含以下通信和计算：

①在k+1(k＝1,2,…)时刻，如果机器人Ri(i＝2)对机器人Rj(j＝1,3)进行了相对观测

那么机器人Ri(i＝2)把相对观测信息发送给机器人Rj(j＝1,3)；

②在时间段[k+1,k+2)(k=1,2,…)内，以k时刻协同定位的完成机器人{3}_k为起点，3个机器人建立一条新的通信链，按照在通信链上的位置将机器人依次记为{1}_k+1，{2}_k+1,{3}_k+1；

与k＝1时刻类似，用

表示通信链上前i个机器人{{1}_k+1，…,{i}_k+1}所融合的k+1时刻的观测量集合。k时刻协同定位的完成机器人{3}_k获得全体机器人协同定位的结果

其中

是全体机器人截至k（k＝1,2,…）时刻历史观测量的集合，即

机器人{1}_k+1，也即{n}_k，对联合状态估计

中的状态顺序进行相应调整，得到

k+1(k＝1,2,…)时刻的协同定位计算由机器人{1}_k+1启动。机器人{i}_k+1(i＝2,3)在接收到机器人{i-1}_k+1发来的消息

之后，启动本地计算；

机器人{i}_k+1(i＝1,2,3)按照公式(5)进行状态预测，计算如下：

$p(x_{k+1}^{{\left\{1\right\}}_{k+1}},...,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},x_k^{{\left\{3\right\}}_{k+1}})=\&Integral;p(x_{k+1}^{{\left\{1\right\}}_{k+1}},...,x_k^{{\{i-1\}}_{k+1}},x_k^{{\left\{i\right\}}_{k+1}},...,x_k^{{\left\{3\right\}}_{k+1}})\&CenterDot;p\left(x_{k+1}^{{\left\{i\right\}}_{k+1}}\right|x_k^{{\left\{i\right\}}_{k+1}}){\mathrm{dx}}_k^{{\left\{i\right\}}_{k+1}}---\left(22\right)$

具体而言，对于机器人{1}_k+1，状态预测计算为

$p(x_{k+1}^{{\left\{1\right\}}_{k+1}},x_k^{{\left\{2\right\}}_{k+1}},x_k^{{\left\{3\right\}}_{k+1}})=\&Integral;p(x_k^{{\left\{1\right\}}_{k+1}},x_k^{{\left\{2\right\}}_{k+1}},x_k^{{\left\{3\right\}}_{k+1}})\&CenterDot;p\left(x_{k+1}^{{\left\{1\right\}}_{k+1}}\right|x_k^{{\left\{1\right\}}_{k+1}}){\mathrm{dx}}_k^{{\left\{1\right\}}_{k+1}}---\left(23\right)$

对于机器人{2}_k+1，状态预测计算为

$p(x_{k+1}^{{\left\{1\right\}}_{k+1}},x_{k+1}^{{\left\{2\right\}}_{k+1}},x_k^{{\left\{3\right\}}_{k+1}})=\&Integral;p(x_{k+1}^{{\left\{1\right\}}_{k+1}},x_k^{{\left\{2\right\}}_{k+1}},x_k^{{\left\{3\right\}}_{k+1}})\&CenterDot;p\left(x_{k+1}^{{\left\{2\right\}}_{k+1}}\right|x_k^{{\left\{2\right\}}_{k+1}}){\mathrm{dx}}_k^{{\left\{2\right\}}_{k+1}}---\left(24\right)$

对于机器人{3}_k+1，状态预测计算为

$p(x_{k+1}^{{\left\{1\right\}}_{k+1}},x_k^{{\left\{2\right\}}_{k+1}},x_{k+1}^{{\left\{3\right\}}_{k+1}})=\&Integral;p(x_{k+1}^{{\left\{1\right\}}_{k+1}},x_{k+1}^{{\left\{2\right\}}_{k+1}},x_k^{{\left\{3\right\}}_{k+1}})\&CenterDot;p\left(x_{k+1}^{{\left\{3\right\}}_{k+1}}\right|x_k^{{\left\{3\right\}}_{k+1}}){\mathrm{dx}}_k^{{\left\{3\right\}}_{k+1}}---\left(25\right)$

如果机器人R1进行了GPS观测

并且机器人R1对应{i}_k+1（i∈{1,2,3}），那么机器人R1在进行状态预测后按照公式（6）进行对外观测更新，得：

$p(x_{k+1}^{{\left\{1\right\}}_{k+1}},...,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},...,x_k^{{\left\{3\right\}}_{k+1}}|z_{k+1}^{{\left\{i\right\}}_{k+1}})=\frac{p(x_{k+1}^{{\left\{1\right\}}_{k+1}},...,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},...,x_k^{{\left\{3\right\}}_{k+1}})p\left(z_{k+1}^{{\left\{i\right\}}_{k+1}}\right|x_{k+1}^{{\left\{i\right\}}_{k+1}})}{p\left(z_{k+1}^{{\left\{i\right\}}_{k+1}}\right)}---\left(26\right)$

式中， $p\left(z_{k+1}^{{\left\{i\right\}}_{k+1}}\right)=\&Integral;\&CenterDot;\&CenterDot;\&CenterDot;\&Integral;p(x_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_k^{{\left\{3\right\}}_{k+1}})p\left(z_{k+1}^{{\left\{i\right\}}_{k+1}}\right|x_{k+1}^{{\left\{i\right\}}_{k+1}}){\mathrm{dx}}_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,{\mathrm{dx}}_{k+1}^{{\left\{i\right\}}_{k+1}},{\mathrm{dx}}_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,d{x}_k^{{\left\{3\right\}}_{k+1}},$ 所得估计结果为

简记为 $p(x_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_k^{{\left\{3\right\}}_{k+1}});$

如果存在涉及机器人{i}_k+1(i＝2,3)的相对观测

或者则机器人{i}_k+1(i＝2,3)按照公式（7）进行以下相对观测更新计算：

$p(x_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_k^{{\left\{3\right\}}_{k+1}}|Z_{k+1}^{i\&CenterDot;})=$

$\frac{p(x_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_k^{{\left\{3\right\}}_{k+1}})\underset{j<i}{\mathrm{\&Pi;}}p\left(z_{k+1}^{{\left\{i\right\}}_{k+1}{\left\{j\right\}}_{k+1}}\right|x_{k+1}^{{\left\{i\right\}}_{k+1}},x_{k+1}^{{\left\{j\right\}}_{k+1}})p\left(z_{k+1}^{{\left\{j\right\}}_{k+1}{\left\{i\right\}}_{k+1}}\right|x_{k+1}^{{\left\{i\right\}}_{k+1}},x_{k+1}^{{\left\{j\right\}}_{k+1}})}{\underset{j<i}{\mathrm{\&Pi;}}p\left(z_{k+1}^{{\left\{i\right\}}_{k+1}{\left\{j\right\}}_{k+1}}\right)p\left(z_{k+1}^{{\left\{j\right\}}_{k+1}{\left\{i\right\}}_{k+1}}\right)}---\left(27\right)$

式中， $p\left(z_{k+1}^{{\left\{i\right\}}_{k+1}{\left\{j\right\}}_{k+1}}\right)=\&Integral;\&CenterDot;\&CenterDot;\&CenterDot;\&Integral;p(x_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_k^{{\left\{3\right\}}_{k+1}})p\left(z_{k+1}^{{\left\{i\right\}}_{k+1}{\left\{j\right\}}_{k+1}}\right|x_{k+1}^{{\left\{i\right\}}_{k+1}},x_{k+1}^{{\left\{j\right\}}_{k+1}})d{x}_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,{\mathrm{dx}}_{k+1}^{{\left\{i\right\}}_{k+1}},{\mathrm{dx}}_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,{\mathrm{dx}}_k^{{\left\{3\right\}}_{k+1}},$ $p\left(z_{k+1}^{{\left\{j\right\}}_{k+1}{\left\{i\right\}}_{k+1}}\right)=\&Integral;\&CenterDot;\&CenterDot;\&CenterDot;\&Integral;p(x_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_k^{{\left\{3\right\}}_{k+1}})p\left(z_{k+1}^{{\left\{j\right\}}_{k+1}{\left\{i\right\}}_{k+1}}\right|x_{k+1}^{{\left\{i\right\}}_{k+1}},x_{k+1}^{{\left\{j\right\}}_{k+1}})d{x}_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,{\mathrm{dx}}_{k+1}^{{\left\{i\right\}}_{k+1}},{\mathrm{dx}}_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,{\mathrm{dx}}_{k+1}^{{\left\{3\right\}}_{k+1}};$ 式中

表示机器人{i}_k+1所融合的k+1时刻的机器人之间相对观测量的集合，即 $Z_{k+1}^{i\&CenterDot;}=\left\{z_{k+1}^{{\left\{i\right\}}_{k+1}{\left\{j\right\}}_{k+1}}\right|j<i\}\&cup;\{z_{k+1}^{{\left\{j\right\}}_{k+1}{\left\{i\right\}}_{k+1}}|j<i\};$ 所得估计结果为

其中 $Z_{k+1}^i=Z_{k+1}^{i\&CenterDot;}\&cup;z_{k+1}^{{\left\{i\right\}}_{k+1}}\&cup;Z_{k+1}^{i-1};$

③机器人{i}_k+1(i＝1,2)将联合状态估计

发送给机器人{i+1}_k+1；

当机器人{3}_k+1完成计算后，得到k+1(k=1,2,…)时刻全体机器人协同定位的结果

其中

Claims (1)

1.一种分散式多机器人协同定位算法，针对安装有定位装置、可以相互通信的n个机器人（分别标识为R1,R2,…,Rn），包括以下步骤：

步骤一、k＝1时刻的协同定位计算即估计

其中

表示机器人Ri（i∈{1,…,n}）在k=1时刻的状态，k=1时刻的协同定位计算由全体机器人在时间段[k，k+1)(k=1)内共同完成，具体包含以下通信和计算：

1）在k(k＝1)时刻，如果机器人Ri(i＝1,…,n)对机器人Rj(j＝1,…,n，j≠i)进行了相对观测

那么机器人Ri把相对观测信息发送给机器人Rj；

2）在时间段[k,k+1)(k=1)内，全体机器人满足链式通信拓扑结构；从通信链的一端开始，按照在通信链上的位置将机器人依次记为{1}₁,{2}₁，…,{n}₁；

机器人{i}₁(i＝1,…,n)根据公式(1)进行状态预测计算：

$p\left(x_1^{{\left\{i\right\}}_1}\right)=\&Integral;p\left(x_0^{{\left\{i\right\}}_1}\right)\&CenterDot;p\left(x_1^{{\left\{i\right\}}_1}\right|x_0^{{\left\{i\right\}}_1})d{x}_0^{{\left\{i\right\}}_1}---\left(1\right)$

式中

表示机器人{i}₁的初始状态，p(·)表示随机变量的概率密度函数，p(·|·)表示条件概率密度函数；

如果机器人{i}₁(i＝1,…,n)进行了单机器人对外观测

则根据公式(2)进行对外观测更新计算：

$p\left(x_1^{{\left\{i\right\}}_1}\right|z_1^{{\left\{i\right\}}_1})=\frac{p\left(x_1^{{\left\{i\right\}}_1}\right)p\left(z_1^{{\left\{i\right\}}_1}\right|x_1^{{\left\{i\right\}}_1})}{\&Integral;p\left(x_1^{{\left\{i\right\}}_1}\right)p\left(z_1^{{\left\{i\right\}}_1}\right|x_1^{{\left\{i\right\}}_1})d{x}_1^{{\left\{i\right\}}_1}}---\left(2\right)$

将对外观测

的数值代入上式左边条件概率

得到进行对外观测更新后的状态估计，记为

在语义明确的情况下可以略去观测量，简记为

3）用表示[k，k+1)(k=1)时间段内通信链上前i-1个机器人{{1}₁,…,{i-1}₁}所融合的观测量集合，机器人{i}₁(i＝2,…,n)接收机器人{i-1}₁发来的消息

然后根据公式(3)计算联合状态分布：

$p(x_1^{{\left\{1\right\}}_1},\&CenterDot;\&CenterDot;\&CenterDot;,x_1^{{\left\{i\right\}}_1})=p\left(x_1^{{\left\{i\right\}}_1}\right)\&CenterDot;(p_1^{{\left\{1\right\}}_1},\&CenterDot;\&CenterDot;\&CenterDot;,x_1^{{\{i-1\}}_1})---\left(3\right)$

所得估计结果为 $p(x_1^{{\left\{1\right\}}_1},\&CenterDot;\&CenterDot;\&CenterDot;,x_1^{{\left\{i\right\}}_1})|(z_1^{{\left\{i\right\}}_1}\&cup;Z_1^{i-1});$

如果存在涉及机器人{i}₁(i＝2,…,n)的相对观测

或者

则机器人{i}₁(i＝2,…,n)根据公式(4)进行以下相对观测更新计算：

$p(x_1^{{\left\{1\right\}}_1},\&CenterDot;\&CenterDot;\&CenterDot;,x_1^{{\left\{i\right\}}_1}|Z_1^{i\&CenterDot;})=p(x_1^{{\left\{1\right\}}_1},\&CenterDot;\&CenterDot;\&CenterDot;,x_1^{{\left\{i\right\}}_1})\&CenterDot;\frac{\underset{j<i}{\mathrm{\&Pi;}}p\left(z_1^{{\left\{i\right\}}_1{\left\{j\right\}}_1}\right|x_1^{{\left\{i\right\}}_1},x_1^{{\left\{j\right\}}_1})p\left(z_1^{{\left\{j\right\}}_1{\left\{i\right\}}_1}\right|x_1^{{\left\{i\right\}}_1},x_1^{{\left\{j\right\}}_1})}{\underset{j<i}{\mathrm{\&Pi;}}p\left(z_1^{{\left\{i\right\}}_1{\left\{j\right\}}_1}\right)p\left(z_1^{{\left\{j\right\}}_1{\left\{i\right\}}_1}\right)}---\left(4\right)$

式中 $p\left(z_1^{{\left\{i\right\}}_1{\left\{j\right\}}_1}\right)=\&Integral;\&CenterDot;\&CenterDot;\&CenterDot;\&Integral;p(x_1^{{\left\{1\right\}}_1},\&CenterDot;\&CenterDot;\&CenterDot;,x_1^{{\left\{i\right\}}_1})p\left(z_1^{{\left\{i\right\}}_1{\left\{j\right\}}_1}\right|x_1^{{\left\{i\right\}}_1},x_1^{\left\{j\right\}}1)d{x}_1^{{\left\{1\right\}}_1}\&CenterDot;\&CenterDot;\&CenterDot;{\mathrm{dx}}_1^{{\left\{i\right\}}_1},$

$p\left(z_1^{{\left\{j\right\}}_1{\left\{i\right\}}_1}\right)=\&Integral;\&CenterDot;\&CenterDot;\&CenterDot;\&Integral;p(x_1^{{\left\{1\right\}}_1},\&CenterDot;\&CenterDot;\&CenterDot;,x_1^{{\left\{i\right\}}_1})p\left(z_1^{{\left\{j\right\}}_1{\left\{i\right\}}_1}\right|x_1^{{\left\{i\right\}}_1},x_1^{{\left\{j\right\}}_1})d{x}_1^{{\left\{1\right\}}_1}\&CenterDot;\&CenterDot;\&CenterDot;d{x}_1^{{\left\{i\right\}}_1};$

表示机器人{i}₁所融合的k＝1时刻的机器人之间相对观测量的集合，即 $Z_1^{i\&CenterDot;}=\left\{z_1^{{\left\{i\right\}}_1{\left\{j\right\}}_1}\right|j<i\}\&cup;\{z_1^{{\left\{j\right\}}_1{\left\{i\right\}}_1}|j<i\};$ 记 $Z_1^i=Z_1^{i\&CenterDot;}\&cup;z_1^{{\left\{i\right\}}_1}\&cup;Z_1^{i-1},$ 则进行相对观测更新后的联合状态估计为

简记为

4）机器人{i}₁(i＝1,…,n-1)将估计结果发送给机器人{i+1}₁，当机器人{n}₁按照公式(1)-(4)完成计算后，得到k=1时刻全体机器人协同定位的结果其中

是全体机器人在k＝1时刻的观测量集合，包括单个机器人的对外观测和机器人之间的相对观测；

步骤二、k+1(k=1,2,…)时刻的协同定位计算即估计

其中

表示机器人Ri（i∈{1，…,n}）在k+1时刻的状态，k+1(k=1,2,…)时刻的协同定位计算由全体机器人在时间段[k+1,k+2)(k=1,2,…)内共同完成，具体包含以下通信和计算：

1）在k+1(k＝1,2,…)时刻，如果机器人Ri(i＝1,…,n)对机器人Rj(j＝1,…,n,j≠i)进行了相对观测

那么机器人Ri把相对观测信息发送给机器人Rj；

2）在时间段[k+1,k+2)(k=1,2,…)内，以k时刻协同定位的完成机器人{n}_k为起点，n个机器人建立一条新的通信链，按照在通信链上的位置将机器人依次记为{1}_k+1，{2}_k+1,…,{n}_k+1；

与k＝1时刻类似，用

表示时间段[k+1,k+2)(k=1,2,…)内通信链上前i个机器人{{1}_k+1，…,{i}_k+1}所融合的观测量集合，k时刻协同定位的完成机器人{n}_k获得全体机器人协同定位的结果

其中是全体机器人截至k（k＝1,2,…）时刻历史观测量的集合，即

机器人{1}_k+1，也即{n}_k，对联合状态估计中的状态顺序进行调整，得到

k+1(k＝1,2,…)时刻的协同定位计算由机器人{1}_k+1启动，机器人{i}_k+1(i＝2,…,n)在接收到机器人{i-1}_k+1发来的消息

之后，启动本地计算；

机器人{i}_k+1(i＝1,…,n)根据公式(5)进行状态预测计算：

$p(x_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_k^{{\left\{n\right\}}_{k+1}})=\&Integral;p(x_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_{k+1}^{{\{i-1\}}_{k+1}},x_k^{{\left\{i\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_k^{{\left\{n\right\}}_{k+1}})\&CenterDot;p\left(x_{k+1}^{{\left\{i\right\}}_{k+1}}\right|x_k^{{\left\{i\right\}}_{k+1}})d{x}_k^{{\left\{i\right\}}_{k+1}}---\left(5\right)$

如果机器人{i}_k+1(i＝1,…,n)进行了单机器人对外观测则根据公式(6)进行对外观测更新计算：

$p(x_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_k^{{\left\{n\right\}}_{k+1}}|z_{k+1}^{{\left\{i\right\}}_{k+1}})=\frac{p(x_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_k^{{\left\{n\right\}}_{k+1}})p\left(z_{k+1}^{{\left\{i\right\}}_{k+1}}\right|x_{k+1}^{{\left\{i\right\}}_{k+1}})}{p\left(z_{k+1}^{{\left\{i\right\}}_{k+1}}\right)}---\left(6\right)$

式中， $p\left(z_{k+1}^{{\left\{i\right\}}_{k+1}}\right)=\&Integral;\&CenterDot;\&CenterDot;\&CenterDot;\&Integral;p(x_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_k^{{\left\{n\right\}}_{k+1}})p\left(z_{k+1}^{{\left\{i\right\}}_{k+1}}\right|x_{k+1}^{{\left\{i\right\}}_{k+1}})d{x}_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,d{x}_{k+1}^{{\left\{i\right\}}_{k+1}},d{x}_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,d{x}_k^{{\left\{n\right\}}_{k+1}},$ 所得估计结果为

简记为 $p(x_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_k^{{\left\{n\right\}}_{k+1}});$

如果存在涉及机器人{i}_k+1(i＝2,…,n)的相对观测

或者

则机器人{i}_k+1(i＝2,…,n)根据公式(7)进行相对观测更新计算：

$p(x_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_k^{{\left\{n\right\}}_{k+1}}|Z_{k+1}^{i\&CenterDot;})=$

$\frac{p(x_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_k^{{\left\{n\right\}}_{k+1}})\underset{j<i}{\mathrm{\&Pi;}}p\left(z_{k+1}^{{\left\{i\right\}}_{k+1}{\left\{j\right\}}_{k+1}}\right|x_{k+1}^{{\left\{i\right\}}_{k+1}},x_{k+1}^{{\left\{j\right\}}_{k+1}})p\left(z_{k+1}^{{\left\{j\right\}}_{k+1}{\left\{i\right\}}_{k+1}}\right|x_{k+1}^{{\left\{i\right\}}_{k+1}},x_{k+1}^{{\left\{j\right\}}_{k+1}})}{\underset{j<i}{\mathrm{\&Pi;}}p\left(z_{k+1}^{{\left\{i\right\}}_{k+1}{\left\{j\right\}}_{k+1}}\right)p\left(z_{k+1}^{{\left\{j\right\}}_{k+1}{\left\{i\right\}}_{k+1}}\right)}---\left(7\right)$

式中， $p\left(z_{k+1}^{{\left\{i\right\}}_{k+1}{\left\{j\right\}}_{k+1}}\right)=\&Integral;\&CenterDot;\&CenterDot;\&CenterDot;\&Integral;p(x_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_k^{{\left\{n\right\}}_{k+1}})p\left(z_{k+1}^{{\left\{i\right\}}_{k+1}{\left\{j\right\}}_{k+1}}\right|x_{k+1}^{{\left\{i\right\}}_{k+1}},x_{k+1}^{{\left\{j\right\}}_{k+1}})d{x}_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,{\mathrm{dx}}_k^{{\left\{i\right\}}_{k+1}},{\mathrm{dx}}_k^{{\{i+1\}}_{k+1}}\&CenterDot;\&CenterDot;\&CenterDot;,{\mathrm{dx}}_k^{{\left\{n\right\}}_{k+1}},$ $p\left(z_{k+1}^{{\left\{j\right\}}_{k+1}{\left\{i\right\}}_{k+1}}\right)=\&Integral;\&CenterDot;\&CenterDot;\&CenterDot;\&Integral;p(x_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,x_k^{{\left\{n\right\}}_{k+1}})p\left(z_{k+1}^{{\left\{j\right\}}_{k+1}{\left\{i\right\}}_{k+1}}\right|x_{k+1}^{{\left\{i\right\}}_{k+1}},x_{k+1}^{{\left\{j\right\}}_{k+1}})d{x}_{k+1}^{{\left\{1\right\}}_{k+1}},\&CenterDot;\&CenterDot;\&CenterDot;,{\mathrm{dx}}_k^{{\left\{i\right\}}_{k+1}},{\mathrm{dx}}_k^{{\{i+1\}}_{k+1}}\&CenterDot;\&CenterDot;\&CenterDot;,{\mathrm{dx}}_k^{{\left\{n\right\}}_{k+1}};$ 式中

表示机器人{i}_k+1所融合的k+1时刻的机器人之间相对观测量的集合，即 $Z_{k+1}^{i\&CenterDot;}=\left\{z_{k+1}^{{\left\{i\right\}}_{k+1}{\left\{j\right\}}_{k+1}}\right|j<i\}\&cup;\{z_{k+1}^{{\left\{j\right\}}_{k+1}{\left\{i\right\}}_{k+1}}|j<i\};$ 所得估计结果为

其中 $Z_{k+1}^i=Z_{k+1}^{i\&CenterDot;}\&cup;z_{k+1}^{{\left\{i\right\}}_{k+1}}\&cup;Z_{k+1}^{i-1};$

3）机器人{i}_k+1(i＝1,…,n-1)将联合状态估计

发送给机器人{i+1}_k+1；

当机器人{n}_k+1完成计算后，得到k+1(k=1,2,…)时刻全体机器人协同定位的结果

其中

Images