【技术实现步骤摘要】
基于感知哈希的激光SLAM闭环检测方法及系统
本专利技术涉及定位与导航领域，具体地，涉及一种基于感知哈希的激光SLAM闭环检测方法及系统。
技术介绍
SLAM(simultaneouslocalizationandmapping),即时定位与地图构建。问题可以描述为：将一个机器人放入未知环境中的未知位置，是否有办法让机器人一边移动一边逐步描绘出此环境完全的地图，在递增地建立地图的同时，利用已建立的地图来同步刷新自身的位置。这是一个相辅相成不断迭代的过程，也是移动机器人自主完成作业任务需要解决的基本问题。目前依据使用的传感器不同，SLAM技术主要分为两大类：基于激光雷达的激光SLAM和基于摄像头的视觉SLAM。在测量平台移动的过程中，由于前端位姿估计和后端优化均基于部分相邻数据，之前产生的误差将不可避免地累计到下一个时刻，使得整个SLAM将会出现累积误差，长期估计的结果将不可靠。为消除这类误差，SLAM系统中引入闭环检测模块。闭环检测，又称回环检测(LoopClosureDetection)，主要解决位置估计随时间漂移的问题，通过识别测量平台曾经达到过的场景，从而在位姿优化中添加时隔更加久远的约束，使之得到更好的、全局一致的位姿估计。另一方面，由于闭环检测提供了当前数据与所有历史数据的关联，在跟踪丢失后，还可以利用闭环检测进行重定位。因此，闭环检测对整个SLAM系统精度和鲁棒性有非常明显的提升。传统的激光SLAM闭环检测算法主要有如下两种：ICP算法，即迭代最近点(IterativeClosestPoi...
【技术保护点】
1.一种基于感知哈希的激光SLAM闭环检测方法，其特征在于，包括：/n步骤M1：激光扫描雷达每次旋转预设角度过程中扫描获得的点云，经过前端里程计和后端优化得到激光雷达位姿；/n步骤M2：根据获得的激光雷达位姿，构建每次旋转的深度图；/n步骤M3：根据每次旋转的深度图，计算深度图中心部分圆形区域的强度中心，确定深度图主方向；/n步骤M4：根据深度图主方向对深度图中各点坐标旋转变换，构建变换后的深度图；/n步骤M5：根据变换后的深度图感知哈希计算图片指纹，获得图片指纹；/n步骤M6：将获得图片指纹与每次旋转的深度图指纹依次计算汉明距离，根据汉明距离计算相似度，当相似度最大值大于阈值时，则判断为闭环。/n
【技术特征摘要】
1.一种基于感知哈希的激光SLAM闭环检测方法，其特征在于，包括：
步骤M1：激光扫描雷达每次旋转预设角度过程中扫描获得的点云，经过前端里程计和后端优化得到激光雷达位姿；
步骤M2：根据获得的激光雷达位姿，构建每次旋转的深度图；
步骤M3：根据每次旋转的深度图，计算深度图中心部分圆形区域的强度中心，确定深度图主方向；
步骤M4：根据深度图主方向对深度图中各点坐标旋转变换，构建变换后的深度图；
步骤M5：根据变换后的深度图感知哈希计算图片指纹，获得图片指纹；
步骤M6：将获得图片指纹与每次旋转的深度图指纹依次计算汉明距离，根据汉明距离计算相似度，当相似度最大值大于阈值时，则判断为闭环。
2.根据权利要求1所述的一种基于感知哈希的激光SLAM闭环检测方法，其特征在于，所述步骤M1中扫描获得的点云经过前端里程计和后端优化得到的激光雷达位姿包括：
对于第k次扫描，记第k次扫描起始时刻为tk，结束时刻为tk+1，两时刻间雷达位姿变换表达式为：
其中，tx，ty，tz分别表示雷达位姿沿雷达坐标系x，y，z轴的平移变换，θx，θy，θz分别表示旋转变换，符合右手定则；[]T表示转置；
使用线性插值法计算每次扫描时的雷达位姿：
其中，ti表示第i次扫描对应时刻，表示第i次扫描时雷达位姿；
所述步骤M2包括：
步骤M2.1：根据每次扫描的激光雷达位姿，依据欧氏变换，将当前扫描获得的点云投影到当前旋转起始时刻：
其中，为扫描时获得的点云各点坐标，为对应的旋转起始时刻坐标，为中前三项，R根据罗德里格斯公式求得：
其中：
为ω所对应的斜对称矩阵：
其中，I表示单位矩阵，w表示三维向量，ω＝(ω1，ω2，ω3)，w1，w2，w3分别表示w三个维度的值，表示中第四项至第六项；
步骤M2.2：根据当前扫描获得的点云投影到当前旋转起始时刻的点云坐标建立当前旋转所对应的深度图；中点云各点坐标为三维坐标(x，y，z)，转换为当前旋转所对应的深度图，即点云各点坐标(x，y)对应的深度D(x，y)＝z，即tk时刻，坐标(x，y)各点对应的深度D(x，y)。
3.根据权利要求1所述的一种基于感知哈希的激光SLAM闭环检测方法，其特征在于，所述步骤M3包括：
步骤M3.1：取深度图中心部分圆形区域；设深度图宽度为w，高度为h，令圆形区域半径圆形区域圆心与深度图中心重合；
计算深度图强度中心，所取的圆形区域表达式如下：
其中，(x，y)满足：x2+y2≤r2，则强度中心C定义为：C(xc，yc)；
步骤M3.2：根据强度中心C计算向量的方向，即深度图主方向：
所述步骤M4包括：
步骤M4.1：为实现图像的旋转不变性，对深度图D中心部分进行坐标变换，得到变换后的深度图Dθ，对于变换后的深度图Dθ中的坐标Dθ(x，y)在深度图D中对应的坐标D(x′，y′)满足以下关系：
Dθ(x，y)＝D(x′，y′)(12)
其中，Rθ为旋转矩阵，表达式如下：
变换后的深度图Dθ坐标(x，y)取值范围满足：变换后的深度图为边长像素的正方形；
对变换后的深度图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表达式如下：
其中，c(u)为补偿系数，c(v)表示补偿系数；如下所示：
步骤M5.3：获取矩阵F左上角低频部分，其中，k为大于1的整数，矩阵F低频部分集中了深度图的主要信息，记为
步骤M5.4：计算F′中所有元素的平均值，记为将F′中所有大于的元素设为“1”，小于的元素设为“0”；
步骤M5.5：将矩阵F′各列相连，转换为向量hk，即为第k次旋转对应深度图的图片指纹。
5.根据权利要求1所述的一种基于感知哈希的激光SLAM闭环检测方法，其特征在于，所述步骤M6包括：
步骤M6.1：将hk与hj，j∈{1，2，…，k-1}，分别计算汉明距离dkj，公式如下：
dkj＝||hk-hj||(16)
其中，hj表示每次旋转的深度图指纹；
步骤M6.2：根据汉明距离计算相似度skj，公式如下：
找到相似度的最大值ski：
当相似度的最大值ski大于阈值Ts，则判定当前雷达第k次旋转与第i次旋转时所在位置相同，即回环成立。
<...
【专利技术属性】
技术研发人员：王贺升，谢熠辰，赵小文，
申请(专利权)人：上海交通大学，
类型：发明
国别省市：上海;31
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。