CN112363153A - 一种料堆边缘检测方法及系统 - Google Patents

Abstract

本发明涉及一种料堆边缘检测方法及系统。该方法包括：获取雷达探测的料堆切削面的二维点云数据；雷达固定于取料机悬臂的侧边，且位于斗轮的侧边；雷达用于扫描取料机悬臂前方的料堆切削面，雷达的扫描面为竖直平面；根据取料机悬臂的回转方向，将二维点云数据扩展为三维点云数据；对三维点云数据进行预处理；对当前帧对应的处理后的三维点云数据进行平面拟合；计算当前帧对应的物料面倾角；判断当前帧对应的物料面倾角是否小于物料安息角；如果是，确定当前帧雷达的扫描面处于物料堆垛边缘的外侧；如果否，确定当前帧雷达的扫描面处于物料堆垛边缘的内侧。本发明可以提高料堆边缘检测的准确度和及时性。

Description

一种料堆边缘检测方法及系统

技术领域

本发明涉及取料作业过程检测领域，特别是涉及一种料堆边缘检测方法及系统。

背景技术

目前，国内散杂货取料作业以人工控制取料为主，半自动化和自动化控制取料由于技术限制仅占很小部分；人工取料作业时，取料机司机通过人眼观察分析料堆形状，按照堆垛具体情况进行分层(一般分为三层)，判断取料机斗轮对物料堆的切入点；然后手动控制取料机的走行、回转和悬臂俯仰等。在通常采取的回转取料方式中，根据取料机悬臂的初始状态和料堆的相对位置选择向左或向右回转进行取料，自回转过程开始上料称为进入料堆；当达到一定旋转角度后，在取料机斗轮即将旋出料堆时，取料机向前走行一定距离进行反方向回转取料，如此反复作业直到当前层取料数量要求，继而取料机走行重复上述过程进行下一层取料作业。因此，不对料堆边界进行检测，预判取料机斗轮旋出料堆时间，则每次悬臂回转过程中都可能使斗轮旋出料堆两边，斗轮空转造成皮带上物料断流，大大影响取料工作效率。

在上述过程中，判断斗轮出料堆边界主要依靠人眼、高清摄像头、光学雷达和斗轮功率数据。在人工操作情况下，即使天气晴朗光线良好，但由于司机室通常安置于悬臂右侧，所以对于斗轮左侧旋转出料堆的情况司机很难准确判断，经常出现悬臂摆出料堆的情况；如果照明不足或者在雨雪雾的天气条件下，则人眼基本无法识别估计垛边距离。在半自动化和自动化控制的情况下，通常由高清摄像头、光学传感器(如激光雷达)和斗轮功率作为料堆边界检测手段；在利用光学设备时，必须要求现场天气情况、光线条件和工作环境良好。如果在大雾、大雨或者摄像头和传感器表面被污渍覆盖则会完全失去作业能力，甚至会低于人工操作状态下的工作效率。如果使用斗轮功率作为检测依据，虽然能够避免天气和光线的影响，但是悬臂回转取料过程中是无法通过斗轮功率变化趋势检测垛边的；比如，当斗轮内旋时由于取料层逐渐变厚，斗轮负载功率也会逐渐升高，直至转出垛边斗轮功率断崖式下降。因此，不论以哪种现存方式进行取料作业和边界判断，料堆边界的检测准确度较低，且及时性差。

发明内容

本发明的目的是提供一种料堆边缘检测方法及系统，以提高料堆边缘检测的准确度，提高及时性。

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

一种料堆边缘检测方法，包括：

获取雷达探测的料堆切削面的二维点云数据；所述雷达固定于取料机悬臂的侧边，且位于斗轮的侧边；所述雷达用于扫描所述取料机悬臂前方的料堆切削面，所述雷达的扫描面为竖直平面；

根据所述取料机悬臂的回转方向，将所述二维点云数据扩展为三维点云数据；

对所述三维点云数据进行预处理，得到处理后的三维点云数据；

对当前帧对应的处理后的三维点云数据进行平面拟合，得到当前帧对应的拟合平面；

计算当前帧对应的物料面倾角；所述当前帧对应的物料面倾角为当前帧对应的拟合平面与水平面之间的夹角；

判断当前帧对应的物料面倾角是否小于物料安息角；

当当前帧对应的物料面倾角小于物料安息角时，确定当前帧雷达的扫描面处于物料堆垛边缘的外侧；

当当前帧对应的物料面倾角不小于物料安息角时，确定当前帧雷达的扫描面处于物料堆垛边缘的内侧。

可选的，所述获取雷达探测的料堆切削面的二维点云数据，具体包括：

当所述取料机悬臂的回转方向为向右方旋转时，获取第一雷达探测的料堆切削面的二维点云数据；所述第一雷达固定于取料机悬臂的右侧边，且位于斗轮的右侧边；

