CN110887490B - 一种激光定位导航的关键帧选取方法、介质、终端和装置 - Google Patents

Abstract

本发明公开一种激光定位导航的关键帧选取方法、介质、终端和装置，根据建图过程中估计的机器人精确位置，将激光栅格化，从而计算所有被激光束访问的单元格熵的前后变化量之和，得到基于当前激光栅格化导致的地图熵的变化，根据地图熵的变化量确定当前激光是否为关键帧，从而在保证运算速度的情况下，提高了关键帧激光选取的有效性，且不容易产生地图特征信息的冗余，提升了激光SLAM的效率，在实际应用中具有重要意义。

Description

一种激光定位导航的关键帧选取方法、介质、终端和装置

【技术领域】

本发明涉及机器人领域，尤其涉及一种激光定位导航的关键帧选取方法、介质、终端和装置。

【背景技术】

SLAM(Simultaneous Localization and Mapping)，是移动机器人定位导航的核心技术,全称为同时定位与建图。SLAM问题可以描述为:机器人在未知环境中从一个未知位置开始移动,在移动过程中根据自身携带的传感器及构建的实时地图，通过slam技术进行实时位姿估计，同时不断更新与扩展地图，逐渐构建环境的完整地图。采用激光雷达实现的称为激光slam，采用摄像头实现的称为视觉slam。激光slam在地图更新的过程中不需要将每帧激光更新到地图中，只用选择包含更多环境特征与信息的激光更新到地图，从而减少了地图特征信息的冗余，同时提高了建图的速度，这些激光称为关键帧激光。选择关键帧的方法有多种，比如距离上一关键帧的帧数是否足够多，每隔固定帧数选择一个关键帧，这样效果不好，运动很慢的时候，就会选择大量相似的关键帧，冗余，而运动快的时候又丢失了很多重要的帧；距离最近关键帧的距离是否足够远，比如相对于前一关键帧的位移或者角度变化量超出一定阈值就新建一个关键帧，该方法如果对着同一个物体来回扫就会出现大量相似关键帧；根据关键帧包含新的特征点数量及比例选择关键帧，该方法计算比较复杂，运算效率低。

【发明内容】

本发明提供了一种激光定位导航的关键帧选取方法、介质、终端和装置，解决了以上所述的技术问题。

本发明解决上述技术问题的技术方案如下：一种激光定位导航的关键帧选取方法，包括以下步骤：

步骤1，构建当前环境信息对应的环境栅格地图，对所述环境栅格地图初始化；所述环境栅格地图中每个单元格至少包含变量visits和变量n，所述变量visits表示当前单元格被激光束访问的次数，变量n表示当前单元格被激光束击中的次数；

步骤2，获取当前帧的激光点云数据，并根据机器人的实时位姿将所述激光点云数据栅格化，根据栅格化结果计算所述环境栅格地图的地图熵变化量；

步骤3，判断所述地图熵变化量是否大于预设阈值，若是，将所述当前帧确定为关键帧，并将当前帧的激光点云数据更新到所述环境栅格地图，若否，则不对所述环境栅格地图进行更新。

在一个优选实施方式中，对所述环境栅格地图初始化后，环境栅格地图中每个单元格被激光束击中和访问的次数均为0。

在一个优选实施方式中，计算所述环境栅格地图的地图熵变化量，具体包括以下步骤：

S201，获取当前帧的激光束访问前每个单元格的初始visits值和初始n值，计算每个单元格的第一熵值；

S202，按照获取顺序遍历当前帧的所有激光束，根据每个激光束访问单元格的方式对每个单元格的visits值和n值进行更新，并获取遍历完成后每个单元格的当前visits值和当前n值，计算每个单元格的第二熵值；

S203，求取所述第二熵值和所述第一熵值的差作为每个单元格的熵变化量；

S204，累加所有单元格的熵变化量生成所述地图熵变化量。

在一个优选实施方式中，根据所述变量visits值和所述变量n值计算单元格熵值具体为：

当visits＝0时，entropyCell＝-log(0.5)；

当visits＝n或者n＝0时，entropyCell＝0；

