CN109522832A - 一种基于点云片段匹配约束和轨迹漂移优化的回环检测方法 - Google Patents

Abstract

一种基于点云片段匹配约束和轨迹漂移优化的回环检测方法，采用实时激光雷达里程计与建图LOAM系统，但是LOAM系统在长时间建图之后会产生较大漂移，所以提出一种点云片段匹配约束和轨迹漂移优化的回环检测方法。通过对激光雷达获取的点云进行分割、描述、匹配，从而找到回环关系，发现回环之后利用显式循环闭合ELCH算法进行回环调整，并且提出优化算法来消除点云地图调整过程出现的局部不一致。本发明在优化之后提出位姿预测与补偿算法对回环之后的位姿进行预测和补偿。从而能达到最大化减少漂移误差的效果。

Description

一种基于点云片段匹配约束和轨迹漂移优化的回环检测方法

技术领域

本发明涉及到计算机视觉、机器学习等技术领域，具体涉及一种基于点云片段匹配约束和轨迹漂移优化的回环检测方法。

背景技术

在许多机器人应用中，比如空中地图测绘，灾难救援，太空探索等任务，通常没有预先建立好的地图信息。SLAM系统的出现为这一问题提供了很好的解决方案，它能赋予机器人探索未知环境的能力。机器人感知环境并通过SLAM技术实现了在未知环境中的同时定位与建图。而让机器人有感知外界能力的是各式各样的传感器。常用的传感器有相机，激光雷达,惯性传感器，GPS等。目前大多数SLAM系统使用相机作为传感器。随着无人车，无人机的发展，人们对SLAM技术的精度要求越来越高，而光学相机作为一种被动传感器受环境的影响非常大，例如光照条件发生剧烈变化时，视觉SLAM系统极有可能发生丢失等情况；当光照条件不足时，视觉SLAM也无法工作；当相机在高速运动的时候，相机无法获得稳定的图像，甚至无法初始化。而激光雷达作为一种主动传感器有着测距精确和辐射范围广的优势，并且能获得精确的三维点云信息，即使在高速运动情况下也有很高的鲁棒性。所以利用激光雷达采集数据进行SLAM系统的开发能够避免上述问题，具有很好的应用前景。

发明内容

本发明的目的是设计一种基于点云片段匹配约束和轨迹漂移优化的回环检测方法，解决激光雷达建图过程中轨迹漂移的问题，并且增强激光雷达建图的鲁棒性。

为了解决上述技术问题本发明提供如下的技术方案：

一种基于点云片段匹配约束和轨迹漂移优化的回环检测方法，包括以下步骤：

1)利用激光雷达获取时序点云数据，该数据经过实时激光雷达里程计与建图(Lidar Odometry and Mapping in Real-time，简称LOAM)系统产生位姿，设当前获取的点云为P_i，其中帧序号为i；

2)将获取的点云点云进行分割，分割算法采用欧几里得分割方法，在分割的过程中，采用EigenValue的方法，将分割后的点云进行描述，描述好的点云片段储存在建立好的数据结构中作为点云片段数据库；

3)获取的点云数据都要与之前建立的点云数据库进行比对，来找到匹配的数据，并且利用Kd-Tree来加快搜索的速度；利用随机森林算法，事先对点云数据进行匹配训练，建立训练好的随机森林模型；将输入的分割片段通过随机森林算法与点云片段数据库中进行决策，提取候选点云片段，并且利用欧几里得滤波来滤除距离过远的候选点云从而减少搜索空间；然后采用随机抽样一致性测试(RANSAC)方法来滤除存在偏差的候选匹配对，从而获得鲁棒的候选匹配点云对；

4)利用迭代最近点算法(Iterative Closest Point，简称ICP)算法，计算出当前点云和匹配的点云之间的漂移矩阵，在回环调整上，采用显式循环闭合(An Explicit LoopClosing Technique for 6D SLAM，简称ELCH)算法，通过图优化的方式将漂移误差转移到图中的各个节点中；

5)检测到回环之后，提出一种消除局部不一致性的方法，通过将位姿从绝对位姿分解为相对位姿来消除局部不一致；

6)在回环之后，提出一种位姿预测和补偿的方法，在回环后还能对位姿进行轻量位姿矫正，从而能在回环后减少位姿漂移；

7)最后经过优化的位姿输出到系统，得到最后的建图结果。

本发明的技术构思为：通过对激光雷达获取的点云进行分割、描述、匹配，从而找到回环关系。发现回环之后利用ELCH算法进行回环调整，并且利用消除局部不一致的算法，将调整后的位姿和当前收到的位姿进行调整。并且提出位姿预测与补偿算法对回环之后的位姿进行预测和补偿。从而能达到最大化减少漂移误差的效果。

