[发明专利]一种基于Karto SLAM的鲁棒闭环检测新方法

权利要求书

1.一种基于Karto SLAM的鲁棒闭环检测新方法，其特征在于步骤如下：

步骤1：接收激光雷达扫描仪扫描的环境信息；

步骤2：利用快速窗口滤波算法对输入的激光雷达抖动信息进行预处理：

步骤2.1：设定窗口大小，确定窗口中激光雷达数据点个数；

步骤2.2：计算窗口内所有点的均值

步骤2.3：设定参数δ，比较p(x)与δ·p'(x)的大小；

步骤2.4：如果p(x)大于δ·p'(x)，p(x)替换为δ·p'(x)，反之，p(x)小于δ·p'(x)，认为数据未波动，p(x)保持不变；

步骤3：通过上一时刻位姿数据和里程计数据对初始位置进行预测；

步骤4：若当前帧不是初始位姿，判断当前帧是否为关键帧，如果为关键帧，则进行下一步扫描匹配工作；

步骤5：将激光雷达获得的数据信息以设定的角分辨率和角平移值进行投影，获得查找表；

步骤6：在查找表上，进行帧-图方式的扫描匹配，遍历搜索区域后获得响应值最高的位置，得到机器人的角度；

步骤7：查找表投影到子地图上，响应值最高的位置即为机器人的位置；

步骤8：扫描匹配搜索过程采用粗匹配和精匹配来提高搜索效率；

步骤9：扫描匹配得到节点位置均值和协方差后添加边和顶点；

步骤10：采用延迟决策策略来优化识别闭环检测过程：

步骤10.1：计算a、b两点匹配的响应和协方差；

步骤10.2：当a、b两点的响应达到阈值，然后计算c、d两点匹配的响应和协方差；

步骤10.3：如果c、d两点也满足条件，计算四个位置点的相对位姿乘积T₁·T₂·T₃·T₄，判断T₁·T₂·T₃·T₄是否小于设定的阈值，如果满足条件，进行闭环检测，若不满足，返回寻找新的闭环点；

步骤11：满足闭环检测条件，进行闭环检测；

步骤12：调整边，进行全局闭环，采用稀疏位姿调整方式进行后端优化；

步骤13：输出进行全局优化后的位姿和地图。

2.根据权利要求1所述的一种基于Karto SLAM的鲁棒闭环检测新方法，其特征在于所述步骤4中的当前帧不是初始位姿，判断当前帧是否为关键帧，其中建立节点依据以下三个条件：

(1)设置建立节点最小时间段3600s，如果超过最小时间段且里程计小于最小移动距离，则建立一个节点；

(2)设置最小距离0.2m，里程计移动长度超过最小距离，则建立一个节点；

(3)设置最小偏转角10°，如果里程计转向超过最小偏转角，则建立一个节点。

3.根据权利要求1所述的一种基于Karto SLAM的鲁棒闭环检测新方法，其特征在于所述步骤5中的将激光雷达获得的数据信息以设定的角分辨率和角平移值进行投影，获得n个角度的查找表，由里程计预测可以得到当前预估的姿态角，真实的姿态角必定在预估的姿态角附近。

4.根据权利要求1所述的一种基于Karto SLAM的鲁棒闭环检测新方法，其特征在于所述步骤6进行帧-图方式的扫描匹配具体为：通过当前帧与running-scans进行匹配，所述的running-scans指的是满足一定数据规模且首末两帧距离在一定距离范围内的实时维护的局部激光数据链；将查找表投影到子地图上，此时的子地图已有局部激光数据链生成；维护当前局部数据链并利用高斯进行模糊，假设查找表击中n个点，由于高斯模糊的作用击中的每个点得分不同，累加击中的点的得分并除以可以达到的最高得分，得到响应值。

5.根据权利要求1所述的一种基于Karto SLAM的鲁棒闭环检测新方法，其特征在于所述步骤7中的子地图指的是矩形形状的兴趣区域，这个兴趣区域也可以理解为参考模型，搜索区域为以里程计估计的位置为中心的一个矩形区域，搜索区域表示机器人最终位置的可能范围，在扫描匹配时，通过遍历搜索区域来获取响应值最高的位置。