農機不間斷組合導航系統探究

肖 鵬，周志峰，趙 勇

(上海工程技術大學 機械工程學院，上海 201620)

0 引言

導航定位技術是農業機械實現自動導航駕駛的關鍵技術之一[1]。農業自動導航系統中應用較廣泛的導航定位技術主要有全球衛星導航系統(global navigation satellite system，GNSS)導航、慣性導航、視覺定位導航等[2]。2種或多種導航方式結合稱為組合導航，有利于發揮不同導航方式的優點，揚長避短，使得系統的導航能力、精度、可靠性等顯著提高。文獻[3-8]先后對多種組合導航在農業機械導航中的應用進行了研究。文獻[5]提出建立由差分全球定位系統(differential global positioning system，DGPS)、陀螺儀和加速度計組成的導航系統，利用卡爾曼濾波的方法對農機的位置、速度和航向角進行估計，為農機提供可靠的導航信息。文獻[6]提出利用全球定位系統(global positioning system，GPS)和慣性導航系統(inertial navigation system，INS)組合導航技術獲取載體導航信息，根據預設軌跡參數計算出載體的目標前輪轉向角，以該目標前輪轉向角與當前實際轉角的差值作為控制輸入，實現載體的路徑跟蹤控制。文獻[7]提出了GPS和捷聯式慣性導航系統(strapdown inertial navigation system，SINS)聯合導航算法，利用SINS提供的姿態信息修正GPS定位數據，通過多傳感器融合算法準確測量姿態角，減少GPS定位誤差，更真實反映農用車輛的運動狀態。文獻[8]提出通過GNSS信號質量與航向角度的變化幅度為指導的INS/GNSS組合導航信息融合策略，在直線、轉向行駛過程中，融合航向數據的平均絕對誤差得到了有效控制。

本文針對農機田間作業過程中出現的短時間GNSS信號、實時動態(real-time kinematic，RTK)信號丟失，農機導航系統無法連續工作的問題，采用SINS和GNSS的組合導航方法進行不間斷導航研究，在GNSS信號、RTK信號短時間丟失期間為農機提供導航信息，通過農機實際行駛數據的仿真試驗，驗證了方法的有效性。

1 SINS/GNSS組合導航系統結構

組合導航結構框圖如圖1所示。圖1中，組合導航系統主要由慣導子系統、GNSS子系統和擴展卡爾曼濾波器3部分組成。慣導子系中，陀螺儀、慣性測量單元(inertial measurement unit，IMU)用于測量農機行駛過程中的比力和角速率，SINS解算單元則根據IMU的輸出，解算出SINS系統的導航輸出信息，即姿態、位置和速度。GNSS子系統主要包括基站和GNSS接收機。GNSS接收機根據基站提供的高精度RTK數據，給出高精度位置和速度信息。最后，利用GNSS子系統解算得到的位置、速度與慣導解算出的位置、速度之差作為SINS/GNSS組合濾波器(擴展卡爾曼濾波器)的輸入，得到慣導系統狀態誤差估計值。將狀態誤差估計值的陀螺儀和加速度計漂移反饋給慣導系統以對其進行校正，狀態誤差估計值中的位置和速度誤差對慣導解算后的位置和速度信息進行校準。

圖1 SINS/GNSS位置速度組合導航結構

2 SINS解算

根據IMU輸出的角速率和比力信息，對慣導系統進行解算，從而得到農機的姿態位置以及速度等導航相關信息。傳統的姿態更新算法有歐拉角法、方向余弦法和四元數法。四元數法具有算法簡單、計算量小等優點，在工程實際中常采用。本文基于捷聯式慣導系統，采用四元素法對姿態進行解算。具體解算過程如下：

由四元素對姿態矩陣進行更新計算，四元素微分方程為

(1)

四元素更新算法為

(2)

k+1時刻的姿態角為

(3)

導航坐標系中的比力計算為

(4)

速度微分方程為

(5)

機體的位置可由式(6)求得

(6)

式中：L、λ、h分別代表經度、緯度和高度；RM為橫向曲率半徑，東西方向測得；RN為子午圈曲率半徑，南北方向測得。

3 SINS/GNSS組合濾波器設計

SINS誤差狀態由位置誤差、速度誤差、姿態誤差角、加速度計零偏誤差以及陀螺漂移誤差等組成。本文擴展卡爾曼濾波器以SINS誤差作為系統狀態，狀態方程為

(7)

