基于低成本慣導(dǎo)和運(yùn)動(dòng)約束的農(nóng)機(jī)高精度定位系統(tǒng)研究

袁洪良 楊浚宇 唐 睿 杜建偉

(1.同濟(jì)大學(xué)電子與信息工程學(xué)院, 上海 201804; 2.中移(成都)產(chǎn)業(yè)研究院, 成都 610041)

0 引言

自主導(dǎo)航農(nóng)機(jī)是實(shí)現(xiàn)智能農(nóng)業(yè)的核心技術(shù)裝備,可以全程參與耕、種、管、收環(huán)節(jié)的生產(chǎn)活動(dòng),達(dá)成提高作業(yè)效率、作業(yè)質(zhì)量、土地利用率和降低勞動(dòng)力成本等目標(biāo)[1-5]。這些自主生產(chǎn)作業(yè)活動(dòng)需要較高的定位精度支持,通常在厘米級(jí)。全球?qū)Ш叫l(wèi)星系統(tǒng)(GNSS)[6-8]可以提供準(zhǔn)確的絕對(duì)定位信息,但具有低頻率和易受環(huán)境影響的缺點(diǎn)。相比之下,微機(jī)電慣性導(dǎo)航系統(tǒng)(MEMS-INS)可以在不受外部環(huán)境干擾的情況下穩(wěn)定地提供相對(duì)定位信息[9-10],但具有誤差累積的局限性。GNSS/INS組合導(dǎo)航系統(tǒng)[11-13]可以通過(guò)擴(kuò)展卡爾曼濾波器(KF)有效地融合來(lái)自每個(gè)子系統(tǒng)的數(shù)據(jù),提供穩(wěn)定的高精度定位信息。因此,越來(lái)越多的GNSS/INS組合系統(tǒng)正在取代GNSS導(dǎo)航系統(tǒng),成為農(nóng)業(yè)機(jī)械自主導(dǎo)航系統(tǒng)的主要組成部分。然而,高性能的IMU成本非常高,影響自主導(dǎo)航農(nóng)機(jī)推廣,因此有必要研究基于低成本IMU的GNSS/INS組合導(dǎo)航系統(tǒng),適應(yīng)農(nóng)業(yè)場(chǎng)景需求。

但是,GNSS/INS組合導(dǎo)航系統(tǒng)應(yīng)用在農(nóng)機(jī)上,存在一些技術(shù)問(wèn)題。比如,大部分農(nóng)機(jī)沒(méi)有懸架系統(tǒng),當(dāng)農(nóng)機(jī)作業(yè)時(shí),必然存在較大的車(chē)體振動(dòng),由此導(dǎo)致GNSS測(cè)量精度下降,方差增大,估計(jì)誤差增加[14],從而影響定位和控制效果。其次,農(nóng)機(jī)在農(nóng)田作業(yè)時(shí),也有可能會(huì)遇到衛(wèi)星信號(hào)遮擋、干擾或收星數(shù)不足等問(wèn)題,導(dǎo)致GNSS短暫中斷,GNSS/INS組合導(dǎo)航系統(tǒng)將退化為純慣性導(dǎo)航系統(tǒng)[15-18],導(dǎo)航誤差會(huì)隨著時(shí)間的推移迅速增長(zhǎng),可能導(dǎo)致農(nóng)機(jī)失控,造成經(jīng)濟(jì)損失。最后,GNSS/INS組合導(dǎo)航系統(tǒng)在低速或零速時(shí)存在航向角誤差不可觀測(cè)的問(wèn)題[19-21],導(dǎo)致航向角隨著時(shí)間發(fā)散。

許多研究中使用神經(jīng)網(wǎng)絡(luò)[22]或者增加外部傳感器[23-24]來(lái)解決GNSS中斷時(shí)定位誤差發(fā)散的問(wèn)題,但是對(duì)于農(nóng)機(jī),使用高算力的人工智能芯片或者增加外部傳感器會(huì)增加系統(tǒng)的成本和復(fù)雜度,不利于在農(nóng)業(yè)應(yīng)用中大范圍推廣。

