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

Abstract

本发明公开了一种基于最大公共子图的栅格地图融合方法，包括以下步骤：S1、创建环境的栅格地图；S2、提取待融合的栅格地图的Harris角点；S3、从每个待融合的栅格地图中提取出三个角点；S4、判断输入的三对角点是否可以构成三角形同构方案，如果不可以，则返回S3；如果可以，则执行S5；S5、迭代构造多边形同构方案；S6、判断待融合的栅格地图中是否还有角点未被带入三角形同构方案，如果有，则返回S3；如果没有，则执行S7；S7、选择最优的多边形同构方案，以及对应的最优变换矩阵；S8、根据最优的变换矩阵以及融合规则，实现栅格地图融合。本发明能可靠的实现栅格地图的融合，并且具有融合精度高的优点。

Description

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

技术领域

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

背景技术

同时定位与地图创建对于移动机器人来说是一项关键技术，移动机器人具备该项技术可以通过自身携带的传感器探索未知环境并且完成更加复杂的工作。学术界对此项技术的研究已经有三十多年了，也提出了很多有效的单机器人SLAM的方法。单机器人SLAM方法大体上可以分为两种：一是基于滤波的方法，二是基于优化的方法。近些年，随着一些高效的求解方法的提出，graph-SLAM成为了单机器人SLAM研究的重点。

Graph-SLAM可以分为前端和后端两个部分。其中前端主要实现图的构建，这里面包括扫描匹配和闭环检测；后端主要用来对图进行优化。Graph-SLAM的一个难点在于如何进行子图的融合，特别是闭环检测。

随着单机器人在理论与实践上取得的突破，近些年来，多机器人SLAM成为了移动机器人领域的研究重点。与单机器人相比，多机器人SLAM可以并行工作，这就节省了建图的时间，提高建图的效率；多机器人SLAM有很好的抗干扰性，当一个或几个机器人出现故障，其他机器人可以继续协作完成地图的创建，这在实践中显得尤为重要；另外，多机器人可以获得更多的环境信息，提高了所创建地图的精确度。

由此可见，无论是单机器人graph-SLAM还是多机器人SLAM，都必须解决地图的融合问题。因此，需要研究出一种精度高、速度快的地图融合方法，这可以扩大移动机器人的应用范围，对社会智能化发展有着重要的意义。

发明内容

为了解决上述技术问题，本发明提供了一种基于最大公共子图的栅格地图融合方法。

为了达到上述目的，本发明的技术方案如下：

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

S1、两个机器人创建环境的栅格地图或者一个机器人在不同的时间创建环境的栅格地图，并且两个栅格地图之间存在重叠；

S2、提取待融合的栅格地图的Harris角点；

S3、从每个待融合的栅格地图中提取出三个角点；

S4、判断输入的三对角点是否可以构成三角形同构方案，如果不可以，则返回S3；如果可以，则执行S5；

S5、迭代构造多边形同构方案；

S6、判断待融合的栅格地图中是否还有角点未被带入三角形同构方案，如果有，则返回S3；如果没有，则执行S7；

S7、选择最优的多边形同构方案，即为最大公共子图，以及对应的最优变换矩阵；

S8、根据最优的变换矩阵以及融合规则，实现栅格地图融合。

优选的，步骤S2中的角点提取方法使用的是Harris角点提取算法。

优选的，步骤S4中的三角形同构方案包括以下步骤：

S41.判断同一栅格地图中的三个角点是否可以构成三角形，如果不可以，则返回步骤S3；如果可以，则进行步骤S42；

S42.计算同一栅格地图中三个角点构成的三角形的边长，并按照从小到大的顺序排列；

S43.使用同构误差公式计算误差，同构误差公式具体为：

e＝{|a₁-b₁|+|a₂-b₂|+|a₃-b₃|}

其中，{a₁，a₂，a₃}以及{b₁，b₂，b₃}代表按照从小到大排列的三角形的边长；

