一种基于角点特征点选取与快速描述子的点云配准方法

摘要

本发明提供一种基于角点特征点选取与快速描述子的点云配准方法，涉及移动机器人、点云匹配和计算机视觉领域。该基于角点特征点选取与快速描述子的点云配准方法，包括以下步骤：步骤一：S1.特征提取：采取Harris3D进行角点检测，之后在进行改进ISS特征描述子计算；S2.匹配对扩展：确认S1中的描述子差异度，所述差异度包括距离相似度、角度相异度；步骤二：ICP精配准。将传统降采样替换为3D‑Harris角点检测，减少了特征描述的总次数，仅对特征显著区域进行特征描述，提升了点云匹配的效率和有效性，降低了配准时间，进一步，为大规模环境下地图创建工作提供方便。

说明书

技术领域

本发明涉及移动机器人、点云匹配和计算机视觉技术领域，具体为一种基于角点特征点选取与快速描述子的点云配准方法。

背景技术

随着计算机和传感器设备的迅速发展，移动机器人技术已经广泛地应用于人类生产和生活相关的各个领域。移动机器人在自主执行各种任务的过程中，需要获得可靠的位姿信息，而位姿信息的获取依赖于精确的环境地图。为此，除了安装各种执行机构外，移动机器人上还必须安装大量的例如激光雷达、声纳、红外相机和视觉相机等感知设备。这些安装了多种传感器的移动机器人可用于采集环境感知信息，然后借助于同步定位与地图创建(Simultaneous Localization and Mapping，SLAM)技术创建合适的环境地图并实现其自身的可靠定位，以保证移动机器人顺利执行各种任务，该项技术的核心算法即就为点云配准技术。目前，很多自主移动机器人均具备SLAM功能。这些自主移动机器人已在各个领域得到广泛应用，主要包括：无人驾驶汽车，农林业，服务业、军事和矿产资源开采等领域。下面将介绍SLAM技术在上述几个领域中的应用：

1)无人驾驶汽车

无人驾驶车是当今人工智能领域的热点问题，它在国防和智能辅助安全驾驶等领域有着广泛应用，研究具有人工智能的无人驾驶平台可减少战场上和交通事故中的人员伤亡。与普通车相比，无人驾驶车增加了诸如激光雷达，毫米波雷达，视觉相机和红外相机等传感器。它在行驶的过程中需要借助于这些传感器采集车体周围环境的感知信息，然后利用相关的SLAM算法和技术将感知信息转化成计算机能够理解的环境地图，并提供给规划模块进行运动规划，以获得合适的控制输入量驱动无人驾驶车向前行驶。现有无人驾驶车定位严重的依赖于GPS信号，在无GPS信号的环境下，SLAM技术的重要性将显得更加突出。

2)矿产资源开采

矿产资源是人类生产和生活的必需品，因此如何合理地开采和利用矿产资源是人类面临的一大挑战。在中国，澳大利亚和美国等资源大国，存在着数以万计的矿井，能否绘制精确的矿井地图，对于资源的合理开采和矿难救援均可起到关键的作用。而采用人工绘制矿井地图的方式即费时又无法保证精确性和可靠性，对于一些存在安全隐患的废弃矿井，人工根本无法绘制出可靠和精确的矿井地图。目前，国内矿难事故频发，救援人员在救援过程中急需精确的矿井地图。为此，可在矿车上安装计算平台、激光雷达传感器和视频采集设备，然后利用SLAM算法和技术自主地绘制出二维或三维矿井地图。与人工绘制方式相比，利用移动机器人绘制地图的方式更加可靠且精确，目前SLAM技术已成功应用于矿井地图的绘制工作中。但对于规模较大的矿井，其地图创建仍然是一个有待解决的问题。

3)农林业种植与防护

