農(nóng)機(jī)不間斷組合導(dǎo)航系統(tǒng)探究

肖 鵬，周志峰，趙 勇

(上海工程技術(shù)大學(xué) 機(jī)械工程學(xué)院，上海 201620)

0 引言

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

本文針對(duì)農(nóng)機(jī)田間作業(yè)過程中出現(xiàn)的短時(shí)間GNSS信號(hào)、實(shí)時(shí)動(dòng)態(tài)(real-time kinematic，RTK)信號(hào)丟失，農(nóng)機(jī)導(dǎo)航系統(tǒng)無法連續(xù)工作的問題，采用SINS和GNSS的組合導(dǎo)航方法進(jìn)行不間斷導(dǎo)航研究，在GNSS信號(hào)、RTK信號(hào)短時(shí)間丟失期間為農(nóng)機(jī)提供導(dǎo)航信息，通過農(nóng)機(jī)實(shí)際行駛數(shù)據(jù)的仿真試驗(yàn)，驗(yàn)證了方法的有效性。

1 SINS/GNSS組合導(dǎo)航系統(tǒng)結(jié)構(gòu)

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

圖1 SINS/GNSS位置速度組合導(dǎo)航結(jié)構(gòu)

2 SINS解算

根據(jù)IMU輸出的角速率和比力信息，對(duì)慣導(dǎo)系統(tǒng)進(jìn)行解算，從而得到農(nóng)機(jī)的姿態(tài)位置以及速度等導(dǎo)航相關(guān)信息。傳統(tǒng)的姿態(tài)更新算法有歐拉角法、方向余弦法和四元數(shù)法。四元數(shù)法具有算法簡(jiǎn)單、計(jì)算量小等優(yōu)點(diǎn)，在工程實(shí)際中常采用。本文基于捷聯(lián)式慣導(dǎo)系統(tǒng)，采用四元素法對(duì)姿態(tài)進(jìn)行解算。具體解算過程如下：

由四元素對(duì)姿態(tài)矩陣進(jìn)行更新計(jì)算，四元素微分方程為

(1)

四元素更新算法為

(2)

k+1時(shí)刻的姿態(tài)角為

(3)

導(dǎo)航坐標(biāo)系中的比力計(jì)算為

(4)

速度微分方程為

(5)

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

(6)

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

3 SINS/GNSS組合濾波器設(shè)計(jì)

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

(7)

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

(8)

最后通過擴(kuò)展卡爾曼濾波，對(duì)SINS位置、速度、姿態(tài)誤差進(jìn)行估計(jì)，其計(jì)算方法為

(9)

GkΔt；Mk為系統(tǒng)量測(cè)方程的雅克比矩陣。

4 實(shí)驗(yàn)及結(jié)果分析

圖2 東方紅LX704

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

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

圖3 導(dǎo)航位置誤差

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

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

圖4 短時(shí)衛(wèi)星信號(hào)丟失1

圖5 短時(shí)衛(wèi)星信號(hào)丟失2

對(duì)4個(gè)時(shí)段的導(dǎo)航位置誤差最大絕對(duì)值進(jìn)行統(tǒng)計(jì)，統(tǒng)計(jì)結(jié)果如表1所示。

表1 最大導(dǎo)航絕對(duì)位置誤差

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

5 結(jié)束語

針對(duì)農(nóng)機(jī)田間作業(yè)環(huán)境中，由于遮擋或干擾，GNSS信號(hào)、RTK信號(hào)短時(shí)間丟失，導(dǎo)致農(nóng)機(jī)導(dǎo)航系統(tǒng)無法持續(xù)工作的問題。本文對(duì) SINS /GNSS 組合導(dǎo)航方法進(jìn)行了研究。SINS導(dǎo)航系統(tǒng)誤差作為狀態(tài)變量，SINS與RTK-GNSS位置速度之差為觀測(cè)變量，采用擴(kuò)展卡爾曼濾波器對(duì)SINS導(dǎo)航誤差進(jìn)行估計(jì)，并利用導(dǎo)航誤差估計(jì)結(jié)果來消除SINS導(dǎo)航系統(tǒng)隨時(shí)間的增加而積累的導(dǎo)航誤差。

利用實(shí)驗(yàn)場(chǎng)地采集的原始數(shù)據(jù)對(duì)算法進(jìn)行仿真試驗(yàn)。為了驗(yàn)證該算法的穩(wěn)定，對(duì)篩選出的最優(yōu)數(shù)據(jù)進(jìn)行了20次仿真試驗(yàn)。為了驗(yàn)證算法的可靠性，仿真試驗(yàn)長(zhǎng)達(dá)1 800 s，并模擬了不同頻率信號(hào)丟失的情形。試驗(yàn)結(jié)果表明：該導(dǎo)航算法穩(wěn)定、可靠，最大導(dǎo)航位置誤差小于11 cm；可以在短時(shí)GNSS信號(hào)、RTK信號(hào)丟失期間，為農(nóng)業(yè)機(jī)械提供可靠的導(dǎo)航參數(shù)。