基于真实环境点云的智能电铲挖掘轨迹规划研究

高槐泽， 张天赐， 付 涛， 崔允浩， 孙 伟， 宋学官

（大连理工大学 机械工程学院， 辽宁 大连 116024）

0 引言

大型矿用电铲中的核心设备，因其环境适应性强、挖掘效率高等原因，被广泛用于各大矿山开采系统中。但传统电铲由于人工作业时振动剧烈、环境恶劣等多因素影响，往往导致整体开采效率低下[1]。

随着智慧矿山的提出，近年来已得到广泛关注。目前，电铲智能化工作主要包括两个方面： 环境感知与轨迹规划。 三维点云处理是环境感知的重要环节之一。 目前，国内外学者已提出多种可用来解决点云处理问题的算法。Zucker 最先提出区域增长算法， 经改进后被广泛用于点云处理中[2]；朱建新等针对挖掘机实际工况，提出一种基于PFH 特征的目标识别方法，通过聚类分离获得目标点云[3]。由于不同的挖掘轨迹对应了挖掘过程中不同的挖掘阻力、功率等重要指标，因此挖掘轨迹的好坏将直接决定挖掘质量。 孙伟等使用T 型曲线进行电铲轨迹优化研究[4]；毕秋实使用对数螺旋曲线，结合电铲结构等参数，利用迭代计算挖掘轨迹[5]。

虽然目前有关环境感知与轨迹规划的工作已取得一定进展， 然而大多数工作仅关注点云处理或电铲轨迹规划，没有将二者统筹结合。 因此，本文针对智能电铲在真实环境下的自主挖掘问题，提出了一种基于七段式S 型速度曲线的挖掘轨迹优化方法与一种基于Kd-tree 与K 最邻近搜索的物料点云数据查找方法来计算挖掘体积。 将单位挖掘体积下的最小能耗作为目标函数建立轨迹优化模型，使用GA 算法对挖掘轨迹及参数进行优化，从而获得最优挖掘参数。

1 点云配准

1.1 点云预处理

点云处理基于C++的PCL 点云库进行研究[6]。 加载源点云与目标点云的PCD 文件， 设置点云数据类型为PointXYZ。 去除点云数据中存在的无效点，点云降采样使用体素栅格法进行数据量缩减，提高计算效率。 点云法向量是基于最小二乘法构建局部拟合平面进行计算。

1.2 点云配准

点云粗配准使用基于FPFH 特征的SAC-IA 算法。FPFH 是一种基于点及其近邻点间法线夹角、点间连线夹角关系的特征描述子，由PFH 改进而来[7]，计算复杂度降低，在实际应用中更为常见。粗配准计算得到的变换矩阵将作为精配准的初始值。

点云精配准使用ICP 迭代最近点算法，通过寻找源点云与目标点云中的最邻近点对， 计算出最优匹配参数：旋转矩阵R 与平移向量t，使得误差函数最小[8]，误差函数为：

式中：n—最邻近点对的个数；pi—源点云P 中的一点，qi—目标点云Q 中与pi对应的最邻近点。。

2 点云分割

2.1 水平校准及地面点去除

由于矿场地面凹凸不平导致点云数据存在相对于水平面的侧倾角与俯仰角误差， 在地面点去除前须进行水平校准。 在水平面内以垂直于电铲履带方向为x 轴，以平行于履带方向为y 轴，以垂直于水平面方向为z 轴建立坐标系。 坐标转换矩阵如下所示：

地面点通过设置高度阈值进行去除，若点云数据的z值小于最大高度，则认其为地面点，筛去；若大于或等于最大高度，则认其为非地面点，保留。

2.2 区域生长算法

区域生长算法作为一种点云分割方法， 能够将点云中具有相同特征的连通区域分割出来，并能够提供很好的边界信息和分割结果[6]。 基本原理是在已知点云中按照一定条件选取种子点作为生长起点， 在该点附近寻找相似特征的邻域点进行比对， 将符合条件的邻域点放置进种子区域内作为新的种子点继续向外生长， 直至在该点云搜索不到符合条件的点，即算法停止。

3 轨迹规划

3.1 设计方法及设计变量确定

由于T 型速度曲线加速度不连续， 在作业中会使机械系统受到柔性冲击。 因此， 本文选用一个加速度连续的，呈线性的七段式S 型速度曲线。 挖掘轨迹采用两个不同方向的速度曲线来描述， 分别为沿x 轴方向上的Vx（t）与沿y 轴方向上的Vy（t）：

式中i=1～6；T4=T-（T1+T2+T3+T5+T6+T7），T1～T7分别为速度曲线各段时间。 T 为轨迹规划总时间。 为了保证挖掘初始与终止时刻的平稳性， 两个方向上的初始与终止速度与加速度均设置为0，各时间段的末端速度分别为：