树木覆盖着全球大部分面积并且对二氧化碳下降、动物群落、水文湍流调节和巩固土壤起着重要作用，是构成地球生物圈当中的一个最重要方面。但木材又是工业、农业和建筑等行业的重要原材料，因此如何高效地种植树木和有效地利用树木也是一个重要的问题。为了保证树木的快速生长，需要合理控制树木的密度；为了有效地利用树木资源，需要了解树木的大小以及已成材树木的位置。虽然人工也能完成上述工作，但成本较高且精度较低，而利用安装了激光雷达和视觉传感器的移动机器人，通过设计合理的运动路径，并借助于SLAM算法和技术，即可快速自主地获得精确的林区树木分布地图。该地图中除了包含树木的位置信息外，还可包含树木的大小信息，这些结果可用于控制林区树木的种植密度，以便高效地种植树木和有效地利用木材。目前加拿大、芬兰等国家的研究者已将SLAM技术用于辅助工人进行林业种植与防护，此外SLAM技术已逐渐在农业种植中得到应用，但大多只能在平地上实现同步定位与地图创建。

4)家庭和社会服务

人口老龄化问题是全球面临的一个重要问题。为解决人口老龄化等所导致的家庭和社会服务问题，研发出具有高性价比的家庭和社会服务机器人已成为了许多研究机构和公司的关注点，这些服务机器人可替代人工执行自主吸尘，搬运物件和导航服务等功能。而执行上述任务一个最基本的前提是服务机器人能实现自身的精确定位和导航，一种简单可靠的方法是在服务机器人上安装一些传感器，并按一定的规则在室内运动并采集环境数据，然后利用SLAM算法和技术建立一幅完整的室内环境地图。在执行日常的服务时，可采用SLAM算法实现服务机器人的精确定位以及动态地修改环境地图。但对于包含较多运动物体的环境，家庭和社会服务移动机器人的SLAM算法需要进一步提高可靠性和稳定性。

5)特殊环境中的探索

地球上资源大部分为不可再生资源。随着人类文明的进步，人类可开采的资源已日益枯竭，这就促使人类探索特殊环境中的资源。在火山口、深海、北极、其它行星以及受污染的环境中可能蕴藏着各种丰富的资源，但受空气、压力和温度等因素的限制，人类目前还无法到达或长期驻扎在这些特殊环境中，因而限制了人类在这些环境中的活动。而移动机器人可以适应各种特殊的环境，因此研制具有勘探特殊环境功能的移动机器人是人工智能领域的一大热点。显然，在勘探的过程中需要对移动机器人进行可靠定位，而可靠的定位必须依赖于精确的环境地图，这些功能均可由安装了特殊传感器的移动机器人借助于SLAM技术实现。

总之，SLAM技术已经成为人工智能和移动机器人研究领域中及其重要且极富挑战性的课题。它不仅在陆地移动机器人上取得的广泛的应用，而且在水下移动机器人和空中移动机器人上也逐渐得到了应用。其目的是为了创建环境地图并实现移动机器人的精确定位，确保移动机器人可顺利地执行各项任务。虽然相关文献已给出了各种有效的解决方法，但大部分方法只适用于解决中小规模环境下的地图创建问题。而实际应用中，经常需要解决大规模环境下的地图创建问题。与中小规模环境下的地图创建问题，大规模环境下的地图创建问题更具复杂性。

发明内容

(一)解决的技术问题

针对现有技术的不足，本发明提供了一种基于角点特征点选取与快速描述子的点云配准方法，解决了利用SLAM技术对大规模环境下地图创建中复杂性高，创建难度高的问题。

(二)技术方案

为实现以上目的，本发明通过以下技术方案予以实现：一种基于角点特征点选取与快速描述子的点云配准方法，包括以下步骤：

步骤一：

S1.特征提取：采取Harris3D进行角点检测，之后在进行改进ISS特征描述子计算；

S2.匹配对扩展：确认S1中的描述子差异度，所述差异度包括距离相似度、角度相异度；

步骤二：ICP精配准。

