CN112241002B - 一种基于Karto SLAM的鲁棒闭环检测新方法 - Google Patents

Abstract

本发明涉及一种基于Karto SLAM的鲁棒闭环检测新方法，首先设计快速窗口滤波算法对激光数据进行滤波，然后使用延迟决策策略对闭环检测部分进行优化，优化后的闭环检测与算法原闭环检测方法不同之处在于该策略不会在a、b两点观察到了相同的环境信息后就认为机器人来到了原来的地方，在c、d点观察到了相同的环境信息后才会触发闭环检测。在一定程度上提高了***闭环检测的准确率，进一步提高了机器人后续路径规划、导航等相关工作的精度，解决了机器人在特殊场景(如巷道、走廊)下的定位与建图问题，因此在一定程度上也提高了该算法的实用性和鲁棒性。

Description

一种基于Karto SLAM的鲁棒闭环检测新方法

技术领域

本发明设计属于机器人定位与导航技术领域，具体涉及一种基于Karto SLAM的鲁棒闭环检测新方法。

背景技术

同步定位与构图(SLAM)被描述为机器人在未知环境的未知位置通过自身的传感器来时刻更新自己的位置和地图。近年来，SLAM相关智能机器人相关产业如火如荼，激光SLAM关键技术在智能机器人产品中也广泛应用，如扫地机器人、无人机、自动驾驶汽车等。激光SLAM中基于图优化方法的SLAM算法与基于滤波的SLAM不同之处在于它根据历史所有的观测数据，将机器人不同时刻的位姿作为顶点，把机器人在不同位置上的观测所产生的约束作为图优化顶点之间的边或约束，基于图优化方法的SLAM在环境地图较大的情况下也能够很好的完成定位与建图任务，具有精度高、速度快的优点。

基于图优化方法的SLAM闭环检测过程主要有帧-帧、帧-图、图-图三种检测方法。帧-图因比其他两种方法计算量小，而且检测较为准确而成为了目前主流闭环检测方法。回环检测即闭环检测，指机器人在建图过程识别出曾经来过某个地方的能力。随着时间推移，机器人行走路径延长，累计误差也会增大，使用闭环检测来优化机器人位姿、减少累计误差变得至关重要。正确的闭环检测能够及时矫正地图，有助于机器人快速、准确的进行后续路径规划或者导航等工作，错误的闭环检测将会导致构图杂乱歪扭，定位失败。

正常情况下现有的Karto SLAM算法能够较好的满足实际建图与定位要求，但是在一些特殊的应用场景，如巷道、走廊这些环境特征较为相似的应用场景下该算法同样存在闭环检测极易出现错误的情况，并且机器人激光数据采集设备容易因路面不平坦、设备本身不稳定导致返回的激光数据存在抖动和噪声，因此为了提高闭环检测的精度，本发明提出了一种基于Karto SLAM的鲁棒闭环检测新方法，利用快速窗口滤波算法处理抖动激光雷达数据，输入滤波后的激光雷达数据进行扫描匹配和闭环检测，采用延迟决策来提高闭环检测的准确率，让机器人在特殊场景下也能很好的完成同步定位与构图。

发明内容

要解决的技术问题

为了解决现有的Karto SLAM算法在由于地面不平稳设备发生抖动情况下并处于环境特征较为相似(如巷道、走廊)的应用场景下容易出现错误的闭环检测问题，本发明提供一种基于Karto SLAM的鲁棒闭环检测新方法。

技术方案

一种基于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：输出进行全局优化后的位姿和地图。

本发明技术方案更进一步的说：所述步骤4中的当前帧不是初始位姿，判断当前帧是否为关键帧，其中建立节点依据以下三个条件：

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

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

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

本发明技术方案更进一步的说：所述步骤5中的将激光雷达获得的数据信息以设定的角分辨率和角平移值进行投影，获得n个角度的查找表，由里程计预测可以得到当前预估的姿态角，真实的姿态角必定在预估的姿态角附近。

本发明技术方案更进一步的说：所述步骤6进行帧-图方式的扫描匹配具体为：通过当前帧与running-scans进行匹配，所述的running-scans指的是满足一定数据规模且首末两帧距离在一定距离范围内的实时维护的局部激光数据链；将查找表投影到子地图上，此时的子地图已有局部激光数据链生成；维护当前局部数据链并利用高斯进行模糊，假设查找表击中n个点，由于高斯模糊的作用击中的每个点得分不同，累加击中的点的得分并除以可以达到的最高得分，得到响应值。

