鉆錨機(jī)器人鉆臂軌跡規(guī)劃方法研究

雷孟宇，張旭輝,2，楊文娟,2，董 征，萬(wàn)繼成，張 超，杜昱陽(yáng),2

(1.西安科技大學(xué) 機(jī)械工程學(xué)院，陜西 西安 710054；2.陜西省礦山機(jī)電裝備智能監(jiān)測(cè)重點(diǎn)實(shí)驗(yàn)室，陜西 西安 710054)

當(dāng)前煤礦巷道支護(hù)主要依靠人工操作單體式鉆機(jī)完成，支護(hù)效率低，勞動(dòng)強(qiáng)度大，同時(shí)由于支護(hù)過(guò)程處于空頂距下作業(yè)，安全隱患大[1-3]。為提高巷道支護(hù)自動(dòng)化程度，提升支護(hù)效率，多鉆臂錨桿臺(tái)車應(yīng)用逐漸增多。針對(duì)多鉆臂支護(hù)系統(tǒng)，合理的鉆錨孔孔序規(guī)劃策略和鉆臂軌跡規(guī)劃方法不僅符合作業(yè)規(guī)范的要求，還能夠避免多鉆臂相互干涉的影響，減少能源消耗和設(shè)備磨損，提升支護(hù)效率[4-5]。

鉆錨孔孔序規(guī)劃問(wèn)題實(shí)質(zhì)上是多目標(biāo)多對(duì)象任務(wù)分配問(wèn)題，通過(guò)合理的任務(wù)分配提高多對(duì)象協(xié)同工作效率。針對(duì)多鉆臂孔序規(guī)劃問(wèn)題，國(guó)內(nèi)外眾多專家、學(xué)者對(duì)其進(jìn)行了研究。目前常用的多任務(wù)多對(duì)象任務(wù)分配方法主要包括利用蟻群算法實(shí)現(xiàn)孔序規(guī)劃[6-8]、遺傳算法實(shí)現(xiàn)最優(yōu)規(guī)劃[9-11]、優(yōu)化函數(shù)方法實(shí)現(xiàn)孔序規(guī)劃[12]和匈牙利算法實(shí)現(xiàn)任務(wù)分配[13]等。優(yōu)化函數(shù)方法簡(jiǎn)單地將炮孔空間位置的直線距離作為代價(jià)函數(shù)，將其最優(yōu)解作為孔序規(guī)劃結(jié)果，該方法并未完全考慮機(jī)械臂實(shí)際運(yùn)動(dòng)軌跡。蟻群算法實(shí)現(xiàn)孔序規(guī)劃以最短路徑或最短時(shí)間為目標(biāo)進(jìn)行優(yōu)化得到最優(yōu)解，提高了鉆孔效率，但鉆臂運(yùn)行過(guò)程可能會(huì)發(fā)生碰撞等問(wèn)題。匈牙利算法任務(wù)分配可實(shí)現(xiàn)效益矩陣最大化，但多機(jī)械臂系統(tǒng)可能存在軌跡干涉問(wèn)題。以上研究主要集中在多鉆機(jī)錨桿臺(tái)車孔序規(guī)劃和多任務(wù)分配方法，同時(shí)由于巷道支護(hù)工藝流程復(fù)雜，以上方法在煤礦巷道支護(hù)方面研究應(yīng)用較少。

針對(duì)鉆臂軌跡規(guī)劃方法，眾多學(xué)者進(jìn)行了卓有成效的研究?；诟倪M(jìn)人工勢(shì)場(chǎng)法[14]、多項(xiàng)式插值法[15]、智能算法[16-19]和深度學(xué)習(xí)方法[20]的機(jī)械臂軌跡規(guī)劃方法已經(jīng)在分揀、協(xié)作等場(chǎng)景研究。基于智能算法的機(jī)械臂軌跡規(guī)劃方法能夠快速收斂，確定最優(yōu)軌跡，基于深度學(xué)習(xí)方法的軌跡規(guī)劃在連續(xù)變動(dòng)任務(wù)實(shí)驗(yàn)中有更高的適應(yīng)性和魯棒性。以上研究集中在機(jī)械臂軌跡規(guī)劃和最優(yōu)軌跡優(yōu)化方法，研究對(duì)象多為工業(yè)機(jī)械臂，未涉及到煤礦鉆錨裝備機(jī)械臂的軌跡規(guī)劃。煤礦井下鉆錨作業(yè)環(huán)境中，在鉆孔順序確定的基礎(chǔ)上，合理的軌跡規(guī)劃能夠減少振動(dòng)沖擊，保證鉆臂運(yùn)動(dòng)過(guò)程快速平滑，有效提升鉆錨效率和智能化程度。

因此，提出一種集成懸臂式掘進(jìn)機(jī)和多自由度機(jī)械臂的鉆錨機(jī)器人，通過(guò)自主定位、鉆錨孔自動(dòng)尋孔等功能，完成適應(yīng)一般地質(zhì)條件的巷道自動(dòng)支護(hù)作業(yè)。在機(jī)器人化掘錨一體機(jī)模型樣機(jī)的基礎(chǔ)上，針對(duì)鉆臂結(jié)構(gòu)進(jìn)行運(yùn)動(dòng)學(xué)分析，通過(guò)蒙特卡羅方法計(jì)算鉆臂運(yùn)動(dòng)空間，確定鉆錨機(jī)器人有效作業(yè)范圍；基于支護(hù)作業(yè)工藝要求提出鉆錨孔孔序規(guī)劃策略；利用五次多項(xiàng)式插值方法進(jìn)行鉆臂末端軌跡規(guī)劃，以期為鉆錨裝備自動(dòng)化、智能化奠定基礎(chǔ)。

