CN114543666A - 一种基于矿场环境感知的堆料面预测方法 - Google Patents

Abstract

一种基于矿场环境感知的堆料面预测方法。首先进行矿场环境感知，将堆料与墙壁进行分类，并在此基础上完成堆料面预测。所述矿场环境感知，包括点云预处理、配准、融合、水平校准、地面与堆料上方无用点去除、聚类、分类及提取，其中环境点云数据是通过3D激光雷达进行采集，所述的环境感知方法包括点云预处理、配准、融合、水平校准、地面等无用点去除、聚类、分类及提取。所述堆料面预测模型是基于Kd树与K最邻近搜索进行建立。本发明实现了点云配准与分割方法的有效融合，能对矿场环境中的堆料点进行有效提取及利用，基于真实堆料点云建立的堆料面预测模型可用于三维挖掘体积计算，可实现计算结果的准确性。

Description

一种基于矿场环境感知的堆料面预测方法

技术领域

本发明属于智能挖掘机研究领域的环境感知技术领域，提出了一种基于矿场感知的堆料面预测方法。

背景技术

矿用挖掘机是矿山开采系统中的核心设备，传统挖掘机需手动操作进行挖掘，但由于设备尺寸巨大，操作人员的视线可能会被挖掘机的铲斗、动臂等组件遮挡，或露天环境常伴有恶劣天气，也会使视线受阻，无法了解到矿场及堆料的实时情况，在挖掘过程中可能造成挖掘碰撞，动臂倾覆等问题，极易产生意外事故。

另外，智能挖掘机在挖掘轨迹优化中需计算挖掘体积，来验证算法的有效性，目前，多数挖掘体积的计算还停留在二维层面，无法实现基于真实堆料点云的挖掘体积计算，这对智能挖掘机的后续研究造成了很大的影响。

发明内容

为了解决现有技术存在的上述问题，本发明要设计一种基于矿场环境的点云处理方法，使点云配准与点云分割方法有效融合，将堆料点进行提取与有效利用。并且设计了一种基于KD树与K最邻近搜索的堆料面预测模型，可实现三维下挖掘体积的计算。

为了实现上述目的，本发明的技术方案如下：

一种基于矿场环境感知的堆料面预测方法，首先进行矿场环境感知，将堆料与墙壁进行分类，并在此基础上完成堆料面预测。包括以下步骤：

第一步，进行矿场环境感知，包括点云预处理、配准、融合、水平校准、地面与堆料上方无用点去除、聚类、分类及提取。具体包括以下步骤：

(1)首先，通过电铲实验平台上安装的两个不同位置的激光雷达扫描采集获取两幅矿场环境点云原始数据，命名为源点云与目标点云，对两幅原始点云进行数据预处理。

所述的预处理包括点云NAN点(非数据点)的去除与降采样，降采样的目的是为了降低点云数据量，以此提升计算效率。体素栅格降采样方法是通过构建三维体素栅格，计算每一个三维栅格内全部点的重心，近似以一个点P_c代替栅格内的其余点：

其中，n为每个三维栅格内的数据点个数，x_i、y_i、z_i分别为栅格内各点的坐标值。

(2)点云配准，由初始配准与精配准两个环节组成。

2.1)初始配准方法选用SAC-IA采样一致性配准算法，可根据给定点对间距离阈值选取有限个对应点对进行计算。初始配准需首先计算源点云与目标点云的法向量及表面曲率。搜索点云中各点的K个邻域点，基于最小二乘的局部表面拟合方法拟合平面S，计算公式如下：

式中，

为平面S的法线，d为平面S到坐标原点的距离，s_i为各点重心值，K为邻域点个数，argmin函数表示在目标函数最小时，获取该函数中自变量的值。

以拟合平面S的切平面法线近似作为各点的法向量。各点的K个邻域点的重心在拟合平面S上，同时法向量满足范数等于1，故将求解法向量的问题转换为求解协方差矩阵P，对协方差矩阵P进行特征值分解，最小的特征值所对应的特征向量即为平面S的法向量，计算公式如下：

