CN115847412A - 一种基于分离框的异轨堆、取料机防碰撞运动规划方法 - Google Patents

Abstract

本发明公开了一种基于分离框的异轨堆、取料机防碰撞运动规划方法，通过防撞框确定、碰撞检测，最后基于分离框的大机运动策略规划，通过GJK算法检测到两个大机的矩形虚拟框发生碰撞预警时，自动判断大机防碰撞运动策略，本发明的方法当检测到两个大机的矩形虚拟框发生碰撞预警时，能够自动规划大机的防碰撞运动策略，大大提高了堆场作业效率。

Description

一种基于分离框的异轨堆、取料机防碰撞运动规划方法

技术领域

本发明涉及港口堆场作业技术领域，尤其涉及的是一种基于分离框的异轨堆料机、取料机防碰撞运动规划方法。

背景技术

现有的大机间防碰撞方法多基于大机的行走、俯仰以及回转数据，这些数据一般通过安装在大机上的编码器获得，然而一旦出现行走轮打滑、刹车制动偏移等情况，会直接影响数据的准确性，进而导致初始的防碰撞方法失效。另外一种方法是通过在大机悬臂上安装激光、雷达设备，利用目标的反射波来检测周围的障碍物以避免大臂和障碍物发生碰撞，然而散料堆场中往往粉尘较大、气候恶劣，这些因素都会在一定程度上对激光雷达信号产生干扰。

在一些文献中，还有一类堆、取料机防碰撞方法是基于北斗或者GPS数据。GPS的定位精度可达到10mm级，且不易受外界环境气候的影响[艾立军.基于GPS开发的堆取料机定位系统[J].冶金设备管理与维修,2018,36(2):3.]。尹艳艳等[尹艳艳,吕崇晓.一种基于OBB矩形碰撞检测算法的堆取料机防碰撞方法[J].水运工程,2017(12):4.]采用GPS定位模块对大机进行定位，基于定位数据采用2D的OBB(orient bounding box)碰撞检测算法对大机悬臂进行空间建模与碰撞检测，实现机器臂碰撞预警与安全停机的目的。李长安[李长安.堆取料机空间防碰撞系统设计与实现[J].硅谷,2012(21):3.]通过GPS数据计算出两个大机之间的最小距离，从而判断出堆取料机发生碰撞的可能性大小，然后需要工作人员进行相应处理。

这些方法的结论一般都是当出现碰撞预警的时候进行停机操作，在进行停机操作后往往都需要人工判断确定大机的进退、旋转等后续动作，没有自动给出后续大机防碰撞作业的运动策略，大大降低了堆场作业效率。

发明内容

本发明所要解决的技术问题是针对现有技术的不足提供一种基于分离框的异轨堆、取料机防碰撞运动规划方法。

本发明的技术方案如下：

一种基于分离框的异轨堆料机、取料机防碰撞运动规划方法，包括以下步骤：

步骤一：防撞框确定

首先根据堆料机、取料机上GPS的空间数据确定大机的虚拟防撞框；虚拟防撞框预留了足够的空间来保证虚拟防撞框发生碰撞时大机不会发生碰撞，而且又能保证最大限度保留大机的作业空间；虚拟防撞框的位置和姿态随着大机的位置和姿态的变化而变化，通过自适应算法动态确定虚拟防撞框的位置和姿态；

步骤二：碰撞检测

基于虚拟防撞框来进行碰撞检测；采用Gilbert-Johnson-Keerthi(GJK)算法来对堆、取料机的虚拟防撞框进行碰撞检测；

步骤三：基于分离框的大机运动策略规划

当通过GJK算法检测到两个大机的矩形虚拟框发生碰撞预警时，自动判断大机防碰撞运动策略；

在大机坐标系xoy中，堆、取料机的位姿根据大机头部在第一、二、三、四象限的情况分为四类；从俯视堆场的角度看，大机的运动有四种：前进、后退、悬臂左转、悬臂右转；定义X轴正方向为大机前进方向，Y轴负方向为大机后退方向，逆时针为悬臂左转，顺时针为悬臂右转；当检测发生碰撞时，那么至少有一台大机的虚拟框一条边和另一台大机的虚拟框一条边相交，通过两线段直线方程求交点很容易确定两个虚拟框的碰撞位置；运动策略从大机前进、后退运动以及大机悬臂左转、右转运动两个方面来进行设计；假设堆料机为B2D，取料机为B3Q；

31)、判断大机的前进、后退动作，此时不需要考虑两台大机虚拟碰撞框的焦点位置，具体运动策略如下：

311)堆、取料机一台头部在一、二象限，一台头部在三、四象限，则只可能是堆料机虚拟框头部与取料机虚拟框头部发生碰撞，那么，

