宋 杰，晉 帆

(昆明理工大學(xué)信息工程與自動(dòng)化學(xué)院，昆明 650500)

0 引言

現(xiàn)代主要的導(dǎo)航系統(tǒng)有捷聯(lián)慣導(dǎo)系統(tǒng)(SINS)、衛(wèi)星全球定位系統(tǒng)(GPS)、無線電導(dǎo)航和組合導(dǎo)航系統(tǒng)等。其中SINS有誤差積累的問題，而GPS的信號(hào)容易受到遮蔽和干擾。對(duì)于高速運(yùn)動(dòng)和長(zhǎng)時(shí)間飛行的載體，用單一的導(dǎo)航系統(tǒng)，很難達(dá)到理想的導(dǎo)航精度。為了提高導(dǎo)航的精度，采用將幾種導(dǎo)航系統(tǒng)組合起來，能有效提高導(dǎo)航的精度，更好的滿足載體對(duì)導(dǎo)航系統(tǒng)的要求[1－2]。

在組合導(dǎo)航系統(tǒng)中，如何將數(shù)據(jù)進(jìn)行有效的融合成為一個(gè)關(guān)鍵問題。常用的方法是卡爾曼濾波(Kalman filter，KF)，卡爾曼濾波是一種最小方差估計(jì)的方法。比較常用的還有Sage-Husa自適應(yīng)卡爾曼濾波，利用觀測(cè)數(shù)據(jù)進(jìn)行濾波遞推的同時(shí)，通過時(shí)變?cè)肼暪乐灯?，?shí)時(shí)估計(jì)和修正系統(tǒng)噪聲和量測(cè)噪聲的統(tǒng)計(jì)特性。Sage-Husa自適應(yīng)卡爾曼濾波的濾波精度要優(yōu)于卡爾曼濾波，但是Sage-Husa自適應(yīng)卡爾曼濾波容易出現(xiàn)濾波發(fā)散，其中穩(wěn)定性和收斂性不能完全保證。文中主要針對(duì)Sage-Husa自適應(yīng)卡爾曼濾波的發(fā)散進(jìn)行研究，采用一種改進(jìn)的自適應(yīng)卡爾曼濾波抑制濾波發(fā)散，并且通過與卡爾曼濾波器的比較來驗(yàn)證濾波效果。

1 SINS/GPS組合導(dǎo)航原理

捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)的核心是用計(jì)算機(jī)來實(shí)現(xiàn)的慣性平臺(tái)。SINS用捷聯(lián)陀螺儀測(cè)量的載體角速度解算姿態(tài)矩陣，從姿態(tài)矩陣的元素中提取載體的姿態(tài)和航向信息;并用姿態(tài)矩陣把加速度計(jì)的輸出從載體坐標(biāo)系變換到導(dǎo)航坐標(biāo)系，然后進(jìn)行導(dǎo)航解算[1]。在SINS中，對(duì)于高速運(yùn)行和長(zhǎng)時(shí)間的飛行后，其積累誤差越來越大，使精度降低。在組合導(dǎo)航系統(tǒng)中，需要借助GPS的輸出來提高組合導(dǎo)航系統(tǒng)的精度。

在文中，導(dǎo)航坐標(biāo)系采用東－北－天地理坐標(biāo)系。在組合導(dǎo)航系統(tǒng)中，其組合方式是采用位置速度組合的方式，用GPS和SINS輸出的位置和速度信息的差值作為觀測(cè)量，經(jīng)組合的濾波器估計(jì)慣性導(dǎo)航系統(tǒng)的誤差，然后對(duì)慣性導(dǎo)航系統(tǒng)進(jìn)行校正。SINS/GPS組合導(dǎo)航系統(tǒng)的結(jié)構(gòu)圖如圖1。

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

2 抑制濾波發(fā)散

2.1 常規(guī)卡爾曼濾波

組合導(dǎo)航系統(tǒng)離散數(shù)學(xué)模型的狀態(tài)方程和量測(cè)方程：

式中：Xk為k時(shí)刻的18維狀態(tài)向量，也是被估計(jì)向量;Zk為k時(shí)刻的6維量測(cè)向量;φk為k－1到 k時(shí)刻的系統(tǒng)一步轉(zhuǎn)移矩陣(18×18階);Wk－1為k－1時(shí)刻的系統(tǒng)噪聲;Гk－1為系統(tǒng)噪聲矩陣;Hk為k時(shí)刻的量測(cè)矩陣(6×18階);Vk為k時(shí)刻的6維量測(cè)噪聲。系統(tǒng)噪聲是不相關(guān)的高斯白噪聲?？柭鼮V波是一種基于誤差方差最小準(zhǔn)則的遞推線性最小方差估計(jì)算法，其離散卡爾曼濾波方程如式(2)～式(6)：

其中式(2)和式(5)是時(shí)間修正方程，其余3個(gè)方程是量測(cè)修正方程。式中：Rk和Qk分別稱為系統(tǒng)噪聲和量測(cè)噪聲的方差矩陣;Kk為濾波增益矩陣;Pk為估計(jì)均方差矩陣，而Pk|k－1為一步預(yù)測(cè)均方差誤差矩陣。

2.2 自適應(yīng)Sage-Husa濾波算法的改進(jìn)