当所述取料机悬臂的回转方向为向左方旋转时，获取第二雷达探测的料堆切削面的二维点云数据；所述第二雷达固定于取料机悬臂的左侧边，且位于斗轮的左侧边。

可选的，所述当当前帧对应的物料面倾角小于物料安息角时，确定当前帧雷达的扫描面处于物料堆垛边缘的外侧，之后还包括：

当所述取料机悬臂的回转方向为向右方旋转时，根据所述取料机悬臂向右旋转的速度和所述雷达与所述斗轮之间的距离，确定斗轮的右侧边处于物料堆垛边缘的外侧的时间；

当所述取料机悬臂的回转方向为向左方旋转时，根据所述取料机悬臂向左旋转的速度和所述雷达与所述斗轮之间的距离，确定斗轮的左侧边处于物料堆垛边缘的外侧的时间。

可选的，所述当所述取料机悬臂的回转方向为向右方旋转时，根据所述取料机悬臂向右旋转的速度和所述雷达与所述斗轮之间的距离，确定斗轮的右侧边处于物料堆垛边缘的外侧的时间，之后还包括：

根据所述斗轮的宽度和所述取料机悬臂向右旋转的速度，确定取料机旋出物料堆垛边缘的剩余时间；

所述当所述取料机悬臂的回转方向为向左方旋转时，根据所述取料机悬臂向左旋转的速度和所述雷达与所述斗轮之间的距离，确定斗轮的左侧边处于物料堆垛边缘的外侧的时间，之后还包括：

根据所述斗轮的宽度和所述取料机悬臂向左旋转的速度，确定取料机旋出物料堆垛边缘的剩余时间。

可选的，所述当当前帧对应的物料面倾角不小于物料安息角时，确定当前帧雷达的扫描面处于物料堆垛边缘的内侧，之后还包括：

当所述取料机悬臂的回转方向为向右方旋转时，确定当前斗轮的右侧边处于物料堆垛边缘的内侧；

当所述取料机悬臂的回转方向为向左方旋转时，确定当前斗轮的左侧边处于物料堆垛边缘的内侧。

可选的，所述根据所述取料机悬臂的回转方向，将所述二维点云数据扩展为三维点云数据，具体包括：

建立雷达的二维直角坐标系；所述二维直角坐标系的原点为所述雷达的中心，所述二维直角坐标系的第一坐标轴与雷达平面垂直，所述二维直角坐标系的第二坐标轴与雷达扫描面共面且与所述雷达平面也共面；

确定所述二维点云数据在所述二维直角坐标系中的二维坐标；

根据所述取料机悬臂的回转方向，基于所述二维直角坐标系构建三维直角坐标系；所述三维直角坐标系的第一坐标轴为所述二维直角坐标系的第一坐标轴，所述三维直角坐标系的第二坐标轴为所述二维直角坐标系的第二坐标轴，所述三维直角坐标系的第三坐标轴为垂直于所述第一坐标轴和所述第二坐标轴的坐标轴，所述三维直角坐标系的原点为所述取料机悬臂开始回转时所述雷达的中心；

根据所述取料机悬臂的回转参数，确定所述二维点云数据中每一个点在所述第三坐标轴上的坐标值，确定所述二维点云数据中每一个点的三维坐标，得到所述三维点云数据。

可选的，所述对所述三维点云数据进行预处理，得到处理后的三维点云数据，具体包括：

筛选所述三维点云数据中处于限定坐标范围内的点，得到筛选后的三维点云数据；所述限定坐标范围为三维坐标值中第一坐标值处于第一限定范围内且第二坐标值处于第二限定范围内；

剔除所述筛选后的三维点云数据中的伪影点，得到剔除后的三维点云数据；

采用统计滤波器，剔除所述剔除后的三维点云数据中的异常点，得到处理后的三维点云数据。

可选的，所述对当前帧对应的处理后的三维点云数据进行平面拟合，得到当前帧对应的拟合平面，具体包括：

利用最小二乘法，对当前帧对应的处理后的三维点云数据进行平面法向量求解，得到当前帧对应的处理后的三维点云数据对应的平面法向量；

根据所述平面法向量，确定所述当前帧对应的处理后的三维点云数据对应的拟合平面。

可选的，所述计算当前帧对应的物料面倾角，具体包括：

计算所述平面法向量在XOZ坐标平面上的投影，得到二维向量；所述XOZ坐标平面为所述第一坐标轴和所述第二坐标轴所在的平面；

根据所述二维向量与所述第一坐标轴之间的夹角，通过补角计算，得到所述拟合平面与所述第一坐标轴之间的夹角；

结合取料机悬臂的仰角和雷达与水平面之间的夹角，对所述拟合平面与所述第一坐标轴之间的夹角进行修正，得到所述拟合平面与水平面之间的夹角。

本发明还提供一种料堆边缘检测系统，包括：