1 鉆錨機(jī)器人結(jié)構(gòu)及功能

巷道成形主要包括巷道掘進(jìn)和巷道支護(hù)，當(dāng)前巷道成形主要有3 種方式：懸臂式掘進(jìn)機(jī)完成巷道截割，錨桿臺(tái)車完成巷道支護(hù)作業(yè)，該種方式支護(hù)效率有所提升，但是對(duì)巷道寬度要求較高，同時(shí)懸臂式掘進(jìn)機(jī)和錨桿臺(tái)車來(lái)回折返較為耗時(shí)；連續(xù)采煤機(jī)配合錨桿鉆機(jī)完成巷道截割和支護(hù)作業(yè)，該種方式截割和支護(hù)作業(yè)可同時(shí)作業(yè)，且效率較高，但是僅適用于地質(zhì)條件較好的巷道；目前應(yīng)用最多的還是懸臂式掘進(jìn)機(jī)完成巷道截割，工人操作單體錨桿鉆機(jī)完成巷道支護(hù)作業(yè)任務(wù)，該種操作方式支護(hù)效率低，安全性差。因此，巷道成形存在裝備自動(dòng)化程度低，工人勞動(dòng)強(qiáng)度大，工作效率無(wú)法有效提升。

為解決以上難題，提出集掘進(jìn)和支護(hù)流程于一體的鉆錨機(jī)器人，如圖1 所示。鉆錨機(jī)器人主要由懸臂式掘進(jìn)機(jī)和兩個(gè)六自由度機(jī)械臂組成。懸臂式掘進(jìn)機(jī)完成巷道掘進(jìn)作業(yè)任務(wù)；兩個(gè)六自由度機(jī)械臂分別集成在懸臂式掘進(jìn)機(jī)機(jī)身兩側(cè)，該機(jī)械臂是針對(duì)煤礦巷道支護(hù)工藝及當(dāng)前支護(hù)裝備存在問(wèn)題等專門設(shè)計(jì)的，兩個(gè)鉆臂結(jié)構(gòu)相同，該鉆臂包含5 個(gè)旋轉(zhuǎn)關(guān)節(jié)和一個(gè)移動(dòng)關(guān)節(jié)，前兩個(gè)旋轉(zhuǎn)關(guān)節(jié)和第3 個(gè)移動(dòng)關(guān)節(jié)決定了鉆機(jī)末端鉆臂在空間中的大致位置，后3 個(gè)旋轉(zhuǎn)關(guān)節(jié)類似于手腕關(guān)節(jié)，決定了鉆機(jī)末端鉆臂在空間中的姿態(tài)，實(shí)現(xiàn)鉆機(jī)的對(duì)準(zhǔn)，理論上可以到達(dá)空間任意一點(diǎn)。根據(jù)巷道尺寸不同選擇機(jī)械臂，以完成自動(dòng)鉆錨作業(yè)。

圖1 鉆錨機(jī)器人結(jié)構(gòu)Fig.1 Structure of anchor drilling robot

鉆機(jī)作為末端執(zhí)行器布置在鉆臂末端，臨時(shí)支護(hù)裝置能夠提供安全的作業(yè)環(huán)境，每個(gè)旋轉(zhuǎn)關(guān)節(jié)和移動(dòng)關(guān)節(jié)分別配備拉繩位移傳感器和傾角儀，實(shí)時(shí)測(cè)量各個(gè)關(guān)節(jié)運(yùn)動(dòng)變化量。鉆錨機(jī)器人配置多套視覺(jué)系統(tǒng)，能夠?qū)崿F(xiàn)鉆錨孔識(shí)別定位及構(gòu)成閉環(huán)的視覺(jué)伺服控制系統(tǒng)，有效提高支護(hù)裝備自動(dòng)化程度，提高支護(hù)效率。

鉆錨孔間排距等支護(hù)參數(shù)根據(jù)巷道尺寸和地質(zhì)條件確定，但在實(shí)際支護(hù)過(guò)程，由于頂板凹凸不平且存在施工誤差，鉆錨孔并不能完全嚴(yán)格按照設(shè)計(jì)的間排距排列，因此在鉆錨機(jī)器人機(jī)身上布置一套雙目視覺(jué)系統(tǒng)，用于采集鋼帶圖像并確定鉆錨孔中心空間坐標(biāo)并實(shí)現(xiàn)鉆錨機(jī)器人基于位置的視覺(jué)伺服控制，鉆臂根據(jù)雙目視覺(jué)系統(tǒng)反饋信息運(yùn)動(dòng)至目標(biāo)位置，實(shí)現(xiàn)鉆臂粗略控制；同時(shí)在鉆機(jī)末端配置一套單目視覺(jué)系統(tǒng)，用于采集鉆錨孔圖像信息并實(shí)現(xiàn)鉆臂基于圖像的視覺(jué)伺服控制，鉆臂根據(jù)視覺(jué)系統(tǒng)反饋信息實(shí)現(xiàn)鉆臂的精確控制到達(dá)鉆錨孔中心，完成鉆孔及錨固作業(yè)任務(wù)。臨時(shí)支護(hù)裝置能夠?qū)搸Ч潭ㄔ陧敯迳希峁┌踩鳂I(yè)環(huán)境。

