CN113902782A - 一种挖掘机周边障碍物三维点云的快速配准方法及系统 - Google Patents

Abstract

本发明公开了一种挖掘机周边障碍物三维点云的快速配准方法及系统，包括获取挖掘机周边障碍物三维点云并进行预处理；提取源点云和目标点云的特征点进行匹配；基于迭代最近点ICP算法，引入质心高权重，利用奇异向量分解SVD算法求解旋转矩阵和平移矩阵，完成配准。通过点云预处理进行下采样和点云精简，然后对源点云和目标点云的关键点特征进行匹配，为ICP精确配准提供初始变换矩阵，使用Kd‑Tree快速检测匹配关系。通过引入质心高权重ω_i完成精确配准，这种方法可以提高ICP算法的计算速度，在室外挖掘机工作场景下具有更好的精确度。

Description

一种挖掘机周边障碍物三维点云的快速配准方法及系统

技术领域

本发明涉及3D扫描技术领域，特别是涉及一种挖掘机周边障碍物三维点云的快速配准方法及系统。

背景技术

本部分的陈述仅仅是提到了与本发明相关的背景技术，并不必然构成现有技术。

自主挖掘机器人的研究目的是取代传统的挖掘机，能够在恶劣环境中自主作业。自主挖掘机器人离不开传感器对环境的感知，需通过传感器来获取环境信息，从而完成自身的定位以及对环境障碍物的检测，为导航提供依据。环境感知一直是挖掘机器人关键技术之一，自主机器人能够安全可靠的作业离不开精准的检测和定位系统，也是路径规划的基础。在复杂的野外环境下，障碍物的尺寸大小不一，形状各异，既有凸起障碍物也有凹进障碍物，还有一些在杂草丛中，难以分辨，这些给障碍物检测和分割技术带来了很大的挑战。因此，环境感知系统通常配备多类传感器，最常用的是激光雷达和CCD传感器。

挖掘机是露天开采的重要装备，但由于开采现场环境恶劣，人员健康与安全无法保障，熟练的设备操作人员难以培养，生产效率难以最大化，所以无人挖掘机的研究越来越引起重视。而对于周围环境的三维感知是实现无人挖掘机的关键技术，针对挖掘机环境感知的相关研究对于实现无人挖掘机具有重要意义。

三维激光点云数据处理是三维激光点云三维场景重建过程中非常重要的环节，三维激光点云数据处理包含三维激光点云的数据去噪、精简、配准等一系列步骤，三维激光点云数据处理的结果直接影响了三维激光点云模型重建的精度和质量。三维激光点云配准方法按照特征匹配搜索空间的不同分为全局配准方法和局部配准方法两大类。全局三维激光点云配准方法是指在整个三维激光点云的区域中搜索特征匹配点，局部三维激光点云配准则是在三维激光点云的某个特定区域中搜索特征匹配点。三维激光点云的特征配准采用三维激光点云的数据角点、边缘等。

三维点云配准的实质是计算同一物体或场景不同视点下采集到的点云之间的变换关系，以将其统一到同一坐标系下得到完整的点云。由于数据采集传感器的视角有限，单次测量只能得到目标场景的部分数据，点云配准能够将多次测量的数据拼接起来以获得完备的点云，为后续三维建模、目标识别、场景理解奠定数据基础，因而点云配准方法一直以来都是三维计算机视觉领域的热点问题。尽管近年来关于三维点云配准的研究取得了重大进展，但它仍然是研究人员面临的一大难题。

发明内容

为了解决现有技术的不足，本发明提供了一种挖掘机周边障碍物三维点云的快速配准方法及系统，提高ICP算法的计算速度，加快挖掘机周边障碍物三维点云的快速配准，增加系统的实时性和可靠性。

本发明的第一个方面提供一种挖掘机周边障碍物三维点云的快速配准方法，包括：

获取挖掘机周边障碍物三维点云并进行预处理；

提取源点云和目标点云的特征点进行匹配；

基于迭代最近点ICP算法，引入质心高权重，利用奇异向量分解SVD算法求解旋转矩阵和平移矩阵，完成配准。

本发明的第二个方面提供一种挖掘机周边障碍物三维点云的快速配准系统，包括：

