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

1.本发明涉及智能机器人技术领域，具体涉及一种多个机器人鲁棒协同定位方法。


背景技术：

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


技术实现要素：

5.有鉴于此，本发明提供了一种多个机器人鲁棒协同定位方法，能够适用于大范围、gps拒止环境，在大量错误数据关联存在的情况下实现多机器人间的正确协同定位。
6.为达到上述目的，本发明的技术方案包括如下步骤：
7.针对多个机器人组成的系统，其中多个机器人各自获取环境信息，分别构建环境的多个子地图，其中每个机器人构建一个局部地图，每个局部地图由多个子地图构成，构建子地图之间的约束；子地图之间的约束由若干匹配点对构成。
8.构建向量滤除模型，用于过滤错误数据关联，错误数据关联包括错误约束和正确约束中的错误点对。
9.构建混合概率模型，从几何距离、语义关联和向量一致性的角度分别计算出数据关联中约束和点对的正确概率。
10.双层期望最大化算法确定子地图的位姿并去除错误的约束，实现多机器人协同定位。
11.进一步地，多个机器人组成的系统，对于其中一个机器人所构建的局部地图，至少存在另外一个机器人构建的局部地图，两者间有重叠区域。
12.进一步地，构建向量滤除模型，用于过滤错误约束，所构建的向量滤除模型包括用
于约束的向量滤除模型和用于点对的向量滤除模型。
13.具体包括如下步骤：
14.构建用于约束的向量滤除模型，用于滤除错误的约束，用于约束的向量滤除模型具体为：
15.对于机器人h建立的第k个子地图和机器人j建立的第l个子地图其中ps,p
t
为子地图中的点，qs,q
t
为子地图中的点；定义标志变量v(s,t)，得到式(1)的关系；如果(ps,qs)和(p
t
,q
t
)都是正确的，那么v(s,t)＝1，并且称(ps,qs)和(p
t
,q
t
)为一致的一对向量对；其他情况下v(s,t)＝0。
[0016][0017]
其中δ为正常数。
[0018]
一个约束或者一个匹配点对的正确性基于式(1)进行估计；
[0019]
一个约束c是一个子地图和回环约束或者一个里程计约束为c中的每两个点对(ps,qs)和(p
t
,q
t
)计算v(s,t)的值，那么c中所有v(s,t)的和定义为式(2)中的
[0020][0021]
其中nc是c中的匹配点对的数量。
[0022]
在用于约束的向量滤除模型中，里程计约束被当成正确的回环约束，一个回环约束正确的概率基于来计算，其中定义为所有的平均值；式(3)给出了约束c的正确概率。
[0023][0024]
其中为的上限，∈为的上限，σ设置为
[0025]
构建用于点对的向量滤除模型，用于过滤错误的点对，用于点对的向量滤除模型具体为：
[0026]
对于一个正确约束ci中的任意一个匹配点对(ps,qs)，定义式(4)中所示的满足向量一致性的点对数
[0027][0028]
越大，点对(ps,qs)正确的概率越大，式(5)中定义点对正确概率
[0029][0030]
其中∈1设置为而σ1设置为
[0031]
由此完成了向量滤除模型对约束和点对的建模。
[0032]
进一步地，构建混合概率模型，从几何距离、语义关联和向量一致性的角度分别计算出数据关联中约束和点的正确概率，具体为：
[0033]
建立存在错误数据关联的协同定位的问题模型：令o，l，t0分别为里程计约束集合o、回环约束集合l、子地图初始位姿集合t0；定义两种隐变量，关于约束的隐变量和关于点对的隐变量或者0表示是一个正确或错误的回环约束，而或0表示(ps,qs)是一个正确或错误的点对；令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
)＝pg(z|o,l,t
(i)th
)
·
ps(z|o,l,t
(i)th
)
·
pv(z|o,l,t
(i)th
)
ꢀꢀ
(8)
[0041]
pg(z|o,l,t
(i)th
)、ps(z|o,l,t
(i)th
)、pv(z|o,l,t
(i)th
)是分别与几何距离、语义信息和向量一致性相关的概率值，这三种概率值彼此独立，将p(z|o,l,t
(i)th
)表示为三种概率值的乘积。
[0042]
下面用回环约束和点对分别替换z，得到两个子模型的概率表达式：
[0043]
(a)为回环约束建立混合概率模型，具体为：
[0044]
对于一个回环约束式(8)中的三种概率值将分别定义如下。
[0045]
被简写为其大小与中匹配点对的空间距离的和相关；被建模如下：
[0046]
[0047]
其中正相关于点对距离平方和，定义如下式(10)，是中点对的数量，φ是参数；
[0048][0049]
其中为第h个机器人建立的第k个子地图的位姿，θ为常数；
[0050]
简写成其大小与中含有相同语义标签的匹配点对的比例相关，该比例定义为的值越大，正确的概率越大；定义如下式(11)：
[0051][0052]
其中τ设置为0.1；
[0053]
简写为其定义即为向量滤除模型中的式(3)。
[0054]
是式(3)、(9)、(11)的乘积，简写为
[0055]
(b)为点对建立混合概率模型，具体为：
[0056]
对于正确约束中的一个匹配点对(ps,qs)，式(8)中的三种概率值将分别定义如下。
[0057]
简写为其大小与ps和qs的空间距离相关；被建模如下：
[0058][0059]
简写为当ps和qs的语义标签相同时其值为1，当ps和qs的语义标签不同时其值为0。
[0060]
简写为其定义与式(5)相同；对于(ps,qs)，如果与其一致的向量对的数量越多，则(ps,qs)正确的概率越大。
[0061]
是式(5)、式(12)和的乘积，简写为
[0062]
进一步地，双层期望最大化算法确定子地图的位姿并去除错误的约束，实现多机器人协同定位，具体为：
[0063]
(a)第一层期望最大化算法即第一层em算法
[0064]
在第一层em算法中，子地图的位姿没有配准，关于约束的隐变量为
[0065]
第一层em算法的e步中：计算回环约束的正确概率。考虑到不同机器人输出的子地
图的初始空间距离很大，在第一次执行e步时，式(9)中的不能表示的正确与否，据此第一层em算法的e步公式重新写为：
[0066][0067]
第一层em算法的m步中：是目标函数的权重；目标函数定义在式(14)中，通过最小化目标函数更新子地图的位姿；里程计约束被认为是正确的约束，其权重设置为1；
[0068][0069]
(b)第二层期望最大化算法即第二层em算法
[0070]
在第二层em算法中，输入的子地图位姿由第一层em算法输出得到，点对隐变量是输入的回环约束的正确概率大于阈值p0；
[0071]
第二层em算法的e步中：基于式(8)计算每一个输入点对(ps,qs)正确的概率：
[0072][0073]
第二层em算法的m步中：是该层目标函数的权重，通过最小化目标函数更新子地图的位姿，目标函数定义如式(16)，考虑到错误的点对可能分别存在于回环约束和里程计约束中，定义来表示机器人h建立的第k个子地图与机器人j建立的第l个子地图之间的约束，定义d为所有输入约束的集合：
[0074][0075]
第二层em算法输出配准后的子地图的位姿；层次化的em算法在式(17)满足时停止迭代：
[0076][0077]
其中是判断是否收敛的阈值。
[0078]
滤除错误数据关联并配准所有子地图后，即完成了极端错误情况下的多机器人协同定位任务，最终输出子地图位姿。
[0079]
有益效果：
[0080]
1：本发明提供了一种多个机器人鲁棒协同定位方法，首先，使用向量滤除模型过滤大多数错误数据关联。二、混合概率模型从几何距离、语义关联和向量一致性的角度分别计算出数据关联中约束和点的正确概率。最后，双层期望最大化算法迭代更新计算子地图的位姿并去除错误的数据关联，实现多机器人协同定位。本发明设计了双层期望最大化算法，其中第一层和第二层分别以约束的正确概率和匹配点对的正确概率作为隐变量，做到同时筛除错误的回环约束和正确约束中的错误匹配点对，提升了极端错误情况下多机协同定位的精度。
[0081]
2：本发明首先从匹配点对的几何一致性出发，通过向量滤除模型考虑约束和匹配
点对的正确性，并结合几何空间距离和语义一致性，设计了混合概率模型，从几何距离、语义一致性和向量滤除模型三个方面估计数据关联正确的概率。使用混合概率模型综合从多角度估计约束和匹配点对的正确概率，提升了极端错误情况下多机协同定位的鲁棒性。
附图说明
[0082]
图1为本发明提供的一种多个机器人鲁棒协同定位方法框架图。
具体实施方式
[0083]
下面结合附图并举实施例，对本发明进行详细描述。
[0084]
本发明的目的是提供一种针对大范围、gps拒止环境的多机器人鲁棒协同定位算法，可以在大量错误数据关联存在的情况下实现多机器人间的正确协同定位。方法框架见图1。
[0085]
下面结合图1并举实施例，对本发明进行详细描述。
[0086]
本发明的鲁棒协同定位算法由三个模块组成：向量滤除模型、混合概率模型和双层期望最大化算法。首先，使用向量滤除模型过滤大多数错误数据关联。二、混合概率模型从几何距离、语义关联和向量一致性的角度分别计算出数据关联中约束和点的正确概率。最后，双层期望最大化算法迭代更新计算子地图的位姿并去除错误的数据关联，实现多机器人协同定位。
[0087]
为使本发明的目的、技术方案和优势更加清晰，以下结合具体实施案例对本发明进一步详细说明。
[0088]
步骤一：多个机器人(一般为2-3个，更多数量的话，对于一个机器人其构建的局部地图，至少存在另外一个机器人构建的局部地图，两者间有重叠区域)各自获取环境信息，分别构建环境的多个子地图(每个机器人构建一个局部地图，每个局部地图由多个子地图构成)。之后使用特征描述子对各子地图进行特征提取、匹配，构建子地图之间的约束，由于子地图重叠度低、感知混叠等原因，这些约束中可能包含大量错误约束。对于一个机器人构建的局部地图，其所包含的相邻的子地图之间有约束，子地图按照时间顺序构建，对于多个机器人的局部地图，随机地产生多机器人之间子地图的约束。
[0089]
子地图间的约束一般由若干匹配点对构成。
[0090]
步骤二：对于机器人h建立的第k个子地图和机器人j建立的第l个子地图其中ps,p
t
为子地图中的点，qs,q
t
为子地图中的点；定义标志变量v(s,t)，可以得到式(1)的关系；如果(ps,qs)和(p
t
,q
t
)都是正确的，那么v(s,t)＝1，并且称(ps,qs)和(p
t
,q
t
)为一致的一对向量对；其他情况下v(s,t)＝0。
[0091][0092]
一个约束或者一个匹配点对的正确性可以基于式(1)进行估计。δ为正常数，其数值接近于0，实际取值可以是0.1、0.2。
[0093]
构建用于约束的向量滤除模型，用于滤除错误数据关联，错误数据关联包括错误约束和正确约束中的错误点对。本发明实施例中用于约束的向量滤除模型具体为：
[0094]
一个约束c可以是子地图和之间的回环约束或者一个里程计约束为c中的每两个点对(ps,qs)和(p
t
,q
t
)计算v(s,t)的值，那么c中所有v(s,t)的和可以定义为式(2)中的
[0095][0096]
其中nc是c中的匹配点对的数量。
[0097]
在该模型中，里程计约束总是被当成正确的回环约束。那么，一个回环约束正确的概率可以基于来计算，其中定义为所有的平均值。式(3)给出了估计的c的正确概率。
[0098][0099]
其中并且将σ设置为
[0100]
构建用于点对的向量滤除模型，用于过滤错误的点对，用于点对的向量滤除模型具体为：
[0101]
对于一个正确约束ci中的任意一个匹配点对(ps,qs)，定义式(4)中所示的满足向量一致性的点对数
[0102][0103]
越大，点对(ps.qs)正确的概率越大。在式(5)中定义该概率
[0104][0105]
其中∈1可以设置为而σ1可以设置为
[0106]
由此完成了向量滤除模型对约束和点对正确概率的建模。
[0107]
步骤三：从几何距离、语义和向量一致性三个方面构建约束和点对正确的概率模型，称为混合概率模型，估计所有约束和点对的正确概率值。
[0108]
(a)建立存在错误数据关联的协同定位的问题模型。
[0109]
令o，l，t0分别为里程计约束集合o、回环约束集合l、子地图初始位姿集合t0。由于错误数据关联的存在，因此协同定位需要实现找到正确的数据关联和所有子地图的正确位姿。定义两种隐变量：第一种隐变量和第二种隐变量或者0表示是一个正确或错误的回环约束，而或0表示(ps,qs)是一个正确或错误的点对。令z
l
和zp
分别是和的集合，用z＝{z
l
,z
p
}统称所有种类的隐变量。那么协同定位问题可以建模为式(6)的最大似然估计问题(maximum likelihood estimation，简称mle)。
[0110]
t＝argmax p(o,l|t,z)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
[0111]
其中t为子地图的位姿集合。
[0112]
式(6)所表示的问题可以用期望最大化算法(expectation maximization，简称em)迭代求解，如式(7)所示。
[0113][0114]
在e步中，给定由第i次迭代得到的o、l和t
(i)th
(t
(i)th
为第i次迭代时子地图位姿的集合)，计算p(z|o,l,t
(i)th
)。在m步中，通过最大化似然函数的期望更新t。
[0115]
p(z|o,l,t
(i)th
)可以拆分成式(8)的三个子函数，这里z可以分别替换成和
[0116]
p(z|o,l,t
(i)th
)＝pg(z|o,l,t
(i)th
)
·
ps(z|o,l,t
(i)th
)
·
pv(z|o,l,t
(i)th
)
ꢀꢀ
(8)
[0117]
pg(z|o,l,t
(i)th
)、ps(z|o,l,t
(i)th
)、pv(z|o,l,t
(i)th
)是分别与几何距离、语义信息和向量一致性相关的概率值。这三种概率值彼此独立，因此可以将p(z|o,l,t
(i)th
)表示为他们的乘积。
[0118]
下面用回环约束和点对分别替换z，可以得到两个子模型的概率表达式。
[0119]
(b)为建立混合概率模型
[0120]
对于一个回环约束式(8)中的三种概率值将分别定义如下。
[0121]
被简写为其大小与中匹配点对的空间距离的和相关。被建模如下：
[0122][0123]
其中点对距离平方和定义如下式(10)，是中点对的数量，φ是参数。
[0124][0125]
其中为第h个机器人建立的第k个子地图的位姿，θ为常数。
[0126]
简写成其大小与中含有相同语义标签的匹配点对的比例相关，该比例定义为的值越大，正确的概率越大；定义如下式(11)：
[0127][0128]
其中τ设置为0.1。
[0129]
简写为其定义即为向量滤除模型中的式(3)；
[0130]
是式(3)、(9)、(11)的乘积，简写为
[0131]
(c)为建立混合概率模型
[0132]
对于正确约束中的一个匹配点对(ps,qs)，式(8)中的三种概率值将分别定义如下。
[0133]
简写为其大小与ps和qs的空间距离相关；被建模如下：
[0134][0135]
简写为当ps和qs的语义标签相同时其值为1，当ps和qs的语义标签不同时其值为0。
[0136]
简写为其定义与式(5)相同；对于(ps,qs)，如果与其一致的向量对的数量越多，则(ps,qs)正确的概率越大。
[0137]
是式(5)、式(12)和的乘积，简写为
[0138]
总之，混合概率模型从三个方面建立了数据关联正确的概率，相比于单独的语义或几何距离信息而言可以更准确地滤除错误的数据关联。
[0139]
步骤四：双层期望最大化算法
[0140]
层次化期望最大化算法包含两个em过程，主要迭代更新数据关联正确的概率和子地图的位姿，配准不同机器人输出的子地图，实现多机器人的协同定位。
[0141]
(a)第一层期望最大化算法即第一层em算法：
[0142]
在第一层em算法中，子地图的位姿没有配准，关于约束的隐变量为
[0143]
e步：在第一层em算法的e步中，主要计算回环约束的正确概率。考虑到不同机器人输出的子地图的初始空间距离很大，在第一次执行e步时，式(9)中的不能表示的正确与否。因此，第一层em算法的e步公式可以重新写为：
[0144][0145]
m步：在第一层em算法的m步中，是目标函数的权重。目标函数定义在式(14)中，通过最小化目标函数更新子地图的位姿。在本发明中，里程计约束被认为是正确的约束，其权重设置为1。
[0146][0147]
(b)第二层期望最大化算法即第二层em算法：
[0148]
在第二层em算法中，输入的子地图位姿由第一层em算法输出得到，点对隐变量是输入的回环约束的正确概率大于阈值p0。
[0149]
e步：基于式(8)计算每一个输入点对(ps,qs)正确的概率，如下式所示。
[0150][0151]
m步：在第二层em算法的m步中，是该层目标函数的权重。通过最小化目标函数更新子地图的位姿，目标函数定义如式(16)所示。考虑到错误的点对可能分别存在于回环约束和里程计约束中，定义来表示机器人h建立的第k个子地图与机器人j建立的第l个子地图之间的约束，定义d为所有输入约束的集合。
[0152][0153]
第二层em算法输出配准后的子地图的位姿。层次化的em算法在式(17)满足时停止迭代。
[0154][0155]
其中是判断是否收敛的阈值。
[0156]
滤除错误数据关联并配准所有子地图后，即完成了极端错误情况下的多机器人协同定位任务，最终输出子地图位姿。
[0157]
综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。