CN113569982A - 基于二维激光雷达特征点模板匹配的位置识别方法及装置 - Google Patents

Abstract

本发明提供一种基于二维激光雷达特征点模板匹配的位置识别方法，包括，机器人进行建图，提取每个关键帧的特征点，并计算特征点的描述子；对各个关键帧的信息及每个关键帧中的所有描述子的数据进行保存，在保存的过程中进行字典的训练，计算每个关键帧的词袋库；机器人重定位时，提取定位时当前关键帧的特征点，并计算描述子，生成当前关键帧的词袋库；将当前关键帧的词袋库与建图时保存的所有关键帧的词袋库逐一计算相似度，获取最相似的关键帧；将当前关键帧的数据在最相似的关键帧对应的位姿处与地图进行匹配，进行重定位。本发明的有益效果是将遍历匹配地图上所有位置改变为对比当前帧的词袋库和建图帧的词袋库，极大程度上提高匹配速度。

Description

基于二维激光雷达特征点模板匹配的位置识别方法及装置

技术领域

本发明属于自主导航与定位技术领域，尤其是涉及一种基于二维激光雷达特征点模板匹配的位置识别方法及装置。

背景技术

为了实现机器人的自主移动，人们往往需要对环境进行建图，在建图后获取机器人在地图中的定位以执行任务。对于搭载二维激光雷达的机器人，在未知机器人在地图中的定位时，需要通过识别出曾经到过的位置来确定机器人的定位，这一过程称为重定位。识别出曾经到过的位置这一技术，称为“位置识别”。

目前，基于二维激光雷达的位置识别技术往往使用比较暴力的方法：即通过将当前激光数据的发射原点尝试放置在地图中所有可能的地方，计算发射原点在该处时激光轮廓和该处地图的匹配程度，这种方法不仅造成了巨大的计算量，而且无法满足机器人重定位的实时性。

发明内容

鉴于上述问题，本发明提供一种基于二维激光雷达特征点模板匹配的位置识别方法及装置，以解决现有技术存在的以上或者其他问题。

为解决上述技术问题，本发明采用的技术方案是：一种基于二维激光雷达特征点模板匹配的位置识别方法，包括，

机器人进行建图，在建图过程中，提取每个关键帧的特征点，并计算特征点的描述子；

对各个关键帧的信息及每个关键帧中的所有描述子的数据进行保存，在保存的过程中进行字典的训练，计算每个关键帧的词袋库；

机器人重定位时，提取定位时当前关键帧的特征点，并计算描述子，生成当前关键帧的词袋库；

将当前关键帧的词袋库与建图时保存的所有关键帧的词袋库逐一计算相似度，获取最相似的关键帧；

将当前关键帧的数据在最相似的关键帧对应的位姿处与地图进行匹配，进行重定位。

进一步的，在建图过程中，提取每个关键帧的特征点步骤中，特征点的计算公式为：

其中，S(x；t)为一维激光数据的距离形状函数，t为激光点的序号。

进一步的，在建图过程中，提取每个关键帧的特征点，并计算特征点的描述子步骤中，采用形状上下文方法进行描述子计算，包括：

对关键帧中的任一特征点做极坐标，进行角度区域和距离区域的划分；

统计每一个区域内的轮廓点数，并进行归一化处理，生成任一特征点的直方图。

进一步的，将当前关键帧的词袋库与建图时保存的所有关键帧的词袋库逐一计算相似度，获取最相似的关键帧步骤中，通过比对特征点的描述子来判断各个特征点之间的相似程度，关键帧用向量表示，关键帧＝[第一类型的特征点的数量，第二类型的特征点的数量，第三类型的特征点的数量，……]，相似度的计算使用两个向量的汉明距离表示。

进一步的，字典包括所有单词，单词为每一种类型的描述子，词袋库为每一个关键帧中的所有描述子对应的单词。

进一步的，字典的构建步骤包括：

a、随机选取k个特征，设为中心；

b、计算每个特征与每个中心之间的距离，取距离最小的特征作为归类；

c、重新计算所有类的均值，若均值变化很小，则算法收敛，退出；否则返回步骤c。

进一步的，字典采用k叉树表达，k叉树的层数为d，每层分叉数为k，直至叶子层。

进一步的，将当前关键帧的数据在最相似的关键帧对应的位姿处与地图进行匹配，进行重定位步骤中，判断匹配分数是否大于阈值，若是，则判定重定位成功。

