CN104236618A - 泵车间臂架姿态防碰撞检测方法及系统 - Google Patents

Abstract

本发明公开了一种泵车间臂架姿态防碰撞检测方法及系统，泵车为两台或两台以上，包括如下步骤：步骤a)获取泵车间各臂架的多个臂节姿态信息；步骤b)根据步骤a)所获信息，判断各臂架的多个臂节运动趋势；步骤c)根据步骤a)和步骤b)的信息，将各臂架的多个臂节姿态信息设于同一参考系后，再计算各臂架的多个臂节姿态信息和距离值；步骤d)根据步骤c)的计算值进行预警处理。该检测系统应用于至少两台泵车上，包括：姿态感知单元、趋势判断单元、处理单元和报警单元。使用该方法的泵车间臂架姿态防碰撞检测系统后，可提高群体作业时臂架的安全性，进而有效降低事故的发生率，因此应用前景十分广阔。

Description

泵车间臂架姿态防碰撞检测方法及系统

技术领域

本发明涉及泵车臂架姿态检测领域，特别是涉及一种泵车间臂架姿态防碰撞检测方法及系统。

背景技术

随着城市化的发展，泵车因其机动、灵活、高效等优点，已成为诸多现代化建设中不可缺少的重要施工设备。在泵车的操作过程中，操作员往往只能根据自身操作经验目测障碍物及目标的位置来操作臂架展开，或者是通过使用对讲机以语音交流的方式在指挥员的指挥下操作臂架展开。然而，随着泵车的操作环境越来越复杂恶劣，臂架越来越长，加之其臂节臂架个数增多，经常出现操作员难以或不能目测到臂架位置及其周边的情况，且语音方式进行交流时信息量有限且缺乏准确性，使操作员与指挥员之间的交流难以准确协调，容易导致操作员误操作，造成安全事故，故而泵车臂架的安全性操作显得尤为重要。

目前，对臂架全问题的研究主要是针对单台泵车，一般采用红外线或激光传感器对臂架与周边障碍物以及臂架与施工现场底部或顶部进行实时距离检测，根据距离来判断臂架的安全性，如中国发明专利申请CN 102621993A公开了一种臂架控制系统，包括：高度检测单元，用于实时检测臂架的高度信息，并根据高度信息获取最高位置高度；判断单元，用于根据最高位置高度，判断臂架的运动趋势；控制单元，根据臂架的最高位置高度、运动趋势及预定的高度阀值控制臂架的运动。但由于红外或激光传感器所发射的红外线或激光被臂架本身所阻挡，臂架与障碍物的碰撞信息检测可能不准确，所以中国发明专利申请CN 102032883A公开了一种利用了声音在固体中的传播来检测碰撞位置点的系统和方法，该检测系统包括：传感系统，至少包括一个位于机械装置的臂架的第一位置的第一噪声传感器和一个位于机械装置的臂架的第二位置的第二噪声传感器；中央控制单元，与传感系统连接，接收传感系统感测的噪声信号，根据第一噪声传感器和第二噪声传感器所接收到的时间差来判断臂架与障碍物的碰撞位置点。

然而，在大型工程和群体工程越来越多的大规模基础建设中，单台泵车已经无法满足施工需要，为了保证施工中对作业工程的全部覆盖以及工作效率，必然会出现多台泵车共同作业的情况，此时臂架的姿态信息对于泵车间臂架的安全检测就显得尤为重要。但就目前基于红外线或激光传感器的实时距离检测方法不适用于多台泵车间的臂架姿态的安全检测，因为多臂节臂架的折叠式放置会使得红外线或激光更容易被泵车本身的臂节所阻挡，因而无法及时的对不同泵车间的臂架所处相对姿态的安全与否进行检测；并且现有的基于各类角度传感器的臂架姿态获取方法由于传感器本身以及计算过程累计误差的影响已经难以满足智能化和安全化的要求。现有技术中，也出现了一些防碰撞算法或防碰撞预警系统相关专利，如基于包围盒技术的机械臂碰撞检测算法，但该算法主要是实现机械手臂对虚拟物体的抓取动作，针对此刻机械手臂与虚拟物体的位置关系进行碰撞判断，无法预先对其之间的动作进行碰撞预测；又如中国实用新型专利CN 202558512U公开了一种塔群防撞监控系统，包括多个防撞监控子系统，每个防撞监控子系统分别设置在塔群内的一个塔机上，每个防撞监控子系统包括微处理器、时钟、传感器单元、无线传输单元、键盘、显示器和报警电路；其中，防撞监控子系统的时钟的时间信息输出端连接微处理器的时间信息输入端，传感器单元的采集信号输出端连接微处理器的采集信号输入端，键盘的预置信号输出端连接微处理器的预置信号输入端，微处理器的显示信号输出端连接显示器的显示信号输入端，微处理器的报警信号输出端连接报警电路的报警信号输入端，无线传输单元的传输控制信号输入输出端连接微处理器的传输控制信号输出输入端，所有防撞监控子系统的无线传输单元之间通过无线通信的方式进行数据传输。上述专利虽然公开了塔群的防撞技术，但其只是针对多台设备的单个臂架进行预警，不适用于大型工程或群体工程对多台泵车间多个臂架的防碰撞预警。

因此，现在迫切需要提出一种泵车间的臂架姿态防碰撞检测方法及系统，以提高群体作业时臂架的安全性，进而有效降低事故的发生率。

发明内容

针对现有技术存在的上述问题，本发明的目的是提供一种泵车间臂架姿态防碰撞检测方法，适用于泵车群体作业时检测其安全性，进而降低事故的发生率。

