CKF在SINS動基座大失準角初始對準中的應用

于 飛， 孫 騫， 張 亞

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

CKF在SINS動基座大失準角初始對準中的應用

于 飛， 孫 騫， 張 亞

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

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

船舶工程；初始對準；動機座；大方位失準角；容積卡爾曼濾波

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

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

1 SINS大失準角誤差模型

(1)

(2)

速度誤差方程[12]為

(3)

(4)

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

(5)

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

(6)

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

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

2.1非線性SINS建模

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

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

(7)

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

2.2CKF濾波算法

CKF濾波算法是一種基于高斯假設的貝葉斯濾波方法，它根據(jù)Cubature準則，通過對2n個同等權值的Cubature點進行非線性方程轉換后產(chǎn)生新的點來給出下一時刻系統(tǒng)的狀態(tài)，從而完成預測。與傳統(tǒng)的UKF濾波方法相比，CKF的采樣點較少，能夠降低計算量、提高濾波的收斂速度；CKF中的Cubature點集是通過積分得到的，權值永遠為正，可以有效解決UKF在高維系統(tǒng)中權值為負時出現(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)和量測的非線性轉換；過程噪聲wk和量測噪聲vk為互不相關的高斯白噪聲，且滿足wk～(0,Qk)，vk～(0,Rk)。

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

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

3 仿真分析和試驗驗證

3.1仿真及分析

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

(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內作勻加速直線運動；東向和北向加速度為ax=ay=0.2 m/s2，30 s之后作勻速直線運動。艦船初始經(jīng)緯度分別為東經(jīng)126.670 5°、北緯45.779 6°，初始水平失準角為φe=φn=1°,初始方位失準角為φu=10°，陀螺常值漂移為0.01(°)/h，陀螺隨機噪聲為0.005(°)/h，加速度計零偏為10-5g0，加速度計隨機噪聲為0.5×10-5g0。

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

圖1 水平失準角估計誤差對比曲線

圖2 方位失準角估計誤差對比曲線

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

3.2試驗驗證

采用實驗室自行研制的高精度光纖陀螺慣導系統(tǒng)在某地進行試驗，試驗設備主要包括光纖陀螺慣導系統(tǒng)、多普勒計程儀、PHINS、水面艦船、導航計算機及相關電源，其中自研光纖陀螺慣導系統(tǒng)見圖3，艦載實驗設備安裝見圖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，隨后艦船為勻速直線運動。取60 s粗對準結果作為精對準開始時的初始失準角，則初始失準角為0.896°、0.434°、4.158°。分別采用此處所提算法和傳統(tǒng)的UKF濾波算法對系統(tǒng)狀態(tài)變量進行估計。由于采用這兩種方法后水平失準角誤差估計精度均較高且收斂速度均較快，因此只給出方位失準角誤差對比曲線(見圖5)。

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

圖4 實船試驗設備圖

圖5 方位失準角估計誤差對比曲線

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

4 結 語

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

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

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

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

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

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

[6] 顧冬晴，秦永元. 船用捷聯(lián)慣導系統(tǒng)運動中對準的UKF設計[J]. 系統(tǒng)工程與電子技術，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大方位失準角初始對準[J]. 儀器儀表學報, 2012, 33(2): 327-333.

[12] 龍瑞, 秦永元, 夏家和. CDKF在捷聯(lián)慣導系統(tǒng)大失準角初始對準的應用[J]. 西北工業(yè)大學學報, 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

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

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

1000-4653(2014)02-0048-05

U666.12

A