CN111476305A - Slam闭环检测方法、装置、介质和设备 - Google Patents

Abstract

本公开涉及一种SLAM闭环检测方法、装置、介质和设备，属于定位技术领域，能够提升闭环检测的速度和准确性，确保定位准确。一种SLAM闭环检测方法，包括：基于当前帧中的当前位置数据，从关键帧数据库中寻找闭环候选帧，其中每个所述关键帧中至少包括时间戳数据、位置数据、姿态角数据、高度数据和点云数据；从所述闭环候选帧中剔除所述姿态角数据与所述当前帧中的当前姿态角数据不匹配的闭环候选帧，并从所述闭环候选帧中剔除所述高度数据与所述当前帧中的当前高度数据不匹配的闭环候选帧；以及将剔除后的剩余闭环候选帧中的点云数据与所述当前帧中的当前点云数据进行匹配，确定是否闭环检测成功。

Description

SLAM闭环检测方法、装置、介质和设备

技术领域

本公开涉及定位技术领域，具体地，涉及一种SLAM闭环检测方法、装置、介质和设备。

背景技术

在即时定位与地图构建(Simultaneous Localization and Mapping，SLAM)领域，闭环检测技术是最关键、最基础的技术之一。然而，目前的闭环检测技术仍然存在着闭环检测准确度不高的问题。

发明内容

本公开的目的是提供一种SLAM闭环检测方法、装置、介质和设备，能够提升闭环检测的速度和准确性，确保定位准确。

根据本公开的第一实施例，提供一种SLAM闭环检测方法，包括：基于当前帧中的当前位置数据，从关键帧数据库中寻找闭环候选帧，其中每个所述关键帧中至少包括时间戳数据、位置数据、姿态角数据、高度数据和点云数据；从所述闭环候选帧中剔除所述姿态角数据与所述当前帧中的当前姿态角数据不匹配的闭环候选帧，并从所述闭环候选帧中剔除所述高度数据与所述当前帧中的当前高度数据不匹配的闭环候选帧；以及将剔除后的剩余闭环候选帧中的点云数据与所述当前帧中的当前点云数据进行匹配，确定是否闭环检测成功。

根据本公开的第二实施例，提供一种SLAM闭环检测装置，包括：寻找模块，用于基于当前帧中的当前位置数据，从关键帧数据库中寻找闭环候选帧，其中每个所述关键帧中至少包括时间戳数据、位置数据、姿态角数据、高度数据和点云数据；剔除模块，用于从所述闭环候选帧中剔除所述姿态角数据与所述当前帧中的当前姿态角数据不匹配的闭环候选帧，并从所述闭环候选帧中剔除所述高度数据与所述当前帧中的当前高度数据不匹配的闭环候选帧；以及匹配模块，用于将剔除后的剩余闭环候选帧中的点云数据与所述当前帧中的当前点云数据进行匹配，确定是否闭环检测成功。

根据本公开的第三实施例，提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现根据本公开第一实施例所述方法的步骤。

根据本公开的第四实施例，提供一种电子设备，包括：存储器，其上存储有计算机程序；处理器，用于执行所述存储器中的所述计算机程序，以实现根据本公开第一实施例所述方法的步骤。

通过采用上述技术方案，由于首先基于当前帧中的当前位置数据从关键帧数据库中寻找闭环候选帧，之后从闭环候选帧中剔除姿态角数据与当前帧中的当前姿态角数据不匹配的闭环候选帧并从闭环候选帧中剔除高度数据与当前帧中的当前高度数据不匹配的闭环候选帧，最后将剔除后的剩余闭环候选帧中的点云数据与当前帧中的当前点云数据进行匹配以确定是否闭环检测成功，因此，在闭环检测过程中融合了姿态数据、高度数据、点云数据，能够很好地处理相似场景(例如长走廊、有相似结构的不同楼层等)问题，大大提升了闭环检测的速度和准确性，确保了定位准确。

本公开的其他特征和优点将在随后的具体实施方式部分予以详细说明。

附图说明

附图是用来提供对本公开的进一步理解，并且构成说明书的一部分，与下面的具体实施方式一起用于解释本公开，但并不构成对本公开的限制。在附图中：

图1是根据本公开一种实施例的SLAM闭环检测方法的流程图。

图2是闭环候选帧的示意图。

图3是一栋高层居民楼的楼梯间示意图。

图4是根据本公开实施例的闭环检测方法与现有闭环检测方法的闭环检测准确度对比示意图。

图5是根据本公开一种实施例的SLAM闭环检测装置的示意框图。

图6是根据一示例性实施例示出的一种电子设备的框图。

具体实施方式

