遲鳳陽，孫 楓，徐 博

（哈爾濱工程大學(xué)自動(dòng)化學(xué)院，黑龍江哈爾濱 150001）

0 引言

在捷聯(lián)慣性導(dǎo)航系統(tǒng)（SINS）中，初始對(duì)準(zhǔn)是一項(xiàng)關(guān)鍵技術(shù)。對(duì)準(zhǔn)精度與對(duì)準(zhǔn)時(shí)間是影響整個(gè)系統(tǒng)性能的2個(gè)重要指標(biāo)，而濾波技術(shù)在初始對(duì)準(zhǔn)中具有極其重要的作用［1］。在初始失準(zhǔn)角較小的情況下，SINS的誤差模型可以簡(jiǎn)化為線性模型，一般用卡爾曼（Kalman）濾波方法進(jìn)行處理;當(dāng)初始失準(zhǔn)角較大時(shí)，SINS的誤差模型呈現(xiàn)非線性，需要采用非線性濾波技術(shù)進(jìn)行處理［2］。

擴(kuò)展卡爾曼濾波［3，4］（EKF）是目前常用的一種非線性濾波方法，EKF雖然簡(jiǎn)單而且容易實(shí)現(xiàn)，但它只是將非線性系統(tǒng)進(jìn)行一階線性近似，并且需要計(jì)算雅克比行列式，增加了算法的計(jì)算量，影響狀態(tài)的估計(jì)精度。Julier S等人提出了基于 UT 變換的無跡卡爾曼濾波［5，6］（UKF）算法，UKF 直接利用非線性模型，使用UT變換得到一組sigma采樣點(diǎn)來實(shí)現(xiàn)對(duì)狀態(tài)向量后驗(yàn)分布的近似，避免了線性化誤差，使得狀態(tài)均值和方差的估計(jì)精度提高到二階矩。UKF的精度雖然有所提高，但是隨著系統(tǒng)維數(shù)的增大，不但計(jì)算量迅速增加，而且濾波性能急劇下降，甚至?xí)?dǎo)致發(fā)散。Arasaratnam Ienkaran等人提出了基于Cubature變換的容積卡爾曼濾波［7］（CKF）算法，CKF利用球形積分準(zhǔn)則優(yōu)化了UKF中的sigma點(diǎn)的采樣方法和權(quán)重分配。在高維數(shù)系統(tǒng)中，CKF的濾波精度要高于UKF。針對(duì)于非線性、非高斯?fàn)顟B(tài)系統(tǒng)，文獻(xiàn)［8，9］中提出了基于序貫蒙特—卡羅方法的粒子濾波（PF）算法，PF無需對(duì)狀態(tài)模型進(jìn)行線性化和高斯假設(shè)，可以處理任意非線性、非高斯系統(tǒng)中的狀態(tài)估計(jì)問題。但是PF的缺點(diǎn)是重要性密度函數(shù)難以選取，且容易出現(xiàn)粒子退化問題［10，11］。

本文將 CKF 和 Gauss-Newton 迭代方法［12，13］相結(jié)合，得到迭代CKF（iterated CKF，ICKF）算法。利用ICKF算法獲得PF算法的重要性密度函數(shù)，從而提出一種新的迭代容積PF（ICPF）算法，有效地解決了粒子退化問題。仿真結(jié)果和實(shí)驗(yàn)結(jié)果表明:將ICPF算法應(yīng)用到SINS大失準(zhǔn)角初始對(duì)準(zhǔn)的過程中，有效地提高了對(duì)準(zhǔn)的精度。

1 SINS靜基座大方位失準(zhǔn)角誤差模型的建立

SINS初始對(duì)準(zhǔn)是建立姿態(tài)矩陣初始值的過程。由于粗對(duì)準(zhǔn)后的水平失準(zhǔn)角很小，而方位失準(zhǔn)角比較大，在這種情況下從當(dāng)?shù)氐乩碜鴺?biāo)系（t系）到計(jì)算地理坐標(biāo)系（t'系）之間的轉(zhuǎn)換矩陣為

式中 φx，φy，φz分別為東向、北向和方位失準(zhǔn)角。

SINS的速度誤差和失準(zhǔn)角模型如下［14，15］

