一种基于二维激光扫描仪角点特征的闭环检测方法与流程

本发明属于激光定位与导航技术领域，特别地涉及一种基于二维激光扫描仪角点特征的闭环检测方法。在闭环检测方法中，涉及了二维点云的签名生成方法和相似点云间的相对位姿计算方法。

背景技术：

在slam中，只是依靠scanmatch或者里程计来建立地图的话，由于误差累积问题的存在，最终生成的地图会与实际环境有很大的偏差，而闭环算法作为校正这一偏差的重要方法，具有非常大的研究价值。一般而言，闭环算法可以分为两个步骤，首先需要进行地点识别，然后需要计算相似两帧之间的相对位姿进而生成图模型。地点识别的主要任务是当移动机器人从地点a出发后行进一段时间再度回到地点a时，需要能够识别出它之前到过地点a，即能检测到当前在a地扫到的数据帧f与出发时在a地扫到的数据帧f’具有相对较高的相似度。在地点识别完成之后，移动机器人需要计算两帧之间的相对位姿，同时根据得到的相对位姿也可以对两帧之间的相似度进行验证。在视觉领域，学者们已经对闭环算法进行了非常广泛的研究。相比之下，基于2dlidar的slam闭环算法研究工作就显得寥寥无几。而在基于2dlidar的闭环算法本就不多的研究工作中，利用栅格地图的研究又占了很大的比重。利用原始2d点云数据来进行闭环检测的工作不多的一个重要原因就是上一章中所提到的，即针对2dlidar进行特征提取的研究工作不多。而特征点作为闭环检测中地点识别环节的重要判定依据，与其相关的研究工作的缺乏直接导致了在原始2d点云数据上进行闭环检测工作的缺乏。从2010年以来，随着flirt、falko等算法的提出，基于2dlidar原始数据的闭环检测算法也打下了基础。在之后的时期里，也出现了一些很有意义的基于2dlidar的闭环检测工作，诸如gfp、glare、gsr以及glarot(glarerotation-invariant)等，其中前三者是基于flirt特征，glarot是基于falko。这些工作的一个共同点就是利用这些针对2dlidar的特征提取算法来将每帧数据压缩为一张用提取出的特征点生成的签名，这样就可以通过计算两张签名之间的距离大小来判断两帧数据之间的相似性，从而完成地点识别的任务。签名的生成方式可以分为两大类，一种需要通过结合特征描述子，例如gfp用特征描述子顺时针方向组合形成的短语来作为单帧数据的签名，另一种则是通过对特征点之间的相对位置关系进行离散化生成一个二维数组来作为单帧数据的签名，其中的代表是glare。glare对单帧数据中每对特征点之间连成的线段的距离d和角度θ进行离散化成nd和nθ，考虑到噪声的影响，glare没有采用one-hot的方式而是用一个以(nd,nθ)为中心的二元高斯分布来进行建模，最后所有特征点对形成的二元高斯分布之和作为最后的签名。gsr是在glare的基础上进行了改进，它把特征点之间的角度定义为两点法向量的角度之差，这使得glare具有了旋转不变性，相应的代价是计算量变大。glarot与glare和gsr不同之处在于它是基于falko特征。与gsr一样，它也具有旋转不变性，但它实现旋转不变性的方式与gsr实现旋转不变性的方式并不相同。gsr是在glare的基础上，改变了签名生成方式来获得旋转不变性，而glarot的签名生成方式与glare相同，它通过改变计算两张签名之间相似度的方式来获得旋转不变性，这样使得glarot的计算量相比gsr要减轻很多。检索得到相似度最高的帧后，需要解算出两帧之间的相对位姿。

技术实现要素：

本发明针对构建二维点云地图累积误差过大的情况，设计了一种基于二维激光扫描仪角点特征的闭环检测方法，用于减小二维点云地图创建过程中的累积误差。本发明具体可以分为点云签名生成以及相对位姿计算两大部分；第一，提出了一种具有旋转不变性的点云签名生成方法；第二，提出了一种高精度的相似点云之间相对位姿的计算方法。

本发明的技术方案为：一种基于二维激光扫描仪角点特征的闭环检测方法，利用从二维激光扫描仪获得的二维点云数据中提取出的角点特征f，对二维点云数据c进行以下步骤处理：