二维点云数据获取模块，用于获取雷达探测的料堆切削面的二维点云数据；所述雷达固定于取料机悬臂的侧边，且位于斗轮的侧边；所述雷达用于扫描所述取料机悬臂前方的料堆切削面，所述雷达的扫描面为竖直平面；

三维点云数据扩展模块，用于根据所述取料机悬臂的回转方向，将所述二维点云数据扩展为三维点云数据；

预处理模块，用于对所述三维点云数据进行预处理，得到处理后的三维点云数据；

平面拟合模块，用于对当前帧对应的处理后的三维点云数据进行平面拟合，得到当前帧对应的拟合平面；

物料面倾角计算模块，用于计算当前帧对应的物料面倾角；所述当前帧对应的物料面倾角为当前帧对应的拟合平面与水平面之间的夹角；

判断模块，用于判断当前帧对应的物料面倾角是否小于物料安息角；

当前帧雷达的扫描面处于物料堆垛边缘的外侧确定模块，用于当当前帧对应的物料面倾角小于物料安息角时，确定当前帧雷达的扫描面处于物料堆垛边缘的外侧；

当前帧雷达的扫描面处于物料堆垛边缘的内侧确定模块，用于当当前帧对应的物料面倾角不小于物料安息角时，确定当前帧雷达的扫描面处于物料堆垛边缘的内侧。

根据本发明提供的具体实施例，本发明公开了以下技术效果：

本发明基于离散点云平面拟合原理，对散杂货取料过程中料堆边界进行检测，能够在雷达二维扫描面的情况下根据雷达时序位置变化情况，计算出扫描过程的三维点云数据。进而通过直接分析拼接后的三维点云数据能够提高数据信息量，还能作为异常点剔除的依据，可以克服雷达伪影多和分辨率低的缺点，进而提高料堆边缘检测的准确度。

从采用的设备角度出发，对比目前大部分采用光学设备作为感知手段的方法，本发明采用的雷达探测具有全天时、全天候的优点，不再需要考虑如摄像头或激光探头、激光雷达的光学镜面污损问题；避免了由于散杂料堆场的恶劣环境，清洁和维护光学设备将耗费大量人力，并且防止了可能会在取料工作过程中由于污损，导致中断作业等问题；也避免了人工取料作业和半自动取料作业中由于人眼观察错误造成的问题，提高了料堆边界检测的实时性。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明料堆边缘检测方法的流程示意图；

图2为本发明中雷达安装位置示意图；

图3为本发明取料现场悬臂取料机回转取料过程图片；

图4为本发明料堆边缘检测系统的结构示意图；

图5为本发明具体实施例中斗轮右侧雷达一帧的点云数据示意图；

图6为本发明具体实施例中二维点云数据和拼接后三维点云数据的示意图；

图7为本发明具体实施例中剔除异常点过程的三维点云数据示意图；

图8为本发明具体实施例中拟合平面拼接的三维曲面图和等高线图；

图9为本发明具体实施例中雷达检测到的物料面倾角的曲线图。

具体实施方式

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

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

图1为本发明料堆边缘检测方法的流程示意图。如图1所示，本发明料堆边缘检测方法包括以下步骤：

步骤100：获取雷达探测的料堆切削面的二维点云数据。如图2所示，雷达1固定于取料机悬臂2的侧边，且位于斗轮的侧边。如图3所示，所述雷达用于扫描所述取料机悬臂前方的料堆切削面，所述雷达的扫描面为竖直平面。由于取料机悬臂2左右两侧的金属结构略有差异，故根据实际情况将雷达安装于前侧无任何遮挡物的位置，本发明采用毫米波雷达作为探测装置，取料过程中雷达的探测面与斗轮的侧面平行。

本发明至少在取料机悬臂的两侧各安装一个雷达，当取料机悬臂的回转方向为向右方旋转时，采用取料机悬臂右侧的第一雷达探测料堆切削面的二维点云数据。具体的，所述第一雷达固定于取料机悬臂的右侧边，且位于斗轮的右侧边。当所述取料机悬臂的回转方向为向左方旋转时，采用取料机悬臂左侧的第二雷达探测料堆切削面的二维点云数据。具体的，所述第二雷达固定于取料机悬臂的左侧边，且位于斗轮的左侧边。

步骤200：根据取料机悬臂的回转方向，将二维点云数据扩展为三维点云数据。具体的，本发明根据时间序列和悬臂旋转角速度将毫米波雷达扫描的二维平面离散点扩展为三维点云数据，即成为一个三维坐标系下，扫描平面随时间构成的三维点云数据。具体过程如下：

Step1：建立雷达的二维直角坐标系。所述二维直角坐标系的原点为所述雷达的中心，所述二维直角坐标系的第一坐标轴与雷达平面垂直，第一坐标轴的正向为由雷达中心指向堆料的方向；所述二维直角坐标系的第二坐标轴与雷达扫描面共面且与所述雷达平面也共面，第二坐标轴为由雷达中心指向上方的方向。