本文以東風(fēng)DF1004-2型拖拉機(jī)為實(shí)驗(yàn)平臺(tái),設(shè)計(jì)BDS/INS組合導(dǎo)航系統(tǒng)。同時(shí),為了解決BDS信號(hào)中斷時(shí)的定位誤差發(fā)散和組合導(dǎo)航系統(tǒng)中航向角誤差累積問(wèn)題,設(shè)計(jì)零速修正(ZUPT)和航向約束算法。該方法將在不增加其他外部傳感器以及系統(tǒng)成本的基礎(chǔ)上,解決當(dāng)前農(nóng)機(jī)組合導(dǎo)航系統(tǒng)中的固有問(wèn)題,以利于系統(tǒng)在農(nóng)業(yè)中應(yīng)用推廣。

1 BDS/INS組合導(dǎo)航系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)

1.1 硬件架構(gòu)設(shè)計(jì)

硬件架構(gòu)設(shè)計(jì)如圖1所示,包括流動(dòng)站和基準(zhǔn)站?；鶞?zhǔn)站接收來(lái)自北斗衛(wèi)星的導(dǎo)航信號(hào),并解析RTK校正數(shù)據(jù),通過(guò)433 MHz無(wú)線電臺(tái)發(fā)送到安裝在農(nóng)機(jī)上的流動(dòng)站。流動(dòng)站接收來(lái)自北斗衛(wèi)星的導(dǎo)航信號(hào),同時(shí)應(yīng)用來(lái)自基準(zhǔn)站的RTK數(shù)據(jù)解算出高精度定位信息。自動(dòng)駕駛控制器完成INS的機(jī)械編排,并通過(guò)擴(kuò)展卡爾曼濾波(EKF)得出組合定位結(jié)果。

圖1 組合導(dǎo)航系統(tǒng)架構(gòu)

本研究MEMS-IMU模塊為IMU383ZA-400,北斗定位模塊為UB482,如圖2所示。

圖2 MEMS-IMU和GNSS模塊

1.2 組合算法設(shè)計(jì)

BDS/INS組合導(dǎo)航系統(tǒng)算法分為兩部分:INS機(jī)械編排和基于EKF的組合導(dǎo)航算法。

導(dǎo)航坐標(biāo)系定義為北東地坐標(biāo)系,用n表示;農(nóng)機(jī)坐標(biāo)系用b表示;地心地固坐標(biāo)系用e表示;慣性坐標(biāo)系用i表示。機(jī)械編排在n系下進(jìn)行,具體過(guò)程如圖3所示。MEMS-IMU輸出的比力和角速度經(jīng)過(guò)誤差補(bǔ)償后,通過(guò)坐標(biāo)轉(zhuǎn)換矩陣,轉(zhuǎn)化到n系下。其中,比力經(jīng)過(guò)比力方程計(jì)算后可以得到對(duì)應(yīng)的加速度,繼續(xù)對(duì)其進(jìn)行積分,即可獲得由INS推算出的位置和速度信息。角速度則通過(guò)姿態(tài)計(jì)算微分方程對(duì)坐標(biāo)轉(zhuǎn)換矩陣進(jìn)行更新,從而根據(jù)其與姿態(tài)角之間的對(duì)應(yīng)關(guān)系得出姿態(tài)信息。

圖3 機(jī)械編排

對(duì)應(yīng)的微分方程為

(1)

h——農(nóng)機(jī)高度

vD——農(nóng)機(jī)垂向速度

選取組合導(dǎo)航系統(tǒng)的15維狀態(tài)向量為

