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

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

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

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

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

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

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

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

(1)

(2)

(3)

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

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

(4)

(5)

(6)

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

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

由兩點間的距離公式：

(7)

(8)

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

(9)

即可得：

(10)

(11)

(12)

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

(13)

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

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

2.1 系統(tǒng)組成

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

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

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

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

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

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

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

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

2.3 軟硬件設(shè)計

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

圖4 軟件挖掘檢測界面

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

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

表2 PLC后臺返回數(shù)據(jù)

3 實驗與結(jié)果分析

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

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

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

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

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

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

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

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

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

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

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

f(X)=0.06191X+0.3476

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

f′(X)=0.0231X+0.3179

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

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

對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)測鏟斗的X坐標(biāo)、Y坐標(biāo)和H坐標(biāo)；X2、Y2、H2分別表示利用載波相位差分技術(shù)測量鏟斗的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，達到目前國內(nèi)領(lǐng)先水平。試驗結(jié)果表明基于載波相位差分技術(shù)的水平定姿挖掘裝置是可行的，且實時性和高效性滿足挖掘機自動化控制的要求。

4 結(jié)束語

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