ISR-CDKF在SINS大方位失準(zhǔn)角初始對(duì)準(zhǔn)中的應(yīng)用

賈鶴鳴, 宋文龍, 牟宏偉, 車延庭

(1. 東北林業(yè)大學(xué) 機(jī)電工程學(xué)院, 哈爾濱 150040； 2. 中國(guó)運(yùn)載火箭技術(shù)研究院, 北京100076；3. 哈爾濱工程大學(xué) 自動(dòng)化學(xué)院, 哈爾濱 150001)

0 引 言

初始對(duì)準(zhǔn)是捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS: Strapdown Inertial Navigation System)的一項(xiàng)關(guān)鍵技術(shù)[1-3]。隨著對(duì)慣性技術(shù)要求的不斷提高, 大方位失準(zhǔn)角方法的提出使得對(duì)非線性濾波算法的研究具有十分重要的意義。

近年來, 中心差分卡爾曼濾波(CDKF: Central Difference Kalman Filter)在非線性估計(jì)領(lǐng)域中得到了廣泛的應(yīng)用[4,5]。該算法雖然克服了擴(kuò)展卡爾曼濾波(EKF: Extended Kalman Filter)由于線性化誤差而導(dǎo)致濾波器精度降低和需要計(jì)算雅可比矩陣(Jacobian)的缺點(diǎn)[6], 但是CDKF存在計(jì)算量大以及算法不穩(wěn)定等缺點(diǎn)[7]。針對(duì)以上不足, 筆者提出了迭代測(cè)量更新的平方根中心差分卡爾曼濾波(ISR-CDKF: Iterative Square Root Central Difference Kalman Filter)算法。該算法利用協(xié)方差平方根代替協(xié)方差參加遞推運(yùn)算, 保證了濾波的數(shù)值穩(wěn)定性。同時(shí), 在此基礎(chǔ)上改進(jìn)了量測(cè)更新方法, 重復(fù)利用觀測(cè)信息, 提高了非線性估計(jì)精度。

筆者將ISR-CDKF應(yīng)用于SINS大方位失準(zhǔn)角初始對(duì)準(zhǔn)中, 通過仿真比較, 進(jìn)一步表明了ISR-CDKF不僅可提高系統(tǒng)的精度, 而且保證了數(shù)值穩(wěn)定性, 驗(yàn)證了該算法的可行性和優(yōu)越性。

1 大方位失準(zhǔn)角初始對(duì)準(zhǔn)誤差模型

(1)

其中φx、φy、φz為3個(gè)歐拉角； 當(dāng)φx,φy很小時(shí), cosφx=cosφy=1, sinφx≈φx, sinφy≈φy, 則

(2)

1.1 姿態(tài)角誤差方程

(3)

忽略二階小量, 式(3)化簡(jiǎn)可得

(4)

(5)

(6)

(7)

(8)

1.2 速度誤差方程

SINS的理論速度微分方程為

(9)

SINS導(dǎo)航解算的速度微分方程為

(10)

(11)

1.3 捷聯(lián)慣導(dǎo)系統(tǒng)非線性初始對(duì)準(zhǔn)模型

假設(shè)陀螺儀的測(cè)量誤差為常值漂移εb； 加速度計(jì)的測(cè)量誤差為常值零偏b； 在靜基座初始對(duì)準(zhǔn)條件下,其中R為地球半徑,L為緯度, 忽略重力誤差項(xiàng)δgt以及不考慮δvz, 則由式(8)和式(11)得到系統(tǒng)狀態(tài)方程為

(12)

令狀態(tài)向量

陀螺和加速度計(jì)的零均值高斯白色噪聲向量

以SINS兩水平速度誤差Z=δVt作為觀測(cè)量, 建立非線性對(duì)準(zhǔn)模型如下

(13)