本发明技术方案更进一步的说：所述步骤7中的子地图指的是矩形形状的兴趣区域，这个兴趣区域也可以理解为参考模型，搜索区域为以里程计估计的位置为中心的一个矩形区域，搜索区域表示机器人最终位置的可能范围，在扫描匹配时，通过遍历搜索区域来获取响应值最高的位置。

本发明技术方案更进一步的说：所述步骤7具体如下：

步骤7.1：构造粗分辨率和细分辨率两个似然场；

步骤7.2：首先在粗分辨率似然场上进行搜索，获取最优位姿；

步骤7.3：将粗分辨率最优位姿对应的栅格进行细分辨率划分，然后再进行细分辨率搜索，再次得到最优位姿；

步骤7.4：粗分辨率地图的栅格的似然场值为对应的细分辨率地图对应的所有栅格的最大值。

本发明技术方案更进一步的说：所述步骤8采取的是三维窗口遍历寻优实时相关性扫描匹配的方式进行的；采取的是双分辨率的粗分辨率和细分辨率的两次搜索；分为以下四步：

步骤8.1：构造粗分辨率和细分辨率两个似然场；

步骤8.2：首先在粗分辨率似然场上进行搜索，获取最优位姿；

步骤8.3：把粗分辨率最优位姿对应的栅格进行细分辨率划分；

步骤8.4：然后再进行细分辨率搜索，再次得到最优位姿，粗分辨率地图的栅格的似然场值为对应的细分辨率地图对应空间的所有栅格的最大值。

本发明技术方案更进一步的说：所述步骤9具体如下：

步骤9.1：当前帧与前一帧节点相连添加边；

步骤9.2：当前帧与距离当前一定距离内的数据链相连添加边；

步骤9.3：以当前节点为搜索中心，进行广度优先搜索，得到所有关联的节点，用当前节点对利用关联的节点生成的激光数据链进行匹配，若响应值达到一定阈值，则产生一条一端是当前节点，另一端是数据链中质心距离当前节点质心最近的节点的边。

有益效果

本发明所提出的一种基于Karto SLAM的鲁棒闭环检测新方法，首先设计快速窗口滤波算法对激光数据进行滤波，然后使用延迟决策策略对闭环检测部分进行优化，优化后的闭环检测与算法原闭环检测方法不同之处在于该策略不会在a、b两点观察到了相同的环境信息后就认为机器人来到了原来的地方，在c、d点观察到了相同的环境信息后才会触发闭环检测。在一定程度上提高了***闭环检测的准确率，进一步提高了机器人后续路径规划、导航等相关工作的精度，解决了机器人在特殊场景(如巷道、走廊)下的定位与建图问题，因此在一定程度上也提高了该算法的实用性和鲁棒性。

附图说明

图1为扫描匹配搜索区域示意图；

图2为粗匹配、精匹配示意图；

图3为延迟决策策略示意图；

图4为扫描匹配原理示意图；

图5为闭环检测原理示意图；

图6为快速窗口滤波处理前后实验效果图；

图7为Karto SLAM和Lazy Karto算法建图效果；

图8为两种算法相对误差对比折线图。

具体实施方式

现结合实施例、附图对本发明作进一步描述：

在现有的激光SLAM算法当中，基于滤波算法的激光SLAM存储的协方差矩阵和状态量随着环境地图的扩大呈平方增长关系，Karto SLAM基于图优化的方法，该算法采用相关加速方法和相关性匹配方法，解决了基于滤波方式的SLAM在大地图范围环境中计算压力大、建图崩溃的问题，同时回环检测环节的加入让激光SLAM及时矫正地图、构建大尺度高精度地图成为了可能。但是，Karto SLAM在设备抖动时或者处于特征相似环境中建图效果不好，甚至又可能闭环检测失败，建图崩溃。

本发明实例提供了一种基于Karto SLAM，设计针对设备抖动的快速窗口滤波算法和采用延迟决策策略的方法，攻克了机器人在路面颠簸、设备抖动情况导致激光数据出现毛刺现象以及机器人在特征相似环境下易出现回环检测误判的技术难题。

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