一种基于二维激光雷达特征点模板匹配的位置识别装置，包括：

特征点提取模块，用于获取每个关键帧的特征点；

描述子计算模块，用于计算特征点的描述子；

存储模块，用于保存建图过程中所有关键帧的信息及其描述子的数据；

字典训练模块，用于使用保存的数据进行字典训练；

查询模块，用于当前关键帧查询建图时保存的所有关键帧，并计算和每一关键帧的相似度；

地图匹配模块，用于将当前关键帧在最相似的关键帧对应的位姿处和地图进行匹配，进行重定位。

一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的一个或多个程序，当一个或多个程序被处理器执行时，使得电子设备执行上述的基于二维激光雷达特征点模板匹配的位置识别方法。

一种计算机可读存储介质，计算机可读存储介质上存储有计算机指令，当计算机指令在电子设备的处理器上运行时，使得计算机存储介质所在的设备执行上述的基于二维激光雷达特征点模板匹配的位置识别方法。

由于采用上述技术方案，在机器人建图过程中，提取每一个关键帧的特征点，并对其进行描述子的计算，并对关键帧的序号、描述子进行保存，根据保存的数据进行字典的构建及训练，计算每个关键帧的词袋库，重定位时，获取当前关键帧的特征点及其描述子，查询训练好的字典，生成当前关键帧的词袋库，通过对比当前帧词袋库和建图时所有帧的词袋库的相似程度，找到最相似的建图关键帧，进行定位，将遍历匹配地图上所有位置改变为对比当前帧的词袋库和建图帧的词袋库，减小计算量，可以极大程度上提高匹配速度，找到定位，很大程度上提高了重定位的速度，耗时短，满足机器人重定位的实时性。

附图说明

图1是本发明的一实施例的建图过程的逻辑流程图；

图2是本发明的一实施例的重定位的逻辑流程图；

图3是本发明的一实施例的特征点标注示意图；

图4是本发明的一实施例的一维激光数据的距离形状函数示意图；

图5是本发明的一实施例的形状上下文对数极坐标示意图；

图6是本发明的一实施例的形状上下文直方图的示意图；

图7是本发明的一实施例的某一特征点的极坐标直方图；

图8是本发明的一实施例的字典的k叉树结构示意图；

图9是本发明的一实施例的使用和不使用激光特征匹配的重定位耗时对比图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步的说明。

图1和图2示出了本发明的一实施例的逻辑流程图，本发明的一实施例涉及一种基于二维激光雷达特征点模板匹配的位置识别方法及装置，适用于携带二维激光雷达的自主移动机器人从未知状态获取先验地图中的定位，将遍历匹配地图上所有位置改变为对比当前帧的词袋库和建图帧的词袋库，可以极大程度上提高匹配速度，找到定位，提高了重定位的速度。

一种基于二维激光雷达特征点模板匹配的位置识别方法，包括，

机器人进行建图，在建图过程中，提取每个关键帧的特征点，并计算特征点的描述子；具体地，开始建图时，接收激光数据，使用激光雷达建图时，不一定每帧激光数据都使用，通常做法是每隔一段距离使用一帧，这样的帧称为关键帧，提取每个关键帧的特征点，例如：墙角点、端点等，如图3所示，图中圆圈圈出的即是特征点的位置。

在使用激光雷达建图过程中，提取每个关键帧的特征点时，考虑激光雷达数据中的距离数据，可以把一帧激光数据简化为一堆一维数据的序列，将维激光数据的距离形状函数记为S(x；t)，对S(x；t)求二阶导数可得：

(D₂*S)(x；t)＝S(x-1；t)-2S(x；t)+S(x+1；t)

则特征点的计算公式为：

其中，S(x；t)为一维激光数据的距离形状函数，t为激光点的序号。

因此，求解上述公式的极值便可以找到特征点，如图4所示的A点。

提取每个关键帧的特征点，并计算该提取出来的特征点的描述子，每一个关键帧内具有多个特征点，所以，在提取特征点时，提取出关键帧的所有特征点。描述子是用于描述特征点特性的一种参数，通过对特征点进行编码来标识每个特征点，在重定位过程中，系统通过比对特征点的描述子来判断各个特征点之间的相似程度。

在本申请中，采用形状上下文方法进行描述子计算，包括：

