一种多个机器人鲁棒协同定位方法技术

本发明专利技术公开了一种多个机器人鲁棒协同定位方法，涉及智能机器人技术领域，可以在大量错误数据关联存在的情况下实现多机器人间的正确协同定位。包括如下步骤：针对多个机器人组成的系统，其中多个机器人各自获取环境信息，每个机器人构建一个局部地图，每个局部地图由多个子地图构成，构建子地图之间的约束；子地图之间的约束由若干匹配点对构成。构建向量滤除模型，用于过滤错误约束和点对。构建混合概率模型，从几何距离、语义关联和向量一致性的角度分别计算出数据关联中约束和点的正确概率。双层期望最大化算法确定子地图的位姿并去除错误的数据关联，实现多机器人协同定位。位。位。

【技术实现步骤摘要】
一种多个机器人鲁棒协同定位方法


[0001]本专利技术涉及智能机器人
，具体涉及一种多个机器人鲁棒协同定位方法。

技术介绍

[0002]与单个机器人相比，多机器人系统更高效、更稳健地执行任务。在大范围和GPS拒止环境，估计准确多机器人系统之间的相对位置是多机器人协同导航的基础。在这种情况下，机器人需要分享他们对环境的观察结果以建立数据关联，来估计机器人间的相对位置。考虑到大范围环境下机器人获取的环境数据量庞大，且机器人间进行数据传输的带宽有限，传输原始传感器数据效率低下，因此一般分享机器人构建的环境子地图，高效传输数据。综上，现有的协同定位方法一般通过匹配由不同机器人构建的多个子地图实现多机器人协同定位。
[0003]协同定位中不同机器人所观察环境的重叠度往往较低，且不同机器人的视角差异较大，这让检测多个机器人经过的相同场景，成为一项具有挑战性的任务。为了匹配不同机器人对环境的观察结果，首先需要用特征描述符表征环境特征。然而最先进的特征描述符也不足以唯一地表征环境，这会不可避免地导致错误数据关联的产生。而在协同定位中，机器人的大视角差异和子地图之间的低重叠度将进一步加剧错误数据关联的产生。存在大量错误数据关联的情况下，现有协同定位方法往往会失败。
[0004]因此，迫切需要设计一种针对大范围、GPS拒止环境的鲁棒协同定位算法，在大量错误数据关联存在的情况下仍然能实现多机器人间的正确协同定位。

技术实现思路

