掘進(jìn)機(jī)精準(zhǔn)定位方法與掘進(jìn)機(jī)器人系統(tǒng)

盧新明，閆長青，袁照平

（1.山東科技大學(xué)山東省智慧礦山信息技術(shù)重點實驗室，山東 青島 266590；2.山東藍(lán)光軟件有限公司山東省智慧礦山軟件工程技術(shù)研究中心，山東 泰安 271000；3.山東科技大學(xué)信息工程系，山東 泰安 271019；4.山東科技大學(xué)機(jī)械與電子工程系，山東 泰安 271019）

1 引言

懸臂式掘進(jìn)機(jī)是目前煤礦開采和井下巷道掘進(jìn)的主要設(shè)備，掘進(jìn)機(jī)的遠(yuǎn)程化、智能化、機(jī)器人化直接關(guān)系到煤礦的安全開采和礦工的生命安全。

研究人員對掘進(jìn)機(jī)自動化和智能化進(jìn)行了大量研究[1-2]。目前，掘進(jìn)機(jī)已經(jīng)能夠?qū)崿F(xiàn)遠(yuǎn)程控制[3-5]，但由于未能實現(xiàn)地下空間的自主測量和定位，無法建立掘進(jìn)機(jī)自身位姿和巷道之間的位置關(guān)系，無法自動感知和定位周圍環(huán)境，因此只能依靠視頻進(jìn)行作業(yè)，無法實現(xiàn)精確開采。掘進(jìn)機(jī)機(jī)器人化的核心問題——定位和導(dǎo)航已有了大量的研究[6-13]，主要發(fā)展了基于超寬帶技術(shù)的定位方法[10]、基于激光交匯的定位方法[2,12-13]和基于慣性導(dǎo)航技術(shù)的定位方法[14-15]。但超寬帶方法本身存在精度問題，很難精確控制切割精度。基于激光交匯技術(shù)的定位安裝過于復(fù)雜，需要測量多個點進(jìn)行定位，難以在復(fù)雜的井下環(huán)境中配置應(yīng)用。基于慣性導(dǎo)航的方法需要計算姿態(tài)角和距離，存在累積誤差問題。上述方法目前普遍處于理論研究和系統(tǒng)仿真階段，尚未發(fā)現(xiàn)智能掘進(jìn)機(jī)器人在井下的實際應(yīng)用。另外，不少定位仿真算法對姿態(tài)角的使用存在誤區(qū)，混淆了慣性實測結(jié)果和預(yù)定旋轉(zhuǎn)角的因果關(guān)系。慣性導(dǎo)航儀輸出的是實測姿態(tài)角，而使用實測姿態(tài)角對原坐標(biāo)系旋轉(zhuǎn)變換無法得到具有同樣姿態(tài)角的坐標(biāo)系統(tǒng)，如對3 個軸均旋轉(zhuǎn)45°，得到的坐標(biāo)系的姿態(tài)角并非45°，從而使這些算法結(jié)果的可信度存疑。受定位方法的制約，目前沒有可以應(yīng)用于生產(chǎn)的可精確定位掘進(jìn)機(jī)器人系統(tǒng)，極大地制約了掘進(jìn)機(jī)器人的應(yīng)用和煤礦安全精確開采問題的解決[3]。

制約掘進(jìn)機(jī)機(jī)器人化的另一個關(guān)鍵問題是對周圍環(huán)境進(jìn)行感知需要傳感器設(shè)備聯(lián)網(wǎng)。現(xiàn)代物聯(lián)網(wǎng)技術(shù)的核心可以實現(xiàn)設(shè)備入網(wǎng)和互聯(lián)，實時傳輸狀態(tài)數(shù)據(jù)至處理設(shè)備，目前該技術(shù)已成為實現(xiàn)感知礦山、智慧礦山的關(guān)鍵支撐技術(shù)[16-18]。借助物聯(lián)網(wǎng)技術(shù)可以使掘進(jìn)機(jī)實時感知礦山的狀態(tài)，實現(xiàn)掘進(jìn)機(jī)機(jī)器人化[19]。