对关键帧中的任一特征点做极坐标，进行角度区域和距离区域的划分；

统计每一个区域内的轮廓点数，并进行归一化处理，生成任一特征点的直方图。

例如：如图5，在形状“A”中任意选一点对其做极坐标，并进行角度区域和距离区域划分，然后统计每个区域内的轮廓点数并进行归一化处理，这样就生成了该点的直方图，如图6所示。图7形象化了描述某特征点的极坐标直方图，其中格子的颜色越深表示落入该格子的激光点越多。

对各个关键帧的信息及每个关键帧中的所有描述子的数据进行保存，在保存的过程中进行字典的训练，计算每个关键帧的词袋库；在提取了每一个关键帧的所有特征点并计算每一个特征点的描述子后，进行保存，将关键帧的序号信息及其相关的特征点和描述子保存到运行内存中，也就是，运行内存中保存有建图时的所有的关键帧的序号信息、位姿及该关键帧的所有特征点及描述子等数据；建图结束后，保存程序运行建图过程中保存在运行内存里的数据，进行字典的训练，计算每个关键帧的词袋库。

其中，字典包括所有单词，单词为每一种类型的描述子，词袋库为每一个关键帧中的所有描述子对应的单词。具体地，建图结束后，将所有的描述子进行归类，并给每一种类型的描述子设定一个标签，称为“单词”。所有单词的集合称为“字典”。在获取字典之后，查询每一个关键帧中所有的描述子对应的单词，构成单词群组，此单词群组称为“词袋库”，即一个关键帧一个词袋库。构建字典和获取历史关键帧词袋库的过程称为“训练”，使用保存的数据训练字典。

字典中的每个“单词”都是某一类特征的集合，即生成字典的过程是一个聚类过程，这里使用经典的K-means算法构建系统字典。字典的构建步骤包括：

a、随机选取k个特征，如：c1,c2,c3,...，将这k个特征设为中心；

b、计算每个特征与每个中心之间的距离，取距离最小的特征作为归类；

c、重新计算所有类的均值，若均值变化很小，则算法收敛，退出；否则返回步骤c。

由于每个关键帧都可以提取出较多的特征点，因此字典规模通常是很庞大的，为了减少查找单词的时间成本，在本实施例中，采用一种简单实用的树结构来进行字典的表达，在本实施例中，字典采用k叉树表达，如图8所示，k叉树的层数(深度)为d，每层分叉数为k，即对每层节点下的样本再次聚成k类，直至叶子层，这使其可容纳kd个单词，k、d根据实际需求进行选择调整设置，这里不做具体要求。在实际查找过程，叶子层以上的中间节点仅在快速查找时使用，以此保证了查找的效率。

那么，机器人就可以通过查找字典知晓每个关键帧存在哪些类型的特征以及特征的个数。给字典里的单词一个编号，如：记“墙角”为w1、“边缘”为w2、“断点”为w3等等。

根据获取的关键帧的词袋库，可以知道，关键帧含有的单词，例如：关键帧含有两个w1，一个w2，3个w3，则可记其内容为：设定关键帧为A，则，A＝2w1+w2+3w3。

在本实施例中，关键帧用向量表示，则，关键帧＝[第一类型的特征点的数量，第二类型的特征点的数量，第三类型的特征点的数量，……]，即为bow向量。那么，关键帧A用向量表示为：A＝[2，1，3，0，0，0...]，将每个关键帧的单词描述计算并保存下来。

机器人重定位时，提取定位时当前关键帧的特征点，并计算描述子，生成当前关键帧的词袋库；具体地，在机器人重新定位时，首先，加在训练好的字典，然后，开始定位，接受激光数据，提取当前关键帧的特征点并计算其描述子，进行字典的查询，查询描述子对应的单词，获取当前关键帧的词袋库，计算出当前关键帧的bow向量。

将当前关键帧的词袋库与建图时保存的所有关键帧的词袋库逐一计算相似度，获取最相似的关键帧：通过比对特征点的描述子来判断各个特征点之间的相似程度，使用当前关键帧的bow向量查询建图时保存的所有关键帧，计算和每一帧的相似度，获取最相似的关键帧。

相似度的计算使用两个向量的汉明距离表示。汉明距离是指两个等长字符串在对应位置上不同字符的数目。设定：关键帧A和关键帧B的bow向量分别为a，b，则，两者相似度s为：||a-b||1，其中范数L1为各元素绝对值之和，若a＝b，则s＝1，若a,b完全相反，则s＝0。

