CN113671522B - 基于语义约束的动态环境激光slam方法 - Google Patents

基于语义约束的动态环境激光slam方法 Download PDF

Info

Publication number
CN113671522B
CN113671522B CN202110768468.5A CN202110768468A CN113671522B CN 113671522 B CN113671522 B CN 113671522B CN 202110768468 A CN202110768468 A CN 202110768468A CN 113671522 B CN113671522 B CN 113671522B
Authority
CN
China
Prior art keywords
semantic
map
environment
projection
dynamic
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
Application number
CN202110768468.5A
Other languages
English (en)
Other versions
CN113671522A (zh
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.)
Information Engineering University of PLA Strategic Support Force
Original Assignee
Information Engineering University of PLA Strategic Support Force
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 Information Engineering University of PLA Strategic Support Force filed Critical Information Engineering University of PLA Strategic Support Force
Priority to CN202110768468.5A priority Critical patent/CN113671522B/zh
Publication of CN113671522A publication Critical patent/CN113671522A/zh
Application granted granted Critical
Publication of CN113671522B publication Critical patent/CN113671522B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/42—Simultaneous measurement of distance and other co-ordinates
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/24—Classification techniques
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/02—Neural networks
    • G06N3/04—Architecture, e.g. interconnection topology
    • G06N3/045—Combinations of networks
    • 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
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/10—Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Electromagnetism (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Artificial Intelligence (AREA)
  • General Engineering & Computer Science (AREA)
  • Evolutionary Computation (AREA)
  • Computing Systems (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • General Health & Medical Sciences (AREA)
  • Computational Linguistics (AREA)
  • Biophysics (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Biomedical Technology (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Molecular Biology (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Biology (AREA)
  • Health & Medical Sciences (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Image Analysis (AREA)

Abstract

本发明提供一种基于语义约束的动态环境激光SLAM方法。该方法包括：使用球面投影方式对3D雷达点云数据P进行处理得到投影图I，进而根据投影图I，计算得到法向量图N；按照地图要素分类分级方法进行环境要素类别预定义，然后对所述投影图I按照预定义的环境要素类别进行语义分割，得到语义分割结果S；建立各环境要素类别的权重值并将其作为先验知识对投影图I上的像素进行环境要素预筛选，分为动态要素、静态要素和待定要素；初始化语义标识图，并基于所述语义分割结果S和场景上下文信息，采用环境要素筛选算法判断待定要素的动态性，更新语义标识图G；将语义标识图G作为语义约束添加到frame‑to‑map的ICP算法中，得到位姿估计结果，进而生成语义地图。

Description

基于语义约束的动态环境激光SLAM方法
技术领域
本发明涉及人工智能技术领域，尤其涉及一种基于语义约束的动态环境激光SLAM方法。
背景技术
随着人工智能、5G、物联网等技术的发展，智能机器人与人共融共生态势已悄然显现，尤其是在单调重复、危险、未知环境中，智能机器人更具有优势。智能机器人高效执行既定任务的前提是对所处环境及其预期影响的准确“理解”。这涉及到智能机器人的环境感知、环境表示建模和空间推理计算等一系列理论和技术问题，既是新一代人工智能的关键共性技术，也是人工智能时代测绘科学关注的新问题。
SLAM作为智能机器人环境探测与感知的关键基础性技术，已在近十年间取得显著进展，但目前多数主流算法都只在静态环境下稳定运行。然而现实世界是动态变化的，既有短时序条件下可变的地物要素，如移动的汽车、行人等，又有在长时序中变化的环境要素，比如四季变化或昼夜更替带来的环境变化等。动态变化的环境给SLAM的位姿估计、特征提取与跟踪以及地图构建等模块提出了新的挑战。
为解决动态环境SLAM问题，目前已形成了两种主流研究思路：过滤掉环境中的动态物体或是使用多时刻的地图来反映环境的动态变化。将环境中的运动物体过滤掉能够尽可能减小数据关联的错误以此来提高位姿估计的精度，该方面的研究中，基于先验信息结合视觉特征进行动态性判断的研究较多，其中涉及到深度学习技术与经典SLAM算法的结合(参考文献1：Saputra M R U,Markham A,Trigoni N.Visual SLAM and structure frommotion in dynamic environments:A survey[J].ACM Computing Surveys(CSUR),2018,51(2):1-36.)。从深度学习方法上，以语义分割与物体检测为主；从实现效果与研究基数上来看，室内环境多于室外环境，且多以RGB-D传感器为主。RGB-D传感器在使用RGB图像进行语义分割的基础上结合深度图像能够在SLAM过程中实现更为准确的动态物体检测与跟踪。DynaSLAM使用Mask RCNN与ORB-SLAM2相结合的方式实现了动态环境下的视觉SLAM，但该方法剔除了所有可运动的物体(比如停在路边的汽车等)易造成数据关联有误(参考文献2：Bescos B,Fácil J M,Civera J,et al.DynaSLAM:Tracking,mapping,and inpainting indynamic scenes[J].IEEE Robotics and Automation Letters,2018,3(4):4076-4083)；Dynamic-SLAM提出漏检补偿算法与选择性跟踪算法来提高位姿估计的精度(参考文献3：Xiao L,Wang J,Qiu X,et al.Dynamic-SLAM:Semantic monocular visual localizationand mapping based on deep learning in dynamic environment[J].Robotics andAutonomous Systems,2019,117:1-16.)。而在大规模室外环境中，单独使用激光雷达或是与RGB相机结合的方式较为普遍。例如SuMa++以语义分割结果为约束改进ICP算法实现动态环境下的激光雷达SLAM(参考文献4：Chen X,Milioto A,Palazzolo E,et al.Suma++:Efficient lidar-based semantic slam[C]//2019IEEE/RSJ International Conferenceon Intelligent Robots and Systems(IROS).IEEE,2019:4530-4537.)；以及使用图像语义信息辅助进行位姿校正实现点云配准；或是从点云中提取简单语义特征构建特征地图等方法。
发明内容
针对传统的SLAM方法通常因环境中动态要素的干扰导致数据关联有误造成位姿估计偏差的问题，本发明提供一种基于语义约束的动态环境激光雷达SLAM方法，使用卷积神经网络实现点云的语义分割，以先验知识为指导建立环境要素分类准则，对语义分割结果进行预处理；而后进一步根据环境上下文信息确定环境中的动态要素；最后实现位姿解算与语义地图构建。
本发明提供的一种基于语义约束的动态环境激光雷达SLAM方法，包括：
步骤1：使用球面投影方式对3D雷达点云数据P进行处理得到投影图I，进而根据投影图I，计算得到法向量图N；
步骤2：按照地图要素分类分级方法进行环境要素类别预定义，然后对所述投影图I按照预定义的环境要素类别进行语义分割，得到语义分割结果S；
步骤3：建立各环境要素类别的权重值并将其作为先验知识对投影图I上的像素进行环境要素预筛选，分为动态要素、静态要素和待定要素；
步骤4：初始化语义标识图，并基于所述语义分割结果S和场景上下文信息，采用环境要素筛选算法判断待定要素的动态性，更新语义标识图G；
步骤5：将语义标识图G作为语义约束添加到frame-to-map的ICP算法中，得到位姿估计结果，进而生成语义地图。
进一步地，步骤1中，将点云数据P上的第i个点Pi＝(x，y，z)通过公式(1)与投影图I上的第i个像素点Ii＝(u，v)一一对应：
Figure BDA0003151598300000031
其中，
Figure BDA0003151598300000032
fup与fdown分别表示激光雷达垂直视场角的上限与下限；w为投影图I的宽度；h为投影图I的高度。
进一步地，步骤1中，按照公式(2)计算投影图I上每个像素点的法向量，进而得到法向量图N：
Ni(u，v)＝(I(u+1,v)-I(u-1,v))×(I(u,v+1)-I(u,v-1)) (2)
其中，(u,v)表示投影图I上第i个像素点Ii的值，Ni(u，v)表示像素点Ii的法向量。
进一步地，在计算投影图I上边界区域像素点的法向量时，先采用公式(3)和(4)对像素点进行处理：
Figure BDA0003151598300000033
Figure BDA0003151598300000034
其中，w为投影图I的宽度；h为投影图I的高度。
进一步地，步骤2中，将环境要素类别预定义为6大类和14小类；所述6大类包括“建筑”、“地表”、“自然”、“车辆”、“路边地物”、“人与动物”；所述14小类包括“建筑”、“停车场”、“道路”、“人行路”、“其他地表道路”、“树木”、“植被”、“汽车”、“其他车辆”、“独立地物”、“交通标识”、“行人”、“骑行者”、“动物”。
进一步地，步骤4中，初始化语义标识图，具体为：当像素被判定位静态要素时，该像素的语义标识取值为1；当像素被判定为动态要素时，该像素的语义标识取值为0；当像素被判定待定要素时，该像素的语义标识取值为0.3。
进一步地，步骤4中，所述环境要素筛选算法具体包括：
按照公式(5)计算相邻帧间静态要素的像素平均位移值
Figure BDA0003151598300000041
Figure BDA0003151598300000042
其中，符号⊙是点乘运算符，表示两矩阵对应元素相乘；w为投影图I的宽度；h为投影图I的高度；
Figure BDA0003151598300000043
表示上一帧的位姿变换参数，St表示t时刻的语义分割结果，Gt表示t时刻的语义标识图；
根据像素的平均位移值
Figure BDA0003151598300000044
判断待定要素的动态性，具体为：将满足下述约束条件的像素划分为静态要素；然后更新语义标识图Gt；
约束条件：
Figure BDA0003151598300000045
其中，ρ为设定的阈值权重值，St(u,v)表示St中像素值为(u,v)的语义分割结果。
进一步地，相邻帧间的投影图像It-1,It的变换关系表示为
Figure BDA0003151598300000046
进一步地，步骤5具体包括：
定义最小化误差函数为：
Figure BDA0003151598300000047
Figure BDA0003151598300000048
其中，i表示ICP算法中需要进行配准的点的个数；
对于Frame-to-MapICP中的每次迭代，使用Levenberg-Marquardt递增地更新6自由度的相对位姿：
Figure BDA0003151598300000049
其中，
Figure BDA00031515983000000410
为雅克比矩阵；/>
Figure BDA00031515983000000411
为对应于法向量nt的残差；对角矩阵/>
Figure BDA00031515983000000412
Figure BDA00031515983000000413
使用λ来正则化Hessian矩阵，/>
Figure BDA00031515983000000414
为权重矩阵，是包含对应于残差ri的权重值ωi的对角矩阵；
一旦Frame-to-Map ICP算法达到临界条件，就通过ξ∈se(3)解算出变换矩阵
Figure BDA00031515983000000415
并将其作为扫描帧frame与地图map对齐的位姿/>
Figure BDA00031515983000000416
Figure BDA00031515983000000417
其中，O表示世界坐标系。
进一步地，基于语义标识图与激光雷达传感器的前进方向，为残差ri设置权重值ωi，具体为：正对前进方向的像素权重值高于侧方及后方方向的像素权重值；其中，将雷达点云数据按角度等分为三份，将其前进方向对应的投影图像成像区间取值为(0.33w,0.67w)。
本发明的有益效果：
(1)本发明提出的动态环境激光雷达SLAM框架，在语义分割的基础上，结合先验知识实现了动态环境中的即时定位与语义地图构建，其中，基于上下文信息的环境要素筛选策略能够准确过滤环境中的动态物体，为高效鲁棒的位姿估计提供参考依据。
(2)在KITTI与SemanticPOSS数据集上对本发明方法在动态环境中的性能进行评估，实验证明本发明方法能够有效提高位姿估计的精度，具备可靠的性能、准确性和鲁棒性。
附图说明
图1为本发明实施例提供的基于语义约束的动态环境激光SLAM方法的流程示意图；
图2为本发明实施例提供的点云投影示意图；
图3为本发明实施例提供的基于先验知识的环境要素分类分级示意图；
图4为本发明实施例提供的基于上下文信息的环境要素筛选示意图；
图5为本发明实施例提供的frame-to-map ICP示意图；
图6为本发明实施例提供的权重矩阵赋值示意图；
图7为本发明实施例提供的KITTI数据集实验结果示意图；
图8为本发明实施例提供的语义地图依次为00 05 08序列数据；
图9为本发明实施例提供的SemanticPOSS数据集实验结果示意图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
如图1所示，本发明实施例提供一种基于语义约束的动态环境激光SLAM方法，包括以下步骤：
S101：使用球面投影方式对3D雷达点云数据P进行处理得到投影图I，进而根据投影图I，计算得到法向量图N；如图2所示。
作为一种可实施方式，将点云数据P上的第i个点Pi＝(x,y,z)通过公式(1)与投影图I上的第i个像素点Ii＝(u,v)一一对应：
Figure BDA0003151598300000061
其中，
Figure BDA0003151598300000062
fup与fdown分别表示激光雷达垂直视场角的上限与下限；w为投影图I的宽度，其取值与激光雷达水平分辨率成反比；h为投影图I的高度，其取值为激光雷达线程数。公式(1)所示的投影函数保证了点云上任一点Pi在三维中的邻近点经投影变换后依旧为像素Ii的相邻像素，且在最近邻查找上能够显著提高效率。
作为一种可实施方式，按照公式(2)计算投影图I上每个像素点的法向量，进而得到法向量图N：
Ni(u，v)＝(I(u+1,v)-I(u-1,v))×(I(u,v+1)-I(u,v-1)) (2)
其中，(u,v)表示投影图I上第i个像素点Ii的值，Ni(u，v)表示像素点Ii的法向量。
需要注意的是，由于球面投影特性以及所得投影图像宽度高度的取值方式，投影图像I的左右边界部分在原始点云数据中是相连通的，即可能会存在物体被分为两部分，并同时出现在投影图像I左右两侧的现象，而投影图像I的上下边界由激光雷达的垂直视场角决定，不会出现上下相连通的情况。考虑到上述特点，在计算投影图I上边界区域像素点的法向量时，本发明实施例采用如公式(3)和(4)所示的处理方案：
Figure BDA0003151598300000063
Figure BDA0003151598300000071
S102：按照地图要素分类分级方法进行环境要素类别预定义，然后对所述投影图I按照预定义的环境要素类别进行语义分割，得到语义分割结果S；
具体地，依照地图要素分类分级的指导思想，将环境要素分为6大类(“建筑”、“地表”、“自然”、“车辆”、“路边地物”、“人与动物”)，14小类(“建筑”、“停车场”、“道路”、“人行路”、“其他地表道路”、“树木”、“植被”、“汽车”、“其他车辆”、“独立地物”、“交通标识”、“行人”、“骑行者”、“动物”)；本发明实施例中，采用全连接卷积神经网络框架的语义分割网络RangeNet++；
可以看出，本步骤是通过将空间中的每个三维点投影为平面上的像素之后，对平面上的像素进行语义分割，这样做的好处有以下两点：第一、不论是在性能、效率还是数据集训练上，基于图像的语义分割成果都显优于基于点云的语义分割研究成果；第二、在位姿估计时，因数据维度减少，基于图像相邻像素的遍历方式在效率上明显优于三维点云的最近邻查找匹配。
S103：建立各环境要素类别的权重值并将其作为先验知识对投影图I上的像素进行环境要素预筛选，分为动态要素、静态要素和待定要素；
具体地，以标定物体类别为输出的语义分割结果，是目前在语义层面上帮助机器人理解环境的重要信息，本发明将其作为机器人判断环境中动静态要素的重要依据。
作为一种可实施方式，如图3所示，本发明对6大类14小类环境要素建立了动态性量化指标，动态至静态对应取值从0至1。为了更准确的区分出环境中的动静态要素，同时为SLAM位姿估计提供尽可能多且鲁棒性更好的特征，依据设置的上下限阈值对环境要素进行判断。显而易见的是，低于静态阈值及高于动态阈值的要素会被非常容易的区分，而介于静态阈值与动态阈值之间的环境要素，本发明将其称为待定要素，是需要进行进一步地判定其动态性的。
S104：初始化语义标识图，并基于所述语义分割结果S和场景上下文信息，采用环境要素筛选算法判断待定要素的动态性，更新语义标识图G；
具体地，通过研究可知，待定要素通常具备这样的特点，其具备动态属性，但在一定时间内静止于环境中。以城市环境为例，短时间或长时间停放在路边的车辆就满足上述特征。当具备动态属性的环境要素静止于环境中时，若粗暴的将其归类为运动物体，在进行位姿估计时会影响位姿解算的准确性与鲁棒性，出现由于特征数目锐减，相邻帧间对应关系减弱而导致解算偏差较大的现象。通过采用本发明所设计的环境要素筛选算法，可以更准确的确定当前环境中的静态要素(不论其属性是动态还是静态)。
将相邻帧间的同名点Pp,Pq的变换关系表示为Pp＝TpqPq，
Figure BDA0003151598300000081
其中Tpq由旋转矩阵Rpq∈SO(3)平移向量/>
Figure BDA0003151598300000082
构成。相应地，相邻帧间的投影图像It-1,It的变换关系可表示为/>
Figure BDA0003151598300000083
作为一种可实施方式，本发明实施例中，为了准确的使用静态环境要素进行位姿估计，引入语义标识图G(u，v)＝{0,1}，其本质是二维矩阵，大小与投影图像I，语义分割图S，法向量图N等相同，即w×h。语义标识图的初值依据量化指标赋值，当像素判定为静态要素时，语义标识取值为1；为动态要素时取值为0；待定要素取值为0.3。
Figure BDA0003151598300000084
以语义标识图为基础，结合场景上下文信息对待定要素的动态性加以判定。场景上下文信息包含相邻帧间信息与当前帧中蕴含的交叉验证信息。当前帧中的信息是交叉验证的过程，例如道路与汽车相交时，即可得出静态道路与动态汽车的判定结果；停车区域与汽车相交时，即可得出静态停车区域与静态汽车的判定结果。针对相邻帧间信息的动态要素判定，本发明实施例设计的环境要素筛选算法，如图4所示，具体包括：首先按照公式(5)计算相邻帧间静态要素的像素平均位移值
Figure BDA0003151598300000085
Figure BDA0003151598300000086
其中，符号⊙是点乘运算符，即两矩阵对应元素相乘。
Figure BDA0003151598300000087
表示上一帧的位姿变换参数，St表示t时刻的语义分割结果，Gt表示t时刻的语义标识图。假定相邻帧间的位姿变化不显著，在尚未得到准确位姿估计结果的情况下，选用上一帧的位姿变换参数/>
Figure BDA0003151598300000091
作为初值计算像素平均位移值。
然后根据像素的平均位移值
Figure BDA0003151598300000092
判断待定要素的动态性，具体为：将满足下述约束条件的像素划分为静态要素，反之为动态要素：
约束条件：
Figure BDA0003151598300000093
其中，ρ为设定的阈值权重值，St(u,v)表示St中像素值为(u,v)的语义分割结果。
对应地，语义标识图进行更新(为静态要素时，Gi(u,v)＝1；为动态要素时，Gi(u,v)＝0)，得到t时刻语义标识图Gt。
S105：将语义标识图G作为语义约束添加到frame-to-map的ICP算法中，得到位姿估计结果，进而生成语义地图。
具体地，位姿估计通常被描述为非线性优化问题，考虑到面元地图的特性，本发明实施例采用了基于语义约束的frame-to-map ICP(如图5所示)。在语义标识图G的帮助下，本发明实施例能够准确利用环境中的静态要素进行位姿估计。
定义最小化误差函数为：
Figure BDA0003151598300000094
Figure BDA0003151598300000095
其中，i表示ICP算法中需要进行配准的点的个数；
具体地，ICP算法为两帧点云间的配准算法，其原理为从两帧点云的点集中，选取具有对应性的点集，可表达为i个点，通过两个点集中各i个点进行配准。
对于Frame-to-MapICP中的每次迭代，使用Levenberg-Marquardt递增地更新6自由度的相对位姿：
Figure BDA0003151598300000096
其中，
Figure BDA0003151598300000097
为雅克比矩阵；/>
Figure BDA0003151598300000098
为对应于法向量nt的残差；对角矩阵/>
Figure BDA0003151598300000099
Figure BDA00031515983000000910
使用λ来正则化Hessian矩阵，/>
Figure BDA00031515983000000911
为权重矩阵，是包含对应于残差ri的权重值ωi的对角矩阵；
一旦Frame-to-Map ICP算法达到临界条件，就通过ξ∈se(3)解算出变换矩阵
Figure BDA00031515983000000912
并将其作为扫描帧frame与地图map对齐的位姿/>
Figure BDA00031515983000000913
Figure BDA00031515983000000914
其中，O表示世界坐标系。
在Frame-to-MapICP算法中，本发明实施例基于语义标识图与激光雷达传感器的前进方向，为残差ri设置权重值ωi，具体为：正对前进方向的像素权重值高于侧方及后方方向的像素权重值；其中，将雷达点云数据按角度等分为三份，将其前进方向对应的投影图像成像区间取值为(0.33w,0.67w)。
具体地，语义标识图能够在进行配准与位姿估计时过滤掉环境中的动态要素，而不需要使用全部的像素信息，同时这也意味着不需要再为动静态要素迭代设置权重值。因此，在语义标识图的帮助下，将权重值的设置原则与激光雷达传感器的前进方向相关联，即正对前进方向的像素权重值高于侧方及后方方向的像素权重值。前进方向的扫描数据为定位定姿与地图构建带来的增益显著高于侧方及后方方向，换言之，前进方向的数据为real新获取数据，侧方与后方方向的数据与之前帧的扫描数据相重叠。据此，本发明实施例将雷达点云数据按角度等分为三份，为便于计算取值，将其前进方向对应的投影图像成像区间取值为(0.33w,0.67w)。如图6所示，进一步得到用于位姿解算的权重值矩阵w。
为了验证本发明提供的动态环境激光SLAM方法的有效性，本发明还提供有以下实验数据。
选用两个不同的室外环境激光雷达数据集进行实验。KITTI数据集是众多SLAM算法的评价基准，在此数据集上进行实验可以测试本发明方法的性能并且便于与其他算法进行横向对比。SemanticPOSS与KITTI数据集在采集设备、采集区域以及数据内容上均有较大差异，基于SemanticPOSS数据集的实验能够在纵向上印证本发明方法的鲁棒性与稳定性。为了直观的评价本发明方法在动态环境数据集上的实验效果，实验选用主流的动态环境下激光雷达SLAM方法以及视觉SLAM方法作为对比。实验设备为搭载Intel i7-9700k CPU,16GB RAM以及一张GeForce RTXTM2080Ti GPU的台式电脑。
(1)KITTI数据集
KITTI数据集由Velodyne HDL-64E采集得到，自发布以来始终是SLAM算法评价的主流数据集。它包含了urban、highway以及country等典型室外环境的11组数据，能够体现出动态环境的特点。并且KITTI数据集涵盖的室外环境中大部分的动态物体是静止的，这能够更好地测试我们提出的环境要素筛选算法的性能。
对于SLAM算法的评价，我们采用绝对位姿误差(Absolute Pose Error,APE)的定量评价指标，在计算中使用Sim(3)Umeyama来对准。我们选用EVO位姿评价工具(参考GruppM.evo:Python package for the evaluation of odometry and slam[J].Note:https://github.com/MichaelGrupp/evo Cited by:Table,2017,7)对位姿估计结果进行评价，该工具的评价结果包括误差地图、均方根误差、平均误差、中位误差和标准差等。我们选择基于雷达数据的SuMa++算法，以及动态环境中表现优异的视觉SLAM算法-DynaSLAM与DM-SLAM(Lu X,Wang H,Tang S,et al.DM-SLAM:Monocular SLAM in dynamic environments[J].Applied Sciences,2020,10(12):4252)作为对比。
表1显示了绝对位姿误差的定量指标对比结果。图7以可视化图表的形式展现了本发明方法的详细细节以及与SuMa++方法的对比情况。图8中分别为序列00,05,08三组数据生成的语义地图。在同视觉SLAM方法的对比中，本发明方法在6个序列数据中取得了显著进步，另5个序列数据不如视觉SLAM方法。在与SuMa++的定量评价中，本发明方法在6个序列数据中取得了显著进步，在4个序列数据中有较少提升，而在Sequence 02数据中的表现出现滑坡。考虑到KITTI数据集中多数动态物体均静止于环境中，实验结果有力的表明了环境要素筛选算法的有效性，其能够提高位姿解算的精度，提升SLAM系统的鲁棒性。
Figure BDA0003151598300000111
(2)SemanticPOSS数据集
SemanticPOSS数据集是由Hesaitech Pandora激光雷达采集了北京大学内的6组校园环境数据所组成的，表2显示了其与主流室外数据集动态要素的对比情况(Pan Y,GaoB,Mei J,et al.Semanticposs:A point cloud dataset with large quantity ofdynamic instances[C]//2020IEEE Intelligent Vehicles Symposium(IV).IEEE,2020:687-693.)。与多数动态物体静止于环境中的KITTI数据集相比，SemanticPOSS数据集虽然体量小，但涵盖了更多的动态要素，更符合动态环境的特点，另外该数据集6组数据的真值轨迹相对稳定，不存在闭环，能够有针对性的测试方法在高动态环境下的准确性与鲁棒性。
Figure BDA0003151598300000121
因SuMa++目前仅能在KITTI数据集中实现，尚不能在SemanticPOSS数据集上实现，因此仅对本发明方法进行定量评价。我们对该数据集中6组数据依次展开实验，从表3能够看出本发明方法取得了较为优异的结果。图9为可视化结果，x与y方向上取得了较好的位姿估计结果，而z方向上的偏差是造成位姿估计不准确的主要原因。Z方向上出现偏差的主要原因是由获取的点云数据降维生成投影图像的过程中造成的，将三维数据降维为二维图像不可避免地损失一个维度的信息，尽管通过深度图与法向量图进行完善，但仍存在误差。
Figure BDA0003151598300000122
总的来说，通过在KITTI数据集与SemanticPOSS数据集上的实验，本发明方法由于有效地消除了环境中动态要素的干扰，提高了位姿估计的准确度，增强了SLAM的性能，取得了较为优异的结果。
本发明在语义信息的约束下构造了一个激光雷达SLAM框架，利用环境要素筛选算法来提高其在动态环境中的性能。这个框架分为四个模块，点云投影，语义分割，动态要素筛选以及语义地图构建。本发明所提出的考虑上下文信息的环境要素筛选算法，简单有效地提高了动态要素判定的鲁棒性和准确性。本发明所构建的动态环境激光雷达SLAM框架，在语义分割的基础上，结合先验知识实现了动态环境中的即时定位与语义地图构建。通过上述实验结果表明，本发明方法在动态环境下具备可靠的性能、准确性和鲁棒性。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (8)

1.基于语义约束的动态环境激光SLAM方法，其特征在于，包括：
步骤1：使用球面投影方式对3D雷达点云数据P进行处理得到投影图I，进而根据投影图I，计算得到法向量图N；
步骤2：按照地图要素分类分级方法进行环境要素类别预定义，然后对所述投影图I按照预定义的环境要素类别进行语义分割，得到语义分割结果S；
步骤3：建立各环境要素类别的权重值并将其作为先验知识对投影图I上的像素进行环境要素预筛选，分为动态要素、静态要素和待定要素；
步骤4：初始化语义标识图，并基于所述语义分割结果S和场景上下文信息，采用环境要素筛选算法判断待定要素的动态性，更新语义标识图G；
所述环境要素筛选算法具体包括：
按照公式(5)计算相邻帧间静态要素的像素平均位移值
Figure FDA0004210209470000011
Figure FDA0004210209470000012
其中，符号⊙是点乘运算符，表示两矩阵对应元素相乘；w为投影图I的宽度；h为投影图I的高度；
Figure FDA0004210209470000013
表示上一帧的位姿变换参数，St表示t时刻的语义分割结果，Gt表示t时刻的语义标识图；
根据像素的平均位移值
Figure FDA0004210209470000014
判断待定要素的动态性，具体为：将满足下述约束条件的像素划分为静态要素；然后更新语义标识图Gt；
约束条件：
Figure FDA0004210209470000015
其中，ρ为设定的阈值权重值，St(u,v)表示St中像素值为(u,v)的语义分割结果；
步骤5：将语义标识图G作为语义约束添加到frame-to-map的ICP算法中，得到位姿估计结果，进而生成语义地图；具体包括：
定义最小化误差函数为：
Figure FDA0004210209470000016
Figure FDA0004210209470000017
其中，i表示ICP算法中需要进行配准的点的个数；
对于Frame-to-MapICP中的每次迭代，使用Levenberg-Marquardt递增地更新6自由度的相对位姿：
Figure FDA0004210209470000021
其中，
Figure FDA0004210209470000022
为雅克比矩阵；/>
Figure FDA0004210209470000023
为对应于法向量nt的残差；对角矩阵
Figure FDA0004210209470000024
使用λ来正则化Hessian矩阵，/>
Figure FDA0004210209470000025
为权重矩阵，是包含对应于残差ri的权重值ωi的对角矩阵；
一旦Frame-to-Map ICP算法达到临界条件，就通过ξ∈se(3)解算出变换矩阵
Figure FDA0004210209470000026
并将其作为扫描帧frame与地图map对齐的位姿/>
Figure FDA0004210209470000027
Figure FDA0004210209470000028
其中，O表示世界坐标系。
2.根据权利要求1所述的基于语义约束的动态环境激光SLAM方法，其特征在于，步骤1中，将点云数据P上的第i个点Pi＝(x,y,z)通过公式(1)与投影图I上的第i个像素点Ii＝(u,v)一一对应：
Figure FDA0004210209470000029
其中，
Figure FDA00042102094700000210
fup与fdown分别表示激光雷达垂直视场角的上限与下限；w为投影图I的宽度；h为投影图I的高度。
3.根据权利要求1所述的基于语义约束的动态环境激光SLAM方法，其特征在于，步骤1中，按照公式(2)计算投影图I上每个像素点的法向量，进而得到法向量图N：
Ni(u，v)＝(I(u+l，v)-I(u-1，v))×(I(u，v+1)-I(u，v-1)) (2)
其中，(u,v)表示投影图I上第i个像素点Ii的值，Ni(u，v)表示像素点Ii的法向量。
4.根据权利要求3所述的基于语义约束的动态环境激光SLAM方法，其特征在于，在计算投影图I上边界区域像素点的法向量时，先采用公式(3)和(4)对像素点进行处理：
Figure FDA00042102094700000211
Figure FDA0004210209470000031
其中，w为投影图I的宽度；h为投影图I的高度。
5.根据权利要求1所述的基于语义约束的动态环境激光SLAM方法，其特征在于，步骤2中，将环境要素类别预定义为6大类和14小类；所述6大类包括“建筑”、“地表”、“自然”、“车辆”、“路边地物”、“人与动物”；所述14小类包括“建筑”、“停车场”、“道路”、“人行路”、“其他地表道路”、“树木”、“植被”、“汽车”、“其他车辆”、“独立地物”、“交通标识”、“行人”、“骑行者”、“动物”。
6.根据权利要求1所述的基于语义约束的动态环境激光SLAM方法，其特征在于，步骤4中，初始化语义标识图，具体为：当像素被判定位静态要素时，该像素的语义标识取值为1；当像素被判定为动态要素时，该像素的语义标识取值为0；当像素被判定待定要素时，该像素的语义标识取值为0.3。
7.根据权利要求1所述的基于语义约束的动态环境激光SLAM方法，其特征在于，相邻帧间的投影图像It-1,It的变换关系表示为
Figure FDA0004210209470000032
8.根据权利要求7所述的基于语义约束的动态环境激光SLAM方法，其特征在于，基于语义标识图与激光雷达传感器的前进方向，为残差ri设置权重值ωi，具体为：正对前进方向的像素权重值高于侧方及后方方向的像素权重值；其中，将雷达点云数据按角度等分为三份，将其前进方向对应的投影图像成像区间取值为(0.33w,0.67w)。
CN202110768468.5A 2021-07-07 2021-07-07 基于语义约束的动态环境激光slam方法 Active CN113671522B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110768468.5A CN113671522B (zh) 2021-07-07 2021-07-07 基于语义约束的动态环境激光slam方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110768468.5A CN113671522B (zh) 2021-07-07 2021-07-07 基于语义约束的动态环境激光slam方法

Publications (2)

Publication Number Publication Date
CN113671522A CN113671522A (zh) 2021-11-19
CN113671522B true CN113671522B (zh) 2023-06-27

Family

ID=78539018

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110768468.5A Active CN113671522B (zh) 2021-07-07 2021-07-07 基于语义约束的动态环境激光slam方法

Country Status (1)

Country Link
CN (1) CN113671522B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114511600A (zh) * 2022-04-20 2022-05-17 北京中科慧眼科技有限公司 基于点云配准的位姿计算方法和系统
CN115451939B (zh) * 2022-08-19 2024-05-07 中国人民解放军国防科技大学 基于检测分割的动态场景下的并行slam方法

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110781262A (zh) * 2019-10-21 2020-02-11 中国科学院计算技术研究所 基于视觉slam的语义地图的构建方法
US10650278B1 (en) * 2017-07-21 2020-05-12 Apple Inc. Semantic labeling of point clouds using images
CN111340881A (zh) * 2020-02-18 2020-06-26 东南大学 一种动态场景下基于语义分割的直接法视觉定位方法
CN111563442A (zh) * 2020-04-29 2020-08-21 上海交通大学 基于激光雷达的点云和相机图像数据融合的slam方法及系统
CN111652179A (zh) * 2020-06-15 2020-09-11 东风汽车股份有限公司 基于点线特征融合激光的语义高精地图构建与定位方法
CN111968129A (zh) * 2020-07-15 2020-11-20 上海交通大学 具有语义感知的即时定位与地图构建系统及方法
CN112116657A (zh) * 2020-08-07 2020-12-22 中国科学院深圳先进技术研究院 基于表检索的同时定位与建图方法和装置
CN112268559A (zh) * 2020-10-22 2021-01-26 中国人民解放军战略支援部队信息工程大学 复杂环境下融合slam技术的移动测量方法
CN112465021A (zh) * 2020-11-27 2021-03-09 南京邮电大学 基于图像插帧法的位姿轨迹估计方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10685446B2 (en) * 2018-01-12 2020-06-16 Intel Corporation Method and system of recurrent semantic segmentation for image processing
US11157527B2 (en) * 2018-02-20 2021-10-26 Zoox, Inc. Creating clean maps including semantic information

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10650278B1 (en) * 2017-07-21 2020-05-12 Apple Inc. Semantic labeling of point clouds using images
CN110781262A (zh) * 2019-10-21 2020-02-11 中国科学院计算技术研究所 基于视觉slam的语义地图的构建方法
CN111340881A (zh) * 2020-02-18 2020-06-26 东南大学 一种动态场景下基于语义分割的直接法视觉定位方法
CN111563442A (zh) * 2020-04-29 2020-08-21 上海交通大学 基于激光雷达的点云和相机图像数据融合的slam方法及系统
CN111652179A (zh) * 2020-06-15 2020-09-11 东风汽车股份有限公司 基于点线特征融合激光的语义高精地图构建与定位方法
CN111968129A (zh) * 2020-07-15 2020-11-20 上海交通大学 具有语义感知的即时定位与地图构建系统及方法
CN112116657A (zh) * 2020-08-07 2020-12-22 中国科学院深圳先进技术研究院 基于表检索的同时定位与建图方法和装置
CN112268559A (zh) * 2020-10-22 2021-01-26 中国人民解放军战略支援部队信息工程大学 复杂环境下融合slam技术的移动测量方法
CN112465021A (zh) * 2020-11-27 2021-03-09 南京邮电大学 基于图像插帧法的位姿轨迹估计方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
动态环境下的语义地图构建;齐少华;徐和根;万友文;付豪;;《计算机科学》;第47卷(第09期);全文 *
基于深度学习的动态场景语义SLAM;房立金;刘博;万应才;;《华中科技大学学报(自然科学版)》;第48卷(第01期);全文 *

Also Published As

Publication number Publication date
CN113671522A (zh) 2021-11-19

Similar Documents

Publication Publication Date Title
Caltagirone et al. Fast LIDAR-based road detection using fully convolutional neural networks
Pinggera et al. Lost and found: detecting small road hazards for self-driving vehicles
Asvadi et al. 3D Lidar-based static and moving obstacle detection in driving environments: An approach based on voxels and multi-region ground planes
Barnes et al. Find your own way: Weakly-supervised segmentation of path proposals for urban autonomy
Shinzato et al. Road terrain detection: Avoiding common obstacle detection assumptions using sensor fusion
EP4191532A1 (en) Image annotation
CN103413308B (zh) 一种障碍物检测方法和装置
US10477178B2 (en) High-speed and tunable scene reconstruction systems and methods using stereo imagery
EP3844672A1 (en) Structure annotation
CN113671522B (zh) 基于语义约束的动态环境激光slam方法
Ye et al. Robust lane extraction from MLS point clouds towards HD maps especially in curve road
Ouyang et al. A cgans-based scene reconstruction model using lidar point cloud
CN111880191A (zh) 基于多智能体激光雷达和视觉信息融合的地图生成方法
Muresan et al. Real-time object detection using a sparse 4-layer LIDAR
Dwivedi et al. Bird's Eye View Segmentation Using Lifted 2D Semantic Features.
Kühnl et al. Visual ego-vehicle lane assignment using spatial ray features
Delmerico et al. Building facade detection, segmentation, and parameter estimation for mobile robot stereo vision
Gomez-Donoso et al. Three-dimensional reconstruction using SFM for actual pedestrian classification
CN116879870A (zh) 一种适用于低线束3d激光雷达的动态障碍物去除方法
Guo et al. A feasible region detection method for vehicles in unstructured environments based on PSMNet and improved RANSAC
Wirges et al. Fast and robust ground surface estimation from LiDAR measurements using uniform B-Splines
Han et al. GardenMap: Static point cloud mapping for Garden environment
KR20220151572A (ko) IPM 이미지와 정밀도로지도(HD Map) 피팅을 통해 노면객체의 변화를 자동으로 판단하고 갱신하는 정밀도로지도 자동갱신 방법 및 시스템
Mandici et al. Aggregate road surface based environment representation using digital elevation maps
Lu et al. A geometric convolutional neural network for 3d object detection

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