CN111429344A - 基于感知哈希的激光slam闭环检测方法及系统 - Google Patents

基于感知哈希的激光slam闭环检测方法及系统 Download PDF

Info

Publication number
CN111429344A
CN111429344A CN202010102282.1A CN202010102282A CN111429344A CN 111429344 A CN111429344 A CN 111429344A CN 202010102282 A CN202010102282 A CN 202010102282A CN 111429344 A CN111429344 A CN 111429344A
Authority
CN
China
Prior art keywords
depth map
rotation
module
radar
calculating
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
CN202010102282.1A
Other languages
English (en)
Other versions
CN111429344B (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.)
Shanghai Jiaotong University
Original Assignee
Shanghai Jiaotong 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 Shanghai Jiaotong University filed Critical Shanghai Jiaotong University
Priority to CN202010102282.1A priority Critical patent/CN111429344B/zh
Publication of CN111429344A publication Critical patent/CN111429344A/zh
Application granted granted Critical
Publication of CN111429344B publication Critical patent/CN111429344B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/521—Depth or shape recovery from laser ranging, e.g. using interferometry; from the projection of structured light
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T3/00—Geometric image transformations in the plane of the image
    • G06T3/40—Scaling of whole images or parts thereof, e.g. expanding or contracting
    • G06T3/4007—Scaling of whole images or parts thereof, e.g. expanding or contracting based on interpolation, e.g. bilinear interpolation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T3/00—Geometric image transformations in the plane of the image
    • G06T3/60—Rotation of whole images or parts thereof
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/60—Analysis of geometric attributes
    • G06T7/62—Analysis of geometric attributes of area, perimeter, diameter or volume
    • 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
    • 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/10032—Satellite or aerial image; Remote sensing
    • G06T2207/10044—Radar image
    • 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/20—Special algorithmic details
    • G06T2207/20048—Transform domain processing
    • G06T2207/20052—Discrete cosine transform [DCT]

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Physics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Data Mining & Analysis (AREA)
  • Computational Mathematics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Mathematical Analysis (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Computing Systems (AREA)
  • Optics & Photonics (AREA)
  • Algebra (AREA)
  • Geometry (AREA)
  • Image Analysis (AREA)
  • Image Processing (AREA)

Abstract

本发明提供了一种基于感知哈希的激光SLAM闭环检测方法及系统，包括:激光扫描雷达每次旋转预设角度过程中扫描获得的点云，经过前端里程计和后端优化得到激光雷达位姿；根据获得的激光雷达位姿，构建每次旋转的深度图；根据每次旋转的深度图，计算深度图中心部分圆形区域的强度中心，确定深度图主方向；根据深度图主方向对深度图中各点坐标旋转变换，构建变换后的深度图；根据变换后的深度图感知哈希计算图片指纹，获得图片指纹；将获得图片指纹与每次旋转的深度图指纹依次计算汉明距离，根据汉明距离计算相似度，当相似度最大值大于阈值时，则判断为闭环。本发明计算量小，适用于廉价的嵌入式设备，具有实时性，准确性高，抗噪声能力强的特点。

Description

基于感知哈希的激光SLAM闭环检测方法及系统
技术领域
本发明涉及定位与导航领域，具体地，涉及一种基于感知哈希的激光SLAM闭环检测方法及系统。
背景技术
SLAM(simultaneous localization and mapping),即时定位与地图构建。问题可以描述为：将一个机器人放入未知环境中的未知位置，是否有办法让机器人一边移动一边逐步描绘出此环境完全的地图，在递增地建立地图的同时，利用已建立的地图来同步刷新自身的位置。这是一个相辅相成不断迭代的过程，也是移动机器人自主完成作业任务需要解决的基本问题。目前依据使用的传感器不同，SLAM技术主要分为两大类：基于激光雷达的激光SLAM和基于摄像头的视觉SLAM。在测量平台移动的过程中，由于前端位姿估计和后端优化均基于部分相邻数据，之前产生的误差将不可避免地累计到下一个时刻，使得整个SLAM将会出现累积误差，长期估计的结果将不可靠。为消除这类误差，SLAM系统中引入闭环检测模块。闭环检测，又称回环检测(Loop Closure Detection)，主要解决位置估计随时间漂移的问题，通过识别测量平台曾经达到过的场景，从而在位姿优化中添加时隔更加久远的约束，使之得到更好的、全局一致的位姿估计。另一方面，由于闭环检测提供了当前数据与所有历史数据的关联，在跟踪丢失后，还可以利用闭环检测进行重定位。因此，闭环检测对整个SLAM系统精度和鲁棒性有非常明显的提升。
传统的激光SLAM闭环检测算法主要有如下两种：
ICP算法，即迭代最近点(Iterative Closest Point)：该算法是一种点集对点集配准方法，借助迭代优化的思想，通过旋转、平移使得两个点集之间累计的距离最小，从而实现相似部分点云的匹配。该方法有较为可靠的准确率，然而运算效率较低，在搜索对应点的过程中，计算量非常大，难以在廉价的嵌入式设备上实现实时运行。
词袋法(Bag of Words,BoW)：该算法是一种基于特征点的匹配方法，首先从每次扫描的点云中提取部分特征点和特征描述，再将所有特征描述进行聚类，并组织成树的形式方便搜索构成词典，当两片点云中包含词典中一定量的相似点时，可以认为其存在相似性。然而该方仅仅基于图像局部特征，没有考虑特征点之间的相互关系，且特征点的稠密程度往往取决于工作环境，因而应用在激光SLAM上时准确度和可靠性较差。
发明内容
针对现有技术中的缺陷，本发明的目的是提供一种基于感知哈希的激光SLAM闭环检测方法及系统。
根据本发明提供的一种基于感知哈希的激光SLAM闭环检测方法，包括:
步骤M1：激光扫描雷达每次旋转预设角度过程中扫描获得的点云，经过前端里程计和后端优化得到激光雷达位姿；
步骤M2：根据获得的激光雷达位姿，构建每次旋转的深度图；
步骤M3：根据每次旋转的深度图，计算深度图中心部分圆形区域的强度中心，确定深度图主方向；
步骤M4：根据深度图主方向对深度图中各点坐标旋转变换，构建变换后的深度图；
步骤M5：根据变换后的深度图感知哈希计算图片指纹，获得图片指纹；
步骤M6：将获得图片指纹与每次旋转的深度图指纹依次计算汉明距离，根据汉明距离计算相似度，当相似度最大值大于阈值时，则判断为闭环。
优选地，所述步骤M1中扫描获得的点云经过前端里程计和后端优化得到的激光雷达位姿包括：
对于第k次扫描，记第k次扫描起始时刻为tk,结束时刻为tk+1，两时刻间雷达位姿变换表达式为：
Figure BDA0002387258450000021
其中，tx，ty，tz分别表示雷达位姿沿雷达坐标系x，y，z轴的平移变换，θx，θy，θz分别表示旋转变换，符合右手定则；[]T表示转置；
使用线性插值法计算每次扫描时的雷达位姿：
Figure BDA0002387258450000022
其中，ti表示第i次扫描对应时刻，
Figure BDA0002387258450000031
表示第i次扫描时雷达位姿；
所述步骤M2包括：
步骤M2.1：根据每次扫描的激光雷达位姿，依据欧氏变换，将当前扫描获得的点云投影到当前旋转起始时刻：
Figure BDA0002387258450000032
其中，
Figure BDA0002387258450000033
为扫描时获得的点云各点坐标，
Figure BDA0002387258450000034
为对应的旋转起始时刻坐标，
Figure BDA0002387258450000035
为
Figure BDA0002387258450000036
中前三项，R根据罗德里格斯公式求得:
Figure BDA0002387258450000037
其中：
Figure BDA0002387258450000038
Figure BDA0002387258450000039
为ω所对应的斜对称矩阵:
Figure BDA00023872584500000310
Figure BDA00023872584500000311
其中，I表示单位矩阵，w表示三维向量，ω＝(ω1，ω2，ω3),w1，w2，w3分别表示w三个维度的值，
Figure BDA00023872584500000312
表示
Figure BDA00023872584500000313
中第四项至第六项；
步骤M2.2：根据当前扫描获得的点云投影到当前旋转起始时刻的点云坐标建立当前旋转所对应的深度图，
Figure BDA00023872584500000314
中点云各点坐标为三维坐标(x，y，z)，转换为当前旋转所对应的深度图，即点云各点坐标(x，y)对应的深度D(x，y)＝z，即tk坐标(x，y)各点对应的深度D(x，y)。
优选地，所述步骤M3包括：
步骤M3.1：取深度图中心部分圆形区域；设深度图宽度为w，高度为h，令圆形区域半径
Figure BDA00023872584500000315
圆形区域圆心与深度图中心重合；
计算深度图强度中心，所取的圆形区域表达式如下：
Figure BDA00023872584500000316
Figure BDA00023872584500000317
其中，(x，y)满足：x2+y2≤r2，则强度中心C定义为：C(xc，yc)；
步骤M3.2：根据强度中心C计算向量
Figure BDA0002387258450000041
的方向，即深度图主方向：
Figure BDA0002387258450000042
所述步骤M4包括：
步骤M4.1：为实现图像的旋转不变性，对深度图D中心部分进行坐标变换，得到变换后的深度图Dθ，对于变换后的深度图Dθ中的坐标Dθ(x，y)在深度图D中对应的坐标D(x′，y′)满足以下关系：
Figure BDA0002387258450000043
Dθ(x，y)＝D(x′，y′) (12)
其中，Rθ为旋转矩阵，表达式如下：
Figure BDA0002387258450000044
变换后的深度图Dθ坐标(x，y)取值范围满足：
Figure BDA0002387258450000045
变换后的深度图为边长
Figure BDA0002387258450000046
像素的正方形；
对变换后的深度图Dθ中的坐标Dθ(x，y)在深度图D中对应的坐标D(x′，y′)使用双线性插值法计算；
优选地，所述步骤M5包括：
步骤M5.1：将深度图Dθ尺寸缩小为N×N，得到D′θ∈RN×N，R代表实数集合，RN×N代表N×N矩阵，且矩阵每个元素为实数；
步骤M5.2：对尺寸缩小后的深度图进行离散余弦变换，定义F∈RN×N为离散余弦变换结果，离散余弦变换结果F表达式如下：
Figure BDA0002387258450000047
其中，c(u)为补偿系数，c(v)表示补偿系数；如下所示：
Figure BDA0002387258450000048
步骤M5.3：获取矩阵F左上角
Figure BDA0002387258450000049
低频部分，其中，k为大于1的整数，矩阵F低频部分集中了深度图的主要信息，记为
Figure BDA00023872584500000410
步骤M5.4：计算F′中所有元素的平均值，记为
Figure BDA0002387258450000051
将F′中所有大于
Figure BDA0002387258450000052
的元素设为“1”，小于
Figure BDA0002387258450000053
的元素设为“0”；
步骤M5.5：将矩阵F′各列相连，转换为向量hk，
Figure BDA0002387258450000054
即为第k次旋转对应深度图的图片指纹。
优选地，所述步骤M6包括：
步骤M6.1：将hk与hj，j∈{1，2，…，k-1}，分别计算汉明距离dkj，公式如下:
dkj＝||hk-hj|| (16)
其中，hj表示每次旋转的深度图指纹；
步骤M6.2：根据汉明距离计算相似度skj，公式如下：
Figure BDA0002387258450000055
找到相似度的最大值ski：
Figure BDA0002387258450000056
当相似度的最大值ski大于阈值Ts，则判定当前雷达第k次旋转与第i次旋转时所在位置相同，即回环成立。
根据本发明提供的一种基于感知哈希的激光SLAM闭环检测系统，包括:
模块M1：激光扫描雷达每次旋转预设角度过程中扫描获得的点云，经过前端里程计和后端优化得到激光雷达位姿；
模块M2：根据获得的激光雷达位姿，构建每次旋转的深度图；
模块M3：根据每次旋转的深度图，计算深度图中心部分圆形区域的强度中心，确定深度图主方向；
模块M4：根据深度图主方向对深度图中各点坐标旋转变换，构建变换后的深度图；
模块M5：根据变换后的深度图感知哈希计算图片指纹，获得图片指纹；
模块M6：将获得图片指纹与每次旋转的深度图指纹依次计算汉明距离，根据汉明距离计算相似度，当相似度最大值大于阈值时，则判断为闭环。
优选地，所述模块M1中扫描获得的点云经过前端里程计和后端优化得到的激光雷达位姿包括：
对于第k次扫描，记第k次扫描起始时刻为tk,结束时刻为tk+1，两时刻间雷达位姿变换表达式为：
Figure BDA0002387258450000061
其中，tx，ty，tz分别表示雷达位姿沿雷达坐标系x，y，z轴的平移变换，θx，θy，θz分别表示旋转变换，符合右手定则；[]T表示转置；
使用线性插值法计算每次扫描时的雷达位姿：
Figure BDA0002387258450000062
其中，ti表示第i次扫描对应时刻，
Figure BDA0002387258450000063
表示第i次扫描时雷达位姿；
所述模块M2包括：
模块M2.1：根据每次扫描的激光雷达位姿，依据欧氏变换，将当前扫描获得的点云投影到当前旋转起始时刻：
Figure BDA0002387258450000064
其中，
Figure BDA0002387258450000065
为扫描时获得的点云各点坐标，
Figure BDA0002387258450000066
为对应的旋转起始时刻坐标，
Figure BDA0002387258450000067
为
Figure BDA0002387258450000068
中前三项，R根据罗德里格斯公式求得:
Figure BDA0002387258450000069
其中：
Figure BDA00023872584500000610
Figure BDA00023872584500000611
为ω所对应的斜对称矩阵:
Figure BDA00023872584500000612
Figure BDA00023872584500000613
其中，I表示单位矩阵，w表示三维向量，ω＝(ω1，ω2，ω3),w1，w2，w3分别表示w三个维度的值，
Figure BDA00023872584500000614
表示
Figure BDA00023872584500000615
中第四项至第六项；
模块M2.2：根据当前扫描获得的点云投影到当前旋转起始时刻的点云坐标建立当前旋转所对应的深度图，
Figure BDA00023872584500000616
中点云各点坐标为三维坐标(x，y，z)，转换为当前旋转所对应的深度图，即点云各点坐标(x，y)对应的深度D(x，y)＝z，即tk坐标(x，y)各点对应的深度D(x，y)。
优选地，所述模块M3包括：
模块M3.1：取深度图中心部分圆形区域；设深度图宽度为w，高度为h，令圆形区域半径
Figure BDA0002387258450000071
圆形区域圆心与深度图中心重合；
计算深度图强度中心，所取的圆形区域表达式如下：
Figure BDA0002387258450000072
Figure BDA0002387258450000073
其中，(x，y)满足：x2+y2≤r2，则强度中心C定义为：C(xc，yc)；
模块M3.2：根据强度中心C计算向量
Figure BDA0002387258450000074
的方向，即深度图主方向：
Figure BDA0002387258450000075
所述模块M4包括：
模块M4.1：为实现图像的旋转不变性，对深度图D中心部分进行坐标变换，得到变换后的深度图Dθ，对于变换后的深度图Dθ中的坐标Dθ(x，y)在深度图D中对应的坐标D(x′，y′)满足以下关系：
Figure BDA0002387258450000076
Dθ(x，y)＝D(x′，y′) (12)
其中，Rθ为旋转矩阵，表达式如下：
Figure BDA0002387258450000077
变换后的深度图Dθ坐标(x，y)取值范围满足：
Figure BDA0002387258450000078
变换后的深度图为边长
Figure BDA0002387258450000079
像素的正方形；
对变换后的深度图Dθ中的坐标Dθ(x，y)在深度图D中对应的坐标D(x＇，y′)使用双线性插值法计算；
优选地，所述模块M5包括：
模块M5.1：将深度图Dθ尺寸缩小为N×N，得到D′θ∈RN×N，R代表实数集合，RN×N代表N×N矩阵，且矩阵每个元素为实数；
模块M5.2：对尺寸缩小后的深度图进行离散余弦变换，定义F∈RN×N为离散余弦变换结果，离散余弦变换结果F表达式如下：
Figure BDA00023872584500000710
其中，c(u)为补偿系数，c(v)表示补偿系数；如下所示：
Figure BDA0002387258450000081
模块M5.3：获取矩阵F左上角
Figure BDA0002387258450000082
低频部分，其中，k为大于1的整数，矩阵F低频部分集中了深度图的主要信息，记为
Figure BDA0002387258450000083
模块M5.4：计算F′中所有元素的平均值，记为
Figure BDA0002387258450000084
将F′中所有大于
Figure BDA0002387258450000085
的元素设为“1”，小于
Figure BDA0002387258450000086
的元素设为“0”；
模块M5.5：将矩阵F′各列相连，转换为向量hk，
Figure BDA0002387258450000087
即为第k次旋转对应深度图的图片指纹。
优选地，所述模块M6包括：
模块M6.1：将hk与hj，j∈{1，2，…，k-1}，分别计算汉明距离dkj，公式如下:
dkj＝||hk-hj|| (16)
其中，hj表示每次旋转的深度图指纹；
模块M6.2：根据汉明距离计算相似度skj，公式如下：
Figure BDA0002387258450000088
找到相似度的最大值ski：
Figure BDA0002387258450000089
当相似度的最大值ski大于阈值Ts，则判定当前雷达第k次旋转与第i次旋转时所在位置相同，即回环成立。
与现有技术相比，本发明具有如下的有益效果：
1、本发明设计了一种轻量级的激光SLAM闭环检测方法，闭环检测问题转换为深度图匹配问题。首先将点云经过投影等变换后构造深度图，之后根据图像方向进行坐标旋转，再利用感知哈希算法通过离散余弦变换获取图片指纹，并通过计算图片指纹的汉明距离的方式完成闭环检测。值得注意的是，本发明计算量较小，便于在嵌入式设备上实时完成，且结果基于图像整体特征，准确率较高，抗噪声能力强的特点。另外，由于图片指纹基于与平均值的大小比较，因而本算法对于局部噪声不敏感，具有较好的稳定性。
2、本发明创造性的将强度中心与感知哈希相结合，通过计算图片的强度中心坐标确定图片方向，进而对图像进行旋转坐标变换，确保匹配过程中的旋转不变性，可以大幅提高匹配的准确率。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
图1为闭环检测流程；
图2为深度图构建流程；
图3为坐标旋转变换流程；
图4为感知哈希匹配流程；
图5为坐标旋转示意图；
图6为双线性插值法示意图。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
本发明提供了一种基于感知哈希的激光SLAM闭环检测方法，该方法将SLAM闭环检测的问题转化为深度图相似图片检索问题，特点是首先确定强度中心的坐标确定图片方向，并进行坐标转换以确保旋转不变性，之后使用感知哈希对深度图进行编码，进而在哈希编码的基础上进行相似度计算，实现相似深度图的检索进而完成SLAM闭环检测。
根据本发明提供的一种基于感知哈希的激光SLAM闭环检测方法，包括:
步骤M1：3D激光扫描雷达每次旋转180°过程中扫描获得的点云，经过前端里程计和后端优化得到激光雷达位姿；
具体地，所述步骤M1中扫描获得的点云经过前端里程计和后端优化得到的激光雷达位姿包括：
对于第k次扫描，记第k次扫描起始时刻为tk,结束时刻为tk+1，两时刻间雷达位姿变换表达式为：
Figure BDA0002387258450000101
其中，tx，ty，tz分别表示雷达位姿沿雷达坐标系x，y，z轴的平移变换，θx，θy，θz分别表示旋转变换，符合右手定则；[]T表示转置；
使用线性插值法计算每次扫描时的雷达位姿：
Figure BDA0002387258450000102
其中，ti表示第i次扫描对应时刻，
Figure BDA0002387258450000103
表示第i次扫描时雷达位姿；
步骤M2：根据获得的激光雷达位姿，获得当前旋转的点云信息，构建每次旋转的深度图；
所述步骤M2包括：
步骤M2.1：根据每次扫描的激光雷达位姿，依据欧氏变换，将当前扫描获得的点云投影到当前旋转起始时刻：
Figure BDA0002387258450000104
其中，
Figure BDA0002387258450000105
为扫描时获得的点云各点坐标，
Figure BDA0002387258450000106
为对应的旋转起始时刻坐标，
Figure BDA0002387258450000107
为
Figure BDA0002387258450000108
中前三项，R根据罗德里格斯公式求得:
Figure BDA0002387258450000109
其中：
Figure BDA00023872584500001010
Figure BDA00023872584500001011
为ω所对应的斜对称矩阵:
Figure BDA00023872584500001012
Figure BDA00023872584500001013
其中，I表示单位矩阵，w表示三维向量，ω＝(ω1，ω2，ω3),w1，w2，w3分别表示w三个维度的值，
Figure BDA00023872584500001014
表示
Figure BDA00023872584500001015
中第四项至第六项；
步骤M2.2：根据当前扫描获得的点云投影到当前旋转起始时刻的点云坐标建立当前旋转所对应的深度图，
Figure BDA00023872584500001016
中点云各点坐标为三维坐标(x，y，z)，转换为当前旋转所对应的深度图，即点云各点坐标(x，y)对应的深度D(x，y)＝z，即tk坐标(x，y)各点对应的深度D(x，y)。
步骤M3：根据每次旋转的深度图，计算深度图中心部分圆形区域的强度中心，确定深度图主方向；
具体地，所述步骤M3包括：
步骤M3.1：取深度图中心部分圆形区域；设深度图宽度为w，高度为h，令圆形区域半径
Figure BDA0002387258450000111
圆形区域圆心与深度图中心重合；
计算深度图强度中心，所取的圆形区域表达式如下：
Figure BDA0002387258450000112
Figure BDA0002387258450000113
其中，(x，y)满足：x2+y2≤r2，则强度中心C定义为：C(xc，yc)；
步骤M3.2：根据强度中心C计算向量
Figure BDA0002387258450000114
的方向，即深度图主方向：
Figure BDA0002387258450000115
步骤M4：根据深度图主方向对深度图中各点坐标旋转变换，构建变换后的深度图；
所述步骤M4包括：
步骤M4.1：为实现图像的旋转不变性，对深度图D中心部分进行坐标变换，得到变换后的深度图Dθ，对于变换后的深度图Dθ中的坐标Dθ(x，y)在深度图D中对应的坐标D(x′，y′)满足以下关系：
Figure BDA0002387258450000116
Dθ(x，y)＝D(x′，y′) (12)
其中，Rθ为旋转矩阵，表达式如下：
Figure BDA0002387258450000117
变换后的深度图Dθ坐标(x，y)取值范围满足：
Figure BDA0002387258450000118
变换后的深度图为边长
Figure BDA0002387258450000119
像素的正方形；
由于以上计算得到的x′，y′可能不为整数，对变换后的深度图Dθ中的坐标Dθ(x，y)在深度图D中对应的坐标D(x′，y′)使用双线性插值法计算；
Figure BDA00023872584500001110
Figure BDA0002387258450000121
其中，dx1，dx2，dy1，dy2如下定义：
Figure BDA0002387258450000122
Figure BDA0002387258450000123
Figure BDA0002387258450000124
Figure BDA0002387258450000125
步骤M5：根据变换后的深度图感知哈希计算图片指纹，获得图片指纹；
具体地，所述步骤M5包括：
步骤M5.1：将深度图Dθ尺寸缩小为N×N，这里的N为4的倍数，得到D′θ∈RN×N，R代表实数集合，RN×N代表N×N矩阵，且矩阵每个元素为实数；
步骤M5.2：对尺寸缩小后的深度图进行离散余弦变换，定义F∈RN×N为离散余弦变换结果，离散余弦变换结果F表达式如下：
Figure BDA0002387258450000126
其中，c(u)为补偿系数，c(v)表示补偿系数；如下所示：
Figure BDA0002387258450000127
步骤M5.3：获取矩阵F左上角
Figure BDA0002387258450000128
低频部分，矩阵F低频部分集中了深度图的主要信息，记为
Figure BDA0002387258450000129
步骤M5.4：计算F′中所有元素的平均值，记为
Figure BDA00023872584500001210
将F′中所有大于
Figure BDA00023872584500001211
的元素设为“1”，小于
Figure BDA00023872584500001212
的元素设为“0”；
步骤M5.5：将矩阵F′各列相连，转换为向量
Figure BDA00023872584500001213
即为第k次旋转对应深度图的图片指纹。
步骤M6：将获得图片指纹与每次依次按照步骤M3、M4、M5得到每次旋转的深度图指纹依次计算汉明距离，根据汉明距离计算相似度，当相似度最大值大于阈值时，则判断为闭环。
具体地，所述步骤M6包括：
步骤M6.1：将hk与hj，j∈{1，2，…，k-1}，分别计算汉明距离dkj，公式如下:
dkj＝||hk-hj|| (16)
其中，hj表示每次旋转的深度图指纹；
步骤M6.2：根据汉明距离计算相似度skj，公式如下：
Figure BDA0002387258450000131
找到相似度的最大值ski：
Figure BDA0002387258450000132
当相似度的最大值ski大于阈值Ts，则判定当前雷达第k次旋转与第i次旋转时所在位置相同，即回环成立。
当缩小后尺寸N＝32时，则相似度阈值Ts＝90.6度。
根据本发明提供的一种基于感知哈希的激光SLAM闭环检测系统，包括:
模块M1：3D激光扫描雷达每次旋转180°过程中扫描获得的点云，经过前端里程计和后端优化得到激光雷达位姿；
具体地，所述模块M1中扫描获得的点云经过前端里程计和后端优化得到的激光雷达位姿包括：
对于第k次扫描，记第k次扫描起始时刻为tk,结束时刻为tk+1，两时刻间雷达位姿变换表达式为：
Figure BDA0002387258450000133
其中，tx，ty，tz分别表示雷达位姿沿雷达坐标系x，y，z轴的平移变换，θx，θy，θz分别表示旋转变换，符合右手定则；[]T表示转置；
使用线性插值法计算每次扫描时的雷达位姿：
Figure BDA0002387258450000134
其中，ti表示第i次扫描对应时刻，
Figure BDA0002387258450000135
表示第i次扫描时雷达位姿；
模块M2：根据获得的激光雷达位姿，获得当前旋转的点云信息，构建每次旋转的深度图；
所述模块M2包括：
模块M2.1：根据每次扫描的激光雷达位姿，依据欧氏变换，将当前扫描获得的点云投影到当前旋转起始时刻：
Figure BDA0002387258450000136
其中，
Figure BDA0002387258450000141
为扫描时获得的点云各点坐标，
Figure BDA0002387258450000142
为对应的旋转起始时刻坐标，
Figure BDA0002387258450000143
为
Figure BDA0002387258450000144
中前三项，R根据罗德里格斯公式求得:
Figure BDA0002387258450000145
其中：
Figure BDA0002387258450000146
Figure BDA0002387258450000147
为ω所对应的斜对称矩阵:
Figure BDA0002387258450000148
Figure BDA0002387258450000149
其中，I表示单位矩阵，w表示三维向量，ω＝(ω1，ω2，ω3),w1，w2，w3分别表示w三个维度的值，
Figure BDA00023872584500001410
表示
Figure BDA00023872584500001411
中第四项至第六项；
模块M2.2：根据当前扫描获得的点云投影到当前旋转起始时刻的点云坐标建立当前旋转所对应的深度图，
Figure BDA00023872584500001412
中点云各点坐标为三维坐标(x，y，z)，转换为当前旋转所对应的深度图，即点云各点坐标(x，y)对应的深度D(x，y)＝z，即tk坐标(x，y)各点对应的深度D(x，y)。
模块M3：根据每次旋转的深度图，计算深度图中心部分圆形区域的强度中心，确定深度图主方向；
具体地，所述模块M3包括：
模块M3.1：取深度图中心部分圆形区域；设深度图宽度为w，高度为h，令圆形区域半径
Figure BDA00023872584500001413
圆形区域圆心与深度图中心重合；
计算深度图强度中心，所取的圆形区域表达式如下：
Figure BDA00023872584500001414
Figure BDA00023872584500001415
其中，(x，y)满足：x2+y2≤r2，则强度中心C定义为：C(xc，yc)；
模块M3.2：根据强度中心C计算向量
Figure BDA00023872584500001416
的方向，即深度图主方向：
Figure BDA00023872584500001417
模块M4：根据深度图主方向对深度图中各点坐标旋转变换，构建变换后的深度图；
所述模块M4包括：
模块M4.1：为实现图像的旋转不变性，对深度图D中心部分进行坐标变换，得到变换后的深度图Dθ，对于变换后的深度图Dθ中的坐标Dθ(x，y)在深度图D中对应的坐标D(x′，y′)满足以下关系：
Figure BDA0002387258450000151
Dθ(x，y)＝D(x′，y＇) (12)
其中，Rθ为旋转矩阵，表达式如下：
Figure BDA0002387258450000152
变换后的深度图Dθ坐标(x，y)取值范围满足：
Figure BDA0002387258450000153
变换后的深度图为边长
Figure BDA0002387258450000154
像素的正方形；
由于以上计算得到的x′，y′可能不为整数，对变换后的深度图Dθ中的坐标Dθ(x，y)在深度图D中对应的坐标D(x′，y′)使用双线性插值法计算；
Figure BDA0002387258450000155
其中，dx1，dx2，dy1，dy2如下定义：
Figure BDA0002387258450000156
Figure BDA0002387258450000157
Figure BDA0002387258450000158
Figure BDA0002387258450000159
模块M5：根据变换后的深度图感知哈希计算图片指纹，获得图片指纹；
具体地，所述模块M5包括：
模块M5.1：将深度图Dθ尺寸缩小为N×N，这里的N为4的倍数，得到D′θ∈RN×N，R代表实数集合，RN×N代表N×N矩阵，且矩阵每个元素为实数；
模块M5.2：对尺寸缩小后的深度图进行离散余弦变换，定义F∈RN×N为离散余弦变换结果，离散余弦变换结果F表达式如下：
Figure BDA00023872584500001510
其中，c(u)为补偿系数，c(v)表示补偿系数；如下所示：
Figure BDA0002387258450000161
模块M5.3：获取矩阵F左上角
Figure BDA0002387258450000162
低频部分，矩阵F低频部分集中了深度图的主要信息，记为
Figure BDA0002387258450000163
模块M5.4：计算F′中所有元素的平均值，记为
Figure BDA0002387258450000164
将F′中所有大于
Figure BDA0002387258450000165
的元素设为“1”，小于
Figure BDA0002387258450000166
的元素设为“0”；
模块M5.5：将矩阵F′各列相连，转换为向量
Figure BDA0002387258450000167
即为第k次旋转对应深度图的图片指纹。
模块M6：将获得图片指纹与每次依次按照步骤M3、M4、M5得到每次旋转的深度图指纹依次计算汉明距离，根据汉明距离计算相似度，当相似度最大值大于阈值时，则判断为闭环。
具体地，所述模块M6包括：
模块M6.1：将hk与hj，j∈{1，2，…，k-1}，分别计算汉明距离dkj，公式如下:
dkj＝||hk-hj|| (16)
其中，hj表示每次旋转的深度图指纹；
模块M6.2：根据汉明距离计算相似度skj，公式如下：
Figure BDA0002387258450000168
找到相似度的最大值ski：
Figure BDA0002387258450000169
当相似度的最大值ski大于阈值Ts，则判定当前雷达第k次旋转与第i次旋转时所在位置相同，即回环成立。
当缩小后尺寸N＝32时，则相似度阈值Ts＝90.6度。
以下优选例对本发明作进一步详细说明：
在优选例中，所述步骤M1中，雷达扫描器每次可以扫描一个平面，视角为180°。为实现3D扫描，扫描器由马达驱动，以扫描器水平时扫描平面为参照，由-90°旋转至90°，扫描器每次扫描后，点云中各点坐标可以直接由传感器获取。
在优选例中，所述步骤M1和M2中,雷达坐标系{L}为一个3D直角坐标系，其原点位于雷达的几何中心，x轴指向雷达左侧，y轴指向上方，z轴指向雷达前方，在雷达第k次旋转时，其雷达坐标系记为{Lk}，其中点i的坐标记为
Figure BDA0002387258450000171
特别说明的是，激光SLAM运行过程中，前端里程计和后端优化将会实时计算雷达每次旋转位姿变换
Figure BDA0002387258450000172
在闭环检测过程中可以直接获取而无需额外计算。
在优选例中，所述步骤M2.2中，具体实现如下：
将点云数据转为深度图，即将平面深度图上的每个像素点对应空间中某一小块点云。例如将角分辨率定义为0.25°，意味着深度图临近的像素点对应的点云中的点与雷达坐标系原点连线夹角相差0.25°。
在优选例中，利用pcl库中的range_image即可实现点云到深度图的转换，设置参数角度分辨率为0.25°，设置水平视角和垂直视角均为180°，即可将传感器前方点云构建为720×720的深度图。
在优选例中，步骤M3.1选取的中心部分为圆心与深度图中心重合，半径r＝360像素的圆。
在优选例中，步骤M3.1和步骤M3.2中计算图像强度中心C的坐标以及方向角θ仅涉及到上述圆中的各点，与边缘部分无关。
在优选例中，所述步骤M4.1中，包含如下步骤：
(1)确定变换后的深度图Dθ的坐标范围，如下所示：
Figure BDA0002387258450000173
即变换后的深度图Dθ大小为509×509，中心与原深度图D中心重合。
(2)如步骤M4.1所述计算变换后的深度图各点取值Dθ(x，y)，参见附图5：
Dθ(x，y)＝D(x′，y′)＝D(x·cosθ-y·sihθ，x·sinθ+y·cosθ)
(3)对于非整数的x′，y′，如步骤M4.1所述使用双线性插值法计算，见附图6。
在优选例中，所述步骤M5.1中，变换后的深度图尺度需由509×509，压缩为32×32，记为D′θ，图片缩放通过双线性插值法实现，在实际操作中，可以使用OpenCV库中的resize函数完成。
在优选例中，所述步骤M5.2中，为便于计算，应将离散余弦变换的计算转换为矩阵形式，如下所示：
F＝AD′θAT
Figure BDA0002387258450000174
其中，F∈R32×32为离散余弦变换后的结果，D′θ∈R32×32为变换后的深度图，c(i)为步骤S3.2所述补偿系数，矩阵A∈R32×32，其中各项元素如上所述。实际操作中可以使用numpy库完成以上运算，加快运算速度。
在优选例中，所述步骤M5.3中，只需保留矩阵F左上角8*8的部分，记作F′∈R8×8。
在优选例中，所述步骤M5.4中，将F′中大于平均值的元素均设为“1”，小于平均值的元素均设为“0”，可以得到一个8*8的矩阵，每个元素均为“0”或“1”。
在优选例中，所述步骤M5.5中，将上一步所得矩阵按列相接，得到hk∈R64，如下所示:
Figure BDA0002387258450000181
在优选例中，所述步骤M6中，如下计算当前深度图指纹向量hk与已有深度图指纹hj，j∈{1，2...，k-1}相似度：
Figure BDA0002387258450000182
设定相似度阈值Ts＝90.6度，若相似度大于这一阈值则判定回环成立。
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (10)

1.一种基于感知哈希的激光SLAM闭环检测方法，其特征在于，包括：
步骤M1：激光扫描雷达每次旋转预设角度过程中扫描获得的点云，经过前端里程计和后端优化得到激光雷达位姿；
步骤M2：根据获得的激光雷达位姿，构建每次旋转的深度图；
步骤M3：根据每次旋转的深度图，计算深度图中心部分圆形区域的强度中心，确定深度图主方向；
步骤M4：根据深度图主方向对深度图中各点坐标旋转变换，构建变换后的深度图；
步骤M5：根据变换后的深度图感知哈希计算图片指纹，获得图片指纹；
步骤M6：将获得图片指纹与每次旋转的深度图指纹依次计算汉明距离，根据汉明距离计算相似度，当相似度最大值大于阈值时，则判断为闭环。
2.根据权利要求1所述的一种基于感知哈希的激光SLAM闭环检测方法，其特征在于，所述步骤M1中扫描获得的点云经过前端里程计和后端优化得到的激光雷达位姿包括：
对于第k次扫描，记第k次扫描起始时刻为tk，结束时刻为tk+1，两时刻间雷达位姿变换表达式为：
Figure FDA0002387258440000011
其中，tx，ty，tz分别表示雷达位姿沿雷达坐标系x，y，z轴的平移变换，θx，θy，θz分别表示旋转变换，符合右手定则；[]T表示转置；
使用线性插值法计算每次扫描时的雷达位姿：
Figure FDA0002387258440000012
其中，ti表示第i次扫描对应时刻，
Figure FDA0002387258440000013
表示第i次扫描时雷达位姿；
所述步骤M2包括：
步骤M2.1：根据每次扫描的激光雷达位姿，依据欧氏变换，将当前扫描获得的点云投影到当前旋转起始时刻：
Figure FDA0002387258440000014
其中，
Figure FDA0002387258440000015
为扫描时获得的点云各点坐标，
Figure FDA0002387258440000016
为对应的旋转起始时刻坐标，
Figure FDA0002387258440000021
为
Figure FDA0002387258440000022
中前三项，R根据罗德里格斯公式求得：
Figure FDA0002387258440000023
其中：
Figure FDA0002387258440000024
Figure FDA0002387258440000025
为ω所对应的斜对称矩阵：
Figure FDA0002387258440000026
Figure FDA0002387258440000027
其中，I表示单位矩阵，w表示三维向量，ω＝(ω1，ω2，ω3)，w1，w2，w3分别表示w三个维度的值，
Figure FDA0002387258440000028
表示
Figure FDA0002387258440000029
中第四项至第六项；
步骤M2.2：根据当前扫描获得的点云投影到当前旋转起始时刻的点云坐标建立当前旋转所对应的深度图；
Figure FDA00023872584400000210
中点云各点坐标为三维坐标(x，y，z)，转换为当前旋转所对应的深度图，即点云各点坐标(x，y)对应的深度D(x，y)＝z，即tk时刻，坐标(x，y)各点对应的深度D(x，y)。
3.根据权利要求1所述的一种基于感知哈希的激光SLAM闭环检测方法，其特征在于，所述步骤M3包括：
步骤M3.1：取深度图中心部分圆形区域；设深度图宽度为w，高度为h，令圆形区域半径
Figure FDA00023872584400000211
圆形区域圆心与深度图中心重合；
计算深度图强度中心，所取的圆形区域表达式如下：
Figure FDA00023872584400000212
Figure FDA00023872584400000213
其中，(x，y)满足：x2+y2≤r2，则强度中心C定义为：C(xc，yc)；
步骤M3.2：根据强度中心C计算向量
Figure FDA00023872584400000214
的方向，即深度图主方向：
Figure FDA00023872584400000215
所述步骤M4包括：
步骤M4.1：为实现图像的旋转不变性，对深度图D中心部分进行坐标变换，得到变换后的深度图Dθ，对于变换后的深度图Dθ中的坐标Dθ(x，y)在深度图D中对应的坐标D(x′，y′)满足以下关系：
Figure FDA0002387258440000031
Dθ(x，y)＝D(x′，y′) (12)
其中，Rθ为旋转矩阵，表达式如下：
Figure FDA0002387258440000032
变换后的深度图Dθ坐标(x，y)取值范围满足：
Figure FDA0002387258440000033
变换后的深度图为边长
Figure FDA0002387258440000034
像素的正方形；
对变换后的深度图Dθ中的坐标Dθ(x，y)在深度图D中对应的坐标D(x′，y′)使用双线性插值法计算。
4.根据权利要求1所述的一种基于感知哈希的激光SLAM闭环检测方法，其特征在于，所述步骤M5包括：
步骤M5.1：将深度图Dθ尺寸缩小为N×N，得到D′0∈RN×N，R代表实数集合，RN×N代表N×N矩阵，且矩阵每个元素为实数；
步骤M5.2：对尺寸缩小后的深度图进行离散余弦变换，定义F∈RN×N为离散余弦变换结果，离散余弦变换结果F表达式如下：
Figure FDA0002387258440000035
其中，c(u)为补偿系数，c(v)表示补偿系数；如下所示：
Figure FDA0002387258440000036
步骤M5.3：获取矩阵F左上角
Figure FDA0002387258440000037
低频部分，其中，k为大于1的整数，矩阵F低频部分集中了深度图的主要信息，记为
Figure FDA0002387258440000038
步骤M5.4：计算F′中所有元素的平均值，记为
Figure FDA0002387258440000039
将F′中所有大于
Figure FDA00023872584400000312
的元素设为“1”，小于
Figure FDA00023872584400000310
的元素设为“0”；
步骤M5.5：将矩阵F′各列相连，转换为向量hk，
Figure FDA00023872584400000311
即为第k次旋转对应深度图的图片指纹。
5.根据权利要求1所述的一种基于感知哈希的激光SLAM闭环检测方法，其特征在于，所述步骤M6包括：
步骤M6.1：将hk与hj，j∈{1，2，…，k-1}，分别计算汉明距离dkj，公式如下：
dkj＝||hk-hj|| (16)
其中，hj表示每次旋转的深度图指纹；
步骤M6.2：根据汉明距离计算相似度skj，公式如下：
Figure FDA0002387258440000041
找到相似度的最大值ski：
Figure FDA0002387258440000042
当相似度的最大值ski大于阈值Ts，则判定当前雷达第k次旋转与第i次旋转时所在位置相同，即回环成立。
6.一种基于感知哈希的激光SLAM闭环检测系统，其特征在于，包括：
模块M1：激光扫描雷达每次旋转预设角度过程中扫描获得的点云，经过前端里程计和后端优化得到激光雷达位姿；
模块M2：根据获得的激光雷达位姿，构建每次旋转的深度图；
模块M3：根据每次旋转的深度图，计算深度图中心部分圆形区域的强度中心，确定深度图主方向；
模块M4：根据深度图主方向对深度图中各点坐标旋转变换，构建变换后的深度图；
模块M5：根据变换后的深度图感知哈希计算图片指纹，获得图片指纹；
模块M6：将获得图片指纹与每次旋转的深度图指纹依次计算汉明距离，根据汉明距离计算相似度，当相似度最大值大于阈值时，则判断为闭环。
7.根据权利要求6所述的一种基于感知哈希的激光SLAM闭环检测系统，其特征在于，所述模块M1中扫描获得的点云经过前端里程计和后端优化得到的激光雷达位姿包括：
对于第k次扫描，记第k次扫描起始时刻为tk，结束时刻为tk+1，两时刻间雷达位姿变换表达式为：
Figure FDA0002387258440000043
其中，tx，ty，tz分别表示雷达位姿沿雷达坐标系x，y，z轴的平移变换，θx，θy，θz分别表示旋转变换，符合右手定则；[]T表示转置；
使用线性插值法计算每次扫描时的雷达位姿：
Figure FDA0002387258440000051
其中，ti表示第i次扫描对应时刻，
Figure FDA0002387258440000052
表示第i次扫描时雷达位姿；
所述模块M2包括：
模块M2.1：根据每次扫描的激光雷达位姿，依据欧氏变换，将当前扫描获得的点云投影到当前旋转起始时刻：
Figure FDA0002387258440000053
其中，
Figure FDA0002387258440000054
为扫描时获得的点云各点坐标，
Figure FDA0002387258440000055
为对应的旋转起始时刻坐标，
Figure FDA0002387258440000056
为
Figure FDA0002387258440000057
中前三项，R根据罗德里格斯公式求得：
Figure FDA0002387258440000058
其中：
Figure FDA0002387258440000059
Figure FDA00023872584400000510
为ω所对应的斜对称矩阵：
Figure FDA00023872584400000511
Figure FDA00023872584400000512
其中，I表示单位矩阵，w表示三维向量，ω＝(ω1，ω2，ω3)，w1，w2，w3分别表示w三个维度的值，
Figure FDA00023872584400000513
表示
Figure FDA00023872584400000514
中第四项至第六项；
模块M2.2：根据当前扫描获得的点云投影到当前旋转起始时刻的点云坐标建立当前旋转所对应的深度图，
Figure FDA00023872584400000515
中点云各点坐标为三维坐标(x，y，z)，转换为当前旋转所对应的深度图，即点云各点坐标(x，y)对应的深度D(x，y)＝z，即tk坐标(x，y)各点对应的深度D(x，y)。
8.根据权利要求6所述的一种基于感知哈希的激光SLAM闭环检测系统，其特征在于，所述模块M3包括：
模块M3.1：取深度图中心部分圆形区域；设深度图宽度为w，高度为h，令圆形区域半径
Figure FDA0002387258440000061
圆形区域圆心与深度图中心重合；
计算深度图强度中心，所取的圆形区域表达式如下：
Figure FDA0002387258440000062
Figure FDA0002387258440000063
其中，(x，y)满足：x2+y2≤r2，则强度中心C定义为：C(xc，yc)；
模块M3.2：根据强度中心C计算向量
Figure FDA0002387258440000064
的方向，即深度图主方向：
Figure FDA0002387258440000065
所述模块M4包括：
模块M4.1：为实现图像的旋转不变性，对深度图D中心部分进行坐标变换，得到变换后的深度图Dθ，对于变换后的深度图Dθ中的坐标Dθ(x，y)在深度图D中对应的坐标D(x′，y′)满足以下关系：
Figure FDA0002387258440000066
Dθ(x，y)＝D(x′，y′) (12)
其中，Rθ为旋转矩阵，表达式如下：
Figure FDA0002387258440000067
变换后的深度图Dθ坐标(x，y)取值范围满足：
Figure FDA0002387258440000068
变换后的深度图为边长
Figure FDA0002387258440000069
像素的正方形；
对变换后的深度图Dθ中的坐标Dθ(x，y)在深度图D中对应的坐标D(x′，y′)使用双线性插值法计算。
9.根据权利要求6所述的一种基于感知哈希的激光SLAM闭环检测系统，其特征在于，所述模块M5包括：
模块M5.1：将深度图Dθ尺寸缩小为N×N，得到D′0∈RN×N，R代表实数集合，RN×N代表N×N矩阵，且矩阵每个元素为实数；
模块M5.2：对尺寸缩小后的深度图进行离散余弦变换，定义F∈RN×N为离散余弦变换结果，离散余弦变换结果F表达式如下：
Figure FDA00023872584400000610
其中，c(u)为补偿系数，c(v)表示补偿系数；如下所示：
Figure FDA0002387258440000071
模块M5.3：获取矩阵F左上角
Figure FDA0002387258440000072
低频部分，其中，k为大于1的整数，矩阵F低频部分集中了深度图的主要信息，记为
Figure FDA0002387258440000073
模块M5.4：计算F′中所有元素的平均值，记为
Figure FDA0002387258440000074
将F′中所有大于
Figure FDA0002387258440000075
的元素设为“1”，小于
Figure FDA0002387258440000076
的元素设为“0”；
模块M5.5：将矩阵F′各列相连，转换为向量hk，
Figure FDA0002387258440000077
即为第k次旋转对应深度图的图片指纹。
10.根据权利要求6所述的一种基于感知哈希的激光SLAM闭环检测系统，其特征在于，所述模块M6包括：
模块M6.1：将hk与hj，j∈{1，2，…，k-1}，分别计算汉明距离dkj，公式如下：
dkj＝||hk-hj|| (16)
其中，hj表示每次旋转的深度图指纹；
模块M6.2：根据汉明距离计算相似度skj，公式如下：
Figure FDA0002387258440000078
找到相似度的最大值ski：
Figure FDA0002387258440000079
当相似度的最大值ski大于阈值Ts，则判定当前雷达第k次旋转与第i次旋转时所在位置相同，即回环成立。
CN202010102282.1A 2020-02-19 2020-02-19 基于感知哈希的激光slam闭环检测方法及系统 Active CN111429344B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010102282.1A CN111429344B (zh) 2020-02-19 2020-02-19 基于感知哈希的激光slam闭环检测方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010102282.1A CN111429344B (zh) 2020-02-19 2020-02-19 基于感知哈希的激光slam闭环检测方法及系统

Publications (2)

Publication Number Publication Date
CN111429344A true CN111429344A (zh) 2020-07-17
CN111429344B CN111429344B (zh) 2022-04-26

Family

ID=71547184

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010102282.1A Active CN111429344B (zh) 2020-02-19 2020-02-19 基于感知哈希的激光slam闭环检测方法及系统

Country Status (1)

Country Link
CN (1) CN111429344B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111862162A (zh) * 2020-07-31 2020-10-30 湖北亿咖通科技有限公司 回环检测方法及系统、可读存储介质、电子设备
CN112053390A (zh) * 2020-09-04 2020-12-08 上海懒书智能科技有限公司 一种基于点云变换匹配的定位方法
CN112230243A (zh) * 2020-10-28 2021-01-15 西南科技大学 一种移动机器人室内地图构建方法
CN113470113A (zh) * 2021-08-13 2021-10-01 西南科技大学 一种融合brief特征匹配与icp点云配准的零部件姿态估计方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107403163A (zh) * 2017-07-31 2017-11-28 武汉大学 一种基于深度学习的激光slam闭环检测方法
CN108008394A (zh) * 2017-11-28 2018-05-08 安徽工程大学 仿生声纳二维经历图绘制方法、存储介质及设备
CN110120093A (zh) * 2019-03-25 2019-08-13 深圳大学 一种多元特征混合优化的rgb-d室内三维测图方法及系统
CN110163968A (zh) * 2019-05-28 2019-08-23 山东大学 Rgbd相机大型三维场景构建方法及系统
WO2020030951A1 (en) * 2018-08-08 2020-02-13 Abyssal S.A. System and method of operation for remotely operated vehicles for simultaneous localization and mapping

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107403163A (zh) * 2017-07-31 2017-11-28 武汉大学 一种基于深度学习的激光slam闭环检测方法
CN108008394A (zh) * 2017-11-28 2018-05-08 安徽工程大学 仿生声纳二维经历图绘制方法、存储介质及设备
WO2020030951A1 (en) * 2018-08-08 2020-02-13 Abyssal S.A. System and method of operation for remotely operated vehicles for simultaneous localization and mapping
CN110120093A (zh) * 2019-03-25 2019-08-13 深圳大学 一种多元特征混合优化的rgb-d室内三维测图方法及系统
CN110163968A (zh) * 2019-05-28 2019-08-23 山东大学 Rgbd相机大型三维场景构建方法及系统

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
CHUNXU CHEN等: "Trajectory Optimization of LiDAR SLAM Based on Local Pose Graph", 《CSNC 2019》 *
HENG ZHANG等: "Loop Closing Detection in RGB-D SLAM Combining Appearance and Geometric Constraints", 《SENSORS》 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111862162A (zh) * 2020-07-31 2020-10-30 湖北亿咖通科技有限公司 回环检测方法及系统、可读存储介质、电子设备
CN112053390A (zh) * 2020-09-04 2020-12-08 上海懒书智能科技有限公司 一种基于点云变换匹配的定位方法
CN112053390B (zh) * 2020-09-04 2023-12-22 上海懒书智能科技有限公司 一种基于点云变换匹配的定位方法
CN112230243A (zh) * 2020-10-28 2021-01-15 西南科技大学 一种移动机器人室内地图构建方法
CN113470113A (zh) * 2021-08-13 2021-10-01 西南科技大学 一种融合brief特征匹配与icp点云配准的零部件姿态估计方法
CN113470113B (zh) * 2021-08-13 2023-07-21 西南科技大学 一种融合brief特征匹配与icp点云配准的零部件姿态估计方法

Also Published As

Publication number Publication date
CN111429344B (zh) 2022-04-26

Similar Documents

Publication Publication Date Title
Li et al. Point cloud registration based on one-point ransac and scale-annealing biweight estimation
CN111429344B (zh) 基于感知哈希的激光slam闭环检测方法及系统
CN111750820B (zh) 影像定位方法及其系统
EP2048599B1 (en) System and method for 3D object recognition
Khoshelham Extending generalized hough transform to detect 3d objects in laser range data
CN108122256B (zh) 一种逼近状态下旋转目标位姿测量的方法
Azad et al. Stereo-based 6d object localization for grasping with humanoid robot systems
CN110966991A (zh) 一种无控制点下的单幅无人机影像定位方法
CN102722697B (zh) 一种无人飞行器视觉自主导引着陆的目标跟踪方法
Li et al. Automatic targetless LiDAR–camera calibration: a survey
Houshiar et al. A study of projections for key point based registration of panoramic terrestrial 3D laser scan
EP3404583A1 (en) System and method for model adaptation
CN111798453A (zh) 用于无人驾驶辅助定位的点云配准方法及其系统
Bu et al. Semi-direct tracking and mapping with RGB-D camera for MAV
Ma et al. Visual homing via guided locality preserving matching
CN114529681A (zh) 一种手持双相机建筑物温度场三维模型构建方法和系统
Pandey et al. Alignment of 3D point clouds with a dominant ground plane
KR101733288B1 (ko) 방향정보를 이용한 객체 검출기 생성 방법, 이를 이용한 객체 검출 장치 및 방법
Kang et al. A robust image matching method based on optimized BaySAC
CN112634377B (zh) 扫地机器人的相机标定方法、终端和计算机可读存储介质
Tornow et al. A multi-agent mobile robot system with environment perception and HMI capabilities
Zang et al. Camera localization by CAD model matching
Lakshmi et al. Study of feature based image registration algorithms for navigation of unmanned aerial vehicles
Jackson et al. Adaptive registration of very large images
Hu et al. Accurate fiducial mapping for pose estimation using manifold optimization

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