針對上述問題，本文結(jié)合慣性導(dǎo)航儀、指北儀和具有跟蹤功能的全站儀等設(shè)備，構(gòu)建物聯(lián)網(wǎng)，研發(fā)了新的高精度定位模型和解算方法，并據(jù)此研發(fā)了掘進(jìn)機(jī)器人系統(tǒng)。通過慣性導(dǎo)航儀、指北儀和全站儀的結(jié)合，解決了慣性導(dǎo)航的累積誤差問題；建立了只需測量一點即可實現(xiàn)精準(zhǔn)定位的一點定位模型，解決了現(xiàn)有方法井下配置復(fù)雜的問題；在解算時拋棄了旋轉(zhuǎn)姿態(tài)角定位的方式，確保了算法的可靠性；通過物聯(lián)網(wǎng)實現(xiàn)了對掘進(jìn)機(jī)周圍環(huán)境的精確感知，研發(fā)了掘進(jìn)機(jī)器人系統(tǒng)，實現(xiàn)了掘進(jìn)機(jī)機(jī)器人化。

2 掘進(jìn)機(jī)器人物聯(lián)網(wǎng)構(gòu)建

由于掘進(jìn)機(jī)在礦井下工作，要實現(xiàn)掘進(jìn)機(jī)機(jī)器人化需要實時采集掘進(jìn)機(jī)自身的狀態(tài)信息和周圍的環(huán)境信息，以確定自身的位姿及其與巷道的關(guān)系，進(jìn)而感知周圍環(huán)境，因此本文在互聯(lián)網(wǎng)的基礎(chǔ)上引入了物聯(lián)網(wǎng)，以便相關(guān)狀態(tài)采集設(shè)備可以連入網(wǎng)絡(luò)。

本文所提掘進(jìn)機(jī)器人系統(tǒng)的主要設(shè)備包括遠(yuǎn)程遙控設(shè)備、全站儀、指北儀、慣性導(dǎo)航儀、遠(yuǎn)程工控機(jī)、現(xiàn)場工控機(jī)、可編程邏輯控制器（PLC,programmable logic controller），以及配置了多個傳感器的掘進(jìn)機(jī)。這些設(shè)備分別接入遠(yuǎn)程工控機(jī)和現(xiàn)場工控機(jī)，并由遠(yuǎn)程工控機(jī)通過互聯(lián)網(wǎng)和現(xiàn)場工控機(jī)交換信息。

遠(yuǎn)程遙控設(shè)備即遙控器，提供掘進(jìn)機(jī)運行所需要的指令，通過串口與遠(yuǎn)程工控機(jī)相連。2 個工控機(jī)通過TCP/IP 連接，共處于一個局域網(wǎng)內(nèi)。全站儀和指北儀通過網(wǎng)絡(luò)與現(xiàn)場工控機(jī)連接，全站儀源源不斷地提供測量數(shù)據(jù)以計算坐標(biāo)；指北儀內(nèi)置陀螺儀，以一定的頻率連續(xù)測量全站儀的姿態(tài)角并發(fā)送數(shù)據(jù)。掘進(jìn)機(jī)油缸安裝油缸行程傳感器，測定油缸行程，掘進(jìn)機(jī)共配置8 條油缸，其中4 條負(fù)責(zé)炮頭的運動，另外4 條負(fù)責(zé)掘進(jìn)機(jī)位姿的調(diào)整。根據(jù)炮頭運動相關(guān)的油缸行程可以計算出炮頭坐標(biāo)信息。油缸行程傳感器和其他相應(yīng)設(shè)備與PLC 連接，慣性導(dǎo)航儀通過網(wǎng)絡(luò)實時傳送掘進(jìn)機(jī)的位姿數(shù)據(jù)至工控機(jī)，PLC 通過Modbus TCP 協(xié)議與工控機(jī)連接。整個系統(tǒng)形成了一個物聯(lián)網(wǎng)絡(luò)，整體架構(gòu)如圖1 所示。

圖1 網(wǎng)絡(luò)架構(gòu)

物聯(lián)網(wǎng)是互聯(lián)網(wǎng)的延伸，從互聯(lián)網(wǎng)的角度看，整個網(wǎng)絡(luò)架構(gòu)又可看作客戶機(jī)/服務(wù)器模式。以遠(yuǎn)程工控機(jī)和地面工控機(jī)為中心的網(wǎng)絡(luò)各自組成了客戶機(jī)，以現(xiàn)場工控機(jī)為中心的網(wǎng)絡(luò)和以PLC 為中心的網(wǎng)絡(luò)共同組成了服務(wù)器系統(tǒng)，其中客戶機(jī)系統(tǒng)可以根據(jù)不同的角色和職能配置多個。本文系統(tǒng)采用了三層架構(gòu)，其中地面客戶機(jī)系統(tǒng)用于地面監(jiān)控。

3 掘進(jìn)機(jī)位姿確定方法

