CN109614459B - 应用于二维激光的地图构建回环检测方法及设备 - Google Patents
应用于二维激光的地图构建回环检测方法及设备 Download PDFInfo
- Publication number
- CN109614459B CN109614459B CN201910168523.XA CN201910168523A CN109614459B CN 109614459 B CN109614459 B CN 109614459B CN 201910168523 A CN201910168523 A CN 201910168523A CN 109614459 B CN109614459 B CN 109614459B
- Authority
- CN
- China
- Prior art keywords
- laser
- frame
- current laser
- current
- laser frame
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000001514 detection method Methods 0.000 title claims abstract description 37
- 238000004804 winding Methods 0.000 title abstract 7
- 238000000034 method Methods 0.000 claims abstract description 51
- 238000004364 calculation method Methods 0.000 claims abstract description 19
- 239000013598 vector Substances 0.000 claims description 70
- 230000015654 memory Effects 0.000 claims description 18
- 230000008569 process Effects 0.000 claims description 18
- 238000005259 measurement Methods 0.000 claims description 14
- 230000009191 jumping Effects 0.000 claims description 13
- 238000010276 construction Methods 0.000 claims description 11
- 238000004590 computer program Methods 0.000 claims description 10
- 239000011159 matrix material Substances 0.000 claims description 6
- 230000010365 information processing Effects 0.000 claims description 3
- 238000013507 mapping Methods 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 2
- HOWHQWFXSLOJEF-MGZLOUMQSA-N systemin Chemical compound NCCCC[C@H](N)C(=O)N[C@@H](CCSC)C(=O)N[C@@H](CCC(N)=O)C(=O)N[C@@H]([C@@H](C)O)C(=O)N[C@@H](CC(O)=O)C(=O)OC(=O)[C@@H]1CCCN1C(=O)[C@H]1N(C(=O)[C@H](CC(O)=O)NC(=O)[C@H](CCCN=C(N)N)NC(=O)[C@H](CCCCN)NC(=O)[C@H](CO)NC(=O)[C@H]2N(CCC2)C(=O)[C@H]2N(CCC2)C(=O)[C@H](CCCCN)NC(=O)[C@H](CO)NC(=O)[C@H](CCC(N)=O)NC(=O)[C@@H](NC(=O)[C@H](C)N)C(C)C)CCC1 HOWHQWFXSLOJEF-MGZLOUMQSA-N 0.000 claims 1
- 230000007547 defect Effects 0.000 abstract description 4
- 230000005291 magnetic effect Effects 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 4
- 230000006870 function Effects 0.000 description 3
- 238000012986 modification Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- 230000003287 optical effect Effects 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000001052 transient effect Effects 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
- 230000003936 working memory Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Length Measuring Devices By Optical Means (AREA)
- Image Analysis (AREA)
Abstract
本发明的目的是提供一种应用于二维激光的地图构建回环检测方法及设备,本发明针对传统回环检测方法存在误匹配率高,计算量大,实时性差等缺陷,提供一种基于二维激光的回环检测方法,该方法直接利用二维激光实现回环检测。本发明相比于基于图像的回环检测方法的进步效果在于:本方法不仅计算量小,实时性高,而且在回环检测时具有旋转不变性。
Description
技术领域
本发明涉及计算机领域,尤其涉及一种应用于二维激光的地图构建回环检测方法及设备。
背景技术
回环检测模块是SLAM(地图构建)***中不可或缺的一部分,其主要负责判断机器人是否到达先前的位置,并将检测到的回环信息提供给后端进行处理。SLAM***引入回环检测可以有效地消除SLAM过程中的累计里程误差,构建高精度的环境地图。
目前主流的回环检测采用图像检索的方式,通过提取图像的特征点,将当前帧的特征点和历史关键帧提取的特征点进行配准,如果特征点的匹配对数大于一定的数量,则认为出现回环。该方法受到环境和人工特征点选取的影响,存在误匹配高,计算量大,实时性差,且仅适用于回环时视角不改变的情况。
发明内容
本发明的一个目的是提供一种应用于二维激光的地图构建回环检测方法及设备。
根据本发明的一个方面,提供了一种应用于二维激光的地图构建回环检测方法,该方法包括:
步骤S1,确定机器人的激光雷达的当前激光帧L的主方向,将当前激光的主方向对齐到激光坐标系的x轴;
步骤S2,按照所述激光坐标系中激光测量值的不同,将所述当前激光帧L分为不同的子类,表示所述子类的个数;
步骤S3,计算所述子类对应的平面描述;
步骤S4,合并所述平面描述生成该当前激光帧的直方图F,其中,表示每个平面描述向量的长度:
;
步骤S5,利用矩阵范数,对当前激光帧的直方图和激光地图中历史关键帧的直方图进行相似度测量,若测量得到的相似度大于预设阈值,则判定发生回环。
进一步的,上述方法中,若测量得到的相似度大于预设阈值,则判定发生回环,包括:
当距离小于设定的阈值时,则判定发生回环,其中,平面描述向量和表示直方图和的列向量,即激光帧L每个子类的描述向量:
。
进一步的,上述方法中,确定机器人的激光雷达的当前激光帧的主方向,将当前激光的主方向对齐到激光坐标系的轴,包括:
获取机器人的激光雷达的当前激光帧L,并且判定当前激光帧L是否为关键帧,若是关键帧,确定激光雷达的当前激光帧L的主方向,将当前激光帧L的主方向对齐到激光坐标系的轴,然后结束本步骤跳转到步骤S2;若不是关键帧,重复本步骤。
进一步的,上述方法中,获取机器人的激光雷达的当前激光帧L,并且判定当前激光帧L是否为关键帧,若是关键帧,确定激光雷达的当前激光帧L的主方向,将当前激光帧L的主方向对齐到激光坐标系的x轴,然后结束本步骤跳转到步骤S2;若不是关键帧,重复本步骤,包括:
步骤S111,获取当前激光帧L,根据机器人的当前时刻和上一个时刻之间的里程距离判断当前激光帧是否为关键帧,若是关键帧,跳转到步骤S112;否则,重复本步骤;
步骤S112,为当前激光帧L分配一个方向描述向量[0,0,0,0]和直线穿过当前激光帧的原点;
步骤S113,计算当前激光帧L中每个激光束的欧式空间坐标,计算公式如下:
,
其中,表示当前激光帧L中第个激光束的测量值,表示当前激光帧L中第个观测值相对于激光坐标系的角度值;
步骤S114,计算当前激光帧L每个激光束最近的直线,更新所述方向描述向量,其中,计算过程如下所示:
,
;
步骤S115,根据所述方向描述向量D中的最大值确定当前激光帧L的主方向,根据激光的主方向旋转当前激光帧L到激光坐标系的x轴,然后结束本步骤跳转到步骤S2,其中,计算过程如下,和分别表示直线的斜率和截距:
,
;
;
。
进一步的,上述方法中,按照所述激光坐标系中激光测量值的不同,将所述当前激光帧L分为不同的子类,包括:
将当前激光帧L表示为,其中,表示当前激光帧L中第个激光束的测量值,表示当前激光帧L中第个观测值相对于激光坐标系的角度值;
对当前激光帧L中的每个激光束按照其测量值的大小归到不同的子类。
进一步的,上述方法中,计算所述子类对应的平面描述向量,包括:
为所述子类初始化一个平面描述向量和分配一个网格状的结构体;
对于所述子类的每个成员计算其在欧式坐标系中的位置:
;
计算每个点在中的网格,并将其存储在中;
根据中的存储的点集合计算的协方差及其特征向量和特征值,其中,,
,
,
;
分配m=4条直线穿过的均值,根据上一步计算出的特征值和特征向量,计算所属的子类index:
,
,
其中,表示特征向量和直线之间的距离;
根据所述index,更新的值:
。
根据本发明的另一面,还提供一种应用于二维激光的地图构建回环检测设备,该设备包括:
第一装置,用于确定机器人的激光雷达的当前激光帧L的主方向,将当前激光的主方向对齐到激光坐标系的x轴;
第二装置,用于按照所述激光坐标系中激光测量值的不同,将所述当前激光帧L分为不同的子类,表示所述子类的个数;
第三装置,用于计算所述子类对应的平面描述向量;
第四装置,用于合并所述平面描述向量生成该当前激光帧的直方图F,其中,表示每个平面描述向量的长度:
;
第五装置,用于利用矩阵范数,对当前激光帧的直方图和激光地图中历史关键帧的直方图进行相似度测量,若测量得到的相似度大于预设阈值,则判定发生回环。
进一步的,上述设备中,所述第五装置,用于当距离小于设定的阈值时,则判定发生回环,其中,平面描述向量和表示直方图和的列向量,即激光帧L每个子类的描述向量:
。
进一步的,上述设备中,所述第一装置,用于获取机器人的激光雷达的当前激光帧L,并且判定当前激光帧L是否为关键帧,若是关键帧,确定激光雷达的当前激光帧L的主方向,将当前激光帧L的主方向对齐到激光坐标系的轴,然后结束所述第一装置的执行跳转到所述第二装置执行;若不是关键帧,重复执行所述第一装置。
进一步的,上述设备中,所述第一装置,包括
第一一装置,用于获取当前激光帧L,根据机器人的当前时刻和上一个时刻之间的里程距离判断当前激光帧是否为关键帧,若是关键帧,跳转到第一二装置;否则,重复本第一一装置的执行;
第一二装置,用于为当前激光帧L分配一个方向描述向量[0,0,0,0]和直线穿过当前激光帧的原点;
第一三装置,用于计算当前激光帧L中每个激光束的欧式空间坐标,计算公式如下:
,
其中,表示当前激光帧L中第个激光束的测量值,表示当前激光帧L中第个观测值相对于激光坐标系的角度值;
第一四装置,用于计算当前激光帧L每个激光束最近的直线,更新所述方向描述向量D,其中,计算过程如下所示:
,
;
第一五装置,用于根据所述方向描述向量D中的最大值确定当前激光帧L的主方向,根据激光的主方向旋转当前激光帧L到激光坐标系的x轴,然后结束本第一五装置第执行跳转到所述第二装置执行,其中,计算过程如下,和分别表示直线的斜率和截距:
,
;
;
。
进一步的,上述设备中,,所述第二装置,用于将当前激光帧L表示为,其中,表示当前激光帧L中第个激光束的测量值,表示当前激光帧L中第个观测值相对于激光坐标系的角度值;对当前激光帧L中的每个激光束按照其测量值的大小归到不同的子类。
进一步的,上述设备中,第三装置,用于为所述子类初始化一个平面描述向量和分配一个网格状的结构体;对于所述子类的每个成员计算其在欧式坐标系中的位置:
;
计算每个点在中的网格,并将其存储在中;根据中的存储的点集合计算的协方差及其特征向量和特征值,其中,,
,
,
;
分配m=4条直线穿过的均值,根据上一步计算出的特征值和特征向量,计算所属的子类index:
,
,
其中,表示特征向量和直线之间的距离;
根据所述index,更新的值:
。
根据本发明的另一面,还提供一种计算机可读介质,其上存储有计算机可读指令,所述计算机可读指令可被处理器执行以实现上述任一项所述的方法。
根据本发明的另一面,还提供一种用于在网络设备端信息处理的设备,该设备包括用于存储计算机程序指令的存储器和用于执行程序指令的处理器,其中,当该计算机程序指令被该处理器执行时,触发该设备执行上述任一项所述的方法。
与现有技术相比,本发明针对背景技术中所涉及的传统回环检测方法存在误匹配率高,计算量大,实时性差等缺陷,提供一种基于二维激光的回环检测方法,该方法直接利用二维激光实现回环检测。本发明相比于基于图像的回环检测方法的进步效果在于:本方法不仅计算量小,实时性高,而且在回环检测时具有旋转不变性。
附图说明
通过阅读参照以下附图所作的对非限制性实施例所作的详细描述,本发明的其它特征、目的和优点将会变得更明显:
图1示出本发明一实施例的激光雷达划分子类示意图;
图2示出本发明一实施例的旋转激光主方向到x轴示意图;
图3示出本发明一实施例的应用于二维激光的地图构建回环检测方法流程图。
附图中相同或相似的附图标记代表相同或相似的部件。
具体实施方式
下面结合附图对本发明作进一步详细描述。
在本申请一个典型的配置中,终端、服务网络的设备和可信方均包括一个或多个处理器 (CPU)、输入/输出接口、网络接口和内存。
内存可能包括计算机可读介质中的非永久性存储器,随机存取存储器 (RAM) 和/或非易失性内存等形式,如只读存储器 (ROM) 或闪存(flash RAM)。内存是计算机可读介质的示例。
计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括,但不限于相变内存 (PRAM)、静态随机存取存储器 (SRAM)、动态随机存取存储器 (DRAM)、其他类型的随机存取存储器 (RAM)、只读存储器 (ROM)、电可擦除可编程只读存储器 (EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘 (DVD) 或其他光学存储、 磁盒式磁带,磁带磁盘存储或其他磁性存储设备或任何其他非传输介质,可用于存储可以被计算设备访问的信息。按照本文中的界定,计算机可读介质不包括非暂存电脑可读媒体 (transitory media),如调制的数据信号和载波。
如图3所示,本发明提供一种应用于二维激光的地图构建回环检测方法,所述方法包括:
步骤S1,如图2所示,确定机器人的激光雷达的当前激光帧L的主方向,将当前激光的主方向对齐到激光坐标系的x轴;
步骤S2,如图1所示,按照所述激光坐标系中激光测量值的不同,将所述当前激光帧L分为不同的子类,表示所述子类的个数;
步骤S3,计算所述子类对应的平面描述向量;
步骤S4,合并所述平面描述向量生成该当前激光帧的直方图F,其中,表示子类的个数,表示每个平面描述向量的长度;
;
步骤S5,利用矩阵范数,对当前激光帧的直方图和激光地图中历史关键帧的直方图进行相似度测量,若测量得到的相似度大于预设阈值,则判定发生回环。
在此,本发明针对背景技术中所涉及的传统回环检测方法存在误匹配率高,计算量大,实时性差等缺陷,提供一种基于二维激光的回环检测方法,该方法直接利用二维激光实现回环检测。本发明相比于基于图像的回环检测方法的进步效果在于:本方法不仅计算量小,实时性高,而且在回环检测时具有旋转不变性。
本发明的应用于二维激光的地图构建回环检测方法一实施例中,步骤S5中,若测量得到的相似度大于预设阈值,则判定发生回环,包括:
当距离小于设定的阈值时,则判定发生回环,其中,平面描述向量和表示直方图和的列向量,即激光帧L每个子类的描述向量:
。
在此,若距离小于设定的阈值则SLAM尝试进行回环;否则跳转到步骤S1。
本实施例通过判断距离是否小于设定的阈值,可以跟高效、可靠的判断是否发生回环。
本发明的应用于二维激光的地图构建回环检测方法一实施例中,步骤S1,确定机器人的激光雷达的当前激光帧的主方向,将当前激光的主方向对齐到激光坐标系的轴,包括:
步骤S11,获取机器人的激光雷达的当前激光帧L,并且判定当前激光帧L是否为关键帧,若是关键帧,确定激光雷达的当前激光帧L的主方向,将当前激光帧L的主方向对齐到激光坐标系的轴,然后结束本步骤跳转到步骤S2;若不是关键帧,重复本步骤,以继续获取当前激光帧L。
在此,通过选取为关键帧的当前激光帧L,可以保证后续回环检测的可靠性。
本发明的应用于二维激光的地图构建回环检测方法一实施例中,步骤S11,获取机器人的激光雷达的当前激光帧L,并且判定当前激光帧L是否为关键帧,若是关键帧,确定激光雷达的当前激光帧L的主方向,将当前激光帧L的主方向对齐到激光坐标系的x轴,然后结束本步骤跳转到步骤S2;若不是关键帧,重复本步骤,包括:
步骤S111,获取当前激光帧L,根据机器人的当前时刻和上一个时刻之间的里程距离判断当前激光帧是否为关键帧,若是关键帧,跳转到步骤S112;否则,重复本步骤;
步骤S112,为当前激光帧L分配一个方向描述向量[0,0,0,0]和直线穿过当前激光帧的原点;
步骤S113,计算当前激光帧L中每个激光束的欧式空间坐标,计算公式如下:
,
其中,表示当前激光帧L中第个激光束的测量值,表示当前激光帧L中第个观测值相对于激光坐标系的角度值;
步骤S114,计算当前激光帧L每个激光束最近的直线,更新所述方向描述向量,其中,计算过程如下所示:
,
;
步骤S115,根据所述描述向量D中的最大值确定当前激光帧L的主方向,根据激光的主方向旋转当前激光帧L到激光坐标系的x轴,然后结束本步骤跳转到步骤S2,如图2所示,其中,计算过程如下,其中,和分别表示直线的斜率和截距:
,
;
;
。
本发明的应用于二维激光的地图构建回环检测方法一实施例中,步骤S2,按照所述激光坐标系中激光测量值的不同,将所述当前激光帧L分为不同的子类,其中,的取值为9,包括:
步骤S21,将当前激光帧L表示为,其中,表示当前激光帧L中第个激光束的测量值,表示当前激光帧L中第个观测值相对于激光坐标系的角度值;
步骤S22,对当前激光帧L中的每个激光束按照其测量值的大小归到不同的子类,如图1所示,从而实现准确的归类。
本发明的应用于二维激光的地图构建回环检测方法一实施例中,步骤S3,计算所述子类对应的平面描述向量,包括:
步骤S31,为所述子类初始化一个平面描述向量和分配一个网格状的结构体;
步骤S32,对于所述子类的每个成员计算其在欧式坐标系中的位置:
,
步骤S33,计算每个点在中的网格,并将其存储在中;
步骤S34,根据中的存储的点集合计算的协方差及其特征向量和特征值,其中,,
,
,
;
步骤S35,分配m=4条直线穿过的均值,根据上一步计算出的特征值和特征向量,计算所属的子类index:
,
,
其中,表示特征向量和直线之间的距离;
步骤S36,根据上一步的结果index,更新的值:
。
根据本发明的另一面,还提供一种应用于二维激光的地图构建回环检测设备,该设备包括:
第一装置,用于确定机器人的激光雷达的当前激光帧L的主方向,将当前激光的主方向对齐到激光坐标系的x轴;
第二装置,用于按照所述激光坐标系中激光测量值的不同,将所述当前激光帧L分为不同的子类,表示所述子类的个数;
第三装置,用于计算所述子类对应的平面描述向量;
第四装置,用于合并所述平面描述向量生成该当前激光帧的直方图F,其中,表示每个平面描述向量的长度:
;
第五装置,用于利用矩阵范数,对当前激光帧的直方图和激光地图中历史关键帧的直方图进行相似度测量,若测量得到的相似度大于预设阈值,则判定发生回环。
进一步的,上述设备中,所述第五装置,用于当距离小于设定的阈值时,则判定发生回环,其中,和表示直方图和的列向量,即激光帧L每个子类的描述向量:
。
进一步的,上述设备中,所述第一装置,用于获取机器人的激光雷达的当前激光帧L,并且判定当前激光帧L是否为关键帧,若是关键帧,确定激光雷达的当前激光帧L的主方向,将当前激光帧L的主方向对齐到激光坐标系的轴,然后结束所述第一装置的执行跳转到所述第二装置执行;若不是关键帧,重复执行所述第一装置。
进一步的,上述设备中,所述第一装置,包括
第一一装置,用于获取当前激光帧L,根据机器人的当前时刻和上一个时刻之间的里程距离判断当前激光帧是否为关键帧,若是关键帧,跳转到第一二装置;否则,重复本第一一装置的执行;
第一二装置,用于为当前激光帧L分配一个方向描述向量[0,0,0,0]和直线穿过当前激光帧的原点;
第一三装置,用于计算当前激光帧L中每个激光束的欧式空间坐标,计算公式如下:
,
其中,表示当前激光帧L中第个激光束的测量值,表示当前激光帧L中第个观测值相对于激光坐标系的角度值;
第一四装置,用于计算当前激光帧L每个激光束最近的直线,更新所述方向描述向量,其中,计算过程如下所示:
,
;
第一五装置,用于根据所述方向描述向量D中的最大值确定当前激光帧L的主方向,根据激光的主方向旋转当前激光帧L到激光坐标系的x轴,然后结束本第一五装置第执行跳转到所述第二装置执行,其中,计算过程如下,和分别表示直线的斜率和截距:
,
;
;
。
进一步的,上述设备中,所述第二装置,用于将当前激光帧L表示为,其中,表示当前激光帧L中第个激光束的测量值,表示当前激光帧L中第个观测值相对于激光坐标系的角度值;对当前激光帧L中的每个激光束按照其测量值的大小归到不同的子类。
进一步的,上述设备中,第三装置,用于为所述子类初始化一个平面描述向量和分配一个网格状的结构体;对于所述子类的每个成员计算其在欧式坐标系中的位置:
;
计算每个点在中的网格,并将其存储在中;根据中的存储的点集合计算的协方差及其特征向量和特征值,其中,,
,
,
;
分配m=4条直线穿过的均值,根据上一步计算出的特征值和特征向量,计算所属的子类index:
,
,
其中,表示特征向量和直线之间的距离;
根据所述index,更新的值:
。
根据本发明的另一面,还提供一种计算机可读介质,其上存储有计算机可读指令,所述计算机可读指令可被处理器执行以实现上述任一项所述的方法。
根据本发明的另一面,还提供一种用于在网络设备端信息处理的设备,该设备包括用于存储计算机程序指令的存储器和用于执行程序指令的处理器,其中,当该计算机程序指令被该处理器执行时,触发该设备执行上述任一项所述的方法。
本发明的各设备和存储介质实施例的详细内容,具体可参见各方法实施例的对应部分,在此,不再赘述。
与现有技术相比,本发明针对背景技术中所涉及的传统回环检测方法存在误匹配率高,计算量大,实时性差等缺陷,提供一种基于二维激光的回环检测方法,该方法直接利用二维激光实现回环检测。本发明相比于基于图像的回环检测方法的进步效果在于:本方法不仅计算量小,实时性高,而且在回环检测时具有旋转不变性。
显然,本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的精神和范围。这样,倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内,则本申请也意图包含这些改动和变型在内。
需要注意的是,本发明可在软件和/或软件与硬件的组合体中被实施,例如,可采用专用集成电路(ASIC)、通用目的计算机或任何其他类似硬件设备来实现。在一个实施例中,本发明的软件程序可以通过处理器执行以实现上文所述步骤或功能。同样地,本发明的软件程序(包括相关的数据结构)可以被存储到计算机可读记录介质中,例如,RAM存储器,磁或光驱动器或软磁盘及类似设备。另外,本发明的一些步骤或功能可采用硬件来实现,例如,作为与处理器配合从而执行各个步骤或功能的电路。
另外,本发明的一部分可被应用为计算机程序产品,例如计算机程序指令,当其被计算机执行时,通过该计算机的操作,可以调用或提供根据本发明的方法和/或技术方案。而调用本发明的方法的程序指令,可能被存储在固定的或可移动的记录介质中,和/或通过广播或其他信号承载媒体中的数据流而被传输,和/或被存储在根据所述程序指令运行的计算机设备的工作存储器中。在此,根据本发明的一个实施例包括一个装置,该装置包括用于存储计算机程序指令的存储器和用于执行程序指令的处理器,其中,当该计算机程序指令被该处理器执行时,触发该装置运行基于前述根据本发明的多个实施例的方法和/或技术方案。
对于本领域技术人员而言,显然本发明不限于上述示范性实施例的细节,而且在不背离本发明的精神或基本特征的情况下,能够以其他的具体形式实现本发明。因此,无论从哪一点来看,均应将实施例看作是示范性的,而且是非限制性的,本发明的范围由所附权利要求而不是上述说明限定,因此旨在将落在权利要求的等同要件的含义和范围内的所有变化涵括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。此外,显然“包括”一词不排除其他单元或步骤,单数不排除复数。装置权利要求中陈述的多个单元或装置也可以由一个单元或装置通过软件或者硬件来实现。第一,第二等词语用来表示名称,而并不表示任何特定的顺序。
Claims (8)
1.一种应用于二维激光的地图构建回环检测方法,其中,该方法包括:
步骤S1,确定机器人的激光雷达的当前激光帧L的主方向,将当前激光的主方向对齐到激光坐标系的x轴,包括:
获取机器人的激光雷达的当前激光帧L,并且判定当前激光帧L是否为关键帧,若是关键帧,确定激光雷达的当前激光帧L的主方向,将当前激光帧L的主方向对齐到激光坐标系的x轴,然后结束本步骤跳转到步骤S2;若不是关键帧,重复本步骤;
步骤S2,按照所述激光坐标系中激光测量值的不同,将所述当前激光帧L分为不同的子类Li,i=1...n,n=9表示所述子类的个数;
步骤S3,计算所述子类Li对应的平面描述向量Fi,包括:
为所述子类Li初始化一个平面描述向量Fi=[0,0,0,0]T和分配一个网格状的结构体β;
对于所述子类Li的每个成员(di,θi)计算其在欧式坐标系中的位置(xi,yi):
计算每个点(xi,yi)在β中的网格bi∈β,并将其存储在bi中;
根据bi∈β中的存储的点集合{X1,X2,...,Xs}计算bi的协方差∑i及其特征向量e和特征值λ,其中,
|∑i-λE|=0;
分配m=4条直线πj=1,...,m穿过bi的均值μi,根据上一步计算出的特征值λi=1,2和特征向量ei=1,2,计算bi所属的子类index:
index=arg mini d(e,πi),i=1,2,3,4,
其中,d(e,πi)表示特征向量和直线πi之间的距离;
根据所述index,更新Fi(index)的值:
Fi(index)=Fi(index)+1;
步骤S4,合并所述平面描述向量Fi生成该当前激光帧L的直方图F,其中,m=4表示每个平面描述向量Fi的长度:
步骤S5,利用矩阵范数,对当前激光帧L的直方图F和激光地图中历史关键帧的直方图G进行相似度测量,若测量得到的相似度大于预设阈值,则判定发生回环,包括:
当距离δ(F,G)小于设定的阈值时,则测量得到的相似度大于预设阈值,判定发生回环,其中,平面描述向量Fi和Gi表示直方图F和G的列向量:
2.根据权利要求1所述的方法,其中,获取机器人的激光雷达的当前激光帧L,并且判定当前激光帧L是否为关键帧,若是关键帧,确定激光雷达的当前激光帧L的主方向,将当前激光帧L的主方向对齐到激光坐标系的x轴,然后结束本步骤跳转到步骤S2;若不是关键帧,重复本步骤,包括:
步骤S111,获取当前激光帧L,根据机器人的当前时刻和上一个时刻之间的里程距离判断当前激光帧是否为关键帧,若是关键帧,跳转到步骤S112;否则,重复本步骤;
步骤S112,为当前激光帧L分配一个方向描述向量D=[0,0,0,0]和m=4条直线πj=1,...,m穿过当前激光帧L的原点;
步骤S113,计算当前激光帧L中每个激光束的欧式空间坐标Xi(xi,yi),计算公式如下:
其中,di表示当前激光帧L中第i个激光束的测量值,θi表示当前激光帧L中第i个观测值相对于激光坐标系的角度值;
步骤S114,计算当前激光帧L每个激光束Xi最近的直线πj,更新所述方向描述向量D,其中,计算过程如下所示:
index=arg minj d(Xi,πj),j=1,...,m,
D(index)=D(index)+1;
步骤S115,根据所述方向描述向量D中的最大值确定当前激光帧L的主方向θj,根据激光的主方向θj旋转当前激光帧L到激光坐标系的x轴,然后结束本步骤跳转到步骤S2,其中,计算过程如下,Aj和bj分别表示直线πj的斜率和截距:
j=arg maxi Di,i=1...m,
πj:yj=Ajxj+bj;
θj=atan Aj;
3.根据权利要求2所述的方法,其中,按照所述激光坐标系中激光测量值的不同,将所述当前激光帧L分为不同的子类Li,i=1...n,包括:
将当前激光帧L表示为L(di,θi),其中,di表示当前激光帧L中第i个激光束的测量值,θi表示当前激光帧L中第i个观测值相对于激光坐标系的角度值;
对当前激光帧L中的每个激光束按照其测量值di的大小归到不同的子类Li。
4.一种应用于二维激光的地图构建回环检测设备,其中,该设备包括:
第一装置,用于确定机器人的激光雷达的当前激光帧L的主方向,将当前激光的主方向对齐到激光坐标系的x轴,其中,所述第一装置,用于获取机器人的激光雷达的当前激光帧L,并且判定当前激光帧L是否为关键帧,若是关键帧,确定激光雷达的当前激光帧L的主方向,将当前激光帧L的主方向对齐到激光坐标系的x轴,然后结束所述第一装置的执行跳转到所述第二装置执行;若不是关键帧,重复执行所述第一装置;
第二装置,用于按照所述激光坐标系中激光测量值的不同,将所述当前激光帧L分为不同的子类Li,i=1...n,n=9表示所述子类的个数;
第三装置,用于计算所述子类Li对应的平面描述向量Fi,其中,所述第三装置,用于为所述子类Li初始化一个平面描述向量Fi=[0,0,0,0]T和分配一个网格状的结构体β;对于所述子类Li的每个成员(di,θi)计算其在欧式坐标系中的位置(xi,yi):
计算每个点(xi,yi)在β中的网格bi∈β,并将其存储在bi中;根据bi∈β中的存储的点集合{X1,X2,...,Xs}计算bi的协方差∑i及其特征向量e和特征值λ,其中,
|∑i-λE|=0;
分配m=4条直线πj=1,...,m穿过bi的均值μi,根据上一步计算出的特征值λi=1,2和特征向量ei=1,2,计算bi所属的子类index:
index=arg mini d(e,πi),i=1,2,3,4,
其中,d(e,πi)表示特征向量和直线πi之间的距离;
根据所述index,更新Fi(index)的值:
Fi(index)=Fi(index)+1;
第四装置,用于合并所述平面描述向量Fi生成该当前激光帧L的直方图F,其中,m=4表示每个平面描述向量Fi的长度:
第五装置,用于利用矩阵范数,对当前激光帧L的直方图F和激光地图中历史关键帧的直方图G进行相似度测量,若测量得到的相似度大于预设阈值,则判定发生回环,其中,所述第五装置,用于当距离δ(F,G)小于设定的阈值时,则测量得到的相似度大于预设阈值,判定发生回环,其中,平面描述向量Fi和Gi表示直方图F和G的列向量:
5.根据权利要求4所述的设备,其中,所述第一装置,包括:
第一一装置,用于获取当前激光帧L,根据机器人的当前时刻和上一个时刻之间的里程距离判断当前激光帧是否为关键帧,若是关键帧,跳转到第一二装置;否则,重复本第一一装置的执行;
第一二装置,用于为当前激光帧L分配一个方向描述向量D=[0,0,0,0]和m=4条直线πj=1,...,m穿过当前激光帧L的原点;
第一三装置,用于计算当前激光帧L中每个激光束的欧式空间坐标Xi(xi,yi),计算公式如下:
其中,di表示当前激光帧L中第i个激光束的测量值,θi表示当前激光帧L中第i个观测值相对于激光坐标系的角度值;
第一四装置,用于计算当前激光帧L每个激光束Xi最近的直线πj,更新所述方向描述向量D,其中,计算过程如下所示:
index=arg minj d(Xi,πj),j=1,...,m,
D(index)=D(index)+1;
第一五装置,用于根据所述方向描述向量D中的最大值确定当前激光帧L的主方向θj,根据激光的主方向θj旋转当前激光帧L到激光坐标系的x轴,然后结束本第一五装置第执行跳转到所述第二装置执行,其中,计算过程如下,Aj和bj分别表示直线πj的斜率和截距:
j=arg maxi Di,i=1...m,
πj:yj=Ajxj+bj;
θj=atan Aj;
6.根据权利要求5所述的设备,其中,所述第二装置,用于将当前激光帧L表示为L(di,θi),其中,di表示当前激光帧L中第i个激光束的测量值,θi表示当前激光帧L中第i个观测值相对于激光坐标系的角度值;对当前激光帧L中的每个激光束按照其测量值di的大小归到不同的子类Li。
7.一种计算机可读介质,其上存储有计算机可读指令,所述计算机可读指令可被处理器执行以实现权利要求1至3中任一项所述的方法。
8.一种用于在网络设备端信息处理的设备,该设备包括用于存储计算机程序指令的存储器和用于执行程序指令的处理器,其中,当该计算机程序指令被该处理器执行时,触发该设备执行权利要求1至3中任一项所述的方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910168523.XA CN109614459B (zh) | 2019-03-06 | 2019-03-06 | 应用于二维激光的地图构建回环检测方法及设备 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910168523.XA CN109614459B (zh) | 2019-03-06 | 2019-03-06 | 应用于二维激光的地图构建回环检测方法及设备 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109614459A CN109614459A (zh) | 2019-04-12 |
CN109614459B true CN109614459B (zh) | 2019-05-28 |
Family
ID=66022247
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910168523.XA Active CN109614459B (zh) | 2019-03-06 | 2019-03-06 | 应用于二维激光的地图构建回环检测方法及设备 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109614459B (zh) |
Families Citing this family (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110880010A (zh) * | 2019-07-05 | 2020-03-13 | 电子科技大学 | 基于卷积神经网络的视觉slam闭环检测算法 |
CN110796683A (zh) * | 2019-10-15 | 2020-02-14 | 浙江工业大学 | 一种基于视觉特征联合激光slam的重定位方法 |
CN112162294B (zh) * | 2020-10-10 | 2023-12-15 | 北京布科思科技有限公司 | 一种基于激光传感器的机器人结构检测方法 |
CN112668653A (zh) * | 2020-12-30 | 2021-04-16 | 北京百度网讯科技有限公司 | 基于激光雷达地图的回环检测方法、装置、设备及介质 |
CN113781569B (zh) * | 2021-01-04 | 2024-04-05 | 北京京东乾石科技有限公司 | 一种回环检测方法和装置 |
CN113390370B (zh) * | 2021-05-17 | 2023-01-13 | 珠海格力电器股份有限公司 | 一种对象检测方法、装置、电子设备及存储介质 |
Family Cites Families (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP3078935A1 (en) * | 2015-04-10 | 2016-10-12 | The European Atomic Energy Community (EURATOM), represented by the European Commission | Method and device for real-time mapping and localization |
CN106056643B (zh) * | 2016-04-27 | 2018-10-26 | 深圳积木易搭科技技术有限公司 | 一种基于点云的室内动态场景slam方法及*** |
SG10201700299QA (en) * | 2017-01-13 | 2018-08-30 | Otsaw Digital Pte Ltd | Three-dimensional mapping of an environment |
CN107316328B (zh) * | 2017-05-11 | 2020-07-07 | 浙江大学 | 一种基于二维激光扫描仪角点特征的闭环检测方法 |
CN107403163B (zh) * | 2017-07-31 | 2019-11-08 | 武汉大学 | 一种基于深度学习的激光slam闭环自动检测方法 |
CN108256060B (zh) * | 2018-01-16 | 2021-02-09 | 广州视源电子科技股份有限公司 | 一种闭环检测方法、装置、终端及存储介质 |
-
2019
- 2019-03-06 CN CN201910168523.XA patent/CN109614459B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN109614459A (zh) | 2019-04-12 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109614459B (zh) | 应用于二维激光的地图构建回环检测方法及设备 | |
Cai et al. | Practical optimal registration of terrestrial LiDAR scan pairs | |
KR102068419B1 (ko) | 포인트 클라우드 데이터 수집 궤적을 조정하기 위한 방법, 장치 및 컴퓨터 판독 가능한 매체 | |
US10515460B2 (en) | Neural network-based camera calibration | |
US7496471B2 (en) | Surveying apparatus and method of analyzing measuring data | |
US5495537A (en) | Methods and apparatus for machine vision template matching of images predominantly having generally diagonal and elongate features | |
JP4125786B2 (ja) | 疎配列画像の相関付け | |
US9536306B2 (en) | Vehicle vision system | |
US8406535B2 (en) | Invariant visual scene and object recognition | |
US8559726B2 (en) | Image analysis based on pixel brightness grouping | |
US20100226533A1 (en) | Method of image processing | |
US10645668B2 (en) | Indoor positioning system and method based on geomagnetic signals in combination with computer vision | |
US20220343105A1 (en) | Method and device for 3d shape matching based on local reference frame | |
CN115375870B (zh) | 回环检测优化方法、电子设备及计算机可读存储装置 | |
US20220262033A1 (en) | Method for Determining the Position of a first Image Region in a Correspondence Image, SOC and Control Device, and System for Carrying out the Method, and Computer Program | |
CN116681730A (zh) | 一种目标物追踪方法、装置、计算机设备和存储介质 | |
WO2022078301A1 (zh) | 一种数据处理的方法和装置 | |
CN113808142A (zh) | 一种地面标识的识别方法、装置、电子设备 | |
JP2008116207A (ja) | 画像計測装置、画像計測方法及びプログラム | |
CN116819561A (zh) | 一种点云数据匹配方法、***、电子设备及存储介质 | |
CN115952248A (zh) | 终端设备的位姿处理方法、装置、设备、介质及产品 | |
CN115656991A (zh) | 车辆外参标定方法、装置、设备以及存储介质 | |
US11599743B2 (en) | Method and apparatus for obtaining product training images, and non-transitory computer-readable storage medium | |
Zhang et al. | Effective occlusion handling for fast correlation filter-based trackers | |
US20230015645A1 (en) | 3d shape matching method and device based on 3d local feature description using sghs |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |