梁春苗，姚寧平,2,*，姚亞峰,2，彭 濤

(1.中煤科工西安研究院(集團(tuán))有限公司，陜西 西安 710077；2.煤炭科學(xué)研究總院，北京 100013)

《煤礦機(jī)器人重點(diǎn)研發(fā)目錄》要求探放水、防突和防沖鉆孔機(jī)器人具備自動(dòng)調(diào)整鉆姿、智能鉆孔規(guī)劃和定位功能，目前鉆機(jī)位姿調(diào)節(jié)采用開環(huán)控制，受加工誤差、裝配誤差和零部件磨損等影響，自動(dòng)調(diào)節(jié)時(shí)無法達(dá)到精確定位，進(jìn)而影響瓦斯抽采、探放水、防突和防沖效果。煤礦井下鉆孔自動(dòng)精確定位是實(shí)現(xiàn)鉆孔機(jī)器人自主施工孔群的難點(diǎn)，也是實(shí)現(xiàn)少人化、無人化的關(guān)鍵技術(shù)。

國(guó)內(nèi)外少有煤礦井下鉆孔機(jī)器人鉆臂定位和位姿補(bǔ)償方面研究，而在鑿巖機(jī)器人鉆臂位姿調(diào)節(jié)、控制和定位技術(shù)方面成果較多，已成為自主化鑿巖機(jī)器人的關(guān)鍵技術(shù)[1]。中南大學(xué)率先開展了鑿巖機(jī)器人方向研究，何清華[2]、謝習(xí)華[3]等對(duì)大臂伸縮、推進(jìn)梁伸縮、翻轉(zhuǎn)定位產(chǎn)生的誤差進(jìn)行分析和誤差補(bǔ)償，建立了位姿誤差模型。近幾年中南大學(xué)學(xué)者將研究成果向產(chǎn)品轉(zhuǎn)化，夏毅敏等[4-6]基于有限元仿真，采用正交試驗(yàn)法確定影響臂架柔性誤差的主要參數(shù)，得到柔性誤差補(bǔ)償公式，并建立基于位置誤差的運(yùn)動(dòng)學(xué)誤差標(biāo)定模型，使釬桿的絕對(duì)定位誤差降低了79.6%；并從釬桿末端位姿誤差補(bǔ)償角度出發(fā)，建立基于釬桿運(yùn)動(dòng)學(xué)誤差補(bǔ)償模型和MPGA 求逆解關(guān)節(jié)變量的釬桿定位算法，實(shí)測(cè)釬桿定位誤差小于0.1 m，滿足工程實(shí)際定位要求。吳昊駿從工程應(yīng)用實(shí)際出發(fā)，建立鉆孔位姿參數(shù)和鉆具末端塌落量的函數(shù)關(guān)系，對(duì)定位誤差進(jìn)行補(bǔ)償[7-8]；上述文獻(xiàn)主要針對(duì)工程行業(yè)的鑿巖機(jī)器人或者全電腦鑿巖臺(tái)車，遠(yuǎn)不及工業(yè)機(jī)器補(bǔ)償時(shí)的高精度和實(shí)時(shí)性，張憲民等[9]研究并聯(lián)機(jī)器人時(shí)，基于關(guān)節(jié)間隙使用運(yùn)動(dòng)學(xué)標(biāo)定識(shí)別誤差種類并分析其影響規(guī)律，并在逆運(yùn)動(dòng)學(xué)模型中補(bǔ)償，提高了重復(fù)定位精度，但是缺乏實(shí)時(shí)性。因此，史曉佳[10]、李睿[11]、杜亮[12]等借助在機(jī)器人末端加裝激光跟蹤儀進(jìn)行誤差研制或者在線誤差二次補(bǔ)償，提高了機(jī)器人控制精度，但是忽視了機(jī)械臂因連桿柔性、關(guān)節(jié)柔性等非線性變形的綜合影響導(dǎo)致末端產(chǎn)生偏差，缺乏剛?cè)崂碚撝?，隨后，譚月勝[13]、李文龍[14]等結(jié)合剛?cè)狁詈辖⒛Ｐ瓦M(jìn)行快速補(bǔ)償，得到了有效和實(shí)用的補(bǔ)償效果。

