袁洪良 楊浚宇 唐 睿 杜建偉

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

0 引言

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

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

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

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

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

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

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

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

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

圖2 MEMS-IMU和GNSS模塊

1.2 組合算法設(shè)計

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

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

圖3 機械編排

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

(1)

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

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

選取組合導(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——加速度計測量的比力誤差

將陀螺儀和加速度計誤差建模為一階馬爾可夫過程,因此,離散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——過程噪聲

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

Δt——每個歷元的時間間隔

R——平均曲率半徑

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

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

zk=Hxk+V

(6)

其中

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

V——測量噪聲向量

EKF的更新方程為

(7)

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

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

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

P初始值由實地測試給出,即

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)

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

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

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

(1)零速判斷

對于ZUPT和航向約束兩種算法,判斷農(nóng)機是否處于靜止狀態(tài)是十分重要的問題。這里定義一個新的變量δp為

(8)

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

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

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

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

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

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

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

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

首先,當BDS信號可用時,將BDS給出的速度信息替換為0。因此,觀測量和觀測方程可以改寫為

(9)

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

(10)

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

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

(11)

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

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

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

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

(12)

其中

E=[0 0 1]

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

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

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

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

(13)

(14)

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

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

(15)

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

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

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

(16)

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

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

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

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

2 實驗

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

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

2.1 測試A

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

表1 BDS信號正常時速度誤差的最大值和均方根誤差

圖8 BDS可用時的速度誤差

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

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

表2 BDS信號正常時位置誤差的最大值和均方根誤差

圖9 BDS可用時位置誤差

如圖10和表3所示,BDS/INS組合導(dǎo)航系統(tǒng)可以提供準確的水平姿態(tài)角信息。最大橫滾角誤差為0.265°,對應(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后,對航向角精度的改善不大。應(yīng)用DA之后,BDS/INS/ZUPT/DA將橫滾角的最大誤差減小到0.130°,均方根誤差減小到0.024°,精度提高29%。最大俯仰角誤差減小到0.010°,均方根誤差減小到0.017°,精度提高15%。航向角最大誤差為0.012°,均方根誤差為0.041°,有效抑制了航向角誤差的累積,解決了在靜止時航向角誤差發(fā)散的問題。

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

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

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

2.2 測試B

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

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

圖11 BDS不可用時速度誤差

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

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

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

圖12 BDS不可用時位置誤差

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

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

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

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

3 結(jié)束語

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