于 飛， 孫 騫， 張 亞

(哈爾濱工程大學(xué) 自動化學(xué)院，哈爾濱 150001)

CKF在SINS動基座大失準(zhǔn)角初始對準(zhǔn)中的應(yīng)用

于 飛， 孫 騫， 張 亞

(哈爾濱工程大學(xué) 自動化學(xué)院，哈爾濱 150001)

為了提高動機(jī)座捷聯(lián)慣導(dǎo)系統(tǒng)初始對準(zhǔn)的精度，在推導(dǎo)動基座大方位失準(zhǔn)角非線性模型的基礎(chǔ)上，提出一種基于容積卡爾曼濾波法的動基座初始對準(zhǔn)算法，并對提出的算法進(jìn)行仿真和試驗驗證。結(jié)果表明，與傳統(tǒng)的無跡卡爾曼方法相比，利用該方法能夠在更短的時間內(nèi)更準(zhǔn)確地完成精對準(zhǔn)過程。

船舶工程；初始對準(zhǔn)；動機(jī)座；大方位失準(zhǔn)角；容積卡爾曼濾波

初始對準(zhǔn)技術(shù)是捷聯(lián)慣性導(dǎo)航系統(tǒng)(Strapdown Inertial Navigation System, SINS)的關(guān)鍵技術(shù)之一，對準(zhǔn)精度和對準(zhǔn)速度是其重要技術(shù)指標(biāo)。[1]傳統(tǒng)的初始對準(zhǔn)的精對準(zhǔn)過程是：利用經(jīng)典Φ角法或Ψ角法推導(dǎo)出小失準(zhǔn)角條件下的線性微分方程誤差模型，然后利用標(biāo)準(zhǔn)Kalman濾波最優(yōu)估計方法解決SINS初始對準(zhǔn)問題。[2-3]但是在水面艦船航行過程中，尤其是在海況不佳時，載體會產(chǎn)生大幅晃動，方位粗對準(zhǔn)的精度將很難得到保證。[4]當(dāng)失準(zhǔn)角較大時，線性模型不能正確地描述真實的系統(tǒng)，需要利用非線性模型描述SINS的誤差特性。

對于非線性系統(tǒng)，最常用的濾波方法為擴(kuò)展卡爾曼濾波(Extend Kalman Filter, EKF)和無跡卡爾曼濾波(Unscented Kalman Filter, UKF)。EKF實現(xiàn)簡便、使用廣泛，但是當(dāng)系統(tǒng)為強(qiáng)非線性時，很容易產(chǎn)生線性化誤差，造成濾波器精度下降，甚至發(fā)散。[5]UKF采用UT變換(Unscented Transformation, UT)取代局部線性化，雖然精度高于EKF，但是當(dāng)處理高維數(shù)系統(tǒng)時，容易出現(xiàn)“維數(shù)災(zāi)難”。[6-7]近期提出的容積卡爾曼濾波(Cubature Kalman Filter, CKF)根據(jù)Cubature變換，通過計算一組經(jīng)過非線性系統(tǒng)方程轉(zhuǎn)換的具有相同權(quán)重的點(diǎn)集來給出下一時刻系統(tǒng)狀態(tài)的預(yù)測結(jié)果。[8-9]由于其估計精度高、不容易發(fā)散且計算量小，CKF在非線性濾波問題中的應(yīng)用越來越廣泛。文獻(xiàn)[10]首先將CKF應(yīng)用到系統(tǒng)的初始對準(zhǔn)中，雖然與傳統(tǒng)濾波器相比估計結(jié)果有了很大改善，但是分析僅限于仿真；文獻(xiàn)[11]將CKF濾波器應(yīng)用到SINS大方位失準(zhǔn)角初始對準(zhǔn)中，但是只分析了靜基座下的仿真和實驗。目前很少有文獻(xiàn)對動機(jī)座條件下的大失準(zhǔn)角初始對準(zhǔn)進(jìn)行分析，而艦船絕大多數(shù)時間是處于動機(jī)座下的，因此在文獻(xiàn)[11]的基礎(chǔ)上，利用歐拉平臺誤差角描述動基座下大失準(zhǔn)角SINS非線性誤差模型，詳細(xì)分析CKF濾波在動基座SINS初始對準(zhǔn)中的應(yīng)用。