x=[ΔLΔλΔhδvNδvEδvDδφδθδφ

(2)

式中 ΔL、Δλ、Δh——緯度、經(jīng)度、高度誤差

δvN、δvE、δvD——三軸速度誤差

δθ、δφ、δφ——三軸姿態(tài)誤差

εx、εy、εz——三軸陀螺儀零偏

基于INS的誤差模型[25]在n系下可以表示為

(3)

式中 δrn、δvn、Ψ——位置、速度和姿態(tài)誤差

δgn——重力加速度誤差

δfb——加速度計(jì)測(cè)量的比力誤差

將陀螺儀和加速度計(jì)誤差建模為一階馬爾可夫過(guò)程,因此,離散EKF系統(tǒng)模型可以表示為

xk=Φk,k-1xk-1+Wk-1

(4)

其中

(5)

Φrv=[I3×3Δt]3×3

Φva=[(fn×)Δt]3×3

Φbgbg=diag(e-Δt/Tgb,e-Δt/Tgb,e-Δt/Tgb)

Φbaba=diag(e-Δt/Tab,e-Δt/Tab,e-Δt/Tab)

式中Φk,k-1——狀態(tài)轉(zhuǎn)移矩陣

Wk-1——過(guò)程噪聲

I3×3——3×3單位矩陣

Δt——每個(gè)歷元的時(shí)間間隔

R——平均曲率半徑

Tgb、Tab——陀螺儀和加速度計(jì)的相關(guān)時(shí)間

系統(tǒng)測(cè)量方程可以表示為

zk=Hxk+V

(6)

其中

式中zk、H——系統(tǒng)觀測(cè)量和觀測(cè)矩陣

V——測(cè)量噪聲向量

EKF的更新方程為

(7)

式中K——卡爾曼濾波增益

Q——測(cè)量噪聲協(xié)方差矩陣

P——估計(jì)誤差協(xié)方差矩陣

P初始值由實(shí)地測(cè)試給出,即

P0=diag(0.52,0.52,0.52,0.52,0.52,0.52,0.0872,

0.0872,0.1742,0.000 242,0.000 242,0.000 242,

0.012,0.012,0.012)

因此,整個(gè)BDS/INS組合導(dǎo)航系統(tǒng)框圖如圖4所示。通過(guò)INS機(jī)械編排獲得三軸位置和速度信息之后,將INS與BDS定位結(jié)果間的差值作為位置誤差、速度誤差的觀測(cè)值,利用EKF估計(jì)定位誤差,反饋校正定位結(jié)果。

圖4 BDS/INS組合導(dǎo)航系統(tǒng)框圖

1.3 零速修正和航向約束算法設(shè)計(jì)

(1)零速判斷

對(duì)于ZUPT和航向約束兩種算法,判斷農(nóng)機(jī)是否處于靜止?fàn)顟B(tài)是十分重要的問(wèn)題。這里定義一個(gè)新的變量δp為

(8)

式中 |δp|——相鄰兩個(gè)歷元之間的位置差值

rlat、rlon、rh——BDS給出的當(dāng)前時(shí)刻的緯度、經(jīng)度和高度

r′lat、r′lon、r′h——BDS給出的上一時(shí)刻的緯度、經(jīng)度和高度

Rm、Rn——子午圈和卯酉圈的曲率半徑

當(dāng)|δp|小于設(shè)置的閾值,則判定農(nóng)機(jī)處于靜止?fàn)顟B(tài)并使用ZUPT和航向約束算法。

(2)ZUPT算法設(shè)計(jì)

當(dāng)農(nóng)機(jī)靜止時(shí),其3個(gè)方向的實(shí)際速度為零。然而,來(lái)自GNSS的速度信息存在隨機(jī)誤差。ZUPT通過(guò)將包含隨機(jī)誤差的速度信息替換為零,提高了速度觀測(cè)值的精度,從而提高了組合導(dǎo)航系統(tǒng)的定位精度。此外,當(dāng)GNSS中斷時(shí),ZUPT不會(huì)受到影響,可用于在此期間保持定位精度。

基于這一原理,本文針對(duì)BDS信號(hào)可用和中斷兩種情況,設(shè)計(jì)了ZUPT算法,并通過(guò)改變式(6)中的速度誤差觀測(cè)量來(lái)實(shí)現(xiàn)。

首先,當(dāng)BDS信號(hào)可用時(shí),將BDS給出的速度信息替換為0。因此,觀測(cè)量和觀測(cè)方程可以改寫(xiě)為

(9)

其次,當(dāng)BDS中斷時(shí),組合導(dǎo)航系統(tǒng)會(huì)缺失來(lái)自BDS的位置、速度信息。因此,觀測(cè)量中沒(méi)有位置誤差,但ZUPT可以提供速度誤差觀測(cè)值,此時(shí)系統(tǒng)的觀測(cè)量和觀測(cè)方程可以改寫(xiě)為

(10)

(3)航向角誤差不可觀測(cè)的原因分析