式中，s₀为平面S的重心，K为邻域点个数。

基于上述计算得到的两幅点云的法向量来计算FPFH特征，FPFH(快速点特征直方图)是一种基于点与其邻域点之间法向量夹角、点间连线夹角关系的特征，是一种由PFH改进的特征描述子，在保留了点云主要几何属性的基础上，降低了计算复杂度，初始配准将基于FPFH特征进行初始刚性变换矩阵计算，其目的是为精配准提供一个初始迭代值。FPFH特征计算公式如下：

式中，p_q是查询点，p_k是查询点p_q的各邻域点，W_K为权重，以p_q与各邻域点之间的距离来表示，K为邻域点个数，SPFH为查询点与邻域点间的角度特征。

2.2)精配准选用ICP迭代最近点算法，该算法分别在两幅点云中根据点对间距离阈值，最大迭代次数、前后两次刚性变换矩阵最小容差重复寻找对应点对，直至找到最邻近点(p_i，q_i)，计算出最优的刚性变换矩阵，使得误差函数最小。误差函数公式如下:

式中，n为最邻近点对的数量，p_i为目标点云中的点，q_i为源点云中与p_i对应的最邻近点，R为旋转矩阵，t为平移向量。

(3)点云融合仍使用体素栅格法进行配准融合后点云数据量的缩减，可提升程序运行速度，满足实际要求。

(4)因为3D激光雷达进行扫描时，地面的凹凸不平会使扫描的数据相对于地面带有一个侧倾角与俯仰角误差，所以在点云分割前首先需要进行水平校准步骤，两个角度的测量由WK-55电铲实验样机机身的倾角传感器进行读取，水平校准公式如下所示：

式中，x_s、y_s、z_s为带有角度误差的原始点云数据，α是沿x轴顺时针旋转的侧倾角误差角度，β是沿y轴顺时针旋转的俯仰角误差角度，x、y、z是修正后的点云数据。

(5)因为消除了水平面角度误差，所以使用基于传统的几何特征-高度对地面及堆料上方的无用点进行去除，基于激光雷达安装在实验样机的位置高度，设置最小及最大高度阈值，小于最小高度阈值判断为地面点进行去除，大于最大高度阈值判断为堆料上方无用点进行去除，在两个阈值范围内的点则进行数据存储及保留。

(6)点云聚类选用区域生长算法，根据点云的基本特征将具有类似特征的点进行合并，并将各聚类点集区域进行分离，且确保每一区域带有较好的边缘信息。该算法的基本原理是通过选取初始种子点，在该点邻域内搜索与其有类似特征的点，将满足曲率与法线夹角阈值的点合并到种子点中继续向外搜索，直至没有点可进行合并，算法停止。表面曲率计算公式可表示为：

式中，λ₀≤λ₁≤λ₂为协方差矩阵P各特征值，表面曲率δ越小，表示该点近邻域起伏变化越小，反之亦然。

(7)设置点云数据量阈值与法向量阈值进行类别判断，将堆料与墙壁进行分类。因矿场环境中经过地面及堆料上方无用点去除后，所保留数据量较大的聚类点集仅为墙壁与堆料，故将存储的各聚类点云依次进行点云数据量阈值判断。因激光雷达安装及每次扫描位置固定，矿场左右墙壁各点沿x方向的法向量值基本相同，所以连续随机选取三个点进行法向量阈值判断，以此区分堆料与墙壁，得到真实堆料点云数据和真实墙壁点云数据。

第二步，基于矿场环境感知进行堆料面预测，堆料面预测包括二维KD树建立、K最邻近搜索物料点建立堆料面预测模型与计算挖掘体积；

(1)使用分离出的真实堆料点云数据的x、y坐标建立二维的KD树数据结构；

