盧新明，閆長(zhǎng)青，袁照平

（1.山東科技大學(xué)山東省智慧礦山信息技術(shù)重點(diǎn)實(shí)驗(yàn)室，山東 青島 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)系到煤礦的安全開采和礦工的生命安全。

研究人員對(duì)掘進(jìn)機(jī)自動(dòng)化和智能化進(jìn)行了大量研究[1-2]。目前，掘進(jìn)機(jī)已經(jīng)能夠?qū)崿F(xiàn)遠(yuǎn)程控制[3-5]，但由于未能實(shí)現(xiàn)地下空間的自主測(cè)量和定位，無法建立掘進(jìn)機(jī)自身位姿和巷道之間的位置關(guān)系，無法自動(dòng)感知和定位周圍環(huán)境，因此只能依靠視頻進(jìn)行作業(yè)，無法實(shí)現(xiàn)精確開采。掘進(jìn)機(jī)機(jī)器人化的核心問題——定位和導(dǎo)航已有了大量的研究[6-13]，主要發(fā)展了基于超寬帶技術(shù)的定位方法[10]、基于激光交匯的定位方法[2,12-13]和基于慣性導(dǎo)航技術(shù)的定位方法[14-15]。但超寬帶方法本身存在精度問題，很難精確控制切割精度。基于激光交匯技術(shù)的定位安裝過于復(fù)雜，需要測(cè)量多個(gè)點(diǎn)進(jìn)行定位，難以在復(fù)雜的井下環(huán)境中配置應(yīng)用。基于慣性導(dǎo)航的方法需要計(jì)算姿態(tài)角和距離，存在累積誤差問題。上述方法目前普遍處于理論研究和系統(tǒng)仿真階段，尚未發(fā)現(xiàn)智能掘進(jìn)機(jī)器人在井下的實(shí)際應(yīng)用。另外，不少定位仿真算法對(duì)姿態(tài)角的使用存在誤區(qū)，混淆了慣性實(shí)測(cè)結(jié)果和預(yù)定旋轉(zhuǎn)角的因果關(guān)系。慣性導(dǎo)航儀輸出的是實(shí)測(cè)姿態(tài)角，而使用實(shí)測(cè)姿態(tài)角對(duì)原坐標(biāo)系旋轉(zhuǎn)變換無法得到具有同樣姿態(tài)角的坐標(biāo)系統(tǒng)，如對(duì)3 個(gè)軸均旋轉(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ī)器人化的另一個(gè)關(guān)鍵問題是對(duì)周圍環(huán)境進(jìn)行感知需要傳感器設(shè)備聯(lián)網(wǎng)?，F(xiàn)代物聯(lián)網(wǎng)技術(shù)的核心可以實(shí)現(xiàn)設(shè)備入網(wǎng)和互聯(lián)，實(shí)時(shí)傳輸狀態(tài)數(shù)據(jù)至處理設(shè)備，目前該技術(shù)已成為實(shí)現(xiàn)感知礦山、智慧礦山的關(guān)鍵支撐技術(shù)[16-18]。借助物聯(lián)網(wǎng)技術(shù)可以使掘進(jìn)機(jī)實(shí)時(shí)感知礦山的狀態(tài)，實(shí)現(xiàn)掘進(jìn)機(jī)機(jī)器人化[19]。

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

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

由于掘進(jìn)機(jī)在礦井下工作，要實(shí)現(xiàn)掘進(jìn)機(jī)機(jī)器人化需要實(shí)時(shí)采集掘進(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)場(chǎng)工控機(jī)、可編程邏輯控制器（PLC,programmable logic controller），以及配置了多個(gè)傳感器的掘進(jìn)機(jī)。這些設(shè)備分別接入遠(yuǎn)程工控機(jī)和現(xiàn)場(chǎng)工控機(jī)，并由遠(yuǎn)程工控機(jī)通過互聯(lián)網(wǎng)和現(xiàn)場(chǎng)工控機(jī)交換信息。