低成本MEMS-IMU,通常存在明顯的噪聲。此外,農(nóng)機(jī)作業(yè)時(shí)也會(huì)產(chǎn)生車(chē)體振動(dòng),從而影響IMU的輸出。與這些外部干擾相比,地球的自轉(zhuǎn)速度和重力加速度誤差很小。因此,在IMU的輸出中可以忽略不計(jì),式(3)中的INS誤差方程可以簡(jiǎn)化為

(11)

式中 δθ、δφ、δφ——橫滾角、俯仰角和航向角誤差

由式(11)可以得出,航向角誤差只能通過(guò)前向和右向的比力來(lái)觀測(cè)。然而,農(nóng)機(jī)通常處于靜止或勻速工作狀態(tài),此時(shí)農(nóng)機(jī)前向和右向的比力均為0,因此航向角誤差不可觀測(cè)。這導(dǎo)致了航向角會(huì)隨時(shí)間發(fā)散。與之相比,橫滾角和俯仰角可以通過(guò)垂向比力來(lái)觀測(cè)。

(4)航向約束算法設(shè)計(jì)

在本文所設(shè)計(jì)的系統(tǒng)中,BDS選用雙天線結(jié)構(gòu)(DA),因此測(cè)量出的偏航角具有較高的精度,當(dāng)BDS信號(hào)可用時(shí),將BDS信息中給出的偏航角與INS推算出的航向角之間的差值作為航向角誤差的觀測(cè)值,可以有效地抑制航向角誤差的累積。因此,將式(9)中的測(cè)量方程擴(kuò)展一維,系統(tǒng)的測(cè)量方程可以改寫(xiě)為

(12)

其中

E=[0 0 1]

式中φI——INS推算出的航向角

φB——BDS信息給出的偏航角

然而,式(12)中利用DA的方法需要確保BDS信號(hào)的可用。在系統(tǒng)中斷導(dǎo)致信號(hào)不可用時(shí),該方法便失去了作用。因此,有必要針對(duì)此種情況設(shè)計(jì)另外的航向約束算法。

按照式(11)的處理方式,式(3)中的姿態(tài)角誤差方程可以改寫(xiě)為

(13)

(14)

式中cφ、cθ、cφ、sφ、sθ和sφ表示cosφ、cosθ、cosφ、sinφ、sinθ和sinφ。

將式(14)代入式(13)中,則航向角誤差方程可以表示為

(15)

由式(15)可以得出,當(dāng)農(nóng)機(jī)靜止在農(nóng)田中時(shí),橫滾角和俯仰角接近于零,因此航向角誤差主要來(lái)源為陀螺儀z軸測(cè)量的角速度誤差。因此,可通過(guò)減小z軸角速度誤差來(lái)抑制航向角誤差的發(fā)散。

當(dāng)農(nóng)機(jī)處于靜止?fàn)顟B(tài)時(shí),z軸角速度的真實(shí)值為0,因此可以將0與陀螺儀z軸角速度的差值作為誤差觀測(cè)值?；诖?設(shè)計(jì)了零角速度修正算法(ZARU),與DA組合使用。

同樣將式(10)中的測(cè)量方程擴(kuò)展一維,可以得到此時(shí)的系統(tǒng)測(cè)量方程為

(16)

綜上所述,整個(gè)系統(tǒng)的示意圖如圖5和圖6所示。當(dāng)BDS信號(hào)可用時(shí),由ZUPT和DA輔助的BDS/INS組合導(dǎo)航系統(tǒng)的框圖如圖5所示。INS推算出的航向角與BDS信息給出的航向角之間的差值作為航向角誤差的觀測(cè)量增加到系統(tǒng)中。

圖5 BDS/INS/ZUPT/DA組合導(dǎo)航系統(tǒng)框圖

圖6 BDS/INS/ZUPT/ZARU組合導(dǎo)航系統(tǒng)框圖

當(dāng)BDS中斷時(shí),由ZUPT和ZARU輔助的BDS/INS組合導(dǎo)航系統(tǒng)框圖如圖6所示。與BDS信號(hào)可用時(shí)不同,這里沒(méi)有位置誤差和航向角誤差的觀測(cè)值,但增加了z軸角速度誤差作為觀測(cè)量。

