一種井下救援機械臂的運動學(xué)仿真分析及實現(xiàn)

趙 燕， 王江華

(1.燕京理工學(xué)院 信息科學(xué)與技術(shù)學(xué)院，河北 三河 065201； 2.華北科技學(xué)院 電子信息工程學(xué)院，河北 三河 065201)

0 引 言

近年來，煤礦開采行業(yè)迅猛發(fā)展，礦井安全事故時有發(fā)生。本文針對性地設(shè)計了一種井下救援機械臂[1]。采用一種六輪礦難救援機器人平臺搭載該機械臂，該平臺能平穩(wěn)地應(yīng)對井下崎嶇復(fù)雜的地形，機械臂能輕松應(yīng)對井下狹小復(fù)雜的空間環(huán)境進行救援工作，并幫助施救人員準(zhǔn)確迅速的完成各種救援動作，達到礦難發(fā)生后在井下進行施救的目的，以挽救更多工作人員的生命以及國家財產(chǎn)。

1 總體方案設(shè)計

針對礦井坑道的不平整，平臺采用履帶式底盤，其優(yōu)點是能輕松應(yīng)對各種地形，因此更加適合井下救援。對于機械臂，為應(yīng)對復(fù)雜的工作空間環(huán)境，采用靈活度較高的六自由度方式進行設(shè)計。為保證救援人員能在相對安全的環(huán)境進行施救，防止礦難后的二次事故對救援人員造成生命威脅，采用遙控的方式對整體裝置進行控制。整體機械結(jié)構(gòu)模型如圖1所示。

圖1 整體機械結(jié)構(gòu)模型

2 救援機械臂模型設(shè)計

礦山事故發(fā)生后，落石和泥土坍塌會使井下救援環(huán)境極大受限，會進一步阻礙救援工作的實施，因此機械臂設(shè)計為六自由度，即機械臂具有6個旋轉(zhuǎn)關(guān)節(jié)[2]，以達到克服井下復(fù)雜環(huán)境的目的。采用具有大扭矩的無刷直流伺服電機來驅(qū)動機械臂關(guān)節(jié)，機械臂末端的執(zhí)行器采用氣動裝置完成復(fù)雜的夾取動作，其立體模型如圖2所示。

圖2 機械臂模型及其空間D-H坐標(biāo)系

按照標(biāo)準(zhǔn)的Denavit-Hartenberg[3]方法建立機械臂的坐標(biāo)系，如圖2所示，基坐標(biāo)系與第一關(guān)節(jié)坐標(biāo)系重合。末端執(zhí)行器坐標(biāo)系建立在第六關(guān)節(jié)手腕處。連接桿D-H參數(shù)如表1所示，其表中各符號的含義如表2所示。

對于兩關(guān)節(jié)扭角α的正方向規(guī)定為Xi軸正向轉(zhuǎn)動為正，即Zi-1到Zi繞Xi轉(zhuǎn)動的方向為正；對于兩連桿間的角度θi的正方向規(guī)定為Zi軸正向轉(zhuǎn)動為正，即Xi-1到Xi繞Zi-1轉(zhuǎn)動的方向為正。由的取值范圍能初步了解該機械臂的活動范圍。

表1 連接桿D-H參數(shù)表

表2 符號表

3 救援機械臂運動學(xué)分析

運動學(xué)分析是指根據(jù)已經(jīng)給定的滿足工作需要的末端執(zhí)行器相對參考坐標(biāo)系的位置和姿態(tài)，求取各關(guān)節(jié)的運動參數(shù)[4-5]。該機械臂在進行井下施救的過程中抓取目標(biāo)障礙物，障礙物對應(yīng)的位置和姿態(tài)即機械臂末端抓取裝置的位姿，因此需要求解運動學(xué)的逆解[6]，求解過程如下：

由運動學(xué)正解

i-1Ti=rot(Xi,ai-1)×trans(Xi,ai-1)×

trans(Zi,di)×rot(Zi,θi)

變換關(guān)系式可求得兩連桿的通用變換公式：

(1)

(1) 求解θ1。已知矩陣

