CN114332233B - 一种激光slam回环检测方法和系统 - Google Patents

Abstract

本发明涉及一种激光SLAM回环检测方法和系统。该激光SLAM回环检测方法为将关键帧点云数据转化为BEV视觉图像，对该图像进行实例分割并剔除潜在动态物体后得到实例分割图像，并且，根据实例分割图像中的实例的编号、语义信息及几何中心坐标构建该关键帧对应的实例地图，然后利用该实例地图进行回环检测。本方法减少了数据运算量，优化了回环检测的策略，在保证回环检测准确率的同时能够提高运算速度，进而提高回环检测的效率。

Description

一种激光SLAM回环检测方法和系统

技术领域

本发明涉及回环检测技术领域，特别是涉及一种激光SLAM回环检测方法和系统。

背景技术

在基于激光雷达的同步定位与建图（Simultaneous localization and mapping，SLAM）过程中，当前帧位姿的约束是根据上一帧计算出来的，由于计算的位姿存在误差，在建图的过程中随着误差不断累积导致构建的地图出现偏差，因此需要通过回环检测判断机器人在建图过程中是否再次经过同一位置，若经过同一位置则进行回环矫正，以减小建图的偏差。目前回环检测主要采用三条路径：1、采用任意关键帧之间进行匹配的方法。该方法存在计算量大、效率低的缺点；2、采用融合gps位置的方法。该方法不适用于无GPS或GPS定位不准的条件；3、采用基于语义的方案，该方法准确率高，且无需提供位置信息。但该方法当前存在的问题在于：1）直接对点云进行实例分割计算量大；2）当前的检测模式大多采用针对所有关键帧的全部特征向量进行核验的方法，同样具有较大的计算量。此外，现有的回环检测方法大多忽略了动态物体对检测准确性的干扰。

发明内容

为解决现有技术存在的上述问题，本发明提供了一种激光SLAM回环检测方法和系统。

为实现上述目的，本发明提供了如下方案：

一种激光SLAM回环检测方法，包括：

将关键帧点云数据转化为BEV视觉图像；其中，所述关键帧点云为以预设频率获取的单帧点云；

对所述BEV视觉图像进行实例分割，并根据实例分割结果剔除潜在的动态物体后得到经过处理后的实例分割图像；

确定所述实例分割图像中的实例的几何中心坐标；

根据所述实例的几何中心坐标及实例的语义信息构建实例地图；

基于所述实例地图进行回环检测。

优选地，所述将关键帧点云数据转化为BEV视觉图像，具体包括：

以机器人所在位置为中心，提取场景中预设区域内的关键帧点云数据；

在水平方向上将预设区域划分为不同的网格，并计算每一网格上方点云第一高度、第二高度以及平均强度；

对所述第一高度、所述第二高度和所述平均强度分别进行归一化处理，得到每一网格对应像素的RGB三通道信息；

根据各个像素的坐标及其对应的RGB三通道信息构建BEV视觉图像。

优选地，所述对所述BEV视觉图像进行实例分割，并根据实例分割结果剔除潜在的动态物体后得到经过处理后的实例分割图像，具体包括：

采用darknet53作为backbone构建卷积神经网络得到实例分割模型；

采用标注样本集对所述实例分割模型进行训练；

将所述BEV视觉图像输入至训练好的所述实例分割模型并根据分割结果剔除潜在动态物体后得到所述实例分割图像。

优选地，所述确定所述实例分割图像中的实例的几何中心坐标，具体包括：

将所述所述实例分割图像转化为二值图；

利用Opencv库中的使用函数cvMoments确定所述二值图的矩信息；

使用函数cvGetSpatialMoment获得指定维的第一矩信息和指定维的第二矩信息；

根据所述二值图的矩信息、所述指定维的第一矩信息和所述指定维的第二矩信息确定所述实例的几何中心坐标。

优选地，所述根据所述实例的几何中心坐标及实例的语义信息构建实例地图，具体包括：

按照每一个实例的所述实例的几何中心坐标值采用冒泡排序的方式对各个实例进行编号；

根据实例的编号、语义信息和所述实例的几何中心坐标构建实例地图。

优选地，所述基于所述实例地图进行回环检测，具体包括：