1 SINS大失準(zhǔn)角誤差模型

(1)

(2)

速度誤差方程[12]為

(3)

(4)

由于陀螺儀和加速度計誤差均由常值誤差和零均值高斯白噪聲組成，因此其微分方程為

(5)

位置誤差方程由經(jīng)緯度誤差δλ和δφ組成,

(6)

式(6)中:RM，RN分別為地球子午面半徑和卯酉面半徑;φ為當(dāng)?shù)鼐暥戎?vx，vy分別為東向和北向速度。

2 系統(tǒng)建模與CKF濾波算法

2.1非線性SINS建模

系統(tǒng)噪聲向量為

以SINS與多普勒計程儀的速度之差作為觀測量，建立狀態(tài)方程和觀測方程

(7)

式(7)中:狀態(tài)轉(zhuǎn)移矩陣F(X)和系統(tǒng)噪聲驅(qū)動陣G的具體表達(dá)式參考式(2)～式(6)，W～N(0,Q);量測矩陣H=[02×2，I2×2，02×8]；V為量測噪聲，V～N(0,R)。

2.2CKF濾波算法

CKF濾波算法是一種基于高斯假設(shè)的貝葉斯濾波方法，它根據(jù)Cubature準(zhǔn)則，通過對2n個同等權(quán)值的Cubature點(diǎn)進(jìn)行非線性方程轉(zhuǎn)換后產(chǎn)生新的點(diǎn)來給出下一時刻系統(tǒng)的狀態(tài)，從而完成預(yù)測。與傳統(tǒng)的UKF濾波方法相比，CKF的采樣點(diǎn)較少，能夠降低計算量、提高濾波的收斂速度；CKF中的Cubature點(diǎn)集是通過積分得到的，權(quán)值永遠(yuǎn)為正，可以有效解決UKF在高維系統(tǒng)中權(quán)值為負(fù)時出現(xiàn)的濾波發(fā)散問題，因此CKF濾波算法的數(shù)值穩(wěn)定性及濾波精度均優(yōu)于UKF。[13-14]

考慮如下的非線性離散狀態(tài)空間模型

(8)

式(8)中:xk∈Rn和zk∈Rm分別為k時刻的狀態(tài)向量和量測向量；f(·)和h(·)為系統(tǒng)狀態(tài)和量測的非線性轉(zhuǎn)換；過程噪聲wk和量測噪聲vk為互不相關(guān)的高斯白噪聲，且滿足wk～(0,Qk)，vk～(0,Rk)。

CKF濾波方法的具體算法流程[8,14]為

Yi,k|k-1=h(Xi,k|k-1)

3 仿真分析和試驗驗證

3.1仿真及分析

為驗證動基座初始對準(zhǔn)中CKF濾波方法的性能，進(jìn)行仿真試驗。設(shè)水面艦船的橫搖、縱搖和航向角分別為Ψ,θ,γ，搖擺過程的數(shù)學(xué)建模為

(9)

式(9)中:搖擺幅值分別為：θm=10°,Ψm=6°,γm=8°；搖擺周期為：Tθ=10 s,TΨ=6 s,Tγ=8 s；初始姿態(tài)角分別為:θk=Ψk=0°,γk=30°。水面艦船的東向和北向初始速度為vx=vy=5 m/s，在0～30 s內(nèi)作勻加速直線運(yùn)動；東向和北向加速度為ax=ay=0.2 m/s2，30 s之后作勻速直線運(yùn)動。艦船初始經(jīng)緯度分別為東經(jīng)126.670 5°、北緯45.779 6°，初始水平失準(zhǔn)角為φe=φn=1°,初始方位失準(zhǔn)角為φu=10°，陀螺常值漂移為0.01(°)/h，陀螺隨機(jī)噪聲為0.005(°)/h，加速度計零偏為10-5g0，加速度計隨機(jī)噪聲為0.5×10-5g0。

