挖掘機(jī)北斗高精度自動定位和引導(dǎo)方法

杜素忠, 張錄彬

(萬寶礦產(chǎn)有限公司 蒙育瓦銅礦，北京 100053)

0 引言

挖掘機(jī)作為一種由動力裝置、工作裝置、回轉(zhuǎn)機(jī)構(gòu)、操縱機(jī)構(gòu)、傳動機(jī)構(gòu)、行走機(jī)構(gòu)和輔助設(shè)施等組成的工程機(jī)械，廣泛應(yīng)用于國民經(jīng)濟(jì)各領(lǐng)域。其中：行走機(jī)構(gòu)主要指履帶為基礎(chǔ)的底盤(又稱“底板”)，工作裝置包括大臂、小臂、鏟斗、輔助裝置等[1]。對挖掘機(jī)行走機(jī)構(gòu)和工作裝置進(jìn)行高精度定位[2]并實(shí)現(xiàn)自動引導(dǎo)[3]和跟蹤，能夠提高挖掘機(jī)作業(yè)效率、優(yōu)化作業(yè)效果及降低工作損耗。如：工程施工可盡量避免對周圍物體的破壞，提高作業(yè)精度；水下和涵洞等不可見區(qū)域可實(shí)現(xiàn)設(shè)備精準(zhǔn)操作；礦山采掘中可降低損失和貧化率[4-5]，實(shí)現(xiàn)礦山精益管理。

挖掘機(jī)傳統(tǒng)引導(dǎo)和跟蹤主要依靠人工方式：測量人員提前放樣基準(zhǔn)線和打樁；現(xiàn)場指揮員和挖掘機(jī)操作手根據(jù)經(jīng)驗(yàn)執(zhí)行引導(dǎo)和跟蹤，操作精度往往達(dá)不到要求。目前，國內(nèi)、外部分企業(yè)和研究機(jī)構(gòu)推出了挖掘機(jī)自動定位和跟蹤系統(tǒng)[6-8]，但未公開具體算法，效果評估論述不清晰；部分已公開科研成果和應(yīng)用案例[9-10]在實(shí)現(xiàn)方法、效果評價(jià)和數(shù)據(jù)對比方面尚不全面。

本文將北斗高精度空間信息[11-13]技術(shù)與先進(jìn)傳感器相結(jié)合，輔以計(jì)算機(jī)軟件程序，提出一種實(shí)現(xiàn)挖機(jī)行走機(jī)構(gòu)和工作裝置動態(tài)高精度定位的方法和系統(tǒng)，滿足自動引導(dǎo)和跟蹤要求，利于挖掘機(jī)遠(yuǎn)程監(jiān)督和少人化作業(yè)管理。

1 系統(tǒng)設(shè)計(jì)

1.1 設(shè)備組成

通過北斗導(dǎo)航衛(wèi)星系統(tǒng)(BDS,beidou navigation satellite system)高精度接收機(jī)[14-15]、測量型接收天線、單軸角度傳感器、雙軸角度傳感器[16]和車載計(jì)算機(jī)等，組成圖1所示的挖掘機(jī)高精度定位系統(tǒng)。

圖1 系統(tǒng)結(jié)構(gòu)

其中：車載計(jì)算機(jī)安裝在駕駛室內(nèi)，與角度傳感器和接收機(jī)連接，并安裝有專用軟件程序，實(shí)現(xiàn)工作裝置實(shí)時(shí)高精度定位解算和顯示；兩支接收天線安裝在挖掘機(jī)尾部，盡可能保持在同一水平，并與接收機(jī)連接，天線間連線盡可能與駕駛室方向保持垂直；三套單軸角度傳感器分別安裝在大臂、小臂、鏟斗的合適位置，一套雙軸角度傳感器安裝在駕駛室內(nèi)，角度傳感器共同判斷挖掘機(jī)運(yùn)動姿態(tài)。同時(shí)，可為挖掘機(jī)配置無線網(wǎng)絡(luò)傳輸單元[17-18]，實(shí)現(xiàn)車載計(jì)算機(jī)與后臺服務(wù)器通信，滿足工作指令下達(dá)和作業(yè)實(shí)績反饋。

