基于發(fā)射慣性坐標(biāo)系的空天飛行器組合導(dǎo)航魯棒濾波方法
【專利摘要】本發(fā)明公布了一種基于發(fā)射慣性坐標(biāo)系的空天飛行器組合導(dǎo)航魯棒濾波方法，屬于飛行器組合導(dǎo)航【技術(shù)領(lǐng)域】。該方法包含以下步驟：首先以發(fā)射慣性坐標(biāo)系為參考坐標(biāo)系，進(jìn)行慣性導(dǎo)航算法編排，建立飛行器在發(fā)射慣性坐標(biāo)系下的位置、速度、姿態(tài)解算模型；在此基礎(chǔ)上，建立了包含基本導(dǎo)航參數(shù)誤差和慣性儀表誤差在內(nèi)的導(dǎo)航系統(tǒng)狀態(tài)方程，根據(jù)原始星敏感器測(cè)量信息經(jīng)過轉(zhuǎn)換模塊得到姿態(tài)線性量測(cè)方程；最后采用魯棒濾波方法對(duì)所建立的狀態(tài)方程中各個(gè)狀態(tài)量進(jìn)行估計(jì)和修正。本發(fā)明能夠有效減小地球物理場對(duì)飛行器導(dǎo)航系統(tǒng)的影響，且保證在飛行器導(dǎo)航系統(tǒng)模型不準(zhǔn)確的情況下，濾波器能夠穩(wěn)定工作，提高組合導(dǎo)航系統(tǒng)的精度。
【專利說明】基于發(fā)射慣性坐標(biāo)系的空天飛行器組合導(dǎo)航魯棒濾波方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明公開了一種基于發(fā)射慣性坐標(biāo)系的空天飛行器組合導(dǎo)航魯棒濾波方法，涉 及組合導(dǎo)航【技術(shù)領(lǐng)域】。

【背景技術(shù)】
[0002] 近年來，隨著航空航天技術(shù)的發(fā)展，近空間高超聲速飛行器、空天飛行器等新型飛 行器呈現(xiàn)長航時(shí)、大空域飛行特點(diǎn)，其對(duì)導(dǎo)航系統(tǒng)性能的要求日益提高。
[0003] 傳統(tǒng)航空飛行器通常以地理坐標(biāo)系為參考坐標(biāo)系進(jìn)行捷聯(lián)慣性導(dǎo)航算法編排，在 捷聯(lián)慣性導(dǎo)航系統(tǒng)算法編排中對(duì)重力場、曲率半徑變化等采用直接忽略或簡化處理，主要 考慮到地理系編排較為直觀，且在短時(shí)間、短距離飛行中，該處理方法對(duì)導(dǎo)航系統(tǒng)精度影響 不大。而近空間高超飛行器、空天飛行器等新型飛行器飛行時(shí)間長、飛行距離遠(yuǎn)，地球重力、 曲率半徑等變化較大。這些因素如果在飛行器飛行中忽略或簡化處理，將會(huì)在很大程度上 影響導(dǎo)航精度。
[0004] 此外，單純依靠慣性導(dǎo)航系統(tǒng)提供的導(dǎo)航信息難以滿足飛行器的導(dǎo)航需求，需要 采用組合導(dǎo)航方式提供導(dǎo)航信息。在導(dǎo)航信息融合領(lǐng)域，kalman濾波作為一種線性最小方 差估計(jì)方法，得到了廣泛的應(yīng)用，其要求系統(tǒng)模型必須準(zhǔn)確已知且輸入噪聲為嚴(yán)格的Guass 噪聲。而空天飛行器面臨的飛行環(huán)境復(fù)雜，不確定因素較多，通常會(huì)對(duì)導(dǎo)航系統(tǒng)帶來不可預(yù) 知的影響，難以獲得導(dǎo)航系統(tǒng)的精確模型。在上述條件下，Kalman濾波器的濾波性能將受 到限制。因此，研究基于發(fā)射慣性坐標(biāo)系的空天飛行器組合導(dǎo)航魯棒濾波方法，一方面將能 有效減小因地球物理場描述不準(zhǔn)確而對(duì)導(dǎo)航系統(tǒng)帶來的影響，另一方面能提高組合導(dǎo)航系 統(tǒng)的魯棒性能，具有突出的應(yīng)用前景。


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

