一種量測干擾下用于飛行器姿態(tài)估計的魯棒遞推濾波方法
【專利摘要】本發(fā)明屬于利用魯棒濾波技術進行飛行器姿態(tài)估計的【技術領域】，涉及一種量測干擾下用于飛行器姿態(tài)估計的魯棒遞推濾波方法。本發(fā)明包括：采集飛行器運動過程中陀螺與星敏感器的輸出數(shù)據(jù)；建立量測干擾下基于姿態(tài)四元數(shù)的飛行器非線性狀態(tài)空間模型；進行時間更新，求得一步狀態(tài)預測和預測方差的上界；進行魯棒遞推濾波量測更新，求得最優(yōu)的濾波增益，進而求出k+1時刻的狀態(tài)估計值和方差的上界，將k+1時刻的狀態(tài)估計中四元數(shù)部分進行強制的歸一化約束；輸出姿態(tài)四元數(shù)及陀螺漂移的結果，完成姿態(tài)估計。本發(fā)明采用基于最小方差的魯棒濾波設計，能夠實現(xiàn)在最小方差意義下最優(yōu)濾波增益設計，有利用提高系統(tǒng)的姿態(tài)估計精度。
【專利說明】—種量測干擾下用于飛行器姿態(tài)估計的魯棒遞推濾波方法
【技術領域】
[0001]本發(fā)明屬于利用魯棒濾波技術進行飛行器姿態(tài)估計的【技術領域】，涉及一種量測干擾下用于飛行器姿態(tài)估計的魯棒遞推濾波方法。
【背景技術】
[0002]由陀螺和星敏感器組合的姿態(tài)估計系統(tǒng)由于定姿精度高被廣泛地應用于飛行器中。歐拉角、修正羅德里格斯參數(shù)、方向余弦、四元數(shù)等是飛行器的主要姿態(tài)描述參數(shù)。四元數(shù)由于計算簡單，無三角函數(shù)的運算，同時又能避免歐拉角的奇異性問題，因此，四元數(shù)常被作為姿態(tài)估計系統(tǒng)中的姿態(tài)描述參數(shù)。針對該系統(tǒng)的四元數(shù)姿態(tài)估計模型，許多基于擴展卡爾曼濾波(ExtendedKalmanFilter, EKF)的姿態(tài)估計方法被提出，如加性擴展卡爾曼濾波(Additive Extended Kalman Filter, AEKF)和乘性擴展卡爾曼濾波(MultiplicativeExtended Kalman Filter7MEKF)。然而,擴展卡爾曼濾波僅僅適合于帶加性噪聲的精確已知的系統(tǒng)模型。如果系統(tǒng)模型中存在模型不確定的情況，該算法的性能將會受到嚴重的影響。因此，許多帶模型不確定的非線性濾波算法被發(fā)展，如H00濾波，集值非線性濾波，魯棒濾波設計等，其中，基于最小方差的魯棒濾波設計被證明是一種用于解決系統(tǒng)帶模型不確定情況的有效處理手段，然而大多數(shù)基于最小方差的魯棒濾波都是針對系統(tǒng)只存在一種模型不確定的情況。如果系統(tǒng)中存在兩種或是兩種以上的模型不確定，上述的魯棒濾波算法將會失效。
[0003]針對姿態(tài)估計系統(tǒng)，系統(tǒng)狀態(tài)方程中存在狀態(tài)與高斯白噪聲耦合的乘性噪聲，該噪聲的方差未知，因此被看成是一種狀態(tài)模型不確定，一種魯棒擴展卡爾曼濾波(RobustExtended Kalman Filter, REKF)被提出，但是沒有考慮姿態(tài)估計系統(tǒng)存在其他模型不確定的情況。除了乘性噪聲，由于飛行器在運行過程中存在抖動或是振動的影響，未知的量測干擾將不可避免地出現(xiàn)在姿態(tài)估計系統(tǒng)中，從而導致了測量誤差。雖然這種量測誤差會降低姿態(tài)估計系統(tǒng)的性能以及導致姿態(tài)信息不準確，但是在姿態(tài)估計濾波算法中很少的研究針對這種帶未知量測干擾的情況。有研究指出可以將這種未知量測干擾看成是均值與方差確定的高斯噪聲，然而在實際應用中，這種未知量測干擾的先驗信息往往是無法知道的，僅僅只會知道它的取值范圍，因此，它應該被是看成一種有范圍的量測模型不確定。為了提高姿態(tài)估計系統(tǒng)的估計精度和魯棒性，有必要研究在姿態(tài)估計系統(tǒng)中存在乘性噪聲以及未知量測干擾這兩種模型不確定的情況下的魯棒濾波設計問題。

