任 波 李 強(qiáng)，2 趙 晴 李 浩

(沈陽理工大學(xué)裝備工程學(xué)院1，遼寧 沈陽 110159;

中國人民解放軍66347部隊2，河北 保定 071000;中國人民解放軍61267部隊3，北京 101114)

容積粒子濾波在緊耦合SINS/GPS導(dǎo)航系統(tǒng)中的應(yīng)用

任 波1李 強(qiáng)1，2趙 晴1李 浩3

(沈陽理工大學(xué)裝備工程學(xué)院1，遼寧 沈陽 110159;

中國人民解放軍66347部隊2，河北 保定 071000;中國人民解放軍61267部隊3，北京 101114)

緊耦合SINS/GPS導(dǎo)航系統(tǒng)的數(shù)據(jù)融合過程存在高維狀態(tài)和非線性混合模型，采用常規(guī)的線性卡爾曼濾波很難處理;而一般的非線性粒子濾波存在粒子退化和樣本枯竭的問題。對此，提出將容積粒子濾波(CPF)算法應(yīng)用于SINS/GPS組合導(dǎo)航系統(tǒng)。CPF利用容積卡爾曼濾波算法得到粒子濾波(PF)的重要性密度函數(shù)，并將最新量測值融入系統(tǒng)狀態(tài)的轉(zhuǎn)移過程中，由此產(chǎn)生的預(yù)測樣本接近于系統(tǒng)狀態(tài)的真實后驗概率的樣本。仿真結(jié)果表明，CPF算法的估計性能明顯優(yōu)于標(biāo)準(zhǔn)PF。

容積粒子濾波(CPF) SINS/GPS組合導(dǎo)航 非線性系統(tǒng) 卡爾曼濾波 數(shù)據(jù)融合

0 引言

捷聯(lián)慣性導(dǎo)航系統(tǒng)(strap-down inertial navigation system，SINS)具有自主性和抗干擾能力強(qiáng)的優(yōu)點，而全球定位系統(tǒng)(GPS)能提供全球高精度的三維實時導(dǎo)航信息，將SINS和GPS二者相組合，可以實現(xiàn)優(yōu)勢互補，提供高精度、全天候的導(dǎo)航服務(wù)［1］。SINS/GPS組合導(dǎo)航系統(tǒng)在航空航天、精確制導(dǎo)武器、大地測量和智能交通系統(tǒng)等領(lǐng)域具有很好的使用價值，越來越受到人們的關(guān)注。

緊耦合方式是將GPS系統(tǒng)和SINS系統(tǒng)作為整體進(jìn)行設(shè)計，將偽距和偽距率作為觀測量，利用現(xiàn)代濾波方法實現(xiàn)SINS與GPS的組合。在這種情況下，系統(tǒng)方程是非線性的，卡爾曼濾波難以直接處理。粒子濾波(particle filter，PF)直接采用非線性模型，具有較好的估計精度，受到學(xué)者的青睞［2］。但在 SINS/GPS組合導(dǎo)航系統(tǒng)的數(shù)據(jù)處理過程中，需要考慮陀螺加速度計的隨機(jī)誤差、漂移以及GPS的各種噪聲，且系統(tǒng)方程的狀態(tài)維數(shù)多達(dá)20維。隨著維數(shù)的增加，粒子濾波本身的計算量也急劇增加，很多常規(guī)的粒子濾波算法不能滿足實時性的要求［3］。因此，本文采用一種最新提出 的 非 線 性 濾 波——容 積 粒 子 濾 波［4－5］技 術(shù)(cubature particle filter，CPF)來解決緊耦合 SINS/GPS組合導(dǎo)航系統(tǒng)的數(shù)據(jù)融合問題。

1 容積粒子濾波

