CN110110763B - 一种基于最大公共子图的栅格地图融合方法 - Google Patents

Abstract

本发明公开了一种基于最大公共子图的栅格地图融合方法，包括如下步骤：步骤1、使用ORB算法提取待融合栅格地图G₁和G₂的特征；步骤2、对步骤一中的特征进行聚类，计算聚类中心；步骤3、使用汉明距离计算聚类中心的初始匹配；步骤4、使用回溯法搜索满足约束要求的三组初始匹配；步骤5、根据位置关联计算最大公共子图；步骤6、判断是否还有未被回溯法访问过的初始匹配，如果是，则返回步骤四，否则进入步骤七；步骤7、选择最优最大公共子图方案；步骤8、根据最优最大公共子图方案计算变换矩阵，并结合栅格地图融合策略实现地图的融合。本发明能够解决基于图优化SLAM的前端构建和多机器人SLAM地图融合的问题。

Description

一种基于最大公共子图的栅格地图融合方法

技术领域

本发明属于移动机器人建图领域，特别是单机器人的graph-SLAM以及多机器人SLAM，具体涉及一种基于最大公共子图的栅格地图融合方法。

背景技术

当移动机器人在未知环境中工作且不能通过外部设备提供位姿以及环境信息时，此时就需要机器人根据自身携带的传感器对周围环境进行观测，并且根据观测的信息以及机器人运动信息确定周围环境和自身位姿。同时定位与地图创建(simultaneouslocalization and mapping，SLAM)就是该项技术，拥有该项技术的机器人可以完成导航、路径规划、探索等更加复杂的工作。因此SLAM是移动机器人真正实现自主化和智能化的关键。

基于图优化的方法是当前使用较多的SLAM算法，该方法分为前端和后端。前端根据传感器数据构建位姿图，后端根据位姿图信息使用优化方法计算全局最优的位姿。该方法充分利用SLAM问题的稀疏性，即机器人在不同时刻观测到的数据是有限的这一特点，提高算法的计算效率。前端包括顺序数据关联和环路闭合检测，顺序数据关联是将相邻时刻机器人的观测信息进行比较，环路闭合检测是对不同时刻，机器人的观测信息进行比较，来确定位姿图中边的信息。当使用栅格地图对环境进行描述时，前端需要匹配不同时刻机器人创建的子地图来计算约束信息。

随着近几十年单机器人SLAM在理论和实际应用上取得的突破，多机器人SLAM成为了移动机器人领域研究的重点之一。相比于单机器人，多个机器人可以并行工作，提高工作效率；多个机器人可以携带不同的传感器，提供更加丰富的观测信息，通过对这些观测信息的融合，可以得到更加精确的地图；多机器人对环境进行建图时，当其中一个或者几个机器人出现故障时，可以通过其余机器人继续协作完成环境模型的建立。地图融合是多机器人SLAM的研究重点之一，将每个机器人创建的局部环境地图进行融合，可以得到全局地图。

因此，研究栅格地图融合的问题，对于单机器人和多机器人SLAM都有着重要的意义。

发明内容

本发明目的是提供一种基于最大公共子图的栅格地图融合方法，用来解决基于图优化SLAM的前端构建和多机器人SLAM地图融合的问题。

本发明的技术方案是：一种基于最大公共子图的栅格地图融合方法，包括如下步骤：

步骤1、使用ORB算法提取待融合栅格地图G₁和G₂的特征；

步骤2、对步骤一中的特征进行聚类，计算聚类中心；

步骤3、使用汉明距离计算聚类中心的初始匹配，所述聚类中心的初始匹配由矩阵表示；

步骤4、使用回溯法搜索满足约束要求的三组初始匹配；

步骤5、根据位置关联计算最大公共子图；

步骤6、判断是否还有未被回溯法访问过的初始匹配，如果是，则返回步骤四，否则进入步骤七；

步骤7、选择最优最大公共子图方案；