S44.如果同构误差大于给定的阈值，则返回步骤S3；如果小于给定阈值，则将这三对角点保存，带入步骤S5。

优选的，步骤S5具体为：

S51.假设待融合的栅格地图为G_A和G_B，对应的提取出来的角点集合为V_A和V_B；首先，由步骤S4得到的三角形同构方案M中三对角点对应关系，求出初始变换矩阵为

并计算变换之后的角点距离为：

其中，M＝{M_A，M_B}，m_Ai∈M_A，m_Bi∈M_R，

表示将m_Bi中的坐标根据变换矩阵

做变换；

S52.将M中的角点从角点集合中删除，得到新的角点集合为V_A′和V_B′；

S53.根据初始变换矩阵

将V_B′中的角点坐标变换为

S54.计算V_A′与

中角点的距离，并判断最小的距离是否小于给定阈值，如果小于，将最小值对应的这对角点加入到M中，则M由于加入了角点变成M′；如果大于，则迭代结束；

S55.根据M′计算新的变换矩阵

并且计算距离D₁，D₂：

其中，

S56.比较D₁与D₂的大小，如果D₂≤D₁，则加入的一对角点在两个栅格地图的重叠区域；返回步骤S5的S51，迭代更多的角点来优化变换矩阵；

S57.如果V_A′或者V_B′中有一个为空集，或者V_A′与

中角点的最小距离大于给定阈值，则迭代结束，执行步骤S7。

优选的，步骤S7中的选择最优多边形同构方案具体为：

将多边形同构方案中在G_B中的角点坐标根据该方案得到的变换矩阵做变换，并且求解变换之后的角点坐标与其对应的在G_A中的角点坐标的距离之和，再将该距离和除以多边形同构方案的边数，得到距离误差的平均值，则误差平均值最小的多边形同构方案即为最优方案，其对应的变换矩阵即为最优变换矩阵。

优选的，步骤S8的融合规则具体为：

S81.创建一个空地图，该地图的边长为待融合地图中最大边长的3倍；

S82.将地图G_B中的坐标根据最优变换矩阵做变换；

S83.比较G_B变换后的栅格值与对应的G_A的栅格值，若对应的栅格有一个为占据，则融合后的地图该栅格为占据；若对应的栅格都不为占据，且有一个为空，则融合后的地图该栅格为空；其他的融合后的栅格为未知。

优选的，待融合的栅格地图是以栅格形式表示的局部地图，且待融合的栅格地图必须存在重叠区域。

优选的，栅格地图是由SLAM算法处理机器人的内部以及外部传感器数据进而得到的环境描述。

优选的，栅格地图被划分成等分辨率的栅格，每个栅格的值代表该栅格被障碍物占据的可能，其由空、未知和占据三种状态表示。

优选的，步骤S7中寻找最大公共子图是通过将栅格地图上的角点带入三角形同构方案以及迭代多边形同构方案，进而寻找最优多边形同构方案来实现。

本发明相较于现有技术，具有以下有益效果：本发明通过运用提取出来的栅格地图上的Harris角点来迭代寻找最大公共子图，再通过最大公共子图上角点的对应关系计算最优变换的矩阵，以实现栅格地图的精确融合。

附图说明

图1为本发明所提方法的工作流程图。

图2是待融合的栅格地图。

图3是图2提取出来的Harris角点。

图4是三角形同构方案的结果。

图5是迭代多边形同构方案的结果。

图6是使用本发明提出方法的地图融合结果。

具体实施方式

下面结合附图详细说明本发明的优选实施方式。

为了达到本发明的目的，如图1所示，在本发明的其中一种实施方式中提供一种基于最大公共子图的栅格地图融合方法，包括以下步骤：

S1、两个机器人创建环境的栅格地图或者一个机器人在不同的时间创建环境的栅格地图，并且两个栅格地图之间存在重叠；

