CN115240047A - 一种融合视觉回环检测的激光slam方法及系统 - Google Patents

一种融合视觉回环检测的激光slam方法及系统 Download PDF

Info

Publication number
CN115240047A
CN115240047A CN202210931448.XA CN202210931448A CN115240047A CN 115240047 A CN115240047 A CN 115240047A CN 202210931448 A CN202210931448 A CN 202210931448A CN 115240047 A CN115240047 A CN 115240047A
Authority
CN
China
Prior art keywords
point cloud
key frame
laser
loop
visual
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
CN202210931448.XA
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.)
China University of Mining and Technology Beijing CUMTB
Original Assignee
China University of Mining and Technology Beijing CUMTB
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 China University of Mining and Technology Beijing CUMTB filed Critical China University of Mining and Technology Beijing CUMTB
Priority to CN202210931448.XA priority Critical patent/CN115240047A/zh
Publication of CN115240047A publication Critical patent/CN115240047A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/77—Processing image or video features in feature spaces; using data integration or data reduction, e.g. principal component analysis [PCA] or independent component analysis [ICA] or self-organising maps [SOM]; Blind source separation
    • G06V10/80—Fusion, i.e. combining data from various sources at the sensor level, preprocessing level, feature extraction level or classification level
    • G06V10/806—Fusion, i.e. combining data from various sources at the sensor level, preprocessing level, feature extraction level or classification level of extracted features
    • 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/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • 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/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/08—Systems determining position data of a target for measuring distance only
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/762—Arrangements for image or video recognition or understanding using pattern recognition or machine learning using clustering, e.g. of similar faces in social networks
    • G06V10/763—Non-hierarchical techniques, e.g. based on statistics of modelling distributions
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Electromagnetism (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Software Systems (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Artificial Intelligence (AREA)
  • Health & Medical Sciences (AREA)
  • Computing Systems (AREA)
  • Databases & Information Systems (AREA)
  • Evolutionary Computation (AREA)
  • General Health & Medical Sciences (AREA)
  • Medical Informatics (AREA)
  • Multimedia (AREA)
  • Probability & Statistics with Applications (AREA)
  • Image Analysis (AREA)

Abstract

本发明涉及一种融合视觉回环检测的激光SLAM方法及系统，其方法包括：S1：通过3D激光雷达采集激光点云数据，并构建激光点云关键帧，利用激光点云关键帧获取当前定位信息；S2：将每一个激光点云关键帧中包含的点云注册到世界坐标系下后获取全局点云地图；S3：使用视觉相机采集视觉图像，并融合激光点云关键帧构建点云‑图像融合关键帧以检测回环信息，当检测到回环信息时，获取对应的回环约束及回环轨迹；S4：基于回环轨迹上的所有的点云‑图像融合关键帧建立位姿图模型，利用回环约束作为约束边对位姿图模型进行优化，得到优化后的点云‑图像融合关键帧以更新当前定位信息与全局点云地图。本发明提供的方法及系统可提高激光SLAM系统定位与建图的精度。

Description

一种融合视觉回环检测的激光SLAM方法及系统
技术领域
本发明涉及机器人技术领域，具体涉及一种融合视觉回环检测的激光SLAM方法及系统。
背景技术
同步定位与地图构建(SLAM)技术是指移动机器人通过传感器感知周围环境，估计自身位姿并且建立地图储存环境信息，完成对外部环境感知的过程，是移动机器人智能化的关键技术和先决条件。SLAM系统可分为前端里程计、回环检测、后端优化和地图构建四部分。
回环检测是指机器人再次经过同一地点时，检测到当前时刻与某历史时刻之间的场景一致性，并建立两时刻之间位姿状态的关联关系。机器人可根据当前位姿与历史位姿之间的联系来消除运行过程中的累积误差，获得更加精确的定位与建图结果。激光SLAM是前端应用激光雷达的SLAM系统，在激光SLAM中，待匹配历史帧的数量随系统运行而不断增长，激光回环检测算法的准确性与实时性难以同时保证，导致定位误差逐渐累积，进而影响系统定位与建图的精度。
发明内容
为了解决上述技术问题，本发明提供一种融合视觉回环检测的激光SLAM方法及系统。
本发明技术解决方案为：一种融合视觉回环检测的激光SLAM方法，包括：
步骤S1：激光里程计：通过3D激光雷达采集激光点云数据，提取所述激光点云数据的点云线特征与面特征，并以所述3D激光雷达的位移量为判断条件获取激光点云关键帧，同时将当前所述激光点云关键帧与局部点云地图、将所述局部点云地图与全局点云地图按照所述点云线特征与面特征进行匹配，以获取当前定位信息；
步骤S2：地图构建：将每一个所述激光点云关键帧中包含的点云注册到世界坐标系下，经点云降采样后得到更新后的全局点云地图；
步骤S3：视觉回环检测：使用视觉相机采集视觉图像，并融合所述激光点云关键帧构建点云-图像融合关键帧以检测回环信息，当检测到回环信息时，获取对应的回环约束及回环轨迹；
步骤S4：全局图优化：基于所述回环轨迹上的所有的所述点云-图像融合关键帧建立位姿图模型，将所述回环约束作为约束边添加到所述位姿图模型中，使用GTSAM库对所述位姿图模型进行优化得到优化后的点云-图像融合关键帧，并根据所述优化后的点云-图像融合关键帧更新当前所述定位信息与所述全局点云地图。
本发明与现有激光SLAM技术相比，具有以下优点：
本发明公开了一种融合视觉回环检测的激光SLAM方法及系统，将视觉图像结合到激光回环检测中，通过视觉图像的词袋模型可将包含大量信息的图像数据转化为词袋向量，极大地减少了场景匹配的计算量，提高了图像匹配效率，保证回环检测的实时性。最终将视觉回环应用到全局图优化中，减小了累积误差，提高激光SLAM系统定位与建图的精度。
附图说明
图1为本发明实施例中一种融合视觉回环检测的激光SLAM方法的流程图；
图2为本发明实施例中视觉词典的构建过程示意图；
图3A为本发明实施例中为无回环检测模块的LOAM建图结果示意图；
图3B为本发明实施例中是使用本发明提供的方法得到的建图结果示意图；
图4为本发明实施例中一种融合视觉回环检测的激光SLAM系统的结构框图。
具体实施方式
本发明提供了一种融合视觉回环检测的激光SLAM方法，减小了累积误差，提高激光SLAM系统定位与建图的精度。
为了使本发明的目的、技术方案及优点更加清楚，以下通过具体实施，并结合附图，对本发明进一步详细说明。
实施例一
如图1所示，本发明实施例提供的一种融合视觉回环检测的激光SLAM方法，包括下述步骤：
步骤S1：激光里程计：通过3D激光雷达采集激光点云数据，提取激光点云数据的点云线特征与面特征，并以3D激光雷达的位移量为判断条件获取激光点云关键帧，同时将当前激光点云关键帧与局部点云地图、将局部点云地图与全局点云地图按照点云线特征与面特征进行匹配，以获取当前定位信息；
步骤S2：地图构建：将每一个激光点云关键帧中包含的点云注册到世界坐标系下，经点云降采样后得到更新后的全局点云地图；
步骤S3：视觉回环检测：使用视觉相机采集视觉图像，并融合激光点云关键帧构建点云-图像融合关键帧以检测回环信息，当检测到回环信息时，获取对应的回环约束及回环轨迹；
步骤S4：全局图优化：基于回环轨迹上的所有的点云-图像融合关键帧建立位姿图模型，将回环约束作为约束边添加到位姿图模型中，使用GTSAM库对位姿图模型进行优化得到优化后的点云-图像融合关键帧，并根据优化后的点云-图像融合关键帧更新当前定位信息与全局点云地图。
在一个实施例中，上述步骤S1：激光里程计：通过3D激光雷达采集激光点云数据，提取激光点云数据的点云线特征与面特征，并以3D激光雷达的位移量为判断条件获取激光点云关键帧，同时将激光点云关键帧与局部点云地图、将局部点云地图与全局点云地图按照点云线特征与面特征进行匹配，以获取当前定位信息，具体包括：
首先，在车辆上安装3D激光雷达，可在一个工作周期旋转360°采集激光点云数据，并提取激光点云数据的点云线特征与面特征，同时，计算3D激光雷达运行时的位移量，当位移量超过预定的阈值时创建一帧激光点云关键帧，本发明中阈值设为0.5m，并在当前工作周期[tk,tk+1]的tk+1时刻发布该激光点云关键帧。
其次，将激光点云关键帧与局部点云地图进行特征匹配，以10Hz的频率进行位姿估计，实现了高频里程计粗定位功能；再将局部点云地图与全局点云地图以1Hz的频率进行匹配，实现低频里程计精定位功能，从而获取当前定位信息。
在一个实施例中，上述步骤S2：地图构建：本发明采用了基于关键帧的点云地图构建方法，即将步骤S1得到的每一个激光点云关键帧中包含的点云注册到世界坐标系下，经点云降采样后得到更新后的全局点云地图。
在一个实施例中，上述步骤S3中使用视觉相机采集视觉图像，并融合激光点云关键帧构建点云-图像融合关键帧，具体包括：
步骤S301：获取距离时刻tk+1最近的tp时刻的视觉图像作为视觉图像关键帧与tk+1时刻发布的激光点云关键帧进行关联，构成初始点云-图像融合关键帧，其中，tp在工作周期[tk,tk+1]之间；
由于激光雷达点云关键帧和视觉图像关键帧的采集设备不一样，因此二者的时间戳不一样，因此需要对初始点云-图像融合关键帧中所包含的所有激光点云数据通过线性插值方法进行运动补偿，使其在激光雷达坐标系下，与视觉图像关键帧时间戳进行统一，以实现激光点云关键帧与视觉图像关键帧的时间戳同步。
步骤S302：对[tk,tk+1]时间内初始点云-图像融合关键帧所包含的激光点云数据通过线性插值方法进行运动补偿，首先如公式(1)所示，计算初始点云-图像融合关键帧在ti时刻点云Xi在[tk,ti]之间的位姿变换Tk,i,同理可计算[tk,tp]之间的位姿变换Tk,p如公式(2)：
Figure BDA0003781717010000041
Figure BDA0003781717010000042
其中，Tk,k+1是激光点云数据在激光点云时间戳[tk,tk+1]之间的位姿变换，可由激光里程计直接获得；
将初始点云-图像融合关键帧中的点云Xi利用位姿变换Tk,i转换到tk时刻坐标下，得到点云Xk，再将点云Xk利用位姿变换Tk,p转换到视觉图像时间戳tp时刻下，得到与视觉图像的时间戳同步的Xp，实现了激光点云与视觉图像的时间同步，得到运动补偿后的点云-图像融合关键帧。
在一个实施例中，上述步骤S3中检测回环信息、获取对应的回环约束及回环轨迹具体包括：
步骤S311：选用ORB特征对已有的视觉图像样本集提取特征点集合，使用Kmeans算法对特征点集合进行聚类，将聚类结果作为单词并以树形数据结构存储，构建视觉词典；
本发明实施例使用DBoW3库构建视觉词典，选用具有高实时性的ORB特征对大量视觉图像样本进行特征提取，再使用Kmeans算法对特征点集合进行聚类，将聚类结果作为单词并以树形数据结构存储，实现视觉词典的构建。
步骤S312：对当前视觉图像关键帧进行ORB特征提取得到特征点，在视觉词典中查询特征点对应单词出现的词频，采用TF-IDF方法计算每个单词的分数，得到当前视觉图像关键帧的词袋向量Vk(v1,v2,···vn)；
如图2所示，展示了视觉词典的构建过程，以及利用视觉词典生成单幅视觉图像的词袋向量的过程。
步骤S313：遍历所有历史点云-图像融合关键帧，将其中历史点云-图像融合关键帧中所对应的视觉图像作为历史视觉图像关键帧，并获取所有历史视觉图像关键帧的词袋向量Vh(v1,v2,···vn)，并根据公式(3)～(4)计算Vk(v1,v2,···vn)与Vh(v1,v2,···vn)的曼哈顿距离S得到二者图像相似度η，选取相似度η最高且大于阈值T的历史帧作为回环关键帧：
Figure BDA0003781717010000051
Figure BDA0003781717010000052
其中，vi∈Vk，vj∈Vh，S(k,h)为Vk和Vh之间的曼哈顿距离用来表示词袋向量距离；S(k,k-1))表示当前视觉图像关键帧与前一视觉图像关键帧之间的词袋向量距离；
步骤S314：将检测到回环关键帧的点云-图像融合关键帧的点云特征点投影到视觉图像关键帧，得到视觉图像关键帧特征点二维像素坐标，采用三点估计的方法对二维像素坐标进行深度赋值，可得到当前视觉图像关键帧特征点的三维空间坐标；
步骤S315：利用特征点的三维空间坐标，将特征点与回环关键帧进行特征点匹配，应用PnP算法构建重投影误差模型，通过L-M算法优化求解式(x)最小二乘问题，可得到回环约束。
在一个实施例中，上述步骤S4：全局图优化：基于回环轨迹上的所有的点云-图像融合关键帧建立位姿图模型，将回环约束作为约束边添加到位姿图模型中，使用GTSAM库对位姿图模型进行优化得到优化后的点云-图像融合关键帧，并根据优化后的点云-图像融合关键帧更新当前定位信息与全局点云地图。
本发明实施例使用KITTI数据集的sequence_05对本发明提出的融合视觉回环检测的激光SLAM方法及系统进行实验评估，图3A为无回环检测模块的LOAM建图结果，在方框处建图结果与真实地图的道路点云不匹配，图3B为使用本发明提供的方法得到的建图结果，在方框处建图结果道路点云清晰，与真实地图的道路点云匹配良好。
此外，本发明实施还采用绝对位姿误差(Absolute Pose Error，APE)对轨迹精度进行分析，APE定义为实验估计位姿与真实位姿之差的绝对值，此项指标可以直观的衡量实验轨迹与真实轨迹的接近程度。从表1中可以看出，在数据集Sequence_05上使用APE统计指标，本发明相比于无回环检测模块的LOAM方法均方根误差减少了65.0％，误差平均值减少了59.4％，误差最大值减少了73.6％，因此，使用本发明方法得到的轨迹精度显著提高。
表1 APE统计指标对比(单位：m)
统计指标 LOAM方法 本发明方法
轨迹均方根误差 11.7699 4.1118
误差平均值 9.2648 3.7937
误差中位数 5.9568 3.9271
误差标准差 7.2589 1.5856
误差最大值 33.5559 8.8587
本发明公开了一种融合视觉回环检测的激光SLAM方法，将视觉图像结合到激光回环检测中，通过视觉图像的词袋模型可将包含大量信息的图像数据转化为词袋向量，极大地减少了场景匹配的计算量，提高了图像匹配效率，保证回环检测的实时性。最终将视觉回环应用到全局图优化中，减小了累积误差，提高激光SLAM系统定位与建图的精度。
实施例二
如图4所示，本发明实施例提供了一种融合视觉回环检测的激光SLAM系统，包括下述模块：
激光里程计模块51，用于通过3D激光雷达采集激光点云数据，提取激光点云数据的点云线特征与面特征，并以3D激光雷达的位移量为判断条件获取激光点云关键帧，同时将激光点云关键帧与局部点云地图、将局部点云地图与全局点云地图按照点云线特征与面特征进行匹配，以获取当前定位信息；
地图构建模块52，用于将每一个激光点云关键帧中包含的点云注册到世界坐标系下，经点云降采样后得到更新后的全局点云地图；
视觉回环检测模块53，用于使用视觉相机采集视觉图像，并融合激光点云关键帧构建点云-图像融合关键帧以检测回环信息，当检测到回环信息时，获取对应的回环约束及回环轨迹；
全局图优化模块54，用于基于回环轨迹上的所有的点云-图像融合关键帧建立位姿图模型，将回环约束作为约束边添加到位姿图模型中，使用GTSAM库对位姿图模型进行优化得到优化后的点云-图像融合关键帧，并根据优化后的点云-图像融合关键帧更新当前定位信息与全局点云地图。
提供以上实施例仅仅是为了描述本发明的目的，而并非要限制本发明的范围。本发明的范围由所附权利要求限定。不脱离本发明的精神和原理而做出的各种等同替换和修改，均应涵盖在本发明的范围之内。

Claims (5)

1.一种融合视觉回环检测的激光SLAM方法，其特征在于，包括：
步骤S1：激光里程计：通过3D激光雷达采集激光点云数据，提取所述激光点云数据的点云线特征与面特征，并以所述3D激光雷达的位移量为判断条件获取激光点云关键帧，同时将所述激光点云关键帧与局部点云地图、将所述局部点云地图与全局点云地图按照所述点云线特征与面特征进行匹配，以获取当前定位信息；
步骤S2：地图构建：将每一个所述激光点云关键帧中包含的点云注册到世界坐标系下，经点云降采样后得到更新后的全局点云地图；
步骤S3：视觉回环检测：使用视觉相机采集视觉图像，并融合所述激光点云关键帧构建点云-图像融合关键帧以检测回环信息，当检测到回环信息时，获取对应的回环约束及回环轨迹；
步骤S4：全局图优化：基于所述回环轨迹上的所有的所述点云-图像融合关键帧建立位姿图模型，将所述回环约束作为约束边添加到所述位姿图模型中，使用GTSAM库对所述位姿图模型进行优化得到优化后的点云-图像融合关键帧，并根据所述优化后的点云-图像融合关键帧更新当前所述定位信息与所述全局点云地图。
2.根据权利要求1所述的融合视觉回环检测的激光SLAM方法，其特征在于，所述步骤S1中以所述3D激光雷达的位移量为判断条件获取激光点云关键帧，具体包括：
计算所述3D激光雷达在运行时的位移量，当所述位移量超过阈值时，则建立一帧新的激光点云关键帧，并在当前工作周期[tk,tk+1]的tk+1时刻发布所述激光点云关键帧。
3.根据权利要求1所述的融合视觉回环检测的激光SLAM方法，其特征在于，所述步骤S3中使用视觉相机采集视觉图像，并融合所述激光点云关键帧构建点云-图像融合关键帧，具体包括：
步骤S301：获取距离时刻tk+1最近的tp时刻的视觉图像作为视觉图像关键帧与tk+1时刻发布的所述激光点云关键帧进行关联，构成初始点云-图像融合关键帧，其中，tp在工作周期[tk,tk+1]之间；
步骤S302：对[tk,tk+1]时间内所述初始点云-图像融合关键帧中所包含激光点云数据通过线性插值方法进行运动补偿，首先如公式(1)所示，计算所述初始点云-图像融合关键帧在ti时刻的点云Xi在[tk,ti]之间的位姿变换Tk,i,同理可计算[tk,tp]之间的位姿变换Tk,p如公式(2)：
Figure FDA0003781715000000021
Figure FDA0003781715000000022
其中，Tk,k+1是所述激光点云数据在激光点云时间戳[tk,tk+1]之间的位姿变换，可由激光里程计直接获得；
将所述初始点云-图像融合关键帧中的所有点云Xi利用位姿变换Tk,i转换到tk时刻坐标下，得到点云Xk，再将点云Xk利用位姿变换Tk,p转换到视觉图像时间戳tp时刻下，得到与视觉图像的时间戳同步的Xp，实现了点云与视觉图像的时间同步，得到运动补偿后的点云-图像融合关键帧。
4.根据权利要求1所述的融合视觉回环检测的激光SLAM方法，其特征在于，所述步骤S3中检测回环信息、获取对应的回环约束及回环轨迹，具体包括：
步骤S311：选用ORB特征对已有的视觉图像样本集提取特征点集合，使用Kmeans算法对所述特征点集合进行聚类，将聚类结果作为单词并以树形数据结构存储，构建视觉词典；
步骤S312：对当前所述视觉图像关键帧进行ORB特征提取得到特征点，在所述视觉词典中查询所述特征点对应单词出现的词频，采用TF-IDF方法计算每个所述单词的分数，得到当前所述视觉图像关键帧的词袋向量Vk(v1,v2,···vn)；
步骤S313：遍历所有历史点云-图像融合关键帧，将其中所述历史点云-图像融合关键帧中所对应的视觉图像作为历史视觉图像关键帧，并获取所有所述历史视觉图像关键帧的词袋向量Vh(v1,v2,···vn)，并根据公式(3)～(4)计算Vk(v1,v2,···vn)与Vh(v1,v2,···vn)的曼哈顿距离S得到二者图像相似度η，选取相似度η最高且大于阈值T的历史帧作为回环关键帧：
Figure FDA0003781715000000023
Figure FDA0003781715000000024
其中，vi∈Vk，vj∈Vh，S(k,h)为Vk和Vh之间的曼哈顿距离用来表示词袋向量距离；S(k,k-1))表示当前所述视觉图像关键帧与前一视觉图像关键帧之间的词袋向量距离；
步骤S314：将检测到所述回环关键帧的点云-图像融合关键帧的点云特征点投影到所述视觉图像关键帧，得到所述视觉图像关键帧特征点二维像素坐标，采用三点估计的方法对所述二维像素坐标进行深度赋值，可得到所述当前视觉图像关键帧特征点的三维空间坐标；
步骤S315：利用所述特征点的三维空间坐标，将所述特征点与所述回环关键帧进行特征点匹配，应用PnP算法构建重投影误差模型，通过L-M算法优化求解式(x)最小二乘问题，可得到回环约束。
5.一种融合视觉回环检测的激光SLAM系统，其特征在于，包括下述模块：
激光里程计模块，用于通过3D激光雷达采集激光点云数据，提取所述激光点云数据的点云线特征与面特征，并以所述3D激光雷达的位移量为判断条件获取激光点云关键帧，同时将所述激光点云关键帧与局部点云地图、将所述局部点云地图与全局点云地图按照所述点云线特征与面特征进行匹配，以获取当前定位信息；
地图构建模块，用于将每一个所述激光点云关键帧中包含的点云注册到世界坐标系下，经点云降采样后得到更新后的全局点云地图；
视觉回环检测模块，用于使用视觉相机采集视觉图像，并融合所述激光点云关键帧构建点云-图像融合关键帧以检测回环信息，当检测到回环信息时，获取对应的回环约束及回环轨迹；
全局图优化模块，用于基于所述回环轨迹上的所有的所述点云-图像融合关键帧建立位姿图模型，将所述回环约束作为约束边添加到所述位姿图模型中，使用GTSAM库对所述位姿图模型进行优化得到优化后的点云-图像融合关键帧，并根据所述优化后的点云-图像融合关键帧更新当前所述定位信息与所述全局点云地图。
CN202210931448.XA 2022-08-04 2022-08-04 一种融合视觉回环检测的激光slam方法及系统 Pending CN115240047A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210931448.XA CN115240047A (zh) 2022-08-04 2022-08-04 一种融合视觉回环检测的激光slam方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210931448.XA CN115240047A (zh) 2022-08-04 2022-08-04 一种融合视觉回环检测的激光slam方法及系统

