[发明专利]一种分散式多机器人协同定位算法

权利要求书

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(x1{i}1)=∫p(x0{i}1)·p(x1{i}1|x0{i}1)dx0{i}1---(1)]]>

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

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

p(x1{i}1|z1{i}1)=p(x1{i}1)p(z1{i}1|x1{i}1)∫p(x1{i}1)p(z1{i}1|x1{i}1)dx1{i}1---(2)]]>

将对外观测的数值代入上式左边条件概率得到进行对外观测更新后的状态估计，记为在语义明确的情况下可以略去观测量，简记为

3）用表示[k，k+1)(k=1)时间段内通信链上前i-1个机器人{{1}₁,…,{i-1}₁}所融合的观测量集合，机器人{i}₁(i＝2,…,n)接收机器人{i-1}₁发来的消息然后根据公式(3)计算联合状态分布：

p(x1{1}1,···,x1{i}1)=p(x1{i}1)·(p1{1}1,···,x1{i-1}1)---(3)]]>

所得估计结果为p(x1{1}1,···,x1{i}1)|(z1{i}1∪Z1i-1);]]>

如果存在涉及机器人{i}₁(i＝2,…,n)的相对观测或者则机器人{i}₁(i＝2,…,n)根据公式(4)进行以下相对观测更新计算：

p(x1{1}1,···,x1{i}1|Z1i·)=p(x1{1}1,···,x1{i}1)·Πj<ip(z1{i}1{j}1|x1{i}1,x1{j}1)p(z1{j}1{i}1|x1{i}1,x1{j}1)Πj<ip(z1{i}1{j}1)p(z1{j}1{i}1)---(4)]]>

式中p(z1{i}1{j}1)=∫···∫p(x1{1}1,···,x1{i}1)p(z1{i}1{j}1|x1{i}1,x1{j}1)dx1{1}1···dx1{i}1,]]>

p(z1{j}1{i}1)=∫···∫p(x1{1}1,···,x1{i}1)p(z1{j}1{i}1|x1{i}1,x1{j}1)dx1{1}1···dx1{i}1;]]>表示机器人{i}₁所融合的k＝1时刻的机器人之间相对观测量的集合，即Z1i·={z1{i}1{j}1|j<i}∪{z1{j}1{i}1|j<i};]]>记Z1i=Z1i·∪z1{i}1∪Z1i-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(xk+1{1}k+1,···,xk+1{i}k+1,xk{i+1}k+1,···,xk{n}k+1)=∫p(xk+1{1}k+1,···,xk+1{i-1}k+1,xk{i}k+1,···,xk{n}k+1)·p(xk+1{i}k+1|xk{i}k+1)dxk{i}k+1---(5)]]>

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

p(xk+1{1}k+1,···,xk+1{i}k+1,xk{i+1}k+1,···,xk{n}k+1|zk+1{i}k+1)=p(xk+1{1}k+1,···,xk+1{i}k+1,xk{i+1}k+1,···,xk{n}k+1)p(zk+1{i}k+1|xk+1{i}k+1)p(zk+1{i}k+1)---(6)]]>

式中，p(zk+1{i}k+1)=∫···∫p(xk+1{1}k+1,···,xk+1{i}k+1,xk{i+1}k+1,···,xk{n}k+1)p(zk+1{i}k+1|xk+1{i}k+1)dxk+1{1}k+1,···,dxk+1{i}k+1,dxk{i+1}k+1,···,dxk{n}k+1,]]>所得估计结果为简记为p(xk+1{1}k+1,···,xk+1{i}k+1,xk{i+1}k+1,···,xk{n}k+1);]]>

如果存在涉及机器人{i}_k+1(i＝2,…,n)的相对观测或者则机器人{i}_k+1(i＝2,…,n)根据公式(7)进行相对观测更新计算：

p(xk+1{1}k+1,···,xk+1{i}k+1,xk{i+1}k+1,···,xk{n}k+1|Zk+1i·)=]]>

p(xk+1{1}k+1,···,xk+1{i}k+1,xk{i+1}k+1,···,xk{n}k+1)Πj<ip(zk+1{i}k+1{j}k+1|xk+1{i}k+1,xk+1{j}k+1)p(zk+1{j}k+1{i}k+1|xk+1{i}k+1,xk+1{j}k+1)Πj<ip(zk+1{i}k+1{j}k+1)p(zk+1{j}k+1{i}k+1)---(7)]]>

式中，p(zk+1{i}k+1{j}k+1)=∫···∫p(xk+1{1}k+1,···,xk+1{i}k+1,xk{i+1}k+1,···,xk{n}k+1)p(zk+1{i}k+1{j}k+1|xk+1{i}k+1,xk+1{j}k+1)dxk+1{1}k+1,···,dxk{i}k+1,dxk{i+1}k+1···,dxk{n}k+1,]]>p(zk+1{j}k+1{i}k+1)=∫···∫p(xk+1{1}k+1,···,xk+1{i}k+1,xk{i+1}k+1,···,xk{n}k+1)p(zk+1{j}k+1{i}k+1|xk+1{i}k+1,xk+1{j}k+1)dxk+1{1}k+1,···,dxk{i}k+1,dxk{i+1}k+1···,dxk{n}k+1;]]>式中表示机器人{i}_k+1所融合的k+1时刻的机器人之间相对观测量的集合，即Zk+1i·={zk+1{i}k+1{j}k+1|j<i}∪{zk+1{j}k+1{i}k+1|j<i};]]>所得估计结果为其中Zk+1i=Zk+1i·∪zk+1{i}k+1∪Zk+1i-1;]]>

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

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