上述文獻(xiàn)分別針對(duì)鑿巖機(jī)器人和工業(yè)機(jī)器人兩個(gè)不同行業(yè)誤差補(bǔ)償?shù)男枨?，利用運(yùn)動(dòng)學(xué)建模方法，對(duì)幾何尺寸誤差、關(guān)節(jié)間隙或幾何尺寸誤差與關(guān)節(jié)間隙耦合產(chǎn)生的誤差進(jìn)行分析，建立了剛?cè)峤Y(jié)合的補(bǔ)償模型，實(shí)現(xiàn)了誤差補(bǔ)償，對(duì)于鉆孔機(jī)器人鉆臂定位誤差補(bǔ)償研究具有借鑒意義，然而上述計(jì)算方法無法保證計(jì)算速度和響應(yīng)的實(shí)時(shí)性。因此，筆者通過分析鉆臂定位精度影響因素，借助激光測(cè)距和開孔定位儀實(shí)測(cè)參數(shù)，建立在線鉆臂誤差補(bǔ)償模型，實(shí)現(xiàn)鉆臂實(shí)時(shí)誤差監(jiān)測(cè)與誤差補(bǔ)償，為煤礦井下無人化做好技術(shù)儲(chǔ)備。

1 經(jīng)典鉆臂運(yùn)動(dòng)學(xué)誤差補(bǔ)償方法

1.1 鉆臂運(yùn)動(dòng)學(xué)分析與正逆解求解

1.1.1 鉆臂結(jié)構(gòu)分析與運(yùn)動(dòng)學(xué)建模

鉆臂是鉆進(jìn)機(jī)器人的主要工作機(jī)構(gòu)，鉆臂的主要?jiǎng)幼靼轿唤钦{(diào)節(jié)、升降調(diào)節(jié)、傾角調(diào)節(jié)和孔口位置的平移調(diào)節(jié)。圖1 為鉆臂結(jié)構(gòu)，主要由平移機(jī)構(gòu)、主機(jī)、上穩(wěn)固裝置、傾角調(diào)節(jié)機(jī)構(gòu)、升降機(jī)構(gòu)和方位回轉(zhuǎn)機(jī)構(gòu)6 部分組成。

圖1 鉆臂結(jié)構(gòu)Fig.1 Structure of drilling arm

分析對(duì)鉆臂結(jié)構(gòu)和動(dòng)作可知，鉆臂有傾角回轉(zhuǎn)θ2、方位回轉(zhuǎn)θ1、機(jī)身舉升d1和機(jī)身平移d34 個(gè)自由度。利用D-H 方法建立鉆進(jìn)機(jī)器人鉆臂運(yùn)動(dòng)學(xué)模型，鉆臂坐標(biāo)系如圖2 所示。鉆臂D-H 參數(shù)見表1[15-16]。

表1 鉆臂D-H 參數(shù)Table 1 D-H parameters of drilling arm

圖2 鉆臂坐標(biāo)系Fig.2 Coordinate system of drilling arm

1.1.2 鉆臂正運(yùn)動(dòng)學(xué)求解

兩相鄰關(guān)節(jié)坐標(biāo)系之間的齊次變換矩陣為：

可得到鉆臂執(zhí)行機(jī)構(gòu)末端與鉆臂基坐標(biāo)系的關(guān)系矩陣為：

矩陣中含有θ1、θ2、d1、d3共4 個(gè)未知數(shù)，當(dāng)這4 個(gè)未知的關(guān)節(jié)變量已知時(shí)，即可通過式(2)得到鉆臂末端執(zhí)行機(jī)構(gòu)相對(duì)于鉆臂基坐標(biāo)系的位姿正解。

1.1.3 鉆臂逆運(yùn)動(dòng)學(xué)求解

鉆臂有兩個(gè)轉(zhuǎn)動(dòng)和兩個(gè)移動(dòng)關(guān)節(jié)，滿足Pieper 結(jié)構(gòu)特點(diǎn)，因此，采用代數(shù)解法對(duì)鉆臂進(jìn)行逆運(yùn)動(dòng)學(xué)求解[17-19]。求解用左乘式(2)的兩邊有：

式(3)和式(4)中兩個(gè)矩陣對(duì)應(yīng)矩陣元素相等，因此，可以列方程依次求θ1、θ2、d1、d3。

1.2 鉆臂定位誤差原因分析

機(jī)構(gòu)因加工誤差、變形、傳感器誤差、裝配間隙等，實(shí)際控制時(shí)與理論有偏差。鉆臂屬串聯(lián)機(jī)構(gòu)，從方位、舉升、傾角調(diào)節(jié)、平移油缸等誤差會(huì)逐級(jí)傳遞并放大，在鉆臂末端形成位姿總誤差，位姿誤差的存在會(huì)影響開孔位置精度[20-21]。鉆臂的傾角和方位角調(diào)節(jié)關(guān)鍵部件是回轉(zhuǎn)式減速器，回轉(zhuǎn)減速器為蝸輪蝸桿式結(jié)構(gòu)，其嚙合時(shí)存在間隙，一般回轉(zhuǎn)式減速器精度控制在10 弧分以內(nèi)，同時(shí)托板裝置采用導(dǎo)軌方式與機(jī)身連接，連接處間隙對(duì)機(jī)身末端定位精度將會(huì)有較大影響。因此，機(jī)身在平移時(shí)，傾角和方位角都會(huì)存在誤差。如圖3a 機(jī)身從0～450 mm 移動(dòng)時(shí)，不同傾角時(shí)的傾角誤差；圖3b 機(jī)身從0～450 mm 移動(dòng)時(shí)，不同方位時(shí)的誤差。

圖3 機(jī)身平移引起的傾角和方位角誤差Fig.3 Dip angle and azimuth errors caused by body translation

而因回轉(zhuǎn)減速器間隙存在，當(dāng)轉(zhuǎn)動(dòng)時(shí)對(duì)傾角和方位角誤差影響較大，當(dāng)傾角從-30°～+40°變化時(shí)，最大誤差可達(dá)0.85°。若回轉(zhuǎn)中心距離鉆桿開孔位置2 000 mm，那么高度方向z將引起29.6 mm 的偏差，方位角在從0°～+15°變化時(shí)，最大誤差可達(dá)0.79°，若鉆桿開孔位置在2 000 mm 遠(yuǎn)處，那么x方向?qū)⒁?7.5 mm 的偏差。機(jī)身平移會(huì)使得傾角發(fā)生較大偏移，尤其是平移至300 mm 行程時(shí)，所有傾角狀態(tài)下誤差最大，都將直接影響鉆孔作業(yè)精度，因此，需對(duì)鉆臂進(jìn)行誤差的檢測(cè)與補(bǔ)償，從而提高定位精度。

