CN115343722A - 一种大范围场景下基于回环检测的激光雷达slam方法 - Google Patents

一种大范围场景下基于回环检测的激光雷达slam方法 Download PDF

Info

Publication number
CN115343722A
CN115343722A CN202210796243.5A CN202210796243A CN115343722A CN 115343722 A CN115343722 A CN 115343722A CN 202210796243 A CN202210796243 A CN 202210796243A CN 115343722 A CN115343722 A CN 115343722A
Authority
CN
China
Prior art keywords
point cloud
frame
pose
point
global
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.)
Pending
Application number
CN202210796243.5A
Other languages
English (en)
Inventor
徐晓苏
李诺
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Southeast University
Original Assignee
Southeast University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Southeast University filed Critical Southeast University
Priority to CN202210796243.5A priority Critical patent/CN115343722A/zh
Publication of CN115343722A publication Critical patent/CN115343722A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01SRADIO 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/00Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88Lidar systems specially adapted for specific applications
    • G01S17/89Lidar systems specially adapted for specific applications for mapping or imaging
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804Creation or updating of map data
    • G01C21/3833Creation or updating of map data characterised by the source of data
    • G01C21/3837Data obtained from a single source
    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06TIMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00Image analysis
    • G06T7/10Segmentation; Edge detection
    • G06T7/136Segmentation; Edge detection involving thresholding
    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06TIMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00Image analysis
    • G06T7/30Determination of transform parameters for the alignment of images, i.e. image registration
    • G06T7/33Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Automation & Control Theory (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

本发明公开了一种大范围场景下基于回环检测的激光雷达SLAM方法,包括:首先,利用激光雷达局部曲面点云的曲率进行特征提取;其次,根据点到直线和点到平面的配准方式对相邻两帧的特征点进行加权配准,获取激光雷达当前帧相对于初始帧的全局位姿;然后,将提取的特征点云和全局位姿传入回环检测模块,利用特征点云生成的全局描述子进行回环检测;如果满足回环检测的条件,再利用加权的特征匹配方法获得回环成功的两帧之间的位姿,并将当前帧和回环帧之间的所有位姿加入到位姿图中,进行全局优化,获取优化后的位姿,最后,根据优化后的位姿对特征点云进行变换构建全局增量式地图。本方法通过加权不同特征点的点云信息并且对全局描述子进行改进,以及通过特征匹配的方式进行配准,提高了位姿精度的同时,有效地缩短了回环检测的时间。

Description

一种大范围场景下基于回环检测的激光雷达SLAM方法
技术领域
本发明涉及一种大范围场景下基于回环检测的激光雷达SLAM方法,属于自动驾驶技术,适用于导航与定位领域。
背景技术
SLAM(simultaneous localization and mapping),同步定位与建图,是机器人从未知环境的未知地点出发,在运动过程中通过搭载相机、激光和里程计等传感器重复观测环境特征定位自身位姿,再根据自身位置构建周围环境的增量式地图的技术。针对激光SLAM技术,在无人驾驶、机器人和军事工业方面有着广泛的应用。
在大范围场景中,激光雷达传感器具有运动距离越长,误差越大的特点。当移动机器人长时间移动时,帧间配准不再能够准确估计姿态,这将导致累积误差,导致建立的三维地图中出现鬼影图像,因此,回环检测仍然面临着严峻的挑战。现有的解决回环检测的方案大多是直接采用某一全局描述子进行回环检测和ICP优化,但是存在运算效率底,检测回环精度不高等特征,为了提高大范围场景下激光SLAM的定位精度与回环检测精度和速度,亟需提出一种适用于大范围场景下的激光雷达SLAM方法。
发明内容
为解决上述问题,本发明公开了一种大范围场景下基于回环检测的激光雷达SLAM方法,能有效地提高大范围场景下激光SLAM的定位精度和回环检测的速度。
为达到上述目的,本发明的技术方案如下:
大范围场景下基于回环检测的激光雷达SLAM方法,包括如下步骤:
S1:利用激光雷达局部曲面点云的曲率进行特征提取;
S2:根据点到直线和点到平面的配准方式对相邻两帧的特征点进行加权配准,获取激光雷达当前帧相对于初始帧的全局位姿;
S3:将提取的特征点云和全局位姿传入回环检测模块,利用特征点云生成的全局描述子进行回环检测;
S4:利用加权的特征匹配方法获得回环成功的两帧之间的位姿,判断回环成功;
S5:将当前帧和回环帧之间的所有位姿加入到位姿图中,进行全局优化,获取优化后的位姿;根据优化后的位姿对特征点云进行变换构建全局增量式地图。
优选的,所述步骤S1具体包括以下过程:
利用激光雷达局部曲面点云的曲率进行特征提取,曲率的计算公式为:
Figure RE-GDA0003894505020000021
其中,c表示第k帧上第m个点云的曲率,S表示第k帧同一扫描线上第m个点的临近点数量,为了保证能正确反映点云的局部信息和实时运算速度,一般取10;
Figure RE-GDA0003894505020000022
表示第k帧上第m个和第n点云的坐标。
优选的,所述步骤S2具体包括以下过程:
S2.1生成全局特征子地图,具体包括:对于第一帧点云,直接存储在特征子地图中;对于第k(k≥2)帧点云,通过体素滤波对点云降采样,与上一帧生成的全局特征子地图匹配,利用匹配得到的位姿将第k帧点云加入到上一帧生成的全局特征子地图中,然后对全局子地图进行阈值划分,提取距激光雷达合适区域的点云;
S2.2若
Figure RE-GDA0003894505020000023
为角点点云,则通过点到直线的配准方式对相邻两帧的特征点进行配准,点到线的距离公式表示为:
Figure RE-GDA0003894505020000024
其中,
Figure RE-GDA0003894505020000025
表示激光雷达坐标系下第i个角点,
Figure RE-GDA0003894505020000026
表示激光雷达坐标系到全局坐标系的变换,通常设为单位矩阵,
Figure RE-GDA0003894505020000027
表示全局坐标系下第i个角点到直线的距离,
Figure RE-GDA0003894505020000028
表示激光雷达坐标系下第i个角点在全局特征子地图中找到的两个距离最近点云;
S2.2若
Figure RE-GDA0003894505020000029
为面点点云,则通过点到面的配准方式对相邻两帧的特征点进行配准,点到面的距离公式表示为:
Figure RE-GDA00038945050200000210
其中,
Figure RE-GDA00038945050200000211
表示激光雷达坐标系下第i个面点,
Figure RE-GDA00038945050200000212
表示激光雷达坐标系到全局坐标系的变换,通常设为单位矩阵,
Figure RE-GDA00038945050200000213
表示激光雷达坐标系下第i个面点到平面的距离,
Figure RE-GDA00038945050200000214
表示激光雷达坐标系下第i个角点在全局特征子地图中找到的三个距离最近点云;
S2.3根据激光雷达到点云的距离和点云的强度设计权重模型,公式表示为:
Figure RE-GDA00038945050200000215
Figure RE-GDA00038945050200000216
Figure RE-GDA00038945050200000217
Figure RE-GDA00038945050200000218
其中,wi表示参与点云匹配的特征点云i的总权重,
Figure RE-GDA0003894505020000031
表示点云i到线或面特征的距离权重因子,
Figure RE-GDA0003894505020000032
中ψ可取e或s,表示点云i到线或面特征的距离权重,
Figure RE-GDA0003894505020000033
表示点云i的距离权重因子,d表示点云i到线或面特征的距离,λ表示噪声阈值,取为0.05,
Figure RE-GDA0003894505020000034
表示点云i的强度权重因子,Ii表示点云i的强度,Ij表示点云i最邻域内的点云j的强度,Imax表示强度的最大距离,为定值255。
S2.4通过最小化点到直线与点到面的距离求取最优的全局位姿估计TW
Figure RE-GDA0003894505020000035
优选的,所述步骤S3具体包括以下过程:
S3.1将当前帧点云传入已有的Scan Context方法框架中,通过计算Scan Context生成全局描述子,选取与当前帧相似的候选帧;
S3.2为保证点云足够准确,仅使用激光雷达测量范围为80m内的点,对候选帧的点云进行径向和角向分割,径向分辨率设为1m,角向分辨率设为1°,生成360*80的激光雷达Iris 图像,然后对Iris图像进行傅里叶变换估计两副Iris图像之间的平移,使用Log-Gabor滤波器提取二值特征:
Figure RE-GDA0003894505020000036
其中G(f)表示滤波器的频率响应,f表示滤波器的中心频率,依据激光雷达Iris图像生成, f0、σ表示滤波器的参数,在Log-Gabor滤波器中提取图像Iris特征时,
Figure RE-GDA0003894505020000037
为定值,取0.75;
S3.3计算当前帧和候选帧之间激光雷达Iris二维特征图之间的汉明距离,如果小于一定阈值则可得到当前帧和可能的回环帧;
优选的,所述步骤S4具体包括以下过程:
S4.1将当前帧与回环帧执行步骤S2,计算它们之间的位姿变换矩阵:
Figure RE-GDA0003894505020000038
其中,
Figure RE-GDA0003894505020000039
表示当前帧在世界坐标系下的位姿;
Figure RE-GDA00038945050200000310
表示回环帧在世界坐标系下的位姿,
Figure RE-GDA00038945050200000311
表示当前帧到回环帧的位姿;
S4.2计算两帧之间位姿的平移值,小于一定阈值,则粗略检测回环成功:
Figure RE-GDA00038945050200000312
Figure RE-GDA00038945050200000313
其中,t表示
Figure RE-GDA0003894505020000041
中的平移矩阵,d表示当前帧与回环帧之间的平移值,P(l)作为二值函数判断是否回环成功;threhold设置为30,保证在有累计误差的情况下依然能粗略判断回环是否成功;
S4.3再将通过非线性优化求解器Ceres求解器得到的最后优化代价得分作为判断回环准则,用于精确检测回环成功:
Figure RE-GDA0003894505020000042
其中,scoref表示使用Ceres求解器获得的最后优化代价得分。
优选的,所述步骤S5具体包括以下过程:
将当前帧与回环帧间所有关键帧及位姿都添加到位姿图中,对位姿图采用因子图优化器 GTSAM进行全局优化,更新每一帧的位姿;利用更新的全局位姿修正每一帧的点云,获取全局一致的增量式地图。
相对于现有技术,本发明具有如下优点:
首先,本发明采用距离信息和强度信息对角点到直线的距离和面点到直线的距离进行加权,由于只采用曲率进行选特征点,存在特征点的好坏。当激光雷达移动较小的距离或较小的角度时,远处的特征点通常变化很大,所以考虑特征点距激光雷达的距离作为权重系数;除此之外,还考虑了激光雷达的强度信息,对于匹配的点云,强度越相近,匹配相似度越高;
其次,本发明利用激光雷达Iris特征实现了高效率的回环检测,保证了回环检测精度的同时,有效的提高了检测的效率;
再者,本发明使用在当前帧与回环帧间使用了基于特征的匹配方式,而不是采用传统耗时的ICP点云配准,能够有效的节约时间;对当前帧与回环帧之前的全部位姿采用了全局位姿优化;
最后,本发明在检测到可能的回环帧后,提出了两阶段判定回环检测方法。首先在对当前帧和可能的回环帧进行配准得到两帧之间的位姿后,利用一阶段粗检测方案,有效的去除实际很不符合回环检测的回环帧,只保留在当前帧一定范围内的回环帧;其次,利用二阶段精检测方案,通过Ceres求解器输出最后代价得分,在当前帧范围内找到正确的回环帧。
附图说明
图1为本发明的大范围场景下基于回环检测的激光雷达SLAM方法框图;
图2为本发明的大范围场景下基于回环检测的激光雷达SLAM方法流程图;
具体实施方式
下面结合附图和具体实施方式,进一步阐明本发明,应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
实施例1:本发明所述的一种大范围场景下基于回环检测的激光雷达SLAM方法,实现原理如图1所示,实验流程如图2所示。其流程主要包括以下步骤:
步骤S1,利用激光雷达局部曲面点云的曲率进行特征提取。
利用激光雷达局部曲面点云的曲率进行特征提取,曲率的计算公式为:
Figure RE-GDA0003894505020000051
其中,c表示第k帧上第m个点云的曲率,S表示第k帧同一扫描线上第m个点的临近点数量,为了保证能正确反映点云的局部信息和实时运算速度,一般取10;
Figure RE-GDA0003894505020000052
表示第k帧上第m个和第n点云的坐标。
步骤S2,根据点到直线和点到平面的配准方式对相邻两帧的特征点进行加权配准,获取激光雷达当前帧相对于初始帧的全局位姿;
具体包括以下过程:
生成全局特征子地图,具体包括:对于第一帧点云,直接存储在特征子地图中;对于第 k(k≥2)帧点云,通过体素滤波对点云降采样,与上一帧生成的全局特征子地图匹配,利用匹配得到的位姿将第k帧点云加入到上一帧生成的全局特征子地图中,然后对全局子地图进行阈值划分,提取距激光雷达合适区域的点云;
Figure RE-GDA0003894505020000053
为角点点云,则通过点到直线的配准方式对相邻两帧的特征点进行配准,点到线的距离公式表示为:
Figure RE-GDA0003894505020000054
其中,
Figure RE-GDA0003894505020000055
表示激光雷达坐标系下第i个角点,
Figure RE-GDA0003894505020000056
表示激光雷达坐标系到全局坐标系的变换,通常设为单位矩阵,
Figure RE-GDA0003894505020000057
表示全局坐标系下第i个角点到直线的距离,
Figure RE-GDA0003894505020000058
表示激光雷达坐标系下第i个角点在全局特征子地图中找到的两个距离最近点云;
Figure RE-GDA0003894505020000059
为面点点云,则通过点到面的配准方式对相邻两帧的特征点进行配准,点到面的距离公式表示为:
Figure RE-GDA00038945050200000510
其中,
Figure RE-GDA00038945050200000511
表示激光雷达坐标系下第i个面点,
Figure RE-GDA00038945050200000512
表示激光雷达坐标系到全局坐标系的变换,通常设为单位矩阵,
Figure RE-GDA0003894505020000061
表示激光雷达坐标系下第i个面点到平面的距离,
Figure RE-GDA0003894505020000062
表示激光雷达坐标系下第i个角点在全局特征子地图中找到的三个距离最近点云;
根据激光雷达到点云的距离和点云的强度设计权重模型,公式表示为:
Figure RE-GDA0003894505020000063
Figure RE-GDA0003894505020000064
Figure RE-GDA0003894505020000065
Figure RE-GDA0003894505020000066
其中,wi表示参与点云匹配的特征点云i的总权重,
Figure RE-GDA0003894505020000067
表示点云i到线或面特征的距离权重因子,
Figure RE-GDA0003894505020000068
中ψ可取e或s,表示点云i到线或面特征的距离权重,
Figure RE-GDA0003894505020000069
表示点云i的距离权重因子,d表示点云i到线或面特征的距离,λ表示噪声阈值,取为0.05,
Figure RE-GDA00038945050200000610
表示点云i的强度权重因子,Ii表示点云i的强度,Ij表示点云i最邻域内的点云j的强度,Imax表示强度的最大距离,为定值255。
通过最小化点到直线与点到面的距离求取最优的全局位姿估计TW
Figure RE-GDA00038945050200000611
步骤S3,将提取的特征点云和全局位姿传入回环检测模块,利用特征点云生成的全局描述子进行回环检测;
具体包括以下过程:
将当前帧点云传入已有的Scan Context方法框架中,通过计算Scan Context生成全局描述子,选取与当前帧相似的候选帧;
为保证点云足够准确,仅使用激光雷达测量范围为80m内的点,对候选帧的点云进行径向和角向分割,径向分辨率设为1m,角向分辨率设为1°,生成360*80的激光雷达Iris图像,然后对Iris图像进行傅里叶变换估计两副Iris图像之间的平移,使用Log-Gabor滤波器提取二值特征:
Figure RE-GDA00038945050200000612
其中G(f)表示滤波器的频率响应,f表示滤波器的中心频率,依据激光雷达Iris图像生成, f0、σ表示滤波器的参数,在Log-Gabor滤波器中提取图像Iris特征时,
Figure RE-GDA00038945050200000613
为定值,取0.75;
计算当前帧和候选帧之间激光雷达Iris二维特征图之间的汉明距离,如果小于一定阈值则可得到当前帧和可能的回环帧;
步骤S4,利用加权的特征匹配方法获得回环成功的两帧之间的位姿,判断回环成功;
将当前帧与回环帧执行步骤S2,计算它们之间的位姿变换矩阵:
Figure RE-GDA0003894505020000071
其中,
Figure RE-GDA0003894505020000072
表示当前帧在世界坐标系下的位姿;
Figure RE-GDA0003894505020000073
表示回环帧在世界坐标系下的位姿,
Figure RE-GDA0003894505020000074
表示当前帧到回环帧的位姿;
计算两帧之间位姿的平移值,小于一定阈值,则粗略检测回环成功:
Figure RE-GDA0003894505020000075
Figure RE-GDA0003894505020000076
其中,t表示
Figure RE-GDA0003894505020000077
中的平移矩阵,d表示当前帧与回环帧之间的平移值,P(l)作为二值函数判断是否回环成功;threhold设置为30,保证在有累计误差的情况下依然能粗略判断回环是否成功;
再将通过非线性优化求解器Ceres求解器得到的最后优化代价得分作为判断回环准则,用于精确检测回环成功:
Figure RE-GDA0003894505020000078
其中,scoref表示使用Ceres求解器获得的最后优化代价得分
步骤S5,将当前帧和回环帧之间的所有位姿加入到位姿图中,进行全局优化,获取优化后的位姿;根据优化后的位姿对特征点云进行变换构建全局增量式地图。
将当前帧与回环帧间所有关键帧及位姿都添加到位姿图中,对位姿图采用因子图优化器GTSAM进行全局优化,更新每一帧的位姿;利用更新的全局位姿修正每一帧的点云,获取全局一致的增量式地图。
本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段,还包括由以上技术特征任意组合所组成的技术方案。

Claims (6)

1.一种大范围场景下基于回环检测的激光雷达SLAM方法,其特征在于,所述方法包括以下步骤:
S1:利用激光雷达局部曲面点云的曲率进行特征提取;
S2:根据点到直线和点到平面的配准方式对相邻两帧的特征点进行加权配准,获取激光雷达当前帧相对于初始帧的全局位姿;
S3:将提取的特征点云和全局位姿传入回环检测模块,利用特征点云生成的全局描述子进行回环检测;
S4:利用加权的特征匹配方法获得回环成功的两帧之间的位姿,判断回环成功;
S5:将当前帧和回环帧之间的所有位姿加入到位姿图中,进行全局优化,获取优化后的位姿;根据优化后的位姿对特征点云进行变换构建全局增量式地图。
2.根据权利要求1所述的大范围场景下基于回环检测的激光雷达SLAM方法,其特征在于,所述步骤S1具体包括以下过程:
利用激光雷达局部曲面点云的曲率进行特征提取,曲率的计算公式为:
Figure RE-FDA0003894505010000011
其中,c表示第k帧上第m个点云的曲率,S表示第k帧同一扫描线上第m个点的临近点数量,为了保证能正确反映点云的局部信息和实时运算速度,S一般取10;
Figure RE-FDA0003894505010000012
表示第k帧上第m个和第n点云的坐标。
3.根据权利要求2所述的大范围场景下基于回环检测的激光雷达SLAM方法,其特征在于,所述步骤S2具体包括以下过程:
S2.1生成全局特征子地图,具体包括:对于第一帧点云,直接存储在特征子地图中;对于第k(k≥2)帧点云,通过体素滤波对点云降采样,与上一帧生成的全局特征子地图匹配,利用匹配得到的位姿将第k帧点云加入到上一帧生成的全局特征子地图中,然后对全局子地图进行阈值划分,提取距激光雷达合适区域的点云;
S2.2若
Figure RE-FDA0003894505010000013
为角点点云,则通过点到直线的配准方式对相邻两帧的特征点进行配准,点到线的距离公式表示为:
Figure RE-FDA0003894505010000014
其中,
Figure RE-FDA0003894505010000015
表示激光雷达坐标系下第i个角点,
Figure RE-FDA0003894505010000016
表示激光雷达坐标系到全局坐标系的位姿变换,初值为单位矩阵,
Figure RE-FDA0003894505010000021
表示全局坐标系下第i个角点到直线的距离,
Figure RE-FDA0003894505010000022
表示激光雷达坐标系下第i个角点在全局特征子地图中找到的两个距离最近点云;
S2.2若
Figure RE-FDA0003894505010000023
为面点点云,则通过点到面的配准方式对相邻两帧的特征点进行配准,点到面的距离公式表示为:
Figure RE-FDA0003894505010000024
其中,
Figure RE-FDA0003894505010000025
表示激光雷达坐标系下第i个面点,
Figure RE-FDA0003894505010000026
表示激光雷达坐标系到全局坐标系的变换矩阵,初值为单位矩阵,
Figure RE-FDA0003894505010000027
表示激光雷达坐标系下第i个面点到平面的距离,
Figure RE-FDA0003894505010000028
表示激光雷达坐标系下第i个角点在全局特征子地图中找到的三个距离最近点云;
S2.3根据激光雷达到点云的距离和点云的强度设计权重模型,公式表示为:
Figure RE-FDA0003894505010000029
Figure RE-FDA00038945050100000210
Figure RE-FDA00038945050100000211
Figure RE-FDA00038945050100000212
其中,wi表示参与点云匹配的特征点云i的总权重,
Figure RE-FDA00038945050100000213
表示点云i到线或面特征的距离权重因子,
Figure RE-FDA00038945050100000214
中ψ可取e或s,表示点云i到线或面特征的距离权重,
Figure RE-FDA00038945050100000215
表示点云i的距离权重因子,d表示点云i到线或面特征的距离,λ表示噪声阈值,取为0.05,
Figure RE-FDA00038945050100000216
表示点云i的强度权重因子,Ii表示点云i的强度,Ij表示点云i最邻域内的点云j的强度,Imax表示强度的最大距离,为定值255。
S2.4通过最小化点到直线与点到面的距离求取最优的全局位姿估计TW
Figure RE-FDA00038945050100000217
其中,ρ为鲁棒核函数,Nedge表示角点的数量,Nsurf表示面点的数量。
4.根据权利要求2所述的大范围场景下基于回环检测的激光雷达SLAM方法,其特征在于,所述步骤S3具体包括以下过程:
S3.1将当前帧点云传入已有的Scan Context方法框架中,通过计算Scan Context生成全局描述子,选取与当前帧相似的候选帧;
S3.2为保证点云足够准确,仅使用激光雷达测量范围为80m内的点,对候选帧的点云进行径向和角向分割,径向分辨率设为1m,角向分辨率设为1°,生成360*80的激光雷达Iris图像,然后对Iris图像进行傅里叶变换估计两副Iris图像之间的平移,使用Log-Gabor滤波器提取二值特征:
Figure RE-FDA0003894505010000031
其中G(f)表示滤波器的频率响应,f表示滤波器的中心频率,依据激光雷达Iris图像生成,f0、σ表示滤波器的参数,在Log-Gabor滤波器中提取图像Iris特征时,
Figure RE-FDA0003894505010000032
为定值,取0.75;
S3.3计算当前帧和候选帧之间激光雷达Iris二维特征图之间的汉明距离,如果小于一定阈值则可得到当前帧和可能的回环帧。
5.根据权利要求2所述的大范围场景下基于回环检测的激光雷达SLAM方法,其特征在于,所述步骤S4具体包括以下过程:
S4.1将当前帧与回环帧执行步骤S2,计算它们之间的位姿变换矩阵:
Figure RE-FDA0003894505010000033
其中,
Figure RE-FDA0003894505010000034
表示当前帧在世界坐标系下的位姿;Tl W表示回环帧在世界坐标系下的位姿,
Figure RE-FDA0003894505010000035
表示当前帧到回环帧的位姿;
S4.2计算两帧之间位姿的平移值,小于一定阈值,则粗略检测回环成功:
Figure RE-FDA0003894505010000036
Figure RE-FDA0003894505010000037
其中,t表示
Figure RE-FDA0003894505010000038
中的平移矩阵,d表示当前帧与回环帧之间的平移值,P(l)作为二值函数判断是否回环成功;threhold设置为30,保证在有累计误差的情况下依然能粗略判断回环是否成功;
S4.3再将通过非线性优化求解器Ceres求解器得到的最后优化代价得分作为判断回环准则,用于精确检测回环成功:
Figure RE-FDA0003894505010000039
其中,scoref表示使用Ceres求解器获得的最后优化代价得分。
6.根据权利要求2所述的大范围场景下基于回环检测的激光雷达SLAM,其特征在于,所述步骤S5具体包括以下过程:
将当前帧与回环帧间所有关键帧位姿都添加到位姿图中,对位姿图采用因子图优化器GTSAM进行全局优化;利用更新的全局位姿修正每一帧点云,获取全局一致的增量地图。
CN202210796243.5A 2022-07-07 2022-07-07 一种大范围场景下基于回环检测的激光雷达slam方法 Pending CN115343722A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210796243.5A CN115343722A (zh) 2022-07-07 2022-07-07 一种大范围场景下基于回环检测的激光雷达slam方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210796243.5A CN115343722A (zh) 2022-07-07 2022-07-07 一种大范围场景下基于回环检测的激光雷达slam方法

Publications (1)

Publication Number Publication Date
CN115343722A true CN115343722A (zh) 2022-11-15

Family

ID=83948644

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210796243.5A Pending CN115343722A (zh) 2022-07-07 2022-07-07 一种大范围场景下基于回环检测的激光雷达slam方法

Country Status (1)

Country Link
CN (1) CN115343722A (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116030212A (zh) * 2023-03-28 2023-04-28 北京集度科技有限公司 一种建图方法、设备、车辆及程序产品
CN116148808A (zh) * 2023-04-04 2023-05-23 江苏集萃清联智控科技有限公司 一种基于点云描述符的自动驾驶激光重定位方法和***
CN116299500A (zh) * 2022-12-14 2023-06-23 江苏集萃清联智控科技有限公司 一种融合目标检测和跟踪的激光slam定位方法及设备
CN116452422A (zh) * 2023-04-24 2023-07-18 上海几何伙伴智能驾驶有限公司 一种4d成像毫米波雷达的回环检测方法
CN116481517A (zh) * 2023-06-25 2023-07-25 深圳市普渡科技有限公司 扩展建图方法、装置、计算机设备和存储介质

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116299500A (zh) * 2022-12-14 2023-06-23 江苏集萃清联智控科技有限公司 一种融合目标检测和跟踪的激光slam定位方法及设备
CN116299500B (zh) * 2022-12-14 2024-03-15 江苏集萃清联智控科技有限公司 一种融合目标检测和跟踪的激光slam定位方法及设备
CN116030212A (zh) * 2023-03-28 2023-04-28 北京集度科技有限公司 一种建图方法、设备、车辆及程序产品
CN116148808A (zh) * 2023-04-04 2023-05-23 江苏集萃清联智控科技有限公司 一种基于点云描述符的自动驾驶激光重定位方法和***
CN116148808B (zh) * 2023-04-04 2024-01-26 江苏集萃清联智控科技有限公司 一种基于点云描述符的自动驾驶激光重定位方法和***
CN116452422A (zh) * 2023-04-24 2023-07-18 上海几何伙伴智能驾驶有限公司 一种4d成像毫米波雷达的回环检测方法
CN116452422B (zh) * 2023-04-24 2024-02-20 上海几何伙伴智能驾驶有限公司 一种4d成像毫米波雷达的回环检测方法
CN116481517A (zh) * 2023-06-25 2023-07-25 深圳市普渡科技有限公司 扩展建图方法、装置、计算机设备和存储介质
CN116481517B (zh) * 2023-06-25 2023-10-13 深圳市普渡科技有限公司 扩展建图方法、装置、计算机设备和存储介质

Similar Documents

Publication Publication Date Title
CN115343722A (zh) 一种大范围场景下基于回环检测的激光雷达slam方法
CN110261870B (zh) 一种用于视觉-惯性-激光融合的同步定位与建图方法
JP6760114B2 (ja) 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム
CN113706626B (zh) 一种基于多传感器融合及二维码校正的定位与建图方法
CN106056643B (zh) 一种基于点云的室内动态场景slam方法及***
CN110928301A (zh) 一种检测微小障碍的方法、装置及介质
CN111512317A (zh) 一种多目标实时跟踪方法、装置及电子设备
CN110084830B (zh) 一种视频运动目标检测与跟踪方法
JP7173471B2 (ja) 3次元位置推定装置及びプログラム
CN114972968A (zh) 基于多重神经网络的托盘识别和位姿估计方法
CN113989604B (zh) 基于端到端深度学习的轮胎dot信息识别方法
CN116468786B (zh) 一种面向动态环境的基于点线联合的语义slam方法
Ma et al. Binocular vision object positioning method for robots based on coarse-fine stereo matching
CN109459046B (zh) 悬浮式水下自主航行器的定位和导航方法
CN116309817A (zh) 一种基于rgb-d相机的托盘检测与定位方法
CN114118253B (zh) 一种基于多源数据融合的车辆检测方法及检测装置
CN113406658B (zh) 一种基于点线特征扫描匹配的移动机器人定位方法
WO2022110767A1 (zh) 定位与地图构建方法、装置、机器人及计算机存储介质
CN117115415B (zh) 基于大数据分析的图像标记处理方法和***
KR101594113B1 (ko) 스케일을 고려한 이미지 패치 추적 장치 및 방법
CN114789440B (zh) 基于图像识别的目标对接方法、装置、设备及其介质
JPH07146121A (ja) 視覚に基く三次元位置および姿勢の認識方法ならびに視覚に基く三次元位置および姿勢の認識装置
CN111239761B (zh) 一种用于室内实时建立二维地图的方法
CN114782689A (zh) 一种基于多帧数据融合的点云平面分割方法
CN112950787A (zh) 一种基于图像序列的目标物体三维点云生成方法

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