Step2：确定所述二维点云数据在所述二维直角坐标系中的二维坐标。

Step3：根据所述取料机悬臂的回转方向，基于所述二维直角坐标系构建三维直角坐标系。所述三维直角坐标系的第一坐标轴为所述二维直角坐标系的第一坐标轴，所述三维直角坐标系的第二坐标轴为所述二维直角坐标系的第二坐标轴，所述三维直角坐标系的第三坐标轴为垂直于所述第一坐标轴和所述第二坐标轴的坐标轴，所述三维直角坐标系的原点(0,0,0)为取料开始前完成对垛之后的雷达中心，即取料机悬臂开始回转时的雷达中心。

Step4：根据所述取料机悬臂的回转参数，确定所述二维点云数据中每一个点在所述第三坐标轴上的坐标值，确定所述二维点云数据中每一个点的三维坐标，得到所述三维点云数据。第三坐标轴的坐标为每帧扫描面与首帧扫描面之间的相对距离，根据取料机悬臂的臂长以及旋转角速度和时间，可以确定每帧二维点云数据中每个点在第三坐标轴的坐标值，即可以确定每个点的三维坐标。

步骤300：对三维点云数据进行预处理，得到处理后的三维点云数据。具体过程如下：

筛选所述三维点云数据中处于限定坐标范围内的点，得到筛选后的三维点云数据。所述限定坐标范围为三维坐标值中第一坐标值处于第一限定范围内且第二坐标值处于第二限定范围内。以第一坐标轴为X轴，第二坐标轴为Z轴为例，假设第一限定范围为0～10，第二限定范围为0～15，那么限定坐标范围为X坐标值位于0～10之间，且Z坐标值位于0～15之间的点。

剔除所述筛选后的三维点云数据中的伪影点，得到剔除后的三维点云数据。

采用常用的统计滤波器，剔除所述剔除后的三维点云数据中的异常点，去除噪声，得到处理后的三维点云数据。

步骤400：对当前帧对应的处理后的三维点云数据进行平面拟合，得到当前帧对应的拟合平面。平面拟合的方法有很多，本发明采用求解平面法向量的方法进行平面拟合。首先，对处理后的三维点云数据进行平面法向量求解，其中，设该三维点云数据的拟合平面方程为ax+by+cz＝d，(x,y,z)是扫描得到的三维点云数据中点的坐标，(a,b,c)即是拟合平面的法向量。为方便程序运算，将平面法向量用矩阵表示

即

求解未知数(a,b,c)即为拟合平面的法向量

此时，拟合平面求法向量问题转化为超定方程组求最优解问题，本步骤中选用通过最小二乘法解得未知数作为拟合平面法向量。求得平面法向量之后，便得到拟合平面中的参数，得到拟合平面。

步骤500：计算当前帧对应的物料面倾角。所述当前帧对应的物料面倾角为当前帧对应的拟合平面与水平面之间的夹角。根据拟合平面的表达式可以直接求得拟合平面与第一坐标轴之间的夹角，然后结合取料机悬臂的仰角和雷达与水平面之间的夹角，对拟合平面与第一坐标轴之间的夹角进行修正，便得到所述拟合平面与水平面之间的夹角。

本发明通过拟合平面的平面法向量求得拟合平面与第一坐标轴之间的夹角，具体的，首先计算所述平面法向量在XOZ坐标平面上的投影，得到二维向量，所述XOZ坐标平面为所述第一坐标轴和所述第二坐标轴所在的平面；然后根据所述二维向量与所述第一坐标轴之间的夹角，通过补角计算，得到所述拟合平面与所述第一坐标轴之间的夹角。

步骤600：判断当前帧对应的物料面倾角是否小于物料安息角。如果是，执行步骤700；如果否，执行步骤800。本发明将物料安息角作为判断阈值，在料堆静止基础上，斗轮切削后，物料面倾角通常不会小于安息角。故当物料面倾角α小于物料安息角时判断雷达扫描面已经处于物料堆垛之外。

步骤700：确定当前帧雷达的扫描面处于物料堆垛边缘的外侧。此时根据雷达与斗轮之间距离和悬臂回转角速度可以预知斗轮摆出物料堆垛时间，为判断斗轮出垛时间并选择回转时机提供有效依据。

具体的，当所述取料机悬臂的回转方向为向右方旋转时，根据所述取料机悬臂向右旋转的速度和所述雷达与所述斗轮之间的距离，可以确定斗轮的右侧边处于物料堆垛边缘的外侧的时间。之后，还可以进一步根据所述斗轮的宽度和所述取料机悬臂向右旋转的速度，确定取料机旋出物料堆垛边缘的剩余时间。