選擇重要性概率密度是克服標(biāo)準(zhǔn)粒子濾波方法中粒子退化的關(guān)鍵。較之于擴(kuò)展卡爾曼濾波器算法(extended Kalman filter，EKF)和無損卡爾曼濾波(unscented Kalman filter，UKF)，容 積 卡 爾 曼 濾 波(cubature Kalman filter，CKF)更容易產(chǎn)生真實反映函數(shù)分布的重要性概率密度，CKF能夠使建議性密度函數(shù)更加接近于真實后驗概率密度。動態(tài)非線性系統(tǒng)采用以下兩個離散的系統(tǒng)方程表示。

①狀態(tài)方程:

②量測方程:

以上兩式中:xk和zk分別為k時刻系統(tǒng)的狀態(tài)向量和觀測向量;vk和wk分別為相互獨立的系統(tǒng)噪聲和觀測噪聲，wk～N(0，Qk)，vk～N(0，Rk)。

1.1 容積卡爾曼濾波

CKF算法［6］將非線性濾波歸結(jié)為非線性函數(shù)與高斯概率密度乘積的積分求解問題?；诟咚辜僭O(shè)的貝葉斯算法采用2n個等權(quán)值的求容積點實現(xiàn)非線性逼近。在解決高維非線性濾波問題方面，CKF具有較高的數(shù)值穩(wěn)定性和濾波精度。根據(jù)求容積規(guī)則，可得:

CKF框架結(jié)構(gòu)服從貝葉斯濾波結(jié)構(gòu)，其算法流程可參考文獻(xiàn)［6］。

CKF算法流程概括如下。

①初始化

②時間更新

假設(shè)(k－1) 時刻后驗概率分布 p(xk－1|Zk－1)=N(xk－1;x^

k－1|k－1，Pk－1|k－1)已知，采用 Cholesky 或奇異值方法分解協(xié)方差。③測量更新

1.2 容積粒子濾波算法

令xk是均值為、方差為Pk、服從高斯分布函數(shù)N(xk;，Pk)的隨機(jī)變量，p(xk)為狀態(tài) xk的概率分布，p(zk|xi

k)為似然函數(shù)，借鑒文獻(xiàn)［5］～［8］中關(guān)于CPF算法的框架結(jié)構(gòu)，得到CPF的算法流程如下。

①初始化

當(dāng)k=0時，從初始狀態(tài)分布p(x0)中抽取N個權(quán)值為1/N、方差為P0的粒子集。

②重要性采樣

⑤輸出k時刻的狀態(tài)估計和方差估計

2 SINS/GPS緊耦合系統(tǒng)數(shù)學(xué)模型

SINS/GPS緊耦合系統(tǒng)結(jié)構(gòu)如圖1所示。組合導(dǎo)航系統(tǒng)的濾波器接收GPS跟蹤回路提供的偽距和偽距率的測量值，用于生成SINS中的誤差估計;修正后的SINS數(shù)據(jù)用于輔助GPS跟蹤回路，實現(xiàn)對系統(tǒng)的開環(huán)校正或反饋校正;最后將組合導(dǎo)航系統(tǒng)的導(dǎo)航數(shù)據(jù)輸入到需要實現(xiàn)導(dǎo)航定位的實際系統(tǒng)中。

圖1 系統(tǒng)結(jié)構(gòu)框圖Fig.1 Structure of the system

圖1中，綜合輸入是指組合導(dǎo)航系統(tǒng)將GPS與SINS的導(dǎo)航數(shù)據(jù)綜合以后的數(shù)據(jù)輸入到需要實現(xiàn)導(dǎo)航定位的實際系統(tǒng)中。

2.1 狀態(tài)方程

本文采用的數(shù)學(xué)模型建立在東北天地理坐標(biāo)系的基礎(chǔ)上，以陀螺儀加速度計和GPS接收機(jī)的各種誤差為狀態(tài)變量，GPS/SINS 組合系統(tǒng)的狀態(tài)方程［1，9］為:

式中:X(t)∈R20為狀態(tài)變量;F(t)∈R20×20為系統(tǒng)狀態(tài)系數(shù)矩陣;W(t)∈R11為噪聲向量;G(t)∈R20×11為系統(tǒng)噪聲矩陣。