为实现上述发明目的，本发明采用的技术方案如下：

一种泵车间臂架姿态防碰撞检测方法，泵车为两台或两台以上，包括如下步骤：

步骤a)获取泵车间各臂架的多个臂节姿态信息；

步骤b)根据步骤a)所获信息，判断各臂架的多个臂节运动趋势；

步骤c)根据步骤a)和步骤b)的信息，将各臂架的多个臂节姿态信息设于同一参考系后，再计算各臂架的多个臂节姿态信息和距离值；

步骤d)根据步骤c)的计算值进行预警处理。

优选地，步骤a)的具体操作为：获取距离信息，将获取的距离信息拟合成臂架姿态信息。

距离信息定义：臂架各臂节端点到泵车车体上多个不同位置节点间的距离值。

臂架姿态信息定义：参考方向到转台和臂架末端点在转台平面映射点连线的夹角及该连线的长度以及臂架末端点到转台平面的距离，其中从转台出发沿泵车车尾方向为参考方向。

更优选地，距离信息可通过安装在各臂节端点的未知节点与泵车车体上的已知节点来获得。

更优选地，姿态信息的拟合过程在安装于泵车转台处的中心节点(即簇头)内进行。

优选地，步骤b)的具体操作为：通过对多个连续的臂架的历史姿态信息进行分析，判断其运动趋势。

更优选地，臂架的运动趋势可通过选定某一臂节末端点运动时的多个连续的姿态信息相对转台平面的高度h为主要依据进行判断；具体来说：当h值呈现增大的趋势，则可以判断该节臂节的后续运动呈展开趋势；当h值呈现减小的趋势，则可以判断该节臂节的后续运动呈闭合趋势。

优选地，步骤c)的具体操作为：将至少两台泵车各自臂架的姿态信息转换到同一参考系中，根据同一参考系中臂架的当前姿态信息、趋势信息计算出臂架在未来多个时刻的姿态信息和不同泵车间各臂架的距离值。

更优选地，所述臂架姿态信息转换公式为：