当所述取料机悬臂的回转方向为向左方旋转时，根据所述取料机悬臂向左旋转的速度和所述雷达与所述斗轮之间的距离，可以确定斗轮的左侧边处于物料堆垛边缘的外侧的时间。之后，根据所述斗轮的宽度和所述取料机悬臂向左旋转的速度，确定取料机旋出物料堆垛边缘的剩余时间。

步骤800：确定当前帧雷达的扫描面处于物料堆垛边缘的内侧。而且，当所述取料机悬臂的回转方向为向右方旋转时，此时可以确定当前斗轮的右侧边处于物料堆垛边缘的内侧；当所述取料机悬臂的回转方向为向左方旋转时，此时可以确定当前斗轮的左侧边处于物料堆垛边缘的内侧。

基于图1所示的料堆边缘检测方法，本发明还提供一种料堆边缘检测系统，图4为本发明料堆边缘检测系统的结构示意图。如图4所示，本发明料堆边缘检测系统包括：

二维点云数据获取模块401，用于获取雷达探测的料堆切削面的二维点云数据；所述雷达固定于取料机悬臂的侧边，且位于斗轮的侧边；所述雷达用于扫描所述取料机悬臂前方的料堆切削面，所述雷达的扫描面为竖直平面。

三维点云数据扩展模块402，用于根据所述取料机悬臂的回转方向，将所述二维点云数据扩展为三维点云数据。

预处理模块403，用于对所述三维点云数据进行预处理，得到处理后的三维点云数据。

平面拟合模块404，用于对当前帧对应的处理后的三维点云数据进行平面拟合，得到当前帧对应的拟合平面。

物料面倾角计算模块405，用于计算当前帧对应的物料面倾角；所述当前帧对应的物料面倾角为当前帧对应的拟合平面与水平面之间的夹角。

判断模块406，用于判断当前帧对应的物料面倾角是否小于物料安息角。

当前帧雷达的扫描面处于物料堆垛边缘的外侧确定模块407，用于当当前帧对应的物料面倾角小于物料安息角时，确定当前帧雷达的扫描面处于物料堆垛边缘的外侧。

当前帧雷达的扫描面处于物料堆垛边缘的内侧确定模块408，用于当当前帧对应的物料面倾角不小于物料安息角时，确定当前帧雷达的扫描面处于物料堆垛边缘的内侧。

下面提供一个具体实施例进一步说明本发明的方案。

步骤(1)：二维点云数据探测。

本实施例中，通过悬臂取料机斗轮左右两侧搭载的毫米波雷达作为探测装置，取料过程中雷达探测平面与斗轮平面平行，右探测面向斗轮平面右侧平移4.74米，左探测面向斗轮平面左侧平移4.08米，即得到如图5所示的取料切削面线状点云。

如取料机在取料过程中向右侧回转时，以右侧雷达返回的数据作为判断依据；向左侧回转时以左侧雷达返回的数据作为判断依据。本实施例中所使用的毫米波雷达的扫描视场角为在感应距离0-70米范围内±60度(即共120度)扇形区域和波束垂直方向视场角±10度扇形区域(由于毫米波雷达特性，其不具备分辨波束垂直方向视场角的能力，所以得到数据为被测物相对雷达的欧式距离和方位角)；将毫米波雷达竖直放置，即雷达平面为竖直方向的平面。根据波束扫描特点可知，雷达每帧(共72毫秒)扫描过程为以水平面方向自雷达自身+60度向上扫描至-60度位置(即雷达中心点的垂直射线为0度)。得到原始数据为在极坐标系下的，为方便运算需要转换为平面直角坐标系下，即二维直角坐标系。

步骤(2)：拼接三维点云数据。

本步骤需要首先建立描述雷达每帧扫描平面的雷达局部坐标系(二维直角坐标系)，然后构建用以描述拼接悬臂回转过程中点云数据的三维直角坐标系，进而根据时间序列和悬臂旋转角速度将毫米波雷达扫描的二维平面离散点扩展为三维点云数据，即成为一个三维坐标系下，扫描平面随时间构成的三维点云数据。将扫描得到的五帧二维点云数据拼接在一个世界坐标系下，显然，根据得到的切削面线状点云为每帧平面间引入平移距离即可。根据探测料堆尺度，本实施例中设置每帧平面之间距离为1米，此时平面拟合效果良好，不会出现过拟合现象。

首先，建立雷达局部坐标系。以垂直雷达平面，自雷达中心指向远端的射线为坐标系横轴正向；自雷达中心引出在雷达扫描平面中垂直横轴的射线为雷达局部坐标系纵轴，向上的方向为纵轴的正向。

