CN112102400B

CN112102400B - 基于距离的闭环检测方法、装置、电子设备和存储介质

Info

Publication number: CN112102400B
Application number: CN202010967002.3A
Authority: CN
Inventors: 王珏; 尧冬冬
Original assignee: Shanghai Yunshen Intelligent Technology Co ltd
Current assignee: Shanghai Hongxing Cloud Computing Technology Co ltd
Priority date: 2020-09-15
Filing date: 2020-09-15
Publication date: 2022-08-02
Anticipated expiration: 2040-09-15
Also published as: CN112102400A

Abstract

本发明涉及激光定位领域，本发明提供一种基于距离的闭环检测方法、装置、电子设备和存储介质，其方法包括步骤：基于当前激光关键帧的位姿信息，根据预设距离确定搜索候选子图的距离窗口；获取历史子图数据集中位于所述距离窗口内的候选子图集；将所述当前激光关键帧与候选子图集中的子图进行匹配，以进行所述当前激光关键帧与所述子图的闭环检测。通过本方案基于关键帧位置主动挑选一定距离范围内的子图作为候选子图，再依据相应的匹配方法建立关联关系，依据相应的连续性有效匹配确定有效匹配并整体优化调整，降低匹配的总计算量，增强匹配的效率和精度。

Description

基于距离的闭环检测方法、装置、电子设备和存储介质

技术领域

本发明涉及激光定位领域，尤指一种基于距离的闭环检测方法、装置、电子设备和存储介质。

背景技术

现有技术的闭环检测方法，通常是采用基于特征信息的匹配方法和基于点云匹配的方法，由于对环境感知的激光数据提取的特征数据相对较少而且稳定可靠性相对欠缺，激光场景下主要采取点云匹配的方法。

通常的检测策略主要是采取暴力采样的方式或随机采样的方式进行，这种检测策略缺少一定的目的性和稳定性。同时，检测的样本数量大导致总的匹配消耗增大，导致实际过程中定位效率和精度降低。

发明内容

本发明的目的是提供一种基于距离的闭环检测方法、装置、电子设备和存储介质，实现稳定、准确、高效的闭环检测定位。通过本方案基于关键帧位置主动挑选一定距离范围内的子图作为候选子图，再依据相应的匹配方法建立关联关系，依据相应的连续性有效匹配确定有效匹配并整体优化调整，降低匹配的总计算量，增强匹配的效率和精度。

本发明提供的技术方案如下：

一种基于距离的闭环检测方法，包括步骤：

基于当前激光关键帧的位姿信息，根据预设距离确定搜索候选子图的距离窗口；

获取历史子图数据集中位于所述距离窗口内的候选子图集；

将所述当前激光关键帧与候选子图集中的子图进行匹配，以进行所述当前激光关键帧与所述子图的闭环检测。

进一步优选地，所述将所述当前激光关键帧与候选子图集中的子图进行匹配，具体包括步骤：

当所述当前激光关键帧与子图的匹配分数超过匹配阈值时，则所述当前激光关键帧与所述子图的匹配为有效匹配，将所述子图加入有效约束集；

分析建立的所述有效约束集，判断所述有效约束集是否存在连续的约束关系。

进一步优选地，所述分析建立的所述有效约束集，判断所述有效约束集是否存在连续的约束关系，具体包括步骤：

当连续的关键帧与当前子图存在有效约束，所述当前激光关键帧与连续的子图存在有效约束，则确定所述有效约束集存在连续的约束关系；

当所述当前激光关键帧与连续的子图不存在有效约束时，退出所述当前关键帧的闭环检测。

进一步优选地，在所述当连续的关键帧与当前子图存在有效约束，所述当前激光关键帧与连续的子图存在有效约束，则所述有效约束集存在连续的约束关系之后，具体包括步骤：

以所述当前激光关键帧与子图的位姿为节点，以及所述约束关系为边建立关联的图结构；