当visits≠n、n≠0且visits≠0时，entropyCell＝-(x*log(x)+(1-x)*log(1-x))，x＝n/visits；

其中，visits表示单元格被激光束访问的次数，n表示单元格被激光束击中的次数，entropyCell表示单元格的熵值。

本发明实施例的第二方面提供了一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，实现以上所述的激光定位导航的关键帧选取方法。

本发明实施例的第三方面提供了一种激光定位导航的关键帧选取终端，包括所述的计算机可读存储介质和处理器，所述处理器执行所述计算机可读存储介质上的计算机程序时实现以上所述激光定位导航的关键帧选取方法的步骤。

本发明实施例的第四方面提供一种激光定位导航的关键帧选取装置，包括初始化模块、计算模块和判断模块，

所述初始化模块用于构建当前环境信息对应的环境栅格地图，对所述环境栅格地图初始化；所述环境栅格地图中每个单元格至少包含变量visits和变量n，所述变量visits表示当前单元格被激光束访问的次数，变量n表示当前单元格被激光束击中的次数；

所述计算模块用于获取当前帧的激光点云数据，并根据机器人的实时位姿将所述激光点云数据栅格化，根据栅格化结果计算所述环境栅格地图的地图熵变化量；

所述判断模块用于判断所述地图熵变化量是否大于预设阈值，若是，将所述当前帧确定为关键帧，并将当前帧的激光点云数据更新到所述环境栅格地图，若否，则不对所述环境栅格地图进行更新。

在一个优选实施方式中，对所述环境栅格地图初始化后，环境栅格地图中每个单元格被激光束击中和访问的次数均为0。

在一个优选实施方式中，所述计算模块具体包括：

第一计算单元，用于获取当前帧的激光束访问前每个单元格的初始visits值和初始n值，计算每个单元格的第一熵值；

第二计算单元，用于按照获取顺序遍历当前帧的所有激光束，根据每个激光束访问单元格的方式对每个单元格的visits值和n值进行更新，并获取遍历完成后每个单元格的当前visits值和当前n值，计算每个单元格的第二熵值；

第三计算单元，用于求取所述第二熵值和所述第一熵值的差作为每个单元格的熵变化量；

求和单元，用于累加所有单元格的熵变化量生成所述地图熵变化量。

在一个优选实施方式中，根据所述变量visits值和所述变量n值计算单元格熵值具体为：

当visits＝0时，entropyCell＝-log(0.5)；

当visits＝n或者n＝0时，entropyCell＝0；

当visits≠n、n≠0且visits≠0时，entropyCell＝-(x*log(x)+(1-x)*log(1-x))，x＝n/visits；

其中，visits表示单元格被激光束访问的次数，n表示单元格被激光束击中的次数，entropyCell表示单元格的熵值。

本发明提出的激光SLAM关键帧选取方法，根据建图过程中估计的机器人精确位置，将激光栅格化，从而计算所有被光束访问的单元格熵的前后变化量之和，得到基于当前激光栅格化导致的地图熵的变化，根据地图熵的变化量确定当前激光是否为关键帧，从而在保证运算速度的情况下，提高了关键帧激光选取的有效性，且不容易产生地图特征信息的冗余，提升了激光SLAM的效率，在实际应用中具有重要意义。

为使发明的上述目的、特征和优点能更明显易懂，下文特举本发明较佳实施例，并配合所附附图，作详细说明如下。

【附图说明】

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1是实施例1提供的激光定位导航的关键帧选取方法的流程示意图；

图2是实施例2提供的激光定位导航的关键帧选取装置的结构示意图；

图3是实施例3提供的激光定位导航的关键帧选取终端的结构示意图。

【具体实施方式】

为了使本发明的目的、技术方案和有益技术效果更加清晰明白，以下结合附图和具体实施方式，对本发明进行进一步详细说明。应当理解的是，本说明书中描述的具体实施方式仅仅是为了解释本发明，并不是为了限定本发明。

图1是本发明实施例1提供的一种激光定位导航的关键帧选取方法的流程示意图，如图1所示，包括以下步骤：