步骤8、根据最优方案计算变换矩阵，并结合栅格地图融合策略实现地图的融合。

上文中，本发明使用ORB算法提取待融合栅格地图的特征，用这些特征来表征栅格地图。

上述技术方案中，所述步骤1中的待融合栅格地图是由一个机器人在不同时间对环境进行建模或者由多个机器人对环境进行建模得到的，各所述待融合栅格地图之间存在重叠区域。

上述技术方案中，所述步骤1中的待融合栅格地图是由SLAM算法处理机器人内部和外部传感器数据进而得到的环境描述。

上述技术方案中，所述步骤2的聚类中心通过比较特征点的距离，将距离小于阈值的聚为一类，并且以这一类特征点的几何中心作为聚类中心，聚类中心由这一类中所有特征点的描述子共同描述。

上述技术方案中，所述步骤3中使用汉明聚类对特征点描述子进行计算，并且将距离小于设定阈值的认为满足初始匹配要求，再根据特征点的初始匹配计算出聚类中心的初始匹配。

上述技术方案中，所述步骤4中回溯法搜索的约束条件为：

其中，M_e＝{m₁，…，m_n}表示使用回溯法对聚类中心的初始匹配的每一行进行访问，初始匹配第k行表示G₁中第k个聚类中心和G₂中的哪些聚类中心满足初始匹配。所提方法假设聚类中心的匹配是一一对应的，因此初始匹配中的每一行最多只能有一组匹配在M₀中；

表示G₁中第i个聚类中心和G₂中第i′个聚类中心满足初始匹配要求；/>

和/>

分别表示聚类中心在各自栅格地图上的坐标；∈₀为误差阈值；当M₀中只有一组匹配时，默认满足约束条件。

上述技术方案中，所述步骤5中的位置关联算法的具体步骤为：

步骤51、当回溯法搜索到三组满足约束要求的初始匹配时，根据这三组聚类中心的匹配计算初始变换矩阵；

步骤52、根据初始变换矩阵，将待融合栅格地图G₂中的聚类中心变换到栅格地图G₁的坐标系下；

步骤53、比较变换之后的G₂中的聚类中心与G₁中的聚类中心之间的距离，将距离小于一定阈值且满足初始匹配要求的匹配作为当前初始变换矩阵的最大公共子图方案。

上述技术方案中，所述步骤七中选择最优方案的具体步骤为：

步骤71、将最大公共子图方案中匹配聚类中心数目最多的作为最优方案的候选；

步骤72、如果匹配聚类中心数目最多的最大公共子图方案只有一个，则该方案就是最优方案；

步骤73、如果匹配聚类中心数目最多的最大公共子图方案不止一个，则将匹配聚类中心构成多边形面积最大的作为最优方案。

上述技术方案中，所述步骤8中的栅格地图融合策略具体为：

其中，

表示根据变换矩阵将栅格地图G₂的栅格坐标变换到栅格地图G₁的坐标系下；I(·)表示相应栅格坐标对应的灰度值；G₁₂表示融合后栅格坐标对应的灰度值。

上述技术方案中，本发明可用于多个栅格地图的融合。

本发明的优点是：

本发明使用ORB特征来表征待融合栅格地图；根据ORB对特征点的描述特性，使用汉明距离计算初始匹配；使用回溯法搜索满足约束要求的三组初始匹配，并基于位置关联方法计算最大公共子图；最后选择最优最大公共子图方案，并且计算最优变换的矩阵，以实现栅格地图的精确融合。

附图说明

下面结合附图及实施例对本发明作进一步描述：

图1为本发明实施例一的方法流程图。

图2为本发明实施例一的待融合栅格地图。

图3为本发明实施例一的待融合栅格地图ORB特征点。

图4为本发明实施例一的待融合栅格地图聚类中心。

图5为经本发明的方法处理的地图融合结果。

具体实施方式