a、当X_{5_B2D}≤X_{5_B3Q}，B2D禁止前进，B3Q禁止后退；

b、当X_{5_B2D}＞X_{5_B3Q}，B2D禁止后退，B3Q禁止前进；

其中，X_{5_B2D}、X_{5_B3Q}分别为B2D和B3Q虚拟防撞框的点a₅的横坐标；

312)堆料机或取料机头部都在一、二象限，或都在三、四象限，则只可能是堆料机B2D虚拟框头部与取料机B3Q虚拟框配重发生碰撞，那么：

a、当X_{5_B2D}≤X_{6_B3Q}，B2D禁止前进，B3Q禁止后退；

b、当X_{5_B2D}＞X_{6_B3Q}，B2D禁止后退，B3Q禁止前进；

其中，X_{6_B3Q}为B3Q虚拟防撞框的点a₆的横坐标；

32)、判断大机的左转右转动作，此时，将虚拟碰撞框分离为左右两个半区进行考虑，一个半区为左半区a₁a₄a₆a₅，一个半区为右半区a₂a₃a₆a₅；

321)堆、取料机一台在一、二象限，一台在三、四象限，则只可能是堆料机B2D虚拟框头部与取料机B3Q虚拟框头部发生碰撞，此时堆、取料机碰撞位置左半区只考虑线段a₁a₄和a₁a₅，右半区只考虑线段a₂a₃和a₂a₅：

a、当B2D左半区与B3Q右半区碰撞，此时，B2D禁止左转，B3Q禁止右转；

b、B2D右半区与B3Q右半区碰撞，B2D禁止右转，B3Q禁止右转；

c、B2D左半区与B3Q左半区碰撞，B2D禁止左转，B3Q禁止左转；

d、B2D右半区与B3Q左半区碰撞，B2D禁止右转，B3Q禁止左转；

e、B2D右半区与B3Q左半区和右半区都碰撞，即同时出现情况b和d时，B2D禁止右转，B3Q禁止左转和右转；

以下f、g、h与此情况类似，将对应的碰撞线段替换即可；

f、B2D左半区与B3Q左半区和右半区都碰撞，即同时出现情况a和c时，B2D禁止左转，B3Q禁止左转和右转；

g、B2D左半区和右半区与B3Q左半区碰撞，即同时出现情况c和d时，B2D禁止左转和右转，B3Q禁止左转；

h、B2D左半区和右半区与B3Q右半区碰撞，即同时出现情况a和b时，B2D禁止左转和右转，B3Q禁止右转；

i、B2D左半区和右半区与B3Q左半区和右半区都碰撞，即同时出现情况a、b、c或a、b、d或a、c、d或b、c、d或a、b、c、d时，B2D禁止左转和右转，B3Q禁止左转和右转；

322)堆、取料机都在一、二象限，或都在三、四象限，则只可能是堆料机B2D虚拟框头部与取料机B3Q虚拟框配重发生碰撞，此时堆料机碰撞位置左半区只考虑线段a₁a₄和a₁a₅，右半区只考虑线段a₂a₃和a₂a₅，取料机左半区只考虑线段a₁a₄和a₄a₆，右半区只考虑线段a₂a₃和a₃a₆：

a、B2D左半区与B3Q右半区碰撞，此时，B2D禁止左转，B3Q禁止左转；

b、B2D右半区与B3Q右半区碰撞，B2D禁止右转，B3Q禁止左转；

c、B2D左半区与B3Q左半区碰撞，B2D禁止左转，B3Q禁止右转；

d、B2D右半区与B3Q左半区碰撞，B2D禁止右转，B3Q禁止右转；

e、堆料机B2D右半区与取料机B3Q左半区和右半区都碰撞，即同时出现情况b和d时，B2D禁止右转，B3Q禁止左转和右转；

f、B2D左半区与B3Q左半区和右半区都碰撞，即同时出现情况a和c时，B2D禁止左转，B3Q禁止左转和右转；

g、B2D左半区和右半区与B3Q左半区碰撞，即同时出现情况c和d时，B2D禁止左转和右转，B3Q禁止右转；

h、B2D左半区和右半区与B3Q右半区碰撞，即同时出现情况a和b时，B2D禁止左转和右转，B3Q禁止左转；

i、B2D左半区和右半区与B3Q左半区和右半区都碰撞，即同时出现情况a、b、c或a、b、d或a、c、d或b、c、d或a、b、c、d时，B2D禁止左转和右转，B3Q禁止左转和右转；

在禁止大机做某一运动的时候，大机可以根据具体的现场作业任务进行其他的运动。

根据权利要求1所述的防碰撞运动规划方法，其特征在于，步骤一中，所述自适应算法动态确定虚拟防撞框的位置和姿态：以堆场中一固定位置为整个堆场坐标系XOY的原点；以大机自身的旋转中心为原点建立大机坐标系xoy；

