農(nóng)機(jī)BDS/INS組合導(dǎo)航算法研究*

張曉寒，趙景波，董振振

(青島理工大學(xué)，山東青島，266520)

0 引言

起源于西方發(fā)達(dá)國家的“精細(xì)農(nóng)業(yè)”概念當(dāng)前越來越受到國內(nèi)學(xué)者的重視，依托于現(xiàn)代化信息技術(shù)的快速發(fā)展，對提高我國農(nóng)業(yè)機(jī)械智能化程度和加速我國農(nóng)業(yè)現(xiàn)代化進(jìn)程有著巨大作用[1]。自動駕駛技術(shù)符合農(nóng)業(yè)機(jī)械程度智能化的發(fā)展要求，目前結(jié)合我國自主研發(fā)的北斗導(dǎo)航系統(tǒng)，在農(nóng)機(jī)作業(yè)領(lǐng)域得到迅速發(fā)展[2]。研究農(nóng)機(jī)自動駕駛技術(shù)，促進(jìn)農(nóng)業(yè)現(xiàn)代機(jī)械化和信息化融合[3]，是實(shí)現(xiàn)農(nóng)業(yè)現(xiàn)代化、智能化的現(xiàn)實(shí)選擇。

北斗導(dǎo)航系統(tǒng)是我國自主研發(fā)設(shè)計的導(dǎo)航定位系統(tǒng)，農(nóng)業(yè)設(shè)備在BDS/INS輔助作業(yè)時自動導(dǎo)航，具有顯著應(yīng)用效果，在我國自動駕駛領(lǐng)域擁有良好發(fā)展前景[4]。20世紀(jì)70年代初期，美國最早對衛(wèi)星導(dǎo)航領(lǐng)域展開深入研究，形成了現(xiàn)階段全世界通用的GPS導(dǎo)航系統(tǒng)[5]，廣泛應(yīng)用于軍事領(lǐng)域、民用設(shè)施、商業(yè)設(shè)備等領(lǐng)域。我國于2014年憑借北斗系統(tǒng)的優(yōu)越性在世界海事領(lǐng)域取得了極大的尊重[6]，并獲得了國際合法地位；2017年我國獨(dú)立自主研發(fā)的“北斗三號”組網(wǎng)成功[7]。

20世紀(jì)90年代起，歐美等發(fā)達(dá)國家根據(jù)“精細(xì)農(nóng)業(yè)”的概念開始發(fā)展農(nóng)機(jī)導(dǎo)航于無人駕駛技術(shù)；美國最早將GPS導(dǎo)航與農(nóng)業(yè)機(jī)械相融合，隨后在歐美、日本等國家的發(fā)展下，逐步將農(nóng)機(jī)導(dǎo)航技術(shù)產(chǎn)品化并投入市場[8-11]。我國的農(nóng)業(yè)技術(shù)導(dǎo)航起步相對較晚，目前還沒有進(jìn)入產(chǎn)品化階段；2011年劉向鋒對田間拖拉機(jī)路徑規(guī)劃進(jìn)行了深入研究；2013年賈全開發(fā)出農(nóng)業(yè)機(jī)械自動導(dǎo)航監(jiān)控系統(tǒng)；2016年廣州某公司研發(fā)出包含監(jiān)控終端的農(nóng)機(jī)自動導(dǎo)航駕駛系統(tǒng)；隨后多家企業(yè)和研究機(jī)構(gòu)參與到農(nóng)機(jī)導(dǎo)航的研究之中；2018年馬志艷、歐陽方熙等提出了基于視覺與慣性農(nóng)機(jī)組合導(dǎo)航方法，拓展了農(nóng)機(jī)組合導(dǎo)航的研究思路。