三维点云获取模块，被配置为获取挖掘机周边障碍物三维点云并进行预处理；

特征点匹配模块，被配置为提取源点云和目标点云的特征点进行匹配；

配准模块，被配置为基于迭代最近点ICP算法，引入质心高权重，利用奇异向量分解SVD算法求解旋转矩阵和平移矩阵，完成配准。

本发明的第三个方面提供一种计算机可读存储介质。

一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如上述第一个方面所述的一种挖掘机周边障碍物三维点云的快速配准方法中的步骤。

本发明的第四个方面提供一种计算机设备。

一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如上述第一个方面所述的一种挖掘机周边障碍物三维点云的快速配准方法中的步骤。

与现有技术相比，本发明的有益效果是：

本发明公开采用基于ICP配准算法改进的质心高权重配准算法。由于其在配准过程中增加了对空间质心的配准权重，在室外场景下具有更好的精度，所以对于挖掘机环境感知具有更好的意义。

本发明公开采用体素网格进行下采样，基于法向量对点云进行精简，然后利用OpenMP提高FPFH的计算速度，可以很大程度提高点云的配准速度。通过利用双向近邻点方法，获得具有一定准确性的候选匹配点对，利用分裂法将错误的匹配点对全部剔除，提高配准的精度。

本发明附加方面的优点将在下面的描述中部分给出，或通过本发明的实践了解到。

附图说明

构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

图1为本发明实施例一中挖掘机周边障碍物三维点云的快速配准方法流程图；

图2为本发明实施例一中挖掘机周边障碍物三维点云的快速配准方法中点云配准的关键步骤示意图；

图3为本发明实施例一中挖掘机周边障碍物三维点云的快速配准方法中点云配准的流程图；

图4为本发明的点云配准实验分析对比图。

具体实施方式

应该指出，以下详细说明都是示例性的，旨在对本发明提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本发明的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

在本发明本实施例中，“和/或”仅仅是一种描述关联对象的关联关系，表示可以存在三种关系。例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。另外，在本发明的描述中，“多个”是指两个或多于两个。

另外，为了便于清楚描述本发明实施例的技术方案，在本发明实施例中，采用了“第一”、“第二”等字样对功能和作用基本相同的相同项或相似项进行区分。本领域技术人员可以理解“第一”、“第二”字样并不对数量和执行次序进行限定，并且“第一”、“第二”等字样也并不限定一定不同。

在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

本实施例所有数据的获取都在符合法律法规和用户同意的基础上，对数据的合法应用。

实施例一

如图1-3所示，本实施例提供了一种挖掘机周边障碍物三维点云的快速配准方法，本实施例以该方法应用于服务器进行举例说明，可以理解的是，该方法也可以应用于终端，还可以应用于包括终端和服务器和系统，并通过终端和服务器的交互实现。服务器可以是独立的物理服务器，也可以是多个物理服务器构成的服务器集群或者分布式系统，还可以是提供云服务、云数据库、云计算、云函数、云存储、网络服务器、云通信、中间件服务、域名服务、安全服务CDN、以及大数据和人工智能平台等基础云计算服务的云服务器。终端可以是智能手机、平板电脑、笔记本电脑、台式计算机、智能音箱、智能手表等，但并不局限于此。终端以及服务器可以通过有线或无线通信方式进行直接或间接地连接，本申请在此不做限制。本实施例中，该方法包括以下步骤：

步骤S1：获取挖掘机周边障碍物三维点云并进行预处理；

步骤S2：提取源点云和目标点云的特征点进行匹配。

步骤S3：基于迭代最近点ICP算法，引入质心高权重，利用奇异向量分解SVD算法求解旋转矩阵和平移矩阵，完成配准。

其中，步骤S1：具体包括：

步骤S1.1：利用多线激光雷达获取挖掘周边场景点云数据，利用半径滤波器快速滤波；通过多线激光雷达获取挖掘周边场景点云数据，获取目标点云Q{q₁,q₂,......,q_n}和源点云W{w₁,w₂,......,w_n}，有些点是游离于点云主体外的噪声点，经过对点邻域的统计，利用半径滤波器滤波速度较快的特点进行快速滤波。

