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

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

0 引 言

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

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

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

1 大方位失準角初始對準誤差模型

(1)

其中φx、φy、φz為3個歐拉角； 當φx,φy很小時, cosφx=cosφy=1, sinφx≈φx, sinφy≈φy, 則

(2)

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

(3)

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

(4)

(5)

(6)

(7)

(8)

1.2 速度誤差方程

SINS的理論速度微分方程為

(9)

SINS導航解算的速度微分方程為

(10)

(11)

1.3 捷聯(lián)慣導系統(tǒng)非線性初始對準模型

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

(12)

令狀態(tài)向量

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

以SINS兩水平速度誤差Z=δVt作為觀測量, 建立非線性對準模型如下

(13)

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

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

2.1 中心差分變換

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

y=f(x)

(14)

(15)

(16)

(17)

(18)

(19)

(20)

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

2.2 平方根U變換

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

y=g(x)

(24)

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

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

(25)

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

(26)

(27)

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

(28)

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

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

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

1) 初始化

(29)

2) 確定權(quán)值

(30)

3) 計算時間更新所需的sigma點集

(31)

4) 時間更新

(32)

(33)

(34)

5) 計算測量更新所需的sigma點

(35)

6) 量測更新

(36)

forj=0 ∶n

end

(45)

(46)

3 仿真研究

3.1 仿真條件

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

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

圖1 北向失準角誤差曲線 圖2 天向失準角估計誤差曲線

3.2 仿真結(jié)果分析

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

4 結(jié) 論

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

參考文獻：

[1]劉國海, 李康吉. 基于PDA的GPS定位精度提高方法 [J]. 江蘇大學學報： 自然科學版. 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]張賢達. 矩陣分析與應(yīng)用 [M]. 北京: 清華大學出版社, 2004.

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

[6]肖嵩. 無線信道中的聯(lián)合信源信道編碼研究 [D]. 西安: 西安電子科技大學綜合業(yè)務(wù)網(wǎng)國家重點實驗室, 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組合導航系統(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變換的強跟蹤濾波器 [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動態(tài)導航中的應(yīng)用 [J]. 武漢大學學報: 信息科學版, 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組合導航中的應(yīng)用 [J]. 北京理工大學學報, 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]王虎, 王解先, 白貴霞, 等. 改進的漸消卡爾曼濾波在GPS動態(tài)定位中的應(yīng)用 [J]. 同濟大學學報: 自然科學版, 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]穆靜, 蔡遠利. 迭代容積卡爾曼濾波算法及其應(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組合導航系統(tǒng)分析 [J]. 吉林大學學報: 信息科學版, 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.