$\left\{\begin{array}{c}r=\sqrt{{r_0}^2+{r_Q}^2+2r_0{r}_Q\cos (a_0-\mathrm{\γ}-a_Q)}\\ a=\arctan \frac{r_Q\·\sin a_Q+r_0\·\sin (a_0-\mathrm{\γ})}{r_Q\·\cos a_Q+r_0\·\cos (a_0-\mathrm{\γ})}\\ h=h_Q+h_0\end{array}\right.---\left(1\right),$

其中，定义W为泵车转台的中心点，从转台(W)出发沿泵车车尾方向(V)作为参考方向W₀-V₀和W-V表示两台不同泵车(如泵车0和泵车1)的参考系，(r,a,h)为(r₀,a₀,h₀)转换到W-V中的姿态信息，(r₀,a₀,h₀)为泵车0某测量点在W₀-V₀参考系中的姿态信息；γ为与的夹角，可通过事先标定得到，若V₀绕h轴逆时针旋转到V，则γ为正，反之为负；(r_Q,a_Q,h_Q)为W₀在W-V中的姿态信息。

更优选地，在未来t时刻臂架姿态信息的计算公式为：

$\left\{\begin{array}{c}{r}_{\mathrm{Bi}}=r_{\mathrm{Ai}-1}+(r_{\mathrm{Ai}}-r_{\mathrm{Ai}-1})\cos ({\mathrm{\ω}}_i\·t)-(h_{\mathrm{Ai}}-h_{\mathrm{Ai}-1})\sin ({\mathrm{\ω}}_i\·t)\\ a_{\mathrm{Bi}}=a_{\mathrm{Ai}-1}=a_{\mathrm{Ai}}\\ h_{\mathrm{Bi}}=h_{\mathrm{Ai}-1}+(r_{\mathrm{Ai}}-r_{\mathrm{Ai}-1})\sin ({\mathrm{\ω}}_i\·t)+(h_{\mathrm{Ai}}-h_{\mathrm{Ai}-1})\cos ({\mathrm{\ω}}_i\·t)\end{array}\right.---\left(2\right),$

其中，(r_Bi,a_Bi,h_Bi)为第i节臂节末端在t时刻的姿态信息；(r_Ai-1,a_Ai-1,h_Ai-1)和(r_Ai,a_Ai,h_Ai)分别表示第i-1节臂节和第i节臂节在当前时刻的姿态信息，可通过姿态感知单元获取；ω_i为第i节臂节的角速度，当预测运动趋势竖直展开(即向上)时ω_i＞0，反之，ω_i＜0。

进一步，当得到第i节臂节在t时刻的姿态信息后，且第i节臂节不是最后一节，则随着该臂节而运动的第i+1臂节末端的姿态信息(r_Bi+1,a_Bi+1,h_Bi+1)计算公式为：

$\left\{\begin{array}{c}{r}_{\mathrm{Bi}+1}=r_{\mathrm{Ai}-1}+(r_{\mathrm{Ai}+1}-r_{\mathrm{Ai}-1})\cos ({\mathrm{\ω}}_i\·t)-(h_{\mathrm{Ai}+1}-h_{\mathrm{Ai}-1})\sin ({\mathrm{\ω}}_i\·t)\\ a_{\mathrm{Bi}+1}=a_{\mathrm{Ai}+1}\\ h_{\mathrm{Bi}+1}=h_{\mathrm{Ai}-1}+(r_{\mathrm{Ai}+1}-r_{\mathrm{Ai}-1})\sin ({\mathrm{\ω}}_i\·t)+(h_{\mathrm{Ai}+1}-h_{\mathrm{Ai}-1})\cos ({\mathrm{\ω}}_i\·t)\end{array}\right.---\left(3\right)$

其中，(r_Ai+1,a_Ai+1,h_Ai+1)表示i+1臂节末端的当前时刻的姿态信息，可通过姿态感知单元获取，各臂节在t时刻的姿态信息构成了整个臂架的姿态信息。

判断或计算不同泵车间各臂架的距离值，通过将结构相似的泵车臂架的各臂节看做是中间部分半径为D的圆柱体，两端部分半径为d的半球体模型，从而不同泵车臂架间的各臂节的距离可由根据空间两线段间距离求解方法求出的同一参考系中臂节模型中心轴线的距离值与对应臂节模型参数之差所决定。

优选地，步骤d)的具体操作为：判断不同泵车间各臂架所处相对姿态是否安全，做出相应的预警处理。

判断所处相对姿态是否将会发生碰撞的标准是将所求的不同泵车臂架间的各臂节的距离值与所设安全距离进行比较。安全距离为1～1.5米，当臂架间的各臂节的距离值小于1米时，为高危安全等级；距离值为1～1.5米时，为低危安全等级；距离值大于1.5米时，为安全等级。

更优选地，当某一预测时刻臂架姿态不安全时，计算出从开始到该危险时刻的前一时刻的总时间，作为安全运动时间，其中，某一预测时刻姿态不安全包括不同泵车间至少有一对臂节的相对姿态不安全。

更优选地，若根据某一预测时刻臂架的姿态信息得出的不同泵车间臂架距离值小于或等于安全距离值，则发出警示信息，将安全运动时间记录下来，以此作为操作员安全操作臂架运动的依据。

本发明的另一目的是提供一种泵车间臂架姿态防碰撞检测系统，该泵车间臂架姿态防碰撞系统采用上述检测方法解决在群体工程中存在的多台泵车的臂架作业面相互重叠而引发的安全问题。

泵车间臂架姿态防碰撞检测系统，该检测系统分别应用于至少两台泵车上，包括：

姿态感知单元，包括信息获取子单元和姿态拟合子单元，用于获取泵车间各臂架的多个臂节姿态信息；

趋势判断单元，用于根据姿态感知单元所获信息，进行判断各臂架的多个臂节运动趋势；

处理单元，包括姿态信息转换子单元和预处理子单元，用于根据臂架姿态信息、趋势信息来判断在未来多个时刻不同泵车间臂架所处相对姿态的安全与否；

报警单元，与处理单元相连，根据处理单元的输出信息做出相应的报警处理；

此外该检测系统的检测方法采用上述任一检测方法。

优选地，信息获取子单元用于获取距离信息，即获取臂架各臂节端点到泵车车体上多个不同位置点间的距离值。

优选地，姿态拟合子单元用于将上述所获取的距离信息拟合成为臂架姿态信息，即拟合所得信息包括参考方向到转台和臂架末端点在转台平面映射点连线的夹角及该连线的长度以及臂架末端点到转台平面的距离，其中从转台出发沿泵车车尾方向为参考方向。

优选地，姿态信息转换子单元用于将至少两台泵车各自臂架的姿态信息同步转换到同一参考系中。

优选地，预处理子单元用于根据同一参考系中臂架当前姿态信息、趋势信息来计算出臂架在未来多个时刻的姿态信息，并由此姿态信息计算出的不同泵车间各臂架的距离值来判断其相对姿态是否安全。

更优选地，预处理子单元还包括当某一预测时刻臂架姿态不安全时，计算出从开始到该危险时刻的前一时刻的总时间，作为安全运动时间。

优选地，报警单元在接收到处理单元发出的在未来某时刻的危险信息时，发出报警信号，并记录安全运动时间，以此作为操作员安全操作臂架运动的依据。

更优选地，报警信号为包括在未来某一时刻将要发生碰撞的具体臂节对信息，换言之，即各臂节间的距离与安全距离间的比较结果。

相比现有技术，本发明提供的泵车间臂架姿态防碰撞检测方法及系统具有以下有益效果：

1.该检测方法及系统适用于大型工程或群体工程中多台泵车间臂架姿态的防碰撞检测；

2.该检测方法及系统是基于测距值来拟合臂架姿态，可以对多个未来时刻的臂架所处姿态进行计算，更加简便；

3.该检测方法及系统基于该姿态信息对未来不同时刻的泵车之间的臂架所处相对姿态进行防碰撞检测，避免了采用红外线或激光传感器时，所发出的红外线或激光被泵车本身的臂节所阻挡，影响泵车之间的臂架姿态防碰撞检测的情况；

4.该检测方法及系统对多个未来时刻进行臂架姿态防碰撞检测，从而在预先检测到危险信息时，可以计算并记录下安全运动时间，以此作为操作员操作臂架安全运动的时间依据；

5.该检测方法及系统根据当前时刻到未来某一时刻臂架发生危险的具体臂节信息，来提醒操作员对该发生危险的臂节进行相应控制，从而提高群体作业时臂架的安全性，进而有效降低事故的发生率。

综上述，本发明提供的泵车间臂架姿态防碰撞检测方法及系统可有效的解决现有技术中针对泵车间臂架无法及时乃至提前获得有效的安全姿态信息等技术难题，通过本发明提供的检测方法和系统可精确的获得多个泵车间臂架的多个臂节的实时及趋势信息，获得相应的安全距离和安全操作时间，提高群体作业时臂架的安全性，进而有效降低事故的发生率。

附图说明

图1为本发明提供的泵车间臂架防碰撞检测方法的流程示意图；

图2为本发明提供的泵车间臂架防碰撞检测方法及系统中的泵车臂架姿态信息示意图；

图3为本发明提供的泵车间臂架防碰撞检测方法及系统中相邻两泵车防碰撞简化示意图；

图4为本发明提供的泵车间臂架防碰撞检测方法及系统中不同泵车间臂架姿态信息的转换示意图；

图5为本发明提供的泵车间臂架防碰撞检测方法及系统中臂架在未来某一时刻的姿态信息计算示意图；

图6为本发明提供的泵车间臂架防碰撞检测方法及系统中泵车间臂架姿态防碰撞检测系统的拓扑图；

图7为本发明实施例1中两泵车在当前时刻与预测时刻所处相对姿态仿真模拟示意图。

具体实施方式

为了能够对本发明有进一步的理解，下述内容以两台四节臂混凝土泵车为例，结合附图和实施例对本发明进行进一步详细说明。

本发明提供了一种泵车间臂架姿态防碰撞检测方法，图1为该检测方法的流程示意图，如图1所示，该方法具体包括以下步骤：

步骤a、获取臂架的姿态信息：

即在获取臂架姿态信息时，首先获取至少两台泵车上各节点的距离信息，其次将所获取的距离信息拟合成臂架姿态信息。距离信息为臂架各臂节端点到泵车车体上多个不同位置点间的距离值，通过安装在各臂节端点的未知节点与泵车车体上的已知节点来获得。姿态信息包括参考方向到转台和臂架末端点在转台平面映射点连线的夹角及该连线的长度以及臂架末端点到转台平面的距离，其中从转台出发沿泵车车尾方向为参考方向；该姿态信息的拟合过程在安装于泵车转台处的中心节点(即簇头)内进行。

图2为泵车臂架姿态信息构成示意图，以转台中心W为原点，从转台(W)出发沿泵车车尾方向(V)作为参考方向则夹角a及长度r以及距离h构成了臂架姿态信息。图3为相邻两泵车防碰撞简化示意图，如图3所示，两泵车均停靠在水平面上，对于泵车1，其中b₀为转台，标记为W，为参考方向，b₁、b₂、b₃、b₄为泵车1各臂节末端点；泵车0的转台标记为W₀，为参考方向。

步骤b、根据所获取的臂架姿态信息判断臂架的运动趋势：

通过得到泵车上各节点的历史姿态信息判断臂架的运动趋势，如得到泵车1的第一节臂节末端点b₁运动时的多个连续的姿态信息，选定其相对转台平面的高度值h为主要依据，如果这些值呈现增大的趋势，则可以判断第一节臂节的后续运动呈展开趋势，如果这些值呈现减小的趋势，则可以判断第一节臂节的后续运动呈闭合趋势。连续采取两个时刻b1的姿态信息，则可以根据姿态信息得到b₁相对于转台平面的高度值h₁和h₂，若h₂-h₁＞0，则判断后续运动呈展开趋势，反之，则判断后续运动呈闭合趋势。

步骤c、根据臂架姿态信息及趋势信息来判断在未来多个时刻不同泵车间臂架所处相对姿态的安全与否。

该过程可以分为三步进行：

第一步，将至少两台泵车各自臂架的姿态信息转换到同一参考系中；如图4所示为不同泵车间臂架姿态信息的转换示意图。设W₀和W分别为泵车0和1转台的中心点，和分别为其参考方向；则W₀-V₀中一测量点P₀(r₀,a₀,h₀)转换到W-V中的姿态信息的计算公式为：

$\left\{\begin{array}{c}r=\sqrt{{r_0}^2+{r_Q}^2+2r_0{r}_Q\cos (a_0-\mathrm{\γ}-a_Q)}\\ a=\arctan \frac{r_Q\·\sin a_Q+r_0\·\sin (a_0-\mathrm{\γ})}{r_Q\·\cos a_Q+r_0\·\cos (a_0-\mathrm{\γ})}\\ h=h_Q+h_0\end{array}\right.---\left(1\right)$

其中，Q(r_Q,a_Q,h_Q)为W₀在W-V下的姿态信息,可通过泵车1车体上的4个节点对泵车0转台处的簇头进行测距，在泵车1的簇头内进行姿态拟合得到；γ为与的夹角，可通过事先标定得到，若V₀绕h轴逆时针旋转到V，则γ为正，反之为负。

第二步，根据同一参考系中臂架当前姿态信息、趋势信息来计算出臂架在未来多个时刻的姿态信息，并由此姿态信息计算出的不同泵车间各臂架的距离值来判断其相对姿态是否安全。当某一预测时刻臂架姿态不安全时，计算出从开始到该危险时刻的前一时刻的总时间，作为安全运动时间。某一预测时刻姿态不安全包括不同泵车间至少有一对臂节的相对姿态不安全。

在泵车0和泵车1间各自的当前臂架姿态信息通过置于泵车车体上的簇头进行无线通信传输与同步转换后，对处于同一参考系中的两泵车的臂架在未来时刻所处姿态信息进行计算。图5为臂架在未来某一时刻的姿态信息计算示意图，如图5所示，若泵车1第2节臂节运动(即i＝2)，在运动趋势上的未来时刻t的姿态信息的计算公式为：

$\left\{\begin{array}{c}{r}_{B2}=r_{A1}+(r_{A2}-r_{A1})\cos ({\mathrm{\ω}}_2\·t)-(h_{A2}-h_{A1})\sin ({\mathrm{\ω}}_2\·t)\\ a_{B2}=a_{A2}\\ h_{B2}=h_{A1}+(r_{A2}-r_{A1})\sin ({\mathrm{\ω}}_2\·t)+(h_{A2}-h_{A1})\cos ({\mathrm{\ω}}_2\·t)\end{array}\right.---\left(2\right)$

其中，(r_B2,a_B2,h_B2)为第2节臂节末端(b₂)在t时刻的姿态信息，(r_A1,a_A1,h_A1)和(r_A2,a_A2,h_A2)分别表示第1节臂节末端(b₁)和第2节臂节末端(b₂)在泵车1的W-V中的当前时刻的姿态信息，可通过姿态感知单元获得；ω₂为第2节臂节的角速度，当预测运动趋势竖直展开(即向上)时ω₂＞0，反之，ω₂＜0。当得到第2节臂节的姿态信息，随着该臂节而运动的第3臂节末端(b₃)的姿态信息(r_B3,a_B3,h_B3)和第4臂节末端(b₄)的姿态信息(r_B4,a_B4,h_B4)计算公式为：

$\left\{\begin{array}{c}{r}_{B3}=r_{A1}+(r_{A3}-r_{A1})\cos ({\mathrm{\ω}}_2\·t)-(h_{A3}-h_{A1})\sin ({\mathrm{\ω}}_2\·t)\\ a_{B3}=a_{A3}\\ h_{B3}=h_{A1}+(r_{A3}-r_{A1})\sin ({\mathrm{\ω}}_2\·t)+(h_{A3}-h_{A1})\cos ({\mathrm{\ω}}_2\·t)\end{array}\right.---\left(3.1\right)$

$\left\{\begin{array}{c}{r}_{B4}=r_{A1}+(r_{A4}-r_{A1})\cos ({\mathrm{\ω}}_2\·t)-(h_{A4}-h_{A1})\sin ({\mathrm{\ω}}_2\·t)\\ a_{B4}=a_{A4}\\ h_{B4}=h_{A1}+(r_{A4}-r_{A1})\sin ({\mathrm{\ω}}_2\·t)+(h_{A4}-h_{A1})\cos ({\mathrm{\ω}}_2\·t)\end{array}\right.---\left(3.2\right)$

其中，(r_A3,a_A3,h_A3)和(r_A4,a_A4,h_A4)分别表示第3臂节和第4臂节末端(b₃、b₄)的当前时刻的姿态信息。此时对臂节2在t时刻姿态信息进行计算时，将得到泵车1在t时刻的整体臂架姿态信息。由此，以Δt为时间间隔时，可计算出t₁、t₂、t₃、t₄等多个未来时刻的臂架姿态信息。

第三步，由上述方法得到两泵车臂架未来某一时刻的姿态信息，计算出泵车0臂架与泵车1臂架之间的距离。由于泵车臂架各臂节的结构相似，根据上述姿态信息计算距离可将其分解为泵车0和1之间的臂节对的距离求解。

以下对一种优选的臂节对的距离求解方法作简要说明：

已知臂节S₁和S₂，E₁(r_E1,a_E1,h_E1)、Q₁(r_Q1,a_Q1,h_Q1)为S₁的中心轴线始端和末端的姿态信息，E₂(r_E2,a_E2,h_E2)、Q₂(r_Q2,a_Q2,h_Q2)为S₂的中心轴线始端和末端姿态信息，求解距离的前提条件是知道两臂节中心轴线上距离最近时的点的信息。求解臂节S₁中心轴上任意一点信息的相关方程为：S₁(s)＝E′₁+s·d₁，其中d₁＝Q′₁-E′₁,0≤s≤1，Q′₁(r_Q1cosa_Q1,r_Q1sina_Q1,h_Q1)，E′₁(r_E1cosa_E1,r_E1sina_E1,h_E1)；求解臂节S₂中心轴线上任意一点信息的相关方程为：S₂(t)＝E′₂+t·d₂，其中d₂＝Q′₂-E′₂,0≤t≤1，Q′₂(r_Q2cosa_Q2,r_Q2sina_Q2,h_Q2)，E′₂(r_E2cosa_E2,r_E2sina_E2,h_E2)。采用高斯消元法求解得出s、t的简化表达式为：s＝(b*t-c)/a,t＝(b*s+f)/e。其中，a＝d₁·d₁，b＝d₁·d₂，c＝d₁·r，e＝d₂·d₂，f＝d₂·r，r＝E′₁-E′₂。当a*e-b*b＝0时，s＝0，否则s＝(b*f-c*e)/(a*e-b*b)，若s值满足(0,1)，则将其带入上述求t的公式，否则选择最接近s的值0或者1带入求t的公式。若所求t值满足(0,1)，则将s、t值带入求臂节上任意一点信息的相关方程进行求解；若所求t值不满足(0,1)，则选择最接近0或者1的值为t值，并将其带入上述变量为t的求s的公式，若s不满足(0,1)则选择最接近0或者1的值为s值，否则所求即为s值，将此时的s、t值带入求臂节中心轴线上任意一点信息的相关方程进行求解，得出两臂节中心轴线距离最近时的两点的姿态信息，通过该信息可求出此时的距离L，该L与对应臂节模型参数之差即为臂节对的距离。

结合臂架的圆柱模型结构假设D为臂节中间部分半径，d为两端半球半径，以及所设定的安全距离Ls，当s、t值均满足(0,1)时，在未来某一时刻臂架所处姿态的臂节中心轴间的距离L小于或等于泵车1臂节的D与泵车0臂节的D与Ls之和，则说明该时刻所处姿态不安全；当s、t值均不满足(0,1)，在未来某一时刻臂架所处姿态的臂节中心轴间的距离L小于或等于泵车1臂节的d与泵车0臂节的d与Ls之和，则说明该时刻所处姿态不安全；当s、t值中有一个不满足(0,1)，在未来某一时刻臂架所处姿态的臂节中心轴间的距离L小于或等于泵车1臂节的d与泵车0臂节的D与Ls之和或泵车1臂节的D与泵车0臂节的d与Ls之和，则说明该时刻所处姿态不安全。对泵车0和1之间的臂架在某时刻进行防碰撞检测时，只要有一组臂节不安全，则整个臂架在即将运动到的时刻所处姿态不安全。对于t₁、t₂、t₃、t₄等多个未来时刻，若在t₄时刻时姿态不安全，则此时的安全运动时间可为2Δt。

步骤d、根据计算结果进行预警处理：

当判断出某一预测时刻臂架姿态不安全时，发出报警信息，并计算出从开始到该危险时刻的前一时刻的总时间，作为安全运动时间，操作人员可以此作为操控臂架安全运动的依据；其中，某一预测时刻姿态不安全包括不同泵车间至少有一对臂节的相对姿态不安全。其中若所设定的安全距离Ls有不同的级别，则此时所发出的报警信号则根据级别的不同发出不同的提示音，并且可根据在当前时刻到未来某一时刻臂架发生危险的具体臂节信息，来提醒操作员对该发生危险的臂节进行相应控制。

因为不同泵车臂架的姿态信息所处的参考系不同，所以在对未来某一时刻两泵车间臂架所处相对姿态进行防碰撞检测时，必须将不同泵车的臂架姿态信息进行同步转换至同一参考系中，所说的参考系可以是其中一台泵车所在的系，不同泵车间各自的臂架姿态信息可以通过置于泵车车体上的簇头进行无线通信传输与同步转换。泵车臂架是由多节臂节组成，若某一臂节运动，且该臂节不是臂架的最后一节，则得到该臂节在未来某一时刻的姿态信息时，还需要计算出随着该臂节而运动的其它臂节的姿态信息，从而得到臂架的整体姿态信息，进而对两台泵车间臂架进行距离计算，由此来判断两泵车所处相对姿态是否安全。

图6为本发明提供的泵车间臂架姿态防碰撞检测系统的拓扑图，如图6所示，该泵车间臂架姿态安全检测系统11应用于至少两台泵车上，包括：姿态感知单元111、趋势判断单元112、处理单元113以及报警单元114。

姿态感知单元111用于获取臂架的姿态信息，包括信息获取子单元111A，用于获取臂架各臂节端点到泵车车体上多个不同节点间的距离值；姿态拟合子单元111B，用于将上述所获取的测距值拟合成为臂架姿态信息。其中，距离信息可通过安装在各臂节端点的未知节点与泵车车体上4个已知节点来获得，可采用到达时间差(TOA，Time of Arrival)来获取，所说4个已知节点不能布置在同一水平面上。并且姿态信息的拟合过程可在簇头内进行，所说簇头是指安装在泵车转台处的中心节点。感知的信息除了泵车自身臂架端点的姿态信息还包括相邻泵车簇头在该泵车中的姿态信息。

趋势判断单元112用于根据所获取的臂架姿态信息，判断臂架的运动趋势。可以根据姿态感知单元111得到多个连续的臂架姿态信息，通过对这些姿态信息进行分析来判断其运动趋势。可以选定与转台平面的距离值为主要依据，对这些值进行分析：如果这些值呈现增大的趋势，则可以判断臂架的后续运动呈展开趋势；如果这些值呈现减小的趋势，则可以判断臂架的后续运动呈闭合趋势，否则，则判断臂架无运动趋势。

处理单元113用于根据臂架姿态信息、趋势信息来判断在未来多个时刻不同泵车间臂架所处相对姿态的安全与否，包括姿态信息转换子单元113A，用于将至少两台泵车各自臂架的姿态信息同步转换到同一参考系中；预处理子单元113B，用于根据同一参考系中臂架当前姿态信息、趋势信息来计算出臂架在未来多个时刻的姿态信息，并由此姿态信息计算出的不同泵车间各臂架的距离值来判断其相对姿态是否安全。其中，预处理子单元还包括当某一预测时刻臂架姿态不安全时，计算出从开始到该危险时刻的前一时刻的总时间，作为安全运动时间。某一预测时刻姿态不安全包括不同泵车间至少有一对臂节的相对姿态不安全。

报警单元114，与处理单元相连，根据处理单元的输出信息做出相应的处理。在接收到处理单元发出的在未来某时刻的危险信息时，发出报警信号，并将安全时间记录下来，以此作为操作员安全操作臂架运动的依据。其中若所设定的安全距离Ls有不同的级别，则此时所发出的报警信号则根据级别的不同发出不同的提示音，并且可根据在当前时刻到未来某一时刻臂架发生危险的具体臂节信息，来提醒操作员对该发生危险的臂节进行相应控制。

将上述检测系统分别应用至泵车间，并采用上述检测方法对泵车间臂架进行仿真防碰撞检测，具体结果如实施例1中表1～4所示。

实施例1

预设同一工地上的两台型号相同的泵车(BC0和BC1)，其结构参数为：整车全长11.520米、全宽2.5米、全高3.990米；臂架垂直高度40.3米、水平长度38.8米、垂直深度26.8米、最小展开高度11.8米；第一节臂长度8.170米、第二节臂长度6.820米、第三节臂长度6.825米、第四节臂长度8.460米、转台旋转角度±270°；安全距离为1.5米。图7为此两泵车的防碰撞仿真示意图，如图7所示，其中包括当前时刻和未来4s时刻的两泵车臂架所处的相对姿态。通过上述检测方法及系统分别获得并计算BC1和BC0的当前时刻的各自臂架姿态信息(r,a,h)，计算结果见表1：

表1

	BC1	BC0	BC0-BC1
				转台	(0,0,0)	(0,0,0)	(29,0,0)
第一节臂末端	(2.794,pi/4,7.676)	(1.419,8*pi/9,8.046)	(28.109,0.039,8.046)
				第二节臂末端	(9.405,pi/4,6)	(7.413,8*pi/9,11.3)	(24.891,0.230,11.3)
第三节臂末端	(16.23,pi/4,4)	(14.202,8*pi/9,12)	(22.654,0.501,12)
				第四节臂末端	(24.635,pi/4,2)	(22.422,8*pi/9,10)	(22.535,0.867,10)

如表1所示：其中第二列(BC1列)为当前时刻泵车BC1臂架的姿态信息；第三列(BC0列)为当前时刻泵车BC0臂架的姿态信息；第四列(BC0-BC1列)为当前时刻泵车BC0的臂架姿态信息通过置于转台处的簇头进行无线通信传输到BC1的簇头，然后根据臂架姿态信息转换公式(见公式(1))计算得到的BC0在BC1中的姿态信息，其中BC0的转台在BC1中的信息为(29,0,0)，参考方向夹角γ为pi/6。然后采取臂节对的距离求解方法对处于同一参考系中的BC1和BC0的当前时刻臂架所处姿态进行碰撞仿真得到的距离(单位：米)计算结果见表2：

表2

如表2所示，将BC1与BC0各臂节对的计算所得间距与安全距离相比较得出：BC1和BC0间臂架当前所处相对姿态安全。

进一步，在当前时刻两泵车臂架所处相对姿态安全时，对下一时刻两泵车臂架姿态进行碰撞预测，假设当前主动泵车为BC1，即操作员将对BC1的第二节臂进行展开的动作，取动作时间为4s，BC0姿态不变，通过上述检测方法及系统分别获得并计算BC1和BC0在未来4s时刻的各自臂架姿态信息(r,a,h)，计算结果见表3：

表3

	4s时BC1	BC0-BC1
			转台	(0,0,0)	(29,0,0)
第一节臂末端	(2.794,pi/4,7.676)	(28.109,0.039,8.046)
			第二节臂末端	(9.614,pi/4,7.671)	(24.891,0.230,11.3)
第三节臂末端	(16.721,pi/4,7.405)	(22.654,0.501,12)
			第四节臂末端	(25.360,pi/4,7.526)	(22.535,0.867,10)

如表3所示，表3中第二列(4s时BC1)为未来4s时刻泵车BC1臂架的姿态信息(见公式(2)、(3.1)、(3.2))；第三列(BC0-BC1)为泵车BC0的臂架姿态信息通过置于转台处的簇头进行无线通信传输到BC1的簇头，然后根据臂架姿态信息转换公式计算得到的BC0在BC1中的姿态信息。然后采取臂节对的距离求解方法对处于同一参考系中的BC1和BC0在未来4s时刻臂架所处姿态进行防碰碰撞仿真得到的距离(单位：米)计算结果见表4：

表4

如表4所示，将BC1与BC0各臂节对的计算所得间距与安全距离相比较得出：BC1的第四节臂与BC0的第四节臂所处相对姿态不安全。说明此刻操作员如果对BC1的第二节臂进行展开动作，4s时刻BC1的第二节臂与BC0的4个臂节所处姿态均安全，且随着BC1第二节臂运动的第三节臂与BC0的4个臂节所处姿态均安全，但是BC1第四节臂与BC0第四节臂所处姿态不安全，那么根据该信息，操作员可以选择同时操作第二节臂和第四节臂动作来避免危险的发生。该检测结果与仿真测量结果相同。

最后有必要在此说明的是：以上实施例只用于对本发明的技术方案作进一步详细地说明，不能理解为对本发明保护范围的限制，本领域的技术人员根据本发明的上述内容做出的一些非本质的改进和调整均属于本发明的保护范围。

Claims (10)

1.泵车间臂架姿态防碰撞检测方法，其特征在于，泵车为两台或两台以上，包括如下步骤：

步骤a)获取泵车间各臂架的多个臂节姿态信息；

步骤b)根据步骤a)所获信息，判断各臂架的多个臂节运动趋势；

步骤c)根据步骤a)和步骤b)的信息，将各臂架的多个臂节姿态信息设于同一参考系后，再计算各臂架的多个臂节姿态信息和距离值；

