堆取料机空间防碰撞系统设计与实现

堆取料机空间防碰撞系统设计与实现李长安（神华黄骅港务有限责任公司 河北 沧州 061113）摘 要： 在国内外煤炭码头中，堆料机、取料机是散货堆场作业的核心设备，堆取料机空间防碰撞系统一直是个难题。

设计一种堆取料机空间防碰撞控制系统，其原理是在堆取料机上的大臂和回转中心安装gps流动站，通过gps的位置信息和空间几何算法，得出两个堆取料机之间的最小距离从而可以判断出堆取料机发生碰撞的可能性，使得工作人员进行相应处理，解决行走编码器不准确、误差累积的问题。

本系统可以实时计算出堆取料机大臂的相对位置和距离，可实现两台甚至多台堆取料机在同一个场垛中安全作业。

关键词： 堆取料机；RTK系统；基准站；流动站；处理单元中图分类号：TF31 文献标识码：A 文章编号：1671－7597（2012）1110067－03全作业，严重影响效率。

故当前一般都采用人工监控的方法来1 背景技术避免空间碰撞事故。

堆取料作业过程中，在一个堆场中经常需要使用多个堆取2 具体设计方案料机进行作业。

图1为堆取料机在堆场作业的示意图，10为堆场，20为堆取料机，30为行走轨道。

在一个堆场中可能有多个笔者设计了一种堆取料机空间防碰撞系统，该系统包括：堆取料机，沿着行走轨道进行行走作业，在行走过程中，多个大臂位置反馈系统、空间数据计算系统、空间防碰撞控制系堆取料机的大臂之间有可能会发生碰撞。

统。

现在分别介绍其详细设计。

2.1 大臂位置反馈系统通常的大臂空间位置反馈都是采用行走、回转、俯仰三个编码器的数值计算得出的，本设计采用当今世界上最先进的定位手段-RTK GPS测量系统。

RTK是能够在野外实时得到厘米级定位精度的测量方法，它采用了载波相位动态实时差分（Real-time kinematic）方法，是GPS应用的重大里程碑。

高精度的GPS测量必须采用载波相位观测值，RTK定位技术就是基于载波相位观测值的实时动态定位技术，它能够实时地提供测站点在指定坐标系中的三维定位结果，并达到厘米级精度。

在RTK作业模式下，基准站通过数据链将其观测值和测站坐标信息一起传送给流动站。

流动站不仅通过数据链接收来自基准站的数据，还要采集GPS观测数据，并在系统内组成差分观测值进行实时处理，同时给出厘米级定位结果，历时不到一秒钟。

基准站的位置可以根据具体需要设置在任意位置，在本系图1 堆取料机在堆场作业的示意图统中，选取中控室安装一台基准站，每台堆取料机上安装两台目前国内包括黄骅港在内的煤炭码头中，没有有效地方法流动站，分别安装在大机回转中心点和大臂头部中心点处，流来提前预知堆取料机20之间是否将要发生碰撞，堆取料机空间防碰撞是堆取料作业过程中的难题。

具体来说，现有的防碰撞方法中需要结合堆取料机的行走数据，而现有的方法获取的堆取料机的行走数据经常不准，由于堆取料机的行走距离很长，都在1500m以上，而用于获取堆取料机的行走数据的行走编码器都安装在行走轮上，堆取料机的行走轨道与堆取料机的行走轮之间的摩擦力不均，一旦堆取料机的行走轮出现打滑现象，行走数据就会出现不准确的情况，长时间累积会造成误差越来越大，即使通过行走定点校正对编码器进行修正，但不准确因素仍无法彻底克服。

其次，对大臂的空间位置计算过程非常复杂，该计算过程需要结合行走、俯仰、回转3个编码器的数值进行空间建模，而图2 流动站在堆取料机上的安装位置示意图这3个编码器都有不同程度的误差，这就造成累积误差，故大臂在图2中，1为1号流动站，2为2号流动站。

两个流动站实时空间坐标的准确性不高。

另一种现有的防碰撞方法是根据2个堆检测空间的三维坐标信息，并通过以太网传输给中控室的取料机是否处于一个场垛进行判断，如果2个堆取料机不在同一plc主机，这样根据“两点确定一条直线”原理，中控plc就可个场垛就可以正常作业。