1.3 鉆臂靜、動(dòng)態(tài)誤差補(bǔ)償及逆解求解

影響鉆臂位姿誤差的靜態(tài)誤差包括結(jié)構(gòu)參數(shù)誤差和制造誤差等，動(dòng)態(tài)誤差即受外力和慣性力，使其柔性變形的誤差。機(jī)構(gòu)誤差分析中的微分矩陣法是采用全站儀和角度儀標(biāo)定相鄰關(guān)節(jié)偏移距離和偏轉(zhuǎn)角，得到結(jié)構(gòu)參量和運(yùn)動(dòng)參量的誤差后，求位姿變換矩陣全微分建立誤差模型。對(duì)式(1)各關(guān)節(jié)傳遞矩陣結(jié)構(gòu)參數(shù)求偏導(dǎo)得：

因此，鉆臂靜態(tài)誤差補(bǔ)償模型為：

利用材料力學(xué)和理論力學(xué)相關(guān)知識(shí)，根據(jù)機(jī)身截面型心計(jì)算慣性矩可得推進(jìn)行程的撓性變形。當(dāng)平移油缸推進(jìn)長(zhǎng)度l從0 到450 mm 變化時(shí)，轉(zhuǎn)角和撓度隨平移長(zhǎng)度的變化曲線如圖4 所示。

圖4 機(jī)身轉(zhuǎn)角和撓度隨平移長(zhǎng)度變化曲線Fig.4 Variation of body rotating angle and deflection with translation length

將撓度f和轉(zhuǎn)角θ兩個(gè)誤差量代入傳遞矩陣0T'3，對(duì)鉆臂末端進(jìn)行誤差補(bǔ)償變換如圖5 所示。

圖5 動(dòng)態(tài)誤差補(bǔ)償坐標(biāo)變換Fig.5 Dynamic error compensation coordinate transformation

圖5 中坐標(biāo)系O2先沿著x軸移動(dòng)塌落量f值，再沿著y軸旋轉(zhuǎn)了θ，變換至O'3，變換關(guān)系為：

由于存在動(dòng)態(tài)誤差和靜態(tài)誤差，因此，鉆臂運(yùn)動(dòng)學(xué)總誤差補(bǔ)償模型式如下：

采用基于RBF 神經(jīng)網(wǎng)絡(luò)法求逆解的方法對(duì)鉆臂總誤差補(bǔ)償模型進(jìn)行逆運(yùn)動(dòng)學(xué)求解，使鉆桿開孔位置達(dá)到期望位姿[18-19]。