G₁、G₂点分别表示悬皮头部和配重尾部的GPS流动站；通过GPS数据可以得到G₁点的坐标[x_G1,y_G1]、G₂点的坐标[x_G2,y_G2]，G₁、G₂两点可以组成一条线段G₁G₂；将线段G₁G₂分别向两端延长，在G₁端得到点a₅，在G₂端得到点a₆，设线段G₁a₅的长度为L₁，线段G₁a₆的长度为L₂；过点a₅、a₆构造包围大机的矩形虚拟防撞框，四个顶点分别为点a₁、a₂、a₃、a₄，线段a₁a₂和a₃a₄都垂直于线段a₅a₆，线段a₁a₅、a₂a₅、a₄a₆、a₃a₆的长度为W；设线段a₅a₆与横轴正方向的夹角为θ，θ的表达式为：

基于θ，可以通过式(2)、(3)、(4)、(5)分别求得点a₁、a₂、a₃、a₄的坐标[x₁,y₁]、[x₂,y₂]、[x₃,y₃]、[x₄,y₄]；

用同样的方法可以求得取料机悬皮头部在第二、三、四象限以及堆料机的虚拟防撞框空间坐标。

所述的防碰撞运动规划方法，步骤二中，GJK算法检测大机虚拟防撞框碰撞情况的核心逻辑是对于给定的两个矩形防撞框P、Q以及一个初始方向，通过迭代的方式构建、更新单纯形(simplex)，并判断单纯形是否包含原点，若包含原点则两个矩形框相交，则虚拟防撞框发生碰撞，否则虚拟防撞框没有发生碰撞。其中，j阶单纯形指的是j维空间中的多胞形，该多胞形是j+1个顶点组成的凸包，所有顶点均是仿射无关的；通常情况下，j的取值为0-3，对于二维空间的多边形，最多用到2阶单纯形；

将两个矩形防撞框中心构成的方向矢量设置为初始方向；根据初始方向，采用支撑函数

计算得到两个矩形明可夫斯基差(Minkowski difference)形状中的一个支撑点并且放在单纯形中；对于给定的方向/>

如果满足公式(1)，那么点k是方向/>

上的支撑点，也就是说存在一点k使得/>

有最大值；

设两个矩形P、Q的顶点集分别为V_P、V_Q，则两个矩形P、Q明可夫斯基差形状的表达式为：

V_P-V_Q＝{v_P-v_Q|v_P∈V_P,v_Q∈V_Q} (2)

取初始方向的反方向作为下一次迭代方向，进入循环迭代步骤：

1)基于迭代方向

采用支撑函数/>

计算出一个新的支撑点t；

2)如果

那么在迭代方向/>

无法找到一个能够跨越原点的支撑点，也就是说无法组成一个能够包含原点的单纯形，即两个矩形虚拟防撞框没有发生碰撞，退出循环迭代；

3)如果新的支撑点t能够跨越原点，则加入到单纯形中，此时，

A、当单纯形中存在两个点，那么以这两点构成直线的垂线朝向原点的方向作为下一次循环迭代的方向；

B、当单纯形中存在三个点，那么判断此三点组成的三角形是否包含原点。如果包含原点，说明发生碰撞两个矩形虚拟防撞框发生碰撞，退出循环迭代；如果不包含原点，则保留距离原点最近的边上的两个点，同样以这两个点所形成直线的垂线朝向原点的方向作为下一次迭代方向；

4)跳转到步骤1)。

所述的防碰撞运动规划方法，悬皮头部在第一象限，可以通过式(2)、(3)、(4)、(5)分别求得点a₁、a₂、a₃、a₄的坐标[x₁,y₁]、[x₂,y₂]、[x₃,y₃]、[x₄,y₄]。

/>

所述的防碰撞运动规划方法，通过式(6)、(7)、(8)、(9)可以求得大机悬皮头部在第二象限的虚拟防撞框空间坐标：

所述的防碰撞运动规划方法，通过式(10)、(11)、(12)、(13)可以求得大机悬皮头部在第三象限的虚拟防撞框空间坐标：

所述的防碰撞运动规划方法，通过式(14)、(15)、(16)、(17)可以求得大机悬皮头部在第四象限的虚拟防撞框空间坐标：

/>

通过防撞框确定、碰撞检测，最后基于分离框的大机运动策略规划，通过GJK算法检测到两个大机的矩形虚拟框发生碰撞预警时，自动判断大机防碰撞运动策略，本发明的方法当检测到两个大机的矩形虚拟框发生碰撞预警时，能够自动规划大机的防碰撞运动策略，大大提高了堆场作业效率。

附图说明

图1为堆、取料机上流动站接收机安装位置示意图；