如图2所示，栅格地图是由SLAM算法处理机器人的内部以及外部传感器数据进而得到的环境描述，待融合的栅格地图是以栅格形式表示的局部地图，从图中可以看出，待融合的栅格地图必须存在重叠区域；

S2、提取待融合的栅格地图的Harris角点；

如图3所示，图中‘*’表示从栅格地图上提取出来的Harris角点；

S3、从每个待融合的栅格地图中提取出三个角点；

S4、判断输入的三对角点是否可以构成三角形同构方案，如果不可以，则返回S3；如果可以，则执行S5；

三角形同构方案是指将同一个栅格地图上的三个角点组成的三角形的边长按照从小到大的顺序排列，再求这些不同地图上的排列好的边长的差的绝对值之和，当这个和小于给定阈值时，则这两个三角形同构，将这些满足三角形同构方案(如图4)的角点带入步骤S5；如果和比给定的阈值大或者一个栅格地图上的三个点不能构成三角形，则返回步骤S3。

S5、迭代构造多边形同构方案；

该方案利用迭代的方法，每次加入一组角点(来自不同的栅格地图)来构造多边形同构。首先，根据输入的角点对应关系来计算初始变换矩阵，并且将这些输入的角点对从原始角点集合中删除。利用初始变换矩阵对需要平移旋转的地图上的输入角点进行变换，并计算变换之后的角点坐标和与其相对应的另一地图上的角点坐标之间的距离。将要平移旋转的那个地图的删除输入角点之后的角点集合根据初始变换矩阵进行变换，求解变换之后的坐标与另一地图删除输入角点之后的角点坐标之间的距离，求出距离的最小值。如果最小值小于给定阈值，则将这对最小距离对应的角点加入同构方案中，用来优化变换矩阵；如果最小距离大于阈值，则迭代结束。根据加入一对角点之后新的同构方案角点之间的对应关系计算新的变换矩阵，对新的同构方案中的待平移旋转地图上的角点根据前一个变换矩阵和新的变换矩阵进行变换，并且分别求解变换之后的坐标与之相对应的另一个地图上的角点坐标之间的距离之和。若新的变换矩阵变换之后的距离之和小于前一变换矩阵变换之后的距离之和，则新加入的角点对在待合并地图的重叠区域；反之则不在重叠区域。该迭代方案直到一个角点集合里面的点都被带入过同构方案或者最小距离之和大于阈值才结束。

S6、判断待融合的栅格地图中是否还有角点未被带入三角形同构方案，如果有，则返回S3；如果没有，则执行S7；

S7、选择最优的多边形同构方案，即为最大公共子图，以及对应的最优变换矩阵；

选择最优的多边形同构方案时将多边形同构方案中待旋转平移的地图上的角点根据其计算出来的变换矩阵进行变换，并且求出变换之后与另一个地图上相对应角点之间的距离之和，再用距离之和除以多边形同构方案的边数得到误差平均值，误差平均值最小的即为最优多边形同构方案，与之对应的变换矩阵即为最优变换矩阵。如图5所示，选择最优多边形同构方案(即最大公共子图)可以精确的找到待融合地图上的重叠区域的角点。

S8、根据最优的变换矩阵以及融合规则，实现栅格地图融合；

求出最优变换矩阵之后，根据此矩阵将待旋转平移的栅格地图的每个栅格坐标进行坐标变换，并且比较变换后的栅格值与另一个待融合的地图相应栅格值，如果两个栅格值有一个为占据，则融合后的地图在该栅格即为占据；如果两个栅格都不为占据，且有一个为空，则融合后的地图在该栅格即为空；否则融合后的地图在该栅格即为未知。

综上所述，本发明提出的基于最大公共子图的栅格地图融合方法可以精确的实现栅格地图的融合。该方法与单机器人的graph-SLAM或者多机器人SLAM相结合，可以提高移动机器人的建图效率以及准确性。

具体地，步骤S4中的三角形同构方案包括以下步骤：

