(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211084214.2 (22)申请日 2022.09.06 (71)申请人 北京理工大 学 地址 100081 北京市海淀区中关村南大街5 号 (72)发明人 王美玲　唐宇杰　岳裕丰　邓一楠　 张骐绘　 (74)专利代理 机构 北京理工大 学专利中心 11120 专利代理师 李微微　代丽 (51)Int.Cl. G06T 7/73(2017.01) G01S 19/42(2010.01) (54)发明名称 一种多个 机器人鲁棒协同定位方法 (57)摘要 本发明公开了一种多个机器人鲁棒协同定 位方法， 涉及智 能机器人技术领域， 可 以在大量 错误数据关联存在的情况下实现多机器人间的 正确协同定位。 包括如下步骤： 针对多个机器人 组成的系统， 其中多个机器人各自获取环境信 息， 每个机器人构建一个局部地图， 每个局部地 图由多个子地图构成， 构建子地图之间的约束； 子地图之间的约束由若干匹配点对构成。 构建向 量滤除模型， 用于过滤错误约束和点对。 构建混 合概率模型， 从几何距离、 语义关联和向量一致 性的角度分别计算出数据关联中约束和点的正 确概率。 双层期望最大化算法确定子地图的位姿 并去除错误的数据关联， 实现多机器人协同定 位。 权利要求书4页 说明书10页 附图1页 CN 115439544 A 2022.12.06 CN 115439544 A 1.一种多个机器人鲁棒协同定位方法， 其特 征在于， 包括如下步骤： 针对多个机器人组成的系统， 其中多个机器人各自获取环境信息， 每个机器人构建一 个局部地图， 每个局部地图由多个子地图构成， 构建子地图之 间的约束； 子地图之间的约束 由若干匹配点对构成； 构建向量滤除模型， 用于过滤错误数据关联， 所述错误数据关联包括错误约束和正确 约束中的错 误点对； 构建混合概率模型， 从几何距离、 语义关联和向量一致性的角度分别计算出数据关联 中约束和点对的正确概 率； 双层期望最大化 算法确定 子地图的位姿并去除错 误的约束， 实现多机器人协同定位。 2.如权利要求1所述的一种 多个机器人鲁棒协同定位方法， 其特征在于， 所述多个机器 人组成的系统， 对于其中一个机器人其构建的局部地图， 至少存在另外一个机器人构建的 局部地图， 两者间有重 叠区域。 3.如权利要求2所述的一种 多个机器人鲁棒协同定位方法， 其特征在于， 所述构建 向量 滤除模型， 用于过滤错误数据关联， 所构建的向量滤除模型包括用于约束的向量滤除模型 和用于点对的向量滤除模型； 具体包括如下步骤： 构建用于约束的向量滤除模型， 用于滤除错误的约束， 所述用于约束的向量滤除模型 具体为： 对于机器人h建立的第k个子地图 和机器人j建立的第l个子地图 其中ps,pt为子 地图 中的点， qs,qt为子地图 中的点； 定义标志变量v(s,t)， 得到式(1)的关系； 如果 (ps,qs)和(pt,qt)都是正确的， 那么v(s,t)＝1， 并且称(ps,qs)和(pt,qt)为一致的一对向量 对； 其他情况下v(s,t)＝0； 其中δ 为正常数； 一个约束或者 一个匹配点对的正确性基于式(1)进行估计； 一个约束C 是一个子地图 和 回环约束 或者一个里程计约束 为C中的每 两个点对( ps,qs)和(pt,qt)计算v(s,t)的值， 那么C中所有v(s,t)之和定义为式(2)中的 其中nC是C中的匹配点对的数量； 在用于约束的向量滤 除模型中， 里程计约束 被当成正确的回环约束， 一个回环约 束 正确的概率基于 来计算， 其中 定义为所有 的平均值； 式(3)给出了估 计的C的正确概 率；权　利　要　求　书 1/4 页 2 CN 115439544 A 2其中 为 的上限 ， ∈为 的上限 ， σ设置为 构建用于点对的向量滤除模型， 用于过滤错误的点对， 所述用于点对的向量滤除模型 具体为： 对于一个正确约束Ci中的任意一个匹配点对(ps,qs)， 定义式(4)中所示的满足向量一 致性的点对数 越大， 点对(ps.qs)正确的概 率越大， 式(5)中定义约束和点对正确概 率 其中∈1设置为 而σ1设置为 由此完成了向量滤除模型对约束和点对的建模。 4.如权利要求1所述的一种 多个机器人鲁棒协同定位方法， 其特征在于， 所述构建混合 概率模型， 从几何距离、 语义关联和向量一致性的角度分别计算出数据关联中约束和点对 的正确概 率， 具体为： 建立存在错误数据关联的协同定位的问题模型： 令O， L， T0分别为里程计约束集合O、 回 环约束集合L、 子地图初始位姿集合T0； 定义两种隐变量， 关于约束的隐变量 和关于点 对的隐变量 或者0表示 是一个正确或错误的回环约束， 而 或0 表示(ps,qs)是一个正确或错误的点对； 令ZL和ZP分别是 和 的集合， 用Z＝{ZL,ZP} 统称所有种类的隐变量； 那么协同定位问题建模为式(6)的最大似然估计问题MLE： T＝argmax  p(O,L|T,Z)                     (6) 其中T为子地图的位姿集 合； 式(6)所表示的问题用期望最大化 算法EM算法迭代求 解， 如式(7)所示； 在EM算法的E步中， 给定由第 i次迭代得到的O、 L和T(i)th， T(i)th为第i次迭代时子地图位 姿的集合， 计算p(Z|O,L,T(i)th)； 在EM算法的M步中， 通过最大化似然函数的期望更新T； p(Z|O,L,T(i)th)拆分成式(8)的三个子函数， 这里Z分别替换成 和 权　利　要　求　书 2/4 页 3 CN 115439544 A 3