式中:φE、φN、φU為平臺誤差角;δVE、δVN、δVU為速度誤差;δL、δλ、δh 為位置誤差;εbx、εby、εbz為陀螺隨機(jī)漂移;εrx、εry、εrz為一階馬爾可夫過程隨機(jī)噪聲;▽dx、▽dy、▽dz為加速度計隨機(jī)偏差;δtu和δtru為GPS等效時鐘誤差相應(yīng)的距離誤差和距離率誤差。

F(t)20×20的表達(dá)式為:

式中:ωg為陀螺儀白噪聲漂移;ωa為加速度計白噪聲漂移;ωr為陀螺儀一階馬爾可夫驅(qū)動白噪聲;ωtu為等效時鐘誤差對應(yīng)的距離誤差;ωtru為等效時鐘頻率誤差對應(yīng)的速度誤差。

2.2 量測方程

①利用SINS與GPS偽距差組成測量方程

令ρI為由SINS提供的載體位置與衛(wèi)星之間的偽距，ρG為GPS接收機(jī)給出的載體位置與衛(wèi)星之間的偽距，則載體上的GPS接收機(jī)相對于第j個衛(wèi)星的偽距可表示為:

式中:δtu為等效時鐘誤差和衛(wèi)星星歷誤差對應(yīng)的距離率誤差;υρj為偽距測量噪聲，它由多路徑效應(yīng)、相對論效應(yīng)、大氣層延遲和接收機(jī)噪聲等引起。

ρ與ρI由以下公式計算得到:

設(shè)(δx，δy，δz)為 SINS 解算出的載體在地球坐標(biāo)系中的位置誤差，則 xI=x+δx、yI=y+δy、zI=z+δz。

將式(29)在(x，y，z)處進(jìn)行泰勒展開，并忽略二階以上高階項，可以得到:

組合系統(tǒng)的偽距差測量方程為:

②利用SINS與GPS偽距率差組成測量方程

SINS提供的載體位置與衛(wèi)星之間的偽距變化率可表示為:

整理可得:

載體上的GPS接收機(jī)相對于第j個衛(wèi)星的偽距率可表示為:

式中:δtru為GPS時鐘頻率誤差等效的距離率誤差;υρ·j為偽距率量測噪聲。

組合系統(tǒng)偽距率差測量方程為:

3 仿真試驗分析

為驗證CPF算法在SINS/GPS緊耦合導(dǎo)航系統(tǒng)中的濾波效果，利用Matlab軟件進(jìn)行仿真試驗。仿真系統(tǒng)主要由軌跡發(fā)生器、SINS仿真子系統(tǒng)、GPS仿真子系統(tǒng)和CPF組合導(dǎo)航濾波器4部分組成。

設(shè)仿真航跡由平飛、轉(zhuǎn)彎、加速和爬升等狀態(tài)組成，設(shè)定載體初始位置(緯度20°、經(jīng)度110°、高度500 m)，仿真時間為1 600 s，則載體飛行航跡的仿真結(jié)果如圖2所示。

圖2 載體飛行航跡三維圖Fig.2 The three-dimensional graphic of carrier flight path

SINS解算頻率為50 Hz，GPS接收頻率為1 Hz，兩者的組合頻率為1 Hz。利用PF進(jìn)行濾波，得到SINS/GPS導(dǎo)航系統(tǒng)的位置誤差曲線如圖3所示。同理，利用CPF進(jìn)行濾波的位置誤差曲線如圖4所示。

圖3 組合導(dǎo)航PF輸出位置誤差曲線Fig.3 Position error curves of integrated navigation with PF

圖4 組合導(dǎo)航CPF輸出位置誤差曲線Fig.4 Position error curves of integrated navigation with CPF

從圖3、圖4可以看出，在相同條件下，將本文采用的CPF與標(biāo)準(zhǔn)PF進(jìn)行對比，可知CPF對位置的估計誤差較小，且隨著時間的推移，誤差逐漸降低，曲線收斂于坐標(biāo)軸。

仿真試驗采用的位置信息是用“經(jīng)緯高”來表示的，為了便于直觀的對比，將圖3與圖4的經(jīng)緯度誤差轉(zhuǎn)換為東北天方向的、以m為單位的誤差，得到位置誤差均值如表1所示。