為了簡(jiǎn)化輸入樣本矩陣，采用歐拉變換RPY 角的方式表達(dá)位姿，計(jì)算式如下：

在訓(xùn)練中選取1 000 組進(jìn)行訓(xùn)練，基于鉆臂運(yùn)動(dòng)學(xué)誤差補(bǔ)償模型所得到的鉆臂末端鉆桿位姿計(jì)算逆解，并取100 組作為測(cè)試樣本數(shù)據(jù)，使用RBF 神經(jīng)網(wǎng)絡(luò)訓(xùn)練得到的逆解與測(cè)試值角度誤差不超過0.028°，位移誤差不超過0.5 mm。

施工中根據(jù)地層信息設(shè)計(jì)鉆孔得到期望位姿，基于鉆臂誤差補(bǔ)償模型的逆運(yùn)動(dòng)學(xué)求解，求逆后的關(guān)節(jié)值輸入驅(qū)動(dòng)鉆臂，利用全站儀測(cè)量鉆臂各關(guān)節(jié)變化后達(dá)到的實(shí)際位姿。圖6 是采用靜態(tài)、動(dòng)態(tài)補(bǔ)償后，回轉(zhuǎn)中心距離鉆桿開孔位置2 000 mm 時(shí)，鉆臂期望位姿與實(shí)際位姿誤差，x方向平均誤差為9.6 mm(對(duì)應(yīng)方位誤差0.28°)，y方向平均誤差為18.2 mm，z方向平均誤差為16 mm(對(duì)應(yīng)傾角誤差0.46°)。

圖6 鉆臂期望位姿與實(shí)際位姿誤差Fig.6 Errors between the expected and actual pose of drilling arm

上述靜態(tài)和動(dòng)態(tài)誤差補(bǔ)償對(duì)于自重和外部產(chǎn)生載荷導(dǎo)致的彈性變形、加工制造和檢測(cè)產(chǎn)生的誤差有效，可滿足工程實(shí)際要求。而使用一段時(shí)間后，因結(jié)合面的磨損間隙發(fā)生變化，補(bǔ)償效果不明顯。同時(shí)，經(jīng)典的全站儀誤差檢測(cè)法較為復(fù)雜，對(duì)測(cè)試人員專業(yè)水平要求較高，因此，本文提出了一種不借助專業(yè)全站儀和測(cè)角儀方法的激光測(cè)距組合定位誤差在線檢測(cè)的方法。

2 基于激光測(cè)距組合的鉆臂定位誤差檢測(cè)

2.1 激光測(cè)距組合位姿誤差檢測(cè)原理

如圖7 所示，鉆臂鉆孔施工前方放置一個(gè)測(cè)試靶板，動(dòng)力頭后部鉆桿中心處安裝高精度激光測(cè)距傳感器，確保激光穿過鉆桿和動(dòng)力頭內(nèi)孔，調(diào)整位置使激光發(fā)射和接收信號(hào)工作正常。將開孔定向儀放置在傾角和方位角回轉(zhuǎn)中心位置，手動(dòng)調(diào)節(jié)傾角和方位角，找到方位和傾角的零位，并得到初始時(shí)的激光測(cè)距距離，再通過改變傾角和方位角后再次測(cè)得距離，采用該方法可獲得數(shù)組傾角、方位角輸入值和實(shí)際計(jì)算值之間的關(guān)系，可得鉆臂在位姿變化時(shí)的誤差量。

圖7 激光測(cè)距發(fā)射、接收與激光斑點(diǎn)位置投射Fig.7 Laser ranging transmission,reception and projection of laser spot position

鉆孔機(jī)器人較頻繁的施工傾角θ2范圍是-30°～+45°、方位角θ1是-15°～+15°，鉆孔時(shí)鉆臂位姿精度會(huì)因平移油缸平移距離d3從0～450 mm 推進(jìn)變化而變化。因此，試驗(yàn)中擬采集方位角-15°、0°和+15°，傾角-30°、-20°、-10°、0°、+10°、+20°、+30°、+45°，在每種姿態(tài)下，將機(jī)身從舉升立柱的中間部位推進(jìn)到最前端，也就是平移油缸推著機(jī)身沿著托板平移450、300、150 和0 mm 時(shí)的4 種狀態(tài)下測(cè)試，與不同傾角和方位角誤差值組合，即方位測(cè)試12 組，傾角測(cè)試32 組。

2.2 激光測(cè)距組合位姿誤差檢測(cè)