图2为虚拟防撞框示意图；

1，基站；2，第一流动站接收机；3，第二流动站接收机；4，堆场煤堆；5，大机行走轨道；6，上层交换机；7，上位机；8，第三流动站接收机；9，第四流动站接收机。

具体实施方式

以下结合具体实施例，对本发明进行详细说明。

本发明利用基于GPS的RTK(Real Time Kinematie)实时动态定位数据来设计堆、取料机的防碰撞运动策略。GPS接收机安置在基准站上进行观测，基准站的位置可以根据具体现场情况设置在任意位置，基准站将已知的测站精密坐标和接收到的卫星信息实时发送给流动站接收机，流动站接收机在进行GPS观测的同时，也接收到基准站的信息，经过对结果进行改正，从而提高定位精度。在发明大机防碰撞运动策略中，流动站接收机在堆、取料机上的安装位置如图1所示。图1中，两个异轨的堆料机、取料机分别设置在两个平行的轨道上，堆料机、取料机交叉设置，堆料机上分别在悬皮头部和回转中心安装第一流动站接收机2和第二流动站接收机3，；取料机上分别在悬皮头部和配重尾部安装第三流动站接收机8和第四流动站接收机9。

一、防撞框确定

为了执行后续的碰撞检测算法和堆、取料机的运动策略，同时考虑要最大限度地保留堆场空间的利用率，首先需要根据堆料机、取料机(以下统称为大机)上GPS的空间数据确定大机的虚拟防撞框。虚拟防撞框预留了足够的空间来保证虚拟防撞框发生碰撞时大机不会发生碰撞，而且又能保证最大限度保留大机的作业空间。虚拟防撞框的位置和姿态随着大机的位置和姿态的变化而变化，通过以下计算方法自适应地确定虚拟防撞框的位置和姿态。

如图2所示，以堆场中一固定位置为整个堆场坐标系XOY的原点。以大机自身的旋转中心为原点建立大机坐标系xoy，从俯视堆场的角度出发，堆、取料机的位姿根据大机头部在第一、二、三、四象限的情况分为四类。以图2中取料机悬皮头部在第一象限为示例图。图中G₁、G₂点分别表示悬皮头部和配重尾部的GPS流动站。通过GPS数据可以得到G₁点的坐标[x_G1,y_G1]、G₂点的坐标[x_G2,y_G2]，G₁、G₂两点可以组成一条线段G₁G₂。将线段G₁G₂分别向两端延长，在G₁端得到点a₅，在G₂端得到点a₆，设线段G₁a₅的长度为L₁，线段G₂a₆的长度为L₂。过点a₅、a₆构造包围大机的矩形虚拟防撞框，四个顶点分别为点a₁、a₂、a₃、a₄，线段a₁a₂和a₃a₄都垂直于线段a₅a₆，线段a₁a₅、a₂a₅、a₄a₆、a₃a₆的长度为W。设线段a₅a₆与横轴正方向的夹角为θ，θ的表达式为：

基于θ，可以通过式(2)、(3)、(4)、(5)分别求得点a₁、a₂、a₃、a₄的坐标[x₁,y₁]、[x₂,y₂]、[x₃,y₃]、[x₄,y₄]。

用类似的方法通过式(6)、(7)、(8)、(9)可以求得大机悬皮头部在第二象限的虚拟防撞框空间坐标：

同样通过式(10)、(11)、(12)、(13)可以求得大机悬皮头部在第三象限的虚拟防撞框空间坐标：

/>

同样通过式(14)、(15)、(16)、(17)可以求得大机悬皮头部在第四象限的虚拟防撞框空间坐标：

二、Gilbert-Johnson-Keerthi碰撞检测算法

在能够实时得到虚拟防撞框的具体位置之后，就可以基于虚拟防撞框来进行碰撞检测了。堆、取料机的虚拟防撞框是矩形，属于凸多边形，在此提出采用Gilbert-Johnson-Keerthi(GJK)算法来对堆、取料机的虚拟防撞框进行碰撞检测。GJK算法检测大机虚拟防撞框碰撞情况的核心逻辑是对于给定的两个矩形防撞框P、Q以及一个初始方向，通过迭代的方式构建、更新单纯形(simplex)，并判断单纯形是否包含原点，若包含原点则两个矩形框相交，则虚拟防撞框发生碰撞，否则虚拟防撞框没有发生碰撞。其中，j阶单纯形指的是j维空间中的多胞形，该多胞形是j+1个顶点组成的凸包，所有顶点均是仿射无关的。通常情况下，j的取值为0-3，对于二维空间的多边形，最多用到2阶单纯形。

将两个矩形防撞框中心构成的方向矢量设置为初始方向。根据初始方向，采用支撑函数