通过图优化算法对所述节点进行优化，得到优化后的节点位姿。

进一步优选地，在所述通过图优化算法对所述节点进行优化，得到优化后的节点位姿之后，包括步骤：

依据优化的节点位姿更新对应的节点和子图的位姿。

另一方面，本发明还提供一种基于距离的闭环检测装置，包括步骤：

确定模块，用于基于当前激光关键帧的位姿信息，根据预设距离确定搜索候选子图的距离窗口；

获取模块，用于获取历史子图数据集中位于所述距离窗口内的候选子图集；

检测模块，用于将所述当前激光关键帧与候选子图集中的子图进行匹配，以进行所述当前激光关键帧与所述子图的闭环检测。

进一步优选地，所述检测模块还用于：

进一步优选地，所述基于距离的闭环检测装置，还包括：

建立模块，用于以所述当前激光关键帧与子图的位姿为节点，以及所述约束关系为边建立关联的图结构；

又一方面，本发明还提供一种电子设备，包括处理器、存储器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器，用于执行所述存储器上所存放的计算机程序，实现所述基于距离的闭环检测方法所执行的操作。

再一方面，本发明还提供一种存储介质，所述存储介质中存储有至少一条指令，所述指令由处理器加载并执行以实现所述基于距离的闭环检测方法所执行的操作。

本发明提供的一种基于距离的闭环检测方法、装置、电子设备和存储介质至少具有以下有益效果：

1)通过本方案基于关键帧位置主动挑选一定距离范围内的子图作为候选子图，再依据相应的匹配方法建立关联关系，依据相应的连续性有效匹配确定有效匹配并整体优化调整，降低匹配的总计算量，增强匹配的效率和精度。

2)本发明实现了基于距离信息进行主动挑选候选节点进行闭环检测，加速了闭环检测的方法，基于设计的挑选策略有目的性的全子图检测，相比采样检测的方式，约束构建更加稳定可靠。

3)通过在激光帧的距离窗口进行主动的搜索方法，使得匹配的目的性更强。

4)在本发明中，对于匹配分数高的子图，进行连续性匹配确认再优化，避免了错误匹配导致的定位出错。

5)通过以关键帧与子图为节点，约束关系为边一起建立关联的图结构，采取图优化算法对整个节点进行优化，优化后的结果再对每个节点进行更新。可以降低闭环检测的算力，减少算法对硬件平台的依赖。

附图说明

下面将以明确易懂的方式，结合附图说明优选实施方式，对一种基于距离的闭环检测方法、装置和电子设备的上述特性、技术特征、优点及其实现方式予以进一步说明。

图1是本发明中一种基于距离的闭环检测方法的一个实施例的流程图；

图2是本发明中一种基于距离的闭环检测方法的另一个实施例的流程图；

图3是本发明中一种基于距离的闭环检测方法的又一个实施例的流程图；

图4是本发明中一种基于距离的闭环检测方法的再一个实施例的流程图；

图5是本发明中一种基于距离的闭环检测方法的再一个实施例的流程图；

图6是本发明中一种基于距离的闭环检测装置的一个实施例的结构示意图；

图7是本发明中一种电子设备的一个实施例的结构示意图。

具体实施方式

以下描述中，为了说明而不是为了限定，提出了诸如特定***结构、技术之类的具体细节，以便透彻理解本申请实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其他实施例中也可以实现本申请。在其他情况中，省略对众所周知的***、装置、电路以及方法的详细说明，以免不必要的细节妨碍本申请的描述。

应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”指示所述描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其他特征、整体、步骤、操作、元素、组件和/或集合的存在或添加。

为使图面简洁，各图中只示意性地表示出了与本发明相关的部分，它们并不代表其作为产品的实际结构。另外，以使图面简洁便于理解，在有些图中具有相同结构或功能的部件，仅示意性地绘示了其中的一个，或仅标出了其中的一个。在本文中，“一个”不仅表示“仅此一个”，也可以表示“多于一个”的情形。