其中f(X)的具體表達(dá)式參考式(12),G為10×5維的矩陣系數(shù),G(1,1)=G(2,2)=G(3,3)=G(4,4)=G(5,5)=1, 量測(cè)矩陣H=[02×3∶I2×2∶02×5],V為量測(cè)噪聲。

2 迭代測(cè)量更新的平方根中心差分卡爾曼濾波

2.1 中心差分變換

中心差分變換是一種基于插值理論的非線性變換方法, 是CDKF算法的基礎(chǔ)。CDKF借助于中心差分變換, 利用sterling插值公式, 用多項(xiàng)式逼近非線性方程的導(dǎo)數(shù)來避免復(fù)雜的求導(dǎo)運(yùn)算, 它采用中心差分代替Tailor展開中的一階和二階導(dǎo)數(shù)[8,9]。

y=f(x)

(14)

(15)

(16)

(17)

(18)

(19)

(20)

其中sxi表示cholesky分解中sx的第i列。

2.2 平方根U變換

U變換是一種通過設(shè)置Sigma樣點(diǎn)和相應(yīng)權(quán)值計(jì)算隨機(jī)變量經(jīng)非線性變換后的統(tǒng)計(jì)特性的方法[11-15]。

y=g(x)

(24)

利用這些樣點(diǎn)通過非線性變換可得到的新Sigma樣點(diǎn)為

y=g(χi)i=0,1,2,…,2L

(25)

其中y的均值和方差分別為

(26)

(27)

通常非負(fù)定的矩陣Py, 在受到計(jì)算誤差等因素干擾后, 會(huì)變得不對(duì)稱或負(fù)定, 從而影響濾波的收斂性和穩(wěn)定性, 進(jìn)而導(dǎo)致濾波器的發(fā)散[16-18]。針對(duì)以上問題, 可將Py分解為

(28)

Py的平方根矩陣Sy可通過Cholesky分解計(jì)算得到, 而它在濾波更新算法中的導(dǎo)出可通過QR分解實(shí)現(xiàn)[5]。在濾波過程中, 用Sy代替Py參加遞推運(yùn)算, 可保證協(xié)方差陣的非負(fù)定性, 從而實(shí)現(xiàn)濾波的有效性。

2.3 迭代測(cè)量更新的平方根CDKF算法(ISR-CDKF)

ISR-CDKF算法具體步驟如下：

1) 初始化

(29)

2) 確定權(quán)值

(30)

3) 計(jì)算時(shí)間更新所需的sigma點(diǎn)集

(31)

4) 時(shí)間更新

(32)

(33)

(34)

5) 計(jì)算測(cè)量更新所需的sigma點(diǎn)

(35)

6) 量測(cè)更新

(36)

forj=0 ∶n

end

(45)

(46)

3 仿真研究

3.1 仿真條件

假設(shè)系統(tǒng)的狀態(tài)x的初始狀態(tài)x(0)=0； 初始水平失準(zhǔn)角φx=φy=0.6°, 方位失準(zhǔn)角φz=10°； 陀螺常值漂移為0.02 (°)/h, 隨機(jī)漂移為0.01 (°)/h； 加速度計(jì)零偏為1×10-4g, 隨機(jī)偏差為0.5×10-4g； 速度測(cè)量誤差為0.1 m/s； 當(dāng)?shù)氐乩砭暥葹?5.779 6°。

根據(jù)上述仿真條件, 分別利用EKF、 CDKF和ISR-CDKF濾波算法, 對(duì)捷聯(lián)慣導(dǎo)系統(tǒng)大方位失準(zhǔn)角初始對(duì)準(zhǔn)非線性誤差模型進(jìn)行濾波仿真, 仿真結(jié)果如圖1和圖2所示。

圖1 北向失準(zhǔn)角誤差曲線 圖2 天向失準(zhǔn)角估計(jì)誤差曲線

3.2 仿真結(jié)果分析