(2)利用K最邻近搜索方法在已建立的KD树数据结构中搜索已规划成功的挖掘轨迹每一时刻下铲斗齿尖的x、y坐标所对应的堆料点云数据中最邻近的某一堆料点的x、y坐标值，将此堆料点在空间点云中的Z_f值近似替代为这一时刻下挖掘齿尖点正上方所对应的真实堆料点的Z_tr值，因激光雷达扫描的点云数据量较大，点云分布密集，故此方法计算得到的挖掘体积精度较高。堆料面预测模型M(x，y)如下式所示：

Z_tr≈Z_f＝M(x，y)

(3)使用建立的堆料面预测模型计算挖掘体积，通过二重积分实现，已知堆料面预测模型M(x，y)与轨迹规划曲线T(x，y)，将积分区域D_xy通过网格划分成n个封闭区域Δδ_i。挖掘体积计算公式如式。所示：

h_(x，y)＝M(x，y)-T(x，y)

式中点(λ_i，μ_i)为当Δδ_i足够小时，积分区域D_xy中任意一点。以

为高，Δδ_i为底面计算每个立方体体积，并进行累加计算得到挖掘体积。

与现有技术相比，本发明具有以下有益效果：

(1)本发明采用3D激光雷达采集矿场环境点云，将现有点云配准与点云分割方法合并使用，可以更好地实现矿场环境的重构与目标点云的分类与提取。

(2)设计一种基于KD树与K最邻近搜索的堆料面预测模型，通过对复杂堆料进行精确的建模，实现了基于真实堆料点云下挖掘体积的计算，有利于后续最优挖掘轨迹的研究。

附图说明

图1是本发明的方法流程图；

图2是本发明的点云配准前后的矿场环境点云示意图；

图3是本发明通过点云分割得到的料堆点云示意图；

图4是本发明建立的堆料面预测模型用于计算三维下的挖掘体积示意图。

具体实施方式

下面结合附图对本发明进行更为详细的描述：

如图1所示，本发明设计的基于矿场环境感知的堆料面预测方法，包括点云预处理、配准、融合、水平校准、地面与堆料上方无用点去除、聚类、分类及提取、获取堆料点云、维KD树建立、K最邻近搜索物料点建立堆料面预测模型与计算挖掘体积。该方法具体实施步骤如下：

A、使用电铲实验平台安装在两个不同位置的3D激光雷达开始扫描矿场环境，读取点云坐标等关键数据；

B、对源点云与目标点云进行点云预处理，包括去除NAN点与降采样，目的是降低点云数据量，且提升计算效率，降采样使用体素栅格法进行处理，分别将源点云的数据量从94194降采样为2074，将目标点云的数据量从87450降采样为2314；

C、计算点云中各点法向量，使用基于最小二乘的局部表面拟合方法拟合平面，将该平面的切平面法向量作为各点的法向量，并确定法线方向；

D、利用各点的协方差矩阵的特征值计算各点表面曲率；

E、利用上述计算得到的法向量作为基础数据计算FPFH特征，并绘制快速点特征直方图；

F、基于计算得到的FPFH特征，选用SAC-IA采样一致性初始配准算法进行初始旋转平移矩阵的计算：

G、将初始配准得到的刚性变换矩阵(旋转平移矩阵)作为初值，进行最终变换矩阵的计算；

H、将该变换矩阵的值与最优变换矩阵进行对比，计算旋转及平移误差，判断配准的有效性，配准效果如图2所示，旋转及平移误差如表1与表2所示；

表1点云配准旋转误差

粗+精配准方案	SAC-IA(FPFH)+ICP
		X轴旋转误差/°	-0.0199864
Y轴旋转误差/°	0.0118638
		Z轴旋转误差/°	0.00593811
平均旋转误差/°	0.0125961

表2点云配准平移误差

粗+精配准方案	SAC-IA(FPFH)+ICP
		X轴平移误差/m	0.0419984