1.2 運(yùn)行流程

車載計(jì)算機(jī)中軟件程序按照圖2基本流程展開，完成挖掘機(jī)高精度定位解算和跟蹤，在此基礎(chǔ)上對比目標(biāo)與實(shí)際位置實(shí)現(xiàn)自動引導(dǎo)。其中：

1)需要定位解算的點(diǎn)：大臂與工作平臺連接點(diǎn)R，大臂與小臂連接點(diǎn)C，小臂與鏟斗連接點(diǎn)D，鏟斗頭部E，行走機(jī)構(gòu)與地面后觸點(diǎn)F。需要說明，R、C、D、E點(diǎn)主要反映挖掘機(jī)工作裝置實(shí)時(shí)運(yùn)行狀態(tài)；F點(diǎn)位于行走機(jī)構(gòu)，主要反映當(dāng)前工作位置“欠挖”、“多挖”等情況。

2)定位解算所需已知?jiǎng)討B(tài)參數(shù)：用戶接收機(jī)獲取天線A和B地理坐標(biāo)、絕對高程等；角度傳感器分別獲取大臂水平夾角δc、小臂水平夾角δd、鏟斗水平夾角δe，以及駕駛室俯仰角δy和橫滾角δx，動態(tài)參數(shù)隨挖掘機(jī)運(yùn)動而發(fā)生實(shí)時(shí)變化。

3)定位解算所需已知靜態(tài)參數(shù)：挖掘機(jī)行走機(jī)構(gòu)完全水平狀態(tài)下，AB連線長度lb，R到AB連線垂線長度Lf，A到F垂直高度Hf，大臂長度Lc，小臂長度Ld，鏟斗長度Le；A到R高差Hr；由R到AB連線做垂線，交點(diǎn)為R’，A到R’距離為rSy。實(shí)際運(yùn)用中，部分參數(shù)測量困難，可選用廠家參考手冊中提供的標(biāo)準(zhǔn)尺寸。

2 姿態(tài)分析

建立本體坐標(biāo)系，分析挖掘機(jī)當(dāng)前相對運(yùn)行姿態(tài)，獲得不同視角下相關(guān)點(diǎn)位的定位數(shù)據(jù)[19]。

圖2 運(yùn)行流程

2.1 側(cè)視二維坐標(biāo)

以A為原點(diǎn)，挖掘機(jī)前進(jìn)方向水平面為X軸、垂直于水平面向上方向?yàn)閅軸，建立圖3所示的側(cè)視坐標(biāo)S1，各點(diǎn)相對坐標(biāo)可以表達(dá)為矩陣V:

(1)

其中：

(2)

圖3 S1坐標(biāo)系

2.2 俯視二維坐標(biāo)

以A為原點(diǎn)，挖掘機(jī)前進(jìn)方向水平面為X軸、AB向量為Y軸，建立圖4挖掘機(jī)正常工作狀態(tài)俯視坐標(biāo)S2。其中，S1主要表達(dá)各點(diǎn)位相對高度和延伸長度，S2主要表達(dá)各點(diǎn)位相對寬度。

圖4 S2坐標(biāo)系

因此，各點(diǎn)坐標(biāo)表達(dá)為矩陣W:

(3)

其中：

r′Sy=rSycosδx

(4)

2.3 對比坐標(biāo)

為與其他姿態(tài)分析方法進(jìn)行對比，從而證明本方法的優(yōu)越性，需要直接建立本體三維坐標(biāo)系S3：以A為原點(diǎn)、向量AR為X軸，向量AB為Y軸，向量FA為Z軸。其中，各點(diǎn)坐標(biāo)表達(dá)為矩陣U：

(5)

矩陣中各元素值可通過已知靜態(tài)參數(shù)和δc、δd、δe之間三角函數(shù)關(guān)系求解，求解考慮δy的影響，不考慮δx影響，這是因?yàn)楹罄m(xù)算法將考慮δx影響。

3 定位解算

3.1 坐標(biāo)轉(zhuǎn)換

