CN104265274A - 一种快速准确实现凿岩钻机断面平行孔自动定位的方法 - Google Patents

Abstract

本发明公开了一种快速准确实现凿岩钻机断面平行孔自动定位的方法，其内容是：分析液压凿岩钻机钻臂定位机构结构组成及各机构动作，根据液压凿岩钻机钻臂系统结构模型，利用D-H法建立液压凿岩钻机钻臂机构关节坐标系，对钻臂定位系统坐标系及连杆参数进行分析，计算出定位末端在基坐标系中的坐标与全部关节变量的函数关系；液压凿岩钻机断面平行孔区域规划：对全部关节变量进行求解：确定凿岩钻机在钻孔过程中定位末端的预定位位置，并计算出在基坐标系中定位末端的预定位坐标和全部目标关节变量，通过闭环实时驱动控制模块调节各个液压缸，判断定位末端是否达预定位位置。本发明大大降低了液压凿岩钻机钻臂系统关节变量的求解难度。

Description

一种快速准确实现凿岩钻机断面平行孔自动定位的方法

技术领域

本发明涉及液压凿岩钻机智能控制技术领域，尤其涉及一种快速准确的实现断面平行孔自动定位的方法。 

背景技术

随着石方开挖施工技术及先进生产工艺的日趋成熟和推广应用,液压凿岩钻机技术取得了很大的发展，如在深孔梯段爆破、大孔径深孔预裂爆破、光面爆破、矿山削顶及开采、交通建设的修边护坡、锚索孔及灌浆孔等的凿岩作业中，都得到了广泛的应用。目前在凿岩开采过程中，隧道断面钻平行孔进行爆破应用最为广泛。然而，在钻平行孔的过程中，钻孔位置的准确定位是整个凿岩钻孔施工的关键，现在普遍采用的方法是通过人工遥控钻臂的运动，实现孔眼的定位，这种方法孔眼定位不准确，效率低，从而严重影响了爆破效果。 

为了解决人工定位的不足，智能化自动定位已经成为凿岩钻机智能控制领域的发展趋势。然而由于液压凿岩钻机工作环境复杂，更由于钻臂系统是一组多自由度度机构，对钻臂末端定位时，需要对凿岩钻机钻臂系统进行运动学正解和逆解分析。然而，对于多自由度串联机构而言，运动学正解较为容易，逆解一直是机器人领域研究的难点，目前所采用的方法大都需要求解大量的非线性超越方程，因而求解速度缓慢、过程复杂，不适应于工程机械通用控制器的要求。 

发明内容

为了克服现有技术存在的上述不足，本发明提供一种快速准确实现液压凿岩钻机断面平行孔自动定位的方法，该方法适应于通用的工程机械控制器，并 具有计算过程简单、结果准确、利于控制、实时性高等优点。 

本发明解决其技术问题所采用的技术方案是： 

一种快速准确实现凿岩钻机断面平行孔自动定位的方法，其具体步骤如下： 

步骤1： 

分析液压凿岩钻机钻臂定位机构结构组成及各机构动作； 

步骤2： 

根据液压凿岩钻机钻臂系统结构模型，利用D-H法建立液压凿岩钻机钻臂机构关节坐标系； 

步骤3： 

对钻臂定位系统坐标系及连杆参数进行分析，计算出定位末端在基坐标系中的坐标与全部关节变量的函数关系； 

步骤4： 

液压凿岩钻机断面平行孔区域规划；钻机的钻臂运动在断面区域内形成一个无规则的平面定位区域，由于推进器的移动，最终形成是一个立体空间；依次将钻臂与推进器协调摆动到最大值，可知断面平行孔定位区域，并确定区域边界范围；断面的平行孔定位区域通常是无规则的，在确定钻臂定位末端位置坐标时，为了便于判断，将无规则的边界进行线性化修正处理，使得修正区域最大限度满足实际区域； 

步骤5： 

对全部关节变量进行求解；依据平行孔钻孔原则，在自动定位的过程中，为了使定位平稳，利于自动控制，尽量不让臂架作±180°运动，由于此限制减少了臂架运动的自由度个数，从而减少了求解关节变量的数目；自动定位的过程中，只考虑臂架的定位，不考虑推进器的钻孔过程，也就是钎杆的运动末端 不考虑，所以可将钻进运动形成的关节变量看作固定量；在钻凿平行孔时，臂架的摆动和俯仰运动与推进器的摆动和俯仰运动是成对存在的，才能保证凿岩钻孔为平行孔，因此，将对称变化的关节变量进行归纳为一个变量；通过上述限制将求解关节变量的个数限制为三个，在已知定位末端坐标的情况下，由步骤3所得的函数关系，反解求出剩余三个关节变量，并导出其他关节变量； 

