全球定位/慣性導(dǎo)航組合導(dǎo)航改進(jìn)UKF算法

韓 博， 胡黃水， 曹學(xué)瑤， 王宏志

(長春工業(yè)大學(xué) 計算機(jī)科學(xué)與工程學(xué)院， 吉林 長春 130012)

0 引 言

目前，單一的導(dǎo)航系統(tǒng)在應(yīng)對不同的場景如市區(qū)或者大型商城，其可靠性大大降低，因此導(dǎo)航的方式也從單一導(dǎo)航演變到組合導(dǎo)航。GPS定位精度雖然高，但其信息不夠全面，頻帶窄，易受干擾。而慣性導(dǎo)航系統(tǒng)(INS)雖然不受外界環(huán)境干擾，但其誤差會隨時間不斷累計，長期精度差。GPS/INS的組合方式則互相彌補(bǔ)了對方缺點，是組合導(dǎo)航中最常用的組合方式[1-3]。

GPS/INS的組合方式分為松組合、緊組合以及深組合。松組合結(jié)構(gòu)簡單，因此操作起來不復(fù)雜，其GPS和INS可獨(dú)立工作，組合方式表現(xiàn)在用GPS修正INS數(shù)據(jù)；緊組合中GPS和INS相互輔助，其基本模式是偽距及偽距率的組合，工程難以實施且計算量大；深組合又叫超緊組合，其將INS的測量值反饋給GPS，并通過GPS和INS組合濾波后的導(dǎo)航信息修正INS[4]。同樣其結(jié)構(gòu)復(fù)雜且計算量大。因此，文中采用松組合方式[5]。

由于標(biāo)準(zhǔn)的卡爾曼濾波(KF)不能處理非線性系統(tǒng)[6]，因此學(xué)者們在KF的基礎(chǔ)上又提出了擴(kuò)展卡爾曼濾波(EKF)、無跡卡爾曼濾波(UKF)、粒子濾波(PF)等。但相比之下，UKF因其精度高且計算量小的特點，被廣泛應(yīng)用在各個領(lǐng)域。但隨著系統(tǒng)維數(shù)和非線性度的增加，其濾波效果也會變差[7]。系統(tǒng)中的不確定性干擾以及自身對誤差模型較敏感，也會導(dǎo)致UKF的濾波精度產(chǎn)生影響[8]。

為了進(jìn)一步提高組合導(dǎo)航精度，學(xué)者們提出一些與其他算法結(jié)合的方式，如文獻(xiàn)[9]提出基于粒子群優(yōu)化的UKF算法，文獻(xiàn)[10]提出神經(jīng)網(wǎng)絡(luò)輔助的GPS/INS組合導(dǎo)航自適應(yīng)UKF算法等。由于算法涉及訓(xùn)練及學(xué)習(xí)，雖然提高了濾波的精度，但大大增加了濾波的計算量。文獻(xiàn)[11]提出基于GPS/INS的自適應(yīng)無跡卡爾曼濾波算法，采用自適應(yīng)窗口對系統(tǒng)噪聲和測量噪聲進(jìn)行修正，同時改進(jìn)次優(yōu)漸消因子，使得狀態(tài)預(yù)測協(xié)方差次優(yōu)漸消因子不需要計算雅克比矩陣求取，最后引入統(tǒng)計量，從而確定模型不確定性檢測閾值。該算法一定程度上提高了濾波精度和收斂性，但對于優(yōu)漸消因子求取過程復(fù)雜，同時加窗的檢測也一定程度上延長了濾波時間。文獻(xiàn)[12]提出一種改進(jìn)的無跡卡爾曼濾波在SINS/GPS組合導(dǎo)航中的應(yīng)用，該算法加入了對觀測量的判斷，并引入抗差因子對異常的觀測量進(jìn)行修正，從而提高系統(tǒng)的濾波精度和魯棒性，但其引入的多個參數(shù)都需要經(jīng)驗取值。文獻(xiàn)[13]提出一種改進(jìn)的UKF在GPS/INS組合導(dǎo)航中的應(yīng)用，在傳統(tǒng)UKF算法上引入最小偏度單形采樣策略,降低了UKF計算量，但其Simga的權(quán)值則需要經(jīng)驗取值。同時引入自適應(yīng)因子調(diào)整過程噪聲，從而降低動態(tài)異常干擾誤差對UKF精度與穩(wěn)定性的影響,未考慮系統(tǒng)噪聲Q以及測量噪聲R。文獻(xiàn)[14]提出一種基于UKF的INS/GPS組合導(dǎo)航系統(tǒng)仿真，通過引入自適應(yīng)因子僅對系統(tǒng)噪聲Q以及測量噪聲R進(jìn)行修正，一定程度上提高濾波精度和收斂速度。

