CN115496792A - 基于人在环优化的点云语义slam的方法、系统及装置 - Google Patents

基于人在环优化的点云语义slam的方法、系统及装置 Download PDF

Info

Publication number
CN115496792A
CN115496792A CN202211160212.7A CN202211160212A CN115496792A CN 115496792 A CN115496792 A CN 115496792A CN 202211160212 A CN202211160212 A CN 202211160212A CN 115496792 A CN115496792 A CN 115496792A
Authority
CN
China
Prior art keywords
point cloud
semantic
optimization
map
cloud semantic
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
CN202211160212.7A
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.)
Hangzhou Innovation Research Institute of Beihang University
Original Assignee
Hangzhou Innovation Research Institute of Beihang 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 Hangzhou Innovation Research Institute of Beihang University filed Critical Hangzhou Innovation Research Institute of Beihang University
Priority to CN202211160212.7A priority Critical patent/CN115496792A/zh
Publication of CN115496792A publication Critical patent/CN115496792A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/50—Depth or shape recovery
    • G06T7/55—Depth or shape recovery from multiple images
    • G06T7/579—Depth or shape recovery from multiple images from motion
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • 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/20—Image preprocessing
    • G06V10/26—Segmentation of patterns in the image field; Cutting or merging of image elements to establish the pattern region, e.g. clustering-based techniques; Detection of occlusion
    • 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/774—Generating sets of training patterns; Bootstrap methods, e.g. bagging or boosting
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/60—Type of objects
    • G06V20/64—Three-dimensional objects
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Multimedia (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Artificial Intelligence (AREA)
  • Computing Systems (AREA)
  • Databases & Information Systems (AREA)
  • Evolutionary Computation (AREA)
  • General Health & Medical Sciences (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Health & Medical Sciences (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种基于人在环优化的机器人点云语义SLAM方法、系统及装置，其中，该方法包括：S1、移动机器人上的激光雷达对目标地点进行数据采集，得到每帧点云的相对位姿信息，完成建图；S2、建立点云连续轨迹，人工观测建图误差，构建稠密因子图，然后基于因子图进行全局优化，调整总体点云错误；S3、基于预定义的点云语义类别并采用标注工具进行叠加地图的点云语义标注，累计点云语义标记结果得到点云语义分割数据集；S4、基于点云语义分割数据集进行分割模型的训练和评估得到训练和评估后的分割模型；S5、基于训练和评估后的分割模型，进行点云语义定位与优化。采用本发明可以实现点云语义定位与优化。

Description

基于人在环优化的点云语义SLAM的方法、系统及装置
技术领域
本发明涉及点云语义SLAM优化的领域，尤其是涉及一种基于人在环优化的点云语义SLAM的方法、系统及装置。
背景技术
随着技术的快速发展，移动机器人的相关应用正从研究阶段逐渐进入日常生活。特别是在抗击Covid-19期间，移动机器人承担了物资配送、消毒、清洁服务等一系列非接触式任务。
移动机器人对环境的自主感知和定位是保证下游任务的最基本功能。目前主流的机器人自主定位技术包括同步定位与建图(simultaneous localization and mapping，SLAM)、超宽带(Ultra-Wide Bandwith，UWB)、轮速计、GPS和多传感器融合方案等。其中，GPS在遮挡环境或室内定位失效；UWB需要额外的施工部署和相当的设备密度；车轮速度计易受地面打滑和车轮形变影响，还需要对机器人底盘进行动力建模，旋转姿态的估计精度也较差。基于开环控制的SLAM算法对设备的依赖性较小，具有更广泛的稳定性和适用的场景的适应性。
现有的SLAM算法大致可以分为基于图像和点云SLAM两类。基于主动感知的激光雷达不受环境光的影响，可以在黑暗的环境中正常使用，这为移动机器人在极端苛刻光照环境的使用提供了可靠的保障。基于LiDAR 的SLAM技术可以为移动机器人提供定位信息和稳定、高保真的环境稠密地图点云，是移动机器人应用部署的关键模块之一。然而，激光雷达点云的稀疏性、随机性和不稳定的数据结构使得配准特征筛选和匹配的难度比图像更复杂，从而会导致计算复杂和引入了累积误差，导致点云图的退化，和全局一致性较差的地图。从而影响基于点云地图的一系列下游应用任务的开展。
现有技术虽然通过对SLAM系统的前端和后端进行了一系列的优化，但总体方案主要依赖低维几何特征，如点线面等，缺乏理解环境语义信息的类人认知模式。此外，针对点云稀疏的低成本的激光雷达点云数据的注释也是极具挑战性，基于语义的SLAM优化方案又受限于对应场景的数据集支持，使得包括深度学习建模、语义SLAM优化在内的技术发展十分缓慢。
发明内容
本发明的目的在于提供一种基于人在环优化的点云语义SLAM的方法、系统及装置，旨在解决点云语义SLAM优化。
本发明提供一种基于人在环优化的点云语义SLAM的方法，包括：
S1、通过移动机器人上的采集模块对目标地点进行数据采集，在采集过程中将SLAM算法作为初始建图算法计算得到每帧点云的相对位姿信息，完成建图；
S2、建立点云连续轨迹，获取人工观测的建图误差信息，基于所述建图误差信息选择建图误差周围正确的点云帧重新进行局部ICP的配准优化，获取关键位姿点之间的闭环，构建稠密因子图，然后基于所述稠密因子图进行全局优化，调整总体建图错误，迭代配准优化和全局优化；
S3、将进行局部ICP配准优化后的点云位姿和优化后的点云位姿对应点云导入标注工具，基于预定义的点云语义类别，采用标注工具进行叠加地图的点云语义标注，累计点云语义标记结果得到点云语义分割数据集；
S4、基于点云语义分割数据集进行点云语义分割模型的训练和评估得到训练和评估后的点云语义分割模型；
S5、基于训练和评估后的分割模型，进行点云语义定位与优化。
本发明还提供一种基于人在环优化的点云语义SLAM的系统，包括：
采集模块，安装于移动机器人，用于对目标地点进行数据采集，在采集过程中将SLAM算法作为初始建图算法计算得到每帧点云的相对位姿信息，完成建图；
优化模块，用于建立连续轨迹，获取人工观测的建图误差信息，基于所述建图误差信息选择建图误差周围正确的点云帧重新进行局部ICP的配准优化，获取关键位姿点之间的闭环，构建稠密因子图，然后基于所述稠密因子图进行全局优化，调整总体建图错误，迭代配准优化和全局优化；
数据集模块，用于将进行局部ICP配准优化后的点云位姿和优化后的点云位姿对应点云导入标注工具，基于预定义的点云语义类别，采用标注工具进行叠加地图的点云语义标注，累计点云语义标记结果得到点云语义分割数据集；
分割模型模块，用于基于点云语义分割数据集进行分割模型的训练和评估得到训练和评估后的分割模型；
定位优化模块，用于基于训练和评估后的分割模型，进行点云语义定位与优化。
本发明实施例还提供一种基于人在环优化的点云语义SLAM的装置，包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现上述方法的步骤。
本发明实施例还提供一种计算机可读存储介质，所述计算机可读存储介质上存储有信息传递的实现程序，所述程序被处理器执行时实现上述方法的步骤。
采用本发明实施例，在没有GPS定位信息或测绘级激光跟踪仪的情况下，快速高效地构建用于语义信息采集的激光雷达点云数据，完善机器人的姿势和最终地图。
在人类认知的帮助下，基于累积的点云地图进行批处理语义标注，提高了贴标签的效率。
上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，依照说明书的内容予以实施，并且为了让本发明的上述和其它目的、特征和优点能够更明显易懂，以下特举本发明的具体实施方式。
附图说明
为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明实施例的基于人在环优化的机器人点云语义SLAM方法的流程图；
图2是本发明实施例的基于人在环优化的机器人点云语义SLAM方法的工作流程图；
图3是本发明实施例的基于人在环优化的机器人点云语义SLAM方法的机器人示意图；
图4是本发明实施例的基于人在环优化的机器人点云语义SLAM方法的地下车库建图结果和对应的地面建筑示意图；
图5是本发明实施例的基于人在环优化的机器人点云语义SLAM方法的人工修正前点云地图存在的误差示意图；
图6是本发明实施例的基于人在环优化的机器人点云语义SLAM方法人工修正后点云地图示意图；
图7是本发明实施例的基于人在环优化的机器人点云语义SLAM方法的地库语义标注类别定义示意图；
图8是本发明实施例的基于人在环优化的机器人点云语义SLAM方法的地图语义标注界面示意图，
图9是本发明实施例的基于人在环优化的机器人点云语义SLAM方法的标注后的全局点云语义示意图；
图10是本发明实施例的基于人在环优化的机器人点云语义SLAM方法的测试示意图；
图11是本发明实施例的基于人在环优化的机器人点云语义SLAM方法的前后端优化示意图；
图12是本发明实施例的基于人在环优化的机器人点云语义SLAM方法的蒙特卡罗粒子滤波的全局粗定位示意图；
图13是本发明实施例的基于人在环优化的机器人点云语义SLAM方法的三种SLAM算法在引入语义优化前后的建图质量对比示意图；
图14是本发明实施例的基于人在环优化的机器人点云语义SLAM系统的示意图；
图15是本发明实施例的基于人在环优化的机器人点云语义SLAM装置的示意图。
具体实施方式
下面将结合实施例对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
方法实施例
根据本发明实施例，提供了一种基于人在环优化的机器人点云语义 SLAM方法，图1是本发明实施例的基于人在环优化的机器人点云语义 SLAM方法的流程图，如图1所示，具体包括：
本发明提出的一种基于人在环优化的机器人点云语义SLAM方法，具体包括：
S1、移动机器人上的激光雷达对目标地点进行数据采集，在采集过程中基于SLAM算法作为初始建图算法得到每帧点云的相对位姿信息，建立地图；
S2、建立连续轨迹，人工观测建图误差后，选择建图误差周围正确的点云帧进行局部ICP的配准优化，同时将闭环边添加到相邻位姿点，构建稠密因子图，然后基于因子图进行全局优化，调整总体点云错误；
S2具体包括：通过将连续点云和连续点云对应的推算位姿进行地图叠加，基于点云数据的采样顺序对每帧点云的中心点进行关联，形成连续轨迹，基于轨迹计算闭环边，并添加所述闭环边构建新闭环；
获取人工观测的建图误差信息，基于所述建图误差信息选择建图误差周围正确的点云帧进行局部配准算法的配准优化；获取关键位姿点之间的闭环，构建稠密因子图，然后基于因子图进行全局优化，调整总体建图错误，迭代所述配准优化和全局优化。
S3、将优化后的点云位姿和对应点云导入Point Labeler标注工具，基于预定义的点云语义类别进行叠加地图的语义标注，累计点云标记结果得到点云语义分割数据集；
S4、基于点云语义分割数据集进行分割模型的训练和评估得到训练和评估后的分割模型；
S4具体包括：将点云语义分割数据集分为训练集和测试集，基于训练集和测试集对基于深度学习的点云语义分割模型进行模型训练和评估，并保留总体分割精度高于某个阈值的模型参数作为实时点云语义分割的模型。
S5、基于训练和评估后的分割模型，进行点云语义定位与优化。
S5具体包括：
将点云发送到实时点云语义分割的模型得到点分割的语义标签，然后将一个单帧点云n×[x，y，z]使用基于激光雷达笛卡尔到极坐标投影公式投影到一个x-o-y平面，对xy轴进行极坐标转化得到2D特征图，基于预定义的点云语义配色进行2D特征图着色，得到2D点云语义特征图；
获得第一帧点云后，基于点云语义信息删除每帧点云的动态语义点，然后快速估计2D位姿，以2D位姿作为初始解估计3D点云位姿，进行SLAM 的3D点云地图构建和2D语义特征地图构建，周期性基于蒙特卡罗粒子滤波对当前扫描点云的2D点云特征图进行全局2D语义特征地图的粗粒度定位；
获取移动机器人在全局点云地图上的初始位置，对里程计前端估计进行修正，对于前端里程计估计时的帧间点云配准，使用语义标签进行类间分隔和类内匹配，实现点云搜索的加速；
同时，检测闭环，当发现新闭环则进行后端的因子图优化，更新全局位姿信息。
具体实施方法如下：
图2是本发明实施例的基于人在环优化的机器人点云语义SLAM方法的工作流程图；如图2所示：
首先，通过手动控制移动机器人在目标场景进行点云数据采集。使用轻量级的SLAM作为初始建图算法。并使用图优化作为SLAM后端优化，轻量级算法可以部署在机器人嵌入式计算单元中，但在机器人旋转速度过大和地面打滑等情况下，里程计的漂移误差较大，且容易在场景相似度高的情况出现退化。因此，获得带有噪声的点云PC和对应的六自由度位姿 [x,y,z,roll,pitch,yaw]序列。
然后，引入离线人在环的交互式SLAM优化。通过将连续点云和对应的推算位姿进行地图叠加，基于顺序将每帧点云的中心点进行关联，形成连续轨迹。人工观测地图寻找点云地图中的建图误差，一般为具有重影的墙面、非平滑的地面和立柱等。通过选择地图误差附近的位姿点的点云，与近邻位姿点云进行局部ICP的精确配准来缓解建图误差。并通过人工选择关键位姿点之间的闭环，或者设置对应的闭环条件(如：相邻帧中心的累积距离大于10m以上，但是空间距离小于3m)生成闭环，进行后端因子图优化。整个过程需要不断的人工循环迭代优化，直到地图中不存在显著可观测的建图错误，具体的优化时间取决于场景的大小和采集数据区域的规模等因素。
接着，基于优化后的点云和位姿序列叠加地图，生成稠密的点云场景信息。通过人工标注的方式框取点云，并标注对应的语义标签信息。通过叠图标注提高了标注效率且减少误差。利用所构建的数据集接着训练点云分割模型来提取语义标签。将标注结果重新拆散为单帧点云，即可获得精准的单帧点云语义信息，重复多次，构建面向点云语义分割的数据集，进行点云语义分割的深度学习模型训练和评估，获得用于实时分割的神经网络模型权重。
最后，基于实时点云和分割得到的语义标签，进行语义SLAM定位的优化。该算法利用点云语义标签信息对移动机器人的SLAM分别进行前后端的优化。
相应地，本发明提供一种基于人在环优化的机器人点云语义SLAM方法，包括以下步骤：
图3是本发明实施例的基于人在环优化的机器人点云语义SLAM方法的机器人示意图；
包含安装在机器人顶部的低成本激光雷达、显示器和计算单元，以及移动机器人底盘。
图4是本发明实施例的基于人在环优化的机器人点云语义SLAM方法的地下车库建图结果和对应的地面建筑示意图；
测试部署的目标环境如图4所示，为一个典型的科研园区的中大型地下车库，该场景内无GPS信号，以及其它辅助定位的基础设施。
步骤1，基于人在环交互的点云语义分割数据采集和标注。首先，通过手动控制移动机器人在目标场景遍历移动采集数据，通过多次采集不同日期的数据来丰富数据中移动目标的多样性。期间可使用任意点云SLAM算法作为初始建图算法，估计每帧点云的相对位姿信息，但存在建图误差。
图5是本发明实施例的基于人在环优化的机器人点云语义SLAM方法的人工修正前点云地图存在的误差示意图；从图中放大的区域可以看到所构建的地图包含了大量的累积偏移和误差。
步骤2，离线人机交互式SLAM位姿校正。当场景范围较大、存在相似结构时，机器人在运动过程中会产生姿态估计偏差，导致最终点云地图畸变，例如，对于长廊，直线会畸变为弧线、退化；对于人造建筑，存在过度厚墙，墙壁重影等畸变现象。此外，一些大的姿态偏移或长期累积误差会导致闭环检测失效。通过人工观测地图中点云的偏差，对上述问题进行发现并修正。具体包括局部ICP调优和闭环优化。其中，局部ICP调优通过人工观测存在建图偏差的区域附近的位姿帧，并依次选择与周围正确的点云帧(作为基准)进行局部ICP配准，估算优化后的位姿进行更新。该步骤能够有效的缓解自动执行时遗漏或优化结果较差的局部数据。最后，通过人工选择任意相邻交近的数据帧添加关联边，或者基于场景大小设置合适的闭环条件，进行自动闭环关联边的添加，构建稠密因子图，然后基于因子图进行全局优化，调整总体点云错误。图6是本发明实施例的基于人在环优化的机器人点云语义SLAM方法人工修正后点云地图示意图；墙面清晰无重影。
本步骤需要不断的人工循环迭代优化，具体的优化时间取决于场景的大小等因素。
图7是本发明实施例的基于人在环优化的机器人点云语义SLAM方法的地库语义标注类别定义示意图；
针对地下车库场景定义所需标注的类别，如图7所示，包括不可辨识的噪点，车辆、摩托车、行人、地面、承重柱、墙面和天花板。
步骤3，场景点云语义人工标注。基于对应部署场景构建的SLAM地图结果定义标注的语义类别，包含一个额外的未定义类别处理无法判断的点。将优化后的点云位姿和对应点云导入Point Labeler标注工具，基于预定义的点云语义类别进行叠加地图的语义标注。由于叠加了大量点云，场景的语义实体可辨识度极大提高，能够缓解由于稀疏点云数量较少的情况下导致的标注错误。同时，通过叠加地图的形式进行标注，极大的提高了标注效率。
图8是本发明实施例的基于人在环优化的机器人点云语义SLAM方法的地图语义标注界面示意图；
图8所示，为机器人所在的地下车库环境，和对应点云地图在标注工具图形界面的示意。
图9是本发明实施例的基于人在环优化的机器人点云语义SLAM方法的标注后的全局点云语义示意图；
通过累积一定规模的点云标注结果构建语义分割数据集，而叠加的点云语义地图，如图9所示：
将多个采集序列的点云标注结果进行合并，构建一个规模满足后续模型训练的点云语义分割数据集。
步骤4，训练点云分割模型。将步骤3标注后的数据集划分为两个不相交的子集，一个用于模型训练，另一个用于模型评估。基于训练集和测试集对基于深度学习的点云语义分割模型进行模型训练和评估，并保留总体分割精度较好的模型参数作为后续实时点云语义分割的模型。训练过程是在一个GPU服务器进行，使用Inteli7-9700CPU，16G内存，硬盘和一个 NVIDIA 2080ti GPU。设置batch＝4，开始使用Adam优化器以0.0001学习率进行训练，并训练所有模型80次。
图10是本发明实施例的基于人在环优化的机器人点云语义SLAM方法的测试示意图；
测试过程选择了RandlaNet、PolarSeg和Cyclinder3D三个模型基于地下车库数据进行训练和测试，评估结果如图8所示，综合考虑速度和分割 mIoU精度选择PolarSeg作为实时的分割模型在机器人计算单元进行部署。
步骤5，基于语义优化的SLAM算法。该步骤有以下几个流程：
流程1.通过点云语义分割将类人空间语义认知能力封装到系统中，对实时获取的每帧点云进行语义分割。同时，基于标注的地图语义进行体素化降采样，获得全局地图的均匀语义地图信息，用于全局定位。在预处理时对点云地图进行离线法向计算，并基于语义标签生成局部栅格地图表征。
图11是本发明实施例的基于人在环优化的机器人点云语义SLAM方法的前后端优化示意图；
流程2.从前端不稳定点云过滤和后端闭环配准优化两方面对现有 SLAM系统进行认知优化。前端里程计估计帧间配准计算时，剔除动态和潜在动态(停止车辆)，优先考虑使用静态目标语义信息进行配准；并通过类内约束来提升特征间的搜索效率。后端闭环检测时，针对t时刻的一帧语义点云进行近邻搜索，构建基于语义的2D环境帧极坐标编码(Semantic Scan Context,SSC)进行粗位姿检测获得[R；t]矩阵的[x；y；yaw]精确信息，作为初值加速ICP的精确配准，一方面提升单帧配准精度，同时提升闭环检测成功率。
图12是本发明实施例的基于人在环优化的机器人点云语义SLAM方法的蒙特卡罗粒子滤波的全局粗定位示意图；
流程3.基于粒子滤波的重定位。在初始启动和每隔一段时间重新定位时进行全局定位纠偏，通过全局栅格地图进行全局均匀的种子撒点，并基于蒙特卡罗定位的粒子滤波器进行粗定位估计。然后基于当前扫描帧点云的语义极坐标编码和地图的语义极坐标编码的相似性，推断出机器人的当前位置。作为初始解提供给步骤2中的前端里程计，用于后续计算。
基于点云进行极坐标投影，生成极坐标语义环境编码，进行帧间的位姿粗估计，然后基于初始解进行ICP或者NDT的精确位姿估计。然后，检测闭环，当发现新闭环则进行后端的因子图优化，更新全局位姿信息；
每隔一段时间，对机器人的位置进行基于蒙特卡罗的粒子滤波全局定位，周期性修正累积误差；
针对流程1，将人类对点云的语义认知能力封装到地图和每帧点云中。其操作如下，每次机器人得到一个新的扫描，首先将点云输入到深度学习分割模型，得到点方向的标签。然后将一个单帧点云n×[x，y，z]，使用基于激光雷达笛卡尔到极坐标投影公式，投影到一个二维地图[W，H]上。投影公式如下：
Figure BDA0003859462980000131
其中，
Figure BDA0003859462980000132
为点的距离，Fv＝Fovup+Fovdown＝30°为垂直角，Hscan＝2°是垂直分辨率，Hr＝Fovv/Hscan+1＝16，Wr＝18000是激光雷达的水平角度分辨率。Fup＝Fovup；r就是半径xyz是点云笛卡尔坐标，F＝Fov， Fov是视场角的缩写，Hscan为垂直角分辨率；Fv＝Fovup+Fovdown＝30°表示垂直分视场角上下相加等于30°。
Hr＝30/2+1＝16是雷达垂直beam数量；
wr＝18000是水平角分辨率为0.02度情况下的扫描采样点数量；
针对流程2，每次机器人初始启动或需要每隔一段时间重新定位时，粒子通过基于粒子滤波器的蒙特卡罗定位(Monte Carlo localization，MCL) 均匀分布在语义图上。一旦粒子分散在地图上的任何地方，可以从相应的粒子生成一个语义范围地图。
MCL实现了一种估计概率密度的递归贝叶斯滤波器
Figure BDA0003859462980000141
根据初始时刻到t时刻的观测数据z1：t和运动估计
Figure BDA0003859462980000142
估计t时刻位姿xt。运动状态更新函数为：
Figure BDA0003859462980000143
其中，η表示归一化常数，
Figure BDA0003859462980000144
表示运动模型，
Figure BDA0003859462980000145
表示观测模型，而
Figure BDA0003859462980000146
表示前一时刻速度的先验概率模型。
使用语义标签索引有助于减少范围地图分布的可变性。为了降低复杂性，将机器人的运动从6个自由度[X，Y，Z，Roll，Pitch，Yaw]限制为3 个自由度[X，Y，Yaw]，只考虑二维平面的运动。因此，机器人的定位是 Lr＝(x，y，yaw)，Lr对应的观测是语义范围地图SRMr＝(W，H)。
针对流程3)，将激光雷达扫描SRMr＝(W，H)生成的当前范围图与所有 粒子的范围图SRMp(i＝0，...，n)＝(W，H)进行比较，并计算了它们的相似性。接着 从相似度最高的语义地图中推断出机器人的当前位置。该观测模型可定义 为具有相同尺度的两幅图像的绝对像素级差值的平均值除以(W*H)。
Figure BDA0003859462980000147
其中，SRMr表示生成的当前范围图，SRMp表示所有粒子的范围图，(W， H)表示观测到的语义地图。
图13是本发明实施例的基于人在环优化的机器人点云语义SLAM方法的三种SLAM算法在引入语义优化前后的建图质量对比示意图；
对比了LOAM、LeGO-LOAM和LIO-SAM三个纯点云SLAM算法和加入点云语义优化后的建图效果对比。其中MME、MPV和MOM均为基于地图拓扑信息熵的量化评估指标，在0.2、0.4和0.5三个不同的体素化密度下，通过增加语义优化，对应算法的评估指标均相对有所下降。说明语义优化策略能够有效降低总体地图的拓扑信息熵，使得地图的稳定性更高，间接说明对应的连续位姿估计精度更好。否则会产生大量重影墙面，而导致较高的地图点云拓扑熵结果。
本发明的优点在于：
提出了一种基于人在环优化的机器人点云语义SLAM方法，本方法可以在室内环境中通过只使用低成本的激光雷达改进移动机器人的定位。
在没有GPS定位信息或测绘级激光跟踪仪的情况下，快速高效地构建用于语义信息采集的激光雷达点云数据，完善机器人的姿势和最终地图。
在人类认知的帮助下，基于累积的点云地图进行批处理语义标注。这大大提高了贴标签的效率。
在语义SLAM整个工作流中引入人机交互协作：交互式SLAM和语义数据标签，得到与实际高度一致的点云语义图。
利用点云语义分割模型模拟人类实时点云语义信息获取的认知能力，提高了速度和准确性。
提出了一种基于点云语义对SLAM前后端优化的方法，并结合基于语义的蒙特卡洛粒子滤波进行周期性全局定位减少累积误差和地图畸变。
系统实施例
根据本发明实施例，提供了一种基于人在环优化的机器人点云语义 SLAM系统，图14是本发明实施例的基于人在环优化的机器人点云语义 SLAM系统的示意图，如图14所示，具体包括：
采集模块，安装于移动机器人，用于对目标地点进行数据采集，在采集过程中将SLAM算法作为初始建图算法计算得到每帧点云的相对位姿信息，完成建图；
优化模块，用于建立连续轨迹，获取人工观测的建图误差信息，基于所述建图误差信息选择建图误差周围正确的点云帧重新进行局部ICP的配准优化，获取关键位姿点之间的闭环，构建稠密因子图，然后基于所述稠密因子图进行全局优化，调整总体建图错误，迭代配准优化和全局优化；
数据集模块，用于将进行局部ICP配准优化后的点云位姿和优化后的点云位姿对应点云导入标注工具，基于预定义的点云语义类别，采用标注工具进行叠加地图的点云语义标注，累计点云语义标记结果得到点云语义分割数据集；
分割模型模块，用于基于点云语义分割数据集进行分割模型的训练和评估得到训练和评估后的分割模型；
定位优化模块，用于基于训练和评估后的分割模型，进行点云语义定位与优化。
优化模块具体用于：通过将连续点云和连续点云对应的推算位姿进行地图叠加，基于点云数据的采样顺序对每帧点云的中心点进行关联，形成连续轨迹，基于轨迹计算闭环边，并添加所述闭环边构建新闭环；
获取人工观测的建图误差信息，基于所述建图误差信息选择建图误差周围正确的点云帧进行局部配准算法的配准优化；获取关键位姿点之间的闭环，构建稠密因子图，然后基于因子图进行全局优化，调整总体建图错误，迭代所述配准优化和全局优化。
分割模型模块具体用于：将点云语义分割数据集分为训练集和测试集，基于训练集和测试集对基于深度学习的点云语义分割模型进行模型训练和评估，并保留总体分割精度高于某个阈值的模型参数作为实时点云语义分割的模型。
定位优化模块具体用于：
将点云发送到实时点云语义分割的模型得到点分割的语义标签，然后将一个单帧点云n×[x，y，z]使用基于激光雷达笛卡尔到极坐标投影公式投影到一个x-o-y平面，对xy轴进行极坐标转化得到2D特征图，基于预定义的点云语义配色进行2D特征图着色，得到2D点云语义特征图；
获得第一帧点云后，基于点云语义信息删除每帧点云的动态语义点，然后快速估计2D位姿，以2D位姿作为初始解估计3D点云位姿，进行SLAM 的3D点云地图构建和2D语义特征地图构建，周期性基于蒙特卡罗粒子滤波对当前扫描点云的2D点云特征图进行全局2D语义特征地图的粗粒度定位；
获取移动机器人在全局点云地图上的初始位置，对里程计前端估计进行修正，对于前端里程计估计时的帧间点云配准，使用语义标签进行类间分隔和类内匹配，实现点云搜索的加速；
同时，检测闭环，当发现新闭环则进行后端的因子图优化，更新全局位姿信息。
装置实施例一
本发明实施例提供一种基于人在环优化的机器人点云语义SLAM装置，如图15所示，包括：存储器150、处理器152及存储在存储器150上并可在处理器152上运行的计算机程序，计算机程序被处理器执行时实现上述方法实施例中的步骤。
装置实施例二
本发明实施例提供一种计算机可读存储介质，计算机可读存储介质上存储有信息传输的实现程序，程序被处理器152执行时实现上述方法实施例中的步骤。
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换本发明各实施例技术方案，并不使相应技术方案的本质脱离本方案的范围。

Claims (10)

1.一种基于人在环优化的机器人点云语义SLAM方法，其特征在于，包括，
S1、通过移动机器人上的采集模块对目标地点进行数据采集，在采集过程中将SLAM算法作为初始建图算法计算得到每帧点云的相对位姿信息，完成建图；
S2、建立点云连续轨迹，获取人工观测的建图误差信息，基于所述建图误差信息选择建图误差周围正确的点云帧重新进行局部ICP的配准优化，获取关键位姿点之间的闭环，构建稠密因子图，然后基于所述稠密因子图进行全局优化，调整总体建图错误，迭代配准优化和全局优化；
S3、将进行局部ICP配准优化后的点云位姿和优化后的点云位姿对应点云导入标注工具，基于预定义的点云语义类别，采用标注工具进行叠加地图的点云语义标注，累计点云语义标记结果得到点云语义分割数据集；
S4、基于点云语义分割数据集进行点云语义分割模型的训练和评估得到训练和评估后的点云语义分割模型；
S5、基于训练和评估后的分割模型，进行点云语义定位与优化。
2.根据权利要求1所述的方法，其特征在于，所述S2具体包括：通过将连续点云和连续点云对应的推算位姿进行地图叠加，基于点云数据的采样顺序对每帧点云的中心点进行关联，形成连续轨迹，基于轨迹计算闭环边，并添加所述闭环边构建新闭环；
获取人工观测的建图误差信息，基于所述建图误差信息选择建图误差周围正确的点云帧进行局部配准算法的配准优化；获取关键位姿点之间的闭环，构建稠密因子图，然后基于因子图进行全局优化，调整总体建图错误，迭代所述配准优化和全局优化。
3.根据权利要求2所述的方法，其特征在于，所述S4具体包括：将点云语义分割数据集分为训练集和测试集，基于训练集和测试集对基于深度学习的点云语义分割模型进行模型训练和评估，并保留总体分割精度高于某个阈值的模型参数作为实时点云语义分割的模型。
4.根据权利要求3所述的方法，其特征在于，所述S5具体包括：
将点云发送到实时点云语义分割的模型得到点分割的语义标签，然后将一个单帧点云n×[x，y，z]使用基于激光雷达笛卡尔到极坐标投影公式投影到一个x-o-y平面，对xy轴进行极坐标转化得到2D特征图，基于预定义的点云语义配色进行2D特征图着色，得到2D点云语义特征图；
获得第一帧点云后，基于点云语义信息删除每帧点云的动态语义点，然后快速估计2D位姿，以2D位姿作为初始解估计3D点云位姿，进行SLAM的3D点云地图构建和2D语义特征地图构建，周期性基于蒙特卡罗粒子滤波对当前扫描点云的2D点云特征图进行全局2D语义特征地图的粗粒度定位；
获取移动机器人在全局点云地图上的初始位置，对里程计前端估计进行修正，对于前端里程计估计时的帧间点云配准，使用语义标签进行类间分隔和类内匹配，实现点云搜索的加速；
同时，检测闭环，当发现新闭环则进行后端的因子图优化，更新全局位姿信息。
5.一种基于人在环优化的机器人点云语义SLAM系统，其特征在于，包括，
采集模块，安装于移动机器人，用于对目标地点进行数据采集，在采集过程中将SLAM算法作为初始建图算法计算得到每帧点云的相对位姿信息，完成建图；
优化模块，用于建立连续轨迹，获取人工观测的建图误差信息，基于所述建图误差信息选择建图误差周围正确的点云帧重新进行局部ICP的配准优化，获取关键位姿点之间的闭环，构建稠密因子图，然后基于所述稠密因子图进行全局优化，调整总体建图错误，迭代配准优化和全局优化；
数据集模块，用于将进行局部ICP配准优化后的点云位姿和优化后的点云位姿对应点云导入标注工具，基于预定义的点云语义类别，采用标注工具进行叠加地图的点云语义标注，累计点云语义标记结果得到点云语义分割数据集；
分割模型模块，用于基于点云语义分割数据集进行分割模型的训练和评估得到训练和评估后的分割模型；
定位优化模块，用于基于训练和评估后的分割模型，进行点云语义定位与优化。
6.根据权利要求5所述的系统，其特征在于，所述优化模块具体用于：通过将连续点云和连续点云对应的推算位姿进行地图叠加，基于点云数据的采样顺序对每帧点云的中心点进行关联，形成连续轨迹，基于轨迹计算闭环边，并添加所述闭环边构建新闭环；
获取人工观测的建图误差信息，基于所述建图误差信息选择建图误差周围正确的点云帧进行局部配准算法的配准优化；获取关键位姿点之间的闭环，构建稠密因子图，然后基于因子图进行全局优化，调整总体建图错误，迭代所述配准优化和全局优化。
7.根据权利要求6所述的系统，其特征在于，所述分割模型模块具体用于：将点云语义分割数据集分为训练集和测试集，基于训练集和测试集对基于深度学习的点云语义分割模型进行模型训练和评估，并保留总体分割精度高于某个阈值的模型参数作为实时点云语义分割的模型。
8.根据权利要求7所述的系统，其特征在于，所述定位优化模块具体用于：
将点云发送到实时点云语义分割的模型得到点分割的语义标签，然后将一个单帧点云n×[x，y，z]使用基于激光雷达笛卡尔到极坐标投影公式投影到一个x-o-y平面，对xy轴进行极坐标转化得到2D特征图，基于预定义的点云语义配色进行2D特征图着色，得到2D点云语义特征图；
获得第一帧点云后，基于点云语义信息删除每帧点云的动态语义点，然后快速估计2D位姿，以2D位姿作为初始解估计3D点云位姿，进行SLAM的3D点云地图构建和2D语义特征地图构建，周期性基于蒙特卡罗粒子滤波对当前扫描点云的2D点云特征图进行全局2D语义特征地图的粗粒度定位；
获取移动机器人在全局点云地图上的初始位置，对里程计前端估计进行修正，对于前端里程计估计时的帧间点云配准，使用语义标签进行类间分隔和类内匹配，实现点云搜索的加速；
同时，检测闭环，当发现新闭环则进行后端的因子图优化，更新全局位姿信息。
9.一种基于人在环优化的机器人点云语义SLAM装置，其特征在于，包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现如权利要求1至4中任一项所述的基于人在环优化的机器人点云语义SLAM方法的步骤。
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有信息传递的实现程序，所述程序被处理器执行时实现如权利要求1至4中任一项所述的基于人在环优化的机器人点云语义SLAM方法的步骤。
CN202211160212.7A 2022-09-22 2022-09-22 基于人在环优化的点云语义slam的方法、系统及装置 Pending CN115496792A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211160212.7A CN115496792A (zh) 2022-09-22 2022-09-22 基于人在环优化的点云语义slam的方法、系统及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211160212.7A CN115496792A (zh) 2022-09-22 2022-09-22 基于人在环优化的点云语义slam的方法、系统及装置

Publications (1)

Publication Number Publication Date
CN115496792A true CN115496792A (zh) 2022-12-20

Family

ID=84471413

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211160212.7A Pending CN115496792A (zh) 2022-09-22 2022-09-22 基于人在环优化的点云语义slam的方法、系统及装置

Country Status (1)

Country Link
CN (1) CN115496792A (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115638788A (zh) * 2022-12-23 2023-01-24 安徽蔚来智驾科技有限公司 语义矢量地图的构建方法、计算机设备及存储介质
CN117496309A (zh) * 2024-01-03 2024-02-02 华中科技大学 建筑场景点云分割不确定性评估方法、系统及电子设备
CN117671013A (zh) * 2024-02-01 2024-03-08 安徽蔚来智驾科技有限公司 点云定位方法、智能设备及计算机可读存储介质

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115638788A (zh) * 2022-12-23 2023-01-24 安徽蔚来智驾科技有限公司 语义矢量地图的构建方法、计算机设备及存储介质
CN117496309A (zh) * 2024-01-03 2024-02-02 华中科技大学 建筑场景点云分割不确定性评估方法、系统及电子设备
CN117496309B (zh) * 2024-01-03 2024-03-26 华中科技大学 建筑场景点云分割不确定性评估方法、系统及电子设备
CN117671013A (zh) * 2024-02-01 2024-03-08 安徽蔚来智驾科技有限公司 点云定位方法、智能设备及计算机可读存储介质
CN117671013B (zh) * 2024-02-01 2024-04-26 安徽蔚来智驾科技有限公司 点云定位方法、智能设备及计算机可读存储介质

Similar Documents

Publication Publication Date Title
CN111325843B (zh) 一种基于语义逆深度滤波的实时语义地图构建方法
US11145073B2 (en) Computer vision systems and methods for detecting and modeling features of structures in images
CN110675418B (zh) 一种基于ds证据理论的目标轨迹优化方法
CN115496792A (zh) 基于人在环优化的点云语义slam的方法、系统及装置
CN111060924B (zh) 一种slam与目标跟踪方法
CN113345018A (zh) 一种动态场景下的激光单目视觉融合定位建图方法
Taneja et al. Geometric change detection in urban environments using images
CN114419147A (zh) 一种救援机器人智能化远程人机交互控制方法及系统
Jeong et al. Multimodal sensor-based semantic 3D mapping for a large-scale environment
CN111046776A (zh) 基于深度相机的移动机器人行进路径障碍物检测的方法
CN113888695A (zh) 基于分支重构配准的非合作目标三维重建方法
CN112991534B (zh) 一种基于多粒度物体模型的室内语义地图构建方法及系统
CN113516664A (zh) 一种基于语义分割动态点的视觉slam方法
Qian et al. Robust visual-lidar simultaneous localization and mapping system for UAV
CN111739066B (zh) 一种基于高斯过程的视觉定位方法、系统及存储介质
CN114325634A (zh) 一种基于激光雷达的高鲁棒性野外环境下可通行区域提取方法
CN115388902A (zh) 室内定位方法和系统、ar室内定位导航方法和系统
CN115496900A (zh) 一种基于稀疏融合的在线碳语义地图构建方法
CN115690138A (zh) 一种融合车载影像与点云的道路边界提取与矢量化方法
Ouyang et al. Semantic slam for mobile robot with human-in-the-loop
CN111583331B (zh) 用于同时定位和地图构建的方法及装置
CN114862953A (zh) 一种基于视觉特征和3d激光的移动机器人重定位方法及装置
Belter et al. Keyframe-Based local normal distribution transform occupancy maps for environment mapping
CN117392347B (zh) 一种地图构建方法、装置、计算机设备及可读存储介质
Guo et al. Research on 3D geometric modeling of urban buildings based on airborne lidar point cloud and image

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