2 實(shí)驗(yàn)

實(shí)驗(yàn)平臺(tái)采用東風(fēng)DF1004-2型拖拉機(jī)搭建。使用第1節(jié)中的架構(gòu),設(shè)計(jì)并制作了基準(zhǔn)站和流動(dòng)站的印刷電路板(PCB)。基準(zhǔn)站放置在沒(méi)有遮蔽的開(kāi)放區(qū)域,以提供連續(xù)的RTK數(shù)據(jù)。流動(dòng)站安裝在拖拉機(jī)的駕駛艙內(nèi)。BDS雙天線水平放置在車(chē)頂上,基線長(zhǎng)度為1.2 m。電臺(tái)天線安裝在左后輪擋泥板上。實(shí)驗(yàn)平臺(tái)構(gòu)成和測(cè)試現(xiàn)場(chǎng)如圖7所示。

本文提出的算法在農(nóng)機(jī)實(shí)驗(yàn)平臺(tái)上進(jìn)行測(cè)試。測(cè)試分為兩部分:測(cè)試A是驗(yàn)證當(dāng)BDS信號(hào)可用時(shí),ZUPT和DA航向約束對(duì)定位精度的改善;測(cè)試B是驗(yàn)證當(dāng)BDS信號(hào)中斷時(shí)ZUPT和ZARU航向約束對(duì)定位精度的改善。兩組測(cè)試實(shí)驗(yàn)中,在檢測(cè)到農(nóng)機(jī)靜止后,ZUPT和航向約束每隔0.5 s應(yīng)用一次。

2.1 測(cè)試A

當(dāng)BDS信號(hào)可用時(shí),將農(nóng)機(jī)靜止于稻田中,通過(guò)BDS/INS、BDS/INS/ZUPT以及BDS/INS/ZUPT/DA獲得的定位誤差如圖8～10所示。表1中給出了定位誤差的最大值和均方根誤差。

表1 BDS信號(hào)正常時(shí)速度誤差的最大值和均方根誤差

圖8 BDS可用時(shí)的速度誤差

如圖8和表1所示,BDS/INS組合導(dǎo)航系統(tǒng)在3個(gè)方向的最大速度誤差分別為0.028、0.044、0.052 m/s,相應(yīng)的均方根誤差分別為0.010、0.013、0.016 m/s。應(yīng)用ZUPT后,3個(gè)方向上的最大速度誤差分別降至0.022、0.033、0.039 m/s,相應(yīng)的均方根誤差也變得更小,分別為0.007、0.010、0.012 m/s。準(zhǔn)確度分別提高30%、23%和25%。而B(niǎo)DS/INS/ZUPT/DA與BDS/INS/ZUPT相比,其對(duì)速度誤差的影響較小。

如圖9和表2所示,對(duì)于BDS/INS,北向和東向的最大位置誤差分別為0.028 m和0.015 m,最大垂向誤差為0.048 m。3個(gè)方向的均方根誤差分別為0.009、0.005、0.017 m。應(yīng)用ZUPT之后,北向和東向的誤差分別減小到0.012 m和0.010 m。垂直方向的最大誤差減小到0.017 m。3個(gè)方向的均方根誤差分別為0.005、0.003、0.006 m,精度分別提高44%、40%和65%。與速度誤差的改善結(jié)果相同,增加DA不會(huì)顯著影響位置誤差。

表2 BDS信號(hào)正常時(shí)位置誤差的最大值和均方根誤差

圖9 BDS可用時(shí)位置誤差

如圖10和表3所示,BDS/INS組合導(dǎo)航系統(tǒng)可以提供準(zhǔn)確的水平姿態(tài)角信息。最大橫滾角誤差為0.265°,對(duì)應(yīng)的均方根誤差為0.069°;最大俯仰角誤差為0.078°,相應(yīng)的均方根誤差為0.030°,航向角誤差在180s內(nèi)累積到13.582°。應(yīng)用ZUPT后,橫滾角和俯仰角的精度得到了提高。其中,橫滾角的最大誤差減小到0.200°,均方根誤差減小到0.034°,精度提高51%。俯仰角的最大誤差減小到0.016°,均方根誤差減小到0.020°,精度提高33%。此外,增加使用ZUPT后,對(duì)航向角精度的改善不大。應(yīng)用DA之后,BDS/INS/ZUPT/DA將橫滾角的最大誤差減小到0.130°,均方根誤差減小到0.024°,精度提高29%。最大俯仰角誤差減小到0.010°,均方根誤差減小到0.017°,精度提高15%。航向角最大誤差為0.012°,均方根誤差為0.041°,有效抑制了航向角誤差的累積,解決了在靜止時(shí)航向角誤差發(fā)散的問(wèn)題。