掘進(jìn)機(jī)實現(xiàn)精準(zhǔn)掘進(jìn)的關(guān)鍵是自身位姿的精準(zhǔn)確定。為此，本文研發(fā)了一種基于慣性導(dǎo)航儀、指北儀和跟蹤式全站儀的精確定位方法[20]，設(shè)計了一種精確定位算法。定位原理如圖2 所示，在掘進(jìn)機(jī)機(jī)身后部安裝一個靶球，由全站儀測量結(jié)果計算靶球相對坐標(biāo)，即靶球在全站儀坐標(biāo)系下的坐標(biāo)（簡稱全站儀坐標(biāo)系），由指北儀測量全站儀位姿，進(jìn)而得出全站儀所在的坐標(biāo)系，從而定位靶球；由靶球位置和慣性導(dǎo)航儀可得掘進(jìn)機(jī)自身坐標(biāo)系，則實現(xiàn)掘進(jìn)機(jī)任意一點定位。由于使用慣性導(dǎo)航儀定位會因二次積分而產(chǎn)生累積誤差，本文方法僅使用慣性導(dǎo)航儀輸出高精度姿態(tài)角，避免了二次積分的累積誤差問題。本文方法僅使用一個靶球和一個激光發(fā)射源即全站儀，即可解決傳統(tǒng)方法安裝部署過于復(fù)雜的問題。

圖2 定位原理

3.1 定位設(shè)備的安裝

指北儀和全站儀的安裝要求共底平面，且使指北儀的測量方向與全站儀的初始設(shè)定方向一致，二者固定在一個防爆箱內(nèi)。

慣性導(dǎo)航儀安裝在掘進(jìn)機(jī)機(jī)身上，且保持慣性導(dǎo)航儀的底面與掘進(jìn)機(jī)機(jī)身平面共面，慣性導(dǎo)航儀的測量方向與機(jī)身前進(jìn)方向一致。

定位所需的激光靶球安裝在掘進(jìn)機(jī)機(jī)身后部某個固定位置處，手動測量并記錄激光靶球到掘進(jìn)機(jī)坐標(biāo)系原點的坐標(biāo)增量(Δxr,Δyr,Δzr)。由全站儀尋找并鎖定激光靶球進(jìn)行定位。

由慣性導(dǎo)航儀可得掘進(jìn)機(jī)的實時姿態(tài)角，包括方位角yaw、俯仰角pitch 和滾動角roll。方位角yaw為掘進(jìn)機(jī)前進(jìn)方向在水平面的投影與北向的夾角，俯仰角pitch 為掘進(jìn)機(jī)前進(jìn)方向Y軸與水平面的夾角，而滾動角roll 為機(jī)身X軸與水平面的夾角。

指北儀測定全站儀的姿態(tài)角，將姿態(tài)角融入算法計算，從而避免了全站儀的調(diào)平處理，減小了人為誤差的引入。

3.2 坐標(biāo)系定義

絕對坐標(biāo)的確定需設(shè)定4 個坐標(biāo)系，包括大地坐標(biāo)系OeXeYeZe、掘進(jìn)機(jī)坐標(biāo)系OrXrYrZr、巷道坐標(biāo)系OlXlYlZl和全站儀坐標(biāo)系OoXoYoZo。

所有坐標(biāo)系均采用右手法則。大地坐標(biāo)系采用東北高模式，即東為X軸正向，北為Y軸正向，向上方向為Z軸正向。

巷道坐標(biāo)系采用從巷道起點沿中線掘進(jìn)的方向為Y軸正向，向右方向為X軸正向，垂直于巷道底板向上方向為Z軸正向。

掘進(jìn)機(jī)坐標(biāo)系采用沿掘進(jìn)機(jī)中線前進(jìn)方向為Y軸正向，沿機(jī)身平面垂直于機(jī)身中線向右方向為X軸正向，垂直于機(jī)身平面向上方向為Z軸正向。

全站儀坐標(biāo)系采用其本身固有的左手法則坐標(biāo)系，即西為X軸正向，北為Y軸正向，向上方向為Z軸正向。

3.3 定位算法