从数据集中读取环境中特征物体距离信息。在同步定位与制图中，根据传感器及算法的不同，地图的表示方法大致分为四类：栅格地图、特征点地图、直接表征法、拓扑地图。Karto SLAM采用的地图表达方式为占据栅格地图，占据栅格地图中的每个像素代表障碍物的分布的概率。在占据栅格地图中，用p(s＝1)表示一个未被占用的点，用p(s＝0)表示该点被占用的概率，用两者的比值代表一个点的状态：

在得到测量值后，更新这个点的状态为：

表示在测量值下s的状态。在占据栅格地图中，一个点的颜色越深代表该点空闲的可能性越大，一个点的颜色越浅代表被占据的可能性越大。

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

中值滤波可以克服发生偶然情况下的波动干扰，当参数变化较快时将不再适用，算术平均滤波算法不适用于在要求计算速度较快的情况下，本发明涉及的快速窗口滤波算法在平滑数据的同时减少了计算量。该算法首先对输入的激光雷达信息进行转化，具体为首先将极坐标下的距离和角度信息转化为直角坐标系下的数据，然后选取窗口并设定窗口大小，计算窗口包含的激光信息点数记为K，然后计算窗口内所有点的均值：

比较p(x)与δ·p'(x)的大小，如下：

其中，参数δ在本发明中设为0.7，对于窗口中的所有数据点，如果p(x)大于δ·p'(x)，认为此处数据波动超过设定范围，此处p(x)被p'(x)替换，进行数据平滑，反之，认为设备未发生抖动。最终输出所有处理后的激光数据用于后续扫描匹配和回环检测。

为了更清晰明了的展示快速窗口滤波算法处理抖动数据的能力，在Matlab仿真环境下对激光雷达数据集进行滤波效果仿真，具体为：

将.bag文件转化为Matlab可用的.mat数据集形式。

启动快速窗口滤波算法对转化后的数据集进行滤波处理。

图6(1)和(2)分别为转化后的数据集结果图与使用快速窗口滤波算法处理抖动数据后的数据集结果图。

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

p(x_0:t,m|z_1:t,u_1:t) (5)

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

其中判断关键帧中建立节点主要依据如下：

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

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

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

步骤5：将激光数据信息以一定的角分辨率和角平移值进行投影，获得查找表；所述的数据信息包括距离周围障碍物的位置信息和角度信息；

图1为查找表示意图，将激光数据信息以一定的角分辨率和角平移值进行投影，获得n个角度的查找表，由里程计预测可以得到当前预估的姿态角，真实的姿态角必定在附近，查找表的角度计算方法如下：

如图1，白色栅格代表高斯模糊效果，黑色位置点代表机器人实际预测位置，灰色点代表栅格地图中的激光数据点。

步骤6：在查找表上，通过相关性扫描匹配的方法进行扫描匹配，通过遍历搜索区域，获得响应值最高的位置，得到机器人的角度信息；

假设机器人从位置x₀和x₁处感知环境信息，机器人在x₀和x₁处都可以看到环境某个部分，存在刚体变换T来对齐z₀与z₁，扫描匹配可以解释为求解两帧雷达数据或者雷达数据与地图之间的旋转矩阵(位移和旋转)的过程；

首先，采用极大似然估计的方式来对扫描匹配进行建模：

其中x_t为***待求解量，x_t-1为前一时刻机器人的位姿、z_t为当前帧激光雷达所有数据点的集合，m_t-1为现有的地图信息，u_t-1为上一时刻到这一时刻的运动相关的观测量(如IMU数据、里程计数据等)。

很显然，激光雷达观测数据中包含了互相独立的点数据，可以对该模型进行简化，式(7)转化为：

在计算式(8)之前，需要对地图进行栅格化，上式将一整帧的观测数据的概率拆成了这一帧数据的每一个点的概率。

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

步骤7.1：构造粗分辨率和细分辨率两个似然场；

步骤7.2：首先在粗分辨率似然场上进行搜索，获取最优位姿；

步骤7.3：将粗分辨率最优位姿对应的栅格进行细分辨率划分，然后再进行细分辨率搜索，再次得到最优位姿；

步骤7.4：粗分辨率地图的栅格的似然场值为对应的细分辨率地图对应的所有栅格的最大值。

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

Karto SLAM算法中，粗匹配时默认搜索角度范围为20°，细匹配默认搜索角度范围为0.2°。

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

在粗匹配和精匹配的过程中，设置最优响应值容忍度为10^-6，如果在匹配过程中某一位姿状态响应值与最优响应值在容忍度内，即可视为相等，节点取值为位姿状态响应值与最优响应值平均值。