表3 BDS可用時(shí)姿態(tài)誤差的最大值和均方根誤差

圖10 BDS可用時(shí)姿態(tài)誤差

綜上所述,BDS/INS組合導(dǎo)航系統(tǒng)可以穩(wěn)定地為農(nóng)機(jī)的自主駕駛提供定位信息。ZUPT通過(guò)提高速度誤差觀測(cè)值的準(zhǔn)確性提高了速度的精度,減小了速度誤差。同時(shí),由于位置是通過(guò)速度積分獲得的,ZUPT改善了速度誤差,因此位置誤差也會(huì)隨之改善。然而,由于航向角誤差不可觀測(cè),而ZUPT并不能提高航向角誤差的可觀測(cè)度,因此不能抑制航向角誤差的發(fā)散。DA通過(guò)提供BDS的偏航角提高了航向角誤差的可觀測(cè)性,從而抑制了組合導(dǎo)航系統(tǒng)中航向角的發(fā)散。同時(shí),由于姿態(tài)誤差之間的耦合,橫滾角和俯仰角的精度也得到了提高。

2.2 測(cè)試B

在本節(jié)的測(cè)試中,農(nóng)機(jī)仍靜止在稻田中。BDS/INS組合導(dǎo)航系統(tǒng)正常運(yùn)行2 s后,人為地屏蔽BDS信號(hào)60 s,以模擬BDS信號(hào)不可用的情況。通過(guò)BDS/INS、BDS/INS/ZUPT和BDS/INS/ZUPT/ZARU獲得的速度誤差、位置誤差和姿態(tài)誤差如圖11～13所示(圖中未完全展示)。BDS信號(hào)中斷的時(shí)刻在圖中用黑色箭頭標(biāo)記。速度誤差、位置誤差和姿態(tài)誤差的最大值和均方根誤差見(jiàn)表4～6。

表4 BDS不可用時(shí)速度誤差的最大值和均方根誤差

圖11 BDS不可用時(shí)速度誤差

如圖11和表4所示,在BDS中斷的60 s內(nèi),速度誤差迅速發(fā)散,3個(gè)方向的最大誤差分別為11.612、20.724、1.307 m/s。相應(yīng)的均方根誤差為3.455、6.225、0.359 m/s,在實(shí)際生產(chǎn)中,這樣的定位結(jié)果顯然是不可用的。應(yīng)用ZUPT后,速度誤差不再發(fā)散,3個(gè)方向上的誤差保持了與BDS信號(hào)可用時(shí)類(lèi)似的精度,最大誤差分別為0.067、0.076、0.053 m/s。相應(yīng)的均方根誤差分別降低到0.021、0.013、0.022 m/s,精度分別提高99%、99%和94%。在此基礎(chǔ)上,進(jìn)一步增加ZARU對(duì)速度沒(méi)有進(jìn)一步的改善。

如圖12和表5所示,在BDS中斷期間,位置誤差隨時(shí)間迅速發(fā)散,最大誤差分別為195.834、332.672、21.741 m。相應(yīng)的均方根誤差分別為56.639、95.574、6.205 m。應(yīng)用ZUPT后,位置誤差的發(fā)散問(wèn)題得到解決。3個(gè)方向的位置誤差在0.06 m以內(nèi),相應(yīng)的均方根誤差分別為0.014、0.018、0.018 m,3個(gè)方向精度均提高90%以上。結(jié)果表明,在沒(méi)有BDS信號(hào)的情況下,ZUPT在農(nóng)機(jī)中的應(yīng)用顯著地改善了位置精度。在ZUPT的基礎(chǔ)上,進(jìn)一步使用ZARU不會(huì)對(duì)位置誤差產(chǎn)生顯著影響。