由于水平失準(zhǔn)角(東向和北向)的估計(jì)結(jié)果相差無幾, 因此這里只給出北向的情況。從圖1可看出, 對(duì)水平失準(zhǔn)角的估計(jì), EKF、 CDKF和ISR-CDKF3種濾波算法的結(jié)果基本一致, 收斂速度均較快, 都能得到較高的估計(jì)精度。從圖2可看出, 對(duì)大方位失準(zhǔn)角的估計(jì), CDKF和ISR-CDKF在收斂速度和估計(jì)精度上, 都得到了比EKF更好的效果。同時(shí), ISR-CDKF與CDKF相比具有更高的濾波精度, 計(jì)算量小, 提高了算法的數(shù)值穩(wěn)定性和運(yùn)算效率。

4 結(jié) 論

筆者提出了一種基于迭代測(cè)量更新的平方根CDKF, 該算法通過迭代狀態(tài)估計(jì)值改善非線性近似精度, 從而提高濾波精度。ISR-CDKF不僅具有CDKF無需計(jì)算雅可比矩陣的優(yōu)點(diǎn), 而且更加易于實(shí)現(xiàn)。ISR-CDKF通過對(duì)協(xié)方差平方根進(jìn)行遞推更新, 避免了協(xié)方差矩陣負(fù)定的情況, 提高了算法的數(shù)值穩(wěn)定性, 而且比CDKF減小了計(jì)算量。仿真結(jié)果充分證實(shí)了其在大方位失準(zhǔn)角初始對(duì)準(zhǔn)中的可行性與優(yōu)越性, 為實(shí)際應(yīng)用提供了理論依據(jù)和計(jì)算方法。

參考文獻(xiàn)：

[1]劉國(guó)海, 李康吉. 基于PDA的GPS定位精度提高方法 [J]. 江蘇大學(xué)學(xué)報(bào)： 自然科學(xué)版. 2005, 26(5)： 448-452.

LIU Guo-hai, LI Kang-ji. Improving GPS Positioning Accuracy Based on PDA [J]. Journal of Jiangsu University: National Science Edition, 2005, 26:(5): 448-452.

[2]NORGAARD M, POUISEN NK, RAVN O. Advances in Derivative-Free State Estimation for Nonlinear Systems [R]. Tech Rep IMM-REP-1998-15: Tech Univ of Denmark, 2000.

[3]NORGAARD M, POUISEN N K, RAVN O. New Developments in State Estimation for Nonlinear System [J]. Automatic, 2000, 36(11)： 1627-1638.

[4]JULIER S J, UHLMANN J K. Unscented Filtering and Nonlinear Estimation [J]. Proceedings of IEEE, 2004, 92(3): 401-422.

[5]張賢達(dá). 矩陣分析與應(yīng)用 [M]. 北京: 清華大學(xué)出版社, 2004.

ZHANG Xian-da. Analysis and Applications [M]. Beijing: Tsinghua Press, 2004.

[6]肖嵩. 無線信道中的聯(lián)合信源信道編碼研究 [D]. 西安: 西安電子科技大學(xué)綜合業(yè)務(wù)網(wǎng)國(guó)家重點(diǎn)實(shí)驗(yàn)室, 2004.

XIAO Song. Redearches on Joint Source Channel Coding in Wireless Channel [D]. Xi’an: The State Key Laboratoy of Integrated Services Networks, Xidian University, 2004.

[7]KAZUFUMI ITO, KAIGI XIONG. Gaussian Filters for Nonlinear Filtering Problems [J]. IEEE Transactions on Automatic Control, 2000, 45(5): 910-927.

[8]VAN DER MERWE R. Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models [D]. OGI School of Science & Engineering, University of Oregon Health & Science, 2004: 62-74.

[9]STANKOVIC J A, RAMAMRITHAM K, SPURI M. Deadline Scheduling for Real-Time Systems: EDF and Related Algorithms [M]. USA: Kluwer Academic Publishers, 1998.