經(jīng)查閱大量文獻(xiàn)發(fā)現(xiàn)，以往利用慣性導(dǎo)航的研究大都混淆了慣性導(dǎo)航儀輸出的實時狀態(tài)的姿態(tài)角和表示旋轉(zhuǎn)過程的歐拉角，而給定一組相同的角度，按一定的順序旋轉(zhuǎn)無法得到相應(yīng)的實時狀態(tài)。導(dǎo)致這種問題的可能原因有兩方面，一方面，因為旋轉(zhuǎn)本身的復(fù)雜性，3 個角度的旋轉(zhuǎn)順序和坐標(biāo)系定義的不同均會導(dǎo)致旋轉(zhuǎn)結(jié)果的不同；另一方面，由于慣性導(dǎo)航近些年才逐漸走向民用且價格高昂，模擬實驗并未用到真正的慣性導(dǎo)航，導(dǎo)致不能發(fā)現(xiàn)其中的問題。目前的研究算法都基于給定所謂姿態(tài)角的旋轉(zhuǎn)，尚未發(fā)現(xiàn)可以正確利用慣導(dǎo)實時狀態(tài)的姿態(tài)角進(jìn)行正確定位的算法。因而本文提出了利用慣導(dǎo)的實時狀態(tài)姿態(tài)角直接求算慣導(dǎo)坐標(biāo)系的3 個向量，進(jìn)而確定慣導(dǎo)坐標(biāo)系進(jìn)行定位的算法。

由圖2 設(shè)計定位算法，原理如下。

由慣性導(dǎo)航儀測得姿態(tài)角α β、和γ，根據(jù)姿態(tài)角計算出掘進(jìn)機(jī)的坐標(biāo)系向量，從而解算出掘進(jìn)機(jī)的坐標(biāo)系OrXrYrZr向量。

由全站儀實時測量掘進(jìn)機(jī)身安裝的定位靶球，輸出距離L、方位角αf和傾角αq，假設(shè)全站儀的基點為(xo,yo,zo)。可以根據(jù)式(1)計算全站儀坐標(biāo)(xob,yob,zob)為

由指北儀測定的全站儀姿態(tài)角，解算出全站儀所在的坐標(biāo)系OoXoYoZo向量。

由全站儀坐標(biāo)系向量、全站儀基點和全站儀坐標(biāo)(xob,yob,zob)，解算出靶球在大地坐標(biāo)系OeXeYeZe的絕對坐標(biāo)(xeb,yeb,zeb)。

根據(jù)靶球所在掘進(jìn)機(jī)坐標(biāo)系OrXrYrZr，向量(rx,ry,rz)和靶球在掘進(jìn)機(jī)坐標(biāo)系中的坐標(biāo)增量(Δxr,Δyr,Δzr)解算出掘進(jìn)機(jī)坐標(biāo)系原點的絕對坐標(biāo)(xr,yr,zr)，如式(2)所示。

算法1 具體描述如下。

算法1定位算法

1)慣性導(dǎo)航儀輸出實時姿態(tài)角：方位角yaw、俯仰角 pitch 和橫滾角roll。

2)假設(shè)掘進(jìn)機(jī)Y軸向量在XOY平面上的投影長度為單位長度1。

3)由yaw 和 pitch 求出Y軸向量。

4)標(biāo)準(zhǔn)化Y軸向量。

5)由于坐標(biāo)系正交，則X軸和Y軸垂直，內(nèi)積為0。

6)已知滾動角roll，并假設(shè)長度為單位向量，求得X軸單位向量。

7)由坐標(biāo)軸的正交性，根據(jù)X、Y軸向量差積，求得Z軸向量。

8)由已知相對坐標(biāo)，根據(jù)所求坐標(biāo)系，求得該坐標(biāo)點在坐標(biāo)系下的投影坐標(biāo)增量。

9)考慮原點計算定位坐標(biāo)。

本文定位算法首先根據(jù)3 個實時姿態(tài)角分別求出掘進(jìn)機(jī)坐標(biāo)系的3 個坐標(biāo)軸X、Y、Z軸向量；然后計算掘進(jìn)機(jī)相對坐標(biāo)點在該坐標(biāo)系中的坐標(biāo)增量；最后考慮原點計算定位坐標(biāo)進(jìn)行定位。由于根據(jù)實時姿態(tài)角直接求算掘進(jìn)機(jī)坐標(biāo)系，因此避免了利用旋轉(zhuǎn)矩陣帶來的不確定性，保證了計算結(jié)果的正確性。

4 掘進(jìn)機(jī)器人系統(tǒng)

本節(jié)基于前述物聯(lián)網(wǎng)和定位算法，設(shè)計并實現(xiàn)掘進(jìn)機(jī)器人系統(tǒng)。

4.1 系統(tǒng)組成

4.1.1 按照空間部署距離劃分

本文系統(tǒng)按照空間部署距離可以劃分為三層，第一層為地面監(jiān)控子系統(tǒng)，第二層為井下遠(yuǎn)程監(jiān)控子系統(tǒng)，第三層為現(xiàn)場作業(yè)子系統(tǒng)。

