井下鉆機換桿控制方案設計

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

（五邑大學 智能制造學部，廣東 江門 529020）

在井下煤炭鉆探中，因為作業(yè)區(qū)域的土質(zhì)不同，常常需要更換不同的鉆桿進行作業(yè). 鉆桿更換就是把鉆桿從儲備箱中取出并夾緊于鉆桿夾持器中. 目前，主要用于煤礦井下施工的鉆孔設備有ZY系列鉆機和MK 系列鉆機[1]，這些鉆機在更換鉆桿時，必須依靠人工實地操作，由于井下存在瓦斯泄露和滲水等情況，會給更換鉆桿的工人帶來較大的安全隱患[2]. 目前主流的ZYWL_4000SY 履帶式鉆機作業(yè)時，雖然可以通過遙控按鈕進行自動換桿，但操作距離也限制在50 m 內(nèi)，操作者依舊在井下，仍然存在安全風險. 因此，本文設計了一套控制方案來彌補現(xiàn)有方案存在的不足. 此方案重點解決如何在井上對井下的鉆桿更換進行監(jiān)視和控制，要求井上控制設備對操作者的控制指令精確采集、井下執(zhí)行機構(gòu)動作穩(wěn)定和中間通訊設備信息交互流暢延遲低等.

1 換桿控制方案的總體設計

人工更換鉆桿可以分為以下步驟：1）取出鉆桿；2）調(diào)整鉆桿平行于主動鉆桿軸線；3）調(diào)整鉆桿末端與主動鉆桿前端平齊；4）從垂直于主動鉆桿軸線的方向?qū)@桿裝入夾持器. 參考人工更換鉆桿的流程，本文選用ZYWL_4000SY 自帶的四關(guān)節(jié)三自由度機械臂作為舉升裝置，如圖1 所示. 機械臂裝在鉆機的提升架上，保證能和主機一起旋轉(zhuǎn)和升降. 工作時，鉆桿末端抓手將鉆桿從鉆桿箱中抓起后，伸縮部件收縮將鉆桿提升到空間有利位置，旋轉(zhuǎn)部件進行旋轉(zhuǎn)將鉆桿調(diào)整到與主動鉆桿軸線平行，手腕部件旋轉(zhuǎn)將鉆桿送入夾持器中，抓手松開，開始鉆孔.

圖1 裝桿機械臂

井上與井下的通信通過4G 無線通信模塊實現(xiàn). 井上安置一個控制臺，包含監(jiān)控屏幕、傳感器指示燈、中控電腦和無線通信模塊；井下安置鉆機及配屬的帶機械臂的仿人形機械手、攝像頭及配屬云臺、溫度傳感器和壓力傳感器. 對控制器的主要電路硬件進行選型保證對機械臂和機械手的有效控制并及時反饋井下信息，包括Leap Motion 手部運動信息采集器、薄膜壓力傳感器、STM32 微處理器、龍尚4G U9300 通信模塊，控制方案各模塊交互原理如圖2 所示.

抓取鉆桿的部件因為沒有配套鉆桿箱，所以不能使用ZYWL_4000SY 自帶的兩爪抓手，本文使用仿人手五指機械手實現(xiàn)多點接觸，保證抓住、抓穩(wěn). 其結(jié)構(gòu)由5 根手指、一個手掌和一個手腕構(gòu)成，包含14 個指節(jié)和14 個指關(guān)節(jié)，每根手指的所有指節(jié)由一根高強度人造纖維串聯(lián)，能夠準確模仿人手的各種動作，結(jié)構(gòu)如圖3 所示.

圖2 交互原理框圖

圖3 抓桿機械手

2 機械臂運動學分析

井上控制臺的操作軟件用Java 語言編寫，負責采集Leap Motion 手部位姿信息，并加工成機械臂和仿形手的執(zhí)行信號. 將信息傳輸?shù)骄碌耐瑫r接收井下傳來的傳感器信息.

根據(jù)機械手的工作過程，機械手末端抓手只在上、下桿過程的始末進行重復的張合，不影響機械臂的運動學分析結(jié)果，同時減少了機械手的1 個關(guān)節(jié)和1 個自由度，提高了控制的可靠性. 根據(jù)三維模型將機械手簡化為關(guān)節(jié)與連桿的組合，該組合包括2 個轉(zhuǎn)動自由度和1 個移動自由度. 建立D-H 坐標系，如圖4 所示.