式中 δv=［δvxδvyδvz］T為速度誤差矢量，φ=［φxφyφz］T為失準(zhǔn)角矢量為載體系到地理系的方向余弦矩陣為載體系到計(jì)算地理系的方向余弦矩陣為加速度計(jì)真實(shí)的比力輸出，δfb為加速度計(jì)的測(cè)量誤差，包括常值零偏和零均值高斯白噪聲為陀螺的測(cè)量誤差，包括常值漂移εb和零均值高斯白噪聲為地球自轉(zhuǎn)角速率在計(jì)算地理系的投影為地理系相對(duì)于地球系的轉(zhuǎn)動(dòng)角速率在計(jì)算地理系的投影為地理系相對(duì)于慣性系的轉(zhuǎn)動(dòng)角速率在計(jì)算地理系上的投影分別為的計(jì)算誤差，δg為重力加速度的計(jì)算誤差，可以忽略。

在靜基座的條件下有v=0，略去垂直通道速度，將式（2）和式（3）展開，得到初始對(duì)準(zhǔn)非線性模型的狀態(tài)方程

式中Rm為子午面內(nèi)的曲率半徑，Rn為與子午面垂直的法線平面的曲率半徑，φ為當(dāng)?shù)氐木暥?/p>

其中，Cij（i=1，2，3;j=1，2，3）為矩陣的第i行、第j列元素，C'ij（i=1，2，3;j=1，2，3）為的第i行、第j列元素。

建立SINS的狀態(tài)空間模型

式中f（X）和G的具體表達(dá)式參考表達(dá)式（4），H=［I2×202×3］，V為零均值高斯分布的觀測(cè)噪聲。

2 CKF算法

由貝葉斯估計(jì)原理可知，基于高斯假設(shè)的貝葉斯濾波估計(jì)算法的核心在于求解具有“非線性函數(shù)×高斯密度”形式被積函數(shù)的加權(quán)積分?？紤]如下形式的多維積分

式中f（x）為任意函數(shù)，Rn為積分區(qū)域，上式的積分的解析解一般無法直接得到，因此，需要用近似的方法獲得。

文獻(xiàn)［7］指出采用 Spherical-Radial容積準(zhǔn)則計(jì)算積分式（6）

式中 ［1］i表示集合［1］的第i列，［1］為完整全對(duì)稱點(diǎn)集，表示n維單位向量e=［1 0…0］T的元素進(jìn)行全排列和改變?cè)胤?hào)所產(chǎn)生的點(diǎn)集;ξi為容積點(diǎn)，wi（i=1，2，…，m）為相應(yīng)的權(quán)值，對(duì)于系統(tǒng)狀態(tài)方程為三維的情況，即n=3 時(shí)有:［1］={（1，0，0）T，（0，1，0）T，（0，0，1）T，（－ 1，0，0）T，（0，－1，0）T，（0，0，－1）T}。

3 ICPF算法

式中C為常數(shù)。

定義代價(jià)函數(shù)為

上式中的最大似然估計(jì)等價(jià)于求代價(jià)函數(shù)的最小值。利用Gauss-Newton迭代方法求解J（xk）最小值的第j+1次迭代公式為

代入式（11）即可計(jì)算迭代更新。

由于標(biāo)準(zhǔn)PF采用狀態(tài)轉(zhuǎn)移先驗(yàn)概率密度作為建議分布，沒有考慮最新量測(cè)值而完全依賴于模型，所以，導(dǎo)致濾波精度下降。本文利用ICKF算法獲得PF算法的重要性密度函數(shù)，在時(shí)刻利用最新量測(cè)數(shù)據(jù)，產(chǎn)生新的后驗(yàn)概率密度分布，同時(shí)通過新的后驗(yàn)概率分布重新產(chǎn)生新的粒子，計(jì)算粒子的權(quán)值并歸一化，最后進(jìn)行重采樣完成狀態(tài)估計(jì)。ICPF算法流程如下:

狀態(tài)變量初值

協(xié)方差陣初值

2）預(yù)測(cè):k≥1，進(jìn)行重要性采樣，利用ICKF對(duì)每個(gè)粒子進(jìn)行更新。

數(shù)字出版產(chǎn)業(yè)目前仍然存在著一些問題，例如，數(shù)字出版物的模式單一、盈利相對(duì)較少，品牌建設(shè)力量不足，數(shù)字出版物的侵權(quán)問題等。

計(jì)算容積點(diǎn) ξi（i=1，2，…，m）

通過狀態(tài)方程傳播容積點(diǎn)

k時(shí)刻的狀態(tài)預(yù)測(cè)值

k時(shí)刻的狀態(tài)誤差協(xié)方差預(yù)測(cè)值

再次進(jìn)行Cholesky分解并計(jì)算容積點(diǎn)

通過量測(cè)方程傳播容積點(diǎn)

k時(shí)刻的量測(cè)預(yù)測(cè)值

k時(shí)刻的估計(jì)互相關(guān)協(xié)方差陣

估計(jì)卡爾曼增益

第j+1次迭代的狀態(tài)和方差估計(jì)