还应当进一步理解，在本申请说明书和所附权利要求书中使用的术语“和/或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。

另外，在本申请的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

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

实施例一

本发明的一个实施例，如图1所示，一种基于距离的闭环检测方法，包括：

S100基于当前激光关键帧的位姿信息，根据预设距离确定搜索候选子图的距离窗口。

具体的，在本实施例中，关键帧是指一帧激光数据，预设距离是根据闭环检测的需求设定的距离，距离窗口具体为以当前激光关键帧的位姿为中心节点，以预设距离为半径的圆形区域，例如5m范围的圆形区域。

S200获取历史子图数据集中位于所述距离窗口内的候选子图集。

在本实施例中，子图是一定数量激光关键帧形成的局部地图，历史子图数据集是指建图过程存储的所有子图序列。

S300将所述当前激光关键帧与候选子图集中的子图进行匹配，以进行所述当前激光关键帧与所述子图的闭环检测。

在实际的基于地图的激光定位场景中，图优化框架下的定位与建图技术主要由关键帧(激光帧)与子图(局部环境的地图)以及关键帧与子图的位姿关联关系构成，其中高效且准确地找到关键帧与子图的关系是十分关键的。

通过本实施例的以预设距离和关键帧的位姿信息建立距离窗口，可以降低匹配的总计算量，同时也增强了匹配的效率和精度。

实施例二

需要说明的是，关于步骤S300中将所述当前激光关键帧与候选子图集中的子图进行匹配，具体包括步骤：

S301当所述当前激光关键帧与子图的匹配分数超过匹配阈值时，则所述当前激光关键帧与所述子图的匹配为有效匹配，将所述子图加入有效约束集。

具体的，当扫描匹配评分超过给定的阈值分数则认为是有效匹配，将子图和关键帧，以及子图在关键帧的表示一起组成一个约束，一系列的有效约束一起组成约束集即为有效约束集。

S302分析建立的所述有效约束集，判断所述有效约束集是否存在连续的约束关系。

在本实施例中，通过建立并分析有效约束集的方式，进行连续性匹配确认再优化，避免了错误匹配导致的定位出错。

实施例三

S303当连续的关键帧与当前子图存在有效约束，所述当前激光关键帧与连续的子图存在有效约束，则确定所述有效约束集存在连续的约束关系。

S304以所述当前激光关键帧与子图的位姿为节点，以及所述约束关系为边建立关联的图结构。

S305通过图优化算法对所述节点进行优化，得到优化后的节点位姿。

S306依据优化的节点位姿更新对应的节点和子图的位姿。

本发明实现了基于距离信息进行主动挑选候选节点进行闭环检测，加速了闭环检测的方法，基于设计的挑选策略有目的性的全子图检测，相比采样检测的方式，约束构建更加稳定可靠。

实施例四

S307当所述当前激光关键帧与连续的子图不存在有效约束时，退出所述当前关键帧的闭环检测。

实施例五

本发明的一个实施例，如图5所示，一种基于距离的闭环检测方法，包括：

第一步，基于当前激光关键帧的位姿信息，依据给定的距离确定子图搜索的窗口区域。

第二步，依据第一步生成的窗口区域，从历史子图数据集中挑选合适的子图作为候选子图集。

第三步，将当前激光关键帧与第二步生成的候选子图集中的所有子图进行匹配。

第四步，第三步的匹配中，如果匹配分数超过给定的阈值则为有效匹配，将其加入有效约束集。

第五步，分析第四步建立的有效约束集，如果从有效约束集中能够找到连续的约束关系，即连续的关键帧与当前子图存在有效约束，当前关键帧与连续的子图存在有效约束，则进入第六步，否则无有效闭环并推出当前关键帧的处理。

第六步，将有效的约束集构建相应的图结构，关键帧和子图的位姿为节点，约束表示的关键帧在子图里的表示作为边；对建立的图结构建立迭代优化处理，得到优化后的节点位姿。