本文應(yīng)用北斗定位系統(tǒng)和慣性導(dǎo)航系統(tǒng)相結(jié)合的BDS/INS聯(lián)合導(dǎo)航系統(tǒng)，以BDS/INS兩者速度差值、位置差值為模型，提出改進(jìn)蜂群算法優(yōu)化BP神經(jīng)網(wǎng)絡(luò)輔助卡爾曼濾波的組合導(dǎo)航方法實(shí)現(xiàn)精準(zhǔn)定位，解決復(fù)雜環(huán)境下BDS失調(diào)、INS單獨(dú)導(dǎo)航誤差大的難題。

1 導(dǎo)航系統(tǒng)分析

在BDS/INS組合導(dǎo)航過程中，BDS/INS可以提供載體的位置、速度、姿態(tài)信息。BDS精度較穩(wěn)定無需實(shí)時校正和經(jīng)常維護(hù)；INS系統(tǒng)是完全獨(dú)立的自主式導(dǎo)航系統(tǒng)，不受外界干擾，具有良好的隱蔽性。BDS和INS之間優(yōu)勢互補(bǔ)，可明顯改善導(dǎo)航系統(tǒng)的精度、輸出頻率及增強(qiáng)系統(tǒng)的可靠性。

在INS系統(tǒng)中，陀螺儀的角速度更新農(nóng)機(jī)的姿態(tài)信息，即航向角[12]。初始速度、初始位置已知時，將加速度計測得的加速度經(jīng)過一次積分得到運(yùn)載體的線速度，再經(jīng)過一次積分得到載體相對導(dǎo)航坐標(biāo)系的位置信息。

當(dāng)作業(yè)農(nóng)機(jī)遇到障礙物信號遮擋時，BDS信號會減弱或丟失，無法給予滿足精度的位置與速度值信息[13]，此時農(nóng)機(jī)組合導(dǎo)航系統(tǒng)單獨(dú)依靠INS系統(tǒng)進(jìn)行定位。INS單獨(dú)導(dǎo)航時，系統(tǒng)自身的積分原理使INS的速度、位置誤差隨時間積累，長時間工作會導(dǎo)致定位信息發(fā)散[14]。

BDS/INS組合導(dǎo)航時，BDS對INS的位置誤差、速度誤差進(jìn)行補(bǔ)償，BDS信息作為INS的量測輸入，校正加速度的積分累計問題，完成精確定位。

本文引入BP神經(jīng)網(wǎng)絡(luò)輔助INS系統(tǒng)。當(dāng)BDS信號良好時，采用BP神經(jīng)網(wǎng)絡(luò)輔助卡爾曼濾波完成訓(xùn)練，減少模型不準(zhǔn)確造成的誤差；當(dāng)BDS信號失鎖情況下利用BP神經(jīng)網(wǎng)絡(luò)進(jìn)行預(yù)測，此時BP神經(jīng)網(wǎng)絡(luò)替代BDS校正INS系統(tǒng)。

2 導(dǎo)航算法研究

2.1 卡爾曼濾波

BDS/INS組合導(dǎo)航中使用算法是基于卡爾曼濾波理論的相關(guān)算法，通過組合導(dǎo)航系統(tǒng)的誤差濾波模型估計慣性導(dǎo)航誤差值，進(jìn)而利用誤差估計值對慣性導(dǎo)航系統(tǒng)進(jìn)行修正，使系統(tǒng)導(dǎo)航精準(zhǔn)[15]。

本文研究模型為BDS和INS的速度與位置差值，將BDS與INS的速度差值、位置差值經(jīng)過卡爾曼濾波器處理，得到組合導(dǎo)航誤差估計值；然后利用誤差估測值反饋校正INS系統(tǒng)，得到最優(yōu)期望輸出。本文在東北天坐標(biāo)系下建立模型[16]，以INS的誤差量作為狀態(tài)量，BDS與INS的速度差值、位置差值作為量測量。

1) 狀態(tài)方程。INS的誤差狀態(tài)向量由與BDS接收機(jī)位置誤差和東北天三方向速度誤差組成，姿態(tài)誤差為東北天方向平臺失準(zhǔn)角，加速度計零偏誤差，以及陀螺儀漂移誤差組成。通過結(jié)合平臺誤差角方程、速度誤差方程、位置誤差方程與加速度計和陀螺儀誤差，得到INS的誤差狀態(tài)方程