然后，建立雷达回转过程三维直角坐标系。由于毫米波雷达固定在斗轮两侧，并跟随斗轮做同圆心不同半径的圆周运动，所以根据运动时角速度的不同，每时刻扫描平面在坐标系中的移动距离也需修正。以取料开始前完成对垛过程，悬臂即将开始回转时的雷达中心为原点(0,0,0)，X轴和Z轴分别是此时雷达局部坐标系的横轴和纵轴，Y轴为自原点垂直XOZ平面的射线，Y轴的正向与悬臂回转方向一致，用以表示经过修正后的每个扫描平面与首个扫描平面相对距离。进而可以根据二维点云数据依次得到拼接后的三维点云数据，如图6所示，图6中(a)部分为二维点云数据，图6中(b)部分为拼接后的三维点云数据。

本实施例中悬臂取料机斗轮右侧雷达距离斗轮为4.74米，斗轮左侧雷达与斗轮距离为4.08米，根据弧长公式反推可得雷达判定出垛后斗轮旋转β度后斗轮将旋转出料堆切削面边界。

式中l表示取料机雷达和斗轮的距离，β表示悬臂回转角度，r表示悬臂半径。计算得到当悬臂向右旋转时，右侧雷达判定出垛后斗轮继续旋转7.76度出垛；同理，左侧为判定出边界后继续旋转6.68度出垛。根据悬臂回转角速度ωrad/s，可知向右旋转时，雷达判定出边界够经过

秒后斗轮旋转出料堆边界；同理，向左旋转时经过

秒后雷达旋转出料堆边界，以此计算取料机旋转出边界的剩余时间。

步骤(3)：对三维点云数据进行预处理。

首先，限制点云数据X轴和Z轴的坐标范围。本实施例中通过模拟信号中滤波的限带滤波思想与取料机取料实际工作状况结合，为避免待取料堆周围料堆或堆场内其他结构的影响，将点云数据限制在以雷达中心为远点的局部坐标系中纵深距离15米内的范围，此时行成了顶角120度、底角30度和腰长30米、底边长52米等腰三角形的平面视场。

然后，再通过虚警概率、多普勒模糊状态、集群不确定状态和雷达散射截面等质量信息数据去掉扫描中的不可靠点，即伪影点。

最后，再利用三维点云中常用的统计滤波器，选取若干帧扫描平面点云拼接的三维点云进行异常点剔除，得到去除噪声后的三维点云。经过对毫米波雷达每帧间隔时间内角速度积分运算，得到旋转过程的圆心角，按照步骤(2)的计算方法，得到前后两帧之间的弧长后计算这段弧首尾两端之间平面直角坐标差值，作为下一帧数据坐标点的修正值(即对下一帧数据所有点纵深距离进行加减)。最后，拼接数据得到这一时间间隔(这一组)的三维点云数据，根据统计滤波原理，计算一组数据所有点到其他点的三维欧式距离，并按一定比例选择出“邻近点”，再求每个点到邻近点的平均距离。这时这一组中所有点都有到邻近点的平均距离，以全局邻近点平均距离标准差为阈值(根据实际需求可以选择放大或缩小阈值)，本次经过多次试验效果比对选择1倍标准差作为阈值；然后将自身邻近平均距离减去全局临近平均距离再与设定的阈值进行比较，超过阈值的点则被定义为离群点即异常点，剔除异常点。如图7所示，图7中(a)部分为原始的三维点云数据，图7中(b)部分为处理后的三维点云数据。

步骤(4)：平面拟合。将上一步骤中完成剔除异常点的一组三维点云数据进行平面法向量求解，其中，设这组点云的拟合平面方程为ax+by+cz＝d，(x,y,z)是扫描得到的三维点云数据，(a,b,c)即是拟合平面的法向量，通过投影方法求得XOZ平面上的向量投影。

首先，为方便程序运算将其矩阵表示

即

求解未知数(a,b,c)即为拟合平面的法向量

此时，拟合平面求法向量问题转化为超定方程组求最优解问题，考虑到取料作业中对料堆边界检测需要实时性，至少需要保证在亚秒级反馈，所以为减少计算量将平面方程中z的系数设为1，改写成ax+by+d＝z的形式，再考虑步骤(5)中是投影至XOZ平面，即此时法向量表示为

故所求参数仅为一个a。根据超定方程最小二乘法求解推导过程可知，将原方程转换为

其中矩阵形式为：

根据克莱默法则求未知数a，又由超定方程最小二乘解法充要条件可知以上方程组必有唯一解，故可将其求法简化为：

至此，计算得到投影至XOZ平面的法向量投影

进而可以得到每组三维点云数据对应的拟合平面。如图8所示，图8中(a)部分为本实施例中拟合平面拼接的三维曲面图，图8中(b)部分为对应的等高线图。

步骤(5)：判断毫米波雷达所扫描的料堆部分的位置。

根据步骤(1)可知，本实施例中每组点云数据的时间间隔为0.35秒，在垛中和垛外得到的三维点云数据拟合出的平面变化主要体现在沿X轴上；而且在这样较小时间间隔下，拟合平面沿Y轴基本无变化。所以，取法向量到坐标平面XOZ上的投影，取二维向量

