戴　卿常允艷

(1.重慶水利電力職業(yè)技術(shù)學(xué)院建筑工程系，重慶永川402100，2.信息工程大學(xué)空間地理信息學(xué)院，河南鄭州 450000；3.中南大學(xué)地球科學(xué)與信息物理學(xué)院，湖南長沙 410083)

?

一種基于MAP噪聲估計器的低成本SINS/GPS的快速UKF算法

戴卿1,2常允艷1,3

(1.重慶水利電力職業(yè)技術(shù)學(xué)院建筑工程系，重慶永川402100，2.信息工程大學(xué)空間地理信息學(xué)院，河南鄭州 450000；3.中南大學(xué)地球科學(xué)與信息物理學(xué)院，湖南長沙 410083)

為進一步提高平方根無跡卡爾曼濾波(SR-UKF)在低成本捷聯(lián)慣導(dǎo)(SINS)/衛(wèi)星導(dǎo)航(GPS)組合系統(tǒng)中的算法精度和實時性，針對系統(tǒng)隨機模型的噪聲統(tǒng)計特征不確定性和協(xié)方差平方根矩陣的復(fù)雜性，結(jié)合最大后驗估計(MAP)和矩陣奇異值分解(SVD)理論建模，提出了一種基于MAP噪聲估計模型的快速UKF算法.通過對SINS/GPS組合導(dǎo)航系統(tǒng)的仿真實驗，研究表明：相比于傳統(tǒng)UKF算法，新算法能夠有效減小噪聲統(tǒng)計模型不確定時對導(dǎo)航精度的制約，提高算法魯棒性，同時降低傳統(tǒng)UKF算法的時間復(fù)雜度，提高數(shù)據(jù)更新實時性.

組合導(dǎo)航；非線性濾波；噪聲統(tǒng)計估計器；SVD奇異值分解；快速UKF算法

現(xiàn)代化導(dǎo)航服務(wù)中，微機電捷聯(lián)式慣性導(dǎo)航(SINS)和衛(wèi)星導(dǎo)航系統(tǒng)(GPS)優(yōu)勢互補的SINS/GPS組合導(dǎo)航技術(shù)具有重量輕、體積小、性能好的特點，實用價值優(yōu)越，被廣泛應(yīng)用于軍事和民用領(lǐng)域.然而導(dǎo)航系統(tǒng)環(huán)境中不確定因素的干擾，會影響導(dǎo)航效果，因此噪聲影響下的非線性系統(tǒng)狀態(tài)估計問題，成為近幾年眾多導(dǎo)航學(xué)者的研究熱點.[1-3]

INS/GPS組合導(dǎo)航的非線性濾波常采用擴展卡爾曼濾波算法(EKF)，通過泰勒一階線性化系統(tǒng)模型來進行近似函數(shù)逼近，但由于截斷誤差的存在可能導(dǎo)致濾波精度降低甚至發(fā)散.[4]確定性采樣濾波算法利用sigma點對函數(shù)概率密度函數(shù)的逼近易于非線性函數(shù)的特點，克服局部線性化和Jacobin矩陣計算的問題，提高濾波精度到二階以上，但卻不能抵制復(fù)雜系統(tǒng)中噪聲的影響.[5]自適應(yīng)抗差UKF算法，利用比例因子調(diào)節(jié)濾波增益，但自適應(yīng)因子的選取需要一定的經(jīng)驗型.[6]為避免因舍入誤差導(dǎo)致的數(shù)值不穩(wěn)定問題，避免矩陣求逆操作，有學(xué)者提出求根UKF算法，提高了算法數(shù)值穩(wěn)定性方面做出了較大貢獻.[7]

本文在此研究基礎(chǔ)之上，通過SVD的奇異值分解引入到平方根狀態(tài)協(xié)方差矩陣計算中，并利用MAP估計下的時變噪聲統(tǒng)計器改善系統(tǒng)在時間更新和量測更新中的噪聲自適應(yīng)能力，得到了帶噪聲估計器的SVD-UKF算法，將該算法應(yīng)用到SINS/GPS組合導(dǎo)航系統(tǒng)中，在保證濾波精度的同時，大大減小了計算負擔(dān).

1　低成本SINS/GPS導(dǎo)航系統(tǒng)

松散耦合系統(tǒng)中慣導(dǎo)和衛(wèi)星導(dǎo)航單元獨立，采用GPS輔助SINS的工作方式，提供具備一定冗余度的高數(shù)據(jù)率位置、速度和姿態(tài)導(dǎo)航信息，是一種較理想的低成本高效導(dǎo)航技術(shù).[8]