将当前关键帧的所述实例地图与一定时间阈值前的历史关键帧实例地图进行对比，按照编号从小到大依次验证两幅实例地图所对应的同一编号的实例的语义信息匹配度和几何中心坐标差值是否在一定的阈值范围内，若实例的语义信息匹配度一致并且几何中心坐标差值在一定的阈值范围内，则认为产生回环，若验证过程中出现同一编号对应的实例语义信息不一致或对应的几何中心坐标差值超出一定的阈值范围，则认为未产生回环，然后对下一历史关键帧进行检测，直至检测成功。

根据本发明提供的具体实施例，本发明公开了以下技术效果：

本发明提供的激光SLAM回环检测方法，将关键帧点云数据转化为BEV视觉图像，并对该图像进行实例分割，然后根据语义剔除潜在动态物体后得到对应的实例分割图像，并且根据实例分割图像中各个实例的编号、语义信息和几何中心坐标构建实例地图并利用该实例地图进行回环检测。本方法减少了数据运算量，优化了回环检测的策略，在保证回环检测准确率的同时能够提高运算速度，进而提高回环检测的效率。

对应于上述提供的激光SLAM回环检测方法，本发明还提供了以下实施系统：

一种激光SLAM回环检测系统，包括：

图像转化模块，用于将关键帧点云数据转化为BEV视觉图像；其中，所述关键帧点云为以预设频率获取的单帧点云；

图像分割模块，用于对所述BEV视觉图像进行实例分割，并根据实例分割结果剔除潜在的动态物体后得到经过处理后的实例分割图像；

坐标确定模块，用于确定所述实例分割图像中的实例的几何中心坐标；

地图构建模块，用于根据所述实例的几何中心坐标及实例的语义信息构建实例地图；

回环检测模块，用于基于所述实例地图进行回环检测。

另一种激光SLAM回环检测系统，包括：

存储器，用于存储计算机软件程序；所述计算机软件程序用于执行上述提供的激光SLAM回环检测方法；

处理器，与所述存储器连接，用于执行所述计算机软件程序。

因本发明提供的激光SLAM回环检测系统实现的技术效果与上述提供的激光SLAM回环检测方法实现的技术效果相同，故在此不再进行赘述。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明提供的激光SLAM回环检测方法的流程图；

图2为本发明实施例提供的构建地图的流程框图；

图3为本发明提供的激光SLAM回环检测系统的结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的目的是提供一种激光SLAM回环检测方法和系统，以在保证回环检测准确性的同时，提高回环检测的效率。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

如图1和图2所示，本发明提供的一种激光SLAM回环检测方法，包括：

步骤100：将关键帧点云数据转化为BEV(Bird Eye View,鸟瞰图)视觉图像。在实施过程中，该步骤的实施步骤可以是：

首先，获取场景内激光雷达扫描的关键帧点云，以机器人所在位置为中心，提取50m×50m范围内的点云。其中，关键帧是指在SLAM过程中，不同于高频的建图过程，而是以一个较低的频率获取激光雷达发出的单帧点云，获取的点云被称为关键帧点云，该关键帧点云用于回环检测，例如激光雷达的频率为10HZ，如果每一帧点云都用来进行回环检测将会产生很大的计算量，通常采用以一定时间间隔或一定距离间隔的方式获取点云用于回环检测即可。

然后，以地球坐标系下正北方为BEV视角下的y轴方向，正东方向为x轴方向，取西南角为坐标原点，将该区域划分为0.3m×0.3m的网格，计算每一个网格上方所包含点云的最高点高度（即第一高度）、最低点高度（即第二高度）以及网格内点云的平均强度并进行归一化处理，作为该网格对应像素的三通道信息，通过这样的方式，就可以将3D点云转化为对应的BEV视角下的2D图像。

提取该网格对应上方的全部点云，作为一个新的点云，将所有点的强度相加取平均即可得到平均强度ave_intensity=∑intensity/N，然后对该提取出的点云进行降采样，遍历其中的全部点，提取其中Z值最大和最小的点作为Zmax和Zmin，上述过程对全部网格进行计算。

例如，取高度范围为0-10米，最高点的z轴坐标值，最低点的z轴坐标值和该范围内的强度值进行归一化处理后乘以255，利用公式表明：Zmax/10*255，Zmin/10*255，ave_intensity*255，将这三个值作为转化后图像的R,G,B（像素的三通道信息）。这样，根据像素的坐标及对应的R,G,B值，即可得到关键帧点云对应的BEV视觉图像。