优选的，所述步骤一中S1特征提取具体为：

一.通过点云边缘结构以及密度分布，寻找特征显著区域点作为特征兴趣，步骤如下：

A.通过体素降采样的方法对待匹配点云进行均匀化，使待匹配点云的重叠区域点密度相当；

B.在数据点云与模型点云上以Harris3D的方法进行局部区域自相关函数计算，以获得检测用滑动正方体在不同位置上的响应；

C.根据最大响应值的原则，均匀的选取特征点；

二.兴趣点特征描述与初匹配，步骤如下：

A.给定一个兴趣点f，在其周围定义个不同的支持半径

其中σ表示预定义描述半径，在每个支持半径的球体内，通过下式来计算出对应的特征矩阵C

其中P

B.通过奇异值分解，3×3的C

其中λ

进一步，通过不同支持半径下特征值的差值来放大局部特征，来生成特征值描述子D：

其中Δs

C.对两个待匹配点云的兴趣点，基于特征值描述子D，进行最近邻查找，形成多组初始匹配。

优选的，所述步骤一中S2的匹配对扩展具体为：

基于特征向量描述子夹角的匹配对应扩展，原点匹配为(f

A.以扩展点距离原始匹配点距离应当相近的原则进行兴趣点筛选：

||d

ε

B.为所有潜在扩展点计算与主匹配点的特征向量夹角关系：

其中点p代表初始匹配点，点i代表待扩展兴趣点，

再通过特征向量夹角结合特征值描述子，构成的约束条件来寻找扩展的特征匹配：

其中ε

C.若没有候选点满足B中所示的约束，那么匹配对扩展算法应转而对下一组原点匹配进行扩展，如果只有一个f

继而将正确的特征匹配添加到匹配集合；

D.对于在对应扩展操作之后可以获得组特征匹配集合m

优选的，所述步骤二中ICP精配准具体为：

通过裁剪ICP算法对最优运动进行进一步优化，所得最小化的目标函数为:

其中Q

(1).根据上一迭代即初始的变换{s

(2).优化当前的两图重叠比例参数及其对应的重叠点集:

(3).更新当前变换:

一旦相似变换在两个连续迭代之间没有明显变化，或者迭代次数超出了预设值，STrICP算法即就获得用于将栅格地图合并的最佳相似变换。

5.根据权利要求4所述的一种基于角点特征点选取与快速描述子的点云配准方法，其特征在于：所述ICP算法中在每次迭代中依次分别优化重叠百分比ξ和

(三)有益效果

本发明提供了一种基于角点特征点选取与快速描述子的点云配准方法。

具备以下有益效果：

1、将传统降采样替换为3D-Harris角点检测，减少了特征描述的总次数，仅对特征显著区域进行特征描述，提升了点云匹配的效率和有效性，降低了配准时间，进一步，为大规模环境下地图创建工作提供方便。

2、将角点匹配对进行对应扩展，增加了正确匹配的置信度，通过增加用以估计相对变换运动的匹配数量，提高了匹配精准度。

3、将裁剪迭代最近点算法应用于最优相对变换，基于数据点云与模型点云重叠的所有点进行变换优化，进一步提高了匹配的精确度。

附图说明

图1为本发明所提出的一种基于角点特征点选取与快速描述子的点云配准方法的整体流程示意图；

图2为本发明所提出的一种基于角点特征点选取与快速描述子的点云配准方法的匹配对扩展示意图；

图3为本发明所提出的一种基于角点特征点选取与快速描述子的点云配准方法的运行效果图；

图4为本发明所提出的一种基于角点特征点选取与快速描述子的点云配准方法的在多个数据集上与多种不同算法的运行结果旋转误差的对比图；

图5为本发明所提出的一种基于角点特征点选取与快速描述子的点云配准方法的在多个数据集上与多种不同算法的运行结果平移误差的对比图；

图6为本发明所提出的一种基于角点特征点选取与快速描述子的点云配准方法的在多个数据集上与多种不同算法的运行时间的对比图。

具体实施方式

下面将结合附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例一：

如图1-6所示，本发明实施例提供一种基于角点特征点选取与快速描述子的点云配准方法，包括以下步骤：

步骤一：

S1.特征提取：采取Harris3D进行角点检测，之后在进行改进ISS特征描述子计算；

S2.匹配对扩展：确认S1中的描述子差异度，其中差异度包括距离相似度、角度相异度；

步骤二：ICP精配准。

其中步骤一中S1特征提取具体为：

一.通过点云边缘结构以及密度分布，寻找特征显著区域点作为特征兴趣，步骤如下：

A.通过体素降采样的方法对待匹配点云进行均匀化，使待匹配点云的重叠区域点密度相当；

B.在数据点云与模型点云上以Harris3D的方法进行局部区域自相关函数计算，以获得检测用滑动正方体在不同位置上的响应；

C.根据最大响应值的原则，均匀的选取特征点；

二.兴趣点特征描述与初匹配，步骤如下：

A.给定一个兴趣点f，在其周围定义个不同的支持半径

其中σ表示预定义描述半径，在每个支持半径的球体内，通过下式来计算出对应的特征矩阵C

其中P

B.通过奇异值分解，3×3的C

其中λ

进一步，通过不同支持半径下特征值的差值来放大局部特征，来生成特征值描述子D：

其中Δs

C.对两个待匹配点云的兴趣点，基于特征值描述子D，进行最近邻查找，形成多组初始匹配。

其中步骤一中S2的匹配对扩展具体为：

基于特征向量描述子夹角的匹配对应扩展，原点匹配为(f

A.以扩展点距离原始匹配点距离应当相近的原则进行兴趣点筛选：

||d

ε

B.为所有潜在扩展点计算与主匹配点的特征向量夹角关系：

其中点p代表初始匹配点，点i代表待扩展兴趣点，

再通过特征向量夹角结合特征值描述子，构成的约束条件来寻找扩展的特征匹配：

其中ε

C.若没有候选点满足B中所示的约束，那么匹配对扩展算法应转而对下一组原点匹配进行扩展，如果只有一个f

继而将正确的特征匹配添加到匹配集合；

D.对于在对应扩展操作之后可以获得组特征匹配集合m

其中步骤二中ICP精配准具体为：

通过裁剪ICP算法对最优运动进行进一步优化，所得最小化的目标函数为:

其中Q

(1).根据上一迭代即初始的变换{s

(2).优化当前的两图重叠比例参数及其对应的重叠点集:

(3).更新当前变换:

一旦相似变换在两个连续迭代之间没有明显变化，或者迭代次数超出了预设值，STrICP算法即就获得用于将栅格地图合并的最佳相似变换。

其中ICP算法中在每次迭代中依次分别优化重叠百分比ξ和

实施例二：

一种基于角点特征点选取与快速描述子的点云配准方法，包括以下步骤：

1)通过点云边缘结构来寻找兴趣点：

角点作为一种几何结构特征，常常描述拐角，边界点等信息。通过将图像中的Harris角点检测思想应用到点云空间中，以点云密度替代图像灰度来寻找，先对点云空间进行三维体素栅格化，将落在每个体素栅格中点的数量近似看作二维图像的像素值，并在基础上，进行前、后、左、右、上、下六个平移方向的计算。如果滑动窗口在各个方向滑动时都发生了点云数量突变，则认为出现了角点，如果只有一个方向出现了突变，认为是遇到了直线线段。具体地，以P点为原点建立一个局部坐标系:Z方向是该点的法线方向，X、Y方向和Z垂直，在P上建立一个边长为k的小正方体检测窗口，若小正方体在点云平坦面上移动时，小正方体里的点云数量应该不变，若小正方体位于点云边缘上，则沿边缘滑动，点云数量几乎不变，沿垂直边缘方向滑动时，点云数量发生改变。若小正方体位于角点上，则沿各方向滑动时，都会大幅改变点云数量；

小正方体每滑动一次，便对其中包含的点云进行主成分分析(PCA)，并利用最小二乘法拟合出一个二次曲面，这里选择只有六项的二次曲面来表示抛物面，可以通过增加更多的项来适应更复杂的曲面。得到如下形式的抛物面：

计算z关于x和y的偏导数，来近似成图像灰度值I：

从而依据的梯度变化来找到点云中的特征角点；

基于梯度变化f

其中σ为常数，是高斯函数的支持度，

2)兴趣点特征描述与初匹配：

对兴趣角点p

cov(p

对于所有支持点p

兴趣点特征描述与初匹配中，特征值描述子为不同支持半径下兴趣点的特征值差值进行级联而成，其生成公式为：

其中Δs

||d

其中ε

3)基于特征向量描述子夹角的匹配对应扩展：

根据兴趣点的特征向量描述信息为初始匹配组合建立匹配扩展，利用待扩展兴趣点特征向量与扩展兴趣点特征向量的夹角来形成相对于两点的特征向量描述子旋转不变性，利用该性质，将已有的多组初始匹配扩展为多组特征匹配集合，以增强特征点匹配间的鲁棒性，然后通过特征点重投影误差来快速的寻找最优匹配集合，然后将其作为刚体运动变换的估计样本，求解点云间相对变换关系，再通过裁剪迭代最近点算法对相对运动进行优化。

基于特征向量描述子夹角的匹配对应扩展中，用于进行匹配对扩展的特征向量夹角的定义为：

其中点p代表初始匹配点，点i代表待扩展兴趣点，表示兴趣点p在支持半径l下所计算得到的特征向量，因此，可以通过以下约束条件来验证潜在的特征匹配；

其中是预设参数，通过消融实验可确定其值，将特征值描述子同时加入考虑因素可以显著降低匹配扩展中错误匹配的匹配数量；

裁剪迭代最近点优化的最小化目标为：

其中为模型点云中与数据点云重叠的部分，为相对变换参数，为优化上述目标函数，采用裁剪迭代最近点算法，其每次迭代都涉及三个步骤：

(1).根据上一迭代即初始的变换{s

(2).优化当前的两图重叠比例参数及其对应的重叠点集:

(3).更新当前变换:

一旦相似变换在两个连续迭代之间没有明显变化，或者迭代次数超出了预设值，STrICP算法即就获得用于将栅格地图合并的最佳相似变换。

其中ICP算法中在每次迭代中依次分别优化重叠百分比ξ和

实施例三：

一种基于角点特征点选取与快速描述子的点云配准方法，包括以下步骤：

步骤1)通过点云边缘结构来寻找兴趣点

角点作为一种几何结构特征，常常描述拐角，边界点等信息。通过将图像中的Harris角点检测思想应用到点云空间中，以点云密度替代图像灰度来寻找，先对点云空间进行三维体素栅格化，将落在每个体素栅格中点的数量近似看作二维图像的像素值，并在基础上，进行前、后、左、右、上、下六个平移方向的计算。如果滑动窗口在各个方向滑动时都发生了点云数量突变，则认为出现了角点，如果只有一个方向出现了突变，认为是遇到了直线线段。具体地，以P点为原点建立一个局部坐标系:Z方向是该点的法线方向，X、Y方向和Z垂直，在P上建立一个边长为k的小正方体检测窗口。若小正方体在点云平坦面上移动时，小正方体里的点云数量应该不变。若小正方体位于点云边缘上，则沿边缘滑动，点云数量几乎不变，沿垂直边缘方向滑动时，点云数量发生改变。若小正方体位于角点上，则沿各方向滑动时，都会大幅改变点云数量。

小正方体每滑动一次，便对其中包含的点云进行主成分分析(PCA)，并利用最小二乘法拟合出一个二次曲面，这里选择只有六项的二次曲面来表示抛物面。可以通过增加更多的项来适应更复杂的曲面。计算z关于x和y的偏导数，来近似成图像灰度值I。

为了解决Harris最初提出的算法中，导数是离散函数的问题，本技术方案使用连续高斯函数对导数进行积分。具体为利用曲面积分来计算梯度协方差矩阵中的各个元素。

可以得到3D-Harris的自相关函数，并依据于此计算3D-Harris响应值。而后，基于图像Harris角点检测响应值筛选的思想，完成3D结构角点的检测；

步骤2)兴趣点特征描述与初匹配

对兴趣角点p

对于所有支持点p

通常情况下，支持区域位于平坦的区域内，n

设s

关于特征向量描述子的生成，首先需标准化特征向量，若n

N＝(n

于是便有角点的几何结构描述(N,D)。接下来首先依靠描述子中的特征值进行特征匹配。通过在兴趣点的特征值描述子D上应用最近邻查找法，构建初始匹配对。

步骤3)基于特征向量描述子夹角的匹配对应扩展：

因为直接以低正确率的原始匹配集来恢复刚体变换

对已有的每一组初始匹配(f

接下来在完成扩展后的诸多特征匹配集合中，依据特征点重投影误差最小原则选出一组最优特征匹配，将其通过裁剪迭代最近点算法进行优化，以得到精确的点云间相对变换。

步骤1)所述的移动方块内点云抛物面公式为：

其中(x,y,z)分别为拟合曲面的坐标，(P

步骤1)所述的对曲面求导后以近似图像I梯度的公式为：

其中(f

步骤1)使用连续高斯函数对导数进行积分。具体利用曲面积分来计算梯度协方差矩阵中的各个元素的公式为：

其中σ为常数，是高斯函数的支持度，

C＝p

步骤1)中所述3D-Harris的自相关函数为:

其中(A,B,C)为梯度协方差矩阵中的各个元素的微积分简化表达；

步骤1)中所述3D-Harris响应值为:

R

其中det(E)表示E的行列式，trace(E)表示E的迹，R

步骤2)中所述的所有支持点p

cov(p

其中l表示支持半径，w表示权重，该式所表达协方差矩阵即为点p

步骤3)中所述的具有旋转不变性的特征向量间夹角θ描述法所采用公式为:

其中

步骤3)中所述匹配对对应扩展中所依据的约束条件公式为:

其中ε

步骤3)中所述裁剪迭代最近点优化中所采用目标函数公式为:

其中ξ表示数据点云P与模型点云Q重叠区域占Q的百分比，Q

实验对比：

本次实验所采用的对比算法均来自领域经典且具有代表性的算法。所有比较的方法都是基于PCL库实现算法编程。实验是在一台内存为16GB的四核2.6GHz计算机上进行。本次实验每种对比算法在每个数据集上均运行五十次，记录并计算五十次结果的平均值。图3为本次实验得一次运行结果。

本次实验共采用四种本领域常用的评价指标来对本文算法进行分析，分别为:匹配适应度得分(Fitness Score)，均方根误差(RMSE)，旋转误差，平移误差，运行时间。这五种评价指标分别从点云配准精度和配准性能两个方面对算法进行评价，数值越小表示效果越好。

匹配适应度得分(Fitness Score)，采用配准后点云对应的最近点距离的平方和作为配准效果的评定，评定指标Fitness Score利用如下公式计算:

均方根误差(RMSE)；

其中:N为对应点对的数量，d为配准后对应点之间的欧式距离，d

旋转、平移误差：

式中:e

本文提出的方法与五种经典算法进行对比试验，在四种数据集上运行的实验结果如下：

Bunny数据集对比实验结果：

Happy Buddha数据集对比实验结果：

Dragon数据集对比实验结果：

Armadillo数据集对比实验结果：

实验结果表明该方法提高了点云配准效率和精度。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。