步骤S1.2：利用体素网格法对点云下采样，基于法向量对点云进行精简。利用体素网格法对点云下采样，有效地降低算法的计算量。同时利用基于法向量的点云精简算法，减少点数量的同时，保证点云的形状特征。

其中，步骤S2：提取源点云和目标点云的特征点进行匹配，具体包括：

步骤S2.1：利用快速点特征直方图FPFH(Fast Point Feature Histograms)，为查询点w求得它和其k邻域内每个点之间的三个特征元素值，然后统计成一个简化点特征直方图SPFH(Simplified Point Feature Histogram)；

步骤S2.2：分别对k邻域中的每个点确定k领域，按第一步分别形成自己的SPFH，对邻域中的各个SPFH进行加权统计：

其中θ_k表示查询点w与给定量空间中的近邻点w_k之间的距离，FPFH(w)表示查询点w的最终直方图，SPFH(w)表示查询点w的简化点特征直方图，SPFH(w_k)表示查询点w的k近邻点的简化点特征直方图。

步骤S2.3：通过匹配点对间的FPFH特征确定与点q_i唯一对应的点w_i，确定对应点云集；

步骤S2.4：利用OpenMP提高FPFH的计算速度；

OpenMP作为共享存储标准而问世，为在多处理机上编写并行程序而设计的一个应用编程接口。它包括一套编译指导语句和一个用来支持它的函数库。

PCL中的类pcl::FPFHEstimationOMP，它的应用程序接口(API)100％兼容单线程pcl::FPFHEstimation。在8核系统中，OpenMP的实现可以比完全同样单核系统上的计算提高6-8倍速度。为了提高计算速度，使用多核/多线程规范，利用OpenMP开发模式来提高计算速度。

步骤S2.5：剔除错误匹配点对，利用双向近邻点方法，获得具有一定准确性的候选匹配点对。然后利用分裂法，以标准差和距离为评价标准，将错误的匹配点对全部剔除，最终获得用于ICP计算的初始目标点云集M和源点云集P；

根据步骤S2.1和步骤S2.2方法求得匹配特征点会存在一些错误匹配的点对，通过利用双向近邻点方法，获得具有一定准确性的候选匹配点对。然后利用分裂法，以标准差和距离为评价标准，将错误的匹配对全部剔除。获得最终用于ICP计算的初始目标点云集M{m₁,m₁,......,m_n}和源点云集P{p₁,p₂,......,p_n}。

FPFH，通过对点云预处理后数据进行k邻域各个SPFH进行加权。OpenMP作为共享存储标准而问世，为在多处理机上编写并行程序而设计的一个应用编程接口。它包括一套编译指导语句和一个用来支持它的函数库。因此利用OpenMP提高FPFH的计算速度，快速检测对应点对的匹配关系。

其中，步骤S3：基于ICP算法，引入质心高权重g_i，利用SVD算法求解旋转矩阵和平移矩阵，完成配准，具体包括：

步骤S3.1：将源点云集P和初始目标点云集M中的点对应关系用以下公式描述为：

m_i＝Rp_i+t*+E_i,i＝1,2,...,n(n>3) (2)

R表示3*3的旋转矩阵；t*表示平移矩阵；E_i表示对应第i个点的误差；n表示控制点对数目，m_i表示初始目标点云集M中的第i个点，p_i表示与点m_i对应源点云集P中的第i个点。

步骤S3.2：通过缩小源点云与目标点云中对应点之间的欧氏距离，建立误差函数

使误差函数最小，即minΔE(R,t*)。

步骤S3.3：求其关于t*的导数，令导数为零，关于R求最小值，得到下式：

步骤3.4：经过计算和化简后得到两帧点云质心，分别为：

其中，

表示初始目标点云集M加权平均的质心；

表示源点云集P加权平均的质心；

步骤S3.5：构建协方差矩阵

其中，U表示3*3的矩阵；对协方差矩阵U做奇异值分解：

U＝HΛJ^T,(Λ＝diag(d_i),d₁>＝d₂>＝d₃>＝0)

其中H，J均为3*3正交矩阵，Λ为协方差矩阵U的特征值组成的对角矩阵。根据H、J^T得到旋转矩阵R＝JH^T，平移变换