(1)

FI(t)——系統(tǒng)矩陣；

XI(t)——系統(tǒng)狀態(tài)向量；

GI(t)——系統(tǒng)噪聲矩陣；

WI(t)——系統(tǒng)噪聲向量。

2) 量測方程。將INS與BDS測得的位置差值、速度差值信息作為量測信息。位置信息包括經(jīng)度、緯度、高度信息，速度信息為所建東北天模型中東、北、天方向速度誤差。結(jié)合INS系統(tǒng)解算出的位置信息與BDS解算出的位置信息可得到位置的量測方程；進(jìn)一步結(jié)合INS系統(tǒng)解算出的速度信息和BDS解算出的速度信息得到速度的量測方程。將位置的量測方程與速度的量測方程結(jié)合得到BDS/INS組合模式下，組合導(dǎo)航的量測方程

=H(t)X(t)+V(t)

(2)

式中：Z(t)——組合導(dǎo)航測量矢量；

X(t)——系統(tǒng)狀態(tài)向量；

H(t)——量測矩陣；

V(t)——量測噪聲；

p——位置分量；

v——速度分量。

3) 卡爾曼濾波校正。BDS/INS聯(lián)合導(dǎo)航系統(tǒng)為非線性系統(tǒng)，在系統(tǒng)的狀態(tài)方程和觀測方程的基礎(chǔ)上，卡爾曼濾波算法可表示為以下7個步驟。

第一步：狀態(tài)一步預(yù)測

(3)

Φk/k-1——k-1時刻到k時刻的狀態(tài)轉(zhuǎn)移矩陣；

第二步：狀態(tài)一步預(yù)測均方誤差

(4)

式中:Qk——k時刻系統(tǒng)噪聲方差矩陣；

Γk——k時刻系統(tǒng)過程噪聲。

第三步：利用k-1時刻協(xié)方差值，計算k時刻協(xié)方差一步預(yù)測值[17]。

(5)

第四步：濾波增益

(6)

式中:Hk——k時刻量測矩陣。

第五步：狀態(tài)估計

(7)

式中:Zk——k時刻量測向量。

第六步：狀態(tài)估計均方誤差

(8)

式中:Rk——k時刻量測噪聲方差矩陣。

第七步：通過不斷迭代，更新各時刻狀態(tài)估計值。

但是卡爾曼濾波也存在自身的缺陷。當(dāng)增益計算維數(shù)很大時，誤差會隨時間積累而發(fā)散[18]；當(dāng)系統(tǒng)參數(shù)不準(zhǔn)確時，影響狀態(tài)估計產(chǎn)生較大誤差，濾波增益矩陣失去準(zhǔn)確的計算能力[19]，使得卡爾曼濾波計算結(jié)果不準(zhǔn)確，系統(tǒng)失去準(zhǔn)確調(diào)控功能[20]。

2.2 神經(jīng)網(wǎng)絡(luò)輔助卡爾曼濾波

為解決BDS信號干擾失調(diào)問題，本文引入BP神經(jīng)網(wǎng)絡(luò)算法(圖1)。當(dāng)BDS信號正常時，BDS/INS組合導(dǎo)航系統(tǒng)對BP神經(jīng)網(wǎng)絡(luò)進(jìn)行訓(xùn)練；當(dāng)BDS信號中斷時，BP神經(jīng)網(wǎng)絡(luò)輸入發(fā)生變化，INS的加速度計輸出及誤差作為BP神經(jīng)網(wǎng)絡(luò)的輸入，預(yù)測相關(guān)速度誤差與位置誤差。

圖1 神經(jīng)網(wǎng)絡(luò)訓(xùn)練模式和預(yù)測模式Fig. 1 Neural network training mode and prediction mode

2.3 蜂群算法優(yōu)化BP神經(jīng)網(wǎng)絡(luò)