圖1　松耦合組合導(dǎo)航系統(tǒng)

1.1松耦合系統(tǒng)狀態(tài)方程

在低成本SINS/GPS松耦合系統(tǒng)中，量測信息采用SINS解算的位置和速度與GPS測量得到的位置和速度之差，無需增加組合系統(tǒng)中GPS狀態(tài)變量，因此該模式下導(dǎo)航系統(tǒng)的狀態(tài)方程可表達如下

(1)

其中FI為對應(yīng)的狀態(tài)轉(zhuǎn)移矩陣，XI為SINS的狀態(tài)變量，包含位置誤差、速度誤差、姿態(tài)誤差角、加速度計零偏誤差、陀螺儀漂移誤差，GI為系統(tǒng)噪聲分配矩陣，WI=[ωgxωgyωgzωaxωayωaz]T為系統(tǒng)噪聲.[9]

1.2松耦合系統(tǒng)量測方程

在松耦合系統(tǒng)量測方程中，量測信息為慣導(dǎo)與GPS的位置差和速度差，位置誤差量測方程和速度量測方程分別為

(2)

(3)

(4)

2　帶噪聲估計器的SVD-UKF算法

在非線性系統(tǒng)中

(5)

(6)

式中狀態(tài)轉(zhuǎn)移矩陣Mk,k-1和觀測矩陣Hk均為非線性，uk和vk狀態(tài)方程的量測方程的噪聲.

產(chǎn)生sigma點2lk-1+1個

(7)

(8)

時間更新：

xbk={xbk,i:xbk,i=Mk,k+1(xak-1,i),i=0,1,…2lk-1}

(9)

(10)

(11)

(12)

其中，

(13)

(14)

wαβk-1,0=wk-1,?0+1+β-α2

(15)

量測更新

(16)

(17)

其中，

(18)

(19)

(20)

σ2k，i

(21)

Γk為閾值，為避免lk過大或過小，設(shè)值域lllklu，時變噪聲估計統(tǒng)計器(次優(yōu)無偏MAP估計器)：

(22)

(23)

(24)

(25)

(26)

3　算法性能分析

基于極大后驗估計原理的帶噪聲統(tǒng)計估計器的UKF算法，通過遞推方程對導(dǎo)航系統(tǒng)噪聲和量測噪聲的數(shù)學(xué)統(tǒng)計特性實時修正與估計，實現(xiàn)噪聲均值和協(xié)方差的無偏估計，最終解決了噪聲統(tǒng)計特性未知或不準確帶來的濾波發(fā)散問題，提高UKF濾波算法的穩(wěn)健性.

4　實驗分析

以地面動態(tài)為載體的濾波仿真系統(tǒng)，動態(tài)數(shù)據(jù)通過動態(tài)仿真與導(dǎo)航傳感器測量誤差數(shù)據(jù)合并的方式獲取，其慣導(dǎo)單元與GPS接收機測量誤差數(shù)據(jù)在實測數(shù)據(jù)中獲得.衛(wèi)星可見數(shù)目如圖2所示與慣導(dǎo)測量狀況如圖3-5所示.GPS測速誤差為0．5m/s，定位誤差為5m，陀螺噪聲的均方根0．5(°)/s，加速度計噪聲的均方根10一3g.慣導(dǎo)輸出周期為0.01s，UKF算法濾波周期為1s，仿真時間1800s，校正方式為反饋校正，校正周期為1s.

圖2　衛(wèi)星可見顯示圖

圖3　翻滾角量測

圖4　俯仰角量測

圖5　航偏角量測

粒子濾波技術(shù)(PF)是貝葉斯遞推框架下的不受非線性和高斯假設(shè)限制的非線性非高斯噪聲系統(tǒng)濾波，計算量較大，但濾波性能好.因此本文為證明新算法的濾波性能，把UKF和PF作為實驗對照項.通過仿真實驗，平面位置誤差結(jié)果如圖6和圖7所示，三種濾波方法在平面位置上的定位結(jié)果較接近，MAP-SVD-UKF和PF略優(yōu)于UKF.速度誤差如圖8和圖9所示，MAP-SVD-UKF和PF輸出精度較高，明顯優(yōu)于UKF算法.姿態(tài)誤差如圖10和圖11所示，通過X和Y方向上的姿態(tài)測量結(jié)果分析發(fā)現(xiàn)，MAP-SVD-UKF和PF明顯優(yōu)于UKF算法.

圖6　東向位置定位誤差

圖7　北向位置定位誤差

圖8　東向速度誤差