Y轴平移误差/m	0.00351286
		Z轴平移误差/m	-0.044333
平均平移误差/m	0.02994809

I、使用体素栅格法对点云配准后的两幅点云进行融合，目的是降低点云数据量，提升点云分割及后续轨迹优化的计算效率；

J、由于地面凹凸不平产生的侧倾角与俯仰角，进行水平校准计算，通过机身的倾角传感器可实时得到每个时刻下的两个角度误差值；

K、对地面点以及堆料面上方的无用点进行去除，根据激光雷达安装位置设置最小高度阈值-1m与最大高度阈值0.55m进行数据处理；

L、使用区域生长算法对保留点云进行聚类计算，设置表面曲率0.04m^-1与法向量夹角阈值15°，将堆料、墙面等点云各部分进行聚类处理；

M、设置点云数据量阈值与法向量阈值，将目标点云-堆料进行分类及提取，如图3所示；

N、使用堆料点云的x、y坐标建立二维KD树结构，利用K最邻近搜索方法在已建立的KD树数据结构中搜索已规划成功的挖掘轨迹每一时刻下铲斗齿尖的x、y坐标所对应的堆料点云数据中最邻近的某一堆料点的x、y坐标值，将此堆料点在空间点云中的Z_f值近似替代为这一时刻下挖掘齿尖点正上方所对应的真实堆料点的Z_tr值，如图4所示；

O、使用二重积分的方法进行挖掘体积的计算，因激光雷达扫描的点云数据量较大，点云分布密集，故此方法计算得到的挖掘体积精度较高。

本说明书仅仅是对发明构思实现形式的列举，本发明的保护范围不应该局限于实施例所述的具体形式，任何在本发明披露的技术范围内的等同构思或者改变，均为本发明的保护范围。

Claims (2)

1.一种基于矿场环境感知的堆料面预测方法，其特征在于，首先进行矿场环境感知，将堆料与墙壁进行分类，并在此基础上完成堆料面预测，包括以下步骤：

第一步，进行矿场环境感知，包括点云预处理、配准、融合、水平校准、地面与堆料上方无用点去除、聚类、分类及提取；具体如下：

(1)通过电铲实验平台上安装的两个不同位置的激光雷达扫描采集获取两幅矿场环境点云原始数据，命名为源点云与目标点云，对两幅原始点云进行数据预处理；所述的预处理包括点云NAN点的去除与降采样；

(2)点云配准，由初始配准与精配准两个环节组成；

2.1)初始配准方法选用SAC-IA采样一致性配准算法，根据给定点对间距离阈值选取有限个对应点对进行计算；初始配准需首先计算源点云与目标点云的法向量及表面曲率；搜索点云中各点的K个邻域点，基于最小二乘的局部表面拟合方法拟合平面S，计算公式如下：

式中，

为平面S的法线，d为平面S到坐标原点的距离，s_i为各点重心值，K为邻域点个数，arg min函数表示在目标函数最小时，获取该函数中自变量的值；

以拟合平面S的切平面法线近似作为各点的法向量；各点的K个邻域点的重心在拟合平面S上，同时法向量满足范数等于1，将求解法向量的问题转换为求解协方差矩阵P，对协方差矩阵P进行特征值分解，最小的特征值所对应的特征向量即为平面S的法向量，计算公式如下：

式中，s₀为平面S的重心，K为邻域点个数；

基于上述计算得到的两幅点云的法向量来计算FPFH特征，初始配准将基于FPFH特征进行初始刚性变换矩阵计算，其目的是为精配准提供一个初始迭代值；FPFH特征计算公式如下：

式中，p_q是查询点，p_k是查询点p_q的各邻域点，W_K为权重，以p_q与各邻域点之间的距离来表示，K为邻域点个数，SPFH为查询点与邻域点间的角度特征；