計(jì)算粒子權(quán)值并歸一化

4）重采樣:計(jì)算有效粒子數(shù)

設(shè)定門限值Nthreshold，若Neff＜Nthreshold，則認(rèn)為粒子出現(xiàn)退化現(xiàn)象需要進(jìn)行重采樣;否則，不進(jìn)行重采樣。

5）狀態(tài)估計(jì)與方差估計(jì)

6）令k=k+1，返回步驟（2）直至濾波結(jié)束。

4 仿真分析

計(jì)算機(jī)仿真參數(shù)如下:系統(tǒng)狀態(tài)初始值^x0為零，初始失準(zhǔn)角 φx=φy=1°，φ2=10°，陀螺常值漂移為 0．01°/h，隨機(jī)漂移0．005°/h，加速度計(jì)的零偏為 1 ×10－5gn，隨機(jī)零偏為0．5 ×10－5gn，速度測(cè)量誤差為 0．1 m/s，SINS 的初始位置為經(jīng)度為 126．67°，緯度為 45．78°，高度為 0 m。針對(duì)上述條件分別利用PF，CPF，ICPF算法對(duì)SINS進(jìn)行靜基座初始對(duì)準(zhǔn)仿真，粒子數(shù)N=1000，仿真時(shí)間t=300 s，仿真結(jié)果如圖1～圖3所示，各種濾波算法的估計(jì)誤差如表1所示。

圖1 橫搖失準(zhǔn)角估計(jì)誤差Fig 1 Estimation error of rolling misalignment angle

從圖1～圖3和表1中能夠看出:PF，CPF與ICPF算法對(duì)于橫搖失準(zhǔn)角和縱搖失準(zhǔn)角的濾波估計(jì)精度和收斂速度比較理想，失準(zhǔn)角穩(wěn)態(tài)誤差很小;而對(duì)大方位失準(zhǔn)角的估計(jì)，ICPF在收斂速度和估計(jì)精度上，要優(yōu)于PF和CPF算法。主要原因是標(biāo)準(zhǔn)的PF算法是以狀態(tài)轉(zhuǎn)移先驗(yàn)概率密度作為建議分布，容易出現(xiàn)粒子退化問題，而ICPF算法在迭代過程中利用最新量測(cè)信息改進(jìn)迭代過程產(chǎn)生的新息方差和協(xié)方差，提高了濾波的收斂速度和數(shù)值穩(wěn)定性。

圖2 縱搖失準(zhǔn)角估計(jì)誤差Fig 2 Estimation error of pitching misalignment angle

圖3 航向失準(zhǔn)角估計(jì)誤差Fig 3 Estimation error of heading misalignment angle

表1 PF，CPF與ICPF的估計(jì)誤差Tab 1 Estimation error of PF，CPF與 ICPF

5 試驗(yàn)驗(yàn)證

教研室自行研制的光纖陀螺是目前國(guó)內(nèi)光學(xué)陀螺中比較先進(jìn)的一種，具有較好的精度和穩(wěn)定性。試驗(yàn)中采用由3只光纖陀螺與3只石英加速度計(jì)及相應(yīng)的信號(hào)處理電路組成的慣性測(cè)量單元（IMU）（見圖4（a）），在SGT—3型慣導(dǎo)測(cè)試轉(zhuǎn)臺(tái)（見圖4（b））上進(jìn)行靜基座對(duì)準(zhǔn)的試驗(yàn)。

圖4 慣性測(cè)量單元與捷聯(lián)慣導(dǎo)測(cè)試平臺(tái)Fig 4 Inertial measurement unit and SINS testing turntable

實(shí)驗(yàn)室轉(zhuǎn)臺(tái)的位置是北緯 45．7796°，東經(jīng) 126．6705°。為了比較在相同數(shù)據(jù)源下CPF和ICPF的濾波性能，采用現(xiàn)場(chǎng)數(shù)據(jù)采集與離線計(jì)算分析的方法。試驗(yàn)中的測(cè)試數(shù)據(jù)經(jīng)過了系統(tǒng)標(biāo)定和系統(tǒng)誤差補(bǔ)償?？紤]對(duì)準(zhǔn)時(shí)間較短，認(rèn)為陀螺的測(cè)量誤差是由常值漂移與白噪聲構(gòu)成，加速度計(jì)測(cè)量誤差由零偏與白噪聲組成。3個(gè)初始失準(zhǔn)角分別為φx=φy=1°，φz=10°。圖5給出了航向失準(zhǔn)角誤差試驗(yàn)曲線圖，表2給出了10次航向失準(zhǔn)角估計(jì)誤差平均值。

