基于載波相位差分技術(shù)的水平定姿挖掘裝置

張 帆， 劉 瑾， 楊海馬， 肖 俊， 何施晶， 袁寶龍

(1.上海工程技術(shù)大學(xué) 電子電氣工程學(xué)院，上海 201620； 2.上海理工大學(xué) 光電信息與計(jì)算機(jī)工程學(xué)院，上海 200093； 3.上?；纯浦悄芸萍加邢薰荆虾?201900)

隨著科學(xué)技術(shù)的不斷進(jìn)步，定位輔助系統(tǒng)已經(jīng)得到了越來越廣泛的應(yīng)用。從近幾年的發(fā)展趨勢(shì)來看，挖掘機(jī)自動(dòng)化作業(yè)成為挖掘機(jī)研究領(lǐng)域的主要方向，國(guó)內(nèi)外一些工程機(jī)械龍頭企業(yè)和高校對(duì)此做了大量的研究工作。在衛(wèi)星導(dǎo)航中，輔助定位通常作為一種輔助定位方式，在連續(xù)性、精度等方面對(duì)衛(wèi)星導(dǎo)航進(jìn)行增強(qiáng)。但是在挖掘設(shè)備中，定位輔助技術(shù)受到了一定的限制[1-12]。挖掘機(jī)自身定位和工作裝置姿態(tài)獲取是挖掘機(jī)自動(dòng)化研究中不可或缺的環(huán)節(jié)[13-15]。裝置姿態(tài)的測(cè)量工作多采用光電編碼器、位移傳感器或傾角傳感器[16-17]。然而挖掘機(jī)在進(jìn)行挖掘作業(yè)過程中，由于地形的復(fù)雜性，工作裝置不可避免地會(huì)與土壤、巖石等發(fā)生碰撞，容易對(duì)傳感器造成一定程度上的損壞[18]?；诖?，筆者提出一種基于載波相位差分技術(shù)的挖掘設(shè)備水平定姿定位輔助方法，彌補(bǔ)了傳統(tǒng)姿態(tài)測(cè)量系統(tǒng)的不足，可安全、可靠、準(zhǔn)確地獲取工作裝置姿態(tài)。

1 姿態(tài)測(cè)量方法

挖掘頭的定姿施工控制模塊包括挖掘頭的實(shí)時(shí)坐標(biāo)定位和定姿施工保持。其中，挖掘頭的實(shí)時(shí)坐標(biāo)定位是以基準(zhǔn)站為原點(diǎn)O建立三維坐標(biāo)系，將挖掘頭的關(guān)節(jié)坐標(biāo)變換與空間GPS定位相結(jié)合，用戶主機(jī)根據(jù)挖掘機(jī)頭所處的坐標(biāo)點(diǎn)相對(duì)于任意兩個(gè)不同方位點(diǎn)的距離變化，計(jì)算挖掘頭的實(shí)時(shí)坐標(biāo)信息，從而定位挖掘頭的實(shí)時(shí)坐標(biāo)；定姿施工保持是在關(guān)節(jié)坐標(biāo)運(yùn)算后，用戶主機(jī)控制挖掘設(shè)備的主臂、次臂做出相應(yīng)動(dòng)作以維持挖掘頭始終保持固定的工作姿態(tài)，垂直施工。

對(duì)于關(guān)節(jié)坐標(biāo)運(yùn)算，采取的坐標(biāo)變換將關(guān)節(jié)坐標(biāo)變換與GPS相結(jié)合，推導(dǎo)出一個(gè)完整的定姿公式，能夠準(zhǔn)確定位挖掘頭的姿態(tài)信息，從而方便駕駛室的施工人員通過用戶主機(jī)控制主次臂關(guān)節(jié)精準(zhǔn)、靈活地定位挖掘。姿態(tài)測(cè)量示意圖如圖1所示。

圖1 姿態(tài)測(cè)量示意圖

GPS中,a為橢球元素，設(shè)基準(zhǔn)站于點(diǎn)O，任一點(diǎn)A為方位點(diǎn)。設(shè)A=(XA，YA，ZA)，此時(shí)A的大地坐標(biāo)為(BA，LA，hA)，即

(1)

(2)

(3)

其中，式(1)～式(3)可以解出任一點(diǎn)A的坐標(biāo)(XA，YA，ZA)。

設(shè)基準(zhǔn)站于點(diǎn)O，任一點(diǎn)B為方位點(diǎn)，設(shè)B=(XB，YB，ZB)，此時(shí)B的大地坐標(biāo)為(BB，LB，hB)，同理可得：

