一种料堆残垛层高计算方法及系统与流程

1.本发明涉及计算机视觉检测及运算领域，特别是涉及一种料堆残垛层高计算方法及系统。


背景技术：

2.随着科技的发展，新型检测技术不断融入传统工业设备，料场管理也逐渐要求信息化和精细化。目前已有个别港口散料堆场引入数字化堆场技术，其堆取料机通过料堆三维模型、卫星定位、雷达系统等实现无人化作业。主流的数字化堆场解决方案仍是通过激光雷达进行三维点云的模型重建，在取料过程中通过取料机姿态感知对取过的料堆点云模型进行删减。但是和堆场面积相比激光扫描范围很小，堆场扫描点云更新较慢，当料堆取料后或者有塌垛现象发生时，已有料场点云模型会出现误差。对取料机自动化运行影响很大，比如取料量达不到策略计算要求，甚至由于点云模型和实际料堆误差较大引起取料机碰撞残垛取料平台和取不到散料的现象。因此，在无人化堆场对料堆残垛二次取料前需要对目标料堆进行扫描，以此计算切削面层高、修正已有料场点云模型以及制定二次取料策略。
3.因此，目前亟需一种料堆残垛层高计算方法，以解决残垛取料前或者点云模型生成时存在的误差造成的取料过程取料机碰撞料堆残垛或者斗轮无法取到散料的现象。


技术实现要素：