计算得到两个矩形明可夫斯基差(Minkowski difference)形状中的一个支撑点并且放在单纯形中。对于给定的方向/>

如果满足公式(1)，那么点k是方向/>

上的支撑点，也就是说存在一点k使得/>

有最大值。

设两个矩形P、Q的顶点集分别为V_P、V_Q，则两个矩形P、Q明可夫斯基差形状的表达式为：

V_P-V_Q＝{v_P-v_Q|v_P∈V_P,v_Q∈V_Q} (2)

取初始方向的反方向作为下一次迭代方向，进入循环迭代步骤：

1)基于迭代方向

采用支撑函数/>

计算出一个新的支撑点t；

2)如果

那么在迭代方向/>

无法找到一个能够跨越原点的支撑点，也就是说无法组成一个能够包含原点的单纯形，即两个矩形虚拟防撞框没有发生碰撞，退出循环迭代；

3)如果新的支撑点t能够跨越原点，则加入到单纯形中，此时，

A、当单纯形中存在两个点，那么以这两点构成直线的垂线朝向原点的方向作为下一次循环迭代的方向；

B、当单纯形中存在三个点，那么判断此三点组成的三角形是否包含原点。如果包含原点，说明发生碰撞两个矩形虚拟防撞框发生碰撞，退出循环迭代；如果不包含原点，则保留距离原点最近的边上的两个点，同样以这两个点所形成直线的垂线朝向原点的方向作为下一次迭代方向；

4)跳转到步骤1)。

三、基于分离框的大机运动策略规划

当通过GJK算法检测到两个大机的矩形虚拟框发生碰撞预警时，本发明设计了自动判断大机防碰撞运动策略。

一般情况下，堆场的堆、取料机在各自轨道上运动，堆料机轨道和取料机轨道是交错设置的，如图1所示。在大机坐标系xoy中，堆、取料机的位姿根据大机头部在第一、二、三、四象限的情况分为四类。从俯视堆场的角度看，大机的运动有四种：前进、后退、悬臂左转、悬臂右转。此外，定义X轴正方向为大机前进方向，Y轴负方向为大机后退方向，逆时针为悬臂左转，顺时针为悬臂右转。当检测发生碰撞时，那么至少有一台大机的虚拟框一条边和另一台大机的虚拟框一条边相交，通过两线段直线方程求交点很容易确定两个虚拟框的碰撞位置。运动策略主要从大机前进、后退运动以及大机悬臂左转、右转运动两个方面来进行设计。假设堆料机为B2D，取料机为B3Q。

1、判断大机的前进、后退动作，此时不需要考虑两台大机虚拟碰撞框的焦点位置，具体运动策略如下：

1)堆、取料机一台头部在一、二象限，一台头部在三、四象限，则只可能是堆料机虚拟框头部与取料机虚拟框头部发生碰撞，那么，

a、当X_{5_B2D}≤X_{5_B3Q}，B2D禁止前进，B3Q禁止后退；

b、当X_{5_B2D}＞X_{5_B3Q}，B2D禁止后退，B3Q禁止前进。

其中，X_{5_B2D}、X_{5_B3Q}分别为B2D和B3Q虚拟防撞框的点a₅(如图2)的横坐标。

2)堆料机或取料机头部都在一、二象限，或都在三、四象限，则只可能是堆料机B2D虚拟框头部与取料机B3Q虚拟框配重发生碰撞，那么

a、当X_{5_B2D}≤X_{6_B3Q}，B2D禁止前进，B3Q禁止后退；

b、当X_{5_B2D}＞X_{6_B3Q}，B2D禁止后退，B3Q禁止前进。

其中，X_{6_B3Q}为B3Q虚拟防撞框的点a₆(如图2)的横坐标。

2、判断大机的左转右转动作，此时，将虚拟碰撞框分离为左右两个半区进行考虑，如图2所示，一个半区为左半区a₁a₄a₆a₅，一个半区为右半区a₂a₃a₆a₅。

1)堆、取料机一台在一、二象限，一台在三、四象限，则只可能是堆料机B2D虚拟框头部与取料机B3Q虚拟框头部发生碰撞，此时堆、取料机碰撞位置左半区只考虑线段a₁a₄和a₁a₅，右半区只考虑线段a₂a₃和a₂a₅：

a、当B2D左半区与B3Q右半区碰撞，则会出现下列表1中的9种碰撞位置情况：

表1B2D头左半区与B3Q头右半区碰撞时的9中情况

此时，B2D禁止左转，B3Q禁止右转。以下b、c、d与此情况类似，将对应的碰撞线段替换即可。

b、B2D右半区与B3Q右半区碰撞，B2D禁止右转，B3Q禁止右转；

c、B2D左半区与B3Q左半区碰撞，B2D禁止左转，B3Q禁止左转；