本发明的有益效果主要表现在：通过回环检测和回环调整能够对回环部分进行全局一致性回环调整，减少建图误差，并且利用提出的优化方法能够提高系统运行效率，并且在回环调整后也能起到微调位姿，从而减少漂移的作用。最后通过该系统建的地图非常精确。

附图说明

图1是基于点云片段匹配约束和轨迹漂移优化的回环检测方法的系统框图。

具体实施方式

下面结合附图对本发明作进一步描述。

参照图1，一种基于点云片段匹配约束和轨迹漂移优化的回环检测方法，包括以下步骤：

1)利用激光雷达获取时序点云数据，该数据经过实时激光雷达里程计与建图(Lidar Odometry and Mapping in Real-time，简称LOAM)系统产生位姿，设当前获取的点云为P_i，其中帧序号为i，获得的时序点云数据通过LOAM系统生成初始的点云地图；

2)将获取的点云和位姿输入到点云分割与匹配(Segment based loop-closurefor 3D point clouds，简称Segmatch)系统中，首先将点云进行分割，分割算法采用欧几里得分割方法。在分割的过程中，采用EigenValue的方法，在这个方法中,我们通过计算点云片段的特征值并将其组合在维度为1×7的特征向量中,该特征向量由点云片段的线性,平面性,散射性,全方位性,各向异性,特征根和曲率测量值组成；然后将分割后的点云进行描述，描述好的点云片段储存在建立好的数据结构中作为点云片段数据库；

3)新获取的点云数据都要与之前建立的点云数据库进行比对，来找到匹配的数据，并且利用Kd-Tree来加快搜索的速度；利用随机森林算法，先对点云数据进行匹配训练，建立训练好的随机森林模型；将输入的分割片段通过随机森林算法与点云片段数据库中进行决策，提取候选点云片段，然后利用滤除欧几里得距离过远的点云来减少搜索空间。点云片段的H的重心为

其中p为点云片段中的点，N为点云片段的点的数量，n为该点云片段中点的序号；设空间中的预匹配点云片段为H_i和H_j,i和j分别为点云的序号,H_i和H_j点云片段的重心之间的欧几里得距离表示为

如果α为自定义的阈值，那么将该点云片段从搜索的空间中滤除，然后采用随机抽样一致性测试(RANSAC)方法来滤除存在偏差的候选匹配对，从而获得鲁棒的候选匹配点云对；

4)利用迭代最近点(Iterative Closest Point，简称ICP)算法，计算出当前点云和匹配的点云之间的漂移矩阵。在回环调整上，采用显式循环闭合(An Explicit LoopClosing Technique for 6D SLAM，简称ELCH)算法，该算法是一种利用图的思想进行优化的算法。

在图模型中，图包含顶点和边，设置边的测量为

E_i.j＝X_i-X_j (3)

其中X_i与X_j是边连接的对应顶点的位姿，并且边的观测值为

其中是观测值与真实值的偏差，那么图模型优化的目标为最小化下列马氏距离

其中为对应的协方差，然后再通过线性插值的方法将漂移误差转移到图的各个节点中，当只存在一个回环的情况下

R_i＝slerp(n-1,R)

其中R_i,t_i分别为回环图中i节点分配的旋转量和平移量，n为图中产生回环顶点的距离，为总的偏移距离，R为总的旋转偏移。slerp()函数为旋转矩阵的插值函数，该差值函数具体步骤为：

首先将旋转矩阵转化为四元数

其中r_i，j为旋转矩阵R中的元素，例如r_1，2为R中第一行第二列的元素，tr(R)为该旋转矩阵的迹；

当r_1，1>r_2,2，r_1，1>r_3，3时，

当r_2，2>r_3，3时，

其他情况时，

然后将对应的四元数转化为对应的轴角a

最后插值后的旋转矩阵为，其中q₀，q_x，q_y，q_z为四元数中的分量；

当存在多个回环时，我们使用Dijistra算法，找出图中回环路径最短的路径，再用上述公式进行优化；

5)检测到回环之后，提出一种消除局部不一致性的方法，利用将位姿分解为相对位姿的方法，在e+1>0时刻的绝对位姿为S_e+1，那么该时刻对于前一时刻的相对位姿为

最后将系统中新输入n时刻的绝对位姿通过公式(14)得到最终的绝对位姿；

其中S_n-1为经过回环调整后的绝对位姿，S为最终新输入帧的绝对位姿；

6)在回环之后，提出一种位姿预测和补偿的方法，如下公式为位姿的补偿，