測(cè)試分兩種狀態(tài)，一種是當(dāng)方位角θ1=0°時(shí)，在不同傾角θ2變化下，測(cè)試平移油缸在4 種推進(jìn)狀態(tài)下的試驗(yàn)數(shù)據(jù)，此時(shí)獲取傾角誤差；另外一種是當(dāng)傾角θ2=0°時(shí)，在不同方位角θ1變化下，測(cè)試平移油缸在4 種推進(jìn)狀態(tài)下的試驗(yàn)數(shù)據(jù)，此時(shí)獲取方位角誤差。

1)激光測(cè)距組合法測(cè)傾角誤差

圖8 是基于激光測(cè)距組合位姿誤差檢測(cè)計(jì)算實(shí)際傾角誤差的原理，O2和O3點(diǎn)分別是舉升和平移變化后傾角轉(zhuǎn)動(dòng)中心，A1點(diǎn)是激光測(cè)距傳感器安裝點(diǎn)，該安裝位置在Y方向與轉(zhuǎn)動(dòng)中心有Ld1距離，Lc1是鉆臂在經(jīng)過高精度開孔定位儀尋找的初始水平點(diǎn)激光測(cè)距測(cè)定的距離，H0是鉆臂舉升初始距離，舉升高度為ΔH1，Hz是在傾角改變和平移油缸平移后，激光測(cè)距傳感器投射點(diǎn)距離車體地面的高度，可作為計(jì)算角度的復(fù)測(cè)值。鉆臂平移油缸平移距離為L(zhǎng)y，當(dāng)在開孔定位軟件界面的將輸入傾角θ2變化后，Lz1是傾角變化后測(cè)距傳感器測(cè)定距離，實(shí)際傾角θ'2可由下式計(jì)算。

圖8 實(shí)際傾角誤差量計(jì)算原理Fig.8 Calculation principle of inclinaiton error

圖9 是測(cè)試得到的傾角θ2、平移距離d3（分別為450、300、150 和0 mm）和傾角誤差Δθ2之間的關(guān)系，4 條線就是4 種平移情況下對(duì)應(yīng)傾角變化時(shí)的誤差曲線?？梢园l(fā)現(xiàn)平移油缸在同一推進(jìn)距離時(shí)，傾角越大，誤差量越大，隨著平移推進(jìn)，同一傾角時(shí)誤差量逐漸增大，說明機(jī)身相對(duì)于托板裝置的間隙、傾角調(diào)節(jié)裝置裝配間隙都會(huì)引起末端誤差的產(chǎn)生。

圖9 傾角、平移距離和傾角誤差之間關(guān)系Fig.9 Relationship of dip angle,translation distance and dip angle error

2)激光測(cè)距組合法測(cè)方位誤差

圖10 是基于激光測(cè)距組合位姿誤差檢測(cè)計(jì)算實(shí)際方位誤差的原理，O0點(diǎn)是方位角轉(zhuǎn)動(dòng)中心，A2點(diǎn)是激光測(cè)距傳感器安裝點(diǎn)，該安裝位置在x方向與轉(zhuǎn)動(dòng)中心有Ld2距離，在x方向與方位角水平線有h距離，Lc2是鉆臂在經(jīng)過高精度開孔定位儀尋找的初始水平點(diǎn)激光測(cè)距測(cè)定的距離，平移油缸平移距離為L(zhǎng)y，在開孔定位軟件界面，將輸入方位角θ1改變，Lz2是方位角變化后測(cè)距傳感器測(cè)定距離，Δl是因方位角變化h增加段的距離，實(shí)際方位角θ'1可由下3 式計(jì)算。

圖10 方位角誤差量計(jì)算原理Fig.10 Calculation principle of azimuth error

存在關(guān)系式：

因此，有：

通過下式可以計(jì)算得到實(shí)際方位角θ'1。

由實(shí)際方位角θ'1可通過下式計(jì)算x、y、z的具體坐標(biāo)值，其中Lc2、Ly、Lz2、h2、L1、H0、ΔH1都是已知量，當(dāng)鉆機(jī)鉆臂距煤臂某一位置時(shí)，實(shí)際方位角下鉆孔開孔位置末端x坐標(biāo)減去理論傾角x坐標(biāo)值，就是誤差在x方向的偏移量Δx，實(shí)際方位角下鉆孔開孔位置末端θ'1減去輸入方位角θ1，即方位角偏移量Δθ1，因此，依據(jù)激光測(cè)距組合位姿誤差的測(cè)試數(shù)據(jù)和下式計(jì)算數(shù)據(jù)。