Publications (1)

Publication Number Publication Date
CN115240047A true CN115240047A (zh) 2022-10-25

Family

ID=83678497

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210931448.XA Pending CN115240047A (zh) 2022-08-04 2022-08-04 一种融合视觉回环检测的激光slam方法及系统

Country Status (1)

Country Link
CN (1) CN115240047A (zh)

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115661299A (zh) * 2022-12-27 2023-01-31 安徽蔚来智驾科技有限公司 车道线地图的构建方法、计算机设备及存储介质
CN115797490A (zh) * 2022-12-19 2023-03-14 广州宸境科技有限公司 基于激光视觉融合的建图方法及系统
CN115880364A (zh) * 2023-02-09 2023-03-31 广东技术师范大学 基于激光点云和视觉slam的机器人位姿估计方法
CN116698007A (zh) * 2023-05-26 2023-09-05 深圳市普渡科技有限公司 融合地图构建方法、装置、机器人和存储介质
CN116958842A (zh) * 2023-09-19 2023-10-27 北京格镭信息科技有限公司 基于激光-视觉融合的地下管线的巡检方法及装置
CN117237553A (zh) * 2023-09-14 2023-12-15 广东省核工业地质局测绘院 一种基于点云图像融合的三维地图测绘系统
CN117387639A (zh) * 2023-09-22 2024-01-12 成都睿芯行科技有限公司 一种基于激光salm的地图更新系统及其方法
CN117470106A (zh) * 2023-12-27 2024-01-30 中铁四局集团第二工程有限公司 狭小空间点云绝对数据采集方法以及模型建立设备
CN117968667A (zh) * 2024-04-02 2024-05-03 国网江苏省电力有限公司常州供电分公司 巡检机器人回环检测的slam点云地图构建方法及系统
CN118155039A (zh) * 2024-05-13 2024-06-07 齐鲁空天信息研究院 多源信息融合的定位地图构建方法、装置、介质及设备