步骤101：对BEV视觉图像进行实例分割，并根据实例分割结果剔除潜在的动态物体后得到经过处理后的实例分割图像。该步骤主要是对步骤1生成的2D图像进行像素级标注。采用darknet53作为backbone构建卷积神经网络，由于应用场景为slam，在BEV视角下几乎不存在实例之间的重叠，故可认为同一语义下互不接触的两部分为不同实例，并根据语义将潜在的动态实例剔除。进行图像实例分割时，也可以采用其他现有的图像分割方法。

因此，在实例分割的基础上，将同一语义下相邻的像素认为是同一实例，否则不是同一实例，即可实现slam场景下点云BEV视角图像的实例分割。将大量训练样本投入网络后即可获得实例分割模型，并利用该实例分割模型对关键帧点云的BEV图像进行实例分割，然后利用语义信息将潜在的动态物体剔除，获得最终的实例分割图像。

步骤102：确定实例分割图像中的实例的几何中心坐标。具体的，将分割好的图像转化为二值图，利用Opencv库中的使用函数cvMoments来计算二值图像的矩信息M00。使用函数cvGetSpatialMoment获得指定维（x，y）的矩信息M01（即第一矩信息）和M10（即第二矩信息），则对应实例的重心坐标为Xc=M10/M00，Yc=M01/M00，（Xc，Yc）记为实例的几何中心坐标。

步骤103：根据实例的几何中心坐标及实例的语义信息构建实例地图。具体的，首先按照每一个实例的坐标值采用冒泡排序的方式对各个实例进行编号，每一个编号的实例与其对应的语义信息、几何中心坐标共同构建为实例地图，该实例地图可充分反映此关键帧所包含的实例信息。

为了准确、快速的找出回环并且保存较少的信息，在计算出每一个分割好的实例的重心坐标（即实例的几何中心坐标）的前提下，根据计算好的重心坐标，计算其到原点的距离A=（X^2+Y^2）^(1/2)，并采用冒泡排序的方法将A从小到大的顺序排列，若A相同，则按照X值从小到大的原则进行排序，则可以获得各序列的实例及其对应的重心坐标。这样就可以在地图上将每一个编号对应的实例的重心位置标记出来，得到实例地图。

步骤104：基于实例地图进行回环检测。本方法的回环检测机制为逐步对比根据两个关键帧的点云所构建的实例地图，包括不同编号实例的语义、数量和位置。

回环检测的原理是当机器人到达一定位置时，其周围的场景与之前到达的场景相同，则认为机器人到达了曾经走过的位置，即产生了回环。

例如，回环检测的实施过程为：

在slam高频建图时，使用单独的一个线程以1Hz频率获取关键帧并将其转化为BEV视角的图像，利用训练好的模型得到分割好的图像，并保存关键帧的实例地图。

在回环检测的线程中，将当前获取的关键帧的实例地图与一定时间阈值前的历史关键帧的实例地图对比，按照编号从小到大依次验证两幅实例地图所对应的同一编号的实例的语义信息匹配度和几何中心坐标差值是否在一定的阈值范围内，若全部符合要求（即实例的语义信息匹配度一致并且几何中心坐标差值在一定的阈值范围内）则认为产生回环，若验证过程中出现同一编号对应实例语义信息不一致或几何中心坐标差值超出一定的阈值范围，则认为未产生回环，然后对下一历史关键帧进行检测，直至检测成功。其中，一定的阈值范围可以依据实际需要进行设定。

对应于上述提供的激光SLAM回环检测方法，本发明还提供了以下实施系统：

其中，如图3所示，一种激光SLAM回环检测系统包括：图像转化模块300、图像分割模块301、坐标确定模块302、地图构建模块303和回环检测模块304。

图像转化模块300用于将关键帧点云数据转化为BEV视觉图像。

图像分割模块301用于对所述BEV视觉图像进行实例分割，并根据实例分割结果剔除潜在的动态物体后得到经过处理后的实例分割图像。

坐标确定模块302用于确定实例分割图像中的实例的几何中心坐标。

地图构建模块303用于根据实例的几何中心坐标及实例语义信息构建实例地图。

回环检测模块304用于基于实例地图进行回环检测。

另一种激光SLAM回环检测系统包括：存储器和处理器。

存储器用于存储计算机软件程序。计算机软件程序用于执行上述提供的激光SLAM回环检测方法。

处理器与存储器连接，用于执行计算机软件程序。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (7)