鉆錨機(jī)器人系統(tǒng)方案為：構(gòu)建鉆錨機(jī)器人運(yùn)動(dòng)學(xué)模型，進(jìn)行運(yùn)動(dòng)學(xué)分析；通過(guò)系統(tǒng)搭載的雙目視覺(jué)測(cè)量系統(tǒng)識(shí)別鉆錨孔，并確定鉆錨孔中心空間坐標(biāo)，將其作為目標(biāo)位置，求解鉆錨機(jī)器人各關(guān)節(jié)變化量；根據(jù)鉆臂結(jié)構(gòu)參數(shù)及鉆錨孔排列方式，構(gòu)建鉆錨孔孔序規(guī)劃策略；基于孔序規(guī)劃策略完成雙鉆臂鉆錨順序，利用五次多項(xiàng)式插值方法完成軌跡規(guī)劃；構(gòu)建基于位置的視覺(jué)伺服控制系統(tǒng)實(shí)現(xiàn)鉆臂的粗略運(yùn)動(dòng)控制和基于圖像的視覺(jué)伺服控制系統(tǒng)實(shí)現(xiàn)鉆臂的精確運(yùn)動(dòng)控制，使鉆臂運(yùn)動(dòng)至目標(biāo)位置，實(shí)現(xiàn)自動(dòng)鉆錨作業(yè)任務(wù)。鉆錨機(jī)器人系統(tǒng)方案如圖2 所示。

圖2 鉆錨機(jī)器人系統(tǒng)方案Fig.2 Scheme of anchor drilling robot system

2 鉆機(jī)目標(biāo)位置及鉆臂關(guān)節(jié)變量分析

2.1 鉆臂運(yùn)動(dòng)學(xué)建模

兩個(gè)六自由度鉆臂對(duì)稱布置在鉆錨機(jī)器人機(jī)身兩側(cè)，因此只需要選取一個(gè)鉆臂進(jìn)行研究分析。依據(jù)改進(jìn)的DH 坐標(biāo)系構(gòu)建法則對(duì)此六自由度鉆臂進(jìn)行分析，首先找出六個(gè)關(guān)節(jié)軸，沿著關(guān)節(jié)軸i的方向?yàn)檩Szi的方向，以關(guān)節(jié)軸i和i+1 的交點(diǎn)或公垂線與關(guān)節(jié)軸i的交點(diǎn)作為連桿坐標(biāo)系xiyizi的原點(diǎn)；規(guī)定xi軸沿公垂線的指向，若關(guān)節(jié)軸i和i+1 相交，則xi軸垂直于關(guān)節(jié)軸i和i+1 所在的平面；最后按照右手定則確定yi軸的方向。圖3 所示xiyizi坐標(biāo)系為基于此原則建立的鉆臂各個(gè)關(guān)節(jié)坐標(biāo)系，其中x0y0z0坐標(biāo)系與x1y1z1坐標(biāo)系重合，固定在基座上。d3為連桿3 的偏移量，θ1—θ6為對(duì)應(yīng)關(guān)節(jié)轉(zhuǎn)動(dòng)角度。

圖3 基于改進(jìn)DH 方法的機(jī)械臂運(yùn)動(dòng)坐標(biāo)系Fig.3 Motion coordinate system of manipulator based on modified DH method

根據(jù)建立的DH 坐標(biāo)系，得到六自由度鉆臂的DH參數(shù)，見(jiàn)表1。

表1 六自由度鉆臂DH 參數(shù)Table 1 D-H coordinate system parameters of the six-degreeof-freedom manipulator

2.2 鉆機(jī)目標(biāo)位置

基于改進(jìn)的DH 坐標(biāo)系構(gòu)建方法建立的參數(shù)表及鉆臂相應(yīng)連桿的參數(shù)，通過(guò)相鄰連桿的坐標(biāo)系轉(zhuǎn)換可得到鉆臂末端執(zhí)行器(鉆機(jī))相對(duì)于鉆臂基坐標(biāo)系的轉(zhuǎn)換關(guān)系，即為鉆臂正運(yùn)動(dòng)學(xué)模型，得到鉆機(jī)目標(biāo)位置相對(duì)鉆臂基坐標(biāo)系的空間位姿。坐標(biāo)系{i}和坐標(biāo)系{i-1}的轉(zhuǎn)換關(guān)系可以用變換矩陣表示[21]：

將鉆錨機(jī)器人鉆臂DH 參數(shù)及對(duì)應(yīng)連桿參數(shù)代入式(1)，可得：

經(jīng)解算，末端執(zhí)行器(鉆機(jī))坐標(biāo)系相對(duì)于基坐標(biāo) 系轉(zhuǎn)換關(guān)系為：

其中：