為改善BP神經(jīng)網(wǎng)絡(luò)，將算法訓(xùn)練結(jié)果收斂到均方誤差全局最小極值，避免輸出陷入局部極小值，使用蜂群算法優(yōu)化BP神經(jīng)網(wǎng)絡(luò)[21]。

2.3.1 人工蜂群算法

用人工蜂群算法(ABC-BP)為蜂群在D維權(quán)值空間求解。采蜜蜂與觀察蜂的數(shù)量相等，等于食物源的數(shù)量，設(shè)為SN。食物源的位置為問題的可能解，采蜜蜂對應(yīng)相應(yīng)的食物源，食物源的吸引度為解的適應(yīng)度。

第i個采蜜蜂搜尋食物源

(9)

式中:φid——區(qū)間(-1,1)上的隨機(jī)數(shù)。

標(biāo)準(zhǔn)ABC算法根據(jù)蜂群的貪婪法則，比較新生成的解X′i={x′i1,x′i2,…,x′id}和原來的解Xi={xi1,xi2,…,xid},依據(jù)食物源質(zhì)量保留其中更好的解。

觀察蜂選擇食物源的概率

(10)

式中:fiti——可能解的適應(yīng)值。

當(dāng)蜂群中采蜜蜂和觀察蜂完成D維空間搜索后，如果食物源的適應(yīng)值在limit步驟內(nèi)沒有提高，則放棄此食物源。此食物源對應(yīng)的采蜜蜂變?yōu)閭刹旆洌瑐刹旆渫ㄟ^式(11)搜索新可能解。

(11)

2.3.2 改進(jìn)人工蜂群算法

通過改進(jìn)采蜜蜂的搜索食物方式和觀察蜂選擇食物源概率，進(jìn)一步改進(jìn)蜂群算法優(yōu)化BP神經(jīng)網(wǎng)絡(luò)，組成改進(jìn)ABC-BP模型。

1) 改進(jìn)搜尋食物源方式。經(jīng)研究，采蜜蜂與觀察蜂以式(10)搜索食物源，在原有食物源的附近隨機(jī)搜索獲得新食物源，并引入全局最優(yōu)解加快收斂速度。采蜜蜂搜索公式如式(12)所示。

(12)

式中:r1——區(qū)間(0,1)間隨機(jī)數(shù)；

r2——區(qū)間(0,1.5)間隨機(jī)數(shù)；

yd——全局最優(yōu)解的第d個變量。

引入全局最優(yōu)解，雖然使蜂群迅速向適應(yīng)度高的食物源集中，提高搜尋解的收斂速度，但破壞了蜂群種群的多樣性，易使得搜索最優(yōu)解變?yōu)榫植孔钚≈怠Ｋ赃M(jìn)一步引入調(diào)節(jié)因子w1與w2，改進(jìn)蜂群算法。此時采蜜蜂搜索

w2×r2×(yd-xid)

(13)

(14)

(15)

式中:iter——當(dāng)前的迭代次數(shù)；

maxcycle——最大迭代次數(shù)；

wmin——調(diào)整因子的最小值；

wmax——調(diào)整因子的最大值。

開始階段w2>w1，選中的食物源使采蜜蜂向全局最優(yōu)解靠近的速度較快，搜尋的食物源更快逼近收斂全局最優(yōu)解；后期階段w1>w2，此時采蜜蜂若以較快的收斂速度向最優(yōu)解靠近時，會陷入局部最優(yōu)。因此在實(shí)驗(yàn)后期加大食物源的隨機(jī)性，避免種群陷入局部最優(yōu)。

2) 改進(jìn)觀察蜂選擇概率。標(biāo)準(zhǔn)ABC算法中，觀察蜂選擇食物源時，蜂群在搜索更新時會向適應(yīng)度較高的解運(yùn)動，尋到的解易為局部極值；在常用的選擇概率中，開始階段觀察蜂選擇適應(yīng)度較小的食物源，減小蜂群向更高食物源靠近的速度，保持了種群多樣性，但是算法初期搜索速度較慢。改進(jìn)的蜂群算法使用新的自適應(yīng)判斷因子