圖9　北向速度誤差

圖10　x方向姿態(tài)誤差

圖11　y方向姿態(tài)誤差

由上圖可知，基于MAP噪聲估計器的SVD-UKF濾波算法穩(wěn)定性較好，位置、速度和姿態(tài)誤差較小，導(dǎo)航定位精度較高.新算法雖比PF算法在精度上有一定的損失，但相比傳統(tǒng)UKF算法性能提升較大.因此，M-SVD-UK算法在能提供高精度導(dǎo)航結(jié)果的同時，計算時間復(fù)雜度相比傳統(tǒng)UKF算法和PF算法要小，對動態(tài)導(dǎo)航定位定姿信息的輸出實時性較好.

5　結(jié)論

本文通過基于UKF框架下的松組合SINS/GPS組合導(dǎo)航系統(tǒng)研究，結(jié)合MAP估計準則下的時變噪聲統(tǒng)計估計器模型和SVD奇異值分解算法，對傳統(tǒng)UKF算法進行改進，實現(xiàn)了一種具備噪聲自適應(yīng)估計的快速UKF算法.將本文提出的新算法，應(yīng)用于低成本SINS/GPS組合導(dǎo)航仿真實驗系統(tǒng)中，仿真結(jié)果驗證了新濾波算法的有效性，采用該算法可以在有效改善噪聲影響下濾波定位結(jié)果的同時，提高計算實時性，有效解決高動態(tài)載體的濾波狀態(tài)估計問題.

[1] 楊元喜.自適應(yīng)動態(tài)導(dǎo)航定位[M].北京:測繪出版社,2006：23.

[2] 張秋昭,張書畢,劉志平,等.基于奇異值分解的魯棒容積卡爾曼濾波及其在組合導(dǎo)航中的應(yīng)用[J].控制與決策,2014(2):341-346.

[3] 高博,黃耀光,李建新.基于自適應(yīng)UKF的單站無源定位算法[J].信息工程大學(xué)學(xué)報,2012(5):578-582.

[4] 常國賓,許江寧.一種新的混合迭代UKF[J].武漢大學(xué)學(xué)報：信息科學(xué)版，2012(6):701-703.

[5] 謝愷, 金波, 周一宇. 基于迭代測量更新的UKF方法[J]. 華中科技大學(xué)學(xué)報：自然科學(xué)版,2007(11): 13-16.

[6] 王小旭,趙琳,夏全喜,等.基于Unscented變換的強跟蹤濾波器[J].控制與決策,2010(7):1063-1068．

[7] 崔乃剛,韓鵬鑫,穆榮軍.基于強跟蹤UKF的導(dǎo)航系統(tǒng)故障檢測方法[J].哈爾濱工程大學(xué)報,2011(10):1295-1299.

[8] 楊文博,李少遠.基于強跟蹤UKF的航天器自主導(dǎo)航間接量測濾波算法[J].系統(tǒng)工程與電子技術(shù),2011(11)：2485-2491．

[9] Ge Q b,Xu D X,Wen C L.Cubatureinformationfilterswithcorrelatednoisesandtheirapplicationsindecentralizedfusion[J].Signal Processing,2014(94):434-444．

[責(zé)任編輯范藻]

A Fast UKF Algorithm Based on MAP Noise Estimator for Low Cost SINS / GPS System

DAI Qing1, 2,CHANG YunYan1, 3

(1. Architectural Engineering Deptment of Chongqing Water Resources and Power Technology College, Chongqing 402100;2. Spatial Geography Information School of Information Engineering University, Zhengzhou Henan 450000;3. Earth Science and Information Physics School of South-Center University, Changsha Hunan 410083, China)

To further improve the performance of unscented Kalman filter (UKF) algorithm in SINS/ GPS navigation system, an improved UKF filtering based on the MAP and SVD algorithm is discussed in this paper. The new algorithm is solved the statistical noise characteristics of the system stochastic model uncertainty and complexity of the square root of the covariance matrix. The simulation experiment results of SINS / GPS integrated navigation system shows that compared with the traditional UKF algorithm, the new algorithm implement in SINS/GPS integrated navigation system could improve navigation accuracy, increase system robustness,and speed up the frequency of update data.

SINS/GPS; nonlinear filtering; noise statistics estimator; SVD

2016-05-25

渝水職院重點項目(K201514、2015006、K201510)

戴卿(1985—)，男，河南洛陽人.講師，博士,主要從事非線性非高斯濾波與組合導(dǎo)航數(shù)據(jù)處理研究.

TN96

A

1674-5248(2016)05-0039-05