d、B2D右半区与B3Q左半区碰撞，B2D禁止右转，B3Q禁止左转。

e、B2D右半区与B3Q左半区和右半区都碰撞，即同时出现情况b和d时，B2D禁止右转，B3Q禁止左转和右转。

以下f、g、h与此情况类似，将对应的碰撞线段替换即可。

f、B2D左半区与B3Q左半区和右半区都碰撞，即同时出现情况a和c时，B2D禁止左转，B3Q禁止左转和右转。

g、B2D左半区和右半区与B3Q左半区碰撞，即同时出现情况c和d时，B2D禁止左转和右转，B3Q禁止左转。

h、B2D左半区和右半区与B3Q右半区碰撞，即同时出现情况a和b时，B2D禁止左转和右转，B3Q禁止右转。

i、B2D左半区和右半区与B3Q左半区和右半区都碰撞，即同时出现情况a、b、c或a、b、d或a、c、d或b、c、d或a、b、c、d时，B2D禁止左转和右转，B3Q禁止左转和右转。

2)堆、取料机都在一、二象限，或都在三、四象限，则只可能是堆料机B2D虚拟框头部与取料机B3Q虚拟框配重发生碰撞，此时堆料机碰撞位置左半区只考虑线段a₁a₄和a₁a₅，右半区只考虑线段a₂a₃和a₂a₅，取料机左半区只考虑线段a₁a₄和a₄a₆，右半区只考虑线段a₂a₃和a₃a₆：

a、B2D左半区与B3Q右半区碰撞，共有下列表2中9种情况：

表2B2D头左半区与B3Q配重右半区碰撞时的9中情况

此时，B2D禁止左转，B3Q禁止左转。以下b、c、d与此情况类似，将对应的碰撞线段替换即可。

b、B2D右半区与B3Q右半区碰撞，B2D禁止右转，B3Q禁止左转。

c、B2D左半区与B3Q左半区碰撞，B2D禁止左转，B3Q禁止右转。

d、B2D右半区与B3Q左半区碰撞，B2D禁止右转，B3Q禁止右转。

e、堆料机B2D右半区与取料机B3Q左半区和右半区都碰撞，即同时出现情况b和d时，B2D禁止右转，B3Q禁止左转和右转。

f、B2D左半区与B3Q左半区和右半区都碰撞，即同时出现情况a和c时，B2D禁止左转，B3Q禁止左转和右转。

g、B2D左半区和右半区与B3Q左半区碰撞，即同时出现情况c和d时，B2D禁止左转和右转，B3Q禁止右转。

h、B2D左半区和右半区与B3Q右半区碰撞，即同时出现情况a和b时，B2D禁止左转和右转，B3Q禁止左转。

i、B2D左半区和右半区与B3Q左半区和右半区都碰撞，即同时出现情况a、b、c或a、b、d或a、c、d或b、c、d或a、b、c、d时，B2D禁止左转和右转，B3Q禁止左转和右转。

在禁止大机做某一运动的时候，大机可以根据具体的现场作业任务进行其他的运动，比如禁止大机左转的话，大机是可以进行右转、前进以及后退运动的。

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (7)

1.一种基于分离框的异轨堆、取料机防碰撞运动规划方法，其特征在于，包括以下步骤：

步骤一：防撞框确定

首先根据堆料机、取料机上GPS的空间数据确定大机的虚拟防撞框；虚拟防撞框预留了足够的空间来保证虚拟防撞框发生碰撞时大机不会发生碰撞，而且又能保证最大限度保留大机的作业空间；虚拟防撞框的位置和姿态随着大机的位置和姿态的变化而变化，通过自适应算法动态确定虚拟防撞框的位置和姿态；

步骤二：碰撞检测

基于虚拟防撞框来进行碰撞检测；采用Gilbert-Johnson-Keerthi(GJK)算法来对堆、取料机的虚拟防撞框进行碰撞检测；

步骤三：基于分离框的大机运动策略规划

当通过GJK算法检测到两个大机的矩形虚拟框发生碰撞预警时，自动判断大机防碰撞运动策略；

在大机坐标系xoy中，堆、取料机的位姿根据大机头部在第一、二、三、四象限的情况分为四类；从俯视堆场的角度看，大机的运动有四种：前进、后退、悬臂左转、悬臂右转；定义X轴正方向为大机前进方向，Y轴负方向为大机后退方向，逆时针为悬臂左转，顺时针为悬臂右转；当检测发生碰撞时，那么至少有一台大机的虚拟框一条边和另一台大机的虚拟框一条边相交，通过两线段直线方程求交点很容易确定两个虚拟框的碰撞位置；运动策略从大机前进、后退运动以及大机悬臂左转、右转运动两个方面来进行设计；假设堆料机为B2D，取料机为B3Q；