步骤1，构建当前环境信息对应的环境栅格地图，对所述环境栅格地图初始化；所述环境栅格地图中每个单元格至少包含变量visits和变量n，所述变量visits表示当前单元格被激光束访问的次数，即单元格被激光扫描过的次数；变量n表示当前单元格被激光束击中的次数，即被标记为障碍物的次数。对所述环境栅格地图初始化后，环境栅格地图中每个单元格被激光束击中和访问的次数均为0。

步骤2，获取当前帧的激光点云数据，并根据机器人的实时位姿将所述激光点云数据栅格化，即计算出激光束访问了地图上哪些栅格，根据栅格化结果计算所述环境栅格地图的地图熵变化量。具体来说，计算所述环境栅格地图的地图熵变化量，具体包括以下步骤：

S201，获取当前帧的激光束访问前每个单元格的初始visits值和初始n值，计算每个单元格的第一熵值；

S202，按照获取顺序遍历当前帧的所有激光束，根据每个激光束访问单元格的方式对每个单元格的visits值和n值进行更新，并获取遍历完成后每个单元格的当前visits值和当前n值，计算每个单元格的第二熵值；

S203，求取所述第二熵值和所述第一熵值的差作为每个单元格的熵变化量；

S204，累加所有单元格的熵变化量生成所述地图熵变化量。

优选的，根据所述变量visits值和所述变量n值计算单元格熵值具体为：

当visits＝0时，entropyCell＝-log(0.5)；

当visits＝n或者n＝0时，entropyCell＝0；

当visits≠n、n≠0且visits≠0时，entropyCell＝-(x*log(x)+(1-x)*log(1-x))，x＝n/visits；

其中，visits表示单元格被激光束访问的次数，n表示单元格被激光束击中的次数，entropyCell表示单元格的熵值。

然后执行步骤3，判断所述地图熵变化量是否大于预设阈值，若是，将所述当前帧确定为关键帧，并将当前帧的激光点云数据更新到所述环境栅格地图，若否，则不对所述环境栅格地图进行更新。

本实施例提出的激光SLAM关键帧选取方法，根据建图过程中估计的机器人精确位置，将激光栅格化，从而计算所有被光束访问的单元格熵的前后变化量之和，得到基于当前激光栅格化导致的地图熵的变化，根据地图熵的变化量确定当前激光是否为关键帧，从而在保证运算速度的情况下，提高了关键帧激光选取的有效性，且不容易产生地图特征信息的冗余，提升了激光SLAM的效率，在实际应用中具有重要意义。

应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。

本发明实施例还提供了一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，实现以上所述的激光定位导航的关键帧选取方法。

图2是本发明实施例2提供的一种激光定位导航的关键帧选取装置的结构示意图，如图2所示，包括初始化模块100、计算模块200和判断模块300，

所述初始化模块100用于构建当前环境信息对应的环境栅格地图，对所述环境栅格地图初始化；所述环境栅格地图中每个单元格至少包含变量visits和变量n，所述变量visits表示当前单元格被激光束访问的次数，变量n表示当前单元格被激光束击中的次数；

所述计算模块200用于获取当前帧的激光点云数据，并根据机器人的实时位姿将所述激光点云数据栅格化，根据栅格化结果计算所述环境栅格地图的地图熵变化量；

所述判断模块300用于判断所述地图熵变化量是否大于预设阈值，若是，将所述当前帧确定为关键帧，并将当前帧的激光点云数据更新到所述环境栅格地图，若否，则不对所述环境栅格地图进行更新。

在一个优选实施方式中，对所述环境栅格地图初始化后，环境栅格地图中每个单元格被激光束击中和访问的次数均为0。

在一个优选实施方式中，所述计算模块200具体包括：

第一计算单元201，用于获取当前帧的激光束访问前每个单元格的初始visits值和初始n值，计算每个单元格的第一熵值；

第二计算单元202，用于按照获取顺序遍历当前帧的所有激光束，根据每个激光束访问单元格的方式对每个单元格的visits值和n值进行更新，并获取遍历完成后每个单元格的当前visits值和当前n值，计算每个单元格的第二熵值；