本文采用SINS解算的位置和速度與GNSS測量到的位置和速度之差作為測量信息。因此，測量方程為

(8)

最后通過擴展卡爾曼濾波，對SINS位置、速度、姿態誤差進行估計，其計算方法為

(9)

GkΔt；Mk為系統量測方程的雅克比矩陣。

4 實驗及結果分析

圖2 東方紅LX704

實驗設計分為2部分：1)采集原始數據；2)根據本文的組合導航方法對采集的原始數據進行仿真試驗，并對試驗結果進行分析。通過駕駛農機在開闊無遮蔽的農田中行走，采集陀螺儀、加速度計以及RTK-GNSS原始數據；利用本文中組合導航的方法對采集的數據進行試驗處理；處理過程中為了驗證該方法能在衛星信號短時丟失時為農機提供可靠的導航信息，人為屏蔽衛星信號；數據經過處理后的結果與RTK-GNSS數據進行對比分析，給出試驗結論。

在采集的多組數據中，根據文獻[9]提出新的特征選擇及分類規則提取方法篩選出一組滿足條件的數據，進行了20次仿真試驗。20次仿真結果數據之間的差異都較小，由此可以判斷該算法穩定。通過長達1 800 s的仿真試驗，來驗證算法是否可靠。衛星信號采樣頻率為10 Hz，農機行駛速度采用農機田間作業時的正常速度是1 m/s。由于農業機械在進行農業作業過程中，橫向位置誤差是作為衡量導航精度的主要參數。因此，本試驗只對位置誤差進行了分析。圖3為試驗的位置結果中的一組。

圖3 導航位置誤差

圖3中橫坐標為實驗時間，縱坐標為誤差精度，其中δL、δλ、δH分別代表SINS/GNSS組合導航算法輸出的東(E)向、北(N)向、天(U)向的位置誤差趨勢曲線。圖3中，在仿真初期，由于SINS導航系統存在較大的初始誤差，天向位置誤差較大，在100 s時，3個方向的位置誤差均收斂。SINS導航系統位置誤差會隨時間不斷的積累。由圖3可知，本文組合導航算法消除了SINS的累積誤差，抑制了導航誤差的發散。

如圖3所示，時間段0～1 400 s沒有對接收到的信號進行人為干擾，為了驗證該算法能在短時間GNSS信號、RTK信號丟失期間提供可靠的導航信息，分別在時間段1 400～1 410 s(時段1)、1 450～1 460 s(時段2)、1 480～1 490 s(時段3)和1 500～1 510 s(時段4)模擬衛星信號丟失的情形。圖3中，信號丟失時間段的局部放大圖如圖4、圖5所示。

圖4 短時衛星信號丟失1

圖5 短時衛星信號丟失2

對4個時段的導航位置誤差最大絕對值進行統計，統計結果如表1所示。

表1 最大導航絕對位置誤差

在農業機械進行開墾、耕作等農業作業過程中橫向位置誤差應小于15 cm，并且在符合農業作業的環境中，GNSS信號、RTK信號丟失是小概率事件，并且丟失的時間小于10 s。由表1可知，在短時間內，由于SINS短期精度高、穩定性好，當信號丟失時間達到10 s時，東、北、天3個方向的位置誤差都在11 cm之內，導航位置精度可以滿足實際農業作業的要求，并且當各時段時間間隔逐較小時(時段3與時段4之間的時間間隔僅有10 s)，該算法也能很好地消除SINS累積誤差，算法的可靠性得到了驗證。

5 結束語

針對農機田間作業環境中，由于遮擋或干擾，GNSS信號、RTK信號短時間丟失，導致農機導航系統無法持續工作的問題。本文對 SINS /GNSS 組合導航方法進行了研究。SINS導航系統誤差作為狀態變量，SINS與RTK-GNSS位置速度之差為觀測變量，采用擴展卡爾曼濾波器對SINS導航誤差進行估計，并利用導航誤差估計結果來消除SINS導航系統隨時間的增加而積累的導航誤差。

利用實驗場地采集的原始數據對算法進行仿真試驗。為了驗證該算法的穩定，對篩選出的最優數據進行了20次仿真試驗。為了驗證算法的可靠性，仿真試驗長達1 800 s，并模擬了不同頻率信號丟失的情形。試驗結果表明：該導航算法穩定、可靠，最大導航位置誤差小于11 cm；可以在短時GNSS信號、RTK信號丟失期間，為農業機械提供可靠的導航參數。