31)、判断大机的前进、后退动作，此时不需要考虑两台大机虚拟碰撞框的焦点位置，具体运动策略如下：

311)堆、取料机一台头部在一、二象限，一台头部在三、四象限，则只可能是堆料机虚拟框头部与取料机虚拟框头部发生碰撞，那么，

a、当X_{5_B2D}≤X_{5_B3Q}，B2D禁止前进，B3Q禁止后退；

b、当X_{5_B2D}＞X_{5_B3Q}，B2D禁止后退，B3Q禁止前进；

其中，X_{5_B2D}、X_{5_B3Q}分别为B2D和B3Q虚拟防撞框的点a₅的横坐标；

312)堆料机或取料机头部都在一、二象限，或都在三、四象限，则只可能是堆料机B2D虚拟框头部与取料机B3Q虚拟框配重发生碰撞，那么：

a、当X_{5_B2D}≤X_{6_B3Q}，B2D禁止前进，B3Q禁止后退；

b、当X_{5_B2D}＞X_{6_B3Q}，B2D禁止后退，B3Q禁止前进；

其中，X_{6_B3Q}为B3Q虚拟防撞框的点a₆的横坐标；

32)、判断大机的左转右转动作，此时，将虚拟碰撞框分离为左右两个半区进行考虑，一个半区为左半区a₁a₄a₆a₅，一个半区为右半区a₂a₃a₆a₅；

321)堆、取料机一台在一、二象限，一台在三、四象限，则只可能是堆料机B2D虚拟框头部与取料机B3Q虚拟框头部发生碰撞，此时堆、取料机碰撞位置左半区只考虑线段a₁a₄和a₁a₅，右半区只考虑线段a₂a₃和a₂a₅：

a、当B2D左半区与B3Q右半区碰撞，此时，B2D禁止左转，B3Q禁止右转；

b、B2D右半区与B3Q右半区碰撞，B2D禁止右转，B3Q禁止右转；

c、B2D左半区与B3Q左半区碰撞，B2D禁止左转，B3Q禁止左转；

d、B2D右半区与B3Q左半区碰撞，B2D禁止右转，B3Q禁止左转；

e、B2D右半区与B3Q左半区和右半区都碰撞，即同时出现情况b和d时，B2D禁止右转，B3Q禁止左转和右转；

以下f、g、h与此情况类似，将对应的碰撞线段替换即可；

f、B2D左半区与B3Q左半区和右半区都碰撞，即同时出现情况a和c时，B2D禁止左转，B3Q禁止左转和右转；

g、B2D左半区和右半区与B3Q左半区碰撞，即同时出现情况c和d时，B2D禁止左转和右转，B3Q禁止左转；

h、B2D左半区和右半区与B3Q右半区碰撞，即同时出现情况a和b时，B2D禁止左转和右转，B3Q禁止右转；

i、B2D左半区和右半区与B3Q左半区和右半区都碰撞，即同时出现情况a、b、c或a、b、d或a、c、d或b、c、d或a、b、c、d时，B2D禁止左转和右转，B3Q禁止左转和右转；

322)堆、取料机都在一、二象限，或都在三、四象限，则只可能是堆料机B2D虚拟框头部与取料机B3Q虚拟框配重发生碰撞，此时堆料机碰撞位置左半区只考虑线段a₁a₄和a₁a₅，右半区只考虑线段a₂a₃和a₂a₅，取料机左半区只考虑线段a₁a₄和a₄a₆，右半区只考虑线段a₂a₃和a₃a₆：

a、B2D左半区与B3Q右半区碰撞，此时，B2D禁止左转，B3Q禁止左转；

b、B2D右半区与B3Q右半区碰撞，B2D禁止右转，B3Q禁止左转；

c、B2D左半区与B3Q左半区碰撞，B2D禁止左转，B3Q禁止右转；

d、B2D右半区与B3Q左半区碰撞，B2D禁止右转，B3Q禁止右转；

e、堆料机B2D右半区与取料机B3Q左半区和右半区都碰撞，即同时出现情况b和d时，B2D禁止右转，B3Q禁止左转和右转；

f、B2D左半区与B3Q左半区和右半区都碰撞，即同时出现情况a和c时，B2D禁止左转，B3Q禁止左转和右转；

g、B2D左半区和右半区与B3Q左半区碰撞，即同时出现情况c和d时，B2D禁止左转和右转，B3Q禁止右转；

h、B2D左半区和右半区与B3Q右半区碰撞，即同时出现情况a和b时，B2D禁止左转和右转，B3Q禁止左转；

i、B2D左半区和右半区与B3Q左半区和右半区都碰撞，即同时出现情况a、b、c或a、b、d或a、c、d或b、c、d或a、b、c、d时，B2D禁止左转和右转，B3Q禁止左转和右转；