【發(fā)明內容】

[0004]本發(fā)明的目的是為了提高姿態(tài)估計的精度和增強系統(tǒng)的魯棒性，提出了一種量測干擾下用于飛行器姿態(tài)估計的魯棒遞推濾波方法(Robust Recursive Filter, RRF)。
[0005]本發(fā)明的目的是這樣實現(xiàn)的:
[0006](I)采集飛行器運動過程中陀螺與星敏感器的輸出數(shù)據(jù)；
[0007](2)建立量測干擾下基于姿態(tài)四元數(shù)的飛行器非線性狀態(tài)空間模型；[0008](2.1)建立飛行器姿態(tài)估計系統(tǒng)的狀態(tài)方程；
[0009]將姿態(tài)四元數(shù)qk和陀螺漂移β k組成維數(shù)為η的狀態(tài)變量&: ?/，建立基于姿態(tài)四元數(shù)的飛行器狀態(tài)方程為:
【權利要求】
1.一種量測干擾下用于飛行器姿態(tài)估計的魯棒遞推濾波方法，其特征在于: (1)采集飛行器運動過程中陀螺與星敏感器的輸出數(shù)據(jù)； (2)建立量測干擾下基于姿態(tài)四元數(shù)的飛行器非線性狀態(tài)空間模型； (2.1)建立飛行器姿態(tài)估計系統(tǒng)的狀態(tài)方程； 將姿態(tài)四元數(shù)qk和陀螺漂移β k組成維數(shù)為η的狀態(tài)變量^ = I^/ P 建立基于姿態(tài)四元數(shù)的飛行器狀態(tài)方程為:
2.根據(jù)權利要求1所述的一種量測干擾下用于飛行器姿態(tài)估計的魯棒遞推濾波方法，其特征在于:步驟(2)中陀螺測量噪聲標準差為Ov = 26875X10_7rad/s1/2，陀螺漂移噪聲標準差為σ u = 89289Χ 10-lclrad/s3/2，陀螺的采樣周期為At = 025s ;星敏感器測量噪聲標準差為Os= 18"，輸出頻率為IHz ;星敏感器的參考矢量設為？'=[1 O Of, ?2=[0 I Ofand ?3=[0 O ij 初始陀螺漂移 β = [0.1 0.1 0.1]T° /h ； σ = 10" , j = χ, y, ζ。
3.根據(jù)權利要求1或2所述的一種量測干擾下用于飛行器姿態(tài)估計的魯棒遞推濾波方法，其特征在于:步驟(3)中初始狀態(tài)估計值為元?=[0 0 0 1 0 0 Of;初始方差陣設為 P。。= diag([(0.1。)2 (0.1。)2 (0.1。)2 (0.1。)2 (0.2° /h)2 (0.2° /h)2 (0.2。/h) ] ；Ak = 0nXn ;為 λ = 0.0001，ε = 01。
4.根據(jù)權利要求3所述的一種量測干擾下用于飛行器姿態(tài)估計的魯棒遞推濾波方法，其特征在于:步驟⑷中Ck+1 = Omxn ；μ = 0.0001 ; ε i = 0.1。
5.根據(jù)權利要求4所述的一種量測干擾下用于飛行器姿態(tài)估計的魯棒遞推濾波方法，其特征在于:步驟(5)中M = 1000。
【文檔編號】G05B13/04GK104020671SQ201410234813
【公開日】2014年9月3日 申請日期:2014年5月30日 優(yōu)先權日:2014年5月30日
【發(fā)明者】錢華明, 黃蔚, 沈忱, 錢林琛, 孫龍, 彭宇, 徐祥 申請人:哈爾濱工程大學