以下结合附图对本公开的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本公开，并不用于限制本公开。

图1是根据本公开一种实施例的SLAM闭环检测方法的流程图。如图1所示，该方法包括以下步骤S11至S13。

在步骤S11中，基于当前帧中的当前位置数据，从关键帧数据库中寻找闭环候选帧。

关键帧指的是角色或物体运动或变化中的关键动作所处的那一帧。在本公开中，如果当前帧相对于上一关键帧而言其运动超过了一定的距离或者旋转超过了一定的角度，则会把当前帧作为新的关键帧加入到关键帧数据库中。

在本公开中，无论是关键帧还是当前帧，每个帧中都至少包括时间戳数据、位置数据、姿态角数据、高度数据和点云数据等信息。

位置数据可以从例如激光里程计中获得。姿态数据可以从例如惯性测量单元(Inertial Measurement Unit，IMU)之类的姿态检测设备处获得。高度数据可以从诸如气压计之类的高度检测设备处获得。点云数据可以从诸如多线激光雷达、双目立体相机、结构光、Tof深度相机之类的深度传感器处获得。

时间戳由收到数据时的系统时间确定。

激光里程计可以用单位旋转矩阵(例如，3×3的旋转矩阵R)和平移向量(例如，3×1的平移向量T)的形式来表示。当然也可以使用三维旋转向量、单位四元数等形式表示。

姿态数据可以用单位旋转矩阵(例如，3×3的旋转矩阵R)表示。

高度数据可以利用气压计测量的气压计算得到。通常，高度每上升9米大气压就降低100帕，所以假设海平面的气压为P₀，机器人所处高度处的气压为P_t，则可以计算出机器人所处的高度H为：

H＝(P₀-P_t)*0.09 (1)

点云数据可以是对深度传感器测量到的原始点云数据进行降采样(例如，体素滤波)后的点云数据，以减小数据量。

在一种实施方式中，可以从关键帧数据库中寻找与当前帧中的当前位置的距离小于预设距离、且与当前帧中的当前时间戳的时间差超过预设时间差值的关键帧，作为闭环候选帧。