第三计算单元203，用于求取所述第二熵值和所述第一熵值的差作为每个单元格的熵变化量；

求和单元204，用于累加所有单元格的熵变化量生成所述地图熵变化量。

在一个优选实施方式中，根据所述变量visits值和所述变量n值计算单元格熵值具体为：

当visits＝0时，entropyCell＝-log(0.5)；

当visits＝n或者n＝0时，entropyCell＝0；

当visits≠n、n≠0且visits≠0时，entropyCell＝-(x*log(x)+(1-x)*log(1-x))，x＝n/visits；

其中，visits表示单元格被激光束访问的次数，n表示单元格被激光束击中的次数，entropyCell表示单元格的熵值。

本发明实施例还提供了一种激光定位导航的关键帧选取终端，包括所述的计算机可读存储介质和处理器，所述处理器执行所述计算机可读存储介质上的计算机程序时实现以上所述激光定位导航的关键帧选取方法的步骤。图3是本发明实施例3提供的激光定位导航的关键帧选取终端的结构示意图，如图3所示，该实施例的激光定位导航的关键帧选取终端8包括：处理器80、可读存储介质81以及存储在所述可读存储介质81中并可在所述处理器80上运行的计算机程序82。所述处理器80执行所述计算机程序82时实现上述各个方法实施例中的步骤，例如图1所示的步骤1至步骤3。或者，所述处理器80执行所述计算机程序82时实现上述各装置实施例中各模块的功能，例如图2所示模块100至300的功能。

示例性的，所述计算机程序82可以被分割成一个或多个模块，所述一个或者多个模块被存储在所述可读存储介质81中，并由所述处理器80执行，以完成本发明。所述一个或多个模块可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序82在所述激光定位导航的关键帧选取终端8中的执行过程。

所述激光定位导航的关键帧选取终端8可包括，但不仅限于，处理器80、可读存储介质81。本领域技术人员可以理解，图3仅仅是激光定位导航的关键帧选取终端8的示例，并不构成对激光定位导航的关键帧选取终端8的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述激光定位导航的关键帧选取终端还可以包括电源管理模块、运算处理模块、输入输出设备、网络接入设备、总线等。

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

所述可读存储介质81可以是所述激光定位导航的关键帧选取终端8的内部存储单元，例如激光定位导航的关键帧选取终端8的硬盘或内存。所述可读存储介质81也可以是所述激光定位导航的关键帧选取终端8的外部存储设备，例如所述激光定位导航的关键帧选取终端8上配备的插接式硬盘，智能存储卡(Smart Media Card，SMC)，安全数字(SecureDigital，SD)卡，闪存卡(Flash Card)等。进一步地，所述可读存储介质81还可以既包括所述激光定位导航的关键帧选取终端8的内部存储单元也包括外部存储设备。所述可读存储介质81用于存储所述计算机程序以及所述激光定位导航的关键帧选取终端所需的其他程序和数据。所述可读存储介质81还可以用于暂时地存储已经输出或者将要输出的数据。

所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

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

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

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

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

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

本发明并不仅仅限于说明书和实施方式中所描述，因此对于熟悉领域的人员而言可容易地实现另外的优点和修改，故在不背离权利要求及等同范围所限定的一般概念的精神和范围的情况下，本发明并不限于特定的细节、代表性的设备和这里示出与描述的图示示例。

Claims (8)

1.一种激光定位导航的关键帧选取方法，其特征在于，包括以下步骤：

步骤1，构建当前环境信息对应的环境栅格地图，对所述环境栅格地图初始化；所述环境栅格地图中每个单元格至少包含变量visits和变量n，所述变量visits表示当前单元格被激光束访问的次数，变量n表示当前单元格被激光束击中的次数；

步骤2，获取当前帧的激光点云数据，并根据机器人的实时位姿将所述激光点云数据栅格化，根据栅格化结果计算所述环境栅格地图的地图熵变化量；

步骤3，判断所述地图熵变化量是否大于预设阈值，若是，将所述当前帧确定为关键帧，并将当前帧的激光点云数据更新到所述环境栅格地图，若否，则不对所述环境栅格地图进行更新；