式中：a和o分別為接近向量和方向向量；n=o×a為法向量，p為位置向量。單位正交向量n、o和a描述了鉆機(jī)相對(duì)于鉆臂基坐標(biāo)系的姿態(tài)，p描述了鉆機(jī)相對(duì)于鉆臂基坐標(biāo)系的位置。θ1、θ2、d3、θ4、θ5和 θ6分別表示鉆臂5 個(gè)旋轉(zhuǎn)變量和一個(gè)平移變量，根據(jù)以上分析，將各個(gè)變量對(duì)應(yīng)值代入式(2)，即可得到鉆機(jī)相對(duì)于鉆臂基坐標(biāo)系的空間位姿，通過(guò)坐標(biāo)系轉(zhuǎn)換可得到其相對(duì)于鉆錨機(jī)器人機(jī)身坐標(biāo)系空間位姿。鉆錨機(jī)器人實(shí)際工作時(shí)通過(guò)自動(dòng)定位模塊獲取鉆錨孔空間位姿，求解鉆臂各個(gè)關(guān)節(jié)變量，因此，還需研究到達(dá)空間某一點(diǎn)時(shí)鉆臂各個(gè)關(guān)節(jié)變量。

2.3 鉆臂關(guān)節(jié)變量

機(jī)械臂正運(yùn)動(dòng)學(xué)分析能夠求解鉆臂各關(guān)節(jié)在一定運(yùn)動(dòng)變化范圍內(nèi)，鉆機(jī)在空間坐標(biāo)系中的位姿；逆運(yùn)動(dòng)學(xué)分析主要是鉆機(jī)空間位姿已知條件下求解各個(gè)關(guān)節(jié)運(yùn)動(dòng)量。本系統(tǒng)中，布置在鉆機(jī)上的雙目視覺(jué)模塊能夠?qū)崿F(xiàn)鉆錨孔空間位姿的解算，在此基礎(chǔ)上求解鉆臂逆運(yùn)動(dòng)學(xué)得到各關(guān)節(jié)運(yùn)動(dòng)變化量是實(shí)現(xiàn)鉆臂自動(dòng)控制、智能錨固的重點(diǎn)。

將式(2)含有θ1的部分移到方程左邊，則有：

其中：

令式(3)左右兩邊元素(2,4)分別相等，則有：

利用三角恒等變換對(duì)上式進(jìn)行處理可得：

同理，令式(3)兩邊元素(1,4)和(3,4)分別相等，則有：

對(duì)式(7)、式(8)進(jìn)行整理可得：

將式(2)含有θ1、θ2和d3的部分移到方程左邊，則有：

令等式(11)兩邊元素(2,3)、(1,3)、(3,3)、(2,1)和(2,2)分別相等，則有：

同理可解得：

通過(guò)以上分析可得雙鉆臂到達(dá)運(yùn)動(dòng)空間位置時(shí)各個(gè)關(guān)節(jié)變化量，但是到達(dá)空間一點(diǎn)各個(gè)關(guān)節(jié)變化量可能會(huì)存在多組解，在軌跡規(guī)劃過(guò)程中應(yīng)選取關(guān)節(jié)運(yùn)動(dòng)量最小或運(yùn)動(dòng)時(shí)間最短的路徑點(diǎn)。

3 鉆錨機(jī)器人雙鉆臂運(yùn)動(dòng)空間

工作空間是指機(jī)械臂末端執(zhí)行器能到達(dá)空間點(diǎn)的集合，鉆機(jī)的工作空間即為鉆機(jī)所能到達(dá)的空間位置。《煤礦安全規(guī)程》和《采礦工程設(shè)計(jì)手冊(cè)》明確規(guī)定了支護(hù)作業(yè)要求，其中包括：錨桿孔實(shí)際鉆孔角度與設(shè)計(jì)角度偏差小于5°，因此，對(duì)于巷道頂板和側(cè)幫支護(hù)時(shí)需要保證鉆機(jī)垂直頂板平面或側(cè)幫板平面，保證實(shí)際鉆孔角度與設(shè)計(jì)角度在偏差范圍內(nèi)。蒙特卡羅方法是基于統(tǒng)計(jì)理論的數(shù)值計(jì)算方法，借助隨機(jī)抽樣來(lái)解決數(shù)學(xué)問(wèn)題，基于蒙特卡羅方法利用Matlab 機(jī)器人工具箱解算該鉆錨機(jī)器人雙鉆臂運(yùn)動(dòng)空間。

計(jì)算鉆臂運(yùn)動(dòng)空間時(shí)，在各個(gè)關(guān)節(jié)變化范圍內(nèi)隨機(jī)取值求解末端點(diǎn)在空間坐標(biāo)系下位置，隨機(jī)選取大量的采樣點(diǎn)，盡可能構(gòu)建出鉆臂完整的運(yùn)動(dòng)空間。圖4為蒙特卡羅方法求解鉆臂運(yùn)動(dòng)空間方案，首先構(gòu)建鉆臂運(yùn)動(dòng)模型，設(shè)定隨機(jī)次數(shù)為Q，設(shè)置各個(gè)關(guān)節(jié)變化量，機(jī)械臂各個(gè)關(guān)節(jié)變量 θi(di)取 值范圍分別為 [θimin,θimax],利用隨機(jī)函數(shù)rand 生成Q個(gè)[0,1]的隨機(jī)點(diǎn)，以(θimax-θimin)?rand為隨機(jī)步長(zhǎng)，求解各個(gè)關(guān)節(jié)變量的隨機(jī)值，即：

圖4 蒙特卡羅方法求解鉆臂運(yùn)動(dòng)空間方案Fig.4 Scheme of manipulator workspace based on Monte Carlo method

