双臂掘进钻车自动化钻孔方法研究

张付祥，　史文军

(河北科技大学 机械工程学院， 河北 石家庄　050018)



双臂掘进钻车自动化钻孔方法研究

张付祥，史文军

(河北科技大学 机械工程学院， 河北 石家庄050018)

在对双臂掘进钻车进行运动学分析的基础上，分析了钻车的作业能力和理想位姿，研究了双臂掘进钻车的钻臂之间及钻臂与巷道之间的碰撞检测方法，提出了一种双臂掘进钻车自动化钻孔方法。在Matlab环境下对该方法进行了仿真验证，结果表明该方法能够使钻臂到达钻孔位姿，实现自动化钻孔，为进一步研究双臂掘进钻车钻臂的运动规划及自动化钻孔奠定了基础。

双臂掘进钻车； 作业能力分析； 理想位姿； 碰撞检测； 自动化钻孔

网络出版地址：http://www.cnki.net/kcms/detail/32.1627.TP.20160601.1028.011.html

0　引言

中国煤矿瓦斯爆炸事故频发，死亡人数众多，瓦斯事故已成为煤矿安全生产的最大威胁[1]。如何实现煤矿高效、高产和安全已成为煤矿进一步发展的关键和核心[2]。高瓦斯矿井瓦斯防治管理的最大难题是煤层瓦斯的有效抽放[3]，而释放瓦斯的有效途径是钻孔[4]。计算机控制的自动化钻孔和钻爆精度等问题的有效解决可提高瓦斯防治管理的安全性和效率。

本文针对CMJ2-27改进型全液压钻井钻车的钻臂机械结构进行运动学分析，并对钻车的作业能力与理想位姿进行分析，提出钻车自动碰撞检测方法，对目前常用的拱形巷道爆破方案进行钻孔规划，为实现钻车自动化钻孔奠定了技术基础。

1　钻车钻臂机械结构及运动学分析

CMJ2-27改进型全液压钻井钻车在作业过程中，所有动作都由操作人员在驾驶室手工机械操作，效率低，钻孔一致性较差。为了解决这些问题，对钻车进行改进，使其具备数控功能。钻车钻臂结构如图1所示。

1-钻杆； 2-滑架； 3-推进器； 4-摆角油缸； 5-俯仰油缸；6-回转臂； 7-升降油缸; 8-整臂摆动油缸

钻车为双臂操作，且2个机械臂的结构基本一致，每个机械臂有5个转动自由度、1个移动自由度。在2个机械臂的各个关节上分别建立关节坐标系。关节坐标系建立规则：每个机械臂按关节和钻车车体物理连接的先后次序在各个关节上依次建立关节坐标系O1X1Y1Z1，O2X2Y2Z2，O3X3Y3Z3，O4X4Y4Z4，O5X5Y5Z5，O6X6Y6Z6，其中第6个自由度为钻杆的移动，坐标系O6X6Y6Z6为建立在钻杆末端的钻杆坐标系。

采用传统的D-H (Denavit-Hartenbergma-trix)法对钻车钻臂进行正向运动学分析，即通过已知的机械臂关节坐标求取钻杆坐标系的位姿。钻臂的每个连杆上都固定一个坐标系，用4×4的齐次变换矩阵来描述相邻2个连杆的空间关系[5]。通过依次变换可最终推导出钻杆相对于基坐标系的位姿，建立钻臂的运动学方程。

逆运动学问题是已知钻杆坐标系的位姿，求相应的关节坐标。运动学逆解解法主要有解析法和代数法。代数法工作量大，往往需要直接观察和经验[6]，而解析法可避免这个问题，因此采用解析法中的分离变量法求解析解。分离变量法首先利用运动方程的不同形式，找出矩阵中简单表达某个未知数的元素，力求得到未知数较少的方程，然后分离出变量，逐个求解。

双臂掘进钻车的运动学分析目的是求解钻车钻杆期望位姿对应的关节坐标，手部位姿为6个自由度，需要6轴联动。钻孔作业采用计算机控制，使其能够根据作业点坐标通过逆运动学求解关节运动控制参数，从而实现钻臂的全方位数字控制作业。

2　钻车作业能力与理想位姿分析

钻车钻臂的空间分析与机器人的空间分析类似[7]。目前大多采用蒙特卡罗法求解机器人工作空间，其为利用随机抽样来求解数学问题的一种数值方法。