其中，计算所述环境栅格地图的地图熵变化量，具体包括以下步骤：

S201，获取当前帧的激光束访问前每个单元格的初始visits值和初始n值，计算每个单元格的第一熵值；

S202，按照获取顺序遍历当前帧的所有激光束，根据每个激光束访问单元格的方式对每个单元格的visits值和n值进行更新，并获取遍历完成后每个单元格的当前visits值和当前n值，计算每个单元格的第二熵值；

S203，求取所述第二熵值和所述第一熵值的差作为每个单元格的熵变化量；

S204，累加所有单元格的熵变化量生成所述地图熵变化量。

2.根据权利要求1所述激光定位导航的关键帧选取方法，其特征在于，对所述环境栅格地图初始化后，环境栅格地图中每个单元格被激光束击中和访问的次数均为0。

3.根据权利要求2所述激光定位导航的关键帧选取方法，其特征在于，根据所述变量visits值和所述变量n值计算单元格熵值具体为：

当visits＝0时，entropyCell＝-log(0.5)；

当visits＝n或者n＝0时，entropyCell＝0；

当visits≠n、n≠0且visits≠0时，entropyCell＝-(x*log(x)+(1-x)*log(1-x))，x＝n/visits；

其中，visits表示单元格被激光束访问的次数，n表示单元格被激光束击中的次数，entropyCell表示单元格的熵值。

4.一种计算机可读存储介质，存储有计算机程序，其特征在于，所述计算机程序被处理器执行时，实现权利要求1-3任一项所述激光定位导航的关键帧选取方法。

5.一种激光定位导航的关键帧选取终端，其特征在于，包括权利要求4所述的计算机可读存储介质和处理器，所述处理器执行所述计算机可读存储介质上的计算机程序时实现如权利要求1-3任一项所述激光定位导航的关键帧选取方法的步骤。

6.一种激光定位导航的关键帧选取装置，其特征在于，包括初始化模块、计算模块和判断模块，

所述初始化模块用于构建当前环境信息对应的环境栅格地图，对所述环境栅格地图初始化；所述环境栅格地图中每个单元格至少包含变量visits和变量n，所述变量visits表示当前单元格被激光束访问的次数，变量n表示当前单元格被激光束击中的次数；

所述计算模块用于获取当前帧的激光点云数据，并根据机器人的实时位姿将所述激光点云数据栅格化，根据栅格化结果计算所述环境栅格地图的地图熵变化量；

所述判断模块用于判断所述地图熵变化量是否大于预设阈值，若是，将所述当前帧确定为关键帧，并将当前帧的激光点云数据更新到所述环境栅格地图，若否，则不对所述环境栅格地图进行更新；

其中，所述计算模块具体包括：

第一计算单元，用于获取当前帧的激光束访问前每个单元格的初始visits值和初始n值，计算每个单元格的第一熵值；

第二计算单元，用于按照获取顺序遍历当前帧的所有激光束，根据每个激光束访问单元格的方式对每个单元格的visits值和n值进行更新，并获取遍历完成后每个单元格的当前visits值和当前n值，计算每个单元格的第二熵值；

第三计算单元，用于求取所述第二熵值和所述第一熵值的差作为每个单元格的熵变化量；

求和单元，用于累加所有单元格的熵变化量生成所述地图熵变化量。

7.根据权利要求6所述激光定位导航的关键帧选取装置，其特征在于，对所述环境栅格地图初始化后，环境栅格地图中每个单元格被激光束击中和访问的次数均为0。

8.根据权利要求7所述激光定位导航的关键帧选取装置，其特征在于，根据所述变量visits值和所述变量n值计算单元格熵值具体为：

当visits＝0时，entropyCell＝-log(0.5)；

当visits＝n或者n＝0时，entropyCell＝0；

当visits≠n、n≠0且visits≠0时，entropyCell＝-(x*log(x)+(1-x)*log(1-x))，x＝n/visits；

其中，visits表示单元格被激光束访问的次数，n表示单元格被激光束击中的次数，entropyCell表示单元格的熵值。