步骤9中添加顶点相当于在图中添加关键帧的位姿。

步骤9中添加边时主要分为以下几步：

步骤9.1：当前激光数据帧与前一激光数据帧相连添加边；

步骤9.2：当前激光数据帧与距离当前激光数据帧一定距离和数量的数据链相连；

步骤9.3：在一定范围内用广度优先搜索的方法进行搜索，以当前节点为搜索中心，开始遍历相邻的在一定范围内的所有关联节点，生成满足一定数量的数据链，用当前节点与激光数据链进行匹配，若响应和协方差达到一定阈值则会产生一条边，这条边一个端点为当前节点，一个端点为激光数据链中质心距离当前节点质心最近的节点。

所述步骤10中闭环检测示意图如图5所示，包括如下步骤：

步骤10.1：根据当前节点,在一定范围内从图优化的图中找到相邻的所有节点；

步骤10.2：采取BFS(Breadth First Search)广度优先搜索的方式，将相邻节点与相连节点添加进临近关联帧中；

步骤10.3：从激光数据管理器中依据id序号选取从前到后满足一定数量的与当前在一定距离范围内，且不在临近关联帧中的候选节点；

步骤10.4：进行帧-图方式的扫描匹配，当匹配的均值和协方差达到一定要求后，认为检测到闭环，得到调整后的节点；

步骤10.5：进行全局闭环检测；

步骤10.6：进行稀疏位姿调整。

所述步骤10中延迟决策策略指的是一般情况下，当a位置点和b位置点观察到相同的环境信息时，算法就会判定为机器人回到了之前所来过的地方，进行闭环检测来优化位姿。而加入延迟决策策略后，算法并不会马上进行闭环检测，只有当c、d两位置点观察到了相同的环境信息时才会进行闭环检测。

步骤11：满足延迟决策闭环检测条件，进行闭环检测，如图5，闭环检测包括如下过程：

步骤11.1：根据当前节点，首先从图中找到一定范围内与当前节点相邻的所有节点；

步骤11.2：采用广度优先搜索的方式，将相邻节点与相连节点添加到临近扫描帧中；

广度优先搜索(Breadth First Search)，也称为宽度优先搜索，搜索时，从初始节点V₀开始，访问所有与初始节点相邻接的节点V₁,V₂,…V_n然后依次访问与V₁,V₂,…V_n相邻接的所有未曾访问过的顶点，同样的方法进行遍历直到所有的节点都被访问过为止，找到满足要求的节点。

步骤11.3：从激光雷达数据中，除去与当前节点相邻节点和相连节点，根据id值依次选取在一定范围内的候选帧直到达到数量要求为止；

步骤11.4：进行帧-图方式的扫描匹配，根据步骤9的延迟决策策略，检测到闭环，得到调整优化过的位姿；

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

图优化问题本质上其实是一个最小二乘问题，也就是求解最佳位姿使得误差平方函数最小：

其中，x_i、x_j为图优化中的顶点，也就是机器人位姿，z_ij为雷达测量值，Ω_ij为信息矩阵也就是误差权重矩阵，一定程度上可以表示x_i、x_j与测量值z_ij的吻合程度。

为了便于表示，我们将误差函数简写为：

e(x_i,x_j,z_ij)＝e_ij(x_i,x_j)＝e_ij(x) (11)

由于受到高斯白噪声的影响，传感器的多变量测量值可以看作以真值为中心的多元高斯分布：

多元高斯分布中，高斯曲线越高瘦，证明在这个方向越确定，因此对于最大似然概率，我们应使其出现在概率最大的地方，对测量平方误差函数进行取对数变换如下：

显然，图优化问题本质上为求解最小二乘问题，然而在实际情况中，机器人位姿间变化函数为非线性的，采用对数值进行迭代求解的方法来解决非线性优化问题，具体为给定一个初始值，从初始值不断迭代更新待优化变量使得目标函数误差最小化，满足收敛条件，停止迭代，得到最优解。用迭代法进行求解该非线性最小二乘问题，一般情况下主要分为Gauss-Newton和Levenberg-Marquardt法，Karto SLAM算法中采用LM法来求解该线性优化问题。

假设初始值为

误差函数在初始值附近进行一阶泰勒展开来获得最优解：

其中，J_ij为误差函数e_ij(x)在

附近的雅可比矩阵，e_ij为

缩写形式。

将式(14)代入误差平方和中：

其中，