经过补角计算得到拟合平面与X轴正方向的夹角；结合悬臂俯仰角度和雷达与堆场地平面夹角的修正，计算出实时拟合的取料面和水平面的夹角α，即取料面倾角，进而得到图9所示的曲线。

式中，State表示斗轮当前状态，1表示斗轮处于取料切削面内，0表示斗轮处于取料切削面外，α表示拟合平面和水平面夹角，A_r表示料堆自然安息角。

本实施例将物料安息角作为判断阈值，如果雷达扫描面处在物料垛中时，经过料堆切削面的取料面倾角一定大于料堆自然安息角，故当拟合平面夹角α小于物料安息角时判断雷达扫描面已经处于物料堆垛之外。根据步骤(2)中时间t计算方法能够得到斗轮旋转出取料面的剩余时间，并为取料机悬臂选择合适的反方向回转时机提供依据。

利用本发明能够在雷达二维扫描面的情况下根据雷达时序位置变化情况，计算出扫描过程的三维点云数据。直接分析拼接后的三维点云数据既能够提高数据信息量，还能作为异常点剔除的依据，可以克服毫米波雷达伪影多和分辨率低的缺点。从设备上，对比目前大部分采用光学设备作为感知手段的方法，毫米波雷达探测具有全天时、全天候的优点，不再需要考虑如摄像头或激光探头、激光雷达的光学镜面污损问题；避免了由于散杂料堆场的恶劣环境，清洁和维护光学设备将耗费大量人力，并且防止了可能会在取料工作过程中由于污损，导致中断作业等问题；也避免了人工取料作业和半自动取料作业中由于人眼观察错误造成的问题。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种料堆边缘检测方法，其特征在于，包括：

获取雷达探测的料堆切削面的二维点云数据；所述雷达固定于取料机悬臂的侧边，且位于斗轮的侧边；所述雷达用于扫描所述取料机悬臂前方的料堆切削面，所述雷达的扫描面为竖直平面；

根据所述取料机悬臂的回转方向，将所述二维点云数据扩展为三维点云数据；

对所述三维点云数据进行预处理，得到处理后的三维点云数据；

对当前帧对应的处理后的三维点云数据进行平面拟合，得到当前帧对应的拟合平面；

计算当前帧对应的物料面倾角；所述当前帧对应的物料面倾角为当前帧对应的拟合平面与水平面之间的夹角；

判断当前帧对应的物料面倾角是否小于物料安息角；

当当前帧对应的物料面倾角小于物料安息角时，确定当前帧雷达的扫描面处于物料堆垛边缘的外侧；

当当前帧对应的物料面倾角不小于物料安息角时，确定当前帧雷达的扫描面处于物料堆垛边缘的内侧。

2.根据权利要求1所述的料堆边缘检测方法，其特征在于，所述获取雷达探测的料堆切削面的二维点云数据，具体包括：

当所述取料机悬臂的回转方向为向右方旋转时，获取第一雷达探测的料堆切削面的二维点云数据；所述第一雷达固定于取料机悬臂的右侧边，且位于斗轮的右侧边；

当所述取料机悬臂的回转方向为向左方旋转时，获取第二雷达探测的料堆切削面的二维点云数据；所述第二雷达固定于取料机悬臂的左侧边，且位于斗轮的左侧边。

3.根据权利要求2所述的料堆边缘检测方法，其特征在于，所述当当前帧对应的物料面倾角小于物料安息角时，确定当前帧雷达的扫描面处于物料堆垛边缘的外侧，之后还包括：

当所述取料机悬臂的回转方向为向右方旋转时，根据所述取料机悬臂向右旋转的速度和所述雷达与所述斗轮之间的距离，确定斗轮的右侧边处于物料堆垛边缘的外侧的时间；

当所述取料机悬臂的回转方向为向左方旋转时，根据所述取料机悬臂向左旋转的速度和所述雷达与所述斗轮之间的距离，确定斗轮的左侧边处于物料堆垛边缘的外侧的时间。

4.根据权利要求3所述的料堆边缘检测方法，其特征在于，所述当所述取料机悬臂的回转方向为向右方旋转时，根据所述取料机悬臂向右旋转的速度和所述雷达与所述斗轮之间的距离，确定斗轮的右侧边处于物料堆垛边缘的外侧的时间，之后还包括：

根据所述斗轮的宽度和所述取料机悬臂向右旋转的速度，确定取料机旋出物料堆垛边缘的剩余时间；

所述当所述取料机悬臂的回转方向为向左方旋转时，根据所述取料机悬臂向左旋转的速度和所述雷达与所述斗轮之间的距离，确定斗轮的左侧边处于物料堆垛边缘的外侧的时间，之后还包括：

根据所述斗轮的宽度和所述取料机悬臂向左旋转的速度，确定取料机旋出物料堆垛边缘的剩余时间。