圖11 是測(cè)試得到的方位角θ1、平移距離d3（分別為450、300、150 和0 mm）和方位角誤差Δθ1之間的關(guān)系，4 條線就是4 種平移情況下對(duì)應(yīng)方位角變化時(shí)的誤差曲線。圖中可以發(fā)現(xiàn)平移油缸在同一推進(jìn)距離時(shí)，方位角越大，誤差量越大，隨著平移推進(jìn)，同一方位角時(shí)誤差量逐漸增大，說明機(jī)身相對(duì)于托板裝置的間隙、方位角調(diào)節(jié)裝置裝配間隙都會(huì)引起末端誤差產(chǎn)生。

圖11 方位角、平移距離和方位角誤差之間關(guān)系Fig.11 Relationship among azimuth,translation distance and azimuth error

2.3 激光測(cè)距組合位姿誤差補(bǔ)償模型

通過激光測(cè)距組合定位誤差檢測(cè)方法測(cè)得了方位角和傾角的誤差，而這兩種誤差變化量比較大，若把這些大的變化歸為桿件結(jié)構(gòu)參數(shù)和運(yùn)動(dòng)變量誤差中的微小旋轉(zhuǎn)和微小位移較為困難，因此，不適用微分矩陣方法來補(bǔ)償，應(yīng)該引入虛擬關(guān)節(jié)，類似增加了一個(gè)剛性桿，在正解矩陣中增加坐標(biāo)轉(zhuǎn)化矩陣，若在機(jī)身平移時(shí)致使傾角引起誤差，就在平移處增加補(bǔ)償矩陣，來補(bǔ)償油缸平移和傾角轉(zhuǎn)動(dòng)引起的傾角誤差，若因方位回轉(zhuǎn)關(guān)節(jié)引起的，就在方位關(guān)節(jié)處增加補(bǔ)償矩陣。

對(duì)利用激光測(cè)距組合定位誤差檢測(cè)方法測(cè)得的位姿誤差進(jìn)行補(bǔ)償，當(dāng)計(jì)入各關(guān)節(jié)誤差時(shí)，鉆臂末端的桿3 相對(duì)于基坐標(biāo)O0的坐標(biāo)變換矩陣為：

1)傾角誤差補(bǔ)償

測(cè)試中發(fā)現(xiàn)改變傾角，末端開孔位置會(huì)產(chǎn)生一個(gè)方位上的偏角Δθ2，同時(shí)還會(huì)產(chǎn)生x軸的偏移量Δz，如圖12 所示，因此，在O2之后引入虛關(guān)節(jié)，增加一個(gè)坐標(biāo)變換矩陣補(bǔ)償傾角關(guān)節(jié)引起的誤差，對(duì)傾角的位姿誤差進(jìn)行補(bǔ)償?shù)茫?/p>

圖12 傾角誤差補(bǔ)償坐標(biāo)變換Fig.12 Coordinate transformation of dip angle error compensation

2)方位誤差補(bǔ)償

上述激光測(cè)距組合定位誤差測(cè)試方法中發(fā)現(xiàn)方位角誤差主要來自關(guān)節(jié)轉(zhuǎn)動(dòng)，末端開孔位置會(huì)產(chǎn)生一個(gè)方位上的偏角Δθ1，即此在O0和O1之間引入虛關(guān)節(jié)，繞Z軸旋轉(zhuǎn)角度Δθ1的齊次變換，變換矩陣如下：

因此，將傾角和方位角的誤差匯總的誤差補(bǔ)償為：

再利用RBF 神經(jīng)網(wǎng)絡(luò)對(duì)總誤差補(bǔ)償矩陣進(jìn)行逆解求解[17-18]，得到的逆解即為誤差補(bǔ)償后的新傾角和方位角的控制輸入量。

3 激光測(cè)距組合位姿誤差補(bǔ)償算法的驗(yàn)證

為了驗(yàn)證激光測(cè)距組合位姿誤差檢測(cè)與補(bǔ)償方法的準(zhǔn)確性，通過激光測(cè)距組合法建立誤差補(bǔ)償模型，對(duì)鉆臂位姿進(jìn)行誤差補(bǔ)償，采用全站儀檢測(cè)進(jìn)行驗(yàn)證，以測(cè)試補(bǔ)償效果。

3.1 位姿誤差檢測(cè)試驗(yàn)設(shè)計(jì)