例如，假设当前帧中的当前位置用(x,y,z)表示，关键帧数据库中的某一关键帧中的位置用(x',y',z')表示。z轴的高度可以由高度数据进行约束，所以在寻找闭环候选帧的时候可以只考虑x轴和y轴的坐标。遍历关键帧数据库，计算当前帧中的当前位置和每个关键帧中的位置之间的距离d：

当d小于预设距离、且当前帧中的当前时间戳与某一关键帧中的时间戳相差超过预设时间差值时，则把该关键帧加入闭环候选帧。

图2是闭环候选帧的示意图。如图2所示，关键帧1、2和n都在闭环搜索区域内，也即关键帧1、2和n中的位置都与当前帧中的当前位置之间的距离小于d，但是关键帧n中的时间戳与当前帧中的当前时间戳之间的时间差小于预设时间差值而关键帧1和2中的时间戳与当前帧中的当前时间戳之间的时间差超过预设时间差值，所以只有关键帧1和2是闭环候选帧。

如果寻找到的闭环候选帧的数量大于0，则执行后续的步骤；反之，说明未检测到闭环，不执行后续的步骤。

在步骤S12中，从闭环候选帧中剔除姿态角数据与当前帧中的当前姿态角数据不匹配的闭环候选帧，并从闭环候选帧中剔除高度数据与当前帧中的当前高度数据不匹配的闭环候选帧。

本公开对基于姿态数据剔除闭环候选帧与基于高度数据剔除闭环候选帧的执行顺序不做限制。下面以先执行基于姿态数据剔除闭环候选帧、后执行基于高度数据剔除闭环候选帧为例进行描述。

首先，遍历步骤S11中寻找到的闭环候选帧，计算每个闭环候选帧中的状态角与当前帧中的当前姿态角之间的姿态角度偏差ΔR：

ΔR＝R′R^-1 (3)

其中，R和R'都是3×3的旋转矩阵，R表示当前帧中的姿态角度，R'表示闭环候选帧中的姿态角度。

ΔR是3×3的旋转矩阵，把它转为3×1的欧拉角

如果

的模长||Δθ||大于预设角度偏差阈值，则把该闭环候选帧剔除。

剔除姿态不匹配的闭环候选帧之后，如果剩下的闭环候选帧的数量大于0，则继续执行高度不匹配的闭环候选帧的剔除，否则说明未检测到闭环，不执行后续步骤。

接着，在剩下的闭环候选帧的数量大于0的情况下，遍历剔除了姿态不匹配的闭环候选帧之后的剩余闭环候选帧，计算每个剩余闭环候选帧中的高度与当前帧中的当前高度之间的高度偏差的绝对值ΔH：

ΔH＝|H′-H| (4)

其中，H是当前帧中的当前高度，H′是剩余闭环候选帧中的高度。

如果ΔH大于预设高度偏差阈值，则把该剩余闭环候选帧剔除。

剔除了高度不匹配的剩余闭环候选帧之后，如果剩下的闭环候选帧的数量大于0，则继续执行后续的步骤，否则说明未检测到闭环，不执行后续步骤。

在步骤S13中，将剔除后的剩余闭环候选帧中的点云数据与当前帧中的当前点云数据进行匹配。

经过步骤S12中的姿态不匹配和高度不匹配剔除处理之后，剩余的闭环候选帧都是有可能的闭环帧了。则，在步骤S13中，可以首先根据这些剩余闭环候选帧中的位置与当前帧中的当前位置之间的距离重新对这些剩余闭环候选帧进行排序，然后，优先使用距离近的剩余闭环候选帧中的点云与当前帧中的当前点云进行匹配，一旦匹配成功，则闭环检测成功；如果匹配失败，则继续使用距离次近的剩余闭环候选帧进行匹配。

而且，在步骤S13中，为了避免剩余闭环候选帧过多而导致程序卡死的问题，可以针对点云匹配次数设定最大次数限制，当匹配次数超过最大次数限制依然没有匹配成功时，则闭环检测失败，退出该步骤。

通过采用上述技术方案，由于首先基于当前帧中的当前位置数据从关键帧数据库中寻找闭环候选帧，之后从闭环候选帧中剔除姿态角数据与当前帧中的当前姿态角数据不匹配的闭环候选帧并从闭环候选帧中剔除高度数据与当前帧中的当前高度数据不匹配的闭环候选帧，最后将剔除后的剩余闭环候选帧中的点云数据与当前帧中的当前点云数据进行匹配，因此，在闭环检测过程中融合了姿态数据、高度数据、点云数据，能够很好地处理相似场景(例如长走廊、有相似结构的不同楼层等)问题，大大提升了闭环检测的速度和准确性，确保了定位准确。

以下以一栋高层居民楼的楼梯间为例，来说明根据本公开实施例的闭环检测方法与现有闭环检测方法的闭环检测准确度。高层居民楼的楼梯间结构如图3所示，层与层之间都是相似结构。建图路线为从1楼——>10楼——>1楼——>18楼——>1楼，图4是根据本公开实施例的闭环检测方法与现有闭环检测方法的闭环检测准确度对比示意图，图中的折线显示了真实高度随时间的变化。在该示例中，用于检测姿态数据的IMU和用于检测气压的气压计都安装在用于检测点云的激光雷达的顶部，IMU和激光雷达的坐标轴方向一样。本领域技术人员应当理解的是，这仅是一个具体的示例，在实际使用过程中，IMU、气压计、激光雷达等的安装位置、坐标轴方向等都可以根据实际情况进行调整。

从图4中可以看出，现有的闭环检测方法总共有两次闭环检测出现了错误，详细情况如下：

·在从1楼往18楼运动的过程中检测到了错误的闭环，在图4中用经过闭环点1的点划线标记，表示在3楼误认为1楼是闭环点。

·在从18楼返回1楼运动的过程中检测到了错误的闭环，在图4中用经过闭环点2的点划线标记，表示在12楼时误认为10楼是闭环点。

而根据本公开实施例的闭环检测方法，在上楼和下楼时都正确检测到了闭环。详细情况如下：

·在从1楼往18楼运动的过程中检测到了闭环，在图4中用经过闭环点1的虚线标记，表示在3楼时检测到曾经到达过该位置。

·在从18楼返回1楼运动的过程中检测到了闭环，在图4中用经过闭环点3的虚线标记，表示在10楼时检测到曾经到达过该位置。

可见，在相似场景下，根据本公开实施例的闭环检测方法的准确度优于现有的闭环检测方法的准确度。

图5是根据本公开一种实施例的SLAM闭环检测装置的示意框图。如图5所示，该装置包括：寻找模块51，用于基于当前帧中的当前位置数据，从关键帧数据库中寻找闭环候选帧，其中每个关键帧中至少包括时间戳数据、位置数据、姿态角数据、高度数据和点云数据；剔除模块52，用于从闭环候选帧中剔除姿态角数据与当前帧中的当前姿态角数据不匹配的闭环候选帧，并从闭环候选帧中剔除高度数据与当前帧中的当前高度数据不匹配的闭环候选帧；以及匹配模块53，用于将剔除后的剩余闭环候选帧中的点云数据与当前帧中的当前点云数据进行匹配，确定是否闭环检测成功。

通过采用上述技术方案，由于首先基于当前帧中的当前位置数据从关键帧数据库中寻找闭环候选帧，之后从闭环候选帧中剔除姿态角数据与当前帧中的当前姿态角数据不匹配的闭环候选帧并从闭环候选帧中剔除高度数据与当前帧中的当前高度数据不匹配的闭环候选帧，最后将剔除后的剩余闭环候选帧中的点云数据与当前帧中的当前点云数据进行匹配，因此，在闭环检测过程中融合了姿态数据、高度数据、点云数据，能够很好地处理相似场景(例如长走廊、有相似结构的不同楼层等)问题，大大提升了闭环检测的速度和准确性，确保了定位准确。

可选地，寻找模块51用于：从关键帧数据库中寻找与当前帧中的当前位置的距离小于预设距离且与当前帧中的当前时间戳的时间差超过预设时间差值的关键帧，作为闭环候选帧。

可选地，剔除模块52用于：计算每个闭环候选帧中的姿态角数据与当前帧中的当前姿态角数据之间的姿态角度偏差；若姿态角度偏差大于预设角度偏差阈值，则从闭环候选帧中剔除相应的闭环候选帧。

可选地，剔除模块52用于：计算每个闭环候选帧中的高度数据与当前帧中的当前高度数据之间的高度偏差；若高度偏差的绝对值大于预设高度偏差阈值，则从闭环候选帧中剔除相应的闭环候选帧。

可选地，匹配模块53用于：按照剩余闭环候选帧中的位置数据与当前帧中的当前位置数据之间距离由近及远的顺序，将剩余闭环候选帧中的点云数据与当前帧中的当前点云数据进行匹配。

关于上述实施例中的装置，其中各个模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不做详细阐述说明。

图6是根据一示例性实施例示出的一种电子设备700的框图。如图6所示，该电子设备700可以包括：处理器701，存储器702。该电子设备700还可以包括多媒体组件703，输入/输出(I/O)接口704，以及通信组件705中的一者或多者。

其中，处理器701用于控制该电子设备700的整体操作，以完成上述的SLAM闭环检测方法中的全部或部分步骤。存储器702用于存储各种类型的数据以支持在该电子设备700的操作，这些数据例如可以包括用于在该电子设备700上操作的任何应用程序或方法的指令，以及应用程序相关的数据，例如联系人数据、收发的消息、图片、音频、视频等等。该存储器702可以由任何类型的易失性或非易失性存储设备或者它们的组合实现，例如静态随机存取存储器(Static Random Access Memory，简称SRAM)，电可擦除可编程只读存储器(Electrically Erasable Programmable Read-Only Memory，简称EEPROM)，可擦除可编程只读存储器(Erasable Programmable Read-Only Memory，简称EPROM)，可编程只读存储器(Programmable Read-Only Memory，简称PROM)，只读存储器(Read-Only Memory，简称ROM)，磁存储器，快闪存储器，磁盘或光盘。多媒体组件703可以包括屏幕和音频组件。其中屏幕例如可以是触摸屏，音频组件用于输出和/或输入音频信号。例如，音频组件可以包括一个麦克风，麦克风用于接收外部音频信号。所接收的音频信号可以被进一步存储在存储器702或通过通信组件705发送。音频组件还包括至少一个扬声器，用于输出音频信号。I/O接口704为处理器701和其他接口模块之间提供接口，上述其他接口模块可以是键盘，鼠标，按钮等。这些按钮可以是虚拟按钮或者实体按钮。通信组件705用于该电子设备700与其他设备之间进行有线或无线通信。无线通信，例如Wi-Fi，蓝牙，近场通信(Near FieldCommunication，简称NFC)，2G、3G或4G，或它们中的一种或几种的组合，因此相应的该通信组件705可以包括：Wi-Fi模块，蓝牙模块，NFC模块。

在一示例性实施例中，电子设备700可以被一个或多个应用专用集成电路(Application Specific Integrated Circuit，简称ASIC)、数字信号处理器(DigitalSignal Processor，简称DSP)、数字信号处理设备(Digital Signal Processing Device，简称DSPD)、可编程逻辑器件(Programmable Logic Device，简称PLD)、现场可编程门阵列(Field Programmable Gate Array，简称FPGA)、控制器、微控制器、微处理器或其他电子元件实现，用于执行上述的SLAM闭环检测方法。

在另一示例性实施例中，还提供了一种包括程序指令的计算机可读存储介质，该程序指令被处理器执行时实现上述的闭环检测方法的步骤。例如，该计算机可读存储介质可以为上述包括程序指令的存储器702，上述程序指令可由电子设备700的处理器701执行以完成上述的SLAM闭环检测方法。

以上结合附图详细描述了本公开的优选实施方式，但是，本公开并不限于上述实施方式中的具体细节，在本公开的技术构思范围内，可以对本公开的技术方案进行多种简单变型，这些简单变型均属于本公开的保护范围。

另外需要说明的是，在上述具体实施方式中所描述的各个具体技术特征，在不矛盾的情况下，可以通过任何合适的方式进行组合。为了避免不必要的重复，本公开对各种可能的组合方式不再另行说明。

此外，本公开的各种不同的实施方式之间也可以进行任意组合，只要其不违背本公开的思想，其同样应当视为本公开所公开的内容。

Claims (10)

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

基于当前帧中的当前位置数据，从关键帧数据库中寻找闭环候选帧，其中每个所述关键帧中至少包括时间戳数据、位置数据、姿态角数据、高度数据和点云数据；

从所述闭环候选帧中剔除所述姿态角数据与所述当前帧中的当前姿态角数据不匹配的闭环候选帧，并从所述闭环候选帧中剔除所述高度数据与所述当前帧中的当前高度数据不匹配的闭环候选帧；以及

将剔除后的剩余闭环候选帧中的点云数据与所述当前帧中的当前点云数据进行匹配，确定是否闭环检测成功。

2.根据权利要求1所述的方法，其特征在于，所述基于当前帧中的当前位置数据，从关键帧数据库中寻找闭环候选帧，包括：

从所述关键帧数据库中寻找与所述当前帧中的当前位置的距离小于预设距离且与当前帧中的当前时间戳的时间差超过预设时间差值的关键帧，作为所述闭环候选帧。

3.根据权利要求1所述的方法，其特征在于，所述从所述闭环候选帧中剔除所述姿态角数据与所述当前帧中的当前姿态角数据不匹配的闭环候选帧，包括：

计算每个所述闭环候选帧中的姿态角数据与所述当前帧中的当前姿态角数据之间的姿态角度偏差；

若所述姿态角度偏差大于预设角度偏差阈值，则从所述闭环候选帧中剔除相应的闭环候选帧。

4.根据权利要求1所述的方法，其特征在于，所述从所述闭环候选帧中剔除所述高度数据与所述当前帧中的高度数据不匹配的闭环候选帧，包括：

计算每个所述闭环候选帧中的高度数据与所述当前帧中的当前高度数据之间的高度偏差；

若所述高度偏差的绝对值大于预设高度偏差阈值，则从所述闭环候选帧中剔除相应的闭环候选帧。

5.根据权利要求1所述的方法，其特征在于，所述将剔除后的剩余闭环候选帧中的点云数据与所述当前帧中的当前点云数据进行匹配，包括：

按照所述剩余闭环候选帧中的位置数据与所述当前帧中的当前位置数据之间距离由近及远的顺序，将所述剩余闭环候选帧中的点云数据与所述当前帧中的当前点云数据进行匹配。

6.一种SLAM闭环检测装置，其特征在于，包括：

寻找模块，用于基于当前帧中的当前位置数据，从关键帧数据库中寻找闭环候选帧，其中每个所述关键帧中至少包括时间戳数据、位置数据、姿态角数据、高度数据和点云数据；

剔除模块，用于从所述闭环候选帧中剔除所述姿态角数据与所述当前帧中的当前姿态角数据不匹配的闭环候选帧，并从所述闭环候选帧中剔除所述高度数据与所述当前帧中的当前高度数据不匹配的闭环候选帧；以及

匹配模块，用于将剔除后的剩余闭环候选帧中的点云数据与所述当前帧中的当前点云数据进行匹配，确定是否闭环检测成功。

7.根据权利要求6所述的装置，其特征在于，所述寻找模块用于：

从所述关键帧数据库中寻找与所述当前帧中的当前位置的距离小于预设距离且与当前帧中的当前时间戳的时间差超过预设时间差值的关键帧，作为所述闭环候选帧。

8.根据权利要求6所述的装置，其特征在于，所述匹配模块用于：

按照所述剩余闭环候选帧中的位置数据与所述当前帧中的当前位置数据之间距离由近及远的顺序，将所述剩余闭环候选帧中的点云数据与所述当前帧中的当前点云数据进行匹配。

9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现权利要求1-5中任一项所述方法的步骤。

10.一种电子设备，其特征在于，包括：

存储器，其上存储有计算机程序；

处理器，用于执行所述存储器中的所述计算机程序，以实现权利要求1-5中任一项所述方法的步骤。

Images