步骤1：利用提出得到的二维点云角点特征f，生成二维点云c的签名，该签名具有旋转不变性的特征。根据两帧点云签名之间的欧氏距离来衡量两帧点云之间的相似性。

步骤2：利用点云角点特征之间的几何结构来计算相似两帧点云之间的相对位姿，并用两帧点云之间的相对位姿构建图模型，对图模型进行优化后输出优化后的全局位姿。

进一步地，点云类型为二维点云，方法旨在对二维点云数据进行闭环检测，减小二维点云地图的累积误差。

进一步地，二维点云的签名通过以下方法得到：

(1.1)对二维点云中任意两个特征点之间形成的线段的距离和角度进行离散化。

(1.2)对离散化后的距离和角度进行高斯建模。

(1.3)综合所有特征点对相应的高斯模型，生成二维点云的归一化后的签名。

(1.4)将得到的签名沿角度轴平移n个槽位，得到二维点云签名对应的n个平行签名。

(1.5)对任意两帧点云的签名，其距离定义为一帧点云的签名与另一帧点云m个平行签名之间最小的欧式距离。

进一步地，两个特征点之间形成的线段的距离和角度离散化通过以下方法得到：

(1.1.1)对其中任意两个特征点fi和fj，其对应的坐标为(xi,yi)和(xj,yj)，则两个特征点之间形成的线段的距离和角度为：

对dij和进行等距离离散化，离散化后的就对应着一个大小为nd行，nψ列的二维直方图中的一个槽：

进一步地，对离散化后的距离和角度进行高斯建模通过以下方法得到：所建立的高斯分布模型gij的中心在nij＝(nd，nψ)，方差为ωij。

进一步地，平行签名通过以下方法得到：签名的角度轴槽的个数为nψ，对得到的签名沿着角度轴循环平移nψ个槽位，得到nψ个平行签名。

进一步地，两帧点云签名之间的距离通过以下方法得到：选出两帧签名signature1、signature2的峰值，其所在的角度槽值分别为nψ1、nψ2，两者之间差值为δψ＝nψ1-nψ2，选取一个合适的范围[δψ-δ,δψ+δ]，从signature2对应的nψ个平行签名中选取平移量在上述范围内的共计2δ个平行签名，并从中选取与signature1之间最小的欧式距离作为两帧签名之间的最终距离。

进一步地，相似两帧点云之间的相对位姿通过以下方法得到：

2.1)两帧签名对应的数据帧中的角点分别构成了一个无向图，无向图中的节点对应数据帧中的角点，无向图中两个节点之间的边为对应数据帧中两角点之间的欧式距离，得到的两个无向图分别为g1(v,e)和g2(v,e)。

2.2)对于g1(v,e)中每一条边edge1i，从g2(v,e)中选取与其长度最接近的边为edge2i，若两条边之间长度的差值在阈值以内，就认为两条边匹配上，反之，认为edge1i在g2(v,e)中不存在与其匹配的边。

2.3)对于匹配上的两条边edge1i和edge2i，分别对应着两个节点为和由此产生个配对，对这4个节点对分别投一票。

2.4)遍历完g1(v,e)中每一条边后，得到对所有点对的投票结果，对g1(v,e)中的每个节点，选取与其关联的配对得票数最高的节点作为g2(v,e)中与之配对的节点，若最高的得票数为0，则认为该节点在g2(v,e)中没有与之匹配的节点。

2.5)计算所有配对上的两个节点之间的平均相对位移作为两帧点云之间的相对位姿。

本发明的有益效果是：

(1)本发明设计了一种二维点云签名生成方法，生成的签名具有旋转不变性，可以根据两帧签名之间的距离来衡量两帧点云之间的相似性。

(2)给出了一种相似两帧点云之间的评价函数，可以精确地计算出两帧点云之间的相对位姿。

附图说明

图1为本发明基于二维激光扫描仪角点特征的闭环检测方法的步骤流程图；

图2为本发明二维点云签名生成示例；

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应该理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

相反，本发明涵盖任何由权利要求定义的在本发明的精髓和范围上做的替代、修改、等效方法以及方案。进一步，为了使公众对本发明有更好的了解，在下文对本发明的细节描述中，详尽描述了一些特定的细节部分。对本领域技术人员来说没有这些细节部分的描述可以完全理解本发明。

基于二维激光扫描仪角点特征的闭环检测方法，利用从二维激光扫描仪获得的二维点云数据中提取出的角点特征f，对二维点云数据c进行以下步骤处理：