將式(14)所得關(guān)節(jié)變量隨機(jī)值代入式(3)即可求得鉆機(jī)末端空間坐標(biāo)，將此步驟重復(fù)Q次，即可得到雙鉆臂運(yùn)動(dòng)空間輪廓，其中，Q取值越大，運(yùn)動(dòng)空間輪廓越接近真實(shí)情況。鉆臂各個(gè)關(guān)節(jié)取值范圍見(jiàn)表2，隨機(jī)次數(shù)Q取值為20 000 次，基于Matlab 仿真雙鉆臂鉆錨機(jī)器人運(yùn)動(dòng)空間如圖5 所示。

表2 六自由度鉆臂各關(guān)節(jié)運(yùn)動(dòng)范圍Table 2 Range of motion of each joint of six-degree-of-freedom manipulator

圖5 鉆臂運(yùn)動(dòng)空間Fig.5 Workspace of the manipulator

圖5a 為鉆錨機(jī)器人雙鉆臂在三維空間下運(yùn)動(dòng)空間，圖中X軸、Y軸和Z軸分別表示沿巷道掘進(jìn)方向、巷道寬度方向和巷道垂直高度方向，單位均為mm；圖5b、圖5c 分別為鉆臂運(yùn)動(dòng)空間在XOZ和XOY平面的投影，鉆錨機(jī)器人兩鉆臂平行分布在機(jī)身兩側(cè)，兩鉆臂之間距離1.2 m，由鉆臂運(yùn)動(dòng)空間在XOZ和XOY平面的投影可以看出，鉆錨機(jī)器人兩鉆臂針對(duì)巷道頂板錨固工作空間適用于巷道寬度不超過(guò)6 000 mm。鉆臂布置在鉆錨機(jī)器人機(jī)身上，距離巷道地面高度約2 m，工作過(guò)程機(jī)身受巷道底板情況影響，存在俯仰、橫滾與航向角，但一般情況下其變化幅度不大，可近似認(rèn)為鉆錨機(jī)器人機(jī)身保持不變。側(cè)幫支護(hù)過(guò)程中，低于鉆錨機(jī)器人機(jī)身位置，鉆臂與機(jī)身發(fā)生干涉無(wú)法垂直側(cè)幫進(jìn)行支護(hù)，側(cè)幫支護(hù)工作高度區(qū)間為2 000～4 500 mm。因此，鉆錨機(jī)器人可適用于巷道6 000 mm×4 500 mm 以內(nèi)的巷道頂板和側(cè)幫支護(hù)作業(yè)。

4 鉆錨孔孔序規(guī)劃及鉆臂軌跡規(guī)劃

4.1 鉆錨孔孔序規(guī)劃策略

孔序規(guī)劃主要是將基于雙目視覺(jué)的鉆錨孔空間位姿信息及其排列信息傳輸至控制系統(tǒng)，系統(tǒng)根據(jù)鉆錨孔空間位姿確定該鉆錨孔位于左側(cè)鉆臂或右側(cè)鉆臂作業(yè)空間范圍內(nèi)，分配錨固作業(yè)任務(wù)給相應(yīng)鉆臂；根據(jù)鉆臂起始位置和目標(biāo)位置，基于筆者提出的五次多項(xiàng)式插值方法規(guī)劃鉆臂軌跡，控制各個(gè)關(guān)節(jié)運(yùn)動(dòng)，鉆臂到達(dá)目標(biāo)位置，完成錨固作業(yè)任務(wù)。

圖6 所示為根據(jù)巷道結(jié)構(gòu)尺寸構(gòu)建雙鉆臂工作空間數(shù)學(xué)模型，XOY為煤礦巷道橫截面絕對(duì)坐標(biāo)系。鉆臂在完成錨固作業(yè)任務(wù)時(shí)，可以分別獨(dú)立完成各自工作空間范圍內(nèi)目標(biāo)點(diǎn)的錨固作業(yè)，若錨固點(diǎn)為奇數(shù)，則中間錨固點(diǎn)最后由左側(cè)鉆臂完成錨固作業(yè)。為了高效快速完成錨固作業(yè)任務(wù)，鉆臂需要協(xié)同作業(yè)。為避免兩鉆臂作業(yè)過(guò)程產(chǎn)生干涉，相互影響，需要考慮兩鉆臂協(xié)同控制問(wèn)題，避免鉆臂干涉，提高錨固效率。兩鉆臂對(duì)稱分布在鉆錨機(jī)器人機(jī)身兩側(cè)，兩側(cè)幫錨固作業(yè)任務(wù)分別由對(duì)應(yīng)側(cè)鉆臂單獨(dú)完成，協(xié)同控制模型主要針對(duì)頂板錨固作業(yè)任務(wù)，需要對(duì)左右鉆臂完成頂板錨固作業(yè)點(diǎn)個(gè)數(shù)和作業(yè)順序進(jìn)行規(guī)劃，使得兩鉆臂能夠協(xié)同作業(yè)，高效完成巷道支護(hù)作業(yè)任務(wù)。

圖6 雙鉆臂工作空間數(shù)學(xué)模型Fig.6 Mathematical model of workspace of two manipulator