将当前关键帧的数据在最相似的关键帧对应的位姿处与地图进行匹配，进行重定位：找到最相似的关键帧后，使用当前激光数据在该关键帧对应的位姿处和地图进行匹配，进一步细化确定机器人的位姿，得到准确的定位；在进行匹配时，判断匹配分数是否大于阈值，若是，则判定重定位成功，否则，重新进行匹配。该阈值根据实际需求进行设置，这里不做具体要求。可以采用ICP匹配法、分支定界匹配法等任意公知可行的匹配方法进行匹配。

采用上述方法，将遍历匹配地图上所有位置改变为对比当前关键帧的词袋库和建图帧的词袋库，可以极大程度上提高匹配速度，找到定位，能够在很大程度上提高了重定位的速度，如图2所示，其中，曲线1为遍历匹配地图上所有位置改变的耗时曲线图，曲线2为使用机关特征匹配的重定位耗时的曲线图，图3为两者耗时之差的曲线图，由图2可以知道，采用本申请的方法进行重定位，能够缩短重定位耗时，能够在很大程度上提高了重定位的速度。

一种基于二维激光雷达特征点模板匹配的位置识别装置，包括：

特征点提取模块，用于获取每个关键帧的特征点；

描述子计算模块，用于计算特征点的描述子；

存储模块，用于保存建图过程中所有关键帧的信息及其描述子的数据；

字典训练模块，用于使用保存的数据进行字典训练；

查询模块，用于当前关键帧查询建图时保存的所有关键帧，并计算和每一关键帧的相似度；

地图匹配模块，用于将当前关键帧在最相似的关键帧对应的位姿处和地图进行匹配，进行重定位。

机器人在建图过程中，特征点提取模块提取每个关键帧的特征点，描述子计算模块对特征点进行特征描述，计算描述子，将关键帧的序号、描述子保存到存储模块(运行内存)中。

当建图结束后，保存程序运行过程中保存在运行内存里的数据，字典训练模块进行字典的训练，计算每个关键帧的词袋库。

当机器人开始定位时，首先加载建图时保存的数据库和字典，特征点提取模块提取当前关键帧的特征点，描述子计算模块计算描述子，查询模块根据描述子查询建图结束后训练好的字典，生成关键帧的词袋库，并通过和数据库中的建图关键帧的词袋逐一计算相似度，找到最为相似的关键帧。地图匹配模块使用当前关键帧的数据在该建图关键帧对应位姿处与地图进行匹配，从而得到定位。

一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的一个或多个程序，当一个或多个程序被处理器执行时，使得电子设备执行上述的基于二维激光雷达特征点模板匹配的位置识别方法。

一种计算机可读存储介质，计算机可读存储介质上存储有计算机指令，当计算机指令在电子设备的处理器上运行时，使得计算机存储介质所在的设备执行上述的基于二维激光雷达特征点模板匹配的位置识别方法。

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

集成的模块如果以软件功能单元的形式实现时，可以存储在一个计算机可读取存储介质中。本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机、服务器或者网络设备等)执行本申请中的实施例所述的自动数槽方法的全部或部分步骤。

前述的存储介质包括：U盘、移动硬盘、只读存储器、随机存取存储器、磁碟或者光盘等各种可以存储程序代码的介质。

由于采用上述技术方案，在机器人建图过程中，提取每一个关键帧的特征点，并对其进行描述子的计算，并对关键帧的序号、描述子进行保存，根据保存的数据进行字典的构建及训练，计算每个关键帧的词袋库，重定位时，获取当前关键帧的特征点及其描述子，查询训练好的字典，生成当前关键帧的词袋库，通过对比当前帧词袋库和建图时所有帧的词袋库的相似程度，找到最相似的建图关键帧，进行定位，将遍历匹配地图上所有位置改变为对比当前帧的词袋库和建图帧的词袋库，减小计算量，可以极大程度上提高匹配速度，找到定位，很大程度上提高了重定位的速度，耗时短，满足机器人重定位的实时性。

以上对本发明的实施例进行了详细说明，但所述内容仅为本发明的较佳实施例，不能被认为用于限定本发明的实施范围。凡依本发明申请范围所作的均等变化与改进等，均应仍归属于本发明的专利涵盖范围之内。

Claims (11)

