本发明涉及堆取料机防碰撞，尤其是涉及一种基于rtk定位的堆取料机防碰撞计算方法。
背景技术：
1、目前，国内外散货码头中，解决同一轨道或相邻轨道堆取料机间的防碰撞问题是保证堆取料安全作业的关键。现有的防碰撞方法需结合堆取料机的行走、回转、俯仰位置数据，这些位置数据通过编码器定位设备获取，编码器长期使用后累计误差越来越大，需定期对编码器校准，因此防碰撞系统无法实时获取各机构准确的定位数据。另一种现有的防碰撞方法是通过配置超声波雷达或激光测距装置等来防止设备之间或者设备与物料之间的碰撞，但是单纯靠检测传感器判断存在盲区问题，还需根据堆取料机的位置和姿态进行防碰撞计算。目前计算方法大多通过三维建模或繁琐的数学算法判断设备之间是否碰撞，算法复杂，计算量大，需要专业的计算机进行防碰撞演算。也有算法通过堆取料机简化后的模型选取几个特征点作为判断依据，会产生报警不及时、不精准的现象。有些码头甚至依然采用人工监控的方法来避免碰撞事故，此种操作方式已无法满足生产需要。基于上述原因，现有的防碰撞方法无法有效避免堆取料机防碰撞问题，使得两台堆取料机无法同时在临近堆场安全作业，影响作业效率，因此亟需一种安全高效便捷的防碰撞计算系统。
2、公开于该背景技术部分的信息仅仅旨在加深对本发明的总体背景技术的理解，而不应当被视为承认或以任何形式暗示该信息构成已为本领域技术人员所公知的现有技术。
技术实现思路
1、本发明的目的在于提供一种基于rtk定位的堆取料机防碰撞计算方法，以解决现有技术中存在的问题。
2、为了实现上述目的，本发明采用以下技术方案：
3、本发明提供一种基于rtk定位的堆取料机防碰撞计算方法，所述计算方法包括如下步骤：
4、s1：建立位置反馈系统：采用rtk测量系统，实时提供测站点在指定坐标系中的空间定位结果，计算出堆取料机行走、回转、俯仰位置数据；
5、s2：建立统一平面坐标系统：通过rtk建立统一平面坐标系统，将轨道位置定位在平面坐标系统中；
6、s3：设定相邻大机安全保护距离：依据现场情况划定安全作业区域或调节报警停机阈值；
7、s4：判断两条线段是否相交，对于判断结果为相交的情况给出报警。
8、进一步，s2中将轨道位置定位在平面坐标系统中的具体方法为：
9、对1号堆取料机头部位置与尾部位置进行平面位置计算，该计算过程需要结合行走、俯仰、回转3个位置的数据，得出头部平面坐标（ x 1 ，y 1）与尾部平面坐标（ x 2 ，y 2），从而可得出1号堆取料机头部至尾部在平面坐标系的线段，在平面直角坐标系中将堆取料机拟合成一条线段；同理求得2号堆取料机头部至尾部所在平面坐标系的线段。
10、进一步，s4中判断两条线段是否相交的具体方法如下：
11、（1）先计算两条线段的斜率，判断是否平行；若平行，则一定不相交；
12、（2）若不平行，求出两条线段的直线方程，联立之，解出交点坐标；
13、（3）将堆取料机头部坐标（ x 1 ，y 1）、尾部（ x 2 ，y 2）与两条线段交点坐标（ x，y）进行排序；只要线段交点坐标落在堆取料机头部与尾部坐标之间，即 x 1≤ x≤ x 2， y 1≤ y≤ y 2则两条线段相交，给出报警。
14、采用上述技术方案，本发明具有如下有益效果：
15、本发明根据rtk建立平面直角坐标系，对堆取料机的平面位置进行计算，把堆取料机拟合成一条线段实时定位于平面直角坐标系中，通过算法实现防碰撞功能。解决了现有技术中计算过程繁琐，数据冗余量大和计算不准确等问题，在有效实现防碰撞作业的前提下，一定程度上提升了防碰撞计算的效率和精确性，促进了无人化堆取料技术的发展。
16、实施方式
17、下面结合实施例对本发明的实施方式作进一步详细描述。以下实施例的详细描述用于示例性地说明本发明的原理，但不能用来限制本发明的范围，即本发明不限于所描述的优选实施例，本发明的范围由权利要求书限定。
18、本实施例提供一种基于rtk定位的堆取料机防碰撞计算方法，所述计算方法包括如下步骤：
19、s1：建立位置反馈系统：采用rtk测量系统，实时提供测站点在指定坐标系中的空间定位结果，计算出堆取料机行走、回转、俯仰位置数据；可精确到厘米级，不受堆取料机自身行走轮打滑和编码器累积误差的影响。
20、s2：建立统一平面坐标系统：通过rtk建立统一平面坐标系统，将轨道位置定位在平面坐标系统中；对1号堆取料机头部位置与尾部位置进行平面位置计算，该计算过程需要结合行走、俯仰、回转3个位置的数据，得出头部平面坐标（ x 1 ，y 1）与尾部平面坐标（ x 2 ，y 2），从而可得出1号堆取料机头部至尾部在平面坐标系的线段（两点确定一条直线），在平面直角坐标系中将堆取料机拟合成一条线段；同理求得2号堆取料机头部至尾部所在平面坐标系的线段。公式如下：
21、x1 := x0 +x_offset + l_head*cos(anglerad)*cos(anglerad_fy);
22、y1 :=y0 +y_offset + l_head*sin(anglerad)*cos(anglerad_fy);
23、x2 := x0 +x_offset - l_tail*cos(anglerad)*cos(anglerad_fy);
24、y2 :=y0 +y_offset - l_tail*sin(anglerad)*cos(anglerad_fy);
25、k1 :=tan(anglerad);
26、b1 :=y1-k1*x1;
27、s3：设定相邻大机安全保护距离：堆取料机在平面坐标系的位置范围主要是由头部坐标与尾部坐标划定，从而确定线段长度，进一步在平面直角坐标系中将堆取料机拟合成一条线段。要实现提前判断堆取料机是否相撞或是划定安全作业区，需把头部或尾部坐标增大，使线段增加出来的长度作为安全作业区域或者是报警停机阈值，因此判断堆取料机防撞问题巧妙地转化为判断增长后的两条线段是否相交的问题，相交即存在碰撞危险。该设计可依据现场情况划定安全作业区域或调节报警停机阈值。
28、s4：判断两条线段是否相交，平面直线有两种关系：相交或平行。直线平行，等价于它们的斜率相等，只需分别计算出斜率即可。把“直线"换成“线段”如何判断两条线段是否相交？和直线不同，线段是有固定长度的，即使它们所属的两条直线相交，这两条线段也不一定相交。判断两条线段是否相交的具体方法如下：
29、（1）先计算两条线段的斜率，判断是否平行；若平行，则一定不相交；
30、（2）若不平行，求出两条线段的直线方程，联立之，解出交点坐标；
31、（3）将堆取料机头部坐标（ x 1 ，y 1）、尾部（ x 2 ，y 2）与两条线段交点坐标（ x，y）进行排序；只要线段交点坐标落在堆取料机头部与尾部坐标之间，即 x 1≤ x≤ x 2， y 1≤ y≤ y 2则两条线段相交，给出报警。
32、运用定比分点公式，判断交点是否在两条线段上。算法如下：
33、if x1 <> x2 then k1 :=(y1 -y2)/(x1-x2);
34、 end_if;
35、b1 :=y1-k1*x1;
36、if x3 <> x4 then k2 :=(y3-y4)/(x3-x4);
37、 end_if;
38、 b2 :=y3-k2*x3;
39、if k1<>k2 then x:=(b2-b1)/(k1-k2);
40、end_if;
41、y:=k1*x+b1;
42、if x1<=x2 then x11:=x1; x22:=x2;
43、else x11:=x2; x22:=x1;
44、 end_if;
45、 if y1<=y2 then y11:=y1; y22:=y2;
46、else y11:=y2; y22:=y1;
47、 end_if;
48、 if x3<=x4 then x33:=x3; x44:=x4;
49、else x33:=x4; x44:=x3;
50、 end_if;
51、 if y3<=y4 then y33:=y3; y44:=y4;
52、else y33:=y4; y44:=y3;
53、 end_if;
54、 if k1=k2 then alarm :=0;
55、elsif (x11 <=x and x<=x22) and (y11<=y and y<=y22) and (x33 <=x and x<=x44) and (y33<=y and y<=y44) then alarm:=1;
56、else alarm :=0;
57、end_if;
58、综上，本发明根据rtk建立平面直角坐标系，对堆取料机的平面位置进行计算，把堆取料机拟合成一条线段实时定位于平面直角坐标系中，通过算法实现防碰撞功能。解决了现有技术中计算过程繁琐，数据冗余量大和计算不准确等问题，在有效实现防碰撞作业的前提下，一定程度上提升了防碰撞计算的效率和精确性，促进了无人化堆取料技术的发展。
59、最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。