4.本发明的目的是提供一种料堆残垛层高计算方法及系统，通过在残垛二次取料前加入的预扫描过程，结合算法自动识别料堆残垛扫描点云中平台平面以计算分层取料中切削面高度，获得准确可靠的层高数据，从而提高料堆残垛层高的计算精度，进而提升取料效率，解决残垛取料前或者点云模型生成时存在的误差造成的取料过程取料机碰撞料堆残垛或者斗轮无法取到散料的现象。
5.为实现上述目的，本发明提供了如下方案：
6.一种料堆残垛层高计算方法，所述方法包括：
7.获取取料机预扫描过程的雷达探测料堆的二维点云数据；
8.根据取料机悬臂的回转角度，将所述二维点云数据转化为三维点云数据；
9.对所述三维点云数据进行预处理，得到处理后的三维点云数据；
10.对所述处理后的三维点云数据进行ransac算法平面识别提取处理，得到当前残垛点云的取料平台平面；
11.计算所述取料平台平面中点云集合的z轴坐标均值，并将所述z轴坐标均值标记为取料平台高度值；
12.判断所述取料平台平面的数量与残垛实际分层数量是否相等；
13.当所述取料平台平面的数量与所述残垛实际分层数量不相等时，则返回至“对所述处理后的三维点云数据进行ransac算法平面识别提取处理，得到当前残垛点云的取料平台平面”的步骤；
14.当所述取料平台平面的数量与所述残垛实际分层数量相等时，则计算各个所述取料平台平面高度值之间的高度差，所述高度差为料堆残垛层高。
15.可选的，所述获取取料机预扫描过程的雷达探测料堆的二维点云数据，具体包括：
16.在所述预扫描过程中，当取料机悬臂的回转方向为逆时针旋转时，获取第一雷达探测的料堆切削面的二维点云数据；所述雷达包括第一雷达和第二雷达，所述第一雷达固定安装于所述取料机悬臂的右侧；
17.当所述取料机悬臂的回转方向为顺时针旋转时，获取第二雷达探测的料堆切削面的二维点云数据；所述第二雷达固定安装于所述取料机悬臂的左侧；
18.所述预扫描过程指对目标料堆进行取料前，根据已有点云模型指示所述取料机行进至指定位置，确定取料起始坐标点后，所述取料机的悬臂回转至预定位置。
19.可选的，所述根据所述取料机悬臂的回转角度，将所述二维点云数据转化为三维点云数据，具体包括：
20.建立雷达的二维直角坐标系xoy；其中，所述二维直角坐标系的原点(0,0)为所述雷达的中心，所述二维直角坐标系的第一坐标轴与雷达平面垂直，所述二维直角坐标系的第二坐标轴与所述第一坐标轴垂直，并与雷达扫描面、雷达平面均共面，所述雷达扫描面为所述取料机悬臂回转过程中，所述雷达沿着回转轨迹移动时扫描的平面；所述二维直角坐标系的第一坐标轴为x轴或者y轴，所述二维直角坐标系的第二坐标轴则对应为y轴或者x轴；
21.确定所述二维点云数据(x,y)在所述二维直角坐标系中的二维坐标；
22.根据所述取料机悬臂的回转方向，利用堆场平面的短边方向和长边方向构建三维直角坐标系xyz；其中，所述三维直角坐标系的原点(0,0,0)为所述取料机悬臂开始回转时所述雷达的中心，所述三维直角坐标系的第一坐标轴为所述堆场长方形平面短边方向，所述三维直角坐标系的第二坐标轴为所述堆场平面长方形长边方向；所述三维直角坐标系的第一坐标轴为x轴或者y轴，所述三维直角坐标系的第二坐标轴则对应为y轴或者x轴，所述三维直角坐标系的z轴为垂直于所述三维直角坐标系的第一坐标轴和第二坐标轴的坐标轴；所述堆场平面为所述取料机进行取料操作且呈长方形的堆场所在的平面；
23.根据所述取料机悬臂的回转角度δ，确定所述二维点云数据(x,y)中每一个点在所述三维直角坐标系上的坐标值(x,y,z)，得到所述三维点云数据。
24.可选的，所述根据所述取料机悬臂的回转角度δ，确定所述二维点云数据(x,y)中每一个点在所述三维直角坐标系上的坐标值(x,y,z)，得到所述三维点云数据，具体包括：
25.计算每一个所述二维点云数据(x,y)在所述三维直角坐标系上的坐标值(x,y,z)：
[0026][0027]
其中，x和y分别表示所述二维点云数据在所述二维直角坐标系的x轴和y轴的坐标值，x、y和z分别表示所述二维点云数据在所述三维直角坐标系的x轴、y轴和z轴的坐标值，δ表示所述取料机悬臂的回转角度，从所述取料机的opc_client中直接读取获得。
[0028]
可选的，所述对所述三维点云数据进行预处理，得到处理后的三维点云数据，具体包括：
[0029]
筛选所述三维点云数据中处于限定坐标范围内的点，得到筛选后的三维点云数据；所述限定坐标范围为三维坐标值中第一坐标值处于第一限定范围内且第二坐标值处于第二限定范围内；
[0030]
剔除所述筛选后的三维点云数据中的伪影点，得到剔除后的三维点云数据；
[0031]
采用统计滤波器，过滤所述剔除后的三维点云数据中的异常点，得到所述处理后的三维点云数据。
[0032]
可选的，所述对所述处理后的三维点云数据进行ransac算法平面识别提取处理，得到当前残垛点云的取料平台平面，具体包括：
[0033]
对所述处理后的三维点云数据进行ransac平面拟合求解，将所述处理后的三维点云数据形成的三维点云集合记为pcd，并在三维点云集合pcd中随机抽取n组样本点，n等于所述三维点云集合pcd中点云数量的三分之一并取整后的整数，每组样本点包含三个采样点；
[0034]
根据每组所述样本点中的三个所述采样点，确定每组所述样本点所在的唯一平面，得到n个所述唯一平面；
[0035]
利用求解平面法向量的方法，对n个所述唯一平面的平面方程进行求解；具体包括：假设三维点云数据的求解平面方程为ax
n
+by
n
+z
n
＝
‑
c，(x
n
，y
n
，z
n
)表示得到的三维点云数据中各点的坐标，ξ＝(a,b,1,c)表示三个所述采样点所确定的平面的法向量，为计算将平面法向量方程组用矩阵表示即：
[0036][0037]
其中，x
n
，y
n
，z
n
表示三维点云数据中三个采样点的三维坐标，n＝1、2或3；a，b，1以及c表示三个采样点所确定的平面的法向量；
[0038]
对未知数(a,b,c)进行求解，得到拟合平面的法向量ξ＝(a,b,1,c)；
[0039]
根据随机选择的n组样本点得到n组平面参数，假设所述n组平面参数的集合为ω
n
＝(ζ1,ζ2…
ζ
n
)，计算所述三维点云集合内所有点到平面1的距离；所述平面1为n个所述唯一平面中的任意一个唯一平面；
[0040]
根据雷达精度将距离阈值设置为γ＝0.4m，将所述三维点云集合中所有与所述平面1距离0.4m以内的点作为料堆残垛平台内点；
[0041]
计算所述n组平面参数中每一组平面参数包含所述料堆残垛平台内点的数量，并按照所述料堆残垛平台内点的数量，对n个所述唯一平面进行递减排序，得到排序队列；
[0042]
将所述排序队列中首个唯一平面作为所述当前残垛点云的取料平台平面。
[0043]
可选的，所述计算所述取料平台平面中点云集合的z轴坐标均值，并将所述z轴坐标均值标记为取料平台高度值，具体包括：
[0044]
采集所述取料平台平面中点云集合中所有点云子集的z轴坐标值；
[0045]
根据所述点云子集的z轴坐标值总和以及所述点云子集的数量，计算得到所述点云子集的z轴坐标平均值；
[0046]
将所述z轴坐标平均值标记作为所述取料平台高度值。
[0047]
可选的，所述判断所述取料平台平面的数量与残垛实际分层数量是否相等，具体包括：
[0048]
将所述取料平台平面的数量，与所述残垛实际分层数量进行大小比较，判断所述取料平台平面的数量是否与所述残垛实际分层数量相等；
[0049]
当所述取料平台平面的数量与所述残垛实际分层数量相等时，则执行“计算各个所述取料平台平面高度值之间的高度差，所述高度差为料堆残垛层高”的步骤，计算所述高度差；
[0050]
当所述取料平台平面的数量与所述残垛实际分层数量不相等时，则返回至“对所述处理后的三维点云数据进行ransac算法平面识别提取处理，得到当前残垛点云的取料平台平面”的步骤，继续获取下一个所述取料平台平面，直至所述取料平台平面的数量与所述残垛实际分层数量相等。
[0051]
可选的，所述计算各个所述取料平台平面高度值之间的高度差，所述高度差为料堆残垛层高，具体包括：
[0052]
将提取到的n个所述取料平台平面分别记为平面1～平面n；
[0053]
将计算得到的平面1～平面n的z轴坐标均值分别记为h1～h
n
；
[0054]
根据h1～h
n
的大小关系，将平面1～平面n分别定义为自上而下的第一层、第二层、第三层至第n层的平台平面；
[0055]
计算第一层取料平台平面和第二层取料平台平面的层高c_high1＝h1‑
h2，计算第二层取料平台平面和第三层取料平台平面的层高c_high2＝h2‑
h3，直至计算第n
‑
1层取料平台平面和第n层取料平台平面的层高c_high
n
‑1＝h
n
‑1‑
h
n
；
[0056]
其中，h
n
表示平面n中点云集合的z轴坐标均值；h
n
‑1表示平面n
‑
1中点云集合的z轴坐标均值；c_high
n
‑1表示料堆残垛相邻两层平面的层高。
[0057]
本发明还提出了一种料堆残垛层高计算系统，所述系统包括：
[0058]
二维点云数据获取模块，用于获取取料机预扫描过程的雷达探测料堆的二维点云数据；
[0059]
三维点云数据转化模块，用于根据取料机悬臂的回转角度，将所述二维点云数据转化为三维点云数据；
[0060]
三维点云数据预处理模块，用于对所述三维点云数据进行预处理，得到处理后的三维点云数据；
[0061]
ransac平面提取模块，用于对所述处理后的三维点云数据进行ransac算法平面识别提取处理，得到当前残垛点云的取料平台平面；
[0062]
取料平台高度计算模块，用于计算所述取料平台平面中点云集合的z轴坐标均值，并将所述z轴坐标均值标记为取料平台高度值；
[0063]
数量判断模块，用于判断所述取料平台平面的数量与残垛实际分层数量是否相等；当所述取料平台平面的数量与所述残垛实际分层数量不相等时，则返回至“ransac平面提取模块对所述处理后的三维点云数据进行ransac算法平面识别提取处理，得到当前残垛
点云的取料平台平面”的步骤；
[0064]
高度差计算模块，用于当所述取料平台平面的数量与所述残垛实际分层数量相等时，则计算各个所述取料平台平面高度值之间的高度差，所述高度差为料堆残垛层高。
[0065]
根据本发明提供的具体实施例，本发明公开了以下技术效果：
[0066]
本发明基于离散点云随机采样一致性(ransac)原理，对散杂货取料机自动取料过程中进行料堆预扫描，能够在雷达仅为二维扫描面的情况下根据雷达位置轨迹变化情况，计算出扫描过程的三维点云数据。进而通过直接分析拼接后的三维点云数据能够提高数据信息量，还能作为异常点剔除的依据，可以克服雷达伪影多的缺点，通过平面自动识别提取计算出料堆残垛层高，提高了计算料堆残垛层高的准确性、可靠性。
[0067]
本发明从采用的设备角度出发，采用的雷达探测具有全天时、全天候的优点，无需考虑传统光学设备检测方法涉及的摄像头或激光探头、激光雷达的光学镜面污损的问题，避免了由于散杂料堆场的恶劣环境，清洁和维护光学设备将耗费大量人力的问题，并且防止了可能会在取料工作过程中由于堆场点云数据更新较慢，取料前料堆三维点云模型误差较大，导致中断作业等问题，通过提高料堆数字模型准确性，有效提高了料堆残垛二次取料的效率。
附图说明
[0068]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0069]
图1为本发明实施例1提出的一种料堆残垛层高计算方法的流程示意图；
[0070]
图2为本发明实施例1中雷达安装位置示意图；
[0071]
图3为本发明实施例1中雷达扫描数据距离和雷达扫描方位角示意图；
[0072]
图4为本发明实施例1中雷达二维直角坐标系xoy和堆场三维直角坐标系xyz的关系示意图；
[0073]
图5为本发明实施例1中雷达扫描二维点云数据示意图；
[0074]
图6为本发明实施例1中剔除异常点前后的三维点云数据示意图；其中，图6(a)为剔除异常点前的三维点云数据示意图，图6(b)为剔除异常点后的三维点云数据示意图；
[0075]
图7为本发明实施例1中提取出的第一、二层料堆残垛取料平面点云示意图；其中，图7(a)为第一层料堆残垛取料平面点云示意图，图7(b)为第二层料堆残垛取料平面点云示意图；
[0076]
图8为本发明实施例1中原始点云提取后剩余点云数据的示意图；
[0077]
图9为本发明实施例1中算法输出的平台数量和层高数据的示意图。
[0078]
图10为本发明实施例2提出的一种料堆残垛层高计算系统的结构框图。
[0079]
附图标记：
[0080]1‑
雷达；101
‑
第一雷达；102
‑
第二雷达；2
‑
取料机悬臂；3
‑
料堆；4
‑
二维点云数据获取模块；5
‑
三维点云数据转化模块；6
‑
三维点云数据预处理模块；7
‑
ransac平面提取模块；8
‑
取料平台高度计算模块；9
‑
数量判断模块；10
‑
高度差计算模块。
具体实施方式
[0081]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0082]
本发明的目的是提供一种料堆残垛层高计算方法及系统，通过在残垛二次取料前加入的预扫描过程，结合算法自动识别料堆残垛扫描点云中平台平面以计算分层取料中切削面高度，获得准确可靠的层高数据，从而提高料堆残垛层高的计算精度，进而提升取料效率，解决残垛取料前或者点云模型生成时存在的误差造成的取料过程取料机碰撞料堆残垛或者斗轮无法取到散料的现象。
[0083]
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
[0084]
实施例1
[0085]
如图1所示，本发明提出了一种料堆残垛层高计算方法，所述方法包括：
[0086]
s1、获取取料机预扫描过程的雷达1探测料堆3的二维点云数据。
[0087]
取料机，也叫斗轮堆取料机，是指一种用于大型干散货堆场的既能堆料又能取料的连续输送的高效装卸机械。由可俯仰和水平摆动且具有输送带的悬臂及其前端的斗轮、机架、运行机构组成，输送带可双向运行，取料时由斗轮取料经悬臂的输送带送出，堆料时则由主输送机运来的货物经由输送带投向堆场。
[0088]
所述取料机的预扫描过程，是指取料机通过雷达1进行预扫描，在对目标料堆3进行取料前，根据雷达1扫描的已有点云模型指示所述取料机行进至指定位置，确定取料起始坐标点后，所述取料机悬臂2回转至预定位置。由于当前料堆3点云模型并非实时更新，为精准无人化取料，本实施例将取料机悬臂2回转15
°
以上，对目标垛和目标点进行扫描探测，以获取实时的、准确的目标料堆3点云模型。
[0089]
图2为本发明实施例1中雷达1安装位置示意图。所述雷达1包括第一雷达101和第二雷达102，所述第一雷达101和所述第二雷达102分别对称安装于所述取料机悬臂2的两侧。所述第一雷达101固定安装于所述取料机悬臂2的右侧，且位于斗轮的右侧边，且距离悬臂回转中心40米处的位置。所述第二雷达102固定安装于所述取料机悬臂2的左侧，且位于斗轮的左侧边，且距离悬臂回转中心40米处的位置。
[0090]
在所述预扫描过程中，以面向料堆3的方向为基准，当取料机悬臂2的回转方向为逆时针旋转时，获取所述第一雷达101探测的料堆3切削面的二维点云数据。当所述取料机悬臂2的回转方向为顺时针旋转时，获取所述第二雷达102探测的料堆3切削面的二维点云数据。本实施例中，所述雷达1用于扫描所述取料机悬臂2前方的残垛料堆3切削面，所述雷达1的扫描面为竖直平面。本发明采用毫米波雷达1作为探测装置，取料过程中雷达1的探测面与斗轮的侧面平行。
[0091]
图3为本发明实施例1中雷达1扫描数据距离和雷达1扫描方位角示意图。本实施例中，通过取料机悬臂2左右两侧搭载的毫米波雷达1作为探测装置，取料过程中雷达1探测平面与斗轮圆形平面平行，右探测面向斗轮平面右侧平移4.74米，左探测面向斗轮平面左侧平移4.08米，当根据当前数字堆场模型走行至预扫描位置时，执行预扫描策略，取料机悬臂
2自料堆3残垛一侧边缘朝残垛中心方向回转，拼接后即得到料堆3残垛的三维点云。如取料机在取料过程中向左侧(即逆时针)回转时，以所述第一雷达101返回的数据作为判断依据；向右侧(即顺时针)回转时以所述第二雷达102返回的数据作为判断依据。本实施例中所使用的毫米波雷达1的扫描视场角为在感应距离0
‑
70米范围内
±
60
°
(即共120
°
)扇形区域和波束垂直方向视场角
±
10
°
扇形区域，由于毫米波雷达1特性，其不具备分辨波束垂直方向视场角的能力，所以得到数据为被测物相对雷达1的距离和方位角a。将毫米波雷达1竖直放置，即雷达1平面为竖直方向的平面。根据波束扫描特点、设备性能指标可知，雷达1性能参数为每帧(扫描时间72毫秒)扫描过程为以水平面方向自雷达1自身+60
°
向上扫描至
‑
60
°
位置，即雷达1中心点的垂直射线为0
°
，得到扫描原始数据，为方便运算需要转换到平面直角坐标系，即二维直角坐标系。
[0092]
根据已有料堆3和取料机粗略相对位置关系制定预扫描策略，预扫描策略制定需根据雷达1安装位置和雷达1性能计算扫描范围，预扫描策略需要指定雷达1和目标残垛第一层平台之间的最近距离d
s
和悬臂回转角度r
s
。以某15米高锥形煤炭散料堆3为例，通过本实施例的雷达1进行扫描探测，其探测范围是
±
60
°
，但由于其搭载于悬臂取料机斗轮两侧，扫描0
°
延伸线距堆场平面为5米，则雷达1扫描0
°
延伸线上方至少需要扫描10米(15m
‑
5m＝10m)。
[0093]
根据三角函数计算距离则此时d
s
必须大于或等于11.55米。当计算回转角度r
s
时，由于雷达1回转是以悬臂回转中心为圆点进行旋转，故需在d
s
基础上增加悬臂回转中心到雷达1的距离40米，得到悬臂回转中心和残垛第一层平台最近点的距离计算回转角度对应的弧长。在悬臂回转空间允许的情况下，预扫描回转角越大，所获得的点云信息也更加丰富，但是当回转范围受到残垛周围空间限制时则至少需要扫描到弧长为10米以上，这样的点云数据有利于提高残垛平台识别的准确率。根据弧长公式计算弧长可得到预扫描回转角度最小值计算公式可计算在d
s
＝11.55米时，r
s
最小角为11
°7′
。显然，考虑到现场工况复杂，指定预扫描策略时需考虑增加一定冗余。因此，本实施例中，优选固定的d
s
＝20m、r
s
＝15
°
的参数执行预扫描过程，探测料堆3残垛三维点云数据。
[0094]
本实施例采用的雷达1探测具有全天时、全天候的优点，无需考虑传统光学设备检测方法涉及的摄像头或激光探头、激光雷达的光学镜面污损的问题，避免了由于散杂料堆场的恶劣环境，清洁和维护光学设备将耗费大量人力的问题，并且防止了可能会在取料工作过程中由于堆场点云数据更新较慢，取料前料堆3三维点云模型误差较大，导致中断作业等问题，通过提高料堆3数字模型准确性，有效提高了料堆3残垛二次取料的效率。
[0095]
需要注意的是，本实施例至少在取料机悬臂2的两侧各安装一个雷达1，由于取料机悬臂2左右两侧结构略有差异，故需要根据实际情况将雷达1安装于前侧无任何遮挡物的位置。并且，本实施例中雷达1的具体安装位置、雷达1和目标残垛第一层平台之间的最近距离d
s
和悬臂回转角度r
s
是优选的一种方案，本实施例不对其进行具体限定，只要保证两个雷达1对称安装于取料机悬臂2和斗轮的两侧，并可实现对料堆3扫描即可。因此，雷达1与悬臂回转中心的距离、d
s
和r
s
的数值并不是固定不变的，其他各种安装位置、安装方式以及其他
数值均可根据实际情况自行确定，都应该在本发明的保护范围之内。
[0096]
s2、根据取料机悬臂2的回转角度，将所述二维点云数据转化为三维点云数据。本实施例根据取料机悬臂2在堆场坐标系下的绝对回转角，将毫米波雷达1扫描的二维平面离散点扩展为三维点云数据，即转化成为一个三维坐标系，扫描平面随取料机悬臂2安装的雷达1运动轨迹构成的三维点云数据。具体包括：
[0097]
s21、建立雷达1的二维直角坐标系xoy。
[0098]
其中，所述二维直角坐标系的原点(0,0)为所述雷达1的中心，所述二维直角坐标系的第一坐标轴与雷达1平面垂直，所述二维直角坐标系的第二坐标轴与所述第一坐标轴垂直，并与雷达1扫描面、雷达1平面均共面，所述雷达1扫描面为所述取料机悬臂2回转过程中，所述雷达1沿着回转轨迹移动时扫描的平面；所述二维直角坐标系的第一坐标轴为x轴或者y轴，所述二维直角坐标系的第二坐标轴则对应为y轴或者x轴，第二坐标轴为由雷达1中心指向上方的方向。也就是说，本实施例以垂直雷达1平面，自雷达1中心指向远端的射线为坐标系横轴正向；自雷达1中心引出在雷达1扫描平面中垂直横轴的射线为雷达1局部坐标系纵轴，向上的方向为纵轴的正向。
[0099]
s22、确定所述二维点云数据(x,y)在所述二维直角坐标系中的二维坐标。
[0100]
s23、根据所述取料机悬臂2的回转方向，利用堆场平面的短边方向和长边方向构建三维直角坐标系xyz。其中，所述三维直角坐标系的原点(0,0,0)为所述取料机悬臂2开始回转时所述雷达1的中心，所述三维直角坐标系的第一坐标轴为所述堆场长方形平面短边方向，所述三维直角坐标系的第二坐标轴为所述堆场平面长方形长边方向。当规定回转中心为三维直角坐标系原点时，根据现场取料机型号不同其回转中心高于地面的高度也不同，由于本实施例主要计算料堆残垛平台高度差，故能够以相对高度进行计算。所述三维直角坐标系的第一坐标轴为x轴或者y轴，所述三维直角坐标系的第二坐标轴则对应为y轴或者x轴，所述三维直角坐标系的z轴为垂直于所述三维直角坐标系的第一坐标轴和第二坐标轴的坐标轴；所述堆场平面为所述取料机进行取料操作且呈长方形的堆场所在的平面。
[0101]
应说明的是，本实施例建立三维直角坐标系xyz，即根据建立的二维直角坐标系，结合取料机悬臂2的回转角度δ，将雷达1二维点云数据转换为以取料机在坝基上回转中心为观测点的三维直角坐标系坐标。图4为本实施例1中雷达1二维直角坐标系xoy和堆场三维直角坐标系xyz的关系示意图。以料堆3堆场较短一侧的方向为x轴，较长一侧的方向为y轴，垂直于xoy面方向为z轴，以预扫描策略中悬臂回转中心为原点(0,0,0)。由于毫米波雷达1固定在取料机悬臂2和斗轮的两侧，跟随取料机悬臂2做同圆心不同半径的圆周运动，所以雷达1扫描面旋转角度与取料机悬臂2回转角度相等。以料堆3残垛预扫描过程初始位置的雷达1中心为原点(0,0,0)，x轴和y轴分别是此时长方形料场坐标系短边方向和长边方向，xoy平面即表示堆场平面，z轴为自原点垂直xoy平面的射线。进而可以根据二维点云数据和规定以取料机回转中心作为观测点后，通过公式(1)依次得到预扫描过程的三维点云数据。如图5所示，图5为本发明实施例1中雷达1扫描二维点云数据示意图。
[0102]
s24、根据所述取料机悬臂2的回转角度δ，确定所述二维点云数据(x,y)中每一个点在所述三维直角坐标系上的坐标值(x,y,z)，得到所述三维点云数据。具体包括：
[0103]
根据以下公式计算每一个所述二维点云数据(x,y)在所述三维直角坐标系上的坐标值(x,y,z)：
[0104][0105]
其中，x和y分别表示所述二维点云数据在所述二维直角坐标系的x轴和y轴的坐标值，x、y和z分别表示所述二维点云数据在所述三维直角坐标系的x轴、y轴和z轴的坐标值，δ表示所述取料机悬臂2的回转角度，从所述取料机的opc_client中直接读取获得。
[0106]
本实施例首先建立描述雷达1每帧扫描平面的雷达1局部坐标系即二维直角坐标系xoy，然后构建用以描述拼接悬臂回转过程中点云数据的三维直角坐标系xyz，进而根据取料机悬臂2回转角度将毫米波雷达1扫描的二维平面离散点扩展为三维点云数据，即成为堆场坐标系即三维坐标系下，扫描平面随雷达1运动轨迹构成的三维点云数据。
[0107]
s3、对所述三维点云数据进行预处理，得到处理后的三维点云数据。具体包括：
[0108]
s31、筛选所述三维点云数据中处于限定坐标范围内的点，得到筛选后的三维点云数据。所述限定坐标范围为三维坐标值中第一坐标值处于第一限定范围内，且第二坐标值处于第二限定范围内。以第一坐标轴为x轴，第二坐标轴为y轴为例，第一限定范围可以为0～25米，第二限定范围也可以为0～25米。本实施例中，首先限制点云数据x轴和z轴的坐标范围。通过模拟信号中滤波的限带滤波思想与取料机取料实际工作状况结合，为避免待取料堆3周围料堆3或堆场内其他结构的影响，将点云数据限制在以雷达1中心为远点的局部坐标系中纵深距离25米内的范围，此时形成了顶角120
°
、底角30
°
和腰长50米、底边长86米等腰三角形的平面视场。
[0109]
s32、剔除所述筛选后的三维点云数据中的伪影点，得到剔除后的三维点云数据。具体的，通过雷达1虚警概率、多普勒状态、集群不确定状态和雷达1散射截面等信号质量信息数据去掉扫描中的不可靠点，即伪影点。通过所用雷达1对每个点反馈的质量信息进行设置阈值过滤，当虚假概率大于25％即认定为伪影点，当多普勒效应状态显示模糊时、雷达1难以分辨，则认定此处为伪影点，当集群点在雷达1中无法识别为运动、静止等状态时认定为伪影点，当雷达1散射截面(rcs值)小于
‑
20db时认定为伪影点。
[0110]
s33、采用统计滤波器，过滤所述剔除后的三维点云数据中的异常点，得到所述处理后的三维点云数据。本实施例采用统计滤波器，利用统计滤波器的滤波作用，对所述剔除后的三维点云数据中的异常点进行过滤，去除噪声后，得到所述处理后的三维点云数据。具体的：
[0111]
在得到料场三维直角坐标系下料堆3残垛三维点云模型后，根据统计滤波原理，计算点云数据每个点到除自身外所有点的三维欧式距离公式为：
[0112][0113]
其中，d
i
表示点云中任一点到其他所有点的欧式距离，(x，y，z)表示点云数据中任一点的三维坐标，(x
i
，y
i
，z
i
)表示点云中除当前自身点外的某一点的三维坐标。按一定比例选择出“邻近点”，本实施例将邻近点定义为其他点与自身欧式距离最小的30个点，再求每个点到自身邻近点的平均距离d
i
：
[0114][0115]
其中，d
i
表示点云中任一点到其他所有点的欧式距离，d
i
表示每个点到自身邻近点的平均距离。
[0116]
需要注意的是，这时求点云数据中全局邻近点的平均距离d
m
，平均距离d
i
求和再除以点云数量，最后以全局邻近点平均距离标准差d
ε
为阈值(根据实际情况可选择放大或缩小阈值)，本实施例经过多次试验效果比对选择1倍标准差作为阈值；具体为当点云中共有n个点时，遍历n个点和其他(n
‑
1)个点之间的欧式距离d
i
；对每个点有(n
‑
1)个欧式距离，距离最小的30个点即为该点的“邻近点”，求出邻近点平均距离d
i
；计算全局邻近点的平均距离d
m
，公式如下：
[0117][0118]
其中，d
m
表示全局邻近点的平均距离，d
i
表示每个点到自身邻近点的平均距离，n表示点云中点的数量。
[0119]
然后计算全局邻近点平均距离标准差d
ε
，公式如下：
[0120][0121]
其中，d
ε
表示全局邻近点平均距离标准差，d
m
表示全局邻近点的平均距离，d
i
表示每个点到自身邻近点的平均距离，n表示点云中点的数量。
[0122]
最后将每个点到自身邻近点的平均距离d
i
减去全局邻近点的平均距离d
m
再与设定的阈值d
ε
进行比较，超过阈值的点则被定义为离群点即异常点，在点云集合中剔除异常点，即当公式中δ
i
大于0时定义第i点为异常点：
[0123]
δ
i
＝d
i
‑
d
m
‑
d
ε
[0124]
其中，d
ε
表示全局邻近点平均距离标准差，d
m
表示全局邻近点的平均距离，d
i
表示每个点到自身邻近点的平均距离，δ
i
表示每个点到自身邻近点的平均距离d
i
与全局邻近点的平均距离d
m
求差后再与设定的阈值d
ε
进行比较得到的比较结果。
[0125]
图6为本实施例1中剔除异常点前后的三维点云数据示意图，其中，图6(a)为剔除异常点前的三维点云数据示意图，图6(b)为剔除异常点后的三维点云数据示意图。剔除前的原始点云的数量有14509个，经过预处理后，最终剩余的点云数量有11968个，记此点云数据集为p。
[0126]
s4、对所述处理后的三维点云数据进行ransac算法平面识别提取处理，得到当前残垛点云的取料平台平面。具体包括：
[0127]
s41、对所述处理后的三维点云数据进行ransac平面拟合求解。即对当前点云进行平行于地面的平面拟合，得到距离阈值内包含点最多的平面并判断为料堆3残垛其中一层的平台面，并记为平面1。然后将判断为料堆3残垛平台面点云删去，重复进行该平面拟合的步骤，根据料场现有残垛层数确定拟合次数，如以料堆3残垛使用三层分取料法为例，则进
行三个平面拟合，依次记为平面2和平面3。具体的：
[0128]
采用ransac算法，将所述处理后的三维点云数据形成的三维点云集合记为pcd，并在三维点云集合pcd中随机抽取n组样本点，n等于所述三维点云集合pcd中点云数量的三分之一并取整后的整数，每组样本点包含三个采样点；
[0129]
根据每组所述样本点中的三个所述采样点，确定每组所述样本点所在的唯一平面，得到n个所述唯一平面；
[0130]
利用求解平面法向量的方法，对n个所述唯一平面的平面方程进行求解。由于所求平面平行于堆场坐标系中xoy平面，故利用点法式方程描述平面参数可将z轴参数设为1，假设三维点云数据的求解平面方程为ax
n
+by
n
+z
n
＝
‑
c，(x
n
，y
n
，z
n
)表示得到的三维点云数据中各点的坐标，n＝1、2或3，ξ＝(a,b,1,c)表示三个所述采样点所确定的平面的法向量，为计算将平面法向量方程组用矩阵表示即：
[0131][0132]
其中，x
n
，y
n
，z
n
表示三维点云数据中三个采样点的三维坐标，n＝1、2或3；a，b，1以及c表示三个采样点所确定的平面的法向量。
[0133]
对未知数(a,b,c)进行求解，得到拟合平面的法向量ξ＝(a,b,1,c)。
[0134]
根据随机选择的n组样本点得到n组平面参数，假设所述n组平面参数的集合为ω
n
＝(ζ1,ζ2…
ζ
n
)，计算所述三维点云集合内所有点到平面1的距离；所述平面1为n个所述唯一平面中的任意一个唯一平面。根据所采用的雷达1扫描精度，将点云中到平面1距离为0.4米以内均标记为平面1点云内点，提取被识别平面内点并在原集合中删去，计算所包含内点最多的平面。显然，包含内点最多的必是三维点云数据中料堆3残垛平面，记此平面子集为平面1。如图7所示，图7为本发明实施例1中提取出的第一、二层料堆3残垛取料平面点云示意图；图7(a)为第一层料堆3残垛取料平面点云示意图，图7(b)为第二层料堆3残垛取料平面点云示意图，第一层料堆3残垛取料平面的点云数量有4679个，第二层料堆3残垛取料平面的点云数量有1790个。
[0135]
s42、根据雷达1精度将距离阈值设置为γ＝0.4m，将所述三维点云集合中所有与所述平面1距离0.4m以内的点作为料堆3残垛平台内点。
[0136]
s43、计算所述n组平面参数中每一组平面参数包含所述料堆3残垛平台内点的数量，并按照所述料堆3残垛平台内点的数量，对n个所述唯一平面进行递减排序，得到排序队列。
[0137]
s44、将所述排序队列中首个唯一平面作为所述当前残垛点云的取料平台平面。
[0138]
本实施例采用随机采样一致性(ransac)方法，在三维点云数据中随机采样n组样本点，并令n等于扫描点云数量的三分之一，且每组包括从所有点中抽取的三个随机点，根据“三个点确定一个平面“的原理，确定唯一平面能够求解平面的法向量参数，参数选择能够有效采样到百分之九十以上的点云数据，有利于提高料堆3残垛平台识别准确度。然后再计算点集内点到平面的距离，选择出n组数据中距离平面
±
0.4米以内数量最多的即为残垛平台平面点云。采用随机采样一致性方法(ransac)拟合残垛取料平台，自动识别不同层取
料平台点云数据后，根据z轴坐标平均值求得每层平台高度，根据每层之间高度差值得到层间高度差，实现残垛层高检测并计算的要求。
[0139]
本发明基于离散点云随机采样一致性(ransac)原理，对散杂货取料机自动取料过程中进行料堆3预扫描，能够在雷达1仅为二维扫描面的情况下根据雷达1位置轨迹变化情况，计算出扫描过程的三维点云数据。进而通过直接分析拼接后的三维点云数据能够提高数据信息量，还能作为异常点剔除的依据，可以克服雷达1伪影多的缺点，通过平面自动识别提取计算出料堆3残垛层高，提高了计算料堆3残垛层高的准确性、可靠性。
[0140]
s5、计算所述取料平台平面中点云集合的z轴坐标均值，并将所述z轴坐标均值标记为取料平台高度值。即在上一个步骤对三个平面拟合，得到平面1、平面2和平面3后，计算三个平面点云数据集合的z轴坐标平均值，记为当前平面高度，并根据比较平面1、平面2和平面3的平面高度判断为料堆3残垛自上向下的第一、二、三层平台。具体包括：
[0141]
s51、采集所述取料平台平面中点云集合中所有点云子集的z轴坐标值。
[0142]
s52、根据所述点云子集的z轴坐标值总和以及所述点云子集的数量，计算得到所述点云子集的z轴坐标平均值。
[0143]
s53、将所述z轴坐标平均值标记作为所述取料平台高度值。
[0144]
s6、判断所述取料平台平面的数量与残垛实际分层数量是否相等。本实施例将多次ransac识别提取的平面累计数量和残垛实际分层数量作为比较参数，通过料堆3自动化取料策略中所分层数作为参数，其中，所述残垛实际分层数量是料堆3残垛自身实际的分层数量，是堆料、取料时的已知数据。具体包括：
[0145]
s61、将所述取料平台平面的数量，与所述残垛实际分层数量进行大小比较，判断所述取料平台平面的数量是否与所述残垛实际分层数量相等。
[0146]
s62、当所述取料平台平面的数量与所述残垛实际分层数量相等时，则执行“计算各个所述取料平台平面高度值之间的高度差，所述高度差为料堆3残垛层高”的步骤，即继续执行步骤s7，计算所述高度差。
[0147]
s63、当所述取料平台平面的数量与所述残垛实际分层数量不相等时，则返回至“对所述处理后的三维点云数据进行ransac算法平面识别提取处理，得到当前残垛点云的取料平台平面”的步骤，即返回至步骤s4，继续获取下一个所述取料平台平面，直至所述取料平台平面的数量与所述残垛实际分层数量相等。即自原始点云集合p中删除步骤s4提取的平面1的内点，并重复步骤s4，提取下一平台平面点云并标记为平面2，以此类推，重复步骤s4可以提取到平面3、平面4
……
平面n。当所述取料平台平面的数量与所述残垛实际分层数量相等时停止重复步骤。
[0148]
本实施例的目标残垛采用分三层取料法，但已将第一层取净后停止，残垛剩余第二、三层平台，执行步骤s4共两次得到点云集合平面1和平面2，删去上述两部分后，原始点云p最终剩余的点云数据如图8所示。
[0149]
s7、当所述取料平台平面的数量与所述残垛实际分层数量相等时，则计算各个所述取料平台平面高度值之间的高度差，所述高度差为料堆3残垛层高。即根据比较平面1、平面2和平面3的平面高度判断为料堆3残垛自上向下的第一、二、三层平台后，将一平台高度减去二平台高度得到第一切削面层高，即层高一，用c_high1表示；将二平台高度减去三平台高度得到第二切削面层高，即层高二，用c_high2表示，以此类推。具体包括：
[0150]
s71、将提取到的n个所述取料平台平面分别记为平面1、平面2、平面3
……
平面n。
[0151]
s72、将计算得到的平面1～平面n的z轴坐标均值分别记为h1、h2、h3、
……
h
n
。
[0152]
s73、根据h1～h
n
的大小关系，将平面1～平面n分别定义为自上而下的第一层、第二层、第三层至第n层的平台平面。
[0153]
s74、计算第一层取料平台平面和第二层取料平台平面的层高c_high1＝h1‑
h2，计算第二层取料平台平面和第三层取料平台平面的层高c_high2＝h2‑
h3，直至计算第n
‑
1层取料平台平面和第n层取料平台平面的层高c_high
n
‑1＝h
n
‑1‑
h
n
。
[0154]
其中，h
n
表示平面n中点云集合的z轴坐标均值；h
n
‑1表示平面n
‑
1中点云集合的z轴坐标均值；c_high
n
‑1表示料堆3残垛相邻两层平面的层高。
[0155]
通过本发明的方法得到料堆3残垛取料平台总高度，以及料堆3残垛切削面各层高度，获得的结果如图9所示，可直接获得取料平台的层数和层高。本发明可为料堆3残垛二次取料策略选择提供依据，同时起到修正数字化堆场点云模型参数的作用。
[0156]
本实施例在数字化堆场的取料机无人化取料为应用背景，通过雷达1二维点云的计算拼接为三维料堆3点云，通过点云数据自动识别料堆3残垛平台，计算取料平台高度和取料切削面层高，针对料堆3残垛二次无人化取料提供实时、精确检测。
[0157]
利用本发明能够在雷达1二维扫描面的情况下根据雷达1运动轨迹变化情况，计算出扫描过程的三维点云数据。直接分析拼接后的三维点云数据既能够提高数据信息量，还能作为异常点剔除的依据，可以克服毫米波雷达1伪影多和分辨率低的缺点。从设备上，对比目前大部分采用光学设备作为感知手段的方法，毫米波雷达1探测具有全天时、全天候的优点，不再需要考虑如摄像头或激光探头、激光雷达1的光学镜面污损问题；避免了由于散杂料堆3场的恶劣环境，清洁和维护光学设备将耗费大量人力，并且防止了可能会在取料工作过程中由于污损，导致中断作业等问题；也避免了在无人化作业中，由于料场点云数学模型更新不及时带来的取料点位置误差和取料策略制定错误的问题。人工取料作业和半自动取料作业中由于人眼观察错误造成的问题。
[0158]
实施例2
[0159]
如图10所示，本发明实施例2提出了一种料堆3残垛层高计算系统，所述系统包括：
[0160]
二维点云数据获取模块4，用于获取取料机预扫描过程的雷达1探测料堆3的二维点云数据。
[0161]
三维点云数据转化模块5，用于根据取料机悬臂2的回转角度，将所述二维点云数据转化为三维点云数据。
[0162]
三维点云数据预处理模块6，用于对所述三维点云数据进行预处理，得到处理后的三维点云数据。
[0163]
ransac平面提取模块7，用于对所述处理后的三维点云数据进行ransac算法平面识别提取处理，得到当前残垛点云的取料平台平面。
[0164]
取料平台高度计算模块8，用于计算所述取料平台平面中点云集合的z轴坐标均值，并将所述z轴坐标均值标记为取料平台高度值。
[0165]
数量判断模块9，用于判断所述取料平台平面的数量与残垛实际分层数量是否相等；当所述取料平台平面的数量与所述残垛实际分层数量不相等时，则返回至“ransac平面提取模块7对所述处理后的三维点云数据进行ransac算法平面识别提取处理，得到当前残
垛点云的取料平台平面”的步骤。
[0166]
高度差计算模块10，用于当所述取料平台平面的数量与所述残垛实际分层数量相等时，则计算各个所述取料平台平面高度值之间的高度差，所述高度差为料堆3残垛层高。
[0167]
本发明提供了一种料堆残垛层高计算方法及系统，通过在残垛二次取料前加入的预扫描过程，结合算法自动识别料堆残垛扫描点云中平台平面以计算分层取料中切削面高度，获得准确可靠的层高数据，从而提高料堆残垛层高的计算精度，进而提升取料效率，解决残垛取料前或者点云模型生成时存在的误差造成的取料过程取料机碰撞料堆残垛或者斗轮无法取到散料的现象。
[0168]
本说明书中各个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。本说明书中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。