S41.判断同一栅格地图中的三个角点是否可以构成三角形，如果不可以，则返回步骤S3；如果可以，则进行步骤S42；

S42.计算同一栅格地图中三个角点构成的三角形的边长，并按照从小到大的顺序排列；

S43.使用同构误差公式计算误差，同构误差公式具体为：

e＝{|a₁-b₁|+|a₂-b₂|+|a₃-b₂|}

其中，{a₁，a₂，a₃}以及{b₁，b₂，b₃}代表按照从小到大排列的三角形的边长；

S44.如果同构误差大于给定的阈值，则返回步骤S3；如果小于给定阈值，则将这三对角点保存，带入步骤S5。

另外，步骤S5具体为：

S51.假设待融合的栅格地图为G_A和G_B，对应的提取出来的角点集合为V_A和V_B；首先，由步骤S4得到的三角形同构方案M中三对角点对应关系，求出初始变换矩阵为

并计算变换之后的角点距离为：

其中，M＝{M_A，M_B}，m_Ai∈M_A，m_Bi∈M_B，

表示将m_Bi中的坐标根据变换矩阵

做变换；

S52.将M中的角点从角点集合中删除，得到新的角点集合为V_A′和V_B′；

S53.根据初始变换矩阵

将V_B′中的角点坐标变换为

S54.计算V_A′与

中角点的距离，并判断最小的距离是否小于给定阈值，如果小于，将最小值对应的这对角点加入到M中，则M由于加入了角点变成M′；如果大于，则迭代结束；

S55.根据M′计算新的变换矩阵

并且计算距离D₁，D₂：

其中，M′＝{M＇_A，M′_B}，m′_Ai∈M′_A，m′_Bi∈M′_B；

S56.比较D₁与D₂的大小，如果D₂≤D₁，则加入的一对角点在两个栅格地图的重叠区域；返回步骤S5的S51，迭代更多的角点来优化变换矩阵；

S57.如果V_A′或者V_B′中有一个为空集，或者V_A′与

中角点的最小距离大于给定阈值，则迭代结束，执行步骤S7。

步骤S7中的选择最优多边形同构方案具体为：

将多边形同构方案中在G_B中的角点坐标根据该方案得到的变换矩阵做变换，并且求解变换之后的角点坐标与其对应的在G_A中的角点坐标的距离之和，再将该距离和除以多边形同构方案的边数，得到距离误差的平均值，则误差平均值最小的多边形同构方案即为最优方案，其对应的变换矩阵即为最优变换矩阵。

步骤S8的融合规则具体为：

S81.创建一个空地图，该地图的边长为待融合地图中最大边长的3倍；

S82.将地图G_B中的坐标根据最优变换矩阵做变换；

S83.比较G_B变换后的栅格值与对应的G_A的栅格值，若对应的栅格有一个为占据，则融合后的地图该栅格为占据；若对应的栅格都不为占据，且有一个为空，则融合后的地图该栅格为空；其他的融合后的栅格为未知。

以上所述的仅是本发明的优选实施方式，应当指出，对于本领域的普通技术人员来说，在不脱离本发明创造构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

Claims (8)

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

S1、两个机器人创建环境的栅格地图或者一个机器人在不同的时间创建环境的栅格地图，并且两个栅格地图之间存在重叠；

S2、提取待融合的栅格地图的Harris角点；

S3、从每个待融合的栅格地图中提取出三个角点；

S4、判断输入的三对角点是否可以构成三角形同构方案，如果不可以，则返回S3；如果可以，则执行S5；

S5、迭代构造多边形同构方案；

S6、判断待融合的栅格地图中是否还有角点未被带入三角形同构方案，如果有，则返回S3；如果没有，则执行S7；

S7、选择最优的多边形同构方案，即为最大公共子图，以及对应的最优变换矩阵；

S8、根据最优的变换矩阵以及融合规则，实现栅格地图融合；

其中，步骤S4中的三角形同构方案包括以下步骤：