(4)

(5)

(6)

其中，由式(4)～式(6)可以解出任一點(diǎn)B的坐標(biāo)(XB，YB，ZB)。

如圖1所示，設(shè)任一方位點(diǎn)A與M點(diǎn)之間的距離為DAM，任一方位點(diǎn)B與M點(diǎn)之間的距離為DBM，挖掘機(jī)頭所處的坐標(biāo)為M=(X0，Y0，Z0)。

由兩點(diǎn)間的距離公式：

(7)

(8)

(X0-XA)(YB-YA)=(Y0-YA)(XB-XA)

(9)

即可得：

(10)

(11)

(12)

其中，結(jié)合式(10)～式(12)可以得到挖掘機(jī)頭所處的坐標(biāo)點(diǎn)(X0，Y0，Z0)。向量AB確定的角度記為角度θ。由圖1姿態(tài)圖可知，θ為AB在xoy平面上的投影與y軸的夾角，其計(jì)算公式為

(13)

結(jié)合關(guān)節(jié)坐標(biāo)變換與GPS，可推導(dǎo)出上述定位姿態(tài)公式，從而確定挖掘頭的位置姿態(tài)信息。

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

2.1 系統(tǒng)組成

挖掘機(jī)系統(tǒng)由主臂、次臂、GPS裝置、通信線纜、駕駛室、用戶主機(jī)、旋轉(zhuǎn)車體、挖掘機(jī)底盤和鏟斗組成。本輔助系統(tǒng)的核心為GPS加上載波相位差分系統(tǒng)以及姿態(tài)傳感裝置。為保證系統(tǒng)可靠性，同時(shí)具有友好的人機(jī)接口，駕駛室中利用工控一體機(jī)顯示屏輔助顯示當(dāng)前挖掘頭的空間姿態(tài)，指導(dǎo)挖掘工作。挖掘機(jī)完成一次挖掘任務(wù)的空間路徑與挖掘機(jī)的工作參數(shù)有關(guān)。挖掘深坑時(shí)，鏟斗提升、回轉(zhuǎn)以及運(yùn)行至下一起始點(diǎn)時(shí)，屬于空間PTP(Point to Point)運(yùn)動(dòng)，軌跡規(guī)劃比較簡(jiǎn)單[19-20]。

設(shè)備工作流程圖如圖2所示。

圖2 設(shè)備工作流程圖

設(shè)備工作時(shí)，首先打開上電開關(guān)，查看通信狀態(tài)燈，閃爍則表示連接正常。GPS工程挖掘頭已接收正常信號(hào)，實(shí)時(shí)通信狀態(tài)指示?？臻g GPS進(jìn)行動(dòng)作測(cè)試，觸發(fā)動(dòng)作測(cè)試鍵，主臂、次臂做對(duì)應(yīng)動(dòng)作，姿態(tài)傳感器指示正常，則表示線路正常。開始對(duì)施工區(qū)域進(jìn)行網(wǎng)格設(shè)置工作：① 施工人員可以用畫圖筆在液晶屏的水平網(wǎng)格面上設(shè)定施工平面，形成對(duì)應(yīng)實(shí)際施工區(qū)域的工作網(wǎng)格；② 工作人員可以手持移動(dòng)GPS終端(即流動(dòng)站)設(shè)定位置保護(hù)點(diǎn)；③ 支持不規(guī)則施工區(qū)的設(shè)定。設(shè)置工作網(wǎng)格結(jié)束后，單擊“開始”按鈕進(jìn)行輔助指示工作；實(shí)時(shí)指示當(dāng)前挖掘頭所處的網(wǎng)格位置。運(yùn)用點(diǎn)陣液晶顯示模塊進(jìn)行平板坐標(biāo)輔助顯示，用戶主機(jī)有效地將定位空間識(shí)別定位在水平面網(wǎng)格中，再根據(jù)流動(dòng)站的觀測(cè)量在水平面網(wǎng)格內(nèi)將實(shí)際的施工區(qū)域?qū)?yīng)設(shè)置為工作網(wǎng)格，可以設(shè)置位置保護(hù)點(diǎn)，便于施工人員完成網(wǎng)格輔助設(shè)計(jì)、布局；然后用戶主機(jī)根據(jù)流動(dòng)站的觀測(cè)量將挖掘頭的實(shí)時(shí)坐標(biāo)在水平面網(wǎng)格中顯示其所處的當(dāng)前網(wǎng)格位置，通過用戶界面可完成網(wǎng)格輔助設(shè)計(jì)、布局，以及施工指示、越界提醒。此外，用戶主機(jī)還根據(jù)姿態(tài)傳感器的監(jiān)控信息控制挖掘設(shè)備的主臂、次臂做出相應(yīng)動(dòng)作以使鏟斗始終保持固定的工作姿態(tài)，垂直施工，從而能夠在短時(shí)間內(nèi)高效地對(duì)施工區(qū)域進(jìn)行挖掘，有效地完成施工設(shè)備挖掘頭空間姿態(tài)的定位指示功能。