2个堆取料机进入一个场垛进行作业以实时的知道堆取料机的大机回转中心和堆取料机的大臂头部时，就对两个堆取料机都进行锁定，使其不能工作，由此避免中心所在轴线的位置了。

堆取料机之间发生碰撞，这严重影响了堆取料机的同场作业。

通过使用流动站和基准站来检测的大臂位置信息可以精确由于以上原因，现有的防碰撞方法无法有效避免堆取料机到厘米级，并且不受堆取料机自身行走轮打滑和其他编码器累空间防碰撞问题，使得2个堆取料机无法同时在同一个堆场中安积误差的影响，因此比现有的防碰撞方法更加准确高效。

克服了现有技术中由于大臂空间位置反馈都是采用行走、回转、俯仰三个编2.2.2 异面情况分析码器的数值计算得出的，而造成的误差累积问题。

如图4所示，在异面的情况下，求两个大臂的最小距离可2.2 空间数据计算系统以采用两条直线间计算公垂线的办法来实现，但是由于两条大空间数据计算系统的主要任务是根据采集到的各堆取料机臂的长度有限，有可能不存在公垂线，这时则需要将两条大机大臂位置信息来计算任意两台堆取料机大臂的空间最小距离。

回转中心和大臂头部中心所构成的线段延长为两条直线，求出可以通过多种方法来根据位置信息确定多个堆取料机中任意两直线的公垂线和两条大臂的交点p1、p2，首先检查交点p1有没个堆取料机的大臂之间的最小距离，由于堆取料机的大臂较有在第一堆取料机的大臂上，如果在则选定p1为第一个点，如长，两个堆取料机之间的距离可以近似看作是两个堆取料机的果不在则选择第一堆取料机的大臂头部的点A作为第一点；然后大臂之间的距离。

所以两个堆取料机之间的最小距离为：一堆检查p2点有没有在第二堆取料机的大臂上，同理如果在，则作取料机的大机回转中心和堆取料机的大臂头部中心所构成的线为第二个点，如果不在，则选中第二堆取料机的大臂头部的点段、与另一堆取料机的大机回转中心和堆取料机的大臂头部中心所构成的线段之间的最小距离。

其中当两个堆取料机的大臂共面时，最小距离为一堆取料机的大臂头部中心到另一个大机回转中心和大臂头部中心所构成的线段的垂直距离或两个堆取料机的大臂头部中心之间的最小距离；当两个堆取料机的大臂异面时，最小距离为一堆取料机的大机回转中心和堆取料机的大臂头部中心所构成的线段、与另一堆取料机的大机回转中心和堆取料机的大臂头部中心所构成的线段的公垂线段的距离或两个堆取料机的大臂头部中心之间的最小距离。

采集到各台堆取料机的位置信息后，可以通过以下过程来建立堆取料机大机回转中心和大臂头部中心所构成的线段的空间直线方程，现在以两个堆取料机为例具体介绍，两个堆取料图4 两个大机异面时的状态示意图机可以称为第一堆取料机和第二堆取料机。

2.2.3 具体算法实现设第一堆取料机的大机回转中心A和大臂头部中心B处的三根据以上原理，可以按照如下过程来确定最小距离。

维坐标分别为A（x1，y1，z1）、B（x2，y2，z2），第二堆取首先需要判断A、B、C、D四个点是否在同一个平面即共面料机的大机回转中心C和大臂头部中心D处的三维坐标分别为C还是异面，具体过程如下：（x3，y3，z3）、D（x4，y4，z4）。

也就是说通过RTK采集了计算A（x1，y1，z1）、B（x2，y2，z2）、C（x3，y3，四个点的坐标，每两个点可以确定一条线段。

根据空间立体几z3）、D（x4，y4，z4）四个点构成的四个向量AB（a1，b1，何，两条直线线段的关系有两种情况，共面和异面。

所谓共面c1）、AC（a2，b2，c2）、AD（a3，b3，c3）、CD（a4，b4，是指两条直线在一个平面内，而异面则指两条直线不在一个平c4）分别为：面内，现在分别介绍两种情况下如何两个堆取料机之间的最小公式1：AB：a1=x1-x2；b1=y1-y2；c1=z1-z2 距离。

