CN115661255A - 一种激光slam回环检测与校正方法 - Google Patents

一种激光slam回环检测与校正方法 Download PDF

Info

Publication number
CN115661255A
CN115661255A CN202211611667.6A CN202211611667A CN115661255A CN 115661255 A CN115661255 A CN 115661255A CN 202211611667 A CN202211611667 A CN 202211611667A CN 115661255 A CN115661255 A CN 115661255A
Authority
CN
China
Prior art keywords
point
descriptor
points
scan context
local
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.)
Granted
Application number
CN202211611667.6A
Other languages
English (en)
Other versions
CN115661255B (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.)
University of Science and Technology of China USTC
Original Assignee
University of Science and Technology of China USTC
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 University of Science and Technology of China USTC filed Critical University of Science and Technology of China USTC
Priority to CN202211611667.6A priority Critical patent/CN115661255B/zh
Publication of CN115661255A publication Critical patent/CN115661255A/zh
Application granted granted Critical
Publication of CN115661255B publication Critical patent/CN115661255B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Image Analysis (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明涉及SLAM系统中机器人的回环检测领域，公开了一种激光SLAM回环检测与校正方法，可以更加鲁棒地检测回环并更好地解算出回环之间的相对位姿变换；通过利用扫描点云的几何中心作为极坐标系原点建立改进型Scan Context全局描述子，增强了Scan Context方法对平移偏差的鲁棒性；并通过设计基于特征点的Scan Context局部描述子集，使得可以同时通过描述子解算出回环相对位姿的旋转分量和平移分量，为后续的迭代最近点操作提供了更优的位姿初始解，因此可以减小迭代最近点操作陷入局部最优解的概率，提高最终解算出的位姿的准确性。本发明整体方案鲁棒性和适应性强、效率高、可移植性好。

Description

一种激光SLAM回环检测与校正方法
技术领域
本发明涉及SLAM系统中机器人的回环检测领域，具体涉及一种激光SLAM回环检测与校正方法。
背景技术
随着科技的发展，通过机器人进行导航的应用越来越广泛。为了实现导航的目的，在许多机器人应用中都需要实时定位和建图系统（SLAM系统），SLAM系统又以激光SLAM和视觉SLAM为主。回环检测是SLAM系统的一个重要模块，它有助于减小位姿累积误差和构建环境的拓扑结构。在激光SLAM系统中，回环检测的关键问题是如何对三维结构信息进行有效编码，以使得机器人系统能够根据激光雷达的感知信息识别曾经访问过的地点和区分模糊地点。一般来说，现有的方法可以分为两类，包括基于传统描述子的方法和基于深度学习的方法。基于传统描述子的方法旨在将无序的3D点云转换为结构化数据表示。为了实现视点变化鲁棒性（这对于循环闭合很重要），大多数方法将场景信息编码为描述子的过程中丢失了详细的几何结构。这一限制使得描述子在位置识别问题上区分度不够，从而导致潜在的误判。如何将稳定的、有区别的几何结构信息保存到全局描述子中，仍然是传统描述子设计的一个挑战。
现有技术中存在以下技术方案试图解决上述问题：
（1）使用由形函数组成的直方图的串联作为描述子，或者采用双阈值方案在3D点云上使用两个距离函数来生成法向量的直方图作为描述子；但这两种方法均在KITTI数据集上表现不佳。
（2）利用点云的法线和质心方向之间的角度构建直方图；但是这种方法舍弃了点的位置信息，这可能会导致描述子的可区分性不足，且计算法线过程耗时较长。
（3）通过节点匹配和几何验证来执行扫描匹配；然而由于此类方法对于对象和平面的特殊性要求，它们通用性不如基于全局描述子的方法。
此外，还存在一种扫描上下文（Scan Context）方法；扫描上下文（Scan Context）方法在对环境信息进行简化编码的同时有着较强的描述能力，与其他传统的描述子相比，性能有了较大提升，且它对点云密度变化、噪声和超参数具有更强的鲁棒性。另外，ScanContext方法的主要优点之一是具有旋转不变性。但是该方法也存在以下不足：
（1）Scan Context方法对平移敏感。具体来说，因Scan Context方法以激光雷达所在位置为极坐标原点来构建描述子，如果存在回环关系的两个位姿之间存在较大的平移偏移，两者对应的描述子中心位置也会有较大平移偏差，而Scan Context描述子之间的相似性是基于严格的逐列对比来计算的，因此会导致应有的闭环检测失败。Scan Context方法试图通过在周围扩大描述范围来解决该问题，但是该方法的实际效果有限且会消耗过多内存。
（2）因Scan Context被设计为一种全局描述子，但它同样可被用于局部特征描述的潜力并没有被重视和利用，且用于全局描述子的Scan Context方法对于回环相对位姿的估计只能提供旋转分量，而无法提供平移分量的估计。
本发明旨在增强Scan Context方法对平移偏差的鲁棒性，通过改进的ScanContext全局描述来进行鲁棒的回环检测，并在原始Scan Context方法中逐列匹配获得相对位姿的旋转分量估计的基础上，设计基于Scan Context的局部描述子，以通过局部描述子匹配提供相对位姿的平移分量估计。
发明内容
为了改进Scan Context方法存在的对平移的鲁棒性差、只用作全局描述子、只能提供回环相对位姿的旋转分量估计而无法提供平移分量估计的问题，本发明提供一种激光SLAM回环检测与校正方法，可增强原始Scan Context方法对平移偏差的鲁棒性，并进一步设计基于Scan Context局部描述子，以基于局部描述子匹配提供的平移分量进行相对位姿估计。
为解决上述技术问题，本发明具体采用如下技术方案：
一种激光SLAM回环检测与校正方法，包括以下步骤：
步骤一、改进型Scan Context全局描述子构建：
将三维激光雷达当前扫描的激光雷达点云
Figure 643163DEST_PATH_IMAGE001
的所有点
Figure 63780DEST_PATH_IMAGE002
，投影到x-y平面，并计算激光雷达点云
Figure 598666DEST_PATH_IMAGE001
的几何中心
Figure 446536DEST_PATH_IMAGE003
：其中，
Figure 184816DEST_PATH_IMAGE004
代表激光雷达点云
Figure 409124DEST_PATH_IMAGE001
中的一个点，
Figure 736201DEST_PATH_IMAGE005
为激光雷达点云
Figure 82868DEST_PATH_IMAGE001
中点的数量，
Figure 495395DEST_PATH_IMAGE006
分别为点
Figure 257815DEST_PATH_IMAGE004
的x坐标、y坐标、z坐标，并以几何中心
Figure 750982DEST_PATH_IMAGE003
为原点建立极坐标系；
在极坐标系的基础上对激光雷达点云
Figure 206234DEST_PATH_IMAGE001
进行分割和编码，得到改进型Scan Context全局描述子；
步骤二、回环检测：
通过Scan Context方法中的回环检测方法，将当前扫描的激光雷达点云
Figure 168374DEST_PATH_IMAGE001
与历 史扫描的激光雷达点云进行回环关系检测，检测到的回环帧为
Figure 734484DEST_PATH_IMAGE007
；
步骤三、局部描述子构建：
去除当前扫描的激光雷达点云
Figure 770573DEST_PATH_IMAGE001
中的地面点，并提取特征点
Figure 209776DEST_PATH_IMAGE008
，记
Figure 596895DEST_PATH_IMAGE009
为特征点集合，
Figure 701117DEST_PATH_IMAGE010
为
Figure 919609DEST_PATH_IMAGE011
中的特征点的数量，将去除地面点后的激 光雷达点云
Figure 716664DEST_PATH_IMAGE001
所有点投影到x-y平面，得到投影点；设定一个距离阈值r，将激光雷达点 云
Figure 325500DEST_PATH_IMAGE001
对应的投影点中与特征点
Figure 813506DEST_PATH_IMAGE012
的平面距离小于r的所有的投影点
Figure 824187DEST_PATH_IMAGE013
，构成的集合记作
Figure 792143DEST_PATH_IMAGE014
；以特征点
Figure 950592DEST_PATH_IMAGE012
为极坐标原点，通过点集
Figure 396617DEST_PATH_IMAGE014
构建特征点
Figure 527384DEST_PATH_IMAGE012
的局部Scan Context描述子
Figure 479291DEST_PATH_IMAGE015
；
特征点
Figure 62719DEST_PATH_IMAGE012
完整的局部描述子
Figure 312435DEST_PATH_IMAGE016
为：
Figure 360025DEST_PATH_IMAGE017
；
其中
Figure 669784DEST_PATH_IMAGE018
和
Figure 740508DEST_PATH_IMAGE019
分别为点集
Figure 105499DEST_PATH_IMAGE014
中的最高点和最低点，
Figure 945279DEST_PATH_IMAGE020
为
Figure 425939DEST_PATH_IMAGE015
的中心点；形成当前扫描 的激光雷达点云
Figure 780697DEST_PATH_IMAGE001
的局部描述子集
Figure 637794DEST_PATH_IMAGE021
，
Figure 145130DEST_PATH_IMAGE022
为局部描述子集
Figure 796692DEST_PATH_IMAGE021
中局部描述子的数量：
Figure 842008DEST_PATH_IMAGE023
；
步骤四、基于局部Scan Context描述子的扫描匹配和相对位姿解算：
Figure 299534DEST_PATH_IMAGE001
对应的回环帧
Figure 848327DEST_PATH_IMAGE007
的局部描述子集设为
Figure 936369DEST_PATH_IMAGE024
，使用改进型Scan Context全 局描述子以及Scan Context方法中的匹配方法对局部描述子集
Figure 517916DEST_PATH_IMAGE021
和
Figure 451237DEST_PATH_IMAGE024
进行逐列匹 配，获得初始的列对齐结果，对应的列位移值记作
Figure DEST_PATH_IMAGE025
；对于局部描述子
Figure 182433DEST_PATH_IMAGE026
，通过中心 点
Figure 441376DEST_PATH_IMAGE027
在全局Scan Context描述子矩阵中获得局部描述子
Figure 930126DEST_PATH_IMAGE028
对应的行索引
Figure 745766DEST_PATH_IMAGE029
和列索引
Figure 269152DEST_PATH_IMAGE030
； 然后将局部描述子
Figure 495734DEST_PATH_IMAGE031
的匹配范围限定在：行索引
Figure 737359DEST_PATH_IMAGE032
和列索引
Figure 278062DEST_PATH_IMAGE033
满足条件一的局 部描述子；
Figure 967538DEST_PATH_IMAGE034
，
Figure 568284DEST_PATH_IMAGE035
为
Figure 31626DEST_PATH_IMAGE024
中局部描述子
Figure 438337DEST_PATH_IMAGE036
的数量；其中条件一为：
Figure 936314DEST_PATH_IMAGE037
；
其中Δ为固定值，表示搜索的范围；
计算局部描述子
Figure 442382DEST_PATH_IMAGE026
和满足条件一的局部描述子
Figure 471649DEST_PATH_IMAGE031
的Scan Context距离，取Scan Context距离最小值对应的局部描述子
Figure 354154DEST_PATH_IMAGE031
与局部描述子
Figure 768955DEST_PATH_IMAGE026
建立对应关系，进而获得具有对应关系的
Figure 445924DEST_PATH_IMAGE028
和
Figure 883859DEST_PATH_IMAGE036
之间的最高点的点对匹配 关系
Figure 884569DEST_PATH_IMAGE038
和最低点的点对匹配关系
Figure 91559DEST_PATH_IMAGE039
；基于多组点对匹配关系
Figure 267326DEST_PATH_IMAGE038
、
Figure 192556DEST_PATH_IMAGE039
， 使用奇异值分解计算当前扫描的激光雷达点云
Figure 416864DEST_PATH_IMAGE001
和回环帧
Figure 556990DEST_PATH_IMAGE007
之间的全局变换，即获 得相对位姿的初始解；在相对位姿的初始解的基础上将相对位姿精确化，以获得回环相对 位姿的最终结果，实现回环检测的校正。
进一步地，步骤一中，在极坐标系的基础上对激光雷达点云
Figure 841341DEST_PATH_IMAGE001
进行分割和编码 得到改进型Scan Context全局描述子时：通过
Figure 988288DEST_PATH_IMAGE040
个等圆心角间隔的径向线和
Figure 78604DEST_PATH_IMAGE041
个同心圆 将x-y平面分为
Figure 260187DEST_PATH_IMAGE040
*
Figure 27023DEST_PATH_IMAGE041
个小格子，z轴方向上不做分割，进而对每个小格子进行编码，将激光 雷达点云
Figure 926846DEST_PATH_IMAGE001
投影在每个小格子中的点的集合记作
Figure 492957DEST_PATH_IMAGE042
，则点集
Figure 591363DEST_PATH_IMAGE042
的编码
Figure 217516DEST_PATH_IMAGE043
为：
Figure 339056DEST_PATH_IMAGE044
；
其中
Figure 521907DEST_PATH_IMAGE045
指点集
Figure 678081DEST_PATH_IMAGE042
中点
Figure 537453DEST_PATH_IMAGE004
的z坐标值，
Figure 146289DEST_PATH_IMAGE046
为按圆心角排列的索引号，
Figure 54202DEST_PATH_IMAGE047
，
Figure 391116DEST_PATH_IMAGE048
为按同心圆排列的索引号，
Figure 359072DEST_PATH_IMAGE049
；
激光雷达点云
Figure 455204DEST_PATH_IMAGE001
所有点集
Figure 229125DEST_PATH_IMAGE042
的编码
Figure 359892DEST_PATH_IMAGE043
形成的
Figure 498750DEST_PATH_IMAGE050
大小的矩阵
Figure 629648DEST_PATH_IMAGE051
，即为所述的改进型Scan Context全局描述子。
进一步地，步骤三中，通过点集
Figure 144943DEST_PATH_IMAGE014
构建特征点
Figure 192533DEST_PATH_IMAGE012
的局部Scan Context描述子
Figure 502292DEST_PATH_IMAGE015
时，如果某一特征点
Figure 307437DEST_PATH_IMAGE012
被计算了局部Scan Context描述子，则对与特征点
Figure 672428DEST_PATH_IMAGE012
的平面距离小 于r的特征点不再计算对应的局部Scan Context描述子；因为这些特征点彼此接近，他们的 局部描述子会相似，对有效的特征匹配不利。
进一步地，步骤一中，计算激光雷达点云
Figure 512208DEST_PATH_IMAGE001
的几何中心
Figure 992868DEST_PATH_IMAGE003
时：
Figure 613205DEST_PATH_IMAGE052
。
进一步地，步骤三中，利用LeGO-LOAM算法去除当前扫描的激光雷达点云
Figure 204723DEST_PATH_IMAGE001
中的 地面点，并提取特征点
Figure 164589DEST_PATH_IMAGE008
。
进一步地，使用迭代最近点方法在相对位姿的初始解的基础上将相对位姿精确化。
与现有技术相比，本发明的有益技术效果是：
本发明可以更加鲁棒地检测回环并更好地解算出回环之间的相对位姿变换。本发明通过利用扫描点云的几何中心作为极坐标系原点建立改进型Scan Context全局描述子，增强了Scan Context方法对平移偏差的鲁棒性；并通过设计基于特征点的Scan Context局部描述子集，使得可以同时通过描述子解算出回环相对位姿的旋转分量和平移分量，为后续的迭代最近点操作提供了更优的位姿初始解，因此可以减小迭代最近点操作陷入局部最优解的概率，提高最终解算出的位姿的准确性。整体方案鲁棒性和适应性强、效率高、可移植性好，且占用计算资源少。
附图说明
图1为本发明中的激光SLAM回环检测与校正方法的流程图；
图2为本发明中点云划分的示意图。
具体实施方式
下面结合附图对本发明的一种优选实施方式作详细的说明。
如图1所示，本发明中的激光SLAM回环检测与校正方法，包括以下步骤：
步骤一、改进型Scan Context全局描述子构建：
将三维激光雷达当前扫描得到的激光雷达点云
Figure 629200DEST_PATH_IMAGE001
的所有点
Figure 408937DEST_PATH_IMAGE002
，投影到x-y平面；并计算激光雷达点云的几何中心
Figure 804146DEST_PATH_IMAGE003
：
Figure 680835DEST_PATH_IMAGE052
；
其中，
Figure 503298DEST_PATH_IMAGE004
代表激光雷达点云中的一个点，
Figure 84845DEST_PATH_IMAGE005
为激光雷达点云中点的数量，
Figure 283745DEST_PATH_IMAGE006
分 别为点的的x坐标、y坐标、z坐标，并以几何中心
Figure 952624DEST_PATH_IMAGE003
为原点建立极坐标系。
在上述极坐标系的基础上，对当前激光雷达点云进行分割和编码，具体包括：
按照等间隔的圆心角和径向距离将x-y平面分为多个小格子，z轴方向上不做分 割，如图2所示。按圆心角分为
Figure 8305DEST_PATH_IMAGE040
份，即得到
Figure 762634DEST_PATH_IMAGE040
个扇形，按径向距离分为
Figure 765225DEST_PATH_IMAGE041
份，即得到
Figure 101660DEST_PATH_IMAGE041
个同 心圆，被扇形和同心圆分割得到的小格子总数为
Figure 265925DEST_PATH_IMAGE040
*
Figure 507550DEST_PATH_IMAGE041
。进而对每个小格子进行编码，若将 投影在每个小格子中的点的集合记作
Figure 110570DEST_PATH_IMAGE042
，
Figure 488462DEST_PATH_IMAGE046
为按圆心角排列的索引号，
Figure 89207DEST_PATH_IMAGE047
，
Figure 864134DEST_PATH_IMAGE048
为按 同心圆排列的索引号，
Figure 208528DEST_PATH_IMAGE053
，则点集
Figure 503243DEST_PATH_IMAGE042
的编码
Figure 274890DEST_PATH_IMAGE043
为：
Figure 225528DEST_PATH_IMAGE044
；
其中
Figure 921083DEST_PATH_IMAGE045
指点集
Figure 273567DEST_PATH_IMAGE042
中点
Figure 216115DEST_PATH_IMAGE004
的z坐标值。
全局描述子为激光雷达点云
Figure 716367DEST_PATH_IMAGE001
所有点集
Figure 402563DEST_PATH_IMAGE042
的编码
Figure 609553DEST_PATH_IMAGE043
形成的
Figure 37517DEST_PATH_IMAGE050
大小 的矩阵
Figure 962748DEST_PATH_IMAGE051
。
步骤二、回环检测：
通过Scan Context方法中的回环检测方法，将当前扫描的激光雷达点云与历史扫 描的激光雷达点云进行回环关系检测。记当前扫描的激光雷达点云为
Figure 187056DEST_PATH_IMAGE001
，记检测到的回 环帧为
Figure 576449DEST_PATH_IMAGE007
。
步骤三、局部描述子构建：
利用LeGO-LOAM算法去除当前扫描的激光雷达点云
Figure 595220DEST_PATH_IMAGE001
中的地面点，并提取特征 点，特征点
Figure 7747DEST_PATH_IMAGE008
包括边缘点集（edge points）和表面点集（surface points），记
Figure 848795DEST_PATH_IMAGE009
为特征点集合。将去除地面点后的激光雷达点云
Figure 30378DEST_PATH_IMAGE001
所有点投 影到x-y平面，得到投影点；设定一个距离阈值r，将激光雷达点云
Figure 485630DEST_PATH_IMAGE001
对应的投影点中与 特征点
Figure 182190DEST_PATH_IMAGE012
的平面距离小于r的所有的投影点
Figure 748301DEST_PATH_IMAGE013
构成的集合记作
Figure 49969DEST_PATH_IMAGE014
。通过以特征点
Figure 987707DEST_PATH_IMAGE012
为极坐 标原点，基于点集
Figure 109247DEST_PATH_IMAGE014
构建特征点
Figure 479048DEST_PATH_IMAGE012
的局部Scan Context描述子，记作
Figure 697540DEST_PATH_IMAGE015
。
另外，若点
Figure 494595DEST_PATH_IMAGE054
被计算了局部Scan Context描述子，则与特征点
Figure 103431DEST_PATH_IMAGE054
的平面距离小于r 的特征点将不再计算这些特征点对应的描述子，因为这些点彼此接近，他们的局部描述子 会相似，对有效的特征匹配不利。特征点
Figure 824393DEST_PATH_IMAGE012
完整的局部描述子
Figure 835075DEST_PATH_IMAGE016
：
Figure 803031DEST_PATH_IMAGE017
；
其中
Figure 961479DEST_PATH_IMAGE018
和
Figure 673084DEST_PATH_IMAGE019
分别为点集
Figure 538271DEST_PATH_IMAGE014
中的最高点和最低点，在随后的扫描匹配过程中可用于 提供刚性位姿约束，
Figure 991643DEST_PATH_IMAGE020
为
Figure 575071DEST_PATH_IMAGE015
的中心点。
通过上述方法，形成当前扫描的激光雷达点云的局部描述子集
Figure 152683DEST_PATH_IMAGE021
，
Figure 872377DEST_PATH_IMAGE022
为局部描 述子集
Figure 182136DEST_PATH_IMAGE021
中局部描述子的数量：
Figure 65909DEST_PATH_IMAGE055
。
步骤四、基于局部Scan Context描述子的扫描匹配和相对位姿解算：
当前扫描的激光雷达点云
Figure 119316DEST_PATH_IMAGE001
的局部描述子集为
Figure 959096DEST_PATH_IMAGE021
，
Figure 502073DEST_PATH_IMAGE001
对应的回环帧
Figure 60093DEST_PATH_IMAGE007
的局部描述子集设为
Figure 651611DEST_PATH_IMAGE024
，使用改进型Scan Context全局描述子
Figure 657482DEST_PATH_IMAGE056
以及Scan Context方法 中的匹配方法进行逐列匹配获得初始的列对齐结果，对应的列位移值记作
Figure 574623DEST_PATH_IMAGE025
。对于局部描述 子
Figure 416677DEST_PATH_IMAGE026
，根据中心点
Figure 811886DEST_PATH_IMAGE027
在
Figure 626258DEST_PATH_IMAGE001
的全局Scan Context描述子矩阵中获得
Figure 261770DEST_PATH_IMAGE028
对应的 行索引
Figure 528803DEST_PATH_IMAGE029
、列索引
Figure 727704DEST_PATH_IMAGE030
，记作
Figure 458899DEST_PATH_IMAGE057
。然后将
Figure 452263DEST_PATH_IMAGE031
的匹配范围限定在
Figure 206592DEST_PATH_IMAGE024
中行索引
Figure 246400DEST_PATH_IMAGE032
、列索引
Figure 769785DEST_PATH_IMAGE033
满足以下条件一的局部描述子中；
Figure 199629DEST_PATH_IMAGE034
，
Figure 503572DEST_PATH_IMAGE035
为
Figure 778695DEST_PATH_IMAGE024
中局部描述子
Figure 422166DEST_PATH_IMAGE036
的 数量；条件一：
Figure 570382DEST_PATH_IMAGE037
；
其中Δ为固定值，表示搜索的范围。若上述索引超出描述子矩阵的范围，则忽略超出的区域。
然后计算
Figure 299303DEST_PATH_IMAGE026
和满足条件一的
Figure 378118DEST_PATH_IMAGE031
的Scan Context距离，取Scan Context距离最小值对应的
Figure 938412DEST_PATH_IMAGE031
与
Figure 710059DEST_PATH_IMAGE026
建立对应关系。然后就获得了两者之 间的最高点的点对匹配关系
Figure 660698DEST_PATH_IMAGE038
和最低点的点对匹配关系
Figure 854787DEST_PATH_IMAGE039
。基于点对匹配关 系，使用奇异值分解（SVD）计算当前扫描的激光雷达点云
Figure 207271DEST_PATH_IMAGE001
和回环帧
Figure 149819DEST_PATH_IMAGE007
之间的全局变 换，即获得相对位姿的初始解。并在该相对位姿的初始解的基础上使用迭代最近点（ICP）方 法将相对位姿精确化，以获得回环相对位姿的最终结果，实现回环检测的校正。
对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内，不应将权利要求中的任何附图标记视为限制所涉及的权利要求。
此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立技术方案，说明书的这种叙述方式仅仅是为了清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

Claims (6)

1.一种激光SLAM回环检测与校正方法，包括以下步骤：
步骤一、改进型Scan Context全局描述子构建：将三维激光雷达当前扫描的激光雷达 点云
Figure 666312DEST_PATH_IMAGE001
的所有点
Figure 824761DEST_PATH_IMAGE002
，投影到x-y平面，并计算激光雷达点云
Figure 536365DEST_PATH_IMAGE001
的几何中心
Figure 401553DEST_PATH_IMAGE003
；其中，
Figure 854924DEST_PATH_IMAGE004
代表激光雷达点云
Figure 438352DEST_PATH_IMAGE001
中的一个点，
Figure 15964DEST_PATH_IMAGE005
为激光雷达点云
Figure 735658DEST_PATH_IMAGE001
中点的数量，
Figure 858466DEST_PATH_IMAGE006
分别为点
Figure 929190DEST_PATH_IMAGE004
的x坐标、y坐标、z坐标，并以几何中心
Figure 982597DEST_PATH_IMAGE003
为原点建 立极坐标系；在极坐标系的基础上对激光雷达点云
Figure 884694DEST_PATH_IMAGE001
进行分割和编码，得到改进型Scan Context全局描述子；
步骤二、回环检测：通过Scan Context方法中的回环检测方法，将当前扫描的激光雷达 点云
Figure 365354DEST_PATH_IMAGE001
与历史扫描的激光雷达点云进行回环关系检测，检测到的回环帧为
Figure 923374DEST_PATH_IMAGE007
；
步骤三、局部描述子构建：去除当前扫描的激光雷达点云
Figure 826477DEST_PATH_IMAGE001
中的地面点，并提取特征 点
Figure 520764DEST_PATH_IMAGE008
，记
Figure 500221DEST_PATH_IMAGE009
为特征点集合，
Figure 279958DEST_PATH_IMAGE010
为
Figure 675167DEST_PATH_IMAGE011
中的特征点的数量，将去除地面点 后的激光雷达点云
Figure 302589DEST_PATH_IMAGE001
所有点投影到x-y平面，得到投影点；设定一个距离阈值r，将激光 雷达点云
Figure 125051DEST_PATH_IMAGE001
对应的投影点中与特征点
Figure 454402DEST_PATH_IMAGE012
的平面距离小于r的所有的投影点
Figure 653302DEST_PATH_IMAGE013
，构成的集 合记作
Figure 624976DEST_PATH_IMAGE014
；以特征点
Figure 618340DEST_PATH_IMAGE012
为极坐标原点，通过点集
Figure 372669DEST_PATH_IMAGE014
构建特征点
Figure 171998DEST_PATH_IMAGE012
的局部Scan Context描述 子
Figure 695383DEST_PATH_IMAGE015
；
特征点
Figure 125227DEST_PATH_IMAGE012
完整的局部描述子
Figure 179902DEST_PATH_IMAGE016
为：
Figure 455026DEST_PATH_IMAGE017
；
其中
Figure 160814DEST_PATH_IMAGE018
和
Figure 495980DEST_PATH_IMAGE019
分别为点集
Figure 224901DEST_PATH_IMAGE014
中的最高点和最低点，
Figure 615300DEST_PATH_IMAGE020
为
Figure 113278DEST_PATH_IMAGE015
的中心点；形成当前扫描的激 光雷达点云
Figure 947242DEST_PATH_IMAGE001
的局部描述子集
Figure 897880DEST_PATH_IMAGE021
，
Figure 780385DEST_PATH_IMAGE022
为局部描述子集
Figure 945919DEST_PATH_IMAGE021
中局部描述子的数量：
Figure 888467DEST_PATH_IMAGE023
；
步骤四、基于局部Scan Context描述子的扫描匹配和相对位姿解算：
Figure 326402DEST_PATH_IMAGE001
对应的回环 帧
Figure 809335DEST_PATH_IMAGE007
的局部描述子集设为
Figure 281905DEST_PATH_IMAGE024
，使用改进型Scan Context全局描述子以及Scan Context方法中的匹配方法对局部描述子集
Figure 395355DEST_PATH_IMAGE021
和
Figure 635099DEST_PATH_IMAGE024
进行逐列匹配，获得初始的列对齐 结果，对应的列位移值记作
Figure 859407DEST_PATH_IMAGE025
；对于局部描述子
Figure 920904DEST_PATH_IMAGE026
，通过中心点
Figure 205255DEST_PATH_IMAGE027
在
Figure 680099DEST_PATH_IMAGE001
的全局 Scan Context描述子矩阵中获得局部描述子
Figure 708098DEST_PATH_IMAGE028
对应的行索引
Figure 889680DEST_PATH_IMAGE029
和列索引
Figure 892403DEST_PATH_IMAGE030
；然后将局部 描述子
Figure 792225DEST_PATH_IMAGE031
的匹配范围限定在：行索引
Figure 420653DEST_PATH_IMAGE032
和列索引
Figure 722321DEST_PATH_IMAGE033
满足条件一的局部描述子；
Figure 82895DEST_PATH_IMAGE034
，
Figure 781599DEST_PATH_IMAGE035
为
Figure 151400DEST_PATH_IMAGE024
中局部描述子
Figure 41996DEST_PATH_IMAGE036
的数量；其中条件一为：
Figure 901368DEST_PATH_IMAGE037
；
其中Δ为固定值，表示搜索的范围；计算局部描述子
Figure 510203DEST_PATH_IMAGE026
和满足条件一的局部 描述子
Figure 496745DEST_PATH_IMAGE031
的Scan Context距离，取Scan Context距离最小值对应的局部描述子
Figure 507426DEST_PATH_IMAGE031
与局部描述子
Figure 537699DEST_PATH_IMAGE026
建立对应关系，进而获得具有对应关系的
Figure 633831DEST_PATH_IMAGE028
和
Figure 345435DEST_PATH_IMAGE036
之间的最高点的点对匹配关系
Figure 525137DEST_PATH_IMAGE038
和最低点的点对匹配关系
Figure 663995DEST_PATH_IMAGE039
；基于点对匹 配关系
Figure 247423DEST_PATH_IMAGE038
、
Figure 559455DEST_PATH_IMAGE039
，使用奇异值分解计算当前扫描的激光雷达点云
Figure 544729DEST_PATH_IMAGE001
和回环帧
Figure 854488DEST_PATH_IMAGE007
之间的全局变换，即获得相对位姿的初始解；在相对位姿的初始解的基础上将相对位 姿精确化，以获得回环相对位姿的最终结果，实现回环检测的校正。
2.根据权利要求1所述的激光SLAM回环检测与校正方法，其特征在于，步骤一中，在极 坐标系的基础上对激光雷达点云
Figure 738261DEST_PATH_IMAGE001
进行分割和编码得到改进型Scan Context全局描述 子时：通过
Figure 791668DEST_PATH_IMAGE040
个等圆心角间隔的径向线和
Figure 631448DEST_PATH_IMAGE041
个同心圆将x-y平面分为
Figure 174425DEST_PATH_IMAGE040
*
Figure 466866DEST_PATH_IMAGE041
个小格子，z轴 方向上不做分割，进而对每个小格子进行编码，将激光雷达点云
Figure 323963DEST_PATH_IMAGE001
投影在每个小格子中 的点的集合记作
Figure 329834DEST_PATH_IMAGE042
，则点集
Figure 981395DEST_PATH_IMAGE042
的编码
Figure 26712DEST_PATH_IMAGE043
为：
Figure 484238DEST_PATH_IMAGE044
；
其中
Figure 33031DEST_PATH_IMAGE045
指点集
Figure 121073DEST_PATH_IMAGE042
中点
Figure 201155DEST_PATH_IMAGE004
的z坐标值，
Figure 400055DEST_PATH_IMAGE046
为按圆心角排列的索引号，
Figure 803355DEST_PATH_IMAGE047
，
Figure 124615DEST_PATH_IMAGE048
为按 同心圆排列的索引号，
Figure 878944DEST_PATH_IMAGE049
；
激光雷达点云
Figure 615956DEST_PATH_IMAGE001
所有点集
Figure 453855DEST_PATH_IMAGE042
的编码
Figure 883700DEST_PATH_IMAGE043
形成的
Figure 859746DEST_PATH_IMAGE050
大小的矩阵
Figure 462766DEST_PATH_IMAGE051
，即为所述的改进型Scan Context全局描述子。
3.根据权利要求1所述的激光SLAM回环检测与校正方法，其特征在于，步骤三中，通过 点集
Figure 106237DEST_PATH_IMAGE014
构建特征点
Figure 441403DEST_PATH_IMAGE012
的局部Scan Context描述子
Figure 983374DEST_PATH_IMAGE015
时，如果某一特征点
Figure 62188DEST_PATH_IMAGE012
被计算了局部 Scan Context描述子，则对与特征点
Figure 560166DEST_PATH_IMAGE012
的平面距离小于r的特征点不再计算对应的局部 Scan Context描述子。
4.根据权利要求1所述的激光SLAM回环检测与校正方法，其特征在于，步骤一中，计算 激光雷达点云
Figure 128550DEST_PATH_IMAGE001
的几何中心
Figure 344768DEST_PATH_IMAGE003
时：
Figure 538858DEST_PATH_IMAGE052
。
5.根据权利要求1所述的激光SLAM回环检测与校正方法，其特征在于，步骤三中，利用 LeGO-LOAM算法去除当前扫描的激光雷达点云
Figure 891342DEST_PATH_IMAGE001
中的地面点，并提取特征点
Figure 833890DEST_PATH_IMAGE008
。
6.根据权利要求1所述的激光SLAM回环检测与校正方法，其特征在于，步骤四中，使用迭代最近点方法在相对位姿的初始解的基础上将相对位姿精确化。
CN202211611667.6A 2022-12-15 2022-12-15 一种激光slam回环检测与校正方法 Active CN115661255B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211611667.6A CN115661255B (zh) 2022-12-15 2022-12-15 一种激光slam回环检测与校正方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211611667.6A CN115661255B (zh) 2022-12-15 2022-12-15 一种激光slam回环检测与校正方法

Publications (2)

Publication Number Publication Date
CN115661255A true CN115661255A (zh) 2023-01-31
CN115661255B CN115661255B (zh) 2023-04-28

Family

ID=85022382

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211611667.6A Active CN115661255B (zh) 2022-12-15 2022-12-15 一种激光slam回环检测与校正方法

Country Status (1)

Country Link
CN (1) CN115661255B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117968667A (zh) * 2024-04-02 2024-05-03 国网江苏省电力有限公司常州供电分公司 巡检机器人回环检测的slam点云地图构建方法及系统

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110910389A (zh) * 2019-10-30 2020-03-24 中山大学 一种基于图描述子的激光slam回环检测系统及方法
CN112665575A (zh) * 2020-11-27 2021-04-16 重庆大学 一种基于移动机器人的slam回环检测方法
CN113792699A (zh) * 2021-09-24 2021-12-14 北京易航远智科技有限公司 一种基于语义点云的对象级快速场景识别方法
CN114187418A (zh) * 2021-12-14 2022-03-15 北京易航远智科技有限公司 回环检测方法、点云地图构建方法、电子设备及存储介质
CN114283250A (zh) * 2021-12-23 2022-04-05 武汉理工大学 高精度的三维点云地图自动拼接与优化方法及系统
CN114299386A (zh) * 2021-12-17 2022-04-08 重庆大学 一种融合激光里程计和回环检测的激光slam方法
CN115420276A (zh) * 2022-08-29 2022-12-02 北京化工大学 一种面向室外场景的多机器人协同定位与建图方法
CN115420275A (zh) * 2022-08-22 2022-12-02 先临三维科技股份有限公司 回环路径的预测方法及装置、非易失性存储介质、处理器

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110910389A (zh) * 2019-10-30 2020-03-24 中山大学 一种基于图描述子的激光slam回环检测系统及方法
CN112665575A (zh) * 2020-11-27 2021-04-16 重庆大学 一种基于移动机器人的slam回环检测方法
CN113792699A (zh) * 2021-09-24 2021-12-14 北京易航远智科技有限公司 一种基于语义点云的对象级快速场景识别方法
CN114187418A (zh) * 2021-12-14 2022-03-15 北京易航远智科技有限公司 回环检测方法、点云地图构建方法、电子设备及存储介质
CN114299386A (zh) * 2021-12-17 2022-04-08 重庆大学 一种融合激光里程计和回环检测的激光slam方法
CN114283250A (zh) * 2021-12-23 2022-04-05 武汉理工大学 高精度的三维点云地图自动拼接与优化方法及系统
CN115420275A (zh) * 2022-08-22 2022-12-02 先临三维科技股份有限公司 回环路径的预测方法及装置、非易失性存储介质、处理器
CN115420276A (zh) * 2022-08-29 2022-12-02 北京化工大学 一种面向室外场景的多机器人协同定位与建图方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
GUANGHUI XUE 等: "An Improved Simultaneous Localization and Mapping Method Fusing LeGO-LOAM and Scan Context for Underground Coalmine" *
MINGCE GUO 等: "3D Lidar SLAM Based on Ground Segmentation and Scan Context Loop Detection" *
周治国 等: "3D激光雷达SLAM 算法综述" *
李炯 等: "基于SR-Context的激光雷达点云闭环检测算法" *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117968667A (zh) * 2024-04-02 2024-05-03 国网江苏省电力有限公司常州供电分公司 巡检机器人回环检测的slam点云地图构建方法及系统

Also Published As

Publication number Publication date
CN115661255B (zh) 2023-04-28

Similar Documents

Publication Publication Date Title
Jiang et al. Robust feature matching using spatial clustering with heavy outliers
Guo et al. Deep learning for 3d point clouds: A survey
Hui et al. Efficient 3D point cloud feature learning for large-scale place recognition
CN106780557B (zh) 一种基于光流法和关键点特征的运动目标跟踪方法
Jayaram et al. Convex hulls in image processing: a scoping review
Li et al. Multi-scale neighborhood feature extraction and aggregation for point cloud segmentation
Lu et al. Deep learning for 3d point cloud understanding: a survey
Salazar et al. Fully automatic expression-invariant face correspondence
CN110334762B (zh) 一种基于四叉树结合orb和sift的特征匹配方法
Qian et al. RSDet++: Point-based modulated loss for more accurate rotated object detection
Li et al. A complex junction recognition method based on GoogLeNet model
Wei et al. Fgr: Frustum-aware geometric reasoning for weakly supervised 3d vehicle detection
Liu et al. A novel rock-mass point cloud registration method based on feature line extraction and feature point matching
Gong et al. A two-level framework for place recognition with 3D LiDAR based on spatial relation graph
CN115661255A (zh) 一种激光slam回环检测与校正方法
Zhu et al. A review of 6d object pose estimation
Sun et al. Oriented point sampling for plane detection in unorganized point clouds
Guo et al. Line-based 3d building abstraction and polygonal surface reconstruction from images
Kong et al. Explicit points-of-interest driven Siamese transformer for 3D LiDAR place recognition in outdoor challenging environments
Liu et al. Multi-modal fusion based on depth adaptive mechanism for 3D object detection
Zhao et al. Learning probabilistic coordinate fields for robust correspondences
Tang et al. Place recognition using line-junction-lines in urban environments
Yang et al. Ground plane matters: Picking up ground plane prior in monocular 3d object detection
Yang et al. GPro3D: Deriving 3D BBox from ground plane in monocular 3D object detection
Liu et al. DS Augmentation: Density-semantics augmentation for 3-D 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