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

1.一种分散式多机器人协同定位方法，针对安装有定位装置、分别标识为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}1,{2}1,…,{n}1；
机器人{i}1，i＝1,…,n根据公式(1)进行状态预测计算：
式中 表示机器人{i}1的初始状态，p(·)表示随机变量的概率密度函数，p(·|·)表示条件概率密度函数；
如果机器人{i}1，i＝1,…,n进行了单机器人对外观测 则根据公式(2)进行对外观测更新计算：
将对外观测 的数值代入(2)式左边条件概率 得到进行对外观测更新后
的状态估计，记为 在语义明确的情况下可以略去观测量，简记为
3)用 i＝2,…,n表示[k,k+1)，k＝1时间段内通信链上前i-1个机器人
{{1}1,…,{i-1}1}所融合的观测量集合，机器人{i}1，i＝2,…,n接收机器人{i-1}1发来的消息 然后根据公式(3)计算联合状态分布：
所得估计结果为
如果存在涉及机器人{i}1，i＝2,…,n的相对观测 j式中
表示 机器 人{i}1 所融 合
的k＝1时刻的机器人之间相对观测量的集合，即 记
则进行相对观测更新后的联合状态估计为 简记为
4)机器人{i}1，i＝1,…,n-1将估计结果 发送给机器人{i+1}1，当机器
人{n}1按照公式(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获得全体机器人协同定位的结果 其中Zk是全体机器人截
至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)进行状态预测计算：
如果机器人{i}k+1，i＝1,…,n进行了单机器人对外观测 则根据公式(6)进行对外观测更新计算：
式中，
所得估计结果为 简记为
如果存在涉及机器人{i}k+1，i＝2,…,n的相对观测 j机器人{i}k+1，i＝2,…,n根据公式(7)进行相对观测更新计算：
式中，
式中 表示机器人{i}k+1所融合的k+1时刻的机器人之间相对观测量的集合，即所得估计结果为
其中
3)机器人{i}k+1，i＝1,…,n-1将联合状态估计
发送给机器人{i+1}k+1；
当机器人{n}k+1完成计算后，得到k+1，k＝1,2,…时刻全体机器人协同定位的结果其中