針對以上問題，在UKF的基礎(chǔ)上引入自適應(yīng)因子，根據(jù)系統(tǒng)狀態(tài)確認(rèn)自適應(yīng)因子的值，從而選擇對系統(tǒng)參數(shù)進(jìn)行修正，使得濾波系統(tǒng)具有自我調(diào)節(jié)能力。仿真實驗表明，文中提出的改進(jìn)UKF算法濾波效果優(yōu)于UKF和文獻(xiàn)[14]提出的UKF算法。

1 組合導(dǎo)航

GPS/INS松組合結(jié)構(gòu)框圖如圖1所示。

圖1 GPS/INS松組合結(jié)構(gòu)框圖

將INS與GPS輸出的速度及位置進(jìn)行做差后，將做差的數(shù)據(jù)傳輸?shù)浇M合導(dǎo)航濾波算法中作為輸入量進(jìn)行數(shù)據(jù)融合，經(jīng)過濾波處理后得到的數(shù)據(jù)對INS輸出的速度及位置進(jìn)行修正[15]。

松組合狀態(tài)方程：

Xk=Fk-1*Xk-1+Wk-1,

(1)

Xk-1=[σL，σλ，σh,σve，σvn，σvu,φe，φn，φu,

εbx，εby，εbz,?ax，?ay，?az]，

式中：σL，σλ，σh----位置誤差;

σve，σvn，σvu----速度誤差;

φe,φn,φu----姿態(tài)角誤差;

εbx，εby，εbz----陀螺儀常值漂移;

?ax，?ay，?az----加速度計的零偏;

Fk-1----狀態(tài)轉(zhuǎn)移矩陣;

Wk-1----系統(tǒng)噪聲[16]。

松組合測量方程

Zk=Hk*Xk+Vk,

(2)

式中：Hk----測量轉(zhuǎn)移矩陣;

Vk----測量白噪聲。

選取GPS與INS速度、位置差值為測量方程的觀測值。

2 改進(jìn)算法設(shè)計

2.1 UKF算法

1)選定濾波初值：

(3)

(4)

(5)

(6)

(7)

(8)

λ----比例因子,λ=α2(n+k)-n。

Sigma點的分布狀態(tài)由α值決定，通常取0≤α≤1，文中取α為0.001。

3)確定Sigma點的權(quán)值大?。?/p>

(9)

(10)

(11)

式(10)中的β為狀態(tài)分布參數(shù)，β≥0，調(diào)節(jié)β值可以提高方差的精度，文中取β=2。

(12)

(13)

(14)

(15)

(16)

(17)

6)更新狀態(tài)。計算狀態(tài)預(yù)測與測量估計的協(xié)方差

(18)

計算濾波增益

(19)

狀態(tài)和方差更新：

(20)

(21)

2.2 改進(jìn)UKF算法(IUKF)

IUKF算法通過監(jiān)測系統(tǒng)濾波是否處于正常狀態(tài)，并自發(fā)地進(jìn)行相應(yīng)的調(diào)節(jié)，從而達(dá)到降低系統(tǒng)不確定性對濾波產(chǎn)生的影響。

1)修正濾波參數(shù)Pk|k-1，PZZ,PXZ。

求殘差系數(shù)

ek=Zk-Zk|k-1。

(22)

求自適應(yīng)因子

(23)

修正參數(shù)Pk|k-1，PZZ,PXZ：

(24)

(25)

(26)

2)修正濾波參數(shù)系統(tǒng)噪聲Q和測量噪聲R。

求加權(quán)系數(shù)

(27)

式中：b----遺忘因子，其取值一般為[0.95，0.99]，文中取0.95[17]。

修正參數(shù)Q,R:

Qk=C*((1-dk)Qk-1+

(28)

Rk=C*((1-dk)Rk-1+

(29)

Q,R在系統(tǒng)中是實時變化的，同時由于理論與實際的噪聲數(shù)值存在偏差，當(dāng)系統(tǒng)的不確定性干擾影響正常濾波過程時，也會導(dǎo)致參數(shù)Q,R出現(xiàn)誤差，若不進(jìn)行修正，誤差則會隨時間不斷累加。當(dāng)C不為1時，則通過式(28)、式(29)修正Q,R，減小由于系統(tǒng)不確定性干擾導(dǎo)致Q,R偏大而導(dǎo)致濾波精度下降的問題；當(dāng)C為1時，則正常對Q,R進(jìn)行修正。

3 仿真測試

通過Matlab對文中提出的IUKF算法以及對比算法UKF、文獻(xiàn)[14]提出的UKF算法進(jìn)行仿真；仿真中將文獻(xiàn)[14]提出的方法記為14-IUKF。

初始濾波參數(shù)見表1。

表1 濾波參數(shù)

仿真總時常為500 s,載體初始位置為東經(jīng)125.39°，北緯44.01°，初始速度為0。

位置誤差東-北-天方向的對比仿真和速度誤差東-北-天方向的對比仿真分別如圖2和圖3所示。

從圖2可以看出，在東向、天向，IUKF算法相比UKF及14-IUKF算法，其仿真曲線更加集中分布在虛線內(nèi)，曲線波動幅度小于另外兩種對比算法。在北向，IUKF算法與14-IUKF算法仿真曲線基本都集中分布在虛線內(nèi)，但I(xiàn)UKF算法的波動幅度略小于14-IUKF算法。

從圖3同樣可以看出，在速度誤差的三個方向?qū)Ρ戎?，IUKF算法曲線波動范圍均小于對比算法14-IUKF和UKF。

最大誤差對比和最小誤差對比分別見表2和表3。

表2 最大誤差對比

(a) 東向

(b) 北向

(c) 天向圖2 東-北-天位置誤差對比

(a) 東向

(b) 北向

(c) 天向圖3 東-北-天速度誤差對比

表3 最小誤差對比

從表2和表3最大誤差、最小誤差的數(shù)值對比可以看出，IUKF在速度方面三個方向的誤差波動范圍分別為2.45～-2.24、4.79～-3.86、0.39～-0.30。對比算法UKF在速度方面三個方向的誤差波動范圍分別為4.46～-3.15、5.92～-4.96、0.75～-1.05。對比算法14-IUKF在速度方面三個方向的誤差波動范圍分別為0.76～-7.34、8.11～-3.40、0.16～-0.18。

IUKF在位置方面三個方向的誤差波動范圍分別為12.86～-9.67、12.13～-11.73、3.73～-2.72。對比算法UKF在速度方面三個方向的誤差波動范圍分別為16.78～-14.06、13.62～-13.01、4.59～-3.37。對比算法14-IUKF在速度方面三個方向的誤差波動范圍分別為13.36～-15.52、12.44～-12.21、2.97～-4.27。

均值誤差對比見表4。

表4 均值誤差對比

從表4三種算法的均值誤差對比可以得到，文中提出的IUKF相比UKF，在位置的三個方向上，均值誤差分別降低了30.66%、21.58%、23.36%；在速度的三個方向上，均值誤差分別降低了4.76%、11.01%、20%。提出的IUKF相比14-IUKF在位置的三個方向上，均值誤差分別降低了16.42%、11.57%、16.33%；在速度的三個方向上，均值誤差分別降低了2.25%、3.96%、11.09%。由最大、最小值誤差以及均值誤差進(jìn)一步證實了文中提出的IUKF整體濾波效果更優(yōu)于UKF以及14-IUKF。

仿真時間對比見表5。

表5 仿真時間對比

從表5三種算法的仿真時間上看，IUKF相比UKF僅增加了0.193 s，相比14-IUKF僅增加了0.137 s。

4 結(jié) 語

提出面向GPS/INS組合導(dǎo)航的IUKF算法研究，通過實驗仿真對比IUKF、UKF及14-IUKF的位置誤差、速度誤差可看出，IUKF算法可以有效改善因系統(tǒng)不確定性干擾導(dǎo)致的整體濾波的精度下降及發(fā)散問題。其仿真時間相比對比算法也并未明顯增加，從而也保障了濾波的實時性。因此，文中提出的IUKF應(yīng)用于GPS/INS組合導(dǎo)航中具有一定的研究意義。