(16)

改進(jìn)的選擇概率公式

(17)

根據(jù)φid與τ比較選擇概率pi,保證種群完整性，并大大提高的搜索初期的搜索速度。

3 算法仿真與數(shù)據(jù)分析

3.1 神經(jīng)網(wǎng)絡(luò)輔助BDS/INS導(dǎo)航效果仿真

本文采用MATLAB軟件對BP神經(jīng)網(wǎng)絡(luò)輔助卡爾曼濾波算法組合導(dǎo)航進(jìn)行仿真驗(yàn)證。將試驗(yàn)農(nóng)機(jī)的地理經(jīng)度、緯度、高度和東向、北向、天向的實(shí)際速度作為對象，進(jìn)行仿真測試得出輸出結(jié)果。對顯示結(jié)果與導(dǎo)航數(shù)據(jù)進(jìn)行對比，得到最優(yōu)導(dǎo)航精度。

本文測試軌跡起始點(diǎn)設(shè)為東經(jīng)143.65°，北緯49.26°，海拔高度設(shè)置為100 m；初始東向速度為2 m/s，北向速度為0 m/s；其他姿態(tài)信息均置零。INS系統(tǒng)的仿真誤差參數(shù)如表1所示。

表1 INS系統(tǒng)仿真參數(shù)設(shè)置Tab. 1 INS system simulation parameter setting

本次仿真設(shè)置在前700 s內(nèi)，鎖定組合導(dǎo)航系統(tǒng)的BDS系統(tǒng)，并在此過程中同步BP神經(jīng)網(wǎng)絡(luò)實(shí)時在線訓(xùn)練；700 s后，試驗(yàn)農(nóng)機(jī)收到外界不利環(huán)境影響，此時BDS信號減弱或失鎖，本次仿真設(shè)置為BDS失鎖狀態(tài)。此時將先前700 s訓(xùn)練的BP神經(jīng)網(wǎng)絡(luò)代替BDS系統(tǒng)，BP神經(jīng)網(wǎng)絡(luò)進(jìn)行預(yù)測模式，對INS進(jìn)行補(bǔ)償校正，分別得到東、北、天向速度誤差與經(jīng)、緯、高向位置誤差變化圖。圖2、圖3為東向速度誤差仿真圖和經(jīng)度位置誤差仿真圖。

圖2 神經(jīng)網(wǎng)絡(luò)輔助東向速度誤差仿真圖Fig. 2 Neural network-assisted eastward velocity error simulation

圖3 神經(jīng)網(wǎng)絡(luò)輔助經(jīng)度誤差仿真圖Fig. 3 Neural network assisted longitude error simulation

圖2和圖3中，若在700 s處未受外界不利環(huán)境影響，BDS信號正常時誤差仿真圖如圖中深色線1所示；若在700 s之后受外界不利環(huán)境影響，BDS信號減弱或者失效誤差仿真圖如圖中淺色線2所示。分析誤差數(shù)據(jù)可得，在BDS失鎖后，東向速度誤差在0.5 m/s之內(nèi)，經(jīng)緯度的位置誤差小于30 m。而在無神經(jīng)網(wǎng)絡(luò)校正的實(shí)際生產(chǎn)生活中，純INS在失去BDS信號700 s后普遍速度誤差大于2 m/s，位置誤差大于500 m。證明了使用訓(xùn)練的BP神經(jīng)網(wǎng)絡(luò)進(jìn)行INS校正，將速度誤差和位置誤差控制在與BDS有效時相近的范圍，相比較于單獨(dú)INS導(dǎo)航的速度誤差發(fā)生較為明顯的改善。

3.2 改進(jìn)的ABC-BP模型訓(xùn)練效果仿真