挖掘機(jī)定位自動引導(dǎo)和跟蹤中，需要解算各點(diǎn)在高斯平面直角坐標(biāo)系的精確坐標(biāo)和絕對高程，以及某些獨(dú)立坐標(biāo)平面系中坐標(biāo)和絕對高程。這需要結(jié)合姿態(tài)分析所得相對坐標(biāo)，進(jìn)行坐標(biāo)轉(zhuǎn)換，從而定位挖掘機(jī)行走機(jī)構(gòu)和工作裝置。

用戶接收機(jī)實(shí)時(shí)獲取天線A和B的經(jīng)度、緯度、絕對高程后，表達(dá)為：

(6)

可將天線A和B經(jīng)度、緯度轉(zhuǎn)換到高斯平面坐標(biāo)，其中：X軸指北，Y軸指東，fx和fy為轉(zhuǎn)換函數(shù)[20]。

(7)

向量AB可用于表示挖掘機(jī)工作機(jī)構(gòu)朝向，對準(zhǔn)確定位挖掘機(jī)各點(diǎn)至關(guān)重要。在標(biāo)識挖掘機(jī)工作機(jī)構(gòu)朝向后，進(jìn)行坐標(biāo)轉(zhuǎn)換，計(jì)算所得結(jié)果不存在二義性。

設(shè)AB與高斯平面直角坐標(biāo)系Y軸夾角θ，坐標(biāo)轉(zhuǎn)換中的角度參數(shù)為β，則：

(8)

如果BGx>AGx：

β=θ-90°

(9)

如果BGx

β=θ+90°

(10)

上述S2中，挖掘機(jī)各點(diǎn)相對坐標(biāo)可轉(zhuǎn)換為高斯平面直角坐標(biāo)系坐標(biāo)：

(11)

絕對高程指各點(diǎn)至地球參考橢球面的距離。因此，上述坐標(biāo)S1矩陣V中，各點(diǎn)絕對高程可簡化表達(dá)為M：

(12)

同樣，可將計(jì)算挖掘機(jī)各點(diǎn)高斯平面直角坐標(biāo)系坐標(biāo)轉(zhuǎn)換到獨(dú)立坐標(biāo)系，比如：礦山內(nèi)部坐標(biāo)、施工區(qū)域坐標(biāo)等。

3.2 高精度定位

3.2.1 定位原理

用戶接收機(jī)通過天線接收的BDS導(dǎo)航電文計(jì)算天線位置。忽略觀測誤差時(shí)，在任意時(shí)刻j，用戶接收機(jī)觀測并接收衛(wèi)星i的導(dǎo)航電文后，可計(jì)算出該衛(wèi)星位置Sij：

Sij=[xij,yij,zij]

(13)

需要求解的接收天線位置Uj未知：

Uj=[xuj,yuj,zuj]

(14)

pj表示Sij與Uj的距離：

(15)

為提高定位精度，需要考慮觀測誤差。則任意時(shí)刻j實(shí)際觀測距離Rj與pj的關(guān)系可以表達(dá)為：

Rj=pj+c·Δtu+c·Δts+Δteph+Δtion+Δttrp+Δm+ε

(16)

其中：Δtu表示接收機(jī)鐘差，Δts表示衛(wèi)星鐘差，Δreph表示星歷誤差帶來的測距誤差，Δrion表示電離層延時(shí)引起的測距誤差，Δrtrp表示對流層延時(shí)引起的測距誤差，Δm表示由多徑等引起的測距誤差，ε表示用戶接收機(jī)本身噪聲引起的測距誤差。

求解式(15)中Uj，需要接收機(jī)在消除誤差基礎(chǔ)上，至少觀測到4顆以上衛(wèi)星，單一衛(wèi)星信號強(qiáng)度應(yīng)高于30 dbm。實(shí)際應(yīng)用中，通常采用多頻用戶接收機(jī)，除接收BDS衛(wèi)星信號外，還同步接收美國全球定位系統(tǒng)(GPS,global positioning system)和俄羅斯導(dǎo)航衛(wèi)星系統(tǒng)(GLONASS,global naya navigatsionnaya sputnikovaya sistema)衛(wèi)星信號，一方面確保定位質(zhì)量，另一方面可用于消除部分誤差。