鉆孔機(jī)器人頻繁施工傾角θ2范圍是-30°～+45°、方位角θ1是-15°～+15°，油缸平移距離d3從0～450 mm 推進(jìn)，因此，試驗(yàn)中擬采集方位角-15°、0°和+15°，傾角-30°、-20°、-10°、0°、+10°、+20°、+30°、+45°，在上述姿態(tài)下，平移油缸推動(dòng)機(jī)身沿托板平移450、300、150和0 mm 下測(cè)試，傾角和方位角組合后，即方位測(cè)試12 組，傾角測(cè)試32 組。

分別用激光測(cè)距組合位姿誤差檢測(cè)方法與全站儀測(cè)試方法進(jìn)行對(duì)比。

全站儀測(cè)試原理如圖13 所示：將機(jī)身簡(jiǎn)化為一條線，在平行于機(jī)身上取3 點(diǎn)A、B、C，A點(diǎn)位于機(jī)身后端，B點(diǎn)位于機(jī)身中前部，C點(diǎn)位于機(jī)身前端，用全站儀記錄機(jī)身上A、B、C點(diǎn)坐標(biāo)，通過計(jì)算3 點(diǎn)z坐標(biāo)所成直線斜率得到傾角，通過3 點(diǎn)坐標(biāo)x坐標(biāo)所成直線斜率得到方位角。

圖13 全站儀驗(yàn)證方法原理示意和測(cè)試照片F(xiàn)ig.13 Total station verification method and test photos

A、B、C3 點(diǎn)與鉆桿中心在x和z方向都有一個(gè)偏移量，利用A、B、C3 點(diǎn)向外插值法，可計(jì)算A、B、C的延伸至孔口位置的坐標(biāo)值，經(jīng)過坐標(biāo)變換，可以間接得到全站儀法推算的鉆桿開孔位置坐標(biāo)。

3.2 誤差補(bǔ)償算法的驗(yàn)證

全站儀測(cè)試也分兩種狀態(tài)，一種是當(dāng)方位角θ1=0°時(shí)，在不同傾角θ2變化下，測(cè)試平移油缸在4 種推進(jìn)狀態(tài)下的試驗(yàn)數(shù)據(jù)，此時(shí)獲取傾角誤差；另外一種是當(dāng)傾角θ2=0°時(shí)，在不同方位角θ1變化下，測(cè)試平移油缸在4 種推進(jìn)狀態(tài)下的試驗(yàn)數(shù)據(jù)，此時(shí)獲取方位角誤差。

表2 列出了使用全站儀，在不同傾角θ2變化下，平移油缸4 中推進(jìn)狀態(tài)下，測(cè)得部分A、B、C3 點(diǎn)坐標(biāo)值。從表中可以看出經(jīng)過激光測(cè)距組合位姿誤差檢測(cè)與補(bǔ)償后，與全站儀測(cè)試結(jié)果接近。

表2 全站儀檢測(cè)傾角驗(yàn)證Table 2 Verification of inclination detected by a total station

圖14 是采用激光測(cè)距組合定位誤差檢測(cè)法與全站儀法誤差對(duì)比曲線，激光測(cè)距組合定位法傾角最大誤差差值在±0.5°以內(nèi)，比未采用誤差補(bǔ)償前的±0.85°提高了41.1%。

圖14 全站儀與激光測(cè)距法傾角檢測(cè)誤差對(duì)比Fig.14 Comparison of dip angle detection errors by total station and laser ranging method

表3 列出了使用全站儀，在不同方位角θ1變化下，平移油缸4 種推進(jìn)狀態(tài)下，測(cè)得部分A、B、C3 點(diǎn)坐標(biāo)值。從表中可以看出經(jīng)過激光測(cè)距組合位姿誤差檢測(cè)與補(bǔ)償后，與全站儀測(cè)試結(jié)果接近。

表3 全站儀檢測(cè)方位角驗(yàn)證Table 3 Verification of azimuth detected by a total station

圖15 是采用激光測(cè)距組合定位誤差檢測(cè)法與全站儀法誤差對(duì)比曲線，激光測(cè)距組合定位法方位角最大誤差差值在±0.5°以內(nèi)，比未采用誤差補(bǔ)償前的±0.8°提高了37.5%。

圖15 全站儀與激光測(cè)距法方位角檢測(cè)誤差對(duì)比Fig.15 Comparison of azimuth detection errors by total station and laser ranging method

4 結(jié)論

a.鑒于煤礦用探放水、防突和防沖鉆孔機(jī)器人精確開孔定位與孔群全自動(dòng)施工的迫切需求，本研究提出了一種激光測(cè)距組合誤差檢測(cè)方法，實(shí)現(xiàn)了鉆臂實(shí)時(shí)誤差監(jiān)測(cè)與誤差自動(dòng)補(bǔ)償。