实施例一：

参见图1所示，本发明提供一种基于最大公共子图的栅格地图融合方法，包括如下步骤：

步骤1、使用ORB算法提取待融合栅格地图G₁和G₂的特征；

其中，栅格地图是通过SLAM算法处理机器人传感器信息得到的环境地图描述；待融合栅格地图是由一个机器人在不同时间创建或者多个机器人在同一时间创建；待融合栅格地图存在重叠区域，参见图2所示。

如图3所示，图中‘*’表示使用ORB算法从待融合栅格地图中提取的特征。从图中可以看出，特征点分布在栅格地图的角点处，这也符合所提算法的要求。但是从图中同样可以看出，在同一角点处分布太多的ORB特征，这会造成算法的重复计算，影响算法的计算效率。

步骤2、对步骤一中的特征进行聚类，计算聚类中心；

如图4所示，图中‘*’表示聚类中心。将距离相近的ORB特征点聚为一类，以该类特征点的几何中心来表示聚类中心。从图中可以看出，聚类中心分布在栅格地图的角点处，且在同一角点处分布较少的聚类中心，降低了接下来算法的计算量。

步骤3、使用汉明距离计算聚类中心的初始匹配，所述聚类中心的初始匹配由矩阵表示；

因为ORB算法对特征点使用二进制形式的描述子进行描述，因此，使用汉明距离计算特征点的初始匹配，并且根据特征点与聚类中心的对应关系，计算聚类中心的初始匹配。

步骤4、使用回溯法搜索满足约束要求的三组初始匹配；

其中，回溯法搜索的约束条件为：

其中，M₀＝{m₁，…，m_n}表示使用回溯法对聚类中心的初始匹配的每一行进行访问，初始匹配第k行表示G₁中第k个聚类中心和G₂中的哪些聚类中心满足初始匹配。所提方法假设聚类中心的匹配是一一对应的，因此初始匹配中的每一行最多只能有一组匹配在M₀中。

表示G₁中第i个聚类中心和G₂中第i′个聚类中心满足初始匹配要求，/>

和/>

分别表示聚类中心在各自栅格地图上的坐标，∈₀为误差阈值。当M₀中只有一组匹配时，默认满足约束条件。

步骤5、根据位置关联计算最大公共子图；

根据回溯法搜索到满足约束条件的三组初始匹配，根据这三组匹配可以计算初始变换矩阵，将待融合栅格地图G₂中的聚类中心变换到栅格地图G₁的坐标系下，比较变换之后G₂中的聚类中心与G₁中的聚类中心之间的距离，将距离小于阈值并且满足初始匹配的聚类中心作为当前初始变换的最大公共子图方案。

步骤6、判断是否还有未被回溯法访问过的初始匹配，如果是，则返回步骤四，否则进入步骤七。

步骤7、选择最优最大公共子图方案；

当所有的最大公共子图方案都计算出来之后，将匹配聚类中心数目最多的并且匹配聚类中心构成的多边形面积最大的作为最优最大公共子图方案。

步骤8、根据最优方案计算变换矩阵，并结合栅格地图融合策略实现地图的融合；

其中，所述步骤8中的栅格地图融合策略具体为：

其中，

表示根据变换矩阵将栅格地图G₂的栅格坐标变换到栅格地图G₁的坐标系下；I(·)表示相应栅格坐标对应的灰度值；G₁₂表示融合后栅格坐标对应的灰度值。

参见图5所示，为根据本发明所提方法进行栅格地图融合的结果，从结果可以看出，本发明的方法可以实现栅格地图的精确融合。

需要说明的是，本发明可用于多个栅格地图的融合。

当然上述实施例只为说明本发明的技术构思及特点，其目的在于让熟悉此项技术的人能够了解本发明的内容并据以实施，并不能以此限制本发明的保护范围。凡根据本发明主要技术方案的精神实质所做的修饰，都应涵盖在本发明的保护范围之内。