在禁止大机做某一运动的时候，大机可以根据具体的现场作业任务进行其他的运动。

2.根据权利要求1所述的防碰撞运动规划方法，其特征在于，步骤一中，所述自适应算法动态确定虚拟防撞框的位置和姿态：以堆场中一固定位置为整个堆场坐标系XOY的原点；以大机自身的旋转中心为原点建立大机坐标系xoy；

G₁、G₂点分别表示悬皮头部和配重尾部的GPS流动站；通过GPS数据可以得到G₁点的坐标

G₂点的坐标/>

G₁、G₂两点可以组成一条线段G₁G₂；将线段G₁G₂分别向两端延长，在G₁端得到点a₅，在G₂端得到点a₆，设线段G₁a₅的长度为L₁，线段G₁a₆的长度为L₂；过点a₅、a₆构造包围大机的矩形虚拟防撞框，四个顶点分别为点a₁、a₂、a₃、a₄，线段a₁a₂和a₃a₄都垂直于线段a₅a₆，线段a₁a₅、a₂a₅、a₄a₆、a₃a₆的长度为W；设线段a₅a₆与横轴正方向的夹角为θ，θ的表达式为：

基于θ，可以通过式(2)、(3)、(4)、(5)分别求得点a₁、a₂、a₃、a₄的坐标[x₁,y₁]、[x₂,y₂]、[x₃,y₃]、[x₄,y₄]；

用同样的方法可以求得取料机悬皮头部在第二、三、四象限以及堆料机的虚拟防撞框空间坐标。

3.根据权利要求1所述的防碰撞运动规划方法，其特征在于，步骤二中，GJK算法检测大机虚拟防撞框碰撞情况的核心逻辑是对于给定的两个矩形防撞框P、Q以及一个初始方向，通过迭代的方式构建、更新单纯形(simplex)，并判断单纯形是否包含原点，若包含原点则两个矩形框相交，则虚拟防撞框发生碰撞，否则虚拟防撞框没有发生碰撞。其中，j阶单纯形指的是j维空间中的多胞形，该多胞形是j+1个顶点组成的凸包，所有顶点均是仿射无关的；通常情况下，j的取值为0-3，对于二维空间的多边形，最多用到2阶单纯形；

将两个矩形防撞框中心构成的方向矢量设置为初始方向；根据初始方向，采用支撑函数

计算得到两个矩形明可夫斯基差(Minkowski difference)形状中的一个支撑点并且放在单纯形中；对于给定的方向/>

如果满足公式(1)，那么点k是方向/>

上的支撑点，也就是说存在一点k使得/>

有最大值；

设两个矩形P、Q的顶点集分别为V_P、V_Q，则两个矩形P、Q明可夫斯基差形状的表达式为：

V_P-V_Q＝{v_P-v_Q|v_P∈V_P,v_Q∈V_Q} (2)

取初始方向的反方向作为下一次迭代方向，进入循环迭代步骤：

1)基于迭代方向

采用支撑函数/>

计算出一个新的支撑点t；

2)如果

那么在迭代方向/>

无法找到一个能够跨越原点的支撑点，也就是说无法组成一个能够包含原点的单纯形，即两个矩形虚拟防撞框没有发生碰撞，退出循环迭代；

3)如果新的支撑点t能够跨越原点，则加入到单纯形中，此时，

A、当单纯形中存在两个点，那么以这两点构成直线的垂线朝向原点的方向作为下一次循环迭代的方向；

B、当单纯形中存在三个点，那么判断此三点组成的三角形是否包含原点。如果包含原点，说明发生碰撞两个矩形虚拟防撞框发生碰撞，退出循环迭代；如果不包含原点，则保留距离原点最近的边上的两个点，同样以这两个点所形成直线的垂线朝向原点的方向作为下一次迭代方向；

4)跳转到步骤1)。

4.根据权利要求2所述的防碰撞运动规划方法，其特征在于，悬皮头部在第一象限，可以通过式(2)、(3)、(4)、(5)分别求得点a₁、a₂、a₃、a₄的坐标[x₁,y₁]、[x₂,y₂]、[x₃,y₃]、[x₄,y₄]。

5.根据权利要求2所述的防碰撞运动规划方法，其特征在于，通过式(6)、(7)、(8)、(9)可以求得大机悬皮头部在第二象限的虚拟防撞框空间坐标：

6.根据权利要求2所述的防碰撞运动规划方法，其特征在于，通过式(10)、(11)、(12)、(13)可以求得大机悬皮头部在第三象限的虚拟防撞框空间坐标：

7.根据权利要求2所述的防碰撞运动规划方法，其特征在于，通过式(14)、(15)、(16)、(17)可以求得大机悬皮头部在第四象限的虚拟防撞框空间坐标：

/>

Images