3.2.2 差分定位

消除式(16)中誤差并獲求解天線A和B精確位置，需要用到距離差分定位，這是挖掘機(jī)各點(diǎn)高精度定位基礎(chǔ)。即：借助事先已知精確位置的參考站，可減弱或消除Δtu、Δts、Δreph，參考站服務(wù)范圍內(nèi)可消除或減小Δrtrp，借助接收機(jī)雙頻或多頻接收能力，可消除Δrion，這是對所使用接收機(jī)的基本要求。Δm與ε與挖掘機(jī)所處地理環(huán)境和接收機(jī)本身特性有關(guān)，無法消除。

因此，挖掘機(jī)高精度定位中，需在附近建立高精度參考站，實(shí)現(xiàn)A和B精確定位。對于精度要求高、長期作業(yè)場所，參考站位置一般固定，并由經(jīng)長時(shí)間連續(xù)觀測BDS星歷后計(jì)算獲得參考站位置；對于精度要求不高或臨時(shí)作業(yè)場所，可在作業(yè)場所附近建立臨時(shí)參考站，并通過傳統(tǒng)測量獲得參考站位置。需要說明，參考站存在服務(wù)范圍限制，且距離參考站越遠(yuǎn)，可獲得的定位精度越低。因此，挖掘機(jī)距離高精度參考站不能過遠(yuǎn)，否則會因超出參考站有效服務(wù)范圍而影響定位精度。

因式(15)中，針對pj的測距方法不同，距離差分定位分為偽距差分定位和載波相位差分定位[21]。載波相位差分定位精度高于偽距差分定位，但存在“整周期模糊度”問題，計(jì)算過程復(fù)雜、耗時(shí)長，通常用于毫米級后處理定位場景，比如安全位移的趨勢分析，在挖掘機(jī)實(shí)時(shí)定位跟蹤和自動引導(dǎo)中實(shí)用性不強(qiáng)。

目前，BDS偽距差分實(shí)時(shí)定位精度中誤差水平20 mm左右、高程30 mm左右[22]。

4 試驗(yàn)結(jié)果與分析

4.1 測試環(huán)境和步驟

在某露天礦山環(huán)境下，選擇小松PC2000-8型號反鏟挖掘機(jī)進(jìn)行試驗(yàn)測試并對比計(jì)算結(jié)果。具體步驟和方法為：

1)在該挖掘機(jī)駕駛室安裝北斗高精度接收機(jī)，在該挖掘機(jī)尾部安裝兩支測量型接收天線，用于接收衛(wèi)星信號。

2)在大臂、小臂和鏟斗上安裝單軸角度傳感器，駕駛室安裝雙軸角度傳感器和車載計(jì)算機(jī)。

3)根據(jù)安裝情況，標(biāo)定表1靜態(tài)參數(shù)，其中，大臂長度Lc、小臂長度Ld、鏟斗長度Le均取自參考手冊標(biāo)準(zhǔn)尺寸，而非實(shí)測結(jié)果。

表1 已知靜態(tài)參數(shù)

4)采用本方法和其他方法分別編制計(jì)算機(jī)軟件程序，并安裝到車載計(jì)算機(jī)，用于獲取不同方法的計(jì)算結(jié)果。

5)選定挖掘機(jī)5組在不同狀態(tài)下動態(tài)參數(shù)進(jìn)行測試并記錄計(jì)算所得數(shù)據(jù)。表2為詳細(xì)動態(tài)參數(shù)。

表2 動態(tài)參數(shù)

6)不同狀態(tài)下，除獲得兩種方法計(jì)算結(jié)果外，還采用傳統(tǒng)測量設(shè)備對各點(diǎn)定位數(shù)據(jù)進(jìn)行實(shí)地測量，作為計(jì)算機(jī)程序結(jié)果對比的基準(zhǔn)。

4.2 計(jì)算結(jié)果和對比