将(19)式代入到误差平方和函数中：

上式中，c＝∑c_ij，b＝∑b_ij，H＝∑H_ij，要使得误差平方和最小，则对式(20)进行求导并使其等于0：

HΔx^*＝-b (21)

上式中，H为***的信息矩阵，在欧氏空间中***的解表示为：

实际上在该***中为非欧氏空间，***的解表示为：

Levenberg-Marquardt法引入迭代因子λ来控制达到收敛的迭代速度：

(H+λI)Δx^*＝-b (24)

不断迭代，直至收敛。

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

具体实施例：

步骤1：实验采用数据集对本算法进行测试，实验中所用到的计算机配置为：***为Ubuntu 16.04，CPU为i5 9400A，基本频率频为2666MHz，内存为8GB。

步骤2：首先，将输入的激光数据集格式利用程序转化为Matlab可用的.mat数据文件格式。

步骤3：运行快速窗口滤波算法程序，对转换后的数据集进行数据预处理工作。

步骤4：图6为激光雷达数据在数据抖动情况下快速窗口滤波算法滤波前后激光数据优化前后的对比图，测试结果表明：快速窗口滤波算法能够很好的平滑激光设备因抖动而出现的地图毛刺现象，滤波后的数据用于算法后续回环检测工作进一步提高了回环检测的准确性和算法的鲁棒性。

步骤5：我们将使用快速窗口滤波处理后的数据使用延迟决策策略后的KartoSLAM算法称为Lazy Karto，实验采用相对位姿误差(Relative Pose Error，RPE)作为评价指标来评估本算法的定位精度，将Karto SLAM算法与Lazy Karto算法的定位精度进行了对比。

图7分别为原Karto SLAM算法和Lazy Karto建图效果，我们选取八处特征点，绘制相对误差对比图如图8，优化后的算法Lazy Karto相比原Karto SLAM算法最大相对误差为28.99％，最小相对误差为4.312％，在容易出现闭环的特征点6、7、8处，原Karto SLAM算法出现建图歪扭、重影等问题，优化后的算法Lazy Karto能够在颠簸路段设备抖动情况下很好的检测出这些闭环点，优化后的算法在一定程度上提高了闭环检测的准确率和鲁棒性。

Claims (8)

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中的子地图指的是矩形形状的兴趣区域，这个兴趣区域也可以理解为参考模型，搜索区域为以里程计估计的位置为中心的一个矩形区域，搜索区域表示机器人最终位置的可能范围，在扫描匹配时，通过遍历搜索区域来获取响应值最高的位置。

6.根据权利要求1所述的一种基于Karto SLAM的鲁棒闭环检测新方法，其特征在于所述步骤7具体如下：

步骤7.1：构造粗分辨率和细分辨率两个似然场；

步骤7.2：首先在粗分辨率似然场上进行搜索，获取最优位姿；

步骤7.3：将粗分辨率最优位姿对应的栅格进行细分辨率划分，然后再进行细分辨率搜索，再次得到最优位姿；

步骤7.4：粗分辨率地图的栅格的似然场值为对应的细分辨率地图对应的所有栅格的最大值。

7.根据权利要求1所述的一种基于Karto SLAM的鲁棒闭环检测新方法，其特征在于所述步骤8采取的是三维窗口遍历寻优实时相关性扫描匹配的方式进行的；采取的是双分辨率的粗分辨率和细分辨率的两次搜索；分为以下四步：

步骤8.1：构造粗分辨率和细分辨率两个似然场；

步骤8.2：首先在粗分辨率似然场上进行搜索，获取最优位姿；

步骤8.3：把粗分辨率最优位姿对应的栅格进行细分辨率划分；

步骤8.4：然后再进行细分辨率搜索，再次得到最优位姿，粗分辨率地图的栅格的似然场值为对应的细分辨率地图对应空间的所有栅格的最大值。

8.根据权利要求1所述的一种基于Karto SLAM的鲁棒闭环检测新方法，其特征在于所述步骤9具体如下：

步骤9.1：当前帧与前一帧节点相连添加边；

步骤9.2：当前帧与距离当前一定距离内的数据链相连添加边；

步骤9.3：以当前节点为搜索中心，进行广度优先搜索，得到所有关联的节点，用当前节点对利用关联的节点生成的激光数据链进行匹配，若响应值达到一定阈值，则产生一条一端是当前节点，另一端是数据链中质心距离当前节点质心最近的节点的边。

Images