第七步，依据第六步优化的节点位姿更新对应的节点和子图的位姿。

示例性的，如图5所示的实施例，需要说明的是：

1.关键帧：一帧激光数据；

2.关键帧距离窗口：关键帧一定距离范围区域，例如5m范围的圆形区域；

3.子图集：子图是一定数量激光关键帧形成的局部地图，建图过程存储的的所有子图序列；

4.候选子图：落在当前关键帧距离窗口范围内的子图；

5.扫描匹配：激光关键帧与地图进行匹配，描述激光帧在子图坐标系下的表示；

6.匹配有效的约束集：扫描匹配评分超过给定的阈值分数认为是有效匹配，将子图和关键帧，以及子图在关键帧的表示一起组成一个约束，一系列的有效约束一起组成约束集；

7.连续约束：连续的激光关键帧与当前子图有约束关系，连续的子图与当前关键帧有约束关系；

8.约束集优化：关键帧与子图为节点，约束关系为边一起建立关联的图结构，采取图优化算法对整个节点进行优化，优化后的结果再对每个节点进行更新。

通过本实施例降低了匹配的总计算量，增强了匹配的效率和精度，同时本实施例属于一种主动的搜索方法，匹配的目的性更强。在本实施例中，通过连续性匹配确认再优化，避免了错误匹配导致的定位出错，进而降低算力，减少算法对硬件平台的依赖。

另一方面，如图6所示，本发明还提供一种基于距离的闭环检测装置，包括步骤：

确定模块601，用于基于当前激光关键帧的位姿信息，根据预设距离确定搜索候选子图的距离窗口；

获取模块602，用于获取历史子图数据集中位于所述距离窗口内的候选子图集；

检测模块603，用于将所述当前激光关键帧与候选子图集中的子图进行匹配，以进行所述当前激光关键帧与所述子图的闭环检测。

此外，所述检测模块还用于：

需要说明的是，所述基于距离的闭环检测装置，还包括：

本发明的一个实施例，如图7所示，一种电子设备100，包括处理器110、存储器120，其中，存储器120，用于存放计算机程序121；处理器110，用于执行存储器120上所存放的计算机程序121，实现上述实施例中方法。

所述电子设备100可以为桌上型计算机、笔记本、掌上电脑、平板型计算机、手机、人机交互屏等设备。所述电子设备100可包括，但不仅限于处理器110、存储器120。本领域技术人员可以理解，图7仅仅是电子设备100的示例，并不构成对电子设备100的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，示例性的：电子设备100还可以包括输入/输出接口、显示设备、网络接入设备、通信总线、通信接口等。通信接口和通信总线，还可以包括输入/输出接口，其中，处理器110、存储器120、输入/输出接口和通信接口通过通信总线完成相互间的通信。该存储器120存储有计算机程序121，该处理器110用于执行存储器120上所存放的计算机程序121，实现上述实施例中的方法。

所述处理器110可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

所述存储器120可以是所述电子设备100的内部存储单元，示例性的：电子设备的硬盘或内存。所述存储器也可以是所述电子设备的外部存储设备，示例性的：所述电子设备上配备的插接式硬盘，智能存储卡(Smart Media Card，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)等。进一步地，所述存储器120还可以既包括所述电子设备100的内部存储单元也包括外部存储设备。所述存储器120用于存储所述计算机程序121以及所述电子设备100所需要的其他程序和数据。所述存储器还可以用于暂时地存储已经输出或者将要输出的数据。