Cited By (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115797490A (zh) * 2022-12-19 2023-03-14 广州宸境科技有限公司 基于激光视觉融合的建图方法及系统
CN115661299A (zh) * 2022-12-27 2023-01-31 安徽蔚来智驾科技有限公司 车道线地图的构建方法、计算机设备及存储介质
CN115661299B (zh) * 2022-12-27 2023-03-21 安徽蔚来智驾科技有限公司 车道线地图的构建方法、计算机设备及存储介质
CN115880364A (zh) * 2023-02-09 2023-03-31 广东技术师范大学 基于激光点云和视觉slam的机器人位姿估计方法
CN116698007A (zh) * 2023-05-26 2023-09-05 深圳市普渡科技有限公司 融合地图构建方法、装置、机器人和存储介质
CN117237553A (zh) * 2023-09-14 2023-12-15 广东省核工业地质局测绘院 一种基于点云图像融合的三维地图测绘系统
CN116958842A (zh) * 2023-09-19 2023-10-27 北京格镭信息科技有限公司 基于激光-视觉融合的地下管线的巡检方法及装置
CN116958842B (zh) * 2023-09-19 2024-01-05 北京格镭信息科技有限公司 基于激光-视觉融合的地下管线的巡检方法及装置
CN117387639A (zh) * 2023-09-22 2024-01-12 成都睿芯行科技有限公司 一种基于激光salm的地图更新系统及其方法
CN117470106A (zh) * 2023-12-27 2024-01-30 中铁四局集团第二工程有限公司 狭小空间点云绝对数据采集方法以及模型建立设备
CN117470106B (zh) * 2023-12-27 2024-04-12 中铁四局集团有限公司 狭小空间点云绝对数据采集方法以及模型建立设备
CN117968667A (zh) * 2024-04-02 2024-05-03 国网江苏省电力有限公司常州供电分公司 巡检机器人回环检测的slam点云地图构建方法及系统
CN118155039A (zh) * 2024-05-13 2024-06-07 齐鲁空天信息研究院 多源信息融合的定位地图构建方法、装置、介质及设备

Similar Documents

Publication Publication Date Title
CN115240047A (zh) 一种融合视觉回环检测的激光slam方法及系统
CN111258313B (zh) 多传感器融合slam系统及机器人
CN112634451B (zh) 一种融合多传感器的室外大场景三维建图方法
CN111563442B (zh) 基于激光雷达的点云和相机图像数据融合的slam方法及系统
CN110261870B (zh) 一种用于视觉-惯性-激光融合的同步定位与建图方法
CN110389348B (zh) 基于激光雷达与双目相机的定位与导航方法及装置
CN110726409B (zh) 一种基于激光slam和视觉slam地图融合方法
CN111275763B (zh) 闭环检测系统、多传感器融合slam系统及机器人
CN109100730B (zh) 一种多车协同快速建图方法
CN111060924B (zh) 一种slam与目标跟踪方法
CN113223045B (zh) 基于动态物体语义分割的视觉与imu传感器融合定位系统
CN114323033B (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN110487286B (zh) 基于点特征投影与激光点云融合的机器人位姿判断方法
WO2024114119A1 (zh) 一种基于双目相机引导的传感器融合方法
CN113447014A (zh) 室内移动机器人、建图方法、定位方法以及建图定位装置
CN113689393A (zh) 一种基于图像和点云实例匹配的三维目标检测算法
CN113959437A (zh) 一种用于移动测量设备的测量方法及系统
CN116953723A (zh) 机车检修车间环境中移动机器人即时定位与地图构建方法
CN116429116A (zh) 一种机器人定位方法及设备
CN113838129B (zh) 一种获得位姿信息的方法、装置以及系统
CN113379915B (zh) 一种基于点云融合的行车场景构建方法
CN113536959A (zh) 一种基于立体视觉的动态障碍物检测方法
CN116862832A (zh) 一种基于三维实景模型的作业人员定位方法
CN115773759A (zh) 自主移动机器人的室内定位方法、装置、设备及存储介质
CN115457497A (zh) 一种基于3d目标检测和多目标追踪检测车辆速度的方法

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