步骤6： 

确定凿岩钻机在钻孔过程中定位末端的预定位位置，并计算出在基坐标系中定位末端的预定位坐标，判断定位末端的预定位坐标是否在断面平行孔区域内，若在此区域内，则进行下一步，若不在此区域内，则调整车体位置，使得定位末端的预定位坐标移动到断面平行孔区域内； 

步骤7： 

由步骤5所得的关节变量之间的数学关系，利用步骤3所得的函数表达式，求解出预定位坐标的全部目标关节变量； 

步骤8： 

通过闭环实时驱动控制模块调节各个液压缸，使全部关节变量达到预设定目标值，并由传感器检测各个关节变量是否达到目标值，若没有达到目标值则继续驱动进行调节，若达到目标值则停止驱动； 

步骤9： 

判断定位末端是否达预定位位置，若达到预定位位置，则驱动钻进机构进行凿岩钻孔，若未达到预定位位置，返回步骤6重新调节，直到达到预定位位置为止； 

步骤10： 

完成凿岩钻孔后，调整钻进机构回到初始位置，确定定位末端的下一个目 标位置，返回步骤6，进行重复定位、钻孔； 

步骤11： 

完成全部预确定的钻孔后，收回钻臂。 

通过以上方法，可实现液压凿岩钻机断面平行孔自动定位。 

本发明的实施对象至少应具备一台液压凿岩钻机、通用控制器、以及安装在液压凿岩钻机钻臂机构上的角度传感器和位移传感器。控制器向多路阀组发出控制信号，实时驱动液压缸使得钻臂各机构动作，并检测各角度或位移传感器反馈值，调整控制信号，驱动钻臂各机构达到目标关节变量，从而完成凿岩钻机钻臂自动定位。 

本发明的有益效果是：该发明具有计算过程简单、快速、准确、实时性强、易实现等优点，适用于液压凿岩钻机断面平行孔区域的自动定位，有利于液压凿岩钻机的智能控制。 

附图说明

图1是液压凿岩钻机钻臂定位机构结构组成图； 

图2是液压凿岩钻机钻臂定位机构关节坐标系； 

图3是液压凿岩钻机断面平行孔定位工作空间； 

图4是液压凿岩钻机断面平行孔区域边界线性化。 

在上述附图中，1.动力头，2.推进器，3.推进器补偿油缸，4.钎杆，5.推进器摆动油缸，6.推进器俯仰油缸，7.臂架，8.推进器翻转油缸，9.钻臂左右变幅油缸。 

具体实施方式

下面结合附图并以某型液压凿岩钻机为例，对本发明进行更详细的说明。 

实施例：某型液压凿岩钻机 

使用该型液压凿岩钻机进行快速准确实现凿岩钻机断面平行孔自动定位的方法，其具体步骤如下： 

步骤1： 

分析液压凿岩钻机钻臂定位机构结构组成及各机构动作。如图1所示，钻臂定位机构包括动力头1、推进器2、推进器补偿油缸3、钎杆4、推进器摆动油缸5、推进器俯仰油缸6、臂架7、推进器翻转油缸8和钻臂左右变幅油缸9；钻臂的升降、摆动动作由对称布置在钻臂体上的钻臂左右变幅油缸9联合驱动完成；推进器2的俯仰、摆动和翻转动作分别由推进器摆动油缸5、推进器俯仰油缸6和推进器翻转油缸8驱动完成。 

步骤2： 

建立液压凿岩钻机钻臂系统坐标系模型。根据液压凿岩钻机钻臂系统结构模型，建立液压凿岩钻机钻臂机构关节坐标系，如图2所示，共建立七个关节坐标系，钻臂定位末端用P点标记，其坐标为(x₇，y₇，z₇)。测量钻臂结构尺寸，得到D-H矩阵的连杆参数，如表1所示。 

表1  液压凿岩钻机钻臂定位系统连杆参数 

表1中，a_i表示从Z_i到Z_i+1沿X_i的距离；α_i表示表示从Z_i到Z_i+1绕X_i旋转的角度；d_i表示从X_i-1到X_i沿Z_i的距离；θ_i表示X_i-1到X_i绕Z_i旋转的角度。 

步骤3： 

确定钻臂定位末端在基坐标系中的坐标。对钻臂定位系统坐标系及连杆参数进行分析，可知钻臂定位末端P点的坐标是关于七个关节变量及连杆参数的函数，运用D-H法计算可得P点的坐标： 