表5 BDS不可用時(shí)位置誤差的最大值和均方根誤差

圖12 BDS不可用時(shí)位置誤差

如圖13和表6所示,當(dāng)BDS中斷時(shí),姿態(tài)誤差逐漸發(fā)散。3個(gè)姿態(tài)角誤差的最大值分別為4.972°、1.365°和4.685°,相應(yīng)的均方根誤差分別為1.491°、0.487°和1.429°。應(yīng)用ZUPT后,橫滾角和俯仰角的精度顯著提高。兩個(gè)姿態(tài)角的誤差均在0.5°以內(nèi),均方根誤差分別降低到0.103°和0.083°,精度分別提高93%和83%。然而,航向角誤差的改善并不明顯,最大誤差和均方根誤差分別為4.056°和1.209°,誤差發(fā)散的問(wèn)題仍然存在。繼續(xù)增加提出的ZARU算法,最大誤差減小到1.932°,均方根誤差減小到0.629°,對(duì)于航向角誤差的精度具有顯著的改善,準(zhǔn)確度提高48%。同時(shí),保持了橫滾角和俯仰角的精度。

表6 BDS不可用時(shí)姿態(tài)誤差的最大值和均方根誤差

圖13 BDS不可用時(shí)姿態(tài)誤差

通過(guò)對(duì)實(shí)驗(yàn)結(jié)果的分析,可以得出BDS/INS組合導(dǎo)航系統(tǒng)在BDS中斷期間退化為純INS,速度誤差、位置誤差和姿態(tài)誤差均逐漸發(fā)散的結(jié)論。ZUPT可以通過(guò)提供速度觀測(cè)值來(lái)抑制速度誤差的發(fā)散。此外,雖然沒(méi)有位置誤差觀測(cè)值,但位置是通過(guò)速度積分獲得的,因此應(yīng)用ZUPT之后,位置誤差的發(fā)散也可以得到解決。故而可以得出,ZUPT在復(fù)雜的農(nóng)業(yè)生產(chǎn)環(huán)境中應(yīng)用于農(nóng)機(jī)可以達(dá)到令人滿意的精度,這表明本文設(shè)計(jì)的組合導(dǎo)航系統(tǒng)和ZUPT算法具有良好的穩(wěn)定性,能夠滿足農(nóng)業(yè)生產(chǎn)的需要。然而,與BDS信號(hào)可用時(shí)相比,由于缺少位置觀測(cè)值,精度略有降低。同時(shí),ZUPT能夠提高橫滾角和俯仰角的精度,但是因?yàn)閆UPT不能夠提高航向角誤差的可觀測(cè)度,因而不能提高航向角精度。繼續(xù)增加ZARU,航向角誤差明顯改善。

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

采用低成本MEMS-IMU設(shè)計(jì)了BDS/INS組合導(dǎo)航系統(tǒng),并在東風(fēng)DF1004-2型拖拉機(jī)上進(jìn)行了測(cè)試和驗(yàn)證。針對(duì)BDS信號(hào)中斷期間定位誤差發(fā)散以及組合導(dǎo)航系統(tǒng)中航向角誤差不可觀測(cè)的問(wèn)題,設(shè)計(jì)了ZUPT和DA與ZARU結(jié)合的航向約束算法?，F(xiàn)場(chǎng)實(shí)驗(yàn)表明,所提出的方法是有效的,BDS/INS組合導(dǎo)航系統(tǒng)可以為農(nóng)業(yè)生產(chǎn)提供準(zhǔn)確的定位信息。當(dāng)BDS可用時(shí),ZUPT可以將位置、速度和水平姿態(tài)的精度分別提高20%、40%和15%以上,使用DA的航向約束可以將航向角精度提高90%以上;BDS中斷時(shí),ZUPT可以將位置、速度的精度提高90%以上,水平姿態(tài)的精度提高80%以上,使用ZARU的航向約束可以將航向角精度提高40%以上。此外,所提出的方法不需要增加額外的傳感器,降低了農(nóng)機(jī)導(dǎo)航系統(tǒng)的成本,有利于該系統(tǒng)推廣。