遠(yuǎn)程遙控設(shè)備即遙控器，提供掘進(jìn)機(jī)運(yùn)行所需要的指令，通過串口與遠(yuǎn)程工控機(jī)相連。2 個(gè)工控機(jī)通過TCP/IP 連接，共處于一個(gè)局域網(wǎng)內(nèi)。全站儀和指北儀通過網(wǎng)絡(luò)與現(xiàn)場(chǎng)工控機(jī)連接，全站儀源源不斷地提供測(cè)量數(shù)據(jù)以計(jì)算坐標(biāo)；指北儀內(nèi)置陀螺儀，以一定的頻率連續(xù)測(cè)量全站儀的姿態(tài)角并發(fā)送數(shù)據(jù)。掘進(jìn)機(jī)油缸安裝油缸行程傳感器，測(cè)定油缸行程，掘進(jìn)機(jī)共配置8 條油缸，其中4 條負(fù)責(zé)炮頭的運(yùn)動(dòng)，另外4 條負(fù)責(zé)掘進(jìn)機(jī)位姿的調(diào)整。根據(jù)炮頭運(yùn)動(dòng)相關(guān)的油缸行程可以計(jì)算出炮頭坐標(biāo)信息。油缸行程傳感器和其他相應(yīng)設(shè)備與PLC 連接，慣性導(dǎo)航儀通過網(wǎng)絡(luò)實(shí)時(shí)傳送掘進(jìn)機(jī)的位姿數(shù)據(jù)至工控機(jī)，PLC 通過Modbus TCP 協(xié)議與工控機(jī)連接。整個(gè)系統(tǒng)形成了一個(gè)物聯(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)的角度看，整個(gè)網(wǎng)絡(luò)架構(gòu)又可看作客戶機(jī)/服務(wù)器模式。以遠(yuǎn)程工控機(jī)和地面工控機(jī)為中心的網(wǎng)絡(luò)各自組成了客戶機(jī)，以現(xiàn)場(chǎng)工控機(jī)為中心的網(wǎng)絡(luò)和以PLC 為中心的網(wǎng)絡(luò)共同組成了服務(wù)器系統(tǒng)，其中客戶機(jī)系統(tǒng)可以根據(jù)不同的角色和職能配置多個(gè)。本文系統(tǒng)采用了三層架構(gòu)，其中地面客戶機(jī)系統(tǒng)用于地面監(jiān)控。

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

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

圖2 定位原理

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

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

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

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

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

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

3.2 坐標(biāo)系定義

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

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

巷道坐標(biāo)系采用從巷道起點(diǎn)沿中線掘進(jìn)的方向?yàn)閅軸正向，向右方向?yàn)閄軸正向，垂直于巷道底板向上方向?yàn)閆軸正向。

掘進(jìn)機(jī)坐標(biāo)系采用沿掘進(jìn)機(jī)中線前進(jìn)方向?yàn)閅軸正向，沿機(jī)身平面垂直于機(jī)身中線向右方向?yàn)閄軸正向，垂直于機(jī)身平面向上方向?yàn)閆軸正向。

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

3.3 定位算法

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

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

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

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

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

由全站儀坐標(biāo)系向量、全站儀基點(diǎn)和全站儀坐標(biāo)(xob,yob,zob)，解算出靶球在大地坐標(biāo)系OeXeYeZe的絕對(duì)坐標(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)系原點(diǎn)的絕對(duì)坐標(biāo)(xr,yr,zr)，如式(2)所示。

算法1 具體描述如下。

算法1定位算法

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

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

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

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

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

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

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

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

9)考慮原點(diǎn)計(jì)算定位坐標(biāo)。

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

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

本節(jié)基于前述物聯(lián)網(wǎng)和定位算法，設(shè)計(jì)并實(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)場(chǎng)作業(yè)子系統(tǒng)。