实际工作中，最为理想的状态是理想钻车位姿时钻孔，即末端姿态为钻臂垂直断面。利用Matlab可求得钻车距离断面1 010 mm时钻孔效果最佳，然后根据钻臂垂直断面的边界条件和最佳距离条件得出理想状态工作空间，如图2所示，(px，py)为用户坐标系上钻孔的坐标。

图2　双臂掘进钻车理想状态工作空间

由图2可知，理想钻车位姿钻孔时，内部包含一些空心区域，即有部分孔位置钻臂达不到。另外，钻车2个钻臂的结构完全相同，工作过程中有相互重叠的区域，在同时移位过程中钻臂之间可能产生相互碰撞。因此，钻车工作时要考虑钻车位姿调整。

3　钻车钻臂碰撞检测方法

在实际工作中，由于双臂掘进钻车的串联式机械臂处于动态的、不确定的非结构性环境中，碰撞现象可能经常发生。如何判断和避免串联式机械臂的碰撞，对于双臂掘进钻车的实际应用非常重要。

钻臂碰撞检测方法：① 针对钻车的工作情况进行碰撞检测理论分析，利用机器人运动学理论，推导出各个关节中心点的坐标表达式，求得任意2个连杆之间的最小距离，采用包络半径法对2个连杆之间的最小距离与2个连杆的包络半径之和进行比较，得到碰撞检测方程；② 针对单臂作业情况进行分析，利用Matlab工具，结合蒙特卡罗法获取单机械臂作业可能发生碰撞的情况；③ 针对双臂同时工作的情况进行分析，利用Matlab工具，结合蒙特卡罗法获取双臂同时作业可能发生碰撞的情况；④ 针对钻车2个机械臂钻杆的目标位姿，利用Matlab工具及钻车的解析逆解表达式获取2个机械臂的各个关节变量；⑤ 单臂作业时，针对蒙特卡罗法检测出的单机械臂各连杆之间可能发生的碰撞情况进行单臂碰撞检测，双臂同时作业时，针对蒙特卡罗法检测出的2个机械臂各连杆之间可能发生的碰撞情况进行双臂碰撞检测。

在钻车实际工作中，钻臂与巷道也可能发生碰撞，加剧钻臂的磨损，需尽量避免。钻臂与巷道的碰撞检测方法：采用两端具有半径为r的半球的圆柱体将连杆包络起来[8]，当钻车钻臂工作时，如果圆柱体两端球心位置(即连杆关节坐标原点)不与巷道碰撞，则整个圆柱体就不会与巷道发生碰撞，即钻臂连杆与巷道之间不发生碰撞。因此只要检测关节坐标原点是否与巷道发生碰撞即可。

通过分析，可将关节坐标原点是否与巷道发生碰撞的问题转换为求关节坐标原点到巷道的最小距离问题：如果关节坐标原点到巷道的最小距离大于包络连杆的半球半径r，则钻臂与巷道之间不发生碰撞，否则发生碰撞。

在钻车实际工作中，还要考虑实时碰撞检测问题，需给出一个安全范围δ(钻臂实际工作时与巷道壁的最小距离)。本文采用煤矿作业时最常用的拱形巷道，巷道作业面宽度为W，高度为H，如图3所示。钻孔规划碰撞检测时δ=0，实时碰撞检测时δ≠0。

图3　巷道作业面结构

4　钻车自动化钻孔规划方法

已知钻杆坐标系O6X6Y6Z6、用户坐标系OuXuYuZu，根据钻孔工艺可确定各个钻孔在用户坐标系上的坐标(px，py)，而对于各孔的姿态确定，理论上需要满足关系：

(1)

式中：nx，ny，nz分别为X6在Xu，Yu，Zu轴上的投影；ox，oy，oz分别为Y6在Xu，Yu，Zu轴上的投影；ax，ay，az分别为Z6在Xu，Yu，Zu轴上的投影。

针对煤矿常见的拱形巷道，在作业面上建立用户坐标系，如图4所示。Ou在底部直线的中点，Xu在底部向右为正，Yu在作业面的左右对称线上向上为正，Zu垂直于工作面向外为正。确定钻臂目标姿态前需要先找几个特征孔(钻车初始位，即钻杆水平的理想位姿时，钻头正对工作面的2个孔；最上面的1个孔，最下面的3个孔；圆弧左右侧45°处2个孔)。然后将所有孔分为16个区域，其中Yu轴左侧的孔由左侧钻臂钻孔，Yu轴右侧的孔由右侧钻臂钻孔，Yu轴上的孔可根据情况选择由左侧或右侧钻臂钻孔。钻孔方案如图4所示。对这16个区域分别设定钻杆末端的目标位姿，见表1。

