一种激光SLAM的回环检测方法与流程

一种激光slam的回环检测方法
技术领域
1.本发明涉及属于移动机器人领域，特别是一种激光slam的回环检测方法。


背景技术：

2.随着时代的发展，人们越来越关注将移动机器人用于实际环境，机器人自动构建周围环境地图(simultaneous localization and mapping,slam)并成功定位自身的研究方向是移动机器人领域的核心问题，尽管具有多个优点，但是机器人自动绘制周围环境并成功定位自身的能力仍然十分有限。目前仅基于激光雷达的slam方法在机器人运动较快、室外环境变化小的情况下长时间运行容易产生误差漂移的现象。
3.回环检测方法可以有效解决激光slam领域的运动漂移问题。现有的回环检测方法多从处理点云特征的特征向量入手，如直方图等，计算量大且在室外场景变化较小、机器人反方向行驶至相同路段的情况下具有局限性。


技术实现要素：

4.有鉴于此，本发明的目的是提供一种激光slam的回环检测方法，可以实现降低点云计算复杂度的同时保证良好的回环检测精度和鲁棒性。
5.本发明采用以下方案实现：一种激光slam的回环检测方法，包括以下步骤：
6.步骤s1：获取单个关键帧三维环境点云数据及其同步惯性测量单元(inertial measurement unit,imu)信息即imu信息；
7.步骤s2：将当前帧点云空间分割为r
×
l个单元空间，将单元空间数据分别存入a
r
×
l
矩阵，以此构建第一描述符；将分割后的点云数据与imu信息存入b
r 3
向量，以此构建第二描述符；
8.步骤s3：用所有历史关键帧的第二描述符构建kdtree；计算当前帧与历史关键帧第二描述符之间的相似性值，用帧间相似性值作为最近邻搜索的边，以此从kdtree中找出n个与当前帧相似度最高的候选相似帧；
9.步骤s4：将候选相似帧与当前帧的第一描述符的列向量依据步骤s3所述相似性值进行候选相似帧与当前帧的回环匹配，得出当前帧与相似性最高的候选相似帧的相对旋转值,以相对旋转值和imu信息作为ndt匹配的初值，得到当前帧与该候选相似帧的相对位姿变化，若变化量小于阈值，则回环检测成功，进行回环修正；否则，回环检测失败，进入下一帧的回环检测。
10.进一步地，所述步骤s1的具体内容为：将去除了nan值的点云作为本回环检测方法的输入点云；获取当前帧的同步imu数据信息,将获取的imu信息进行积分以得到当前帧的位置数据。
11.进一步地，所述步骤s2的具体内容为：
12.以激光雷达为原点，按照极坐标的形式，沿顺时针方向将当前帧点云空间平均分为l列，沿着半径增大方向将当前帧点云空间平均分为r行，将当前帧点云空间分割为r
×
l
个单元空间，在每个单元空间中选取离原点最近的点作为单元空间值，将单元空间值依次存入a
r
×
l
矩阵，完成第一描述符的构建；
13.对单元空间中的点云点数进行计数，若点数大于阈值则认为该单元空间非空，将当前帧点云空间的第r行，r∈[1，r]中非空单元空间的个数作为向量b
r 3
的第r个数据值，将r个数据值都计算完成后，将当前点云的位置数据，作为向量b
r 3
的最后三个值，完成第二描述符的构建。
[0014]
进一步地，步骤s3中所述相似性计算如下：以第二描述符构建kdtree，先对当前帧第二描述符与历史帧第二描述符的最后三个值进行欧氏距离计算，若计算结果高于阈值则丢弃该历史帧，否则，计算当前帧第二描述符与历史帧第二描述符的距离得出候选相似帧，其距离计算公式为：
[0015][0016]
其中，b和b
n
是第m帧点云与第n帧点云的第二描述符。
[0017]
进一步地，步骤s4的具体内容为：
[0018]
用候选相似帧与当前帧的第一描述符的列向量进行步骤s3所述的相似性计算；将当前帧第一描述符按列向量进行平移，得到l个当前帧的第一描述符，将l个第一描述符逐一与候选相似帧进行相似性计算，选择相似性结果最高的一个值做为当前帧与该候选相似帧的相似性值；选取n个候选相似帧中与当前帧相似性最高的帧作为回环检测帧，其相似性判断公式为：
[0019][0020]
其中，s
m
为当前帧的第一描述符，s
n
为候选相似帧的第一描述符，为当前帧的第i列，为候选相似帧的第k列；
[0021]
若当前帧的第x(x∈[1，l])个第一描述符与回环检测帧相似性值最高，则当前帧与当前帧帧之间的相对旋转值q为：
[0022][0023]
以q和向量b
r 3
的后三个值作为初始位姿，进行当前帧与回环检测帧的ndt配准，若配准结果小于阈值，则认为回环检测成功，进行回环修正，否则，回环检测失败，进行下一关键帧的回环检测。
[0024]
与现有技术相比，本发明具有以下有益效果：
[0025]
适用于特征变化显著的场景，降低了点云数据的计算复杂度；在局部路段特征变化不明显的室外场景以及回环行驶方向与历史行驶方向差异大的场景中，可以实现降低点云计算复杂度的同时保证良好的回环检测精度和鲁棒性。
附图说明
[0026]
图1为本发明实施例的方法流程图。
[0027]
图2为本发明实施例的在kitti数据集00场景的轨迹真值与本发明所述方法的轨
迹对比图。
[0028]
图3为本发明实施例的在kitti数据集08场景的轨迹真值与本发明所述方法的轨迹对比图。
具体实施方式
[0029]
下面结合附图及实施例对本发明做进一步说明。
[0030]
应该指出，以下详细说明都是例示性的，旨在对本技术提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本技术所属技术领域的普通技术人员通常理解的相同含义。
[0031]
需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本技术的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
[0032]
如图1所示，本实施例提供一种激光slam的回环检测方法，包括以下步骤：
[0033]
步骤s1：获取单个关键帧三维环境点云数据及其同步惯性测量单元(inertial measurementunit,imu)信息即imu信息；
[0034]
步骤s2：将当前帧点云空间分割为r
×
l个单元空间，将单元空间数据分别存入a
r
×
l
矩阵，以此构建第一描述符；将分割后的点云数据与imu信息存入b
r 3
向量，以此构建第二描述符；
[0035]
步骤s3：用所有历史关键帧的第二描述符构建kdtree(k
‑
dimensional
‑
tree)；计算当前帧与历史关键帧第二描述符之间的相似性值，用帧间相似性值作为最近邻搜索的边，以此从kdtree中找出n个与当前帧相似度最高的候选相似帧；
[0036]
步骤s4：将候选相似帧与当前帧的第一描述符的列向量依据步骤s3所述相似性值进行候选相似帧与当前帧的回环匹配，得出当前帧与相似性最高的候选相似帧的相对旋转值,以相对旋转值和imu信息作为ndt匹配的初值，得到当前帧与该候选相似帧的相对位姿变化，若变化量小于阈值(如0.01)，则回环检测成功，进行回环修正；若回环检测失败，进入下一帧的回环检测；
[0037]
在本实施例中，所述步骤s1的具体内容为：将去除了nan值(点云数据中的距离异常或表面反射的干扰点)的点云作为输入点云；获取与当前点云帧的同步imu数据信息,将获取的imu信息进行积分以得到当前帧的位置数据。
[0038]
在本实施例中，所述步骤s2的具体内容为：
[0039]
以激光雷达为原点，按照极坐标的形式，沿顺时针方向将当前帧点云空间平均分为l列，沿着半径增大方向将当前帧点云空间平均分为r行，将当前帧点云空间分割为r
×
l个单元空间，在每个单元空间中选取离原点最近的点作为单元空间值，将单元空间值依次存入a
r
×
l
矩阵，完成第一描述符的构建；
[0040]
对单元空间中的点云点数进行计数，若点数大于阈值(如30个点)则认为该单元空间非空，将当前帧点云空间的第rr∈[1,r]行,r∈[1,r]中非空单元空间的个数作为向量b
r 3
的第r个数据值，将r个数据值都计算完成后，将当前点云的位置数据，作为向量b
r 3
的最后三个值,完成第二描述符的构建。
[0041]
将每一关键帧的点云的第一描述符和第二描述符以矩阵的形式，以其对应关键帧索引值为命名存入描述符库，以便后续的调用；
[0042]
在本实施例中，步骤s3中所述相似性计算如下：以第二描述符构建kdtree，先对当前帧第二描述符与历史帧第二描述符的最后三个值进行欧式距离计算，若计算结果高于阈值(如3米)则丢弃该历史帧，否则，计算当前帧第二描述符与历史帧第二描述符的距离得出候选相似帧，其距离计算公式为：
[0043][0044]
其中，b和b
n
是第m帧点云与第n帧点云的第二描述符。
[0045]
在本实施例中，步骤s4的具体内容为：
[0046]
用候选相似帧与当前帧的第一描述符的列向量进行步骤s3所述的相似性计算；将当前帧第一描述符按列向量进行平移，得到l个当前帧的第一描述符，将l个第一描述符逐一与候选相似帧进行相似性计算，选择相似性结果最高的一个值做为当前帧与该候选相似帧的相似性值；选取n个候选相似帧中与当前帧相似性最高的帧作为回环检测帧，其相似性判断公式为：
[0047][0048]
其中，s
m
为当前帧的第一描述符，s
n
为候选相似帧的第一描述符，为当前帧的第i列，为候选相似帧的第k列；
[0049]
若当前帧的第x个x∈[1，l]第一描述符与回环检测帧相似性值最高，则当前帧与当前帧帧之间的相对旋转值q为：
[0050][0051]
以q和向量b
r 3
的后三个值作为初始位姿，进行当前帧与回环检测帧的ndt配准，若配准结果小于阈值(如0.01)，则认为回环检测成功，进行回环修正，否则，回环检测失败，进行下一关键帧的回环检测。
[0052]
较佳的，在本实施例中，通过将经过时间一致性处理的点云数据与imu数据进行融合，构建第二描述符，用于快速检索得到候选相似帧；再根据第一描述符将当前帧与候选相似帧进行相似性对比，得出二者的相对旋转变换，将得出的相对旋转作为点云配准算法ndt((normal distribution transform)的初始位姿；最后，将当前帧与候选相似帧所构成的局部地图进行配准以完成回环检测。
[0053]
较佳的，在本实施例中，
[0054]
步骤1：获取单个关键帧点云数据及其同步imu信息；主要的实现方式是：去除点云数据中的距离异常或表面反射的干扰点；获取当前点云帧的同步imu数据信息；将获取的imu信息进行积分以得到当前点云帧的位置数据；
[0055]
步骤2：将当前帧点云空间分割为r
×
l个单元空间，将单元空间数据分别存入a
r
×
l
矩阵，以此构建第一描述符；将分割后的点云数据与imu信息存入b向量，以此构建第二描述符；主要的实现方式是：以激光雷达为原点，按照极坐标的形式，沿顺时针方向将当前帧点
云空间平均分为l列，沿着半径增大方向将当前帧点云空间平均分为r行，将当前帧点云空间分割为r
×
l个单元空间，在每个单元空间中选取离原点最近的点作为单元空间值，将单元空间值依次存入a
r
×
l
矩阵，完成第一描述符的构建；
[0056]
对单元空间中的点云点数进行计数，若点数大于阈值(如30个点)则认为该单元空间非空，将第r行的单元空间中非空的个数作为向量b的第r个数据值，将r个数据值都计算完成后，将当前点云的位置数据，存入向量b，完成第二描述符的构建；
[0057]
将每一关键帧的点云的第一描述符和第二描述符以矩阵的形式，以其对应关键帧索引值为命名存入描述符库，以便后续的调用；
[0058]
步骤3：以第二描述符构建kdtree，先对当前帧第二描述符与历史帧第二描述符的最后三个值进行欧式距离计算，若计算结果高于阈值(如3米)则丢弃该历史帧，否则，
[0059]
通过计算当前帧第二描述符与历史帧第二描述符的相似性得出候选相似帧，其相似性计算公式为：
[0060][0061]
其中b和b
n
是第m帧点云与第n帧点云的第二描述符；
[0062]
步骤4：用候选相似帧与当前帧的第一描述符的列向量进行步骤3所述的相似性计算；将当前帧第一描述符按列向量进行平移，得到l个当前帧的第一描述符，将l个第一描述符逐一与候选相似帧进行相似性计算，选择相似性结果最高的一个值做为当前帧与该候选相似帧的相似性值；选取n个候选相似帧中与当前帧相似性最高的帧作为回环检测帧，其相似性判断公式为：
[0063][0064]
其中，s
m
为当前帧的第一描述符，s
n
为候选相似帧的第一描述符，为当前帧的第i列，为候选相似帧的第k列；
[0065]
若当前帧的第x(x∈[1，l])个第一描述符与回环检测帧相似性值最高，则当前帧与当前帧帧之间的相对旋转值q为：
[0066][0067]
以q和向量b
r 3
的后三个值作为初始位姿，进行当前帧与回环检测帧的ndt配准，若配准结果小于阈值(如0.01)，则认为回环检测成功，进行回环修正，否则，回环检测失败，进行下一关键帧的回环检测。
[0068]
本实施例中，为验证本发明技术方法的可行性，选取kitti数据集中的回环数比较多的00序列和05序列进行实施例所述方案的实现，具体结果如图2(00序列)，图3(05序列)所示，图中为使用本方法进行地图构建的轨迹(实线)与该序列的轨迹真值(虚线，即reference)的全路段轨迹对比结果，轨迹对比图充分说明本方法的合理性以及有效性。
[0069]
以上所述仅为本发明的较佳实施例，凡依本发明申请专利范围所做的均等变化与修饰，皆应属本发明的涵盖范围。