2.2)精配准选用ICP迭代最近点算法，该算法分别在两幅点云中根据点对间距离阈值，最大迭代次数、前后两次刚性变换矩阵最小容差重复寻找对应点对，直至找到最邻近点(p_i，q_i)，计算出最优的刚性变换矩阵，使得误差函数最小；误差函数公式如下:

式中，n为最邻近点对的数量，p_i为目标点云中的点，q_i为源点云中与p_i对应的最邻近点，R为旋转矩阵，t为平移向量；

(3)点云融合仍使用体素栅格法进行配准融合后点云数据量的缩减；

(4)在点云分割前首先需要进行水平校准，两个角度的测量由WK-55电铲实验样机机身的倾角传感器进行读取，水平校准公式如下所示：

式中，x_s、y_s、z_s为带有角度误差的原始点云数据，α是沿x轴顺时针旋转的侧倾角误差角度，β是沿y轴顺时针旋转的俯仰角误差角度，x、y、z是修正后的点云数据；

(5)使用基于传统的几何特征-高度对地面及堆料上方的无用点进行去除，基于激光雷达安装在实验样机的位置高度，设置最小及最大高度阈值，对小于最小高度阈值判断为地面点进行去除，对大于最大高度阈值判断为堆料上方无用点进行去除，在两个阈值范围内的点则进行数据存储及保留；

(6)点云聚类选用区域生长算法，根据点云的基本特征将具有类似特征的点进行合并，并将各聚类点集区域进行分离，且确保每一区域带有较好的边缘信息；该算法通过选取初始种子点，在该点邻域内搜索与其有类似特征的点，将满足曲率与法线夹角阈值的点合并到种子点中继续向外搜索，直至没有点可进行合并，算法停止；

(7)设置点云数据量阈值与法向量阈值进行类别判断，将堆料与墙壁进行分类；因矿场环境中经过地面及堆料上方无用点去除后，所保留数据量较大的聚类点集仅为墙壁与堆料，故将存储的各聚类点云依次进行点云数据量阈值判断；因激光雷达安装及每次扫描位置固定，矿场左右墙壁各点沿x方向的法向量值基本相同，所以连续随机选取三个点进行法向量阈值判断，以此区分堆料与墙壁，得到真实堆料点云数据和真实墙壁点云数据；

第二步，基于矿场环境感知进行堆料面预测，堆料面预测包括二维KD树建立、K最邻近搜索物料点建立堆料面预测模型与计算挖掘体积；

(1)使用第一步步骤(7)分离出的真实堆料点云数据的x、y坐标建立二维的KD树数据结构；

(2)利用K最邻近搜索方法在已建立的KD树数据结构中搜索已规划成功的挖掘轨迹每一时刻下铲斗齿尖的x、y坐标所对应的堆料点云数据中最邻近的某一堆料点的x、y坐标值，将此堆料点在空间点云中的Z_f值近似替代为这一时刻下挖掘齿尖点正上方所对应的真实堆料点的Z_tr值；堆料面预测模型M(x，y)如下式所示：

Z_tr≈Z_f＝M(x，y)

(3)使用建立的堆料面预测模型计算挖掘体积，通过二重积分实现，已知堆料面预测模型M(x，y)与轨迹规划曲线T(x，y)，将积分区域D_xy通过网格划分成n个封闭区域Δδ_i；挖掘体积计算公式如下所示：

h_(x，y)＝M(x，y)-T(x，y)

式中，点(λ_i，μ_i)为当Δδ_i足够小时，积分区域D_xy中任意一点；以

为高，Δδ_i为底面计算每个立方体体积，并进行累加计算得到挖掘体积。

2.根据权利要求1所述的一种基于矿场环境感知的堆料面预测方法，其特征在于，所述第一步步骤(1)中，降采样采用体素栅格降采样方法，其是通过构建三维体素栅格，计算每一个三维栅格内全部点的重心，近似以一个点P_c代替栅格内的其余点：

其中，n为每个三维栅格内的数据点个数，x_i、y_i、z_i分别为栅格内各点的坐标值。

Images