按照規(guī)范要求，巷道支護(hù)需要先完成頂板支護(hù)再進(jìn)行側(cè)幫支護(hù)，其中頂板支護(hù)要求從中間向兩幫進(jìn)行順序施工。因此，鉆錨機(jī)器人兩鉆臂在錨固作業(yè)時(shí)，在頂板中間位置錨固作業(yè)時(shí)最容易發(fā)生干涉，相互影響，隨著頂板錨固作業(yè)任務(wù)從中間向兩幫進(jìn)行，兩鉆臂距離越來(lái)越遠(yuǎn)，不會(huì)發(fā)生干涉。因此，主要研究巷道頂板中間位置附近錨固點(diǎn)協(xié)同作業(yè)任務(wù)問(wèn)題，通過(guò)左右鉆臂錨固點(diǎn)歐氏距離判斷是否可能發(fā)生干涉。

圖7 所示為機(jī)械臂作業(yè)次序，主要研究中間位置錨固點(diǎn)作業(yè)次序及分配原則，圖7a 表示頂板錨固點(diǎn)數(shù)為奇數(shù)，圖7b 表示頂板錨固點(diǎn)數(shù)為偶數(shù)。1—4 分別表示巷道左右側(cè)幫從上往下鉆錨孔序號(hào)，(i-1)—(i+3)分別為巷道頂板從左往右鉆錨孔序號(hào)。假設(shè)相鄰錨固點(diǎn)之間距離為d0，鉆機(jī)工作半徑為R，為保證兩鉆臂錨固作業(yè)過(guò)程不發(fā)生干涉，必須保證左右兩鉆機(jī)工作時(shí)，左右鉆機(jī)的工作半徑之和大于兩個(gè)鉆錨孔之間的距離，即：

圖7 頂板錨固點(diǎn)孔序規(guī)劃Fig.7 Drilling sequence planning for roof anchor points

式中：m為左右鉆錨孔之間包含鉆錨孔個(gè)數(shù)。

1)頂板錨固點(diǎn)為2i+1 個(gè)

當(dāng)頂板錨固點(diǎn)為2i+1 時(shí)，若2R≤ 2d0，即R≤d0，則左側(cè)鉆臂從錨固點(diǎn)i開(kāi)始向錨固點(diǎn)i-1 方向完成錨固作業(yè)，同時(shí)右側(cè)鉆臂從錨固點(diǎn)i+2 開(kāi)始向錨固點(diǎn)i+3 方向完成錨固作業(yè)，最后由左側(cè)鉆臂完成頂板錨固點(diǎn)i+1 的錨固作業(yè)任務(wù)。

若2d0≤ 2R≤ 3d0，即d0≤R≤ 1.5d0，則左側(cè)鉆臂從錨固點(diǎn)i開(kāi)始向錨固點(diǎn)i-1 方向完成錨固作業(yè)，同時(shí)右側(cè)鉆臂從錨固點(diǎn)i+3 開(kāi)始向錨固點(diǎn)i+1 方向完成錨固作業(yè)，之后繼續(xù)從錨固點(diǎn)i+4 向錨固點(diǎn)2i+1 完成頂板錨固作業(yè)任務(wù)；若2R≥ 3d0，則依此類推完成頂板錨固作業(yè)任務(wù)。

2)頂板錨固點(diǎn)為2i個(gè)

當(dāng)頂板錨固點(diǎn)為2i時(shí)，若2R≤d0，即R≤ 0.5d0，則左側(cè)鉆臂從錨固點(diǎn)i開(kāi)始向錨固點(diǎn)i-1 方向完成錨固作業(yè)，同時(shí)右側(cè)鉆臂從錨固點(diǎn)i+1 開(kāi)始向錨固點(diǎn)i+2 方向完成錨固作業(yè)。

若d0≤ 2R≤ 2d0，即0.5d0≤R≤d0，則左側(cè)鉆臂從錨固點(diǎn)i開(kāi)始向錨固點(diǎn)i-1 方向完成錨固作業(yè)，同時(shí)右側(cè)鉆臂從錨固點(diǎn)i+2 開(kāi)始向錨固點(diǎn)i+1 方向完成錨固作業(yè)，之后繼續(xù)從錨固點(diǎn)i+3 向錨固點(diǎn)2i完成頂板錨固作業(yè)任務(wù)。

若2d0≤ 2R≤ 3d0，即d0≤R≤ 1.5d0，則左側(cè)鉆臂從錨固點(diǎn)i開(kāi)始向錨固點(diǎn)i-1 方向完成錨固作業(yè)，同時(shí)右側(cè)鉆臂從錨固點(diǎn)i+3 開(kāi)始向錨固點(diǎn)i+1 方向完成錨固作業(yè)，之后繼續(xù)從錨固點(diǎn)i+4 向錨固點(diǎn)2i完成頂板錨固作業(yè)任務(wù)；若2R≥ 3d0，則依此類推完成頂板錨固作業(yè)任務(wù)。在掘進(jìn)工作面進(jìn)行巷道掘進(jìn)時(shí)，錨索與錨桿支護(hù)會(huì)交錯(cuò)進(jìn)行，對(duì)于錨索的工作任務(wù)，其策略與錨桿一致，服從錨桿的最優(yōu)匹配策略。

4.2 鉆臂軌跡規(guī)劃方法