逆向運動學求解是已知機械手末端執(zhí)行機構(gòu)相對于基準坐標系的姿態(tài)信息，求解出各關(guān)節(jié)對應的運動信息. Leap Motion 至多能采集到人的肘部運動信息，該信息不能一一映射到機械臂各電機轉(zhuǎn)角，所以直接采集手腕坐標信息并作為末端姿態(tài)，并對機械臂的逆運動學分析求解出各關(guān)節(jié)的轉(zhuǎn)動信息. 首先建立D-H 模型：將機械手的3 個關(guān)節(jié)分別定義為傾角臂1、翻轉(zhuǎn)臂2 和伸縮油缸. 根據(jù)表1 的D-H 模型可以得到相鄰關(guān)節(jié)間的齊次變換矩陣，進而可以得到各電機對應轉(zhuǎn)角或伸縮量. 逆向運動學方程中手爪到達指定位置會有多組姿態(tài)的解，為了最大限度避免井下環(huán)境的干擾，系統(tǒng)自動選用伸縮油缸位移量最大的一組解.

圖4 機械臂D-H 坐標系

表1 機械臂D-H 模型

表1中αi為兩關(guān)節(jié)軸線間的夾角，ai為前后兩關(guān)節(jié)軸線間公法線長度，di為關(guān)節(jié)i 軸線上兩條公法線間的距離，θi為垂直于關(guān)節(jié)i 軸平面內(nèi)與上下兩關(guān)節(jié)的公法線之間的夾角.

4 個參數(shù)給定后，建立連桿i 坐標系相對于連桿 i- 1的齊次變換矩陣

其中S1、 C1為sin θi、cosθi的簡寫，該矩陣N包含機械手末端位姿信息，設，Z=0. 則［ X Y Z ］T為機械臂末端坐標系對應基坐標系的位置信息. 在鉆桿輸送過程中確定機械臂末端抓手的位姿，可通過逆運動學求解出兩旋轉(zhuǎn)臂的轉(zhuǎn)角和分配給伸縮油缸的伸縮量.

根據(jù)式（3）和式（4）求解得

其中，1θ 為傾角臂隨動轉(zhuǎn)角，2θ 為翻轉(zhuǎn)臂隨動轉(zhuǎn)角，d3為伸縮部件隨動伸縮量，1L 為傾角臂的長度.根據(jù)求出的1θ 、2θ 、 d3，當規(guī)劃好機械臂末端的位姿以及鉆桿的運動過程后，可調(diào)節(jié)關(guān)節(jié)的旋轉(zhuǎn)角度與移動距離來實現(xiàn)對機械臂的控制.

3 仿人形機械手運動測試

對機械手控制的準確性和實時性進行測試，主要分為以下步驟：第1 步是進行抓握動作同步測試，抓握強度函數(shù)對手指彎曲程度的定義是完全張開為0，完全握緊為1. 先進行極限位置即0 和1的同步性能測試，測試結(jié)果表明Leap Motion 對0 的采集效果非常好；第2 步是測試在抓握強度快速變化時Leap Motion 采集的實時性和舵機的執(zhí)行效果，手掌快速的張開和握緊情況下跟隨情況良好，未出現(xiàn)丟失和停機的情況；第3 步是測試當人手緩慢張開和握緊時手指舵機的跟隨情況，本組實驗從張開到握緊的時間由之前的0.5 s 變成5 s，人手的抖動導致舵機也同步抖動，這對仿人形機械手的同步抓取動作的執(zhí)行是不利的，更換通信的數(shù)據(jù)類型以后情況得到改善，500 次實驗的結(jié)果如表2 所示. 最終使用體驗是，通信設備的檢測和互相傳輸效果很好，但是手掌的初始位置太低影響數(shù)據(jù)可靠性，手掌模型出現(xiàn)實時建?；?，導致抓握數(shù)據(jù)錯誤.

表2 500 次執(zhí)行效果表

4 結(jié)語

本文設計的控制方案使操作者得以在安全舒適的環(huán)境中遙控機械臂進行井下鉆機換桿作業(yè).Leap Motion 手部運動信息采集器具有實時性好、穩(wěn)定性高等特點. 手指坐標的采集可精確到小數(shù)點后五位，在以后的研究中可對封裝程序進一步優(yōu)化，提高機械手同步控制的可靠性.