Claims (7)

1.一种基于最大公共子图的栅格地图融合方法，其特征在于，包括如下步骤：

步骤1、使用ORB算法提取待融合栅格地图G₁和G₂的特征；

步骤2、对步骤一中的特征进行聚类，计算聚类中心；

步骤3、使用汉明距离计算聚类中心的初始匹配，所述聚类中心的初始匹配由矩阵表示；

步骤4、使用回溯法搜索满足约束要求的三组初始匹配；

步骤5、根据位置关联计算最大公共子图；

步骤6、判断是否还有未被回溯法访问过的初始匹配，如果是，则返回步骤四，否则进入步骤七；

步骤7、选择最优最大公共子图方案；

步骤8、根据最优方案计算变换矩阵，并结合栅格地图融合策略实现地图的融合；

所述步骤4中回溯法搜索的约束条件为：

其中，M₀＝{m₁，…，m_n}表示使用回溯法对聚类中心的初始匹配的每一行进行访问；

表示G₁中第i个聚类中心和G₂中第i′个聚类中心满足初始匹配要求；

和/>

分别表示聚类中心在各自栅格地图上的坐标；∈₀为误差阈值；当M₀中只有一组匹配时，默认满足约束条件；

所述步骤5中的位置关联算法的具体步骤为：

步骤51、当回溯法搜索到三组满足约束要求的初始匹配时，根据这三组聚类中心的匹配计算初始变换矩阵；

步骤52、根据初始变换矩阵，将待融合栅格地图G₂中的聚类中心变换到栅格地图G₁的坐标系下；

步骤53、比较变换之后的G₂中的聚类中心与G₁中的聚类中心之间的距离，将距离小于误差阈值∈₀且满足初始匹配要求的匹配作为当前初始变换矩阵的最大公共子图方案；

所述步骤8中的栅格地图融合策略具体为：

其中，

表示根据变换矩阵将栅格地图G₂的栅格坐标变换到栅格地图G₁的坐标系下；I(·)表示相应栅格坐标对应的灰度值；G₁₂表示融合后栅格坐标对应的灰度值。

2.根据权利要求1所述的基于最大公共子图的栅格地图融合方法，其特征在于：所述步骤1中的待融合栅格地图是由一个机器人在不同时间对环境进行建模或者由多个机器人对环境进行建模得到的，各所述待融合栅格地图之间存在重叠区域。

3.根据权利要求1所述的基于最大公共子图的栅格地图融合方法，其特征在于：所述步骤1中的待融合栅格地图是由SLAM算法处理机器人内部和外部传感器数据进而得到的环境描述。

4.根据权利要求1所述的基于最大公共子图的栅格地图融合方法，其特征在于：所述步骤2的聚类中心通过比较特征点的距离，将距离小于阈值的聚为一类，并且以这一类特征点的几何中心作为聚类中心，聚类中心由这一类中所有特征点的描述子共同描述。

5.根据权利要求4所述的基于最大公共子图的栅格地图融合方法，其特征在于：所述步骤3中使用汉明聚类对特征点描述子进行计算，并且将距离小于设定阈值的认为满足初始匹配要求，再根据特征点的初始匹配计算出聚类中心的初始匹配。

6.根据权利要求1所述的基于最大公共子图的栅格地图融合方法，其特征在于：所述步骤7中选择最优方案的具体步骤为：

步骤71、将最大公共子图方案中匹配聚类中心数目最多的作为最优方案的候选；

步骤72、如果匹配聚类中心数目最多的最大公共子图方案只有一个，则该方案就是最优方案；

步骤73、如果匹配聚类中心数目最多的最大公共子图方案不止一个，则将匹配聚类中心构成多边形面积最大的作为最优方案。

7.根据权利要求1至6中任一项所述的基于最大公共子图的栅格地图融合方法，其特征在于：用于多个栅格地图的融合。

Images