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

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

(1.上海工程技術大學 電子電氣工程學院，上海 201620； 2.上海理工大學 光電信息與計算機工程學院，上海 200093； 3.上海淮科智能科技有限公司，上海 201900)

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

1 姿態測量方法

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

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

圖1 姿態測量示意圖

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

(1)

(2)

(3)

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

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

(4)

(5)

(6)

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

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

由兩點間的距離公式：

(7)

(8)

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

(9)

即可得：

(10)

(11)

(12)

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

(13)

結合關節坐標變換與GPS，可推導出上述定位姿態公式，從而確定挖掘頭的位置姿態信息。

2 系統設計

2.1 系統組成

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

設備工作流程圖如圖2所示。

圖2 設備工作流程圖

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

2.2 差分定位系統組成

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

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

圖3 差分定位系統

2.3 軟硬件設計

本系統上位機軟件通過LabVIEW 平臺開發，有4個子界面，分別為設置、GPS數據、GPS圖表和手冊指南，如圖4所示。

圖4 軟件挖掘檢測界面

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

表1 PC發送數據

表2 PLC后臺返回數據

3 實驗與結果分析

選擇一塊空曠施工區域進行試驗，挖掘地段需要考慮崎嶇不平、深淺未知等因素。基于挖掘的特殊性和生產工藝的獨特性，研究基于載波相位差分技術的水平定姿挖掘裝置，系統包括施工區域和挖掘頭的空間GPS定位模塊、挖掘頭的定姿施工控制模塊和挖掘空間位置輔助指示模塊。

為了保證施工挖掘高效順利進行，挖掘機操作室服務器端已經提供挖掘頭當前位置下方的路段地質數據、采場邊界和相關測量數據等。現場應用測試時，只需驗證挖掘機坐標和鏟斗的坐標準確性。該系統利用載波相位差分系統進行定位，完成了整個挖掘機的精確定位和工作裝置的姿態監測。

進行現場驗證時采用標準差分系統，在相同坐標系統、相同差分信息、相同坐標轉換參數下進行測量對比驗證，驗證結果如表3和表4所示。

表3 挖掘設備終端監測挖掘機平面坐標(WGS-84)

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

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

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

表5 挖掘設備終端監測鏟斗坐標(北京 54)

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

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

通過Matlab軟件，對X坐標進行線性擬合，挖掘設備終端監測鏟斗X坐標線性擬合方程為

f(X)=0.06191X+0.3476

利用載波相位差分技術,鏟斗X坐標線性擬合方程為

f′(X)=0.0231X+0.3179

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

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

對H坐標的兩種方法得到的線性擬合方程分別為

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

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

圖5 水平位置(X坐標)結果比較

圖6 水平位置(Y坐標)結果比較

圖7 高度位置結果比較

由圖5～圖7可以看出，其中無論是鏟斗三維坐標數值平面位置還是高度定位誤差，均優于0.1 m，達到目前國內領先水平。試驗結果表明基于載波相位差分技術的水平定姿挖掘裝置是可行的，且實時性和高效性滿足挖掘機自動化控制的要求。

4 結束語

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