在上述條件下，分別利用UKF方法和CKF方法對失準(zhǔn)角誤差估計進(jìn)行仿真，仿真時間為600 s，仿真得到的水平失準(zhǔn)角及方位失準(zhǔn)角估計誤差曲線見圖1和圖2。

圖1 水平失準(zhǔn)角估計誤差對比曲線

圖2 方位失準(zhǔn)角估計誤差對比曲線

由圖1和圖2可以看出，在動基座方位大失準(zhǔn)角的情況下，對于水平失準(zhǔn)角誤差，CKF和UKF的收斂速度和估計精度基本相同；對于方位失準(zhǔn)角誤差，CKF的收斂速度明顯比UKF快，而且在600 s處，CKF的估計穩(wěn)定值為-0.47°，UKF的估計穩(wěn)定值為-0.54°。因此,CKF的估計精度高于UKF。仿真結(jié)果表明，CKF能夠解決動基座方位大失準(zhǔn)角初始對準(zhǔn)問題，且收斂速度快、對準(zhǔn)精度高。

3.2試驗驗證

采用實驗室自行研制的高精度光纖陀螺慣導(dǎo)系統(tǒng)在某地進(jìn)行試驗，試驗設(shè)備主要包括光纖陀螺慣導(dǎo)系統(tǒng)、多普勒計程儀、PHINS、水面艦船、導(dǎo)航計算機(jī)及相關(guān)電源，其中自研光纖陀螺慣導(dǎo)系統(tǒng)見圖3，艦載實驗設(shè)備安裝見圖4。

艦船初始經(jīng)緯度分別為東經(jīng)126.670 5°、北緯45.779 6°，初始橫搖角、縱搖角及航向角分別為0.05°、0.73°、212.5°，水面艦船的初始狀態(tài)為系泊狀態(tài)，加速80 s至東向速度為4.8 m/s、北向速度為7.6 m/s，隨后艦船為勻速直線運(yùn)動。取60 s粗對準(zhǔn)結(jié)果作為精對準(zhǔn)開始時的初始失準(zhǔn)角，則初始失準(zhǔn)角為0.896°、0.434°、4.158°。分別采用此處所提算法和傳統(tǒng)的UKF濾波算法對系統(tǒng)狀態(tài)變量進(jìn)行估計。由于采用這兩種方法后水平失準(zhǔn)角誤差估計精度均較高且收斂速度均較快，因此只給出方位失準(zhǔn)角誤差對比曲線(見圖5)。

圖3 實驗室自研光纖陀螺慣導(dǎo)系統(tǒng)

圖4 實船試驗設(shè)備圖

圖5 方位失準(zhǔn)角估計誤差對比曲線

由圖5可以看出，采用UKF濾波方法后，曲線在600 s后趨于穩(wěn)定，穩(wěn)定值為-0.196°；采用CKF濾波方法后，曲線在400 s后趨于穩(wěn)定，穩(wěn)定值為-0.185°。因此，動基座方位大失準(zhǔn)角條件下，CKF的估計精度比UKF高，且估計時間比UKF短，與仿真結(jié)果一致。

4 結(jié) 語

推導(dǎo)出動機(jī)座SINS方位大失準(zhǔn)角的非線性誤差方程，并建立濾波模型；分析CKF濾波算法，從理論上證實了CKF的優(yōu)勢,并分別對CKF和UKF兩種方法進(jìn)行了仿真。仿真結(jié)果表明,采用CKF方法后濾波精度和收斂速度均優(yōu)于UKF方法。最后,利用實驗室自研的光纖捷聯(lián)慣導(dǎo)系統(tǒng)進(jìn)行實船試驗,試驗結(jié)果與仿真結(jié)論相同，進(jìn)一步證實CKF在動基座大方位失準(zhǔn)角SINS初始對準(zhǔn)中的可行性和優(yōu)越性，為工程上動基座大失準(zhǔn)角初始對準(zhǔn)提供了一種新的方式。

[1] 高偉，張亞，奔粵陽,等. 旋轉(zhuǎn)式慣導(dǎo)系統(tǒng)刻度因數(shù)誤差的分析與補(bǔ)償[J].中國航海，2012, 35(3):1-4.

