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

摘要

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

主权项

一种分散式多机器人协同定位方法，针对安装有定位装置、分别标识为R1,R2,…,Rn可以相互通信的n个机器人，包括以下步骤：步骤一、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)=\int 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^{{\{i\}}_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})}{\int 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)$将对外观测的数值代入(2)式左边条件概率得到进行对外观测更新后的状态估计，记为在语义明确的情况下可以略去观测量，简记为3)用i＝2,…,n表示[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(x_1^{{\left\{1\right\}}_1},...,x_1^{{\{i-1\}}_1})---\left(3\right)$所得估计结果为如果存在涉及机器人{i}₁，i＝2,…,n的相对观测j<i或者j<i，则机器人{i}₁，i＝2,…,n根据公式(4)进行以下相对观测更新计算：$p(x_1^{{\left\{1\right\}}_1},...,x_1^{{\left\{i\right\}}_1}|Z_1^{i·})=p(x_1^{{\left\{1\right\}}_1},...,x_1^{{\left\{i\right\}}_1})·\frac{\underset{j<i}{\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}{\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)=\int ...\int 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})d{x}_1^{{\left\{1\right\}}_1}...d{x}_1^{{\left\{i\right\}}_1},$$p\left(z_1^{{\left\{j\right\}}_1{\left\{i\right\}}_1}\right)=\int ...\int 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})d{x}_1^{{\left\{1\right\}}_1}...d{x}_1^{{\left\{i\right\}}_1};$表示机器人{i}₁所融合的k＝1时刻的机器人之间相对观测量的集合，即记则进行相对观测更新后的联合状态估计为简记为4)机器人{i}₁，i＝1,…,n‑1将估计结果发送给机器人{i+1}₁，当机器人{n}₁按照公式(1)、(2)、(3)、(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,2,…；i＝1,…,n表示时间段[k+1,k+2)，k＝1,2,…内通信链上前i个机器人{{1}_k+1,…,{i}_k+1}所融合的观测量集合，k时刻协同定位的完成机器人{n}_k获得全体机器人协同定位的结果其中Z_k是全体机器人截至k，k＝1,2,…时刻历史观测量的集合，即机器人{1}_k+1，也即{n}_k，对联合状态估计中的状态顺序进行调整，得到$p(x_k^{{\left\{1\right\}}_{k+1}},x_k^{{\left\{2\right\}}_{k+1}},...,x_k^{{\left\{n\right\}}_{k+1}})|Z_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}},...,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},...,x_k^{{\left\{n\right\}}_{k+1}})=\int p(x_{k+1}^{{\left\{1\right\}}_{k+1}},...,x_{k+1}^{{\{i-1\}}_{k+1}},x_k^{{\left\{i\right\}}_{k+1}},...,x_k^{{\left\{n\right\}}_{k+1}})·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}},...,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},...,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}},...,x_{k+1}^{{\left\{i\right\}}_{k+1}},x_k^{{\{i+1\}}_{k+1}},...,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)=\int ...\int 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\{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}},...,d{x}_{k+1}^{{\left\{i\right\}}_{k+1}},d{x}_k^{{\{i+1\}}_{k+1}},...,d{x}_k^{{\left\{n\right\}}_{k+1}},$所得估计结果为$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\{n\right\}}_{k+1}})|(z_{k+1}^{{\left\{i\right\}}_{k+1}}\cup Z_{k+1}^{i-1}\cup Z_k),$简记为$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\{n\right\}}_{k+1}});$如果存在涉及机器人{i}_k+1，i＝2,…,n的相对观测j<i或者j<i，则机器人{i}_k+1，i＝2,…,n根据公式(7)进行相对观测更新计算：$\begin{array}{c}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\{n\right\}}_{k+1}}|Z_{k+1}^{i·})=\\ \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\{n\right\}}_{k+1}})\underset{j<i}{\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}{\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)}\end{array}---\left(7\right)$式中，$p\left(z_{k+1}^{{\left\{i\right\}}_{k+1}{\left\{j\right\}}_{k+1}}\right)=\int ...\int 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\{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}},...,d{x}_{k+1}^{{\left\{i\right\}}_{k+1}},d{x}_k^{{\{i+1\}}_{k+1}},...,d{x}_k^{{\left\{n\right\}}_{k+1}},$$p\left(z_{k+1}^{{\left\{j\right\}}_{k+1}{\left\{i\right\}}_{k+1}}\right)=\int ...\int 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\{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}},...,d{x}_{k+1}^{{\left\{i\right\}}_{k+1}},d{x}_k^{{\{i+1\}}_{k+1}},...,d{x}_k^{{\left\{n\right\}}_{k+1}};$式中表示机器人{i}_k+1所融合的k+1时刻的机器人之间相对观测量的集合，即$Z_{k+1}^{i·}=\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·}\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,…时刻全体机器人协同定位的结果其中$Z_{k+1}=Z_{k+1}^n\cup Z_k.$