速度曲线在两个运动方向上共涉及15 个未知参数，确定未知参数，即确定铲斗齿尖运动轨迹。 最终待优化的设计变量为x=[Tix，J1x，Tiy，J1y，T]（i=1，2，3，5，6，7）， 其中Tix与Tiy分别为x 轴与y 轴方向上除匀速段其余六段时间；J1x，J1y分别为x 轴与y 轴方向上加加速段的加加速度。

3.2 目标函数确定

电铲作业时主要是由提升与推压机构两者协同工作实现挖掘， 前端工作装置使用拉格朗日方程建立动力学模型，用于计算提升与推压力。 挖掘阻力模型建立的具体计算步骤详见文献[1]。 为了保证电铲实际作业时的高满斗率及低耗能， 将单位挖掘体积下的最小能耗作为目标函数，具体表达式为：

3.3 约束函数确定

（1）为了保证每次挖掘结束，挖掘满斗率应满足实际需求。 设置满斗率约束为80%～110%斗容量：

3.4 堆料数据查找方法

Kd-tree 是一种对K 维空间的数据点进行储存以便能快速检索的数据结构，主要应用于多维空间中数据的范围检索或最邻近搜索。

本文基于堆料点云数据的x、y 坐标建立二维Kdtree 数据结构，使用K 最邻近搜索算法在Kd-tree 中搜索挖掘每一时刻下铲斗齿尖的x、y 坐标所对应的点云数据中最邻近的物料点的x、y 坐标值， 将此物料点的z 值近似替代为这一时刻下齿尖点正上方所对应的真实物料点的z 值。 由于本文使用80 线激光雷达进行数据采集，获取点云数据量较大，精度较高且点云分布密集。 因此，利用该方法计算三维空间下的动态挖掘体积也应具有较高的精度。

3.5 优化模型建立与求解

通过上述分析，无人电铲轨迹优化模型可以归纳如下式所示：

4 实验验证

4.1 点云配准结果分析

表1 为设置的ICP 配准收敛参数。粗配准与精配准的程序运行时间分别为3.407s 与0.061s。 点云配准结果见图1。

图1 点云配准结果对比

表1 ICP 配准收敛参数设置

4.2 点云分割实验结果分析

由于电铲机身安装的倾角传感器没有投入使用，故本文仅给出理论解释与计算公式，C++程序中侧倾角与俯仰角均设置为0.0001°。高度阈值设置为0.55m。表2 为区域生长算法设置的参数。

表2 区域生长算法参数设置

点云聚类结果见图2，由于聚类点集中仅有料堆与墙壁点集数据量较大， 因此通过数据量排序可筛除其余无用点。 由于激光雷达安装位置固定， 基站坐标系固定， 每次扫描后点云数据中墙壁点沿x 轴的法向量值分别接近于1（左墙壁）与-1（右墙壁），在排序前三各聚类点集中随机选择三个点，设置法向量最小阈值 为-0.8，最大阈值为0.8，若法向量值处在最小与最大阈值之间，则判断为物料点集，若法向量值处在阈值范围外，则判断为左右墙壁点集。 点云分割得到的堆料点云，见图3。

图2 点云聚类结果

图3 点云分割得到的物料点云

4.3 轨迹规划实验结果分析

仿真环境采用Matlab 2018b， 设置GA 算法中种群规模、 最大进化代数与最大连续不进化代数为50、30 与20，设计变量、 上下边界、优化结果与约束函数见表3 与表4。 提升与推压速度等挖掘参数均满足约束函数要求。 目标函数f为0.0228kW·h/m3， 挖掘体积V 为0.1399m3，满斗率为87.4%，满足实际挖掘满斗率要求。

图4 WK-55 电铲1∶7 缩比实验样机

表3 设计变量在上下边界约束下的优化结果

表4 约束函数取值

图5 挖掘轨迹优化运行结果

图6 最优挖掘轨迹参数

5 结论

本文针对传统电铲在多因素影响下无法实现高效挖掘， 提出了一种基于七段式S 型速度曲线的挖掘轨迹优化方法和一种基于Kd-tree 与K 最邻近搜索的物料点云数据查找方法计算挖掘体积。 通过仿真实验可得到如下结论：

本文所用的点云处理方法可以较好地实现环境点云的配准与物料点云的分割。 挖掘轨迹规划仿真实验得到的各挖掘参数均满足约束函数限制， 挖掘轨迹与目标函数符合实验前预设需求，可以应用于实际样机挖掘实验。在后续研究中， 将继续完善并优化环境点云处理与挖掘轨迹规划算法程序，减少程序运行时间及提高计算效率，以满足实际需求。