中的位姿矢量n，o，a，p，求取各關(guān)節(jié)轉(zhuǎn)角，即算出θ1，θ2，θ3，θ4，θ5，θ6的值。由0T1·0T6=1T得：

(2)

由式(1)經(jīng)過齊次坐標(biāo)變換計算得1T6矩陣，令式(2)等式兩端的元素(2，4)對應(yīng)相等可得：

-sθ1·Px+cθ1·Py=d2

(3)

由此算出：

(4)

(2) 求解θ3。同理，再令式(2)的矩陣方程等式兩端元素(1，4)和(3，4)分別對應(yīng)相等。則有：

(5)

(6)

(7)

(3) 求解θ2。由0T3·0T6=3T6得：

(8)

由式(1)經(jīng)過齊次坐標(biāo)變換計算出3T6矩陣，令式(8)兩端元素(1，4)和(2，4)分別對應(yīng)相等。因此，可求得：

(9)

θ23=θ2+θ3=

(10)

再代入式(6)求的θ3，得：

θ2=θ23-θ3=

(11)

(4) 求解θ4。同理，再令式(8)的矩陣方程等式兩端元素(1，3)和(3，3)分別對應(yīng)相等。則有

(12)

可求出當(dāng)θ5≠kπ(k=0,1……)時，有s5≠0，即

(13)

(14)

根據(jù)已經(jīng)求得的θ1，θ2，θ3，θ4，令式(14)兩邊矩陣的元素(1，3)及(3，3)分別對應(yīng)相等，得出：

(15)

θ5=arctan(-s5/c5)

(16)

(6) 求解θ6。θ6為末端執(zhí)行器的旋轉(zhuǎn)角度，在確定目標(biāo)物體的位置后，根據(jù)抓取目標(biāo)的姿態(tài)進行旋轉(zhuǎn)，這種方式有利于實際施救的操作，如此能快速確定不同情況的不同抓取角度，進一步增加其復(fù)雜環(huán)境的適應(yīng)能力[7-8]。

4 機械臂運動軌跡仿真分析及結(jié)果驗證

4.1 仿真分析

首先采用Matlab對建立的機械臂運動狀態(tài)進行模擬，將表1描述的機械臂數(shù)據(jù)輸入到Matlab的Robot Toolbox仿真模塊，機械臂水平滑動的極限位置在基坐標(biāo)系X0O0Y0中的豎直投影是0從內(nèi)經(jīng)531 mm到外徑1 077 mm形成的圓圈，豎直極限位置在基坐標(biāo)系Z0軸的范圍是[-552 mm，1 164 mm]。底座中心軸運行區(qū)域是Φ1∈[-π，π]，機械臂中心軸運行區(qū)域是。Φ2∈[-π，π]設(shè)置機器人的運行環(huán)境和夾取目標(biāo)及障礙物的相關(guān)信息，并進行三維仿真[9-11]。假設(shè)待抓取物體和釋放目標(biāo)點在基坐標(biāo)系下的齊次矩陣分別為T1和T2：

(17)

(18)

從圖3、4機械臂的仿真驅(qū)動可以看出，機械臂可以在工作空間內(nèi)任意轉(zhuǎn)動，能夠滿足井下苛刻操作空間的要求，表明了其運動學(xué)參數(shù)的合理性[12]；從圖5各關(guān)節(jié)的運動曲線可以看出，各連桿沒有運動錯位，沒有折點，曲線平滑，表明了機械關(guān)節(jié)布置合理，滿足穩(wěn)定運動的運動學(xué)要求[13]。

圖3 機械臂模型初態(tài)位姿

圖4 機械臂模型末態(tài)位姿

圖5 各關(guān)節(jié)角位移(位移)─時間仿真曲線

4.2 仿真結(jié)果驗證

為了對仿真數(shù)據(jù)進行實際驗證，假定礦難發(fā)生后的井下空間狹小，為排除狹窄礦道內(nèi)部的落石進行救援，設(shè)定固定參考系原點位于移動平臺臺面中心，且設(shè)置機械臂的初態(tài)矢量和末態(tài)矢量的參數(shù)如表3、表4所示。

表3 機械臂的初態(tài)矢量