表1 位置誤差均值對比Tab.1 Comparison of the mean position errors

從表1可以看出，CPF對位置的估計值更加接近真實值，組合導(dǎo)航系統(tǒng)的定位精度顯著提高。

4 結(jié)束語

本文提出將最新的非線性濾波方法CPF應(yīng)用到緊耦合SINS/GPS導(dǎo)航系統(tǒng)的數(shù)據(jù)融合中，并設(shè)計了典型飛行航跡來驗證其可行性與優(yōu)越性。仿真結(jié)果表明，與標(biāo)準(zhǔn)PF相比，CPF利用CKF為PF提供重要性概率密度，將最新量測值融入到系統(tǒng)狀態(tài)的轉(zhuǎn)移過程中，有效克服了PF的粒子退化問題。CPF算法提高了導(dǎo)航系統(tǒng)的定位精度和濾波精度，實際效果較好。

［1］王惠南.GPS導(dǎo)航原理與應(yīng)用［M］.北京:科學(xué)出版社，2003:207－248.

［2］ Gustafsson F，Gunnarsson F，Bergman N.Particle filters for positioning，navigation，and tracking［J］.IEEE Transactions on Signal Processing，2002，50(2):425－437.

［3］ Bouaynaya N，Qu W，Schonfeld D.An on line motion-based particle filter for head tracking applications［C］//IEEE International Conference on Acoustics，Speech，and Signal Processing，Philadelphia，USA，March，2005:225－228.

［4］ Havlicek M，Jan J，Brazdil M，et al.Nonlinear estimation of BOLD signal based on cubature particle filter［J］.Biosignal，2010(20):328－332.

［5］ Wu Weisheng，Song Chunlei，Wang Junhou，et al.Cubature Gaussian particle filter for initial alignment of strap down inertial navigation system［C］//2010 First International Conference on Pervasive Computing，Signal Processing and Applications，2010:1196－1200.

［6］ Arasaratnam I，Haykin S.Cubature Kalman filters［J］.IEEE Transactions on Automatic Control，2009，54(6):1254－1269.

［7］ Fernandez C，Vila J.Bayesian nonlinear filtering using quadrature and cubature rules applied to sensor data fusion for positioning［C］//IEEE International Conference on Communications，Cape Town，South Africa，July，2010.

［8］穆靜，蔡遠(yuǎn)利，張俊敏.容積粒子濾波算法及其應(yīng)用［J］.西安交通大學(xué)學(xué)報:自然科學(xué)版，2011，45(8):13－17.

［9］劉建業(yè)，曾慶化，趙偉，等.導(dǎo)航系統(tǒng)理論與應(yīng)用［M］.西安:西北工業(yè)大學(xué)出版社，2010:350－355.

Application of Cubature Particle Filter in Tightly Coupled SINS/GPS Navigation System

Due to high dimensional state and nonlinear mixed model exist in data fusion of SINS/GPS tightly coupled navigation system，conventional linear Kalman filter is incompetent;while general nonlinear particle filter(PF)features particle degradation and sample depletion problems，thus the cubature particle filter(CPF)algorithm is applied in SINS/GPS integrated navigation system.With CPF，the importance density function of PF is obtained by adopting Kalman filtering algorithm，the latest measured values are integrated into the transfer process of system state;the predictive sample produced by this method is very close to real posterior probability sample of system state.The result of simulation indicates that the estimation performance of CPF algorithm is obviously better than standard PF.

Cubature particle filter(CPF) SINS/GPS integrated navigation Nonlinear system Kalman filter Data fusion

TP14

A

資助項目(編號:60974070)。

修改稿收到日期:2013－04－14。

任波(1962－)，男，1990年畢業(yè)于哈爾濱工程大學(xué)導(dǎo)航、制導(dǎo)與控制專業(yè)，獲碩士學(xué)位，教授;主要從事目標(biāo)探測與識別技術(shù)、信息處理與融合技術(shù)等方面的研究工作。