圖5 航向失準(zhǔn)角估計(jì)誤差Fig 5 Estimation error of heading misalignment angle

表2 CPF與ICPF的航向失準(zhǔn)角估計(jì)誤差Tab 2 Estimation error of heading misalignment angle of CPF and ICPF

從試驗(yàn)結(jié)果可以看出:相對(duì)于CPF，ICPF更利于提高初始對(duì)準(zhǔn)精度，從而在SINS靜基座方位大失準(zhǔn)角初始對(duì)準(zhǔn)中應(yīng)當(dāng)選擇ICPF作為濾波方法，這與仿真結(jié)論相一致。

6 結(jié)論

本文深入地研究了ICPF算法，并將其應(yīng)用到SINS大方位失準(zhǔn)角初始對(duì)準(zhǔn)過程中。該算法使用容積數(shù)值積分方法直接計(jì)算非線性隨機(jī)變量的均值和方差，使用Gauss-Newton迭代方法充分利用最新量測(cè)信息改進(jìn)了迭代過程產(chǎn)生的新息方差和協(xié)方差，提高了狀態(tài)估計(jì)精度。ICPF算法利用ICKF得到PF的重要性密度函數(shù)，有效地緩解了粒子退化問題。仿真結(jié)果和試驗(yàn)驗(yàn)證結(jié)果表明:ICPF算法在SINS大方位失準(zhǔn)角初始對(duì)準(zhǔn)中的可行性和優(yōu)越性，是一種有效的非線性濾波算法。

［1］ 嚴(yán)恭敏，嚴(yán)衛(wèi)生，徐德民．基于UKF濾波在SINS大失準(zhǔn)角初始對(duì)準(zhǔn)中的應(yīng)用［J］．中國(guó)慣性技術(shù)學(xué)報(bào)，2008，16（3）:254－264．

［2］ Dmitriyev S P，Stepanov O A，Shepel S V．Nonlinear filtering methods application in INS alignment［J］．IEEE Trans on Aerospace and Electronic Systems，1997，33（1）:260 －271．

［3］ Athans M，Wishner R P，Bertolini A．Suboptimal state estimation for continuous time nonlinear systems from discrete noisy measurements［J］．IEEE Transactions on Automatic Control，1968，13:504－514．

［4］ ，Mariano R S．Nonlinear filters based on Taylor series expansion［J］．Commu Statist Theory and Methods，1996，25（6）:1261－1282．

［5］ Julier S，Uhlmann J K．A new extension of the Kalman filter to nonlinear systems［C］∥Proceedings of the Soeiety of Photo-Optical Instrumentation Engineers（SPIE），1997:182 －193．

［6］ Julier S，Uhlmann J K，Durrant-Whyte H F．A new method for the nonlinear transformation of means and covariances in filters and estimators［J］．IEEE Transactions on Automatic Control，2000，45（3）:477 －482．

［7］ Arasaratnam Ienkaran，Haykin Simon．Cubature Kalman filters［J］．IEEE Transactions on Automatic Control，2009，54（6）:1254－1269．

［8］ Akashi H，Kumamoto H．Construction of discrete-time nonlinear filter by Monte Carlo methods with variance-reducing techniques［J］．Systems and Control，1975，19:211 －221．

［9］ Handschin J E．Monte Carlo techniques for prediction and filtering of non-linear stochastic processes［J］．Automatica，1970，6:555－563．

［10］ Doucet A，Gordon Neil J，Krishnamurthy Vikram．Particle filters for state estimation of jump markov linear systems［J］．IEEE Transactions on Signal Processing，2001，49（3）:613 －624．

［11］ Doucet A，Godsill S，Andrieu C．On sequential Monte Carlo sampling methods for Bayesian filtering［J］．Statistics and Computing，2000，10:197 －208．

［12］ Lefebvre T，Bruyninckx H，Schutter J D．Kalman filters for nonlinear systems:A comparison of performance［J］．International Journal of Control，2004，77（7）:639 － 653．

［13］ Bell Bradley M，Cathey Frederick W．The iterated Kalman filter update as a Gauss-Newton method［J］．IEEE Transactions on Automatic Control，1993，38（2）:294 －297．

［14］ Kong Xiaoying，Nebot E M，Durrant-Whyte H．Development of a nonlinear psi-angle model for large misalignment errors and its application in INS alignment and calibration［C］∥IEEE International Conference on Robotics and Automation，1999:1430 －1435．

［15］向 禮，柳 明，蘇寶庫(kù)．高斯混合粒子濾波器在靜基座捷聯(lián)慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)中的應(yīng)用［J］．大連海事大學(xué)學(xué)報(bào)，2008，34（2）:61－64．