本发明涉及数据处理技术领域，尤其涉及一种闭环检测方法、装置、终端及存储介质。
背景技术：
通过机器人进行地图构建时，往往需要将机器人当前所在的位置，与机器人之前遍历的位置进行相似性匹配，以确定机器人当前所在位置是否为以访问过的地方，上述这种位置匹配方式可认为是机器人的闭环检测。
在基于机器人的地图构建中，一般会采用激光雷达作为传感器获取环境信息，因此闭环检测问题就可看作激光数据扫描匹配结果相似性的问题。现有的闭环检测方法中，通常采用一帧激光数据与一帧激光数据的扫描匹配算法(帧-帧)，但该种方法的不足在于：容易因激光数据的局部相似性高，产生错误的匹配结果，造成错误的闭环检测；此外，在所构建的地图范围较大时，需要匹配的激光数据量会大幅度增加，由此影响闭环检测的实时性。
基于上述问题，技术人员提出了闭环检测的改进方法，如，提取机器人所遍历环境中的小范围特征来形成特征区域，通过比较不同特征区域的相似性来进行闭环检测，又如，用每隔设定帧的激光数据作为一个关键帧，通过比较不同关键帧的相似性来进行闭环检测。上述改进方法的核心思想均相当于选取关键帧用于闭环检测，可以达到减少匹配候选集的数量，提高检测速度的目的，但上述进方法也存在一定的不足：关键帧的选取会随机性的丢失部分数据，导致实际存在闭环关系的两个关键帧因相似部分较少无法检测到存在闭环。
技术实现要素：
本发明实施例提供的一种闭环检测方法、装置、终端及存储介质，不仅提高闭环的检测速度，还有效提高了闭环检测的准确率。
第一方面，本发明实施例提供了一种闭环检测方法，包括：
根据所捕获设定连续帧的激光数据形成当前子地图；
确定所述当前子地图与所选定至少一个候选子地图的相似性匹配得分；
从各所述相似性匹配得分中确定满足第一匹配筛选条件的目标匹配得分；
如果确定存在所述目标匹配得分，则将对应于所述目标匹配得分的候选子地图确定为所述当前子地图的闭环区域。
第二方面，本发明实施例还提供了一种闭环检测装置，包括：
子地图构建模块，用于根据所捕获设定连续帧的激光数据形成当前子地图；
相似得分确定模块，用于确定所述当前子地图与所选定至少一个候选子地图的相似性匹配得分；
目标得分确定模块，用于从各所述相似性匹配得分中确定满足第一匹配筛选条件的目标匹配得分；
闭环区域确定模块，用于当确定存在所述目标匹配得分时，将对应于所述目标匹配得分的候选子地图确定为所述当前子地图的闭环区域。
第三方面，本发明实施例也提供了一种终端，包括：激光雷达和系统里程计，还包括：
一个或多个处理器；
存储装置，用于存储一个或多个程序；
所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现本发明实施例第一方面提供的闭环检测方法。
第四方面，本发明实施例又提供了一种存储介质，其上存储有计算机程序，该程序被处理器执行时实现本发明实施例第一方面提供的闭环检测方法。
本发明实施例提供了一种闭环检测方法、装置、终端及存储介质，该技术方案，能够通过终端上激光雷达捕获设定连续帧的激光数据形成当前子地图，然后将当前子地图与历史子地图集中的候选子地图进行相似性匹配，其匹配过程为确定当前子地图与各候选子地图的相似性匹配得分，并从上述各相似性匹配得分中确定满足筛选匹配条件的目标匹配得分，最终确定目标匹配得分对应的候选子地图与当前子地图的相似度符合闭环检测的要求，由此实现了地图构建中的闭环检测。本技术方案从子地图与子地图进行相似度匹配的角度实现了大范围地图构建时的闭环检测，与现有技术相比，能够有效控制匹配候选集的数量，提高了闭环检测速度，保证了大范围地图构建过程中闭环检测的实时性，同时，本技术方案也有效降低了闭环检测的误检率，提高了闭环检测的准确度。
附图说明
图1是本发明实施例一提供的一种闭环检测方法的流程图；
图2a是本发明实施例二提供的一种闭环检测方法的流程图；
图2b是本发明实施例二提供的闭环检测方法中进行第一匹配得分确定的实现方法流程图；
图2c给出了一个候选子地图的二维查找表的示例图；
图3a是本发明实施例三提供的一种闭环检测方法的流程图；
图3b是本发明实施例三提供的闭环检测方法中进行候选信息确定的实现方法流程图；
图3c给出了基于二维查找表生成二维压缩表的实现示例图；
图3d给出了第一激光数据以栅格地图形式呈现的显示图；
图3e给出了第二激光数据以栅格地图形式呈现的显示图；
图3f给出了包含第一激光数据的第一子地图以栅格地图形式呈现的显示图；
图3g给出了包含第一激光数据的第二子地图以栅格地图形式呈现的显示图；
图4是本发明实施例四提供的一种闭环检测装置的结构示意图；
图5是本发明实施例五提供的一种终端的硬件结构示意图。
具体实施方式
下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部结构。
另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部内容。在更加详细地讨论示例性实施例之前应当提到的是，一些示例性实施例被描述成作为流程图描绘的处理或方法。虽然流程图将各项操作(或步骤)描述成顺序的处理，但是其中的许多操作可以被并行地、并发地或者同时实施。此外，各项操作的顺序可以被重新安排。当其操作完成时所述处理可以被终止，但是还可以具有未包括在附图中的附加步骤。所述处理可以对应于方法、函数、规程、子例程、子程序等等。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或者操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。例如，第一匹配得分和第二匹配得分中的“第一”和“第二”用来区分得分计算操作中两个不同的匹配得分，第一投影得分和第二投影得分中的“第一”和“第二”用来区分得分计算操作中两个不同的投影得分，又如，第一设定平移和/或旋转规则和第二设定平移和/或旋转规则中的“第一”和“第二”用来区分两种不同的平移和/或旋转规则等等。
实施例一
图1是本发明实施例一提供的一种闭环检测方法的流程图，本实施例可适用于在地图构建中将终端当前所在位置与历史位置进行闭环检测的情况，该方法可以由本发明实施例提供的闭环检测装置来执行，该装置可以由硬件和/或软件实现，并一般集成在终端中。
需要说明的是，所述终端具体可指机器人及智能服务设备等能够自行获得周围环境信息进行地图构建的电子终端设备。可以理解的是，基于上述终端进行地图构建时，可以基于本发明实施例提供的闭环检测方法准确快速的确定终端当前所在位置的闭环区域。
如图1所述，本发明实施例一提供的一种闭环检测方法，包括如下步骤：
s101、根据所捕获设定连续帧的激光数据形成当前子地图。
在本实施例中，对于进行地图构建的终端而言，其上往往设置有激光雷达，所述激光雷达具体可以通过发射激光束探测障碍物到终端的距离信息，由此形成激光数据，一般地，激光雷达一帧为单位捕获激光数据在单位帧的时间长度内，可以控制激光雷达旋转，由此获得不同旋转角度下发射激光束返回的距离信息，最终获得一帧包含多份距离信息值的激光数据。示例性地，一帧激光数据的数学形式可表示为：z＝{z1`,z2`,z3`,...,zi`...,zm`}，其中，z表示一帧激光数据，zi`表示激光数据在单位帧内获取的第i份距离信息值，m表示该帧激光数据中所包含距离信息的总数量。
需要说明的是，在地图构建过程中，可以将激光雷达捕获的一帧激光数据转换到所构建的地图中，以通过激光数据确定地图中障碍物的位置信息，由此可以认为激光雷达实时捕获的激光数据可用于地图的构建。本步骤可以通过设定连续帧的激光数据形成一个局部的小地图，以作为本实施例闭环检测所需的当前子地图。
一般地，设定连续帧激光数据形成的当前子地图可以通过栅格地图的形式呈现，也可以仅根据设定连续帧的激光数据所包含的距离信息表示，本步骤主要用来形成包含终端当前所在位置的局部地图，以通过图与图匹配的方式进行闭环检测。
示例性地，本实施例将当前子地图通过栅格地图进行呈现时，需要将设定连续帧的激光数据由距离信息转换转换为激光末端点在全局坐标系中的坐标信息，然后根据激光末端点的坐标信息在栅格地图中进行标识，由此可以形成栅格形式的当前子地图，其中，激光末端点可理解为激光雷达所捕获激光数据中每份距离信息值对应的障碍物所在位置点，在栅格地图中，激光末端点以特定形式进行标识，表示障碍物在地图中的位置。
s102、确定当前子地图与所选定至少一个候选子地图的相似性匹配得分。
在本实施例中，确定当前子地图后，为确定终端当前访问的位置是否为已访问过的历史位置，需要将当前子地图与之前形成的历史子地图进行相似性匹配。所述候选子地图具体可理解为从历史子地图集中选定的用于和当前子地图进行相似性匹配的子地图。
可以理解的是，进行大范围地图构建时，在形成当前子地图之前可能已存在较多数量的历史子地图，如果将当前子地图与所有的历史子地图都进行相似性匹配，则需要耗费较多时间，影响闭环检测的实时性，由此，本步骤可以仅将当前子地图与选定的至少一个候选子地图进行相似性匹配，且本步骤可以确定出当前子地图与任一候选子地图进行相似性匹配时的相似性匹配得分，所述相似性匹配得分可以表征当前子地图与相应候选子地图的相似匹配情况。
在本实施例中，具体可以基于形成当前子地图的激光数据，以及形成各候选子地图的激光数据，通过设定的计算规则确定当前子地图与各候选子地图的相似性得分。示例性地，本实施例设定的计算规则可以是将构成当前子地图的激光数据在各候选子地图基于相应激光数据形成的二维局部地图中进行投影，通过投影所对应的得分值，确定当前子地图与各候选子地图的相似性匹配得分。
s103、从各相似性匹配得分中确定满足第一匹配筛选条件的目标匹配得分。
在本实施中，由于当前子地图与候选子地图的相似性得分越高，则可认为当前子地图与该候选子地图越相似，即，二者在地图中属于同一位置的可能性就越高，因此，本实施例设定的第一匹配筛选条件具体可用于筛选出得分较高的相似性匹配得分，并将筛选出的结果记为目标匹配得分。本实施例为筛选出与当前子地图相似度最高的子地图，优选设定仅有一个相似性匹配得分可作为目标匹配得分。
s104、如果确定存在目标匹配得分，则将对应于目标匹配得分的候选子地图确定为当前子地图的闭环区域。
本实施例基于上述s103确定存在目标匹配得分时，可以将目标匹配得分对应的候选子地图确定为与当前子地图相似度最高的历史子地图，当基于本实施例上述方法确定出目标匹配得分后，可认为当前构建的地图中存在当前子地图的闭环区域，该闭环区域就是所述目标匹配得分对应的候选子地图，当存在闭环区域时，可认为终端当前访问的位置为已访问过的位置，不再需要对该位置重复进行地图构建。
可以理解的是，本实施例的相似性匹配得分中也有可能不存在满足第一筛选条件的目标匹配得分，此时可认为终端当前访问的位置还没有访问过，即在以形成的历史子地图中不存在当前子地图的闭环区域。
需要说明的是，本实施例提供的闭环检测方法可相当于一个循环执行方法，只要终端处于运动状态，就可根据上述s101的操作形成实时的当前子地图并基于后续的操作进行闭环检测。
本发明实施例一提供的闭环检测方法，能够首先通过终端上激光雷达捕获设定连续帧的激光数据形成当前子地图，然后将当前子地图与历史子地图集中的候选子地图进行相似性匹配，其匹配过程为确定当前子地图与各候选子地图的相似性匹配得分，并从上述各相似性匹配得分中确定满足筛选匹配条件的目标匹配得分，最终确定目标匹配得分对应的候选子地图与当前子地图的相似度符合闭环检测的要求，由此实现了地图构建中的闭环检测。利用上述方法，能够从子地图与子地图进行相似度匹配的角度实现大范围地图构建时的闭环检测，与现有技术相比，能够有效控制匹配候选集的数量，提高了闭环检测速度，保证了大范围地图构建过程中闭环检测的实时性，同时，该方法也有效降低了闭环检测的误检率，提高了闭环检测的准确度。
实施例二
图2a是本发明实施例二提供的一种闭环检测方法的流程图，本实施例二在上述各实施例的基础上进行优化。在本实施例中，将确定所述当前子地图与所选定至少一个候选子地图的相似性匹配得分，进一步具体化为：获取所选定至少一个候选子地图的二维查找表，其中，各所述二维查找表预先基于相应候选子地图中设定连续帧激光数据的高斯分布概率密度构建；确定所述当前子地图在各所述二维查找表中的第一匹配得分；将各所述第一匹配得分确定为所述当前子地图与相应候选子地图的相似性匹配得分。
进一步的，本实施例还将从各所述相似性匹配得分中确定满足第一匹配筛选条件的目标匹配得分，进一步优化为：从各所述第一匹配得分中筛选大于或等于第一设定阈值的第一中间匹配得分，并将第一中间匹配得分中的最大值确定为目标匹配得分。
此外，本实施例还进一步将根据所捕获设定连续帧的激光数据形成当前子地图，具体优化为：确定基于终端上激光雷达连续捕获激光数据的帧数；如果所述帧数达到设定值，则根据所捕获各帧激光数据中的激光末端点形成当前子地图。
在上述优化的基础上，本实施例还优化增加了：将所述当前子地图作为新历史子地图添加至所述历史子地图集中，并构建及存储所述新历史子地图的二维查找表，其中，所述二维查找表的元素值为所述新历史子地图中设定连续帧激光数据的高斯分布概率密度值。
如图2a所示，本发明实施例二提供的一种闭环检测方法，具体包括如下步骤：
s201、确定基于终端上激光雷达连续捕获激光数据的帧数。
具体地，在终端运动过程中，可以通过其上的激光雷达实时捕获激光数据，为进行本实施例中图与图匹配确定闭环区域的操作，本步骤首先在形成最近一个历史子地图后累计激光雷达连续捕获激光数据的帧数。可以理解的是，本实施例中累计记录的帧数可以在确定当前子地图后进行归零操作，以用于下一个当前子地图的确定。
s202、如果帧数达到设定值，则根据所捕获各帧激光数据中的激光末端点形成当前子地图。
在本实施例中，当累计的帧数达到设定值时，可认为当前连续捕获的激光数据能够形成一个当前子地图，由此本步骤可根据所捕获各帧激光数据中激光末端点的全局坐标信息以栅格地图形式呈现当前子地图；或者，直接各帧激光数据中激光末端点所对应距离信息值的集合表示当前子地图。
s203、获取所选定至少一个候选子地图的二维查找表。
在本实施例中，所述候选子地图相当于从历史子地图集中选取的与当前子地图进行相似度匹配的子地图，需要说明的是，本实施例为历史子地图集中的任一历史子地图预先构建了相应的二维查找表，因此本步骤可以直接获取所选定至少一个候选子地图的二维查找表。其中，本实施例中历史子地图的二维查找表实际上相当于对该历史子地图的另一种栅格表征形式，可通过二维查找表将当前子地图与二维查找表所对应历史子地图(本实施例主要为候选子地图)进行相似性匹配。
在本实施例中，历史子地图集中各历史子地图对应的二维查找表实际可看作基于相应历史子地图的历史激光数据中各激光末端点构建。具体地，对于任一历史子地图而言，可以确定其对应的设定连续帧的历史激光数据，本实施例可以为历史子地图初始设定一个行列数足够大的空二维表，且可将该空二维表的左上角看做原点，并设定该空二维表中的一个单元格表示一个单位坐标；然后，可以将捕获任一帧历史激光数据时的激光雷达所处的位置作为原点，由此基于各帧历史激光数据的激光末端点对应的距离信息值，通过坐标关系转换将各激光末端点表示在该空二维表中，此时，各激光末端点所占用的单元格的元素值表示为1，其余单元格的元素值仍为0形成中间二维表；之后，采用高斯分布模型作为各激光末端点的噪声模型，并基于该高斯分布模型为中间二维表中的元素值进行高斯分布概率密度的计算，并将计算获取的各高斯分布概率密度值作为各单元格对应的新元素值，从而构建形成历史子地图的二维查找表。
需要说明的是，由于初始设定的空二维表的行列数足够大，本实施例可以在基于高斯分布概率密度值完善该空二维表后，仅保留元素值不存在0的行和列来形成最终的二维查找表。最终形成的二维查找表相当于对历史子地图的另一种表征，对二维查找表的构建相当于将历史子地图所包含连续帧激光数据以似然率区域的形式展现，该种形式能够更好的呈现历史子地图的视图效果。
s204、确定当前子地图在各二维查找表中的第一匹配得分。
在本实施例中，可以根据当前子地图所对应设定连续帧的激光数据的激光末端点，确定当前子地图在各二维查找表中的第一匹配得分。
具体地，图2b是本发明实施例二提供的闭环检测方法中进行第一匹配得分确定的实现方法流程图。如图2b所示，所述确定所述当前子地图在各所述二维查找表中的第一匹配得分，具体包括如下操作：
s2041、基于第一设定平移和/或旋转规则，在各二维查找表中对当前子地图中的激光末端点进行平移和/或旋转。
具体地，为获得当前子地图在各二维查找表中的第一匹配得分，本实施采用了对当前子地图所包括各激光末端点在各二维查找表中进行平移和/或旋转的方式实现，其中，所述第一设定平移和/或旋转规则相当于规定了当前子地图中激光末端点在各所述二维查找表中的具体平移和/或旋转顺序。
需要说明的是，本实施例中的平移、旋转以及平移加旋转相当于将当前子地图中的激光末端点投影到各二维查找表中的三种投影操作方式，且这三种投影操作均需要进行，本实施例中的平移相当于对激光末端点以当前设定的平移变量进行横向平移或纵向平移，其中，所设定的平移变量可以从(0,0)开始，然后以单位步长逐渐增大平移变量，且可以先进行横向平移再进行纵向平移的形式完成平移遍历，也可以先进行纵向平移再进行横向平移的形式完成平移遍历，或者二者混合进行的形式完成平移；本实施例中的旋转相当于对激光末端点以原点为中心进行设定角度增量旋转，本实施例优选设定了角度旋转的区间范围，可以是0°到180°之间的区间范围。
可以理解的是，本步骤可以优选的以并行方式将当前子地图的激光末端点通过上述投影操作方式分别投影在各候选子地图对应的二维查找表中，由此可大幅度减少匹配得分的计算时间。
s2042、获取当前子地图在各二维查找表中每次平移和/或旋转后的第一投影得分。
在本实施例中，对当前子地图中各激光末端点在各所述二维查找表进行一次平移和/或旋转，就相当于将各激光末端点以一种转换关系投影到各所述二维查找表中，进行一次投影后，各激光末端点在各所述二维查找表中将对应一个元素值，对所有激光末端点当前所对应的元素值进行求和计算，则可将元素值之和确定为当前子地图在各所述二维查找表中进行一次平移和/或旋转后的第一投影得分。
示例性地，图2c给出了一个候选子地图的二维查找表的示例图，如图2c所示，查找表21为其中一个候选子地图的二维查找表(仅为示例性描述)，假设当前子地图由2个激光末端点构成(仅为示例性描述)，且根据当前子地图对应的设定连续帧的激光数据，确定上述2个激光末端点相对激光雷达(捕获任一帧激光数据时的激光雷达，设定该激光雷达的位置为相对原点)的坐标信息(相对于上述相对原点的坐标值)分别为(1,2)和(2,3)；假设对上述激光末端点(1,2)和(2,3)在查找表21中进行了一次平移变量为(1,0)的平移投影，则可确定出上述激光末端点(1,2)和(2,3)经过平移后分别落在了查找表21的(2,2)和(3,3)这两个单元格中，且如图2c所示，查找表21中(2,2)和(3,3)这两个单元格的元素值分别为1.6和0.6，即相当于上述两个激光末端点此时对应的元素值分别为1.6和0.6，且可计算出两元素值之和为2.2，由此，可认为当前子地图在该查找表21中进行该次平移后的第一投影得分为2.2。
s2043、基于每次平移和/或旋转后的第一投影得分，确定当前子地图在各二维查找表中的最高第一投影得分。
本实施例中，基于上述操作可以确定出每次对当前子地图中各激光末端点在各二维查找表中进行平移和/或旋转后对应的第一投影得分，本步骤可以从各二维查找表对应的多个第一投影得分中确定出相应的最高第一投影得分。
s2044、将各最高第一投影得分确定为当前子地图在相应二维查找表中的第一匹配得分。
本步骤可以将从各二维查找表确定出的最高第一投影得分记为当前子地图在相应二维查找表中的第一匹配得分。
s205、将各第一匹配得分确定为当前子地图与相应候选子地图的相似性匹配得分。
在本实施例中，基于上述s204确定出当前子地图在各二维查找表中的第一匹配得分后，可以确定各第一匹配得分对应的二维查找表，同时也可以确定各二维查找表对应的候选子地图，由此可确定各第一匹配得分对应的候选子地图，并将各第一匹配得分作为相应候选子地图与当前子地图相似性匹配时的相似性匹配得分，从而实现了当前子地图与各候选子地图的相似性匹配。
s206、从各第一匹配得分中筛选大于或等于第一设定阈值的第一中间匹配得分，并将第一中间匹配得分中的最大值确定为目标匹配得分。
在本实施例中，相当于将第一匹配筛选条件设定为大于或等于第一设定阈值，本实施例中所述第一设定阈值可以人为设定，基于本步骤的操作，可以判定上述确定的各第一匹配得分中是否存在大于或等于第一设定阈值的第一中间匹配得分，如果存在，则需要进一步确定出第一中间匹配得分中的最大值，并将该最大值确定为符合闭环要求的目标匹配得分。
可以理解的是，如果上述确定的各第一匹配得分均小于第一设定阈值，则可认为不存在目标匹配得分，此时可认为已形成的历史子地图集中不存在当前子地图的闭环区域，即，可认为终端当前所访问的位置在之前并没有访问过，由此可将当前子地图构建到全局地图中。
s207、如果确定存在目标匹配得分，则将对应于目标匹配得分的候选子地图确定为当前子地图的闭环区域。
在本实施例中，基于s206确定出目标匹配得分后，可确定目标匹配得分对应的二维查找表，并由此确定所对应的候选子地图，最终将该目标匹配得分对应的候选子地图确定为当前子地图的闭环区域，此时，可认为终端当前所访问的位置在之前已经访问过，由此可对建立当前子地图与该候选子地图的约束关系，以便于实现全局地图构建中的地图优化。
s208、将当前子地图作为新历史子地图添加至所述历史子地图集中，并构建及存储新历史子地图的二维查找表。
可以理解的是，基于上述s201至s207的操作，可以通过图与图匹配的方式来对包含终端当前所在位置的当前子地图进行闭环检测，当终端一直处于运行状态时，对终端当前所在位置进行闭环检测的操作将不断进行，由此，本步骤可以在实现当前子地图的闭环检测后，将当前子地图作为一个新历史子地图添加到历史子地图集中，后续可以继续返回s201形成下一个待进行闭环检测的当前子地图。
此外，当前子地图作为新历史子地图添加到历史子地图集之后，还需要为新历史子地图进行二维查找表的构建，以用于后续新形成当前子地图的闭环检测。其中，所述二维查找表的元素值为所述新历史子地图中设定连续帧激光数据的高斯分布概率密度值。二维查找表的具体构建步骤已在上述s203之后进行描述，这里不再赘述。
需要说明的是，本实施例提供的一种闭环检测方法，其核心思想在于通过子地图与子地图的相似度匹配进行闭环检测。采用该方法进行闭环检测时，首先通过构建的子地图可以很好地去除连续帧激光数据间的冗余信息，由此使得进行图与图相似性匹配时所需的计算量明显小于现有的进行帧与帧或者帧与图的相似性匹配时的计算量，从而达到了减少相似性匹配的计算量，提高相似性匹配效率的目的。此外，由于子地图中包含了连续帧的激光数据，以使子地图中包含的信息量远比单帧激光数据所包含的信息量丰富，从而使得相似性匹配时可比较的边角数量增多，进而可以很好地克服因为地图局部的高度相似而导致错误判断的问题。
本发明实施例二提供的一种闭环检测方法，具体化了当前子地图的确定操作，同时具体化了相似性匹配得分的确定操作，以及具体化了目标匹配得分的确定操作，此外还增加了地图集的更新操作。利用该方法，有效减少了减少相似性匹配的计算量，提高了相似性匹配效率，保证了大范围地图构建过程中闭环检测的实时性，同时也更好地克服了因为地图局部的高度相似而导致错误判断的问题，提高了闭环检测的准确度。
实施例三
图3a是本发明实施例三提供的一种闭环检测方法的流程图，本发明实施例以上述实施例为基础进行优化，在本实施例中，具体优化增加了：根据设定筛选规则从历史子地图集中筛选至少一个候选子地图。
如图3a所示，本发明实施例三提供的一种闭环检测方法，具体包括如下操作：
s301、根据所捕获设定连续帧的激光数据形成当前子地图。
s302、根据设定筛选规则从历史子地图集中筛选至少一个候选子地图。
本实施例为了对当前子地图进行闭环检测，一般考虑将当前子地图与历史子地图集中各历史子地图进行相似性匹配，然而，当历史子地图数量较多时，将当前子地图与所有的历史子地图都进行相似性匹配需要耗费较多匹配时间，影响闭环检测的实时性。
基于本步骤的操作，可以从历史子地图集中筛选出更有可能与当前子地图形成闭环区域的候选子地图，从而可以仅考虑当前子地图与候选子地图的相似性匹配，由此减少相似性匹配的计算量，提高闭环检测的实时性。
本步骤中的设定筛选规则的设定思路可以是首先从历史子地图集中确定出可以与当前子地图进行粗略匹配的待匹配子地图，然后根据粗略匹配结果再从多个待匹配子地图中筛选出进行相似性匹配的候选子地图。
具体地，图3b是本发明实施例三提供的闭环检测方法中进行候选信息确定的实现方法流程图。如图3b所示，所述根据设定筛选规则从历史子地图集中筛选至少一个候选子地图，具体包括如下操作：
s3021、根据设定的定位筛选策略，从历史子地图集中筛选对应于当前子地图的至少两个待匹配子地图。
本步骤考虑从终端当前所在的估计位置出发进行待匹配子地图的筛选。
进一步地，所述根据设定的定位筛选策略，从所述历史子地图集中筛选对应于所述当前子地图的至少两个待匹配子地图，包括：基于终端中的系统里程计确定所述终端在所构建全局地图中的当前估计位置；获取所述历史子地图集中各历史子地图在所述全局地图中的历史位置；基于各所述历史位置及所述当前估计位置，确定各所述历史子地图到所述当前子地图的距离信息；将所述距离信息小于设定距离阈值的历史子地图确定为所述当前子地图的待匹配子地图。
在本实施例中，所述系统里程计相当于终端中的一个硬件装置，可以对终端当前的位置进行粗略估计，由此可以本实施例可以获取终端的当前估计位置，同时也可以获得历史子地图集中各历史子地图在全局地图中的历史位置，最终可以确定出距离终端小于设定距离阈值的待匹配子地图。
需要说明的是，由于实际环境中系统里程计存在误差，随着误差的累积机器人的估计位置越来越不可靠，无法有效缩小匹配候选集的范围，因此可通过选定的图优化方法，如通用图优化算法(generalgraphoptimization，g2o)，对终端的估计位置进行优化计算，以缩小终端当前估计位置与真实位置的误差距离，从而保证大范围地图中的闭环检测不会因数据累积而降低相似性匹配速度。
s3032、获取各待匹配子地图对应的二维压缩表。
本实施例在为历史子地图中各历史子地图构建二维查找表的同时也相应构建了二维压缩表，以通过二维压缩表实现当前子地图与待匹配子地图的粗略匹配。
其中，所述二维压缩表通过对各所述待匹配子地图所对应二维查找表的最大值池化计算生成。具体地，对于各历史子地图的二维查找表，可以采用最大池化的方式进行查找表的压缩优化生成对应的二维压缩表，可以理解的是，所生成的二维压缩表实际相当于对二维查找表进行降低分辨率的操作，首先考虑通过待匹配子地图的二维压缩表与当前子地图进行粗略匹配，可以有效提升闭环检测的整体相似性匹配效率。
本实施例遵循2次方的倍数来压缩历史子地图的二维查找表，假设d代表压缩的倍数，h和l表示二维查找表和二维压缩表的单元格数，那么二维查找表的单元格式与二维压缩表的单元格数在不同的压缩倍数下存在下述关系，即，l＝h÷2d；优选地，本实施例为减少二维压缩表造成的精度损失，采用最大值池化方法确定二维压缩表中各单元格的元素值，即在要合并的多个单元格中选取元素最大值，将元素最大值作为待形成二维压缩表的单元格中的元素值。
本实施例可优选设定d的取值为2，示例性的，图3c给出了基于二维查找表生成二维压缩表的实现示例图，如图3c所示，查找表31表示二维查找表，压缩表32表示压缩合并后的二维压缩表。可以发现，压缩表32中的第一个元素值为查找表31中四个单元格中的元素最大值。
s3023、确定当前子地图在各二维压缩表中的第二匹配得分，并从各第二匹配得分中确定至少一个满足第二筛选匹配条件的候选匹配得分。
本步骤相当于基于待匹配子地图的二维压缩表与当前子地图进行的粗匹配，本步骤同样可根据当前子地图所对应设定连续帧的激光数据的激光末端点，确定当前子地图在各二维压缩表中的第二匹配得分。
具体地，所述确定所述当前子地图在各所述二维压缩表中的第二匹配得分，并从各所述第二匹配得分中确定至少一个满足第二筛选匹配条件的候选匹配得分，包括：基于第二设定平移和/或旋转规则，在各所述二维压缩表中对所述当前子地图中的激光末端点进行平移和/或旋转；获取所述当前子地图在各所述二维压缩表中每次平移和/或旋转后的第二投影得分；基于每次平移和/或旋转后的第二投影得分，确定所述当前子地图在各所述二维压缩表中的最高第二投影得分；将各所述最高第二投影得分确定为所述当前子地图在相应二维压缩表中的第二匹配得分；当各所述第二匹配得分中存在至少一个大于或等于第二设定阈值的第二中间匹配得分时，将各所述第二中间匹配得分确定为候选匹配得分；否则，将各所述第二匹配得分中的最高匹配得分确定为候选匹配得分。
本实施例中，确定当前子地图在各二维压缩表中的第二匹配得分的具体实现操作与上述确定当前子地图在各二维查找表中的第一匹配得分所采用的方式相同，上述确定方式已在s204的步骤后详述，这里不再赘述。
需要注意的是，本步骤可以通过两种方式确定候选匹配得分，即，可以在第二匹配得分中存在大于或等于第二设定阈值的第二中间匹配得分时，直接将确定出的第二中间匹配得分看做候选匹配得分；同时，也可以在第二匹配得分均小于第二设定阈值时，将第二匹配得分中的最高匹配得分看做候选匹配得分。可以发现，本步骤至少可以确定出一个候选匹配得分。
s3024、将各候选匹配得分对应的待匹配子地图确定为候选子地图。
本步骤可以确定各候选匹配得分对应的二维压缩表，同时可以确定这些二维压缩表对应的待匹配子地图，由此可以将筛选出的对应于各候选匹配得分的待匹配子地图确定为候选子地图。
s303、确定当前子地图与所选定至少一个候选子地图的相似性匹配得分。
s304、从各相似性匹配得分中确定满足第一匹配筛选条件的目标匹配得分。
s305、如果确定存在所述目标匹配得分，则将对应于所述目标匹配得分的候选子地图确定为所述当前子地图的闭环区域。
s306、将当前子地图作为新历史子地图添加至历史子地图集中，并构建及存储新历史子地图的二维查找表。
需要说明的是，本发明实施例提供的闭环检测方法，与传统的帧与帧相似性匹配的算法相比，不仅提高了闭环检测的计算速度，还有效提高了闭环检测的准确率。示例性的，图3d给出了第一激光数据以栅格地图形式呈现的显示图；图3e给出了第二激光数据以栅格地图形式呈现的显示图。此处采用了传统的帧与帧相似性匹配的算法对图3d和图3e中的两帧激光数据进行相似性匹配，可以发现，基于传统的帧与帧相似性匹配的算法进行闭环检测时，可以确定出图3d中的第一圆圈33中包括的激光数据与图3e中的第二圆圈34中包括的激光数据具有较高的相似性，由此便确定图3d中的第一激光数据表示的区域相当于图3e中第二激光数据所表示区域的闭环区域。
同时，图3f给出了包含第一激光数据的第一子地图以栅格地图形式呈现的显示图；图3g给出了包含第一激光数据的第二子地图以栅格地图形式呈现的显示图。此处采用了本实施例提供的闭环检测方法对图3f和图3g中的两个子地图进行相似性匹配，可以发现，基于本实施例的闭环检测方法进行闭环检测时，图3f中的第三圆圈35中包括的激光数据相当于图3d中的第一圆圈33中包括的激光数据，图3g中的第四圆圈36包括的激光数据相当于图3e中的第二圆圈34中包括的激光数据，此时，尽管第三圆圈35中包括的激光数据与图3g中的第四圆圈36包括的激光数据仍具有较高的相似性得分，但第一子地图中其他部分与第二子地图中其他部分的相似性得分却较低，进而使得第一子地图与第二子地图的整体匹配时的相似性匹配得分不能满足本实施例设定的第一筛选条件，由此不会将第一子地图确定为第二子地图的闭环区域。通过上述比较，可以发现本实施例提供的闭环检测后能够更准确的进行闭环检测。
本发明实施例三提供的一种闭环检测方法，具体增加了候选子地图的筛选操作，并给出了候选子地图的具体确定操作。利用该方法，能够筛选出最有可能与当前子地图构成闭环关系的候选子地图，从而很大程度减少了图与图匹配时的计算量，保证了大范围地图中的闭环检测不会因数据累积而降低相似性匹配速度，进而有效提高了闭环检测的实时性，同时也更好地克服了因为地图局部的高度相似而导致错误判断的问题，提高了闭环检测的准确度。
实施例四
图4是本发明实施例四提供的一种闭环检测装置的结构示意图，本实施例可适用于在地图构建中将终端当前所在位置与历史位置进行闭环检测的情况，该装置可以由硬件和/或软件实现，并一般集成在终端中。如图4所示，该装置包括：子地图构建模块41、相似得分确定模块42、目标得分确定模块43以及闭环区域确定模块44。
其中，子地图构建模块41，用于根据所捕获设定连续帧的激光数据形成当前子地图；
相似得分确定模块42，用于确定所述当前子地图与所选定至少一个候选子地图的相似性匹配得分；
目标得分确定模块43，用于从各所述相似性匹配得分中确定满足第一匹配筛选条件的目标匹配得分；
闭环区域确定模块44，用于当确定存在所述目标匹配得分时，将对应于所述目标匹配得分的候选子地图确定为所述当前子地图的闭环区域。
在本实施例中，该装置首先通过子地图构建模块41根据所捕获设定连续帧的激光数据形成当前子地图；然后通过相似得分确定模块42确定所述当前子地图与所选定至少一个候选子地图的相似性匹配得分；之后通过目标得分确定模块43从各所述相似性匹配得分中确定满足第一匹配筛选条件的目标匹配得分；最终通过闭环区域确定模块44当确定存在所述目标匹配得分时，将对应于所述目标匹配得分的候选子地图确定为所述当前子地图的闭环区域。
本发明实施例四提供的一种闭环检测装置，从子地图与子地图进行相似度匹配的角度实现了大范围地图构建时的闭环检测，与现有技术相比，能够有效控制匹配候选集的数量，提高了闭环检测速度，保证了大范围地图构建过程中闭环检测的实时性，同时，该装置也有效降低了闭环检测的误检率，提高了闭环检测的准确度。
进一步的，相似得分确定模块42，包括：
查找表获取单元，用于获取所选定至少一个候选子地图的二维查找表，其中，各所述二维查找表预先基于相应候选子地图中设定连续帧激光数据的高斯分布概率密度构建；
第一得分确定单元，用于确定所述当前子地图在各所述二维查找表中的第一匹配得分；
相似得分获取单元，用于将各所述第一匹配得分确定为所述当前子地图与相应候选子地图的相似性匹配得分。
在上述优化的基础上，所述第一得分确定单元，具体用于：
基于第一设定平移和/或旋转规则，在各所述二维查找表中对所述当前子地图中的激光末端点进行平移和/或旋转；获取所述当前子地图在各所述二维查找表中每次平移和/或旋转后的第一投影得分；基于每次平移和/或旋转后的第一投影得分，确定所述当前子地图在各所述二维查找表中的最高第一投影得分；将各所述最高第一投影得分确定为所述当前子地图在相应二维查找表中的第一匹配得分。
进一步地，目标得分确定模块43，具体用于：
从各所述第一匹配得分中筛选大于或等于第一设定阈值的第一中间匹配得分，并将第一中间匹配得分中的最大值确定为目标匹配得分。
进一步的，本实施例还优化包括了：候选信息确定模块45，
其中，候选信息确定模块45，用于根据设定筛选规则从历史子地图集中筛选至少一个候选子地图。
在上述优化的基础上，候选信息确定模块45，包括：
定位筛选单元，用于根据设定的定位筛选策略，从所述历史子地图集中筛选对应于所述当前子地图的至少两个待匹配子地图；
压缩表获取单元，用于获取各所述待匹配子地图对应的二维压缩表，其中，所述二维压缩表通过对各所述待匹配子地图所对应二维查找表的最大值池化计算生成；
候选得分确定单元，用于确定所述当前子地图在各所述二维压缩表中的第二匹配得分，并从各所述第二匹配得分中确定至少一个满足第二筛选匹配条件的候选匹配得分；
候选地图确定单元，用于将各所述候选匹配得分对应的待匹配子地图确定为候选子地图。
进一步地，所述定位筛选单元，具体用于：
基于终端中的系统里程计确定所述终端在所构建全局地图中的当前估计位置；获取所述历史子地图集中各历史子地图在所述全局地图中的历史位置；基于各所述历史位置及所述当前估计位置，确定各所述历史子地图到所述当前子地图的距离信息；将所述距离信息小于设定距离阈值的历史子地图确定为所述当前子地图的待匹配子地图。
同时，候选得分确定单元，具体用于：
基于第二设定平移和/或旋转规则，在各所述二维压缩表中对所述当前子地图中的激光末端点进行平移和/或旋转；获取所述当前子地图在各所述二维压缩表中每次平移和/或旋转后的第二投影得分；基于每次平移和/或旋转后的第二投影得分，确定所述当前子地图在各所述二维压缩表中的最高第二投影得分；将各所述最高第二投影得分确定为所述当前子地图在相应二维压缩表中的第二匹配得分；当各所述第二匹配得分中存在至少一个大于或等于第二设定阈值的第二中间匹配得分时，将各所述第二中间匹配得分确定为候选匹配得分；否则，将各所述第二匹配得分中的最高匹配得分确定为候选匹配得分。
进一步地，子地图构建模块41，具体用于：
确定基于终端上激光雷达连续捕获激光数据的帧数；如果所述帧数达到设定值，则根据所捕获各帧激光数据中的激光末端点形成当前子地图。
在上述优化的基础上，该装置还包括：
地图集更新模块46，用于将所述当前子地图作为新历史子地图添加至所述历史子地图集中，并构建及存储所述新历史子地图的二维查找表，其中，所述二维查找表的元素值为所述新历史子地图中设定连续帧激光数据的高斯分布概率密度值。
上述产品可执行本发明任意实施例所提供的方法，具备执行方法相应的功能模块和有益效果。
实施例五
图5是本发明实施例五提供的一种终端的硬件结构示意图。如图5所示，该终端包括：激光雷达51和系统里程计52，还包括：一个或多个处理器53，图5中以一个处理器53为例；存储装置54；所述终端还可以包括：输入装置55和输出装置56。
所述终端中的激光雷达51、系统里程计52、处理器53、存储装置54、输入装置55和输出装置56可以通过总线或者其他方式连接，图5中以通过总线连接为例。
存储装置54作为一种非暂态计算机可读存储介质，可用于存储软件程序、计算机可执行程序以及模块，如本发明实施例中的闭环检测方法对应的程序指令/模块(例如，附图4所示的子地图构建模块41、相似得分确定模块42、目标得分确定模块43以及闭环区域确定模块44，还包括：候选信息确定模块45以及地图集更新模块46)。处理器53通过运行存储在存储装置54中的软件程序、指令以及模块，从而执行终端的各种功能应用以及数据处理，即实现上述方法实施例的闭环检测方法。
存储装置54可以包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需要的应用程序；存储数据区可存储根据终端的使用所创建的数据等。此外，存储装置54可以包括高速随机存取存储器，还可以包括非暂态性存储器，例如至少一个磁盘存储器件、闪存器件、或其他非暂态性固态存储器件。在一些实施例中，存储装置54可选包括相对于处理器53远程设置的存储器，这些远程存储器可以通过网络连接至终端设备。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
并且，当上述终端所包括一个或者多个程序被所述一个或者多个处理器53执行时，程序进行如下操作：根据所捕获设定连续帧的激光数据形成当前子地图；确定所述当前子地图与所选定至少一个候选子地图的相似性匹配得分；从各所述相似性匹配得分中确定满足第一匹配筛选条件的目标匹配得分；如果确定存在所述目标匹配得分，则将对应于所述目标匹配得分的候选子地图确定为所述当前子地图的闭环区域。
此外，输入装置55可用于接收输入的数字或字符信息，以及产生与终端的用户设置以及功能控制有关的键信号输入。输出装置56可包括显示屏等显示设备。
此外，本发明实施例还提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现本发明实施例一或实施例二提供闭环检测方法，该方法包括：根据所捕获设定连续帧的激光数据形成当前子地图；确定所述当前子地图与所选定至少一个候选子地图的相似性匹配得分；从各所述相似性匹配得分中确定满足第一匹配筛选条件的目标匹配得分；如果确定存在所述目标匹配得分，则将对应于所述目标匹配得分的候选子地图确定为所述当前子地图的闭环区域。
通过以上关于实施方式的描述，所属领域的技术人员可以清楚地了解到，本发明可借助软件及必需的通用硬件来实现，当然也可以通过硬件实现，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如计算机的软盘、只读存储器(read-onlymemory,rom)、随机存取存储器(randomaccessmemory,ram)、闪存(flash)、硬盘或光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述的方法。
注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。