[0005] 本發(fā)明所要解決的技術(shù)問題是：針對(duì)現(xiàn)有技術(shù)的缺陷，提供一種基于發(fā)射慣性坐 標(biāo)系的空天飛行器組合導(dǎo)航魯棒濾波方法，本發(fā)明以發(fā)射慣性坐標(biāo)系為參考坐標(biāo)系，進(jìn)行 慣性導(dǎo)航算法編排，并采用魯棒濾波方法對(duì)導(dǎo)航系統(tǒng)誤差進(jìn)行估計(jì)和修正。
[0006] 本發(fā)明為解決上述技術(shù)問題采用以下技術(shù)方案：
[0007] -種基于發(fā)射慣性坐標(biāo)系的空天飛行器組合導(dǎo)航魯棒濾波方法，所述方法具體步 驟包括：
[0008] 步驟一、以發(fā)射慣性坐標(biāo)系為參考坐標(biāo)系，進(jìn)行慣性導(dǎo)航算法編排，建立飛行器在 發(fā)射慣性坐標(biāo)系下的速度、位置、姿態(tài)解算模型；
[0009] 步驟二、根據(jù)步驟一中飛行器在發(fā)射慣性坐標(biāo)系下的速度、位置、姿態(tài)解算模型， 得到對(duì)應(yīng)的速度、位置、姿態(tài)誤差模型，結(jié)合慣性儀表的誤差模型，構(gòu)建包含基本導(dǎo)航參數(shù) 誤差和慣性儀表誤差在內(nèi)的慣性導(dǎo)航系統(tǒng)的誤差狀態(tài)方程以及線性化量測(cè)方程；
[0010] 步驟三、對(duì)步驟二得出的慣性導(dǎo)航系統(tǒng)的誤差狀態(tài)方程和線性化量測(cè)方程進(jìn)行離 散化處理以及狀態(tài)量、測(cè)量量的更新，采用魯棒濾波方法對(duì)所建立的狀態(tài)方程中各個(gè)狀態(tài) 量進(jìn)行估計(jì)和修正。
[0011] 作為本發(fā)明的進(jìn)一步優(yōu)選方案，步驟一所述的解算模型的表達(dá)形式具體為：
[0012] (101)飛行器在發(fā)射慣性坐標(biāo)系下的速度解算模型為：
[0013]