1.一种基于二维激光雷达特征点模板匹配的位置识别方法，其特征在于：包括，

机器人进行建图，在建图过程中，提取每个关键帧的特征点，并计算所述特征点的描述子；

对各个所述关键帧的信息及每个所述关键帧中的所有描述子的数据进行保存，在保存的过程中进行字典的训练，计算每个所述关键帧的词袋库；

所述机器人重定位时，提取定位时当前关键帧的特征点，并计算描述子，生成当前关键帧的词袋库；

将所述当前关键帧的词袋库与建图时保存的所有关键帧的词袋库逐一计算相似度，获取最相似的关键帧；

将所述当前关键帧的数据在所述最相似的关键帧对应的位姿处与地图进行匹配，进行重定位。

2.根据权利要求1所述的基于二维激光雷达特征点模板匹配的位置识别方法，其特征在于：所述在建图过程中，提取每个关键帧的特征点步骤中，特征点的计算公式为：

其中，S(x；t)为一维激光数据的距离形状函数，t为激光点的序号。

3.根据权利要求1或2所述的基于二维激光雷达特征点模板匹配的位置识别方法，其特征在于：所述在建图过程中，提取每个关键帧的特征点，并计算所述特征点的描述子步骤中，采用形状上下文方法进行所述描述子计算，包括：

对关键帧中的任一特征点做极坐标，进行角度区域和距离区域的划分；

统计每一个区域内的轮廓点数，并进行归一化处理，生成所述任一特征点的直方图。

4.根据权利要求3所述的基于二维激光雷达特征点模板匹配的位置识别方法，其特征在于：所述将当前关键帧的词袋库与建图时保存的所有关键帧的词袋库逐一计算相似度，获取最相似的关键帧步骤中，通过比对特征点的描述子来判断各个特征点之间的相似程度，所述关键帧用向量表示，关键帧＝[第一类型的特征点的数量，第二类型的特征点的数量，第三类型的特征点的数量，……]，所述相似度的计算使用两个向量的汉明距离表示。

5.根据权利要求1或2或4所述的基于二维激光雷达特征点模板匹配的位置识别方法，其特征在于：所述字典包括所有单词，所述单词为每一种类型的描述子，所述词袋库为每一个关键帧中的所有描述子对应的单词。

6.根据权利要求5所述的基于二维激光雷达特征点模板匹配的位置识别方法，其特征在于：所述字典的构建步骤包括：

a、随机选取k个特征，设为中心；

b、计算每个特征与每个中心之间的距离，取距离最小的特征作为归类；

c、重新计算所有类的均值，若均值变化很小，则算法收敛，退出；否则返回步骤c。

7.根据权利要求6所述的基于二维激光雷达特征点模板匹配的位置识别方法，其特征在于：所述字典采用k叉树表达，所述k叉树的层数为d，每层分叉数为k，直至叶子层。

8.根据权利要求1所述的基于二维激光雷达特征点模板匹配的位置识别方法，其特征在于：所述将当前关键帧的数据在所述最相似的关键帧对应的位姿处与地图进行匹配，进行重定位步骤中，判断匹配分数是否大于阈值，若是，则判定重定位成功。

9.一种基于二维激光雷达特征点模板匹配的位置识别装置，其特征在于：包括：

特征点提取模块，用于获取每个关键帧的特征点；

描述子计算模块，用于计算所述特征点的描述子；

存储模块，用于保存建图过程中所有关键帧的信息及其描述子的数据；

字典训练模块，用于使用保存的数据进行字典训练；

查询模块，用于当前关键帧查询建图时保存的所有关键帧，并计算和每一关键帧的相似度；

地图匹配模块，用于将当前关键帧在最相似的关键帧对应的位姿处和地图进行匹配，进行重定位。

10.一种电子设备，其特征在于：包括存储器、处理器及存储在存储器上并可在处理器上运行的一个或多个程序，当所述一个或多个程序被所述处理器执行时，使得所述电子设备执行如权利要求1-8任一项所述的基于二维激光雷达特征点模板匹配的位置识别方法。

11.一种计算机可读存储介质，其特征在于：所述计算机可读存储介质上存储有计算机指令，当所述计算机指令在电子设备的处理器上运行时，使得所述计算机存储介质所在的设备执行如权利要求1-8任一项所述的基于二维激光雷达特征点模板匹配的位置识别方法。

Images