一种分散式多机器人协同定位方法\n技术领域\n[0001] 本发明属于多机器人的定位算法，涉及一种分散式多机器人协同定位方法。\n背景技术\n[0002] 随着科技发展，多机器人(包括室内机器人、室外机器人、无人机、水下潜航器等)系统正在科研、农业、医疗、军事等领域获得广泛应用。机器人定位(即确定机器人的位置、方位角等状态)往往是保证机器人正常工作的一项基本要求。单个机器人通过码盘、惯性测量单元等测量自身的运动信息，可以推算自身的状态。这种推算方式的定位误差会随着时间而累积。机器人可以通过对外观测来抑制这种定位误差的增长，如GPS观测、对已知地标的观测等。\n[0003] 多机器人协同定位(cooperative localization)利用机器人之间的相对观测(距离、相对方位等)，通过一定的信息交换实现机器人之间定位资源共享，从而能够获得比各自定位更优的性能。例如，在一个机器人群中，某些机器人配备了高精度惯导系统和GPS(全球卫星定位系统)，其他机器人则配备了码盘，如果机器人之间可以测量彼此之间的距离或相对方位，那么部分机器人的高精度导航信息可以在机器人之间共享，每个机器人的定位精度相比于独立定位时都会有所提高。又如，在进行协同定位的水下潜航器群中，某一个潜航器浮出水面进行GPS定位可以提高群中多个潜航器的定位精度，在减少机动能耗、增强隐蔽性的同时提高潜航器群的定位精度。\n[0004] 数据融合算法从结构上可以分为集中式(Centralised Data Fusion)、分层式(Hierarchical Data Fusion)和分散式(Decentralised Data Fusion)三种基本形式。①集中式数据融合算法中，相关传感器的原始测量数据被传送到一个处理中心，由处理中心单独完成数据融合。②分层式数据融合算法中，传感器的原始测量数据在本地进行一定的处理，然后逐级上传，在每一级这些信息都进行一定的融合，最后在一个处理中心得到全局估计。③分散式数据融合算法中，每个节点处理本地的传感器数据，节点与节点之间可以进行点对点的通信。节点之间的地位是对等的，既不依赖于单一的处理中心，也没有等级之分。\n[0005] 分散式数据融合结构满足以下要求：没有中心节点；不要求有公共通信信道用以广播；单个节点不需要全局信息作为先验信息。采用分散式数据融合结构的系统应该具有以下优势：模块化；良好的可扩展性；好的容错性。在上述优势的吸引下，分散式协同定位算法成为机器人领域的研究热点。按照分散式数据融合的实现途径，现有的分散式协同定位算法可以分为复制型、解耦型和分布计算型。在复制型分散式协同定位算法中，每个机器人在功能上都相当于一个处理中心，对全体机器人进行定位。复制型分散式协同定位算法具有模块化和良好的容错性，但是可扩展性差。在解耦型分散式协同定位算法中，将协同定位问题解耦为多个相互独立的子问题，每个机器人求解一个或多个子问题。由于协同定位问题中各机器人状态两两相关，必须对这种相关进行近似才能实现解耦，因此解耦型算法会带来精度损失。分布计算型分散式协同定位算法是将某种集中式算法的运算拆分到各个平台上。现有分布计算型分散式协同定位算法对平台间的同步要求严格，并且往往需要在平台间广播信息，从而直接影响了算法的容错性和可扩展性。因此，有必要探求一种能够提升整体性能的分散式协同定位算法。\n发明内容\n[0006] 针对现有分散式多机器人协同定位算法存在的技术问题，本发明的目的是提出一种分散式多机器人协同定位方法，该算法在不对系统模型进行解耦近似的前提下，能够充分实现分散式数据融合结构的三点优势：模块化、良好的可扩展性和好的容错性。\n[0007] 本发明提出的分散式多机器人协同定位方，针对安装有定位装置、分别标识为R1,R2,…,Rn可以相互通信的n个机器人，包括以下步骤：\n[0008] 步骤一、k＝1时刻的协同定位计算即估计 其中 表示机器人Ri，i∈{1,…,n}在k＝1时刻的状态，k＝1时刻的协同定位计算由全体机器人在时间段[k,k+1)，k＝1内共同完成，具体包含以下通信和计算：\n[0009] 1)在k，k＝1时刻，如果机器人Ri，i＝1,…,n对机器人Rj，j＝1,…,n,j≠i进行了相对观测 那么机器人Ri把相对观测信息发送给机器人Rj；\n[0010] 2)在时间段[k,k+1)，k＝1内，全体机器人满足链式通信拓扑结构；从通信链的一端开始，按照在通信链上的位置将机器人依次记为{1}1,{2}1,…,{n}1；\n[0011] 机器人{i}1，i＝1,…,n根据公式(1)进行状态预测计算：\n[0012] \n[0013] 式中 表示机器人{i}1的初始状态，p(·)表示随机变量的概率密度函数，p(·|·)表示条件概率密度函数；\n[0014] 如果机器人{i}1，i＝1,…,n进行了单机器人对外观测 则根据公式(2)进行对外观测更新计算：\n[0015] \n[0016] 将对外观测 的数值代入(2)式左边条件概率 得到进行对外观测更新后的状态估计，记为 在语义明确的情况下可以略去观测量，简记为\n[0017] 3)用 i＝2,…,n表示[k,k+1)，k＝1时间段内通信链上前i-1个机器人{{1}1,…,{i-1}1}所融合的观测量集合，机器人{i}1，i＝2,…,n接收机器人{i-1}1发来的消息 然后根据公式(3)计算联合状态分布：\n[0018] \n[0019] 所得估计结果为\n[0020] 如果存在涉及机器人{i}1，i＝2,…,n的相对观测 j