1)地面監(jiān)控子系統(tǒng)部署在地面上，供地面觀察和地面管理人員使用。地面監(jiān)控子系統(tǒng)主要提供井下作業(yè)的監(jiān)視支持，一般不用于實際作業(yè)控制，經(jīng)授權(quán)后可以進(jìn)行作業(yè)控制。

2)井下遠(yuǎn)程監(jiān)控子系統(tǒng)部署在井下遠(yuǎn)離工作面的地方，供操作人員操作使用。井下遠(yuǎn)程監(jiān)控子系統(tǒng)具體負(fù)責(zé)掘進(jìn)作業(yè)的監(jiān)控。

3)現(xiàn)場作業(yè)子系統(tǒng)部署在掘進(jìn)機(jī)機(jī)身和掘進(jìn)機(jī)附近，不需要人工操作，負(fù)責(zé)現(xiàn)場作業(yè)。

這種系統(tǒng)架構(gòu)使操作人員遠(yuǎn)離采掘工作面，地面管理人員可以隨時檢查井下作業(yè)狀況并根據(jù)需要控制井下作業(yè)。

4.1.2 按照任務(wù)職能劃分

井下遠(yuǎn)程監(jiān)控子系統(tǒng)和現(xiàn)場作業(yè)子系統(tǒng)合作完成掘進(jìn)作業(yè)，按照任務(wù)職能又可以分為3 個子系統(tǒng)：監(jiān)控子系統(tǒng)、定位子系統(tǒng)和執(zhí)行子系統(tǒng)。

1)監(jiān)控子系統(tǒng)的功能是實時監(jiān)視并控制掘進(jìn)機(jī)進(jìn)行遠(yuǎn)程作業(yè)，由一臺工控機(jī)連接遙控設(shè)備，并安裝相應(yīng)的監(jiān)控軟件構(gòu)建。通過遙控器發(fā)送指令，經(jīng)由網(wǎng)絡(luò)傳輸至執(zhí)行子系統(tǒng)，驅(qū)動掘進(jìn)機(jī)進(jìn)行作業(yè)，作業(yè)執(zhí)行情況傳回監(jiān)控子系統(tǒng)實時圖形化顯示。監(jiān)控軟件實時高精度地顯示作業(yè)的進(jìn)展情況，保證作業(yè)安全可靠、高精度運行。

2)定位子系統(tǒng)包括指北儀、具跟蹤功能的全站儀、安裝于掘進(jìn)機(jī)某固定部位的定位靶球和安裝于機(jī)體的慣性導(dǎo)航儀。指北儀和全站儀固定在定位箱內(nèi)，指北儀確定全站儀的位姿并輸出3 個角度，即方位角、俯仰角和滾動角。指北儀安裝方向與北向的夾角即方位角，指北儀安裝方向與水平面的夾角即俯仰角，指北儀安裝方向為Y軸，用右手法則確定的坐標(biāo)系的X軸與大地水平面的夾角為滾動角。全站儀跟蹤定位靶球并返回距離和2 個角度，具有自動跟蹤功能。全站儀和指北儀共同測量并解算靶球的大地坐標(biāo)。慣性導(dǎo)航儀安裝在掘進(jìn)機(jī)機(jī)體上，實時測量掘進(jìn)機(jī)的位姿，輸出3 個與指北儀輸出角類似的角度，但角度的表示范圍有所不同，這3 個姿態(tài)角表示掘進(jìn)機(jī)的位姿。

3)執(zhí)行子系統(tǒng)包括一臺現(xiàn)場工控機(jī)、PLC 和配置了8 條油缸傳感器的掘進(jìn)機(jī)。

現(xiàn)場工控機(jī)通過網(wǎng)絡(luò)接收監(jiān)控系統(tǒng)傳來的指令，通過Modbus TCP 將指令傳遞給PLC，PLC 執(zhí)行指令驅(qū)動掘進(jìn)機(jī)各部件進(jìn)行動作完成掘進(jìn)作業(yè)。同時，現(xiàn)場工控機(jī)查詢PLC 的狀態(tài)信息并根據(jù)需要執(zhí)行相應(yīng)計算，然后把遠(yuǎn)程工控機(jī)所需數(shù)據(jù)分別返回給井下監(jiān)控子系統(tǒng)和地面監(jiān)控子系統(tǒng)。