[2] 嚴(yán)恭敏，嚴(yán)衛(wèi)生，徐德民. 簡化UKF濾波在SINS大失準(zhǔn)角初始對準(zhǔn)中的應(yīng)用[J]. 中國慣性技術(shù)學(xué)報, 2008, 16(3): 253-264.

[3] 秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導(dǎo)航原理[M]. 西安：西北工業(yè)大學(xué)出版社，1998.

[4] 郝艷玲，楊峻巍，陳亮，等. 基于SRCKF的水下航行器動基座初始對準(zhǔn)技術(shù)[J].華中科技大學(xué)學(xué)報：自然科學(xué)版, 2012, 40(2): 123-127.

[5] 李中兵，張歡仁.基于擴(kuò)展卡爾曼濾波的船舶電力推進(jìn)系統(tǒng)仿真[J]. 中國航海，2011, 34(4):45-50.

[6] 顧冬晴，秦永元. 船用捷聯(lián)慣導(dǎo)系統(tǒng)運(yùn)動中對準(zhǔn)的UKF設(shè)計[J]. 系統(tǒng)工程與電子技術(shù)，2006,28(8):1218-1220.

[7] GE Quanbo, LI Wenbin, WEN Chenglin. SCKF-STF-CN: a Universal Nonlinear Filter for Maneuver Target Tracking [J]. Journal of Zhejiang University-Science C: Compute amp; Electron, 2011, 12(8): 678-686.

[8] ARASATNAM I, HAYKIN S. Cubature Kalman Filters [J]. IEEE Transactions on Automatic Control, 2009, 54(6): 1 254-1269.

[9] ARASARATNAM I, HAYKIN S. Cubature Kalman Smoothers [J]. Automatica, 2011(47): 2245-2250.

[10] KUMAR P, BHARANI C, GU Dawei. Cubature Information Filter and Its Applications [C]. 2011 American Control Conference. San Francisco, CA, USA, 2011.

[11] 孫楓, 唐李軍. 基于CKF的SINS大方位失準(zhǔn)角初始對準(zhǔn)[J]. 儀器儀表學(xué)報, 2012, 33(2): 327-333.

[12] 龍瑞, 秦永元, 夏家和. CDKF在捷聯(lián)慣導(dǎo)系統(tǒng)大失準(zhǔn)角初始對準(zhǔn)的應(yīng)用[J]. 西北工業(yè)大學(xué)學(xué)報, 2010, 28(3): 364-368.

[13] 孫楓, 唐李軍. Cubature卡爾曼濾波-卡爾曼濾波算法[J]. 控制與決策, 2012, 27(10): 1 561-1 565.

[14] ARASARATNAM I, HAYKIN S. Cubature Kalman Filter for Continuous-Discrete System: Theory and Simulation [J]. IEEE Transactions on Signal Processing, 2010, 58(10): 4 977-4 993.

ApplyingCKFtoInitialAlignmentofSINSonDynamicBasewithaLargeMisalignmentAngle

YUFei,SUNQian,ZHANGYa
(College of Automation, Harbin Engineering University, Harbin 150001, China)

The non-linear model of the system on the dynamic base with large azimuth misalignment angle is derived and the dynamic base initial alignment algorithm based on Cubature Kalman filter is presented. The simulation and experiment results show that the fine alignment with Cubature Kalman filter achieves better accuracy in shorter time than the one with Unscented Kalman filter.

ship engineering; initial alignment； dynamic base； large azimuth misalignment angle； Cubature Kalman filter

2014-01-28

國家自然科學(xué)基金(51179039)；武器裝備預(yù)研基金(9140A09040211CB0102)

于 飛 (1974-), 男, 黑龍江哈爾濱人，教授，博士生導(dǎo)師，從事捷聯(lián)慣性導(dǎo)航系統(tǒng)及其組合導(dǎo)航技術(shù)研究。E-mail: yufei@hrbeu.edu.cn.

1000-4653(2014)02-0048-05

U666.12

A