步骤d)根据步骤c)的计算值进行预警处理。

2.根据权利要求1所述的泵车间臂架姿态防碰撞检测方法，其特征在于，步骤a)的具体操作为：获取距离信息，将获取的距离信息拟合成臂架姿态信息。

3.根据权利要求1所述的泵车间臂架姿态防碰撞检测方法，其特征在于，步骤b)的具体操作为：通过对多个连续的臂架的历史姿态信息进行分析，判断其运动趋势。

4.根据权利要求1所述的泵车间臂架姿态防碰撞检测方法，其特征在于，步骤c)的具体操作为：将至少两台泵车各自臂架的姿态信息转换到同一参考系中，根据同一参考系中臂架的当前姿态信息、趋势信息计算出臂架在未来多个时刻的姿态信息和不同泵车间各臂架的距离值。

5.根据权利要求4所述的泵车间臂架姿态防碰撞检测方法，其特征在于：所述臂架姿态信息转换公式为： $\left\{\begin{array}{c}r=\sqrt{{r_0}^2+{r_Q}^2+2r_0{r}_Q\cos (a_0-\mathrm{\γ}-a_Q)}\\ a=\arctan \frac{r_Q\·\sin a_Q+r_0\·\sin (a_0-\mathrm{\γ})}{r_Q\·\cos a_Q+r_0\·\cos (a_0-\mathrm{\γ})}\\ h=h_Q+h_0\end{array}\right.,$ 其中，定义W为泵车转台的中心点，从转台(W)出发沿泵车车尾方向(V)作为参考方向W₀-V₀和W-V表示两台不同泵车(如泵车0和泵车1)的参考系，(r,a,h)为(r₀,a₀,h₀)转换到W-V中的姿态信息，(r₀,a₀,h₀)为泵车0某测量点在W₀-V₀参考系中的姿态信息；γ为与的夹角，可通过事先标定得到，若V₀绕h轴逆时针旋转到V，则γ为正，反之为负；(r_Q,a_Q,h_Q)为W₀在W-V中的姿态信息。

6.根据权利要求4所述的泵车间臂架姿态防碰撞检测方法，其特征在于，所述未来t时刻臂架姿态信息计算公式为： $\left\{\begin{array}{c}{r}_{\mathrm{Bi}}=r_{\mathrm{Ai}-1}+(r_{\mathrm{Ai}}-r_{\mathrm{Ai}-1})\cos ({\mathrm{\ω}}_i\·t)-(h_{\mathrm{Ai}}-h_{\mathrm{Ai}-1})\sin ({\mathrm{\ω}}_i\·t)\\ a_{\mathrm{Bi}}=a_{\mathrm{Ai}-1}=a_{\mathrm{Ai}}\\ h_{\mathrm{Bi}}=h_{\mathrm{Ai}-1}+(r_{\mathrm{Ai}}-r_{\mathrm{Ai}-1})\sin ({\mathrm{\ω}}_i\·t)+(h_{\mathrm{Ai}}-h_{\mathrm{Ai}-1})\cos ({\mathrm{\ω}}_i\·t)\end{array}\right.,$ 其中，(r_Bi,a_Bi,h_Bi)为第i节臂节末端在t时刻的姿态信息；(r_Ai-1,a_Ai-1,h_Ai-1)和(r_Ai,a_Ai,h_Ai)分别表示第i-1节臂节和第i节臂节在当前时刻的姿态信息，可通过姿态感知单元获取；ω_i为第i节臂节的角速度，当预测运动趋势竖直展开(即向上)时ω_i＞0，反之，ω_i＜0。

7.根据权利要求4所述的泵车间臂架姿态防碰撞检测方法，其特征在于，当得到第i节臂节在t时刻的姿态信息后，且第i节臂节不是最后一节，则随着该臂节而运动的第i+1臂节末端的姿态信息(r_Bi+1,a_Bi+1,h_Bi+1)计算公式为：

$\left\{\begin{array}{c}{r}_{\mathrm{Bi}+1}=r_{\mathrm{Ai}-1}+(r_{\mathrm{Ai}+1}-r_{\mathrm{Ai}-1})\cos ({\mathrm{\ω}}_i\·t)-(h_{\mathrm{Ai}+1}-h_{\mathrm{Ai}-1})\sin ({\mathrm{\ω}}_i\·t)\\ a_{\mathrm{Bi}+1}=a_{\mathrm{Ai}+1}\\ h_{\mathrm{Bi}+1}=h_{\mathrm{Ai}-1}+(r_{\mathrm{Ai}+1}-r_{\mathrm{Ai}-1})\sin ({\mathrm{\ω}}_i\·t)+(h_{\mathrm{Ai}+1}-h_{\mathrm{Ai}-1})\cos ({\mathrm{\ω}}_i\·t)\end{array}\right.,$ 其中，(r_Ai+1,a_Ai+1,h_Ai+1)表示i+1臂节末端的当前时刻的姿态信息，可通过姿态感知单元获取，各臂节在t时刻的姿态信息构成了整个臂架的姿态信息。

8.根据权利要求1所述的泵车间臂架姿态防碰撞检测方法，其特征在于，步骤d)的具体操作为：判断不同泵车间各臂架所处相对姿态是否安全，做出相应的预警处理。

9.根据权利要求8所述的泵车间臂架姿态防碰撞检测方法，其特征在于：若根据某一预测时刻臂架的姿态信息得出的不同泵车间臂架距离值小于或等于安全距离值，则发出警示信息，将安全运动时间记录下来，以此作为操作员安全操作臂架运动的依据。

10.泵车间臂架姿态防碰撞检测系统，其特征在于，该检测系统分别应用于至少两台泵车上，包括：

姿态感知单元，包括信息获取子单元和姿态拟合子单元，用于获取泵车间各臂架的多个臂节姿态信息；

趋势判断单元，用于根据姿态感知单元所获信息，进行判断各臂架的多个臂节运动趋势；

处理单元，包括姿态信息转换子单元和预处理子单元，用于根据臂架姿态信息、趋势信息来判断在未来多个时刻不同泵车间臂架所处相对姿态的安全与否；

报警单元，与处理单元相连，根据处理单元的输出信息做出相应的报警处理；

此外该检测系统的检测方法为权利要求1～20任一所述的检测方法。