掘進(jìn)機(jī)機(jī)體需要確定所有的機(jī)體參數(shù)，配置8 條油缸的行程傳感器。其中，2 條升降油缸和2條回轉(zhuǎn)油缸負(fù)責(zé)掘進(jìn)機(jī)炮頭的擺動，2 條前鏟板油缸和2 條后支撐油缸負(fù)責(zé)掘進(jìn)機(jī)姿態(tài)的調(diào)整。前4 條油缸行程用于計算炮頭相對于掘進(jìn)機(jī)重心的位置，后4 條油缸行程作為掘進(jìn)機(jī)姿態(tài)調(diào)整的依據(jù)。

控制系統(tǒng)選用PLC 的西門子1500。PLC 連接所有的掘進(jìn)機(jī)傳感器以及掘進(jìn)機(jī)的其他部件，能夠按指令驅(qū)動相關(guān)的部件運動并提供各種參數(shù)和狀態(tài)信息。

4.2 工作流程

掘進(jìn)機(jī)器人工作流程如圖3 所示。其中，向右的虛線箭頭表示掘進(jìn)機(jī)接收從監(jiān)控系統(tǒng)發(fā)來的指令信息流，向左的虛線箭頭表示回傳掘進(jìn)機(jī)作業(yè)的所有相關(guān)狀態(tài)信息流，并計算空間坐標(biāo)回傳。

圖3 掘進(jìn)機(jī)器人工作流程

具體工作原理如下。遙控器以一定頻率發(fā)出指令，遠(yuǎn)程工控機(jī)接受遙控器指令并校驗其完整性，將校驗后的指令發(fā)送至現(xiàn)場工控機(jī)。現(xiàn)場工控機(jī)接受指令后，按照和PLC 的協(xié)議將該指令送入PLC 指定地址，PLC 根據(jù)該指令驅(qū)動掘進(jìn)機(jī)進(jìn)行作業(yè)。PLC發(fā)送指令驅(qū)動掘進(jìn)機(jī)后，會實時讀取掘進(jìn)機(jī)狀態(tài)信息并寫至相應(yīng)寄存器。現(xiàn)場工控機(jī)完成指令發(fā)送后，實時讀取PLC 狀態(tài)寄存器，根據(jù)掘進(jìn)機(jī)油缸的狀態(tài)信息計算炮頭的相對坐標(biāo)，根據(jù)慣導(dǎo)數(shù)據(jù)實時計算掘進(jìn)機(jī)坐標(biāo)系，根據(jù)掘進(jìn)機(jī)坐標(biāo)系把炮頭的原坐標(biāo)轉(zhuǎn)換為大地坐標(biāo)，并發(fā)送至遠(yuǎn)程工控機(jī)，由遠(yuǎn)程工控機(jī)完成坐標(biāo)投影變換和實時顯示。遠(yuǎn)程工控機(jī)遙控器操作用戶根據(jù)軟件系統(tǒng)的實時畫面進(jìn)行下一步操作，如此反復(fù)，最終完成掘進(jìn)任務(wù)。工作流程可分為2 個主要部分，即炮頭定位和實時精確顯示。

4.2.1 炮頭定位

炮頭定位指炮頭坐標(biāo)信息的確定，是工作流程的核心。掘進(jìn)機(jī)自身的位姿由第3 節(jié)所述掘進(jìn)機(jī)位姿確定方法確定，具體為掘進(jìn)機(jī)自身坐標(biāo)系的求算和掘進(jìn)機(jī)原點坐標(biāo)的空間位置解算。炮頭在掘進(jìn)機(jī)坐標(biāo)系下相對于掘進(jìn)機(jī)坐標(biāo)原點的坐標(biāo)增量為(Δx, Δy,Δz)，可以根據(jù)掘進(jìn)機(jī)運動學(xué)原理和掘進(jìn)機(jī)控制炮頭上下擺動和左右擺動的油缸行程確定[8]。根據(jù)3.3 節(jié)定位算法，將掘進(jìn)機(jī)空間坐標(biāo)系向量(rx,ry,rz)、掘進(jìn)機(jī)原點坐標(biāo)(xr,yr,zr)和炮頭坐標(biāo)增量(Δx,Δy,Δz)代入式(2)，可以解算炮頭的絕對空間坐標(biāo)點p，完成炮頭定位。

4.2.2 實時精確顯示