表4 機械臂的末態(tài)矢量

將參數(shù)設(shè)定至機械臂中，實際運動效果良好，機械臂運動快速且連貫，多次重復(fù)試驗沒有出現(xiàn)卡頓現(xiàn)象，能夠精確的夾取目標(biāo)物體，說明該機械臂結(jié)構(gòu)與參數(shù)設(shè)置合理，滿足預(yù)期要求。

5 結(jié) 語

采用六輪礦難救援平臺搭載機械臂的方式，對井下救援機械臂進行了研究和設(shè)計，建立了對應(yīng)的立體機械模型，通過Matlab中的Robotics Toolbox工具對機械臂的運動情況進行了仿真[14-15]。根據(jù)仿真驗證以及實際運動驗證了機械臂的運動可靠性和運動學(xué)合理性，表明該機械臂能夠在狹小的空間內(nèi)很好的完成各種復(fù)雜的動作，非常適合應(yīng)用于礦井事故救援。

參考文獻(References)：

[1] 王 勇，朱 華，葛世榮，等．煤礦救災(zāi)機器人研究現(xiàn)狀及需要重點解決的技術(shù)問題[J]．煤礦機械，2007，28(4)：107-109.

[2] 汪雨萌，錢 樺，譚月勝，等．五自由度采摘機械臂運動學(xué)通用算法應(yīng)用[J]．安徽農(nóng)業(yè)科學(xué)，2012(3)：1757-1759.

[3] 蔡自興．機器人學(xué)[M]．北京：清華大學(xué)出版社，2000.

[4] 張 翮，熊 蓉，褚 健，等．一種全方位移動機器人的運動分析與控制實現(xiàn)[J]．浙江大學(xué)學(xué)報(工學(xué)版)，2004，38(12)：1650-1653，1672．

[5] 衛(wèi)玉昆，焦國太．基于萬向輪系全方位移動機器人運動學(xué)分析[J]．機械研究與應(yīng)用，2016，29(1)：54-56，59．

[6] 張禮華，費藍冰，樓 飛．新型輪腿式地面移動機器人的結(jié)構(gòu)設(shè)計與運動特性分析[J]．中國機械工程，2015，26(21)：2867-2872．

[7] 李世光，申夢茜，王文文，等．一種Mecanum輪式移動平臺增量PID控制系統(tǒng)設(shè)計[J]．山東科技大學(xué)學(xué)報(自然科學(xué)版)，2016，35(1)：86-90，96．

[8] 許志杰．六自由度涂膠工業(yè)機器人的運動學(xué)仿真與標(biāo)定算法研究[D]．廣州：華南理工大學(xué)，2015．

[9] 李憲華，郭永存，宋 韜．六自由度工業(yè)機器人手臂正運動學(xué)分析與仿真[J]．安徽理工大學(xué)學(xué)報(自然科學(xué)版) ，2013，33(2) ：34-38．

[10] 高 藝，馬國慶，于正林，等．一種六自由度工業(yè)機器人運動學(xué)分析及三維可視化仿真[J]．中國機械工程，2016，37(13) ：1726-1731．

[11] 耿 磊．六自由度工業(yè)機器人的建模與仿真研究[D]．沈陽：東北大學(xué)，2013．

[12] 王 燕，孫偉博，陳永兵，等．井下裝藥車自動裝藥系統(tǒng)機械臂運動學(xué)仿真分析[J]．礦山機械，2014(4)：10-14．

[13] 孫偉博．井下乳化炸藥混裝關(guān)鍵技術(shù)研究[D]．沈陽：東北大學(xué)，2013.

[14] 謝 斌，蔡自興．基于MATLAB Robotics Toolbox的機器人學(xué)仿真實驗教學(xué)[J]．計算機教育，2010(19)，140-143.

[15] 陶恒銘．六自由度工業(yè)機器人運動分析與控制技術(shù)的研究[D]．合肥：合肥工業(yè)大學(xué)，2014．

[16] 陶恒銘．六自由度工業(yè)機器人運動分析與控制技術(shù)的研究[D]．合肥：合肥工業(yè)大學(xué)，2014．