一種基于迭代容積卡爾曼濾波姿態(tài)估計(jì)方法
【專(zhuān)利摘要】本發(fā)明公開(kāi)了一種基于迭代容積卡爾曼濾波姿態(tài)估計(jì)方法，包括以下幾個(gè)步驟：步驟一：采集陀螺與星敏感器的輸出數(shù)據(jù)作為量測(cè)量；步驟二：確定狀態(tài)向量和量測(cè)向量；步驟三：在k-1時(shí)刻利用迭代容積卡爾曼濾波估計(jì)出k時(shí)刻的誤差四元數(shù)矢量部分以及陀螺漂移誤差；步驟四：對(duì)于k時(shí)刻的估計(jì)求出四元數(shù)估計(jì)及陀螺漂移估計(jì)，對(duì)姿態(tài)及陀螺漂移進(jìn)行校正，得到修正后k時(shí)刻姿態(tài)及陀螺漂移；步驟五：姿態(tài)估計(jì)非線性離散系統(tǒng)的運(yùn)行時(shí)間為M，若k＝M，則輸出姿態(tài)及陀螺漂移的結(jié)果；若k＜M，則重復(fù)步驟三至步驟四，直至姿態(tài)估計(jì)系統(tǒng)運(yùn)行結(jié)束。本發(fā)明具有估計(jì)精度高、計(jì)算量少的優(yōu)點(diǎn)。
【專(zhuān)利說(shuō)明】一種基于迭代容積卡爾曼濾波姿態(tài)估計(jì)方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明屬于利用非線性濾波技術(shù)及迭代理論進(jìn)行姿態(tài)估計(jì)的【技術(shù)領(lǐng)域】，特別涉及一種基于迭代容積卡爾曼濾波的姿態(tài)估計(jì)方法。
【背景技術(shù)】
[0002]飛行器姿態(tài)估計(jì)的精度直接影響飛行器姿態(tài)控制的精度，因此，姿態(tài)估計(jì)是姿態(tài)控制的關(guān)鍵技術(shù)之一。姿態(tài)估計(jì)方法主要分為兩類(lèi):確定性算法和狀態(tài)估計(jì)法。確定性算法的優(yōu)點(diǎn)在于可以根據(jù)某一時(shí)刻的一組量測(cè)信息直接求解出該時(shí)刻下飛行器的姿態(tài)矩陣，算法簡(jiǎn)單，物理意義明確，但是需要知道當(dāng)前時(shí)刻的至少兩個(gè)獨(dú)立的觀測(cè)矢量，即準(zhǔn)確的量測(cè)信息，而在實(shí)際應(yīng)用中由于其他因素的干擾很難保證觀測(cè)矢量的絕對(duì)準(zhǔn)確。與這類(lèi)方法相反，狀態(tài)估計(jì)法是以狀態(tài)空間模型為基礎(chǔ)的遞推濾波算法，可以將一些非姿態(tài)參數(shù)作為狀態(tài)進(jìn)行估計(jì)，一定程度上克服了不確定因素對(duì)姿態(tài)估計(jì)精度的影響，能夠提高姿態(tài)估計(jì)的精度。
[0003]陀螺與星敏感器組成的非線性姿態(tài)估計(jì)系統(tǒng)由于測(cè)姿精度高，被廣泛的用于飛行器中。歐拉角、修正羅德里格斯參數(shù)、方向余弦、四元數(shù)等是飛行器的主要姿態(tài)描述參數(shù)。擴(kuò)展卡爾曼濾波(Extended Kalman Filter, MEKF)由于其結(jié)構(gòu)簡(jiǎn)單、易于實(shí)現(xiàn)等優(yōu)點(diǎn)被廣泛應(yīng)用于姿態(tài)估計(jì)中。但是，EKF存在一定的局限性:1)由模型線性化引入的截?cái)嗾`差會(huì)導(dǎo)致濾波精度下降，且需要計(jì)算較為復(fù)雜的雅克比矩陣；2)當(dāng)初始狀態(tài)誤差較大或系統(tǒng)模型非線性程度較高時(shí)，會(huì)嚴(yán)重影響濾波精度甚至導(dǎo)致濾波發(fā)散。

【發(fā)明內(nèi)容】

[0004]本發(fā)明的目的是為了提高姿態(tài)估計(jì)的精度，設(shè)計(jì)了一種基于迭代容積卡爾曼濾波姿態(tài)估計(jì)方法。
[0005]本發(fā)明是通過(guò)以下技術(shù)方案實(shí)現(xiàn)的:
[0006]一種基于迭代容積卡爾曼濾波姿態(tài)估計(jì)方法，其特征在于，包括以下幾個(gè)步驟:
[0007]步驟一:采集飛行器運(yùn)動(dòng)過(guò)程中陀螺與星敏感器的輸出數(shù)據(jù)作為量測(cè)量；
[0008]步驟二:確定系統(tǒng)的狀態(tài)向量和量測(cè)向量；
[0009]步驟2.1建立飛行器姿態(tài)估計(jì)系統(tǒng)的非線性誤差四元數(shù)方程，確定系統(tǒng)的狀態(tài)向量;
[0010]非線性誤差四元數(shù)方程為:
[0011]
【權(quán)利要求】
1.一種基于迭代容積卡爾曼濾波姿態(tài)估計(jì)方法，其特征在于，包括以下幾個(gè)步驟: 步驟一:采集飛行器運(yùn)動(dòng)過(guò)程中陀螺與星敏感器的輸出數(shù)據(jù)作為量測(cè)量； 步驟二:確定系統(tǒng)的狀態(tài)向量和量測(cè)向量； 步驟2.1建立飛行器姿態(tài)估計(jì)系統(tǒng)的非線性誤差四元數(shù)方程，確定系統(tǒng)的狀態(tài)向量； 非線性誤差四元數(shù)方程為:
2.根據(jù)權(quán)利要求1所述的一種基于迭代容積卡爾曼濾波姿態(tài)估計(jì)方法，其特征在于:步驟二中，陀螺測(cè)量噪聲標(biāo)準(zhǔn)差為σ v = 0.05° /h，陀螺漂移噪聲標(biāo)準(zhǔn)差為σ； = 0-003 ，輸出頻率為IOOHz，星敏感器測(cè)量噪聲標(biāo)準(zhǔn)差為σ s = 18"，輸出頻率為IHz，初始陀螺漂移β = [I I 1]T。/h，初始姿態(tài)角誤差設(shè)定為[0.5 0.5 15]τ。。
3.根據(jù)權(quán)利要求1或2所述的一種基于迭代容積卡爾曼濾波姿態(tài)估計(jì)方法，其特征在于:步驟三中，濾波的初始姿態(tài)與陀螺漂移的估計(jì)值均設(shè)為零，初始方差陣分別設(shè)為(0.2。)2Ι3Χ3 和(1.2。/h)2I3X3，Nmax = 3。
【文檔編號(hào)】G01C21/20GK103900574SQ201410136157
【公開(kāi)日】2014年7月2日 申請(qǐng)日期:2014年4月4日 優(yōu)先權(quán)日:2014年4月4日
【發(fā)明者】錢(qián)華明, 黃蔚, 沈忱, 孫龍, 富振鐸, 彭宇 申請(qǐng)人:哈爾濱工程大學(xué)