掘進(jìn)機(jī)作業(yè)的狀態(tài)需要實時準(zhǔn)確地顯示在監(jiān)控屏幕上，以便根據(jù)實時工作狀態(tài)進(jìn)行高效精準(zhǔn)的掘進(jìn)作業(yè)。掘進(jìn)機(jī)炮頭在巷道的位置、掘進(jìn)機(jī)相對于巷道的位置和姿態(tài)是需要精確顯示的核心內(nèi)容。為了準(zhǔn)確直觀地呈現(xiàn)上述信息，需要進(jìn)行坐標(biāo)投影變換。具體變換為，將掘進(jìn)機(jī)炮頭的大地坐標(biāo)投影到巷道斷面，并轉(zhuǎn)換為巷道斷面坐標(biāo)系坐標(biāo)，然后直觀呈現(xiàn)巷道、炮頭以及它們之間的相對位置關(guān)系。

具體步驟如下。

1)根據(jù)3.3 節(jié)定位算法，求得掘進(jìn)機(jī)坐標(biāo)系OrXrYrZr向量(rx,ry,rz)和原點坐標(biāo)(xr,yr,zr)。

2)由于掘進(jìn)機(jī)本身為一剛體，除去懸臂和炮頭，機(jī)身可簡化為一個立方體，因此可以根據(jù)定位算法和測量所得坐標(biāo)增量求得掘進(jìn)機(jī)機(jī)身的特征點，包括掘進(jìn)機(jī)機(jī)身上平面的4 個點a、b、c、d和下平面的4個點a1、b1、c1、d1，如圖4 所示。圖4 中，f1f2f3f4為斷面。由于3.2.1 節(jié)已經(jīng)確定了炮頭頂點p，則可以將掘進(jìn)機(jī)炮頭投影至當(dāng)前截割面，顯示實時位置關(guān)系。

圖4 掘進(jìn)機(jī)與斷面示意

3)掘進(jìn)機(jī)的位姿以三視圖的形式呈現(xiàn)，在三視圖中的坐標(biāo)求解方法如下。對于側(cè)視圖，把掘進(jìn)機(jī)的側(cè)平面頂點b、c、c1、b1和炮頭頂點p投影至巷道的左幫斷面，則可以顯示從側(cè)面觀察的實時精確位置關(guān)系。后視圖把掘進(jìn)機(jī)的上平面頂點a、b、c、d投影至巷道截割面，而俯視圖則把前平面abb1a1投影至巷道底板。由此可以得到掘進(jìn)機(jī)和巷道的掘進(jìn)機(jī)的精確姿態(tài)和掘進(jìn)機(jī)相對于巷道的精確位置關(guān)系。

根據(jù)以上步驟，掘進(jìn)機(jī)可以實時感知自身狀態(tài)和炮頭的精確位置，并和周圍環(huán)境進(jìn)行精確交互，從而實現(xiàn)精準(zhǔn)掘進(jìn)。

5 定位算法驗證與掘進(jìn)機(jī)器人應(yīng)用

本文掘進(jìn)機(jī)器人系統(tǒng)已經(jīng)應(yīng)用于實際煤礦開采和掘進(jìn)作業(yè)，巷道寬4.2 m，高4 m，安裝了全站儀的定位系統(tǒng)與掘進(jìn)機(jī)上的定位靶球間距離約為15～40 m。

系統(tǒng)的運行界面如圖5 所示。其中，左側(cè)為模式選擇和狀態(tài)顯示部分，中間主圖顯示了截割斷面、炮頭在斷面中的位置及二者的相對位置關(guān)系，右側(cè)表示掘進(jìn)機(jī)和巷道的精確相對位置關(guān)系和精確位姿。

圖5 右側(cè)從上到下依次為掘進(jìn)機(jī)實時姿態(tài)的側(cè)視圖、俯視圖和后視圖。側(cè)視圖顯示了掘進(jìn)機(jī)相對于巷道位置關(guān)系、炮頭在斷面的高度以及炮頭距切割面的距離關(guān)系。俯視圖顯示了炮頭在水平方向上和巷道的相對位置關(guān)系、炮頭和巷道前方待切割斷面的垂直距離以及炮頭和當(dāng)次截割的煤壁后端面的位置關(guān)系。在狹窄巷道作業(yè)時，還需要注意一種情況，即炮頭看起來可以到達(dá)，但是實際上卻因為懸臂被煤壁卡住無法到達(dá)，為了提醒掘進(jìn)機(jī)司機(jī)，側(cè)視圖中標(biāo)出了懸臂最寬處距離巷道兩幫的距離（斜線的兩端點為懸臂最寬處距離巷道兩幫的距離）。后視圖顯示了從后方觀察到的掘進(jìn)機(jī)機(jī)體和巷道的位置關(guān)系。此外，圖5 中的小圖標(biāo)分別表示掘進(jìn)機(jī)的各種狀態(tài)，如鏟板升降、前后支撐和左右星輪運動等。