【權(quán)利要求】
1. 一種基于發(fā)射慣性坐標(biāo)系的空天飛行器組合導(dǎo)航魯棒濾波方法，其特征在于，所述 方法具體步驟包括： 步驟一、以發(fā)射慣性坐標(biāo)系為參考坐標(biāo)系，進(jìn)行慣性導(dǎo)航算法編排，建立飛行器在發(fā)射 慣性坐標(biāo)系下的速度、位置、姿態(tài)解算模型； 步驟二、根據(jù)步驟一中飛行器在發(fā)射慣性坐標(biāo)系下的速度、位置、姿態(tài)解算模型，得到 對(duì)應(yīng)的速度、位置、姿態(tài)誤差模型，結(jié)合慣性儀表的誤差模型，構(gòu)建包含基本導(dǎo)航參數(shù)誤差 和慣性儀表誤差在內(nèi)的慣性導(dǎo)航系統(tǒng)的誤差狀態(tài)方程以及線性化量測(cè)方程； 步驟三、對(duì)步驟二得出的慣性導(dǎo)航系統(tǒng)的誤差狀態(tài)方程和線性化量測(cè)方程進(jìn)行離散化 處理以及狀態(tài)量、測(cè)量量的更新，采用魯棒濾波方法對(duì)所建立的狀態(tài)方程中各個(gè)狀態(tài)量進(jìn) 行估計(jì)和修正。
2. 如權(quán)利要求1所述的一種基于發(fā)射慣性坐標(biāo)系的空天飛行器組合導(dǎo)航魯棒濾波方 法，其特征在于，步驟一中，所述解算模型的表達(dá)形式具體為： (101) 飛行器在發(fā)射慣性坐標(biāo)系下的速度解算模型為：
其中，v1為飛行器相對(duì)于發(fā)射慣性系的速度，fi1為v1的一階導(dǎo)數(shù)，f b為加速度計(jì)輸出的 比力，G1為地心慣性坐標(biāo)系下的地球萬有引力，g為載體坐標(biāo)系b系到發(fā)射慣性坐標(biāo)系1系 的姿態(tài)轉(zhuǎn)換矩陣，C)為地心慣性坐標(biāo)系i系到發(fā)射慣性坐標(biāo)系1系的姿態(tài)轉(zhuǎn)換矩陣； (102) 飛行器在發(fā)射慣性坐標(biāo)系下的位置解算模型為：
其中，P1為飛行器相對(duì)于發(fā)射慣性坐標(biāo)系的位置，夕為P1的一階導(dǎo)數(shù)； (103) 飛行器在發(fā)射慣性坐標(biāo)系下的姿態(tài)解算模型為：
其中，q為飛行器相對(duì)于發(fā)射慣性系的姿態(tài)四元數(shù)，#為q的一階導(dǎo)數(shù)，瑪|為載體相對(duì) 于發(fā)射慣性系的角速率在載體系上的投影，?表示四元數(shù)乘法。
3. 如權(quán)利要求1所述的一種基于發(fā)射慣性坐標(biāo)系的空天飛行器組合導(dǎo)航魯棒濾波方 法，其特征在于，步驟二所述的慣性導(dǎo)航系統(tǒng)的誤差狀態(tài)方程中，誤差狀態(tài)變量X的具體表 達(dá)形式為： (4)
公式⑷中，δ qi、δ q2、δ q3分別為慣性導(dǎo)航系統(tǒng)誤差中姿態(tài)四元數(shù)誤差δ q的矢量 部分，SPx、SPy、δΡζ分別為慣性導(dǎo)航系統(tǒng)誤差中X軸、Y軸、Z軸方向的位置誤差狀態(tài)量， δνχ、SVy、6\分別為慣性導(dǎo)航系統(tǒng)中X軸、Υ軸、Ζ軸方向的速度誤差狀態(tài)量，εΜ、％、 ε "分別為X軸、Υ軸、Ζ軸方向的陀螺隨機(jī)游走誤差，▽ ax、V ay、V az分別為X軸、Υ軸、Ζ 軸方向的加速度計(jì)隨機(jī)游走誤差。
4. 如權(quán)利要求3所述的一種基于發(fā)射慣性坐標(biāo)系的空天飛行器組合導(dǎo)航魯棒濾波方 法，其特征在于：所述慣性導(dǎo)航系統(tǒng)的誤差狀態(tài)方程具體表達(dá)為：
公式（5)中，X(t)為系統(tǒng)狀態(tài)變量，對(duì)?)為X(t)的一階導(dǎo)數(shù)，A(t)為系統(tǒng)矩陣；B(t) 為噪聲系數(shù)矩陣，W(t)為噪聲矩陣； 根據(jù)星敏感器測(cè)量得出的載體相對(duì)于慣性坐標(biāo)系的姿態(tài)信息，經(jīng)過轉(zhuǎn)換得出載體相對(duì) 于發(fā)射慣性坐標(biāo)系的姿態(tài)信息，采用發(fā)射慣性坐標(biāo)系下姿態(tài)線性化觀測(cè)原理，建立發(fā)射慣 性坐標(biāo)系下的姿態(tài)觀測(cè)量和慣性導(dǎo)航系統(tǒng)誤差狀態(tài)量之間的線性化量測(cè)方程：
公式（6)中，Z(t)為系統(tǒng)姿態(tài)觀測(cè)量矩陣，Ha(t)為姿態(tài)量測(cè)系數(shù)矩陣，Ns(t)為姿態(tài)觀 測(cè)噪聲陣。
5.如權(quán)利要求1所述的一種基于發(fā)射慣性坐標(biāo)系的空天飛行器組合導(dǎo)航魯棒濾波方 法，其特征在于，步驟三中所述的采用魯棒濾波方法對(duì)所建立的狀態(tài)方程中各個(gè)狀態(tài)量進(jìn) 行估計(jì)和修正，其具體步驟包括： (301) 將慣性導(dǎo)航系統(tǒng)的誤差狀態(tài)方程和線性化量測(cè)方程離散化處理：
上述公式中，xk為tk時(shí)刻系統(tǒng)狀態(tài)量，Xg為tkH時(shí)刻系統(tǒng)狀態(tài)量，Ak為tkH時(shí)刻至t k 時(shí)刻系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣，Bk為時(shí)刻至tk時(shí)刻系統(tǒng)的噪聲驅(qū)動(dòng)矩陣，Wg為tk時(shí)刻系 統(tǒng)的噪聲矩陣，Z k為tk時(shí)刻系統(tǒng)姿態(tài)觀測(cè)量矩陣，Hk為tk時(shí)姿態(tài)量測(cè)系數(shù)矩陣，N k為tk時(shí) 刻姿態(tài)觀測(cè)量的噪聲矩陣； (302) 采用魯棒濾波策略對(duì)慣性導(dǎo)航系統(tǒng)誤差進(jìn)行估計(jì)：
上述公式中，Lk為狀態(tài)量的線性組合系數(shù)矩陣，Sk為權(quán)系數(shù)矩陣，?為轉(zhuǎn)換后的權(quán)系數(shù) 矩陣，Kk為tk時(shí)刻帶魯棒濾波因子的濾波增益矩陣，Θ為魯棒濾波因子，pk為t k時(shí)刻帶魯 棒濾波因子的協(xié)方差矩陣，Pk+1為tk+1時(shí)刻帶魯棒濾波因子的協(xié)方差矩陣，Rk為根據(jù)t k時(shí)刻 量測(cè)噪聲的先驗(yàn)知識(shí)得到的正定對(duì)稱矩陣，14+1為tk+1時(shí)刻的濾波估計(jì)值，為tk時(shí)刻的 狀態(tài)估計(jì)值，Qk為根據(jù)tk時(shí)刻系統(tǒng)噪聲的先驗(yàn)知識(shí)得到的正定對(duì)稱矩陣，I為與Pk+1維數(shù)相 同的單位矩陣； (303) 經(jīng)過步驟（302)得到濾波估計(jì)值;后，利用濾波估計(jì)值對(duì)慣性導(dǎo)航系統(tǒng)姿態(tài) 誤差和陀螺隨機(jī)游走誤差進(jìn)行修正。
【文檔編號(hào)】G01C21/20GK104215244SQ201410419869
【公開日】2014年12月17日 申請(qǐng)日期:2014年8月22日 優(yōu)先權(quán)日:2014年8月22日
【發(fā)明者】趙慧, 熊智, 潘加亮, 孫永榮, 郁豐, 劉建業(yè), 許建新, 柏青青, 王融, 王潔, 程嬌嬌, 林愛軍, 戴怡潔, 施麗娟, 孔雪博 申請(qǐng)人:南京航空航天大學(xué)