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

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

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

1 換桿控制方案的總體設(shè)計(jì)

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

圖1 裝桿機(jī)械臂

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

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

圖2 交互原理框圖

圖3 抓桿機(jī)械手

2 機(jī)械臂運(yùn)動(dòng)學(xué)分析

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

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

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

圖4 機(jī)械臂D-H 坐標(biāo)系

表1 機(jī)械臂D-H 模型

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

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

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

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

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

3 仿人形機(jī)械手運(yùn)動(dòng)測(cè)試

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

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

4 結(jié)語(yǔ)

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