S41.判断同一栅格地图中的三个角点是否可以构成三角形，如果不可以，则返回步骤S3；如果可以，则进行步骤S42；

S42.计算同一栅格地图中三个角点构成的三角形的边长，并按照从小到大的顺序排列；

S43.使用同构误差公式计算误差，同构误差公式具体为：

e＝{|a₁-b₁|+|a₂-b₂|+|a₃-b₃|}

其中，{a₁，a₂，a₃}以及{b₁，b₂，b₃}代表按照从小到大排列的三角形的边长；

S44.如果同构误差大于给定的阈值，则返回步骤S3；如果小于给定阈值，则将这三对角点保存，带入步骤S5；

以及，步骤S5具体为：

S51.假设待融合的栅格地图为G_A和G_B，对应的提取出来的角点集合为V_A和V_B；首先，由步骤S4得到的三角形同构方案M中三对角点对应关系，求出初始变换矩阵为

并计算变换之后的角点距离为：

其中，M＝{M_A，M_B}，m_Ai∈M_A，m_Bi∈M_B，

表示将m_Bi中的坐标根据变换矩阵

做变换；

S52.将M中的角点从角点集合中删除，得到新的角点集合为V_A′和V_B′；

S53.根据初始变换矩阵

将V_B′中的角点坐标变换为

S54.计算V_A′与

中角点的距离，并判断最小的距离是否小于给定阈值，如果小于，将最小值对应的这对角点加入到M中，则M由于加入了角点变成M′；如果大于，则迭代结束；

S55.根据M′计算新的变换矩阵

并且计算距离D₁，D₂：

其中，M′＝{M′_A，M′_B}，m′_Ai∈M′_A，m′_Bi∈M′_B；

S56.比较D₁与D₂的大小，如果D₂≤D₁，则加入的一对角点在两个栅格地图的重叠区域；返回步骤S5的S51，迭代更多的角点来优化变换矩阵；

S57.如果V_A′或者V_B′中有一个为空集，或者V_A′与

中角点的最小距离大于给定阈值，则迭代结束，执行步骤S7。

2.根据权利要求1所述的基于最大公共子图的栅格地图融合方法，其特征在于，步骤S2中的角点提取方法使用的是Harris角点提取算法。

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

将多边形同构方案中在G_B中的角点坐标根据该方案得到的变换矩阵做变换，并且求解变换之后的角点坐标与其对应的在G_A中的角点坐标的距离之和，再将该距离和除以多边形同构方案的边数，得到距离误差的平均值，则误差平均值最小的多边形同构方案即为最优方案，其对应的变换矩阵即为最优变换矩阵。

4.根据权利要求1所述的基于最大公共子图的栅格地图融合方法，其特征在于，步骤S8的融合规则具体为：

S81.创建一个空地图，该地图的边长为待融合地图中最大边长的3倍；

S82.将地图G_B中的坐标根据最优变换矩阵做变换；

S83.比较G_B变换后的栅格值与对应的G_A的栅格值，若对应的栅格有一个为占据，则融合后的地图该栅格为占据；若对应的栅格都不为占据，且有一个为空，则融合后的地图该栅格为空；其他的融合后的栅格为未知。

5.根据权利要求1所述的基于最大公共子图的栅格地图融合方法，其特征在于，待融合的栅格地图是以栅格形式表示的局部地图，且待融合的栅格地图必须存在重叠区域。

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

7.根据权利要求1所述的基于最大公共子图的栅格地图融合方法，其特征在于，栅格地图被划分成等分辨率的栅格，每个栅格的值代表该栅格被障碍物占据的可能，其由空、未知和占据三种状态表示。

8.根据权利要求1所述的基于最大公共子图的栅格地图融合方法，其特征在于，步骤S7中寻找最大公共子图是通过将栅格地图上的角点带入三角形同构方案以及迭代多边形同构方案，进而寻找最优多边形同构方案来实现。

Images