自適應(yīng)Sage-Husa濾波是在卡爾曼濾波算法的基礎(chǔ)上改進(jìn)的一種算法，為了提高估計(jì)值的精度。在這種濾波方式中，Sage-Husa自適應(yīng)卡爾曼濾波算法的濾波流程與卡爾曼濾波的方式是一樣的。只是在隨著濾波的遞推算法中，實(shí)時(shí)的估計(jì)系統(tǒng)噪聲和量測(cè)噪聲[2－3]。其中時(shí)變?cè)肼暯y(tǒng)計(jì)遞推估值器為：

其中，dk－1=(1－b)/(1－bk)，0＜b＜1為遺忘因子。通常取b的范圍為0.95＜b＜0.995。在對(duì)系統(tǒng)噪聲統(tǒng)計(jì)特性的計(jì)算過程中，由于系統(tǒng)噪聲和量測(cè)噪聲的方差矩陣在遞推的過程中，方差矩陣變得越來越大，造成Pk趨向無窮大，使得濾波發(fā)散，不能再修正系統(tǒng)噪聲和量測(cè)噪聲。

為了抑制濾波的發(fā)散，對(duì)自適應(yīng)Sage-Husa濾波中的Rk和Qk進(jìn)行改正來解決濾波發(fā)散的問題。改進(jìn)濾波算法后，其時(shí)變?cè)肼暯y(tǒng)計(jì)遞推估值器變?yōu)椋?/p>

改進(jìn)后的自適應(yīng)Sage-Husa算法是實(shí)時(shí)計(jì)算濾波過程中的系統(tǒng)噪聲和量測(cè)噪聲，然后用獲得的系統(tǒng)噪聲和量測(cè)噪聲求出當(dāng)前時(shí)刻的系統(tǒng)噪聲和量測(cè)噪聲的方差矩陣，并通過與以前時(shí)刻的系統(tǒng)噪聲和量測(cè)噪聲的方差矩陣計(jì)算出總體的方差矩陣。這樣可以使得方差矩陣Rk和Qk比較穩(wěn)定，使Pk變得越來越大，起不到修正的作用，造成濾波發(fā)散。

3 仿真結(jié)果與分析

文中仿真的過程中，慣性導(dǎo)航系統(tǒng)的陀螺隨機(jī)常數(shù)為 0.1°/h，隨機(jī)漂移均方差為0.1°/h，一階馬爾可夫噪聲驅(qū)動(dòng)白噪聲均方差為0.1°/h，相關(guān)時(shí)間為200s;加速計(jì)一階馬爾可夫噪聲驅(qū)動(dòng)白噪聲的方差為0.5m/s，相關(guān)時(shí)間為100s。GPS接收機(jī)隨機(jī)偽距測(cè)量誤差為50m，隨機(jī)偽距測(cè)量誤差為5m/s。初始位置為北緯30°、東經(jīng)30°，海拔高度為1000m。飛行航跡在前100s內(nèi)以90m/s的速度飛行，在后100s內(nèi)以60m/s的速度飛行。

經(jīng)過仿真之后得到的結(jié)果如圖2～圖6。

通過仿真結(jié)果圖可以看出在前20s內(nèi)，改進(jìn)后的自適應(yīng)濾波器在各項(xiàng)導(dǎo)航參數(shù)的變化范圍比較大;如果采用自適應(yīng)Sage-Husa濾波器的方法進(jìn)行濾波，很容易造成濾波的發(fā)散。但是改進(jìn)后的自適應(yīng)濾波在后續(xù)的時(shí)間內(nèi)沒有表現(xiàn)出發(fā)散現(xiàn)象。比較在經(jīng)度、緯度和高度的位置誤差，可以明顯看出，改進(jìn)后的自適應(yīng)濾波器的濾波精度要高于卡爾曼濾波器，而且比較穩(wěn)定。在東、北和天向速度的濾波過程中，在速度變化時(shí)刻第100s，可以看出自適應(yīng)濾波在此時(shí)刻，東、北和天向速度都有明顯的變化，而在卡爾曼濾波器中則看不到這種變化，符合卡爾曼對(duì)方差總體的估計(jì)。

圖2 經(jīng)度誤差曲線

圖3 緯度誤差曲線

圖4 高度誤差曲線

圖5 東向速度誤差曲線

圖6 天向速度誤差曲線

圖7 天向速度誤差曲線

4 結(jié)論

通過以上的仿真實(shí)驗(yàn)結(jié)果可以看出，經(jīng)過改進(jìn)的自適應(yīng)Sage-Husa濾波器能夠避免濾波的發(fā)散，通過與卡爾曼濾波的比較，能夠獲得較高的精度和比較穩(wěn)定的狀態(tài)。但是在狀態(tài)發(fā)生變化時(shí)，會(huì)引起各項(xiàng)導(dǎo)航參數(shù)精度的波動(dòng)。

[1]劉建業(yè)，曾慶華，趙偉，等.導(dǎo)航系統(tǒng)理論與應(yīng)用[M].西安：西北工業(yè)大學(xué)出版社，2010.

[2]趙俊梅.GPS/SINS組合導(dǎo)航系統(tǒng)濾波算法研究[D].太原：中北大學(xué)，2006.

[3]周媛媛.基于非線性模型的組合導(dǎo)航濾波算法研究[D].上海：上海交通大學(xué)，2008.

[4]宋迎春，陳正陽.GPS/INS組合導(dǎo)航中的自適應(yīng)濾波算法[J].湘潭大學(xué)自然科學(xué)學(xué)報(bào)，2006，28(2)：95－98.

[5]周愛軍，任光，郭模燦，等.一種新的自適應(yīng)估計(jì)方法在組合導(dǎo)航系統(tǒng)中的應(yīng)用[J].中國(guó)造船，2009，50(1)：69－74.