5.根据权利要求1所述的料堆边缘检测方法，其特征在于，所述当当前帧对应的物料面倾角不小于物料安息角时，确定当前帧雷达的扫描面处于物料堆垛边缘的内侧，之后还包括：

当所述取料机悬臂的回转方向为向右方旋转时，确定当前斗轮的右侧边处于物料堆垛边缘的内侧；

当所述取料机悬臂的回转方向为向左方旋转时，确定当前斗轮的左侧边处于物料堆垛边缘的内侧。

6.根据权利要求1所述的料堆边缘检测方法，其特征在于，所述根据所述取料机悬臂的回转方向，将所述二维点云数据扩展为三维点云数据，具体包括：

建立雷达的二维直角坐标系；所述二维直角坐标系的原点为所述雷达的中心，所述二维直角坐标系的第一坐标轴与雷达平面垂直，所述二维直角坐标系的第二坐标轴与雷达扫描面共面且与所述雷达平面也共面；

确定所述二维点云数据在所述二维直角坐标系中的二维坐标；

根据所述取料机悬臂的回转方向，基于所述二维直角坐标系构建三维直角坐标系；所述三维直角坐标系的第一坐标轴为所述二维直角坐标系的第一坐标轴，所述三维直角坐标系的第二坐标轴为所述二维直角坐标系的第二坐标轴，所述三维直角坐标系的第三坐标轴为垂直于所述第一坐标轴和所述第二坐标轴的坐标轴，所述三维直角坐标系的原点为所述取料机悬臂开始回转时所述雷达的中心；

根据所述取料机悬臂的回转参数，确定所述二维点云数据中每一个点在所述第三坐标轴上的坐标值，确定所述二维点云数据中每一个点的三维坐标，得到所述三维点云数据。

7.根据权利要求1所述的料堆边缘检测方法，其特征在于，所述对所述三维点云数据进行预处理，得到处理后的三维点云数据，具体包括：

筛选所述三维点云数据中处于限定坐标范围内的点，得到筛选后的三维点云数据；所述限定坐标范围为三维坐标值中第一坐标值处于第一限定范围内且第二坐标值处于第二限定范围内；

剔除所述筛选后的三维点云数据中的伪影点，得到剔除后的三维点云数据；

采用统计滤波器，剔除所述剔除后的三维点云数据中的异常点，得到处理后的三维点云数据。

8.根据权利要求1所述的料堆边缘检测方法，其特征在于，所述对当前帧对应的处理后的三维点云数据进行平面拟合，得到当前帧对应的拟合平面，具体包括：

利用最小二乘法，对当前帧对应的处理后的三维点云数据进行平面法向量求解，得到当前帧对应的处理后的三维点云数据对应的平面法向量；

根据所述平面法向量，确定所述当前帧对应的处理后的三维点云数据对应的拟合平面。

9.根据权利要求8所述的料堆边缘检测方法，其特征在于，所述计算当前帧对应的物料面倾角，具体包括：

计算所述平面法向量在XOZ坐标平面上的投影，得到二维向量；所述XOZ坐标平面为所述第一坐标轴和所述第二坐标轴所在的平面；

根据所述二维向量与所述第一坐标轴之间的夹角，通过补角计算，得到所述拟合平面与所述第一坐标轴之间的夹角；

结合取料机悬臂的仰角和雷达与水平面之间的夹角，对所述拟合平面与所述第一坐标轴之间的夹角进行修正，得到所述拟合平面与水平面之间的夹角。

10.一种料堆边缘检测系统，其特征在于，包括：

二维点云数据获取模块，用于获取雷达探测的料堆切削面的二维点云数据；所述雷达固定于取料机悬臂的侧边，且位于斗轮的侧边；所述雷达用于扫描所述取料机悬臂前方的料堆切削面，所述雷达的扫描面为竖直平面；

三维点云数据扩展模块，用于根据所述取料机悬臂的回转方向，将所述二维点云数据扩展为三维点云数据；

预处理模块，用于对所述三维点云数据进行预处理，得到处理后的三维点云数据；

平面拟合模块，用于对当前帧对应的处理后的三维点云数据进行平面拟合，得到当前帧对应的拟合平面；

物料面倾角计算模块，用于计算当前帧对应的物料面倾角；所述当前帧对应的物料面倾角为当前帧对应的拟合平面与水平面之间的夹角；

判断模块，用于判断当前帧对应的物料面倾角是否小于物料安息角；

当前帧雷达的扫描面处于物料堆垛边缘的外侧确定模块，用于当当前帧对应的物料面倾角小于物料安息角时，确定当前帧雷达的扫描面处于物料堆垛边缘的外侧；

当前帧雷达的扫描面处于物料堆垛边缘的内侧确定模块，用于当当前帧对应的物料面倾角不小于物料安息角时，确定当前帧雷达的扫描面处于物料堆垛边缘的内侧。

Images