公式2：AC：a2=x1-x3；b2=y1-y3；c2=z1-z32.2.1 共面情况分析公式3：AD：a3=x1-x4；b3=y1-y4；c3=z1-z4如图3所示，两条直线共面存在两种情况，一种是两条直公式4：CD：a4=x4-x3；b4=y4-y3；c4=z4-z3线平行，一种是两条直线相交。

两条直线平行的情况是指两个首先，需要判断A、B、C、D四个点是否处于一个平面内。

堆取料机的大臂在空间坐标中平行时的状态；而两条直线相交根据四点共面定理，若三个向量的混合积（AB，AC，AD）=0，在实际中则不能发生，因为大臂是实体，不可能出现相交的情即如果况，由于大臂的长度有限，所以在共面的情况下，除了平行，公式5：a1хb2хc3+a2хb3хc1+a3хb1хc2-a3хb2х另一个状态则是不平行，有相交的趋势。

在这两种情况下，出c1-a1хb3хc2-a2хb1хc3=0现两个堆取料机的大臂相碰撞的可能情形有以下三种：一是第则四点共面，否则，说明不共面即异面。

一堆取料机的大臂的头部碰到第二堆取料机的大臂的头部，二在判断出共面的情况下：是第一堆取料机的大臂的头部碰到第二堆取料机的大臂的上下如果左右侧部，三是第二堆取料机的大臂的头部碰到第一堆取料机公式6：（a2хa4+b2хb4+c2хc4）/（sqrt（a2х的大臂的上下左右侧部。

在共面的情况下，不可能出现两个大a2+b2хb2+c2хc2）хsqrt（a4хa4+b4хb4+c4хc4）>=0说明两个向量之间的夹角为0-90°，则第一堆取料机的大臂头部中心到第二堆取料机的大机回转中心和大臂头部中心所构成的线段有垂线。

其中sqrt（）代表平方根运算。

根据两点确定一条直线原则，第二堆取料机的大臂的直线方程可以设为：公式7：（x-x3）/a4=（y-y3）/b4=（z-z3）/c4则过已知点A（x1，y1，z1）且与已知直线CD相垂直的平面的方程为：图3 两个大机共面时的状态示意图公式8：a4（x-x1）+b4（y-y1）+c4（z-z1）=0所以共面情况下算法的原理是求出一个大臂的头部中心到可以通过以上两个方程求出第二堆取料机的直线与垂直平另一个大机回转中心和大臂头部中心所构成的线段的垂直距面的交点（x5，y5，z5），所以，可以通过两点之间的距离计离，当每个大臂头部中心到另一个堆取料机的大机回转中心和算出第一堆取料机的大臂头部中心与第二堆取料机的大机回转大臂头部中心所构成的线段都没有垂线时，可直接求出两个大中心和大臂头部中心所构成的线段的距离为：臂头部中心之间的最小距离。

公式9：Dist1=sqrt（（x1-x5）х（x1-x5）+（y1-y5）XCZх（y1-y5）+（z1-z5）х（z1-z5））中心与另一个公垂点的距离为两个堆取料机之间的最小距离。

否则其中，如果仅满足x1<x7<x2或x2<x7<x1，表明点p1在线段AB中计算第一堆取料机的大臂头部中心与第二堆取料机的大臂而p2不在线段CD中，则头部中心距离，即点A、C之间的距离：公式22：Mindist2= sqrt（（x3-x7）х（x3-x7+（y3-公式10：Dist3= sqrt（a2хa2+b2хb2+c2хc2）y7）х（y3-y7）+（z3-z7）х（z3-z7））同理，如果如果仅满足x3<x8<x4或x4<x8<x3，表明点p1不在线段AB中公式11：（a1хa2+b1хb2+c1хc2）/（sqrt（a1х而p2在线段CD中，则a1+b1хb1+c1хc1）хsqrt（a2хa2+b2хb2+c2хc2））>=0公式23：Mindist2= sqrt（（x8-x1）х（x8-x1+（y8-说明两个向量之间的夹角为0-90°，则第二堆取料机的大y1）х（y8-y1）+（z8-z1）х（z8-z1））臂头部中心到第一堆取料机的大机回转中心和大臂头部中心所如果都不在，则求出两个大臂头部中心的距离构成的线段有垂线。