步骤1：利用提出得到的二维点云角点特征f，生成二维点云c的签名，该签名具有旋转不变性的特征；根据两帧点云签名之间的欧氏距离来衡量两帧点云之间的相似性；

二维点云的签名通过以下方法得到：

1.1)对二维点云中任意两个特征点之间形成的线段的距离和角度进行离散化；

两个特征点之间形成的线段的距离和角度离散化通过以下方法得到：

对其中任意两个特征点fi和fj，其对应的坐标分别为(xi,yi)和(xj,yj)，则两个特征点之间形成的线段的距离dij和角度为：

对dij和进行等距离离散化，离散化后的对应一个大小为nd行，nψ列的二维直方图中的一个槽bin(nd，nψ)：

1.2)对离散化后的距离和角度进行高斯建模；所建立的高斯分布模型gij的中心在nij＝(nd，nψ)，方差为ωij。

1.3)综合所有特征点对相应的高斯模型，生成二维点云的归一化后的签名；

1.4)将得到的签名沿角度轴平移n个槽位，得到二维点云签名对应的n个平行签名；平行签名通过以下方法得到：签名的角度轴槽的个数为nψ，对得到的签名沿着角度轴循环平移nψ个槽位，得到nψ个平行签名。

1.5)对任意两帧点云的签名，其距离定义为一帧点云的签名与另一帧点云m个平行签名之间最小的欧式距离；

两帧点云签名之间的距离通过以下方法得到：选出两帧签名signature1、signature2的峰值，其所在的角度槽值分别为nψ1、nψ2，两者之间差值为δψ＝nψ1-nψ2，选取一个范围[δψ-δ,δψ+δ]，从signature2对应的nψ个平行签名中选取平移量在上述范围内的共计2δ个平行签名，并从中选取与signature1之间最小的欧式距离作为两帧签名之间的最终距离。

步骤2：利用点云角点特征之间的几何结构来计算相似两帧点云之间的相对位姿，并用两帧点云之间的相对位姿构建图模型，相似两帧点云之间的相对位姿通过以下方法得到：

2.1)两帧签名对应的数据帧中的角点分别构成了一个无向图，无向图中的节点对应数据帧中的角点，无向图中两个节点之间的边为对应数据帧中两角点之间的欧式距离，得到的两个无向图分别为g1(v,e)和g2(v,e)；

2.2)对于g1(v,e)中每一条边edge1i，从g2(v,e)中选取与其长度最接近的边为edge2i，若两条边之间长度的差值在阈值以内，就认为两条边匹配上，反之，认为edge1i在g2(v,e)中不存在与其匹配的边；

2.3)对于匹配上的两条边edge1i和edge2i，分别对应两个节点为和由此产生个配对，对这4个节点对分别投一票；

2.4)遍历完g1(v,e)中每一条边后，得到对所有点对的投票结果，对g1(v,e)中的每个节点，选取与其关联的配对得票数最高的节点作为g2(v,e)中与之配对的节点，若最高的得票数为0，则认为该节点在g2(v,e)中没有与之匹配的节点；

2.5)计算所有配对上的两个节点之间的平均相对位移作为两帧点云之间的相对位姿。

最后，对图模型进行优化后输出优化后的全局位姿。

实施例1

参考图1，所示为本发明实施例的针对二维激光扫描仪的角点特征提取方法的步骤流程图，其具体步骤如前所述，不再赘述，仅给出各步骤的参数。

通过分辨率为π/180的二维激光扫描仪获得二维点云数据，进行以下步骤处理：

1.利用二维激光扫描仪获得二维点云数据，提取点云中的角点，对于任意两个角点之间的线段，其端点坐标为(0,0)，(1,1)，线段的长度为1.414m，角度为π/4rad。

2.对线段长度以0.5m为间隔，角度以π/18rad为间隔进行离散化，离散化后线段得到的离散量为(1.414m，π/4rad)∈bin(3，5)。

3.以bin(3，5)为中心进行高斯建模，对应的高斯模型为n(nij-bin(3，5),ωij)。

4.对所有角点重复步骤1-3，累加得到的高斯模型，并对最终得到的签名进行归一化，签名最终的结果如图2。

5.计算相似两帧点云之间的相对位姿，并在图模型中对应的节点之间添加一条边。

6.对得到的图模型进行优化，输出优化后的位姿，完成闭环检测。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。