$\left\{\begin{array}{c}x=(c_{123}{s}_4+s_{134}+c_{14}{s}_2)a_6-(s_5{c}_{123}{c}_4+s_{13}{c}_4+s_{1245})d_7+(c_{1234}{s}_5+s_{1245})d_6+(c_{1234}+s_{13})a_4+(c_{123}+s_{13})d_5+\\ (c_{123}+s_{13})a_3-(c_{123}+s_1{c}_3)d_4+c_1{s}_2{d}_3+c_1{s}_2{d}_3+c_1{a}_1\\ y=(s_1{c}_{23}-c_1{s}_3)a_6-(s_{123}+s_{124})d_7+(s_1{c}_{23}-c_1{s}_4)d_6+(s_1{c}_{23}-c_1{s}_3)a_4+(s_{123}-c_1{s}_{34})d_5+(s_1{c}_{23}-c_1{s}_3)a_3-\\ (s_{13}{c}_{25}+c_{135})d_4+s_{12}{d}_3+s_1{a}_1\\ z=(s_{24}{c}_3-c_{24})a_6-(c_{234}+s_{23}{c}_5)d_7+(s_{234}+c_{245})d_6+(s_2{c}_{34}-c_2{s}_4)a_4-s_{24}{c}_3{d}_5+s_2{c}_3{a}_3+s_{23}{d}_4-c_2{d}_3\end{array}\right.---\left(1\right)$

上式中c₁＝cosθ₁，s₁＝sinθ₁，c₂＝cosθ₂，s₂＝sinθ₂，c₂₃＝cos(θ₂+θ₃)，s₂₃＝sin(θ₂+θ₃)，依此类推。 

步骤4： 

液压凿岩钻机断面平行孔区域规划。钻机的钻臂运动在断面区域内形成一个无规则的平面定位区域，由于推进器的移动，最终形成是一个立体空间，如图3所示。依次将钻臂与推进器协调摆动到最大值，可知断面平行孔定位区域，由公式(1)确定区域边界范围，如图4所示。断面的平行孔定位区域是无规则的，在确定钻臂定位末端位置P点坐标时，需要判断P点是否处于平行孔定位内，为了便于判断，将无规则的边界进行线性化修正处理，使得修正区域最大限度满足实际区域，如图4所示，修正区域占实际区域的96.1％。 

步骤5： 

对关节变量θ₁、θ₂、θ₃、θ₄、θ₅、d₆和d₇进行求解。控制钻臂末端的位置时，必须求解出全部关节变量，从而才能驱动各臂架，实现钻臂末端自动定位。依据平行孔钻孔原则，在自动定位的过程中，为了使定位平稳，利用自动控制，不让臂架作±180°运动，因此不让翻转缸运动，从而固定旋转自由度θ₃。自动定位的过程中，只考虑臂架的定位，不考虑推进器的钻孔过程，也就是钎杆的运动末端不考虑，所以可将d₇看作固定量。在钻凿平行孔时，臂架的摆动和俯仰运动与推进器的摆动和俯仰运动是成对存在的，才能保证凿岩钻孔为平行孔。 

由以上分析，可知θ₁＝-θ₅，θ₂＝-θ₄，θ₃＝0，d₇＝0，将其代人公式(1)可求得 

$\left\{\begin{array}{c}{\mathrm{\θ}}_1=\arcsin \left(\frac{-y}{\sqrt{a^2+42025}}\right)\\ {\mathrm{\θ}}_2=\arcsin (\frac{z}{2040.2}-0.3835)-0.0245\\ d_6=x-1825+50c_1{s}_2+205s_1-2039c_1{c}_2-150c_1\end{array}\right.---\left(2\right)$

上式中， $a=50\sin (\arcsin (\frac{z}{2040.2}-0.3835)-0.0245)-2039\cos (\arcsin (\frac{z}{2040.2}-0.3835)-0.0245)-150,$ 其他参数定义与(1)式相同。 

由钻臂定位末端P点的坐标(x₇，y₇，z₇)反解求出全部关节变量是实现智能化自动定位的基础。 

步骤6： 

确定凿岩钻机在钻孔过程中定位末端P点的预定位位置，并计算出在基坐标系中P点的预定位坐标，判断P点的预定位坐标是否在断面平行孔区域内，若在此区域内，则进行下一步，若不在此区域内，则调整车体位置，使得P点预定位坐标移动到断面平行孔区域内。 

步骤7： 

由步骤5所得的关节变量之间的数学关系θ₁＝-θ₅，θ₂＝-θ₄，θ₃＝0，d₇＝0，利用步骤3所得的函数表达式(1)，求解出预定位坐标的全部目标关节变量θ₁、θ₂、θ₃、θ₄、θ₅、d₆和d₇。 

步骤8： 

通过闭环实时驱动控制模块调节各个液压缸，使全部目标关节变量θ₁、θ₂、θ₃、θ₄、θ₅、d₆和d₇达到目标值，并由传感器检测各个关节变量是否达到目标值，若没有达到目标值则继续驱动进行调节，若达到目标值则停止驱动。 

步骤9： 

判断定位末端P点是否达预定位位置，若达到预定位位置，则驱动钻进机 构进行凿岩钻孔，若未达到预定位位置，返回步骤6重新调节，直到达到预定位位置为止。 

步骤10： 

完成凿岩钻孔后，调整钻进机构回到初始位置，确定定位末端P点的下一个目标位置，返回步骤9，进行重复定位、钻孔。 

步骤11： 

完成全部预确定的钻孔后，收回钻臂。 

本发明的定位坐标计算、闭环实时控制、关节变量反解计算均在控制器中完成，控制器向多路阀组发出控制信号，实时驱动液压缸使得钻臂各机构动作，并检测各角度或位移传感器反馈值，调整控制信号，驱动钻臂各机构达到目标关节变量，从而完成凿岩钻机钻臂自动定位。 

Claims (1)

1.一种快速准确实现凿岩钻机断面平行孔自动定位的方法，其特征在于该方法具体步骤如下：

步骤1：

分析液压凿岩钻机钻臂定位机构结构组成及各机构动作；

步骤2：

根据液压凿岩钻机钻臂系统结构模型，利用D-H法建立液压凿岩钻机钻臂机构关节坐标系；

步骤3：

对钻臂定位系统坐标系及连杆参数进行分析，计算出定位末端在基坐标系中的坐标与全部关节变量的函数关系；

步骤4：

液压凿岩钻机断面平行孔区域规划；钻机的钻臂运动在断面区域内形成一个无规则的平面定位区域，由于推进器的移动，最终形成是一个立体空间；依次将钻臂与推进器协调摆动到最大值，可知断面平行孔定位区域，并确定区域边界范围；断面的平行孔定位区域通常是无规则的，在确定钻臂定位末端位置坐标时，为了便于判断，将无规则的边界进行线性化修正处理，使得修正区域最大限度满足实际区域；

步骤5：

对全部关节变量进行求解；依据平行孔钻孔原则，在自动定位的过程中，为了使定位平稳，利于自动控制，尽量不让臂架作±180°运动，由于此限制减少了臂架运动的自由度个数，从而减少了求解关节变量的数目；自动定位的过程中，只考虑臂架的定位，不考虑推进器的钻孔过程，也就是钎杆的运动末端不考虑，所以可将钻进运动形成的关节变量看作固定量；在钻凿平行孔时，臂架的摆动和俯仰运动与推进器的摆动和俯仰运动是成对存在的，才能保证凿岩钻孔为平行孔，因此，将对称变化的关节变量进行归纳为一个变量；通过上述限制将求解关节变量的个数限制为三个，在已知定位末端坐标的情况下，由步骤3所得的函数关系，反解求出剩余三个关节变量，并导出其他关节变量；

步骤6：

确定凿岩钻机在钻孔过程中定位末端的预定位位置，并计算出在基坐标系中定位末端的预定位坐标，判断定位末端的预定位坐标是否在断面平行孔区域内，若在此区域内，则进行下一步，若不在此区域内，则调整车体位置，使得定位末端的预定位坐标移动到断面平行孔区域内；

步骤7：

由步骤5所得的关节变量之间的数学关系，利用步骤3所得的函数表达式，求解出预定位坐标的全部目标关节变量；

步骤8：

通过闭环实时驱动控制模块调节各个液压缸，使全部关节变量达到预设定目标值，并由传感器检测各个关节变量是否达到目标值，若没有达到目标值则继续驱动进行调节，若达到目标值则停止驱动；

步骤9：

判断定位末端是否达预定位位置，若达到预定位位置，则驱动钻进机构进行凿岩钻孔，若未达到预定位位置，返回步骤6重新调节，直到达到预定位位置为止；

步骤10：

完成凿岩钻孔后，调整钻进机构回到初始位置，确定定位末端的下一个目标位置，返回步骤6，进行重复定位、钻孔；

步骤11：

完成全部预确定的钻孔后，收回钻臂。