图4　钻孔方案

5　计算结果与分析

利用某矿用88孔方案验证理论分析结果，以其中1个孔为例进行说明。

首先根据特定的钻孔方案获取钻孔数据，即作业孔的用户坐标。取其中1个孔，假定作业孔在用户坐标系中的位姿为

T中第1列数据为作业孔X6轴姿态，第2列数据为作业孔Y6轴姿态，第3列数据为作业孔Z6轴姿态，第4列数据为钻孔的用户坐标系坐标。

假定钻车位姿矩阵为

表1　钻杆末端的目标位姿

将数据输入程序，得关节坐标θ1=-15.057 0，θ2=-95.902 0，θ3=-137.677 2，θ4=79.780 0，θ5=-97.213 1，d6=-1 984.1。

通过程序计算出作业孔的关节坐标后，即可通过控制系统控制关节运动，使钻臂到达钻孔位姿，实现自动化钻孔。

6　结语

对CMJ2-27改进型全液压钻井钻车进行作业能力分析、碰撞检测和钻孔规划，得到了自动化钻孔方法，为进一步研究双臂掘进钻车钻臂的轨迹规划奠定了理论基础，为实现CMJ2-27改进型钻车自动化钻孔奠定了技术基础。

[1]代勇.煤矿用液压钻车钻臂系统的研究[D].洛阳：河南科技大学, 2012.

[2]HAN S M, HWANG Y K, WOO N S, et al. Solid-liquid hydrodynamics in a slim hole drilling annulus [J].Journal of Petroleum Science and Engineering, 2010, 70(3/4): 308-319.

[3]柏思忠.瓦斯抽放管道设备ia等级防爆型式探讨[J].工矿自动化,2015,41(5)：55-57.

[4]史建.露天潜孔钻机钻孔自动定位研究[D].长沙：中南大学,2014.

[5]苏学满,孙丽丽,杨明,等.基于matlab的六自由度机器人运动特性分析[J].机械设计与制造,2013(1):78-80.

[6]贺昱曜.机器人PUMA560逆运动方程的新解法[J].机器人,1989,10(3):19-26.

[7]马小蒙,沈惠平,黄涛,等.冗余拟人双臂机器人的运动学及工作空间分析[J].机械科学与技术,2014(11):25-29.

[8]陈靖波,赵猛,张珩.空间机械臂在线实时避障路径规划研究[J].控制工程,2007，14(4):445-447.

[9]姚宁平，张杰，李泉新，等.煤矿井下定向钻孔轨迹设计与控制技术[J].煤炭科学技术,2013,41(3):7-11.

Research on an automatic drilling method for dual-arm drill rig

ZHANG Fuxiang,SHI Wenjun

(School of Mechanical Engineering, Hebei University of Science and Technology,Shijiazhuang 050018, China)

Based on kinematics analysis of dual-arm drill rig, operation ability and ideal position of dual-arm drill rig were analyzed, collision detection methods of dual-arm drill rig between the arms and between the arms and roadway were studied, and an automatic drilling method for dual-arim drill rig was put forward. A simulation experiment was carried out in Matlab. The experimental result shows that the method can make the arms of dirll rig arrive the ideal position and realize automatic drilling, which lays a foundation for further research on trajectory planning of the arms of dual-arm drill rig and automatic drilling.

dual-arm drill rig; operation ability analysis; ideal position; collision detection; automatic drilling

1671-251X(2016)06-0042-04

10.13272/j.issn.1671-251x.2016.06.011

2015-12-31；

2016-04-17；责任编辑：李明。

河北省科技攻关计划资助项目(14211902D)；河北省研究生创新资助项目。

张付祥(1973-)，男，河北青县人，副教授，博士，主要研究方向为机电装备与机器人，E-mail:zhangfx@hebust.edu.cn。

TD421

A网络出版时间：2016-06-01 10:28

张付祥，史文军.双臂掘进钻车自动化钻孔方法研究[J].工矿自动化，2016,42(6)：42-45.