b.該方法不借助全站儀，通過測(cè)距實(shí)時(shí)計(jì)算實(shí)際和理論傾角、方位角差值，作為誤差補(bǔ)償后的新傾角和方位角的控制輸入量，簡(jiǎn)化了測(cè)試方法，提高了測(cè)試精度，使鉆臂精度補(bǔ)償實(shí)現(xiàn)了工程應(yīng)用。

c.通過傳統(tǒng)全站儀測(cè)量方法對(duì)激光測(cè)距組合位姿誤差檢測(cè)方法進(jìn)行了驗(yàn)證，試驗(yàn)表明：采用激光測(cè)距組合定位誤差檢測(cè)法，傾角最大誤差差值在±0.5°以內(nèi)，方位角最大誤差在±0.5°以內(nèi)，比未采用誤差補(bǔ)償前分別提高了41.1%和37.5%。

d.激光測(cè)距組合位姿誤差檢測(cè)與補(bǔ)償法仍需豐富現(xiàn)場(chǎng)試驗(yàn)數(shù)據(jù)，不斷完善補(bǔ)償方法。同時(shí)，后續(xù)還可針對(duì)不同的誤差影響因素，搭建相應(yīng)的補(bǔ)償模型，更加精細(xì)化鉆臂開孔定位誤差補(bǔ)償算法。

符號(hào)注釋：

A1為激光測(cè)距傳感器安裝點(diǎn)；A2為激光測(cè)距傳感器安裝點(diǎn)；C1為cosθ1；C2為cosθ2；d1為機(jī)身舉升距離；d2為鉆臂方位回轉(zhuǎn)中心到機(jī)身平移之間垂直距離；d3為機(jī)身平移距離；f為機(jī)身撓性變形后的撓度；h為x方向與方位角水平線的垂直距離；H0為鉆臂舉升初始距離；ΔH1為舉升高度；Hz為傾角改變和平移油缸平移后，激光測(cè)距傳感器投射點(diǎn)距離車體地面的高度；l為平移油缸推進(jìn)長(zhǎng)度；Δl為因方位角變化h增加段的距離；Lc1為鉆臂在經(jīng)過高精度開孔定位儀尋找的初始水平點(diǎn)激光測(cè)距測(cè)定的距離；Lc2為鉆臂在經(jīng)過高精度開孔定位儀尋找的初始水平點(diǎn)激光測(cè)距測(cè)定的距離；Ld1為激光測(cè)距傳感器安置點(diǎn)在Y方向與轉(zhuǎn)動(dòng)中心的距離；Ld2為激光測(cè)距傳感器安裝點(diǎn)在x方向與轉(zhuǎn)動(dòng)中心的距離；Ly為鉆臂平移油缸平移距離；Lz1為傾角變化后測(cè)距傳感器測(cè)定距離；Lz2為方位角變化后測(cè)距傳感器測(cè)定距離；O0為鉆臂方位回轉(zhuǎn)中心；O1為鉆臂舉升后最末端；O2為鉆臂傾角回轉(zhuǎn)中心；O3為機(jī)身平移后最末端；px、py、pz分別為鉆臂執(zhí)行機(jī)構(gòu)末端與鉆臂基坐標(biāo)系的關(guān)系矩陣中x，y，z方向的位置值；P為末端手爪位姿設(shè)定為輸入樣本矩陣；S1為sinθ1；S2為sinθ2，T為各關(guān)節(jié)變量設(shè)定為樣本輸出矩陣；TΔ為動(dòng)態(tài)誤差補(bǔ)償矩陣；θ為機(jī)身撓性變形后的轉(zhuǎn)角；θ1為方位回轉(zhuǎn)角度；θ1j為激光測(cè)距組合位姿誤差檢測(cè)結(jié)果；θ1l為理論輸入值；θ1q為全站儀測(cè)試后計(jì)算結(jié)果；為實(shí)際計(jì)算的方位角；θ2為傾角回轉(zhuǎn)角度；θ2j為激光測(cè)距組合位姿誤差檢測(cè)結(jié)果；θ2l為理論輸入值；θ2q為全站儀測(cè)試后計(jì)算結(jié)果；為實(shí)際計(jì)算的傾角；αi、ai、θ、di為機(jī)器人中各關(guān)節(jié)結(jié)構(gòu)參數(shù)；(α，β，γ)為歐拉變換RPY。