1)地面監(jiān)控子系統(tǒng)部署在地面上，供地面觀察和地面管理人員使用。地面監(jiān)控子系統(tǒng)主要提供井下作業(yè)的監(jiān)視支持，一般不用于實(shí)際作業(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)場(chǎng)作業(yè)子系統(tǒng)部署在掘進(jìn)機(jī)機(jī)身和掘進(jìn)機(jī)附近，不需要人工操作，負(fù)責(zé)現(xiàn)場(chǎng)作業(yè)。

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

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

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

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

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

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

現(xiàn)場(chǎng)工控機(jī)通過網(wǎng)絡(luò)接收監(jiān)控系統(tǒng)傳來的指令，通過Modbus TCP 將指令傳遞給PLC，PLC 執(zhí)行指令驅(qū)動(dòng)掘進(jìn)機(jī)各部件進(jìn)行動(dòng)作完成掘進(jìn)作業(yè)。同時(shí)，現(xiàn)場(chǎng)工控機(jī)查詢PLC 的狀態(tài)信息并根據(jù)需要執(zhí)行相應(yīng)計(jì)算，然后把遠(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ī)炮頭的擺動(dòng)，2 條前鏟板油缸和2 條后支撐油缸負(fù)責(zé)掘進(jìn)機(jī)姿態(tài)的調(diào)整。前4 條油缸行程用于計(jì)算炮頭相對(duì)于掘進(jìn)機(jī)重心的位置，后4 條油缸行程作為掘進(jìn)機(jī)姿態(tài)調(diào)整的依據(jù)。

控制系統(tǒng)選用PLC 的西門子1500。PLC 連接所有的掘進(jìn)機(jī)傳感器以及掘進(jìn)機(jī)的其他部件，能夠按指令驅(qū)動(dòng)相關(guān)的部件運(yùn)動(dòng)并提供各種參數(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)信息流，并計(jì)算空間坐標(biāo)回傳。

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

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

4.2.1 炮頭定位

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

4.2.2 實(shí)時(shí)精確顯示

掘進(jìn)機(jī)作業(yè)的狀態(tài)需要實(shí)時(shí)準(zhǔn)確地顯示在監(jiān)控屏幕上，以便根據(jù)實(shí)時(shí)工作狀態(tài)進(jìn)行高效精準(zhǔn)的掘進(jìn)作業(yè)。掘進(jìn)機(jī)炮頭在巷道的位置、掘進(jìn)機(jī)相對(duì)于巷道的位置和姿態(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)巷道、炮頭以及它們之間的相對(duì)位置關(guān)系。

具體步驟如下。

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

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

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

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

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

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

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

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

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

圖5 運(yùn)行界面

5.1 算法比較

為了對(duì)比本文所提定位算法與基于旋轉(zhuǎn)姿態(tài)角的定位算法，分別測(cè)試了3 組數(shù)據(jù)樣例。旋轉(zhuǎn)矩陣方法分別對(duì)3 個(gè)角度，即方位角yaw、俯仰角pitch和滾動(dòng)角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軸上時(shí)，才有一種旋轉(zhuǎn)順序可以得到和本文定位算法一致的正確結(jié)果，但用于X軸和Y軸時(shí)對(duì)3 個(gè)姿態(tài)角的旋轉(zhuǎn)順序不確定。而其他坐標(biāo)的定位結(jié)果顯示，無論采用什么角度，進(jìn)行表1 中任一種順序的姿態(tài)角旋轉(zhuǎn)變化，得到的坐標(biāo)和由實(shí)時(shí)姿態(tài)角求得的定位結(jié)果不一致。由此可見，用于旋轉(zhuǎn)的姿態(tài)角和慣導(dǎo)輸出的實(shí)時(shí)狀態(tài)的姿態(tài)角沒有關(guān)系，使用旋轉(zhuǎn)慣導(dǎo)實(shí)時(shí)姿態(tài)角的方式進(jìn)行定位無法得到正確的定位結(jié)果。

5.2 精度分析

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

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

表2 定位坐標(biāo)和實(shí)測(cè)坐標(biāo)的對(duì)比

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

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

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

6 結(jié)束語

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

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