[10]王海勃, 陳紅林, 韓惠珍. CDKF在GPS/SINS組合導(dǎo)航系統(tǒng)非線性模型中的應(yīng)用 [J]. 現(xiàn)在電子技術(shù), 2011, 34(11)： 19-23.

WANG Hai-bo, CHEN Hong-lin, HAN Hui-zhen. Application of CDKF Method in GPS/SINS Integrated Navigation Systems Nonlinear Model [J]. Modern Electronics Technique, 2011, 34(11)： 19-23.

[11]王小旭, 趙琳, 夏全喜, 等. 基于Unscented變換的強(qiáng)跟蹤濾波器 [J]. 控制與決策, 2010, 25(7)： 1063-1068.

WANG Xiao-xu, ZHAO Lin, XIA Quan-xi, et al. Strong Tracking Filter Based on Unscented Transformation [J]. Control and Decision, 2010, 25(7): 1063-1068.

[12]歸慶明, 許阿裴, 韓松輝. 分步抗差自適應(yīng)濾波及其在GPS動(dòng)態(tài)導(dǎo)航中的應(yīng)用 [J]. 武漢大學(xué)學(xué)報(bào): 信息科學(xué)版, 2009, 34(6): 719-723.

GUI Qing-ming, XU A-pei, HAN Song-hui. A Stepped Robust and Adaptive Filtering and Its Applications in GPS Kinematic Navigation [J]. Geomatics and Information Science of Wuhan University, 2009, 34(6): 719-723.

[13]高為廣, 何海波, 陳金平. 自適應(yīng)UKF算法及其在GPS/INS組合導(dǎo)航中的應(yīng)用 [J]. 北京理工大學(xué)學(xué)報(bào), 2009, 34(6): 719-723.

GAO Wei-guang, HE Hai-bo, CHEN Jin-ping. An Adaptive UKF Algorithm and Its Application for GPS/INS Integrated Avigation System [J]. Transactions of Beijing Institute of Technology, 2009, 34(16): 719-723.

[14]LIU G, ZHAO N, LIU C A. RTT Estimation Based on Adaptive Filtering [C]//Proceedings of the 1st International Conference on Information Science and Engineering. Nanjing, China, [s.n.]. 2009: 1842-1846.

[15]孫楓, 唐李軍. Cubature粒子濾波 [J]. 系統(tǒng)工程與電子技術(shù), 2011, 33(11): 2554-2557.

SUN Feng, TANG Li-jun. Cubature Particle Filter [J]. Systems Engineering and Electronic, 2011, 33(11): 2554-2557.

[16]王虎, 王解先, 白貴霞, 等. 改進(jìn)的漸消卡爾曼濾波在GPS動(dòng)態(tài)定位中的應(yīng)用 [J]. 同濟(jì)大學(xué)學(xué)報(bào): 自然科學(xué)版, 2011, 39(1): 124-128.

WANG Hu, WANG Jie-xian, BAI Gui-xia, et al. An Improved Fading Kalman Filter and Its Application to GPS Kinematic Positioning [J]. Journal of Tongji University: Natural Science Edition, 2011, 39(1): 124-128.

[17]穆靜, 蔡遠(yuǎn)利. 迭代容積卡爾曼濾波算法及其應(yīng)用 [J]. 系統(tǒng)工程與電子技術(shù), 2011, 33(7): 1454-1457.

MU Jing, CAI Yuan-li. Iterated Cubature Kalman Filter and Its Application [J]. Systems Engineering and Electronic, 2011, 33(7): 1454-1457.

[18]霍庚, 杜軍波, 丁宏升, 等. 光線SINS/GPS組合導(dǎo)航系統(tǒng)分析 [J]. 吉林大學(xué)學(xué)報(bào): 信息科學(xué)版, 2012, 30(2): 144-150.

HUO Geng, DU Jun-bo, DING Hong-sheng, et al. Analysis and Study of Optical SINS/GPS Integrated Navigation System [J]. Journal of Jilin University: Information Science Edition, 2012, 30(2): 144-150.