遲鳳陽，孫 楓，徐 博

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

0 引言

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

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

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

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

SINS初始對準(zhǔn)是建立姿態(tài)矩陣初始值的過程。由于粗對準(zhǔn)后的水平失準(zhǔn)角很小，而方位失準(zhǔn)角比較大，在這種情況下從當(dāng)?shù)氐乩碜鴺?biāo)系（t系）到計算地理坐標(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)角矢量為載體系到地理系的方向余弦矩陣為載體系到計算地理系的方向余弦矩陣為加速度計真實的比力輸出，δfb為加速度計的測量誤差，包括常值零偏和零均值高斯白噪聲為陀螺的測量誤差，包括常值漂移εb和零均值高斯白噪聲為地球自轉(zhuǎn)角速率在計算地理系的投影為地理系相對于地球系的轉(zhuǎn)動角速率在計算地理系的投影為地理系相對于慣性系的轉(zhuǎn)動角速率在計算地理系上的投影分別為的計算誤差，δg為重力加速度的計算誤差，可以忽略。

在靜基座的條件下有v=0，略去垂直通道速度，將式（2）和式（3）展開，得到初始對準(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為零均值高斯分布的觀測噪聲。

2 CKF算法

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

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

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

式中 ［1］i表示集合［1］的第i列，［1］為完整全對稱點集，表示n維單位向量e=［1 0…0］T的元素進(jìn)行全排列和改變元素符號所產(chǎn)生的點集;ξi為容積點，wi（i=1，2，…，m）為相應(yīng)的權(quán)值，對于系統(tǒng)狀態(tài)方程為三維的情況，即n=3 時有:［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ù)。

定義代價函數(shù)為

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

代入式（11）即可計算迭代更新。

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

狀態(tài)變量初值

協(xié)方差陣初值

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

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

計算容積點 ξi（i=1，2，…，m）

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

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

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

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

通過量測方程傳播容積點

k時刻的量測預(yù)測值

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

估計卡爾曼增益

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

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

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

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

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

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

4 仿真分析

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

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

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

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

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

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

5 試驗驗證

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

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

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

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

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

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

6 結(jié)論

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

［1］ 嚴(yán)恭敏，嚴(yán)衛(wèi)生，徐德民．基于UKF濾波在SINS大失準(zhǔn)角初始對準(zhǔn)中的應(yīng)用［J］．中國慣性技術(shù)學(xué)報，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］向 禮，柳 明，蘇寶庫．高斯混合粒子濾波器在靜基座捷聯(lián)慣導(dǎo)系統(tǒng)初始對準(zhǔn)中的應(yīng)用［J］．大連海事大學(xué)學(xué)報，2008，34（2）:61－64．