軌跡規(guī)劃是運(yùn)動(dòng)規(guī)劃的主要內(nèi)容，主要是通過(guò)在起始點(diǎn)和終點(diǎn)之間插入中間點(diǎn)序列，實(shí)現(xiàn)鉆臂沿著軌跡平穩(wěn)運(yùn)動(dòng)[22]。為保證鉆臂從起始點(diǎn)到目標(biāo)點(diǎn)運(yùn)動(dòng)軌跡平滑，速度和加速度無(wú)突變，三次多項(xiàng)式插值方法軌跡規(guī)劃只能夠保證速度和位移連續(xù)，并不能保證加速度連續(xù)，加速度突變可能會(huì)導(dǎo)致振動(dòng)甚至沖擊，因此，提出采用五次多項(xiàng)式插值方法進(jìn)行鉆臂軌跡規(guī)劃，五次多項(xiàng)式插值方法解決了關(guān)節(jié)角速度變化不平滑且加速度存在跳變的情況。兩鉆臂對(duì)稱布置在鉆錨機(jī)器人機(jī)身兩側(cè)，因此，設(shè)定左右鉆臂分別完成左右各部分頂板和側(cè)幫錨固作業(yè)任務(wù)。本文以右側(cè)鉆臂為研究對(duì)象，研究鉆臂軌跡規(guī)劃問(wèn)題。

設(shè)鉆臂關(guān)節(jié)關(guān)于時(shí)間的運(yùn)動(dòng)函數(shù)為：

式中：ci為五次多項(xiàng)式的系數(shù)，其角速度和角加速度函數(shù)表達(dá)式分別為：

對(duì)初始位置和目標(biāo)位置、角速度和角加速度代入式(17)和式(18)有：

式中：t0、tf分別為關(guān)節(jié)運(yùn)動(dòng)的起始時(shí)間和中止時(shí)間；s(t0)、s(tf)分別為關(guān)節(jié)的初始位置和目標(biāo)位置。

將上式寫成矩陣的形式，則有：

將上式記為AB=P，則B=A-1P，求解上式可得五次多項(xiàng)式各個(gè)系數(shù)。

5 實(shí)驗(yàn)結(jié)果與分析

5.1 基于數(shù)字孿生的鉆錨孔孔序規(guī)劃實(shí)驗(yàn)

采用SolidWorks 按比例構(gòu)建鉆錨機(jī)器人虛擬模型和煤礦巷道虛擬場(chǎng)景，建立鉆錨機(jī)器人運(yùn)動(dòng)學(xué)模型，采用Unity3D 完成虛擬模型動(dòng)作編程及虛擬模型與虛擬場(chǎng)景的耦合，實(shí)現(xiàn)基于數(shù)字孿生的鉆錨孔孔序規(guī)劃仿真實(shí)驗(yàn)。

在孔序規(guī)劃策略基礎(chǔ)上，根據(jù)鉆機(jī)參數(shù)結(jié)構(gòu)、鉆錨孔個(gè)數(shù)及相對(duì)位置對(duì)鉆錨孔進(jìn)行任務(wù)分配及作業(yè)順序規(guī)劃。如圖8 所示，鉆錨機(jī)器人及巷道數(shù)字孿生模型中，鉆機(jī)作業(yè)半徑為0.8 m，每個(gè)鉆錨孔間隔0.6 m。圖8a 中，鋼帶上共計(jì)9 個(gè)鉆錨孔，左側(cè)鉆臂完成鉆錨孔1—5，鉆錨順序?yàn)?—3—2—1—5，右側(cè)鉆臂完成鉆錨孔6—9，鉆錨順序?yàn)?—6—8—9；圖8b 中，鋼帶上共計(jì)8 個(gè)鉆錨孔，左側(cè)鉆臂完成鉆錨孔1—4，鉆錨順序?yàn)?—3—2—1，右側(cè)鉆臂完成鉆錨孔5—8，鉆錨順序?yàn)?—6—5—8。仿真結(jié)果表明，該孔序規(guī)劃策略能避免兩鉆臂工作過(guò)程發(fā)生干涉，有效提升錨固作業(yè)效率。

圖8 頂板錨固點(diǎn)孔序規(guī)劃仿真結(jié)果Fig.8 Simulation results of drilling sequence planning for roof anchor points

5.2 基于五次多項(xiàng)式的鉆臂軌跡規(guī)劃仿真實(shí)驗(yàn)

在Matlab 中利用機(jī)器人工具箱對(duì)鉆臂基于五次多項(xiàng)式插值方法進(jìn)行軌跡規(guī)劃仿真。設(shè)定其初始位置和目標(biāo)位置，初始位置各個(gè)關(guān)節(jié)量分別為[-π/3,-π/3,0,-π/6,-π/2,-π/2]，對(duì)應(yīng)的鉆臂末端空間坐標(biāo)為[250,-433,866]，目標(biāo)位置各個(gè)關(guān)節(jié)量分別為[π/3,0,400,π/6,0,π/2]，對(duì)應(yīng)的鉆臂末端空間坐標(biāo)為[700,1 212,0]。