采用面向?qū)ο缶幊?，?shí)現(xiàn)本方法實(shí)時(shí)定位計(jì)算過程。另外，直接通過三維坐標(biāo)轉(zhuǎn)換，實(shí)現(xiàn)挖掘機(jī)各點(diǎn)位從S3轉(zhuǎn)換到高斯平面坐標(biāo)和高程，是目前公布的一種傳統(tǒng)方法。表3給出測量結(jié)果與上述兩種方法的對比情況：

表3 計(jì)算結(jié)果

1)本方法計(jì)算結(jié)果中，相對A靜止的點(diǎn)R在高斯平面坐標(biāo)系X方向平均誤差0.015、Y方向平均誤差0.069、水平方向平均誤差0.071，高程平均誤差0.154；相對A運(yùn)動的點(diǎn)E在高斯平面坐標(biāo)系X方向平均誤差0.033、Y方向平均誤差0.071、水平方向平均誤差0.078，高程平均誤差0.189。

2)傳統(tǒng)方法計(jì)算結(jié)果中，相對A靜止的點(diǎn)R在高斯平面坐標(biāo)系X方向平均誤差0.202、Y方向平均誤差0.3、水平方向平均誤差0.362，高程平均誤差0.128；相對A運(yùn)動的點(diǎn)E在高斯平面坐標(biāo)系X方向平均誤差0.304、Y方向平均誤差0.624、水平方向平均誤差0.629，高程平均誤差0.163。

3)對比分析可知，傳統(tǒng)坐標(biāo)轉(zhuǎn)換求解方法精度在高程上與本方法結(jié)果接近，在挖掘機(jī)作業(yè)中可接受；但部分水平數(shù)據(jù)誤差偏大，這是由傳統(tǒng)坐標(biāo)轉(zhuǎn)換中補(bǔ)償不足和部分假設(shè)造成。

4)相對A靜止的點(diǎn)和相對A運(yùn)動的點(diǎn)，在計(jì)算結(jié)果誤差上存在差別，不應(yīng)相互替代以說明方法的有效性。

4.3 誤差分析

本方法計(jì)算與傳統(tǒng)測量產(chǎn)生一定偏差的主要原因在于：

1)靜態(tài)參數(shù)標(biāo)定本身存在誤差，具體應(yīng)用中通過實(shí)際測量可適度減小、但不能完全消除。比如，Lc、Ld、Le選擇標(biāo)準(zhǔn)尺寸進(jìn)行標(biāo)定，但實(shí)際尺寸和標(biāo)準(zhǔn)尺寸之間存在一定誤差，對計(jì)算結(jié)果產(chǎn)生一定影響。

2)用戶接收機(jī)和角度傳感器在安裝及實(shí)時(shí)測量結(jié)果存在誤差，所產(chǎn)生動態(tài)參數(shù)將影響計(jì)算結(jié)果。比如，安裝時(shí)不能完全保持水平或安裝時(shí)所記錄初始值存在一定誤差，運(yùn)動狀態(tài)下用戶接收機(jī)和角度傳感器自身測量結(jié)果也存在誤差。

3)挖掘機(jī)工作時(shí)，行走機(jī)構(gòu)和工作裝置各連接位置存在松耦合，會導(dǎo)致標(biāo)定值發(fā)生小幅變化，一定程度上也會影響計(jì)算結(jié)果。

5 結(jié)束語

通過在挖掘機(jī)上安裝北斗接收機(jī)、測量型天線、單軸角度傳感器、雙軸角度傳感器和車載計(jì)算機(jī)等設(shè)備，構(gòu)建側(cè)視和俯視二維坐標(biāo)，合理分析和求解挖掘機(jī)運(yùn)行姿態(tài)下各點(diǎn)相對坐標(biāo)，結(jié)合北斗高精度空間信息技術(shù)和分析算法，經(jīng)坐標(biāo)轉(zhuǎn)化可完成各主要部件高精度定位，實(shí)現(xiàn)挖掘機(jī)自動定位和引導(dǎo)。所述方法水平和高程定位精度誤差可控制在厘米級，優(yōu)于現(xiàn)有其他實(shí)現(xiàn)方法，能夠?qū)崿F(xiàn)挖掘機(jī)自動跟蹤和引導(dǎo)要求，滿足不同行業(yè)工程要求。