1.一种激光SLAM回环检测方法，其特征在于，包括：

将关键帧点云数据转化为BEV视觉图像；其中，所述关键帧点云为以预设频率获取的单帧点云；

对所述BEV视觉图像进行实例分割，并根据实例分割结果剔除潜在的动态物体后得到经过处理后的实例分割图像；

确定所述实例分割图像中的实例的几何中心坐标；

根据所述实例的几何中心坐标及实例的语义信息构建实例地图；

基于所述实例地图进行回环检测；

所述将关键帧点云数据转化为BEV视觉图像，具体包括：

以机器人所在位置为中心，提取场景中预设区域内的关键帧点云数据；

在水平方向上将预设区域划分为不同的网格，并计算每一网格上方点云第一高度、第二高度以及平均强度；

对所述第一高度、所述第二高度和所述平均强度分别进行归一化处理，得到每一网格对应像素的RGB三通道信息；

根据各个像素的坐标及其对应的RGB三通道信息构建BEV视觉图像。

2.根据权利要求1所述的激光SLAM回环检测方法，其特征在于，所述对所述BEV视觉图像进行实例分割，并根据实例分割结果剔除潜在的动态物体后得到经过处理后的实例分割图像，具体包括：

采用darknet53作为backbone构建卷积神经网络得到实例分割模型；

采用标注样本集对所述实例分割模型进行训练；

将所述BEV视觉图像输入到训练好的所述实例分割模型并根据分割结果剔除潜在动态物体后得到所述实例分割图像。

3.根据权利要求1所述的激光SLAM回环检测方法，其特征在于，所述确定所述实例分割图像中的实例的几何中心坐标，具体包括：

将所述实例分割图像转化为二值图；

利用Opencv库中的使用函数cvMoments确定所述二值图的矩信息；

使用函数cvGetSpatialMoment获得指定维的第一矩信息和指定维的第二矩信息；

根据所述二值图的矩信息、所述指定维的第一矩信息和所述指定维的第二矩信息确定所述实例的几何中心坐标。

4.根据权利要求1所述的激光SLAM回环检测方法，其特征在于，所述根据所述实例的几何中心坐标及实例的语义信息构建实例地图，具体包括：

按照每一个实例的所述实例的几何中心坐标值采用冒泡排序的方式对各个实例进行编号；

根据实例的编号、语义信息和所述实例的几何中心坐标构建实例地图。

5.根据权利要求1所述的激光SLAM回环检测方法，其特征在于，所述基于所述实例地图进行回环检测，具体包括：

将当前帧的所述实例地图和历史实例地图进行对比，按照编号从小到大依次验证两幅实例地图所对应的同一编号的实例的语义信息匹配度和几何中心坐标差值是否在阈值范围内，若实例的语义信息匹配度一致并且几何中心坐标差值在阈值范围内，则认为产生回环，若验证过程中出现同一编号对应的实例语义信息不一致或几何中心坐标差值超出阈值范围，则认为未产生回环，然后对下一历史关键帧进行检测，直至检测成功。

6.一种激光SLAM回环检测系统，其特征在于，包括：

图像转化模块，用于将关键帧点云数据转化为BEV视觉图像；其中，所述关键帧点云为以预设频率获取的单帧点云；所述将关键帧点云数据转化为BEV视觉图像，具体包括：

以机器人所在位置为中心，提取场景中预设区域内的关键帧点云数据；

在水平方向上将预设区域划分为不同的网格，并计算每一网格上方点云第一高度、第二高度以及平均强度；

对所述第一高度、所述第二高度和所述平均强度分别进行归一化处理，得到每一网格对应像素的RGB三通道信息；

根据各个像素的坐标及其对应的RGB三通道信息构建BEV视觉图像；

图像分割模块，用于对所述BEV视觉图像进行实例分割，并根据实例分割结果剔除潜在的动态物体后得到经过处理后的实例分割图像；

坐标确定模块，用于确定所述实例分割图像中的实例的几何中心坐标；

地图构建模块，用于根据所述实例的几何中心坐标及实例的语义信息构建实例地图；

回环检测模块，用于基于所述实例地图进行回环检测。

7.一种激光SLAM回环检测系统，其特征在于，包括：

存储器，用于存储计算机软件程序；所述计算机软件程序用于执行如权利要求1-5任意一项所述的激光SLAM回环检测方法；

处理器，与所述存储器连接，用于执行所述计算机软件程序。

Images