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

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

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

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

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

1 經(jīng)典鉆臂運動學(xué)誤差補償方法

1.1 鉆臂運動學(xué)分析與正逆解求解

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

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

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

分析對鉆臂結(jié)構(gòu)和動作可知，鉆臂有傾角回轉(zhuǎn)θ2、方位回轉(zhuǎn)θ1、機身舉升d1和機身平移d34 個自由度。利用D-H 方法建立鉆進(jìn)機器人鉆臂運動學(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 鉆臂正運動學(xué)求解

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

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

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

1.1.3 鉆臂逆運動學(xué)求解

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

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

1.2 鉆臂定位誤差原因分析

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2.1 激光測距組合位姿誤差檢測原理

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

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

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

2.2 激光測距組合位姿誤差檢測

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

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

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

圖8 實際傾角誤差量計算原理Fig.8 Calculation principle of inclinaiton error

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

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

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

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

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

存在關(guān)系式：

因此，有：

通過下式可以計算得到實際方位角θ'1。

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

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

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

2.3 激光測距組合位姿誤差補償模型

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

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

1)傾角誤差補償

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

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

2)方位誤差補償

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

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

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

3 激光測距組合位姿誤差補償算法的驗證

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

3.1 位姿誤差檢測試驗設(shè)計

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

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

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

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

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

3.2 誤差補償算法的驗證

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

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

表2 全站儀檢測傾角驗證Table 2 Verification of inclination detected by a total station

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

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

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

表3 全站儀檢測方位角驗證Table 3 Verification of azimuth detected by a total station

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

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

4 結(jié)論

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

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

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

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

符號注釋：

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