2.2 差分定位系統(tǒng)組成

GPS-5 Hz發(fā)送GPS位置，PC接收數(shù)據(jù)為Hex數(shù)據(jù)，波特率為115200，8數(shù)據(jù)位，無奇偶校驗(yàn)，1停止位，總長(zhǎng)138 B。

如圖3所示，采用GPS加上載波相位差分系統(tǒng)進(jìn)行定位，基準(zhǔn)站的接收機(jī)執(zhí)行兩個(gè)操作：① 接收GPS衛(wèi)星信號(hào)；② 把采集的載波相位觀測(cè)量發(fā)送給流動(dòng)站；流動(dòng)站的接收機(jī)執(zhí)行3個(gè)操作：① 接收GPS衛(wèi)星信號(hào)；② 接收基準(zhǔn)站所發(fā)送的基準(zhǔn)站觀測(cè)量；③ 對(duì)衛(wèi)星信號(hào)與基準(zhǔn)站觀測(cè)量進(jìn)行對(duì)比處理，從而對(duì)觀測(cè)量進(jìn)行修正并傳送給用戶主機(jī)。

圖3 差分定位系統(tǒng)

2.3 軟硬件設(shè)計(jì)

本系統(tǒng)上位機(jī)軟件通過LabVIEW 平臺(tái)開發(fā)，有4個(gè)子界面，分別為設(shè)置、GPS數(shù)據(jù)、GPS圖表和手冊(cè)指南，如圖4所示。

圖4 軟件挖掘檢測(cè)界面

硬件采用的是三菱FX3U-485ADP-MB的PLC，并采用Modbus協(xié)議。后臺(tái)采用Modbus DTU協(xié)議，8位數(shù)據(jù)，偶校驗(yàn)，1個(gè)停止位，波特率為19200。表1和表2分別表示PC發(fā)送的數(shù)據(jù)地址以及PLC后臺(tái)的返回?cái)?shù)據(jù)。

表1 PC發(fā)送數(shù)據(jù)

表2 PLC后臺(tái)返回?cái)?shù)據(jù)

3 實(shí)驗(yàn)與結(jié)果分析

選擇一塊空曠施工區(qū)域進(jìn)行試驗(yàn)，挖掘地段需要考慮崎嶇不平、深淺未知等因素?；谕诰虻奶厥庑院蜕a(chǎn)工藝的獨(dú)特性，研究基于載波相位差分技術(shù)的水平定姿挖掘裝置，系統(tǒng)包括施工區(qū)域和挖掘頭的空間GPS定位模塊、挖掘頭的定姿施工控制模塊和挖掘空間位置輔助指示模塊。

為了保證施工挖掘高效順利進(jìn)行，挖掘機(jī)操作室服務(wù)器端已經(jīng)提供挖掘頭當(dāng)前位置下方的路段地質(zhì)數(shù)據(jù)、采場(chǎng)邊界和相關(guān)測(cè)量數(shù)據(jù)等。現(xiàn)場(chǎng)應(yīng)用測(cè)試時(shí)，只需驗(yàn)證挖掘機(jī)坐標(biāo)和鏟斗的坐標(biāo)準(zhǔn)確性。該系統(tǒng)利用載波相位差分系統(tǒng)進(jìn)行定位，完成了整個(gè)挖掘機(jī)的精確定位和工作裝置的姿態(tài)監(jiān)測(cè)。

進(jìn)行現(xiàn)場(chǎng)驗(yàn)證時(shí)采用標(biāo)準(zhǔn)差分系統(tǒng)，在相同坐標(biāo)系統(tǒng)、相同差分信息、相同坐標(biāo)轉(zhuǎn)換參數(shù)下進(jìn)行測(cè)量對(duì)比驗(yàn)證，驗(yàn)證結(jié)果如表3和表4所示。

表3 挖掘設(shè)備終端監(jiān)測(cè)挖掘機(jī)平面坐標(biāo)(WGS-84)

表4 載波相位差分技術(shù)測(cè)量挖掘機(jī)平面坐標(biāo)(WGS-84)

