井下钻机换杆控制方案设计

唐衡云，王建生，金禹男，陈邦印

（五邑大学 智能制造学部，广东 江门 529020）

在井下煤炭钻探中，因为作业区域的土质不同，常常需要更换不同的钻杆进行作业. 钻杆更换就是把钻杆从储备箱中取出并夹紧于钻杆夹持器中. 目前，主要用于煤矿井下施工的钻孔设备有ZY系列钻机和MK 系列钻机[1]，这些钻机在更换钻杆时，必须依靠人工实地操作，由于井下存在瓦斯泄露和渗水等情况，会给更换钻杆的工人带来较大的安全隐患[2]. 目前主流的ZYWL_4000SY 履带式钻机作业时，虽然可以通过遥控按钮进行自动换杆，但操作距离也限制在50 m 内，操作者依旧在井下，仍然存在安全风险. 因此，本文设计了一套控制方案来弥补现有方案存在的不足. 此方案重点解决如何在井上对井下的钻杆更换进行监视和控制，要求井上控制设备对操作者的控制指令精确采集、井下执行机构动作稳定和中间通讯设备信息交互流畅延迟低等.

1 换杆控制方案的总体设计

人工更换钻杆可以分为以下步骤：1）取出钻杆；2）调整钻杆平行于主动钻杆轴线；3）调整钻杆末端与主动钻杆前端平齐；4）从垂直于主动钻杆轴线的方向将钻杆装入夹持器. 参考人工更换钻杆的流程，本文选用ZYWL_4000SY 自带的四关节三自由度机械臂作为举升装置，如图1 所示. 机械臂装在钻机的提升架上，保证能和主机一起旋转和升降. 工作时，钻杆末端抓手将钻杆从钻杆箱中抓起后，伸缩部件收缩将钻杆提升到空间有利位置，旋转部件进行旋转将钻杆调整到与主动钻杆轴线平行，手腕部件旋转将钻杆送入夹持器中，抓手松开，开始钻孔.

图1 装杆机械臂

井上与井下的通信通过4G 无线通信模块实现. 井上安置一个控制台，包含监控屏幕、传感器指示灯、中控电脑和无线通信模块；井下安置钻机及配属的带机械臂的仿人形机械手、摄像头及配属云台、温度传感器和压力传感器. 对控制器的主要电路硬件进行选型保证对机械臂和机械手的有效控制并及时反馈井下信息，包括Leap Motion 手部运动信息采集器、薄膜压力传感器、STM32 微处理器、龙尚4G U9300 通信模块，控制方案各模块交互原理如图2 所示.

抓取钻杆的部件因为没有配套钻杆箱，所以不能使用ZYWL_4000SY 自带的两爪抓手，本文使用仿人手五指机械手实现多点接触，保证抓住、抓稳. 其结构由5 根手指、一个手掌和一个手腕构成，包含14 个指节和14 个指关节，每根手指的所有指节由一根高强度人造纤维串联，能够准确模仿人手的各种动作，结构如图3 所示.

图2 交互原理框图

图3 抓杆机械手

2 机械臂运动学分析

井上控制台的操作软件用Java 语言编写，负责采集Leap Motion 手部位姿信息，并加工成机械臂和仿形手的执行信号. 将信息传输到井下的同时接收井下传来的传感器信息.

根据机械手的工作过程，机械手末端抓手只在上、下杆过程的始末进行重复的张合，不影响机械臂的运动学分析结果，同时减少了机械手的1 个关节和1 个自由度，提高了控制的可靠性. 根据三维模型将机械手简化为关节与连杆的组合，该组合包括2 个转动自由度和1 个移动自由度. 建立D-H 坐标系，如图4 所示.

逆向运动学求解是已知机械手末端执行机构相对于基准坐标系的姿态信息，求解出各关节对应的运动信息. Leap Motion 至多能采集到人的肘部运动信息，该信息不能一一映射到机械臂各电机转角，所以直接采集手腕坐标信息并作为末端姿态，并对机械臂的逆运动学分析求解出各关节的转动信息. 首先建立D-H 模型：将机械手的3 个关节分别定义为倾角臂1、翻转臂2 和伸缩油缸. 根据表1 的D-H 模型可以得到相邻关节间的齐次变换矩阵，进而可以得到各电机对应转角或伸缩量. 逆向运动学方程中手爪到达指定位置会有多组姿态的解，为了最大限度避免井下环境的干扰，系统自动选用伸缩油缸位移量最大的一组解.

图4 机械臂D-H 坐标系

表1 机械臂D-H 模型

表1中αi为两关节轴线间的夹角，ai为前后两关节轴线间公法线长度，di为关节i 轴线上两条公法线间的距离，θi为垂直于关节i 轴平面内与上下两关节的公法线之间的夹角.

4 个参数给定后，建立连杆i 坐标系相对于连杆 i- 1的齐次变换矩阵

其中S1、 C1为sin θi、cosθi的简写，该矩阵N包含机械手末端位姿信息，设，Z=0. 则［ X Y Z ］T为机械臂末端坐标系对应基坐标系的位置信息. 在钻杆输送过程中确定机械臂末端抓手的位姿，可通过逆运动学求解出两旋转臂的转角和分配给伸缩油缸的伸缩量.

根据式（3）和式（4）求解得

其中，1θ 为倾角臂随动转角，2θ 为翻转臂随动转角，d3为伸缩部件随动伸缩量，1L 为倾角臂的长度.根据求出的1θ 、2θ 、 d3，当规划好机械臂末端的位姿以及钻杆的运动过程后，可调节关节的旋转角度与移动距离来实现对机械臂的控制.

3 仿人形机械手运动测试

对机械手控制的准确性和实时性进行测试，主要分为以下步骤：第1 步是进行抓握动作同步测试，抓握强度函数对手指弯曲程度的定义是完全张开为0，完全握紧为1. 先进行极限位置即0 和1的同步性能测试，测试结果表明Leap Motion 对0 的采集效果非常好；第2 步是测试在抓握强度快速变化时Leap Motion 采集的实时性和舵机的执行效果，手掌快速的张开和握紧情况下跟随情况良好，未出现丢失和停机的情况；第3 步是测试当人手缓慢张开和握紧时手指舵机的跟随情况，本组实验从张开到握紧的时间由之前的0.5 s 变成5 s，人手的抖动导致舵机也同步抖动，这对仿人形机械手的同步抓取动作的执行是不利的，更换通信的数据类型以后情况得到改善，500 次实验的结果如表2 所示. 最终使用体验是，通信设备的检测和互相传输效果很好，但是手掌的初始位置太低影响数据可靠性，手掌模型出现实时建模畸变，导致抓握数据错误.

表2 500 次执行效果表

4 结语

本文设计的控制方案使操作者得以在安全舒适的环境中遥控机械臂进行井下钻机换杆作业.Leap Motion 手部运动信息采集器具有实时性好、稳定性高等特点. 手指坐标的采集可精确到小数点后五位，在以后的研究中可对封装程序进一步优化，提高机械手同步控制的可靠性.