其中为从s时刻到e时刻的位置偏移，X_n+1,X_n为n+1，n图中回环节点中对应的位姿，N为回环中节点的总数，其中最后预测的位姿为S_f，S_f为

其中S_c为补偿参数的李代数写法，S_n为当前获得相对位姿，计算公式为：

S_j为当前获得的位姿，为前一帧位姿的逆，S_j，S_j-1，都为李代数的写法。

Claims (1)

1.一种基于点云片段匹配约束和轨迹漂移优化的回环检测方法，其特征在于，所述方法包括以下步骤：

1)利用激光雷达获取时序点云数据，该数据经过实时激光雷达里程计与建图LOAM系统产生位姿，设当前获取的点云为P_i，其中帧序号为i，获得的时序点云数据通过LOAM系统生成初始的点云地图；

2)将获取的点云和位姿输入到点云分割与匹配系统中，首先将点云进行分割，分割算法采用欧几里得分割方法；在分割的过程中，采用EigenValue的方法，在这个方法中,我们通过计算点云片段的特征值并将其组合在维度为1×7的特征向量中,该特征向量由点云片段的线性,平面性,散射性,全方位性,各向异性,特征根和曲率测量值组成；然后将分割后的点云进行描述，描述好的点云片段储存在建立好的数据结构中作为点云片段数据库；

3)新获取的点云数据都要与之前建立的点云数据库进行比对，来找到匹配的数据，并且利用Kd-Tree来加快搜索的速度；利用随机森林算法，先对点云数据进行匹配训练，建立训练好的随机森林模型；将输入的分割片段通过随机森林算法与点云片段数据库中进行决策，提取候选点云片段，然后利用滤除欧几里得距离过远的点云来减少搜索空间；点云片段的H的重心为

其中p为点云片段中的点，N为点云片段的点的数量，n为该点云片段中点的序号，设空间中的预匹配点云片段为H_i和H_j,i和j分别为点云的序号,H_i和H_j点云片段的重心之间的欧几里得距离表示为

如果α为自定义的阈值，那么将该点云片段从搜索的空间中滤除，然后采用随机抽样一致性测试方法来滤除存在偏差的候选匹配对，从而获得鲁棒的候选匹配点云对；

4)利用迭代最近点ICP算法，计算出当前点云和匹配的点云之间的漂移矩阵，在回环调整上，采用显式循环闭合ELCH算法，在图模型中，图包含顶点和边，设置边的测量为

E_i.j＝X_i-X_j (3)

其中X_i与X_j是边连接的对应顶点的位姿，并且边的观测值为

其中是观测值与真实值的偏差，那么图模型优化的目标为最小化下列马氏距离

其中为对应的协方差，然后再通过线性插值的方法将漂移误差转移到图的各个节点中，当只存在一个回环的情况下

R_i＝slerp(n-1,R)

其中R_i,t_i分别为回环图中i节点分配的旋转量和平移量，n为图中产生回环顶点的距离，为总的偏移距离，R为总的旋转偏移，slerp()函数为旋转矩阵的插值函数，该差值函数的步骤为：

4.1首先将旋转矩阵转化为四元数

其中r_i,j为旋转矩阵R中的元素，例如r_1,2为R中第一行第二列的元素，tr(R)为该旋转矩阵的迹；

当r_1,1>r_2,2，r_1,1>r_3,3时，

当r_2,2>r_3，3时，

其他情况时，

4.2)然后将对应的四元数转化为对应的轴角a

4.3)最后插值后的旋转矩阵为，其中q₀，q_x，q_y，q_z为四元数中的分量；

当存在多个回环时，使用Dijistra算法，找出图中回环路径最短的路径，再用上述公式进行优化；

5)检测到回环之后，提出一种消除局部不一致性的方法，利用将位姿分解为相对位姿的方法，在e+1>0时刻的绝对位姿为S_e+1，那么该时刻对于前一时刻的相对位姿为

其中最后将系统中新输入n时刻的绝对位姿通过

其中S_n-1为经过回环调整后的绝对位姿，S_n为新输入帧的绝对位姿；

6)在回环之后，提出一种位姿预测和补偿的方法，如下公式为位姿的补偿，

其中为从s时刻到e时刻的位置偏移，X_n+1,X_n为n+1，n对应节点的位姿，N为回环中节点的总数，其中最后预测的位姿为S_f，S_f为

其中S_c为补偿参数的李代数写法，S_n为当前获得相对位姿，计算公式为：

S_j为当前获得的位姿，为前一帧位姿的逆，S_j，S_j-1都为李代数的写法。