通信总线是连接所描述的元素的电路并且在这些元素之间实现传输。示例性的，处理器110通过通信总线从其它元素接收到命令，解密接收到的命令，根据解密的命令执行计算或数据处理。存储器120可以包括程序模块，示例性的，内核(kernel)，中间件(middleware)，应用程序编程接口(Application Programming Interface，API)和应用。该程序模块可以是有软件、固件或硬件、或其中的至少两种组成。输入/输出接口转发用户通过输入/输出接口(示例性的，感应器、键盘、触摸屏)输入的命令或数据。通信接口将该电子设备100与其它网络设备、用户设备、网络进行连接。示例性的，通信接口可以通过有线或无线连接到网络以连接到外部其它的网络设备或用户设备。无线通信可以包括以下至少一种：无线保真(WiFi)，蓝牙(BT)，近距离无线通信技术(NFC)，全球卫星定位***(GPS)和蜂窝通信等等。有线通信可以包括以下至少一种：通用串行总线(USB)，高清晰度多媒体接口(HDMI)，异步传输标准接口(RS-232)等等。网络可以是电信网络和通信网络。通信网络可以为计算机网络、因特网、物联网、电话网络。电子设备100可以通过通信接口连接网络，电子设备100和其它网络设备通信所用的协议可以被应用、应用程序编程接口(API)、中间件、内核和通信接口至少一个支持。

在本申请所提供的实施例中，应该理解到，所揭露的装置/电子设备和方法，可以通过其他的方式实现。示例性的，以上所描述的装置/电子设备实施例仅仅是示意性的，示例性的，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，示例性的，多个单元或组件可以结合或者可以集成到另一个***，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性、机械或其他的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本申请各个实施例中的各功能单元可能集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序121发送指令给相关的硬件完成，所述的计算机程序121可存储于一存储介质中，该计算机程序121在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序121可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述存储介质可以包括：能够携带所述计算机程序121的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述存储介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，示例性的：在某些司法管辖区，根据立法和专利实践，计算机可读的存储介质不包括电载波信号和电信信号。所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各程序模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的程序模块完成，即将所述装置的内部结构划分成不同的程序单元或模块，以完成以上描述的全部或者部分功能。实施例中的各程序模块可以集成在一个处理单元中，也可是各个单元单独物理存在，也可以两个或两个以上单元集成在一个处理单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件程序单元的形式实现。另外，各程序模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详细描述或记载的部分，可以参见其他实施例的相关描述。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。

应当说明的是，上述实施例均可根据需要自由组合。以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims

1.一种基于距离的闭环检测方法，其特征在于，包括步骤：

获取历史子图数据集中位于所述距离窗口内的候选子图集；

将所述当前激光关键帧与候选子图集中的子图进行匹配，以进行所述当前激光关键帧与所述子图的闭环检测，具体包括：

分析建立的所述有效约束集，判断所述有效约束集是否存在连续的约束关系，包括：

当连续的激光关键帧与当前子图存在有效约束，所述当前激光关键帧与连续的子图存在有效约束，则确定所述有效约束集存在连续的约束关系，以所述当前激光关键帧与子图的位姿为节点，以及所述约束关系为边建立关联的图结构，对建立的图结构建立迭代优化处理，得到优化后的节点位姿，根据优化的节点位姿更新对应的节点和子图的位姿；

当所述有效约束集不存在连续的约束关系时，退出所述当前激光关键帧的闭环检测。

2.一种基于距离的闭环检测装置，其特征在于，包括步骤：

检测模块，用于将所述当前激光关键帧与候选子图集中的子图进行匹配，以进行所述当前激光关键帧与所述子图的闭环检测；

所述检测模块还用于：当所述当前激光关键帧与子图的匹配分数超过匹配阈值时，则所述当前激光关键帧与所述子图的匹配为有效匹配，将所述子图加入有效约束集；分析建立的所述有效约束集，判断所述有效约束集是否存在连续的约束关系，包括：

3.一种电子设备，其特征在于，包括处理器、存储器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器，用于执行所述存储器上所存放的计算机程序，实现如权利要求1所述基于距离的闭环检测方法所执行的操作。

4.一种存储介质，其特征在于，所述存储介质中存储有至少一条指令，所述指令由处理器加载并执行以实现如权利要求1所述基于距离的闭环检测方法所执行的操作。