圖5 運行界面

5.1 算法比較

為了對比本文所提定位算法與基于旋轉(zhuǎn)姿態(tài)角的定位算法，分別測試了3 組數(shù)據(jù)樣例。旋轉(zhuǎn)矩陣方法分別對3 個角度，即方位角yaw、俯仰角pitch和滾動角roll，以一定的順序旋轉(zhuǎn)，所有可能的旋轉(zhuǎn)順序有6 種，所有可能的旋轉(zhuǎn)順序和相應(yīng)的結(jié)果如表1 所示。

掘進(jìn)機(jī)坐標(biāo)系和大地坐標(biāo)系均采用右手法則，向上為Z軸正向，向右為X軸正向。

根據(jù)表1 可知，只有當(dāng)姿態(tài)角均為45°，且坐標(biāo)在X軸或Y軸上時，才有一種旋轉(zhuǎn)順序可以得到和本文定位算法一致的正確結(jié)果，但用于X軸和Y軸時對3 個姿態(tài)角的旋轉(zhuǎn)順序不確定。而其他坐標(biāo)的定位結(jié)果顯示，無論采用什么角度，進(jìn)行表1 中任一種順序的姿態(tài)角旋轉(zhuǎn)變化，得到的坐標(biāo)和由實時姿態(tài)角求得的定位結(jié)果不一致。由此可見，用于旋轉(zhuǎn)的姿態(tài)角和慣導(dǎo)輸出的實時狀態(tài)的姿態(tài)角沒有關(guān)系，使用旋轉(zhuǎn)慣導(dǎo)實時姿態(tài)角的方式進(jìn)行定位無法得到正確的定位結(jié)果。

5.2 精度分析

為了測試本機(jī)器人的系統(tǒng)誤差，用全站儀實測了大約100 個點，從中選擇誤差較大的能夠體現(xiàn)定位精度的邊界點和拐點測量結(jié)果，并與機(jī)器人系統(tǒng)得到的定位結(jié)果進(jìn)行對比，如表2 所示。

表1 旋轉(zhuǎn)矩陣方法與本文定位方法的結(jié)果對比

表2 定位坐標(biāo)和實測坐標(biāo)的對比

表2 中，mx、my和mz表示實際測量坐標(biāo)的3個坐標(biāo)分量，px、py和pz分別表示定位結(jié)果的坐標(biāo)分量。把實測結(jié)果和定位結(jié)果代入式(3)，可進(jìn)行均方根誤差計算。

其中，mi,pi分別表示實測結(jié)果和定位結(jié)果，n表示坐標(biāo)點的數(shù)目。計算得到3 個坐標(biāo)分量的誤差結(jié)果分別為0.049 m、0.021 m 和0.059 m，三者平均誤差為0.043 m，達(dá)到了較高的截割精度。

本機(jī)器人系統(tǒng)測試運行時間已達(dá)90 天，系統(tǒng)運行穩(wěn)定，能準(zhǔn)確地按照巷道設(shè)計進(jìn)行截割作業(yè)，初步達(dá)到了設(shè)計目標(biāo)，為掘進(jìn)工作面無人化奠定了堅實技術(shù)基礎(chǔ)。

6 結(jié)束語

本文首先基于礦山物聯(lián)網(wǎng)，提出了一種新的高精度的井下定位的方法，且配置方便，易于部署和使用，解決了掘進(jìn)機(jī)機(jī)器人化的核心技術(shù)難題。為推動掘進(jìn)機(jī)機(jī)器人化的快速發(fā)展和安全高效的井下掘進(jìn)奠定了技術(shù)基礎(chǔ)。然后礦山物聯(lián)網(wǎng)和精準(zhǔn)定位方法，研發(fā)了掘進(jìn)機(jī)器人系統(tǒng)，應(yīng)用本文機(jī)器人系統(tǒng)可以實時定位掘進(jìn)機(jī)，精確感知周圍環(huán)境，實現(xiàn)和周圍環(huán)境的精準(zhǔn)交互，從而實現(xiàn)高精度掘進(jìn)。誤差結(jié)果和系統(tǒng)應(yīng)用驗證了本系統(tǒng)的精度和高可用性。

未來的工作有兩方面，一是研發(fā)三維真實鏡像系統(tǒng)，使系統(tǒng)能實時顯示掘進(jìn)的真實場景，從而使掘進(jìn)工作工作面透明化；二是在此基礎(chǔ)上發(fā)展完全自動化的智能掘進(jìn)系統(tǒng)。