步骤S3.6：对初始目标点云集合P进行坐标变换，计算变换矩阵H，即H₁＝RH+t*；

步骤S3.7：计算H₁和P的均方差误差，如果小于预设阈值，结束迭代。否则，查看迭代次数是否到达到阈值，如果大于阈值，失败退出，否则，返回步骤S3.1，点云集H₁代替H，重复上述步骤。

所述的点云预处理，首先利用半径滤波器滤波速度较快的特点进行滤波，然后利用体素网格法对点云下采样，利用基于法向量的点云精简算法进行点云精简，有效的降低算法的计算量。

所述FPFH，通过对点云预处理后数据进行k邻域各个SPFH进行加权，利用OpenMP提高FPFH的计算速度，快速检测对应点对的匹配关系。

通过这种运算方法能够极大的减少关于旋转矩阵表达式和平移向量的计算量，查阅图4不同ICP算法实验对比分析图，通过配准时间和配准精度可以看出，本发明的方法可以提高ICP算法的计算速度，保持很好的精度，增加系统的实时性。

实施例二

本实施例提供了一种挖掘机周边障碍物三维点云的快速配准系统，包括：

三维点云获取模块，被配置为获取挖掘机周边障碍物三维点云并进行预处理；

特征点匹配模块，被配置为提取源点云和目标点云的特征点进行匹配；

配准模块，被配置为基于迭代最近点ICP算法，引入质心高权重，利用奇异向量分解SVD算法求解旋转矩阵和平移矩阵，完成配准。

此处需要说明的是，上述三维点云获取模块、特征点匹配模块和配准模块对应于实施例一中的步骤S1至S3，上述模块与对应的步骤所实现的示例和应用场景相同，但不限于上述实施例一所公开的内容。需要说明的是，上述模块作为系统的一部分可以在诸如一组计算机可执行指令的计算机系统中执行。

上述实施例中对各个实施例的描述各有侧重，某个实施例中没有详述的部分可以参见其他实施例的相关描述。

所提出的系统，可以通过其他的方式实现。例如以上所描述的系统实施例仅仅是示意性的，例如上述模块的划分，仅仅为一种逻辑功能划分，实际实现时，可以有另外的划分方式，例如多个模块可以结合或者可以集成到另外一个系统，或一些特征可以忽略，或不执行。

实施例三

本实施例提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如上述实施例一所述的一种挖掘机周边障碍物三维点云的快速配准方法中的步骤。

实施例四

本实施例提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如上述实施例一所述的一种挖掘机周边障碍物三维点云的快速配准方法中的步骤。

应理解，本实施例中，处理器可以是中央处理单元CPU，处理器还可以是其他通用处理器、数字信号处理器DSP、专用集成电路ASIC，现成可编程门阵列FPGA或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

存储器可以包括只读存储器和随机存取存储器，并向处理器提供指令和数据、存储器的一部分还可以包括非易失性随机存储器。例如，存储器还可以存储设备类型的信息。

在实现过程中，上述方法的各步骤可以通过处理器中的硬件的集成逻辑电路或者软件形式的指令完成。

实施例一中的方法可以直接体现为硬件处理器执行完成，或者用处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器、闪存、只读存储器、可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器，处理器读取存储器中的信息，结合其硬件完成上述方法的步骤。为避免重复，这里不再详细描述。

本领域普通技术人员可以意识到，结合本实施例描述的各示例的单元及算法步骤，能够以电子硬件或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种挖掘机周边障碍物三维点云的快速配准方法，其特征在于，包括：

获取挖掘机周边障碍物三维点云并进行预处理；

提取源点云和目标点云的特征点进行匹配；

基于迭代最近点ICP算法，引入质心高权重，利用奇异向量分解SVD算法求解旋转矩阵和平移矩阵，完成配准。

2.如权利要求1所述的一种挖掘机周边障碍物三维点云的快速配准方法，其特征在于，所述获取挖掘机周边障碍物三维点云并进行预处理，具体为：

利用多线激光雷达获取挖掘机周边场景点云数据；

利用半径滤波器对周边场景点云数据进行快速滤波；