[0005]有鉴于此，本专利技术提供了一种多个机器人鲁棒协同定位方法，能够适用于大范围、GPS拒止环境，在大量错误数据关联存在的情况下实现多机器人间的正确协同定位。
[0006]为达到上述目的，本专利技术的技术方案包括如下步骤：
[0007]针对多个机器人组成的系统，其中多个机器人各自获取环境信息，分别构建环境的多个子地图，其中每个机器人构建一个局部地图，每个局部地图由多个子地图构成，构建子地图之间的约束；子地图之间的约束由若干匹配点对构成。
[0008]构建向量滤除模型，用于过滤错误数据关联，错误数据关联包括错误约束和正确约束中的错误点对。
[0009]构建混合概率模型，从几何距离、语义关联和向量一致性的角度分别计算出数据关联中约束和点对的正确概率。
[0010]双层期望最大化算法确定子地图的位姿并去除错误的约束，实现多机器人协同定位。
[0011]进一步地，多个机器人组成的系统，对于其中一个机器人所构建的局部地图，至少存在另外一个机器人构建的局部地图，两者间有重叠区域。
[0012]进一步地，构建向量滤除模型，用于过滤错误约束，所构建的向量滤除模型包括用
于约束的向量滤除模型和用于点对的向量滤除模型。
[0013]具体包括如下步骤：
[0014]构建用于约束的向量滤除模型，用于滤除错误的约束，用于约束的向量滤除模型具体为：
[0015]对于机器人h建立的第k个子地图和机器人j建立的第l个子地图其中p
s
,p
t
为子地图中的点，q
s
,q
t
为子地图中的点；定义标志变量v(s,t)，得到式(1)的关系；如果(p
s
,q
s
)和(p
t
,q
t
)都是正确的，那么v(s,t)＝1，并且称(p
s
,q
s
)和(p
t
,q
t
)为一致的一对向量对；其他情况下v(s,t)＝0。
[0016][0017]其中δ为正常数。
[0018]一个约束或者一个匹配点对的正确性基于式(1)进行估计；
[0019]一个约束C是一个子地图和回环约束或者一个里程计约束为C中的每两个点对(p
s
,q
s
)和(p
t
,q
t
)计算v(s,t)的值，那么C中所有v(s,t)的和定义为式(2)中的
[0020][0021]其中n
C
是C中的匹配点对的数量。
[0022]在用于约束的向量滤除模型中，里程计约束被当成正确的回环约束，一个回环约束正确的概率基于来计算，其中定义为所有的平均值；式(3)给出了约束C的正确概率。
[0023][0024]其中为的上限，∈为的上限，σ设置为
[0025]构建用于点对的向量滤除模型，用于过滤错误的点对，用于点对的向量滤除模型具体为：
[0026]对于一个正确约束C
i
中的任意一个匹配点对(p
s
,q
s
)，定义式(4)中所示的满足向量一致性的点对数
[0027][0028]越大，点对(p
s
,q
s
)正确的概率越大，式(5)中定义点对正确概率
[0029][0030]其中∈1设置为而σ1设置为
[0031]由此完成了向量滤除模型对约束和点对的建模。
[0032]进一步地，构建混合概率模型，从几何距离、语义关联和向量一致性的角度分别计算出数据关联中约束和点的正确概率，具体为：
[0033]建立存在错误数据关联的协同定位的问题模型：令O，L，T0分别为里程计约束集合O、回环约束集合L、子地图初始位姿集合T0；定义两种隐变量，关于约束的隐变量和关于点对的隐变量或者0表示是一个正确或错误的回环约束，而或0表示(p
s
,q
s
)是一个正确或错误的点对；令Z
L
和Z
P
分别是和的集合，用Z＝{Z
L
,Z
P
}统称所有种类的隐变量；那么协同定位问题建模为式(6)的最大似然估计问题MLE：
[0034]T＝argmax p(O,L|T,Z)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
[0035]其中T表示子地图的位姿集合。。
[0036]式(6)所表示的问题用期望最大化算法(EM算法)迭代求解，如式(7)所示；
[0037][0038]在EM算法的E步中，给定由第i次迭代得到的O、L和T
(i)th
(T
(i)th
为第i次迭代时子地图位姿的集合)，计算p(Z|O,L,T
(i)th
)；在EM算法的M步中，通过最大化似然函数的期望更新T。
[0039]p(Z|O,L,T
(i)th
)拆分成式(8)的三个子函数，这里Z分别替换成和
[0040]p(Z|O,L,T
(i)th
)＝p
G
(Z|O,L,T
(i)th
)
·
p
S
(Z|O,L,T
(i)th
)
·
p
V
(Z|O,L,本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种多个机器人鲁棒协同定位方法，其特征在于，包括如下步骤：针对多个机器人组成的系统，其中多个机器人各自获取环境信息，每个机器人构建一个局部地图，每个局部地图由多个子地图构成，构建子地图之间的约束；子地图之间的约束由若干匹配点对构成；构建向量滤除模型，用于过滤错误数据关联，所述错误数据关联包括错误约束和正确约束中的错误点对；构建混合概率模型，从几何距离、语义关联和向量一致性的角度分别计算出数据关联中约束和点对的正确概率；双层期望最大化算法确定子地图的位姿并去除错误的约束，实现多机器人协同定位。2.如权利要求1所述的一种多个机器人鲁棒协同定位方法，其特征在于，所述多个机器人组成的系统，对于其中一个机器人其构建的局部地图，至少存在另外一个机器人构建的局部地图，两者间有重叠区域。3.如权利要求2所述的一种多个机器人鲁棒协同定位方法，其特征在于，所述构建向量滤除模型，用于过滤错误数据关联，所构建的向量滤除模型包括用于约束的向量滤除模型和用于点对的向量滤除模型；具体包括如下步骤：构建用于约束的向量滤除模型，用于滤除错误的约束，所述用于约束的向量滤除模型具体为：对于机器人h建立的第k个子地图和机器人j建立的第l个子地图其中p
s
,p
t
为子地图中的点，q
s
,q
t
为子地图中的点；定义标志变量v(s,t)，得到式(1)的关系；如果(p
s
,q
s
)和(p
t
,q
t
)都是正确的，那么v(s,t)＝1，并且称(p
s
,q
s
)和(p
t
,q
t
)为一致的一对向量对；其他情况下v(s,t)＝0；其中δ为正常数；一个约束或者一个匹配点对的正确性基于式(1)进行估计；一个约束C是一个子地图和回环约束或者一个里程计约束为C中的每两个点对(p
s
,q
s
)和(p
t
,q
t
)计算v(s,t)的值，那么C中所有v(s,t)之和定义为式(2)中的)计算v(s,t)的值，那么C中所有v(s,t)之和定义为式(2)中的其中n
C
是C中的匹配点对的数量；在用于约束的向量滤除模型中，里程计约束被当成正确的回环约束，一个回环约束正确的概率基于来计算，其中定义为所有的平均值；式(3)给出了估计的C的正确概率；
其中为的上限，∈为的上限，σ设置为构建用于点对的向量滤除模型，用于过滤错误的点对，所述用于点对的向量滤除模型具体为：对于一个正确约束C
i
中的任意一个匹配点对(p
s
,q
s
)，定义式(4)中所示的满足向量一致性的点对数致性的点对数致性的点对数越大，点对(p
s
.q
s
)正确的概率越大，式(5)中定义约束和点对正确概率)正确的概率越大，式(5)中定义约束和点对正确概率其中∈1设置为而σ1设置为由此完成了向量滤除模型对约束和点对的建模。4.如权利要求1所述的一种多个机器人鲁棒协同定位方法，其特征在于，所述构建混合概率模型，从几何距离、语义关联和向量一致性的角度分别计算出数据关联中约束和点对的正确概率，具体为：建立存在错误数据关联的协同定位的问题模型：令O，L，T0分别为里程计约束集合O、回环约束集合L、子地图初始位姿集合T0；定义两种隐变量，关于约束的隐变量和关于点对的隐变量对的隐变量或者0表示是一个正确或错误的回环约束，而或0表示(p
s
,q
s
)是一个正确或错误的点对；令Z
L
和Z
P
分别是和的集合，用Z＝{Z
L
,Z
P
}统称所有种类的隐变量；那么协同定位问题建模为式(6)的最大似然估计问题MLE：T＝argmax p(O,L|T,Z)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)其中T为子地图的位姿集合；式(6)所表示的问题用期望最大化算法EM算法迭代求解，如式(7)所示；在EM算法的E步中，给定由第i次迭代得到的O、L和T
(i)th
...

【专利技术属性】
技术研发人员：王美玲，唐宇杰，岳裕丰，邓一楠，张骐绘，
申请(专利权)人：北京理工大学，
类型：发明
国别省市：