挖掘機(jī)的平面坐標(biāo)可以用它與本初子午線與0°緯線的相對(duì)位置(°)來表示。根據(jù)表3和表4挖掘機(jī)設(shè)備平面坐標(biāo)(WGS-84)對(duì)比可知，挖掘機(jī)平面坐標(biāo)終端檢測(cè)與利用載波相位差分技術(shù)測(cè)量的坐標(biāo)數(shù)值非常接近，檢測(cè)經(jīng)緯度在秒級(jí)稍有差別，經(jīng)度(E)最大相差0.07″，緯度(N)最大相差0.08″，檢測(cè)精度非常高。

對(duì)比表5和表6所示的挖掘機(jī)終端監(jiān)測(cè)鏟斗坐標(biāo)與利用載波相位差分技術(shù)測(cè)量的坐標(biāo)可以看出，鏟斗三維坐標(biāo)數(shù)值平面位置相差最大值為 0.085 m，垂直相差最大值為0.091 m，滿足生產(chǎn)應(yīng)用的需求，表明該裝置應(yīng)用數(shù)據(jù)可靠。

表5 挖掘設(shè)備終端監(jiān)測(cè)鏟斗坐標(biāo)(北京 54)

表6 利用載波相位差分技術(shù)測(cè)量鏟斗坐標(biāo)(北京 54)

對(duì)挖掘設(shè)備終端監(jiān)測(cè)與提出的載波相位差分方法的結(jié)果進(jìn)行比較，三維擬合區(qū)別不明顯。通過Matlab軟件對(duì)X(m)、Y(m)、H(m)坐標(biāo)分別進(jìn)行擬合處理,由于數(shù)據(jù)數(shù)值比較大(超過6位)，擬合不方便，因此只對(duì)十分位進(jìn)行數(shù)據(jù)擬合對(duì)比，效果明顯。

通過Matlab軟件，對(duì)X坐標(biāo)進(jìn)行線性擬合，挖掘設(shè)備終端監(jiān)測(cè)鏟斗X坐標(biāo)線性擬合方程為

f(X)=0.06191X+0.3476

利用載波相位差分技術(shù),鏟斗X坐標(biāo)線性擬合方程為

f′(X)=0.0231X+0.3179

同理，對(duì)Y坐標(biāo)通過兩種方法得到的線性擬合方程分別為

f(Y)=0.03776Y+0.4244f′(Y)=0.04439Y+0.4129

對(duì)H坐標(biāo)的兩種方法得到的線性擬合方程分別為

f(H)=-0.1216H+0.359f′(H)=-0.09705H+0.3415

相關(guān)擬合結(jié)果如圖5～圖7所示。圖中，X1、Y1、H1分別表示挖掘設(shè)備終端監(jiān)測(cè)鏟斗的X坐標(biāo)、Y坐標(biāo)和H坐標(biāo)；X2、Y2、H2分別表示利用載波相位差分技術(shù)測(cè)量鏟斗的X坐標(biāo)、Y坐標(biāo)和H坐標(biāo)。

圖5 水平位置(X坐標(biāo))結(jié)果比較

圖6 水平位置(Y坐標(biāo))結(jié)果比較

圖7 高度位置結(jié)果比較

由圖5～圖7可以看出，其中無論是鏟斗三維坐標(biāo)數(shù)值平面位置還是高度定位誤差，均優(yōu)于0.1 m，達(dá)到目前國(guó)內(nèi)領(lǐng)先水平。試驗(yàn)結(jié)果表明基于載波相位差分技術(shù)的水平定姿挖掘裝置是可行的，且實(shí)時(shí)性和高效性滿足挖掘機(jī)自動(dòng)化控制的要求。

4 結(jié)束語(yǔ)

基于載波相位差分技術(shù)定姿的基本原理，實(shí)現(xiàn)對(duì)挖掘機(jī)的精確定位以及工作裝置的姿態(tài)檢測(cè)。整套裝置進(jìn)行施工作業(yè)時(shí)可保證挖掘機(jī)平面坐標(biāo)以及鏟斗坐標(biāo)準(zhǔn)確，定位精度高，包括對(duì)目標(biāo)位置、鏟斗位置、施工邊界、挖掘機(jī)位置和姿態(tài)及施工范圍圖形化顯示。本裝置滿足了現(xiàn)在工業(yè)施工中挖掘機(jī)定位高精度、高實(shí)時(shí)性和高可靠性的需求，顯著提高了挖掘效率，為科學(xué)開展工農(nóng)業(yè)挖掘工作提供了一種可供參考的技術(shù)方案。