利用体素网格法对滤波后的周边场景点云数据下采样；

基于法向量对采样后周边场景点云数据进行精简。

3.如权利要求1所述的一种挖掘机周边障碍物三维点云的快速配准方法，其特征在于，所述提取源点云和目标点云的特征点进行匹配，包括：

(1)利用快速点特征直方图FPFH，为查询点求得它和其k邻域内每个点之间的三个特征元素值统计成一个简化点特征直方图SPFH；

(2)分别对k邻域中的每个点确定k领域，按(1)中的方式分别形成自己的SPFH，对邻域中的各个SPFH进行加权统计：

(3)通过匹配点对间的FPFH特征确定与点q_i唯一对应的点w_i，确定对应点云集；

(4)利用OpenMP提高FPFH的计算速度；

(5)剔除错误匹配点对，利用双向近邻点方法，获得候选匹配点对；

(6)利用分裂法，以标准差和距离为评价标准，将候选匹配点对中的错误的匹配点对全部剔除，最终获得用于ICP计算的初始目标点云集和源点云集。

4.如权利要求3所述的一种挖掘机周边障碍物三维点云的快速配准方法，其特征在于，所述对领域中的各个SPFH进行加权统计为

其中θ_k表示查询点w与给定量空间中的近邻点w_k之间的距离，FPFH(w)表示查询点w的最终直方图，SPFH(w)表示查询点w的简化点特征直方图，SPFH(w_k)表示查询点w的k近邻点的简化点特征直方图。

5.如权利要求1所述的一种挖掘机周边障碍物三维点云的快速配准方法，其特征在于，所述基于迭代最近点ICP算法，引入质心高权重，利用奇异向量分解SVD算法求解旋转矩阵和平移矩阵，完成配准，具体为：

构建将源点云集P和初始目标点云集M中的点对应关系，用以下公式描述为：m_i＝Rp_i+t*+E_i,i＝1,2,...,n(n>3)；

通过缩小源点云与目标点云中对应点之间的欧氏距离，建立误差函数

使误差函数最小，即minΔE(R,t*)；

求其关于t*的导数，令导数为零，经过计算和化简后得到两帧点云质心，分别为

和

构建协方差矩阵

对协方差矩阵U做奇异值分解，U＝HΛJ^T,(Λ＝diag(d_i),d₁>＝d₂>＝d₃>＝0)，根据H，J得到旋转矩阵R＝JH^T，平移变换矩阵

通过预设阈值，进行迭代，完成点云精确配准。

6.如权利要求5所述的一种挖掘机周边障碍物三维点云的快速配准方法，其特征在于，所述求其关于t*的导数，令导数为零，经过计算和化简后得到两帧点云质心，分别为

和

具体为：

求其关于t*的导数，令导数为零，关于R求最小值，得到下式：

式中，R表示3*3的旋转矩阵；t*表示平移矩阵；E_i表示对应误差；n表示控制点对数目，m_i表示初始目标点云集M中点，p_i表示与点m_i对应源点云集P中的点；

经过计算和化简后得到两帧点云质心，分别为：

7.如权利要求5所述的一种挖掘机周边障碍物三维点云的快速配准方法，其特征在于，所述对协方差矩阵U做奇异值分解，包括：

U＝HΛJ^T,(Λ＝diag(d_i),d₁>＝d₂>＝d₃>＝0)

其中H，J均为3*3正交矩阵，Λ为协方差矩阵U的特征值组成的对角矩阵。根据H、J^T得到旋转矩阵R＝JH^T，平移变换

8.一种挖掘机周边障碍物三维点云的快速配准系统，其特征在于，包括：

三维点云获取模块，被配置为获取挖掘机周边障碍物三维点云并进行预处理；

特征点匹配模块，被配置为提取源点云和目标点云的特征点进行匹配；

配准模块，被配置为基于迭代最近点ICP算法，引入质心高权重，利用奇异向量分解SVD算法求解旋转矩阵和平移矩阵，完成配准。

9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-7中任一项所述的一种挖掘机周边障碍物三维点云的快速配准方法中的步骤。

10.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1-7中任一项所述的一种挖掘机周边障碍物三维点云的快速配准方法中的步骤。

Images