圖9 為基于五次多項(xiàng)式的鉆臂軌跡規(guī)劃所得軌跡、位移、速度、加速度曲線。圖9a 紅色曲線表示鉆臂完成試驗(yàn)起點(diǎn)到終點(diǎn)的軌跡，軌跡平滑無(wú)波動(dòng)；圖9b 表示鉆臂各個(gè)關(guān)節(jié)位移變化曲線，關(guān)節(jié)3 為移動(dòng)關(guān)節(jié)，其位移量從0 逐漸連續(xù)平滑增加至400 mm，其余旋轉(zhuǎn)關(guān)節(jié)均從起始位置關(guān)節(jié)角度連續(xù)平滑至目標(biāo)位置各關(guān)節(jié)角度，整體連續(xù)平滑運(yùn)動(dòng)，各關(guān)節(jié)變量平穩(wěn)運(yùn)動(dòng)無(wú)突變，說(shuō)明兩點(diǎn)之間6 個(gè)關(guān)節(jié)都能夠平滑地進(jìn)行軌跡運(yùn)動(dòng)，圖9c 表示鉆臂各個(gè)關(guān)節(jié)速度變化曲線，關(guān)節(jié)2 和關(guān)節(jié)4 速度變化情況一致，兩條速度變化曲線重合，整體從起始點(diǎn)位置開(kāi)始逐漸增大，各個(gè)關(guān)節(jié)速度到達(dá)峰值后逐漸減小，在目標(biāo)位置時(shí)各個(gè)關(guān)節(jié)速度為零，整體均平穩(wěn)變化，未發(fā)生速度突變現(xiàn)象，圖9d 表示鉆臂各個(gè)關(guān)節(jié)加速度變化曲線，其變化規(guī)律類似于正弦曲線，從起始點(diǎn)位置開(kāi)始加速度逐漸增大，加速度到最大值之后逐漸減小，加速度減小為零時(shí)各個(gè)關(guān)節(jié)速度達(dá)到最大值，隨著加速度繼續(xù)減小，速度逐漸減小，其整體變化過(guò)程平穩(wěn)未發(fā)生突變。基于五次多項(xiàng)式線性插值軌跡規(guī)劃方法所得軌跡平滑，速度和加速度曲線連續(xù)、平滑變化說(shuō)明機(jī)械臂在運(yùn)動(dòng)過(guò)程中不會(huì)產(chǎn)生速度突變和加速度突變現(xiàn)象，提高了鉆臂運(yùn)動(dòng)效率。

5.3 結(jié)果分析

利用三次多項(xiàng)式插值方法對(duì)鉆臂進(jìn)行軌跡規(guī)劃，所得結(jié)果如圖10 所示。圖10a 紅色曲線表示鉆臂末端執(zhí)行器軌跡，圖10b 表示鉆臂各關(guān)節(jié)隨時(shí)間變化情況，從軌跡規(guī)劃起始點(diǎn)到目標(biāo)點(diǎn)，各關(guān)節(jié)變量均呈連續(xù) 平滑增大，與五次多項(xiàng)式插值方法軌跡規(guī)劃所得位移變化曲線圖相似，圖10c 表示鉆臂各關(guān)節(jié)速度變化曲線圖，關(guān)節(jié)2 和關(guān)節(jié)4 速度變化曲線一致，其余曲線均平滑增大，圖10d 表示鉆臂各關(guān)節(jié)加速度變化曲線，關(guān)節(jié)2 與關(guān)節(jié)4 加速度變化曲線一致，其余關(guān)節(jié)加速度均線性變化，變化速率一致，但各關(guān)節(jié)加速度在起始點(diǎn)和目標(biāo)點(diǎn)加速度值均不為零，存在加速度突變情況。對(duì)比分析可知，基于五次多項(xiàng)式插值方法的軌跡規(guī)劃所得位移、速度和加速度變化曲線均連續(xù)且平滑，在起始點(diǎn)和目標(biāo)點(diǎn)加速度從零開(kāi)始逐漸變化，無(wú)突變發(fā)生。

圖10 基于三次多項(xiàng)式插值方法的軌跡規(guī)劃Fig.10 Trajectory planning based on cubic polynomial interpolation method

6 結(jié)論

a.根據(jù)巷道支護(hù)工藝和鉆錨孔數(shù)量等提出一種孔序規(guī)劃策略，在運(yùn)動(dòng)學(xué)分析基礎(chǔ)上提出采用五次多項(xiàng)式插值方法進(jìn)行鉆臂軌跡規(guī)劃，仿真結(jié)果表明本文所述方法能有效避免雙鉆臂干涉，且鉆臂能夠快速平穩(wěn)到達(dá)目標(biāo)位置。

b.設(shè)計(jì)的鉆錨機(jī)器人對(duì)巷道地質(zhì)條件適應(yīng)性強(qiáng)，鉆臂集成在掘進(jìn)機(jī)兩側(cè)能夠減少掘進(jìn)設(shè)備與支護(hù)設(shè)備交替調(diào)度時(shí)間。能夠自動(dòng)完成孔序規(guī)劃和鉆臂軌跡規(guī)劃，提高設(shè)備自動(dòng)化程度，減少支護(hù)作業(yè)人員數(shù)量。

c.鉆錨機(jī)器人較強(qiáng)的環(huán)境適應(yīng)性和較高的自動(dòng)化程度，在各種地質(zhì)環(huán)境的煤礦巷道有廣闊的應(yīng)用前景。后續(xù)研究工作可集中在鉆錨機(jī)器人視覺(jué)伺服控制方法研究，將鉆錨孔自動(dòng)定位技術(shù)、孔序規(guī)劃策略、軌跡規(guī)劃方法和視覺(jué)伺服控制技術(shù)等結(jié)合起來(lái)構(gòu)建智能支護(hù)系統(tǒng)，實(shí)現(xiàn)巷道支護(hù)全流程智能化，提高巷道支護(hù)效率。