為驗(yàn)證改進(jìn)的人工蜂群算法對神經(jīng)網(wǎng)絡(luò)的作用，分別使用ABC-BP和改進(jìn)的ABC-BP算法，以東向速度為對象進(jìn)行訓(xùn)練仿真，設(shè)置最大的迭代次數(shù)為100次，利用目標(biāo)函數(shù)和適應(yīng)度值，對優(yōu)化后的參數(shù)進(jìn)行訓(xùn)練，訓(xùn)練上限為300次。ABC-BP模型和改進(jìn)的ABC-BP模型訓(xùn)練試驗(yàn)仿真結(jié)果如圖4、圖5所示。

圖4 ABC-BP訓(xùn)練結(jié)果Fig. 4 ABC-BP training results

圖5 改進(jìn)ABC-BP訓(xùn)練結(jié)果Fig. 5 Improve ABC-BP training results

通過對比圖4、圖5可以看出，ABC-BP模型在第30次迭代左右達(dá)到收斂，最終誤差約為0.08；改進(jìn)的ABC-BP模型在23次迭代左右達(dá)到收斂，最終誤差約為0.04。可見改進(jìn)的ABC-BP模型相比于的ABC-BP模型，收斂速度快，且精度較高，克服了算法中存在的搜索能力較弱難以跳出局部最優(yōu)解的弊端。

3.3 改進(jìn)的ABC-BP模型預(yù)測試驗(yàn)仿真

在一次農(nóng)機(jī)作業(yè)中，讓其沿正東方向直線行駛，測得其實(shí)際的東向速度，并同時用已訓(xùn)練好的ABC-BP模型和改進(jìn)的ABC-BP模型對農(nóng)機(jī)東向速度分別進(jìn)行預(yù)測。兩模型的預(yù)測東向速度與實(shí)際東向速度對比圖如圖6、圖7所示。

圖6 ABC-BP預(yù)測值與實(shí)際值比較Fig. 6 ABC-BP predicted value compared with actual value

圖7 改進(jìn)ABC-BP預(yù)測值與實(shí)際值比較Fig. 7 Improve the comparison of ABC-BP predicted values with actual values

對比圖6、圖7中的數(shù)據(jù)進(jìn)行分析，為評價估計量與被估計量存在的差異程度，分別計算兩種算法各60組數(shù)據(jù)的平均絕對誤差(MAE)和均方誤差(MSE)，結(jié)果如表2所示。

表2 誤差分析Tab. 2 Error analysis

由此可得，使用改進(jìn)的人工蜂群算法誤差相對于之前有了明顯提高，平均誤差小于0.6 m/s，其預(yù)測值更加接近于實(shí)際數(shù)值，預(yù)測精度有了明顯改善。綜上所述，改進(jìn)ABC-BP模型具有更加精確快速的預(yù)測效果。

4 結(jié)論

1) 本文針對農(nóng)機(jī)在環(huán)境干擾情況下BDS失調(diào)、INS單獨(dú)導(dǎo)航誤差大的不足，以BDS/INS兩者間位置、速度差值為模型，提出蜂群算法優(yōu)化BP神經(jīng)網(wǎng)絡(luò)輔助卡爾曼濾波的算法組合導(dǎo)航，精準(zhǔn)定位農(nóng)機(jī)位置速度信息。

2) 使用了BDS/INS組合導(dǎo)航，結(jié)合了兩者短時間內(nèi)定位精度高、定位沒有誤差積累的優(yōu)勢，提高了定位精度及穩(wěn)定性；引入BP神經(jīng)網(wǎng)絡(luò)輔助卡爾曼濾波，在BDS有效時BP神經(jīng)網(wǎng)絡(luò)訓(xùn)練，BDS失效時BP神經(jīng)網(wǎng)絡(luò)預(yù)測速度位置差，避免單獨(dú)INS導(dǎo)航時誤差不斷增大。

3) 改進(jìn)人工蜂群算法，使系統(tǒng)更快更準(zhǔn)確的更新神經(jīng)網(wǎng)絡(luò)的權(quán)值閾值，提高了訓(xùn)練收斂速度，在速度預(yù)測仿真中將平均誤差控制在了0.6 m/s之內(nèi)，改善了農(nóng)機(jī)導(dǎo)航精準(zhǔn)度。