一種基于采樣點(diǎn)繼承策略的火星著陸器大氣進(jìn)入段導(dǎo)航方法
【專利摘要】一種基于采樣點(diǎn)繼承策略的火星著陸器大氣進(jìn)入段導(dǎo)航方法，本發(fā)明涉及導(dǎo)航制導(dǎo)與控制【技術(shù)領(lǐng)域】，具體涉及一種火星著陸器大氣進(jìn)入段導(dǎo)航方法。本發(fā)明為了解決利用已有的濾波方法進(jìn)行著陸器狀態(tài)估計(jì)時(shí)精度不高的問題。本發(fā)明在濾波起始周期，利用無跡卡爾曼濾波的采樣點(diǎn)生成規(guī)則給出采樣點(diǎn)和采樣點(diǎn)權(quán)值；并根據(jù)傳播后的采樣點(diǎn)和生成的權(quán)值給出著陸器狀態(tài)預(yù)測(cè)值和陸器狀態(tài)方差Px；根據(jù)無跡卡爾曼濾波規(guī)則，計(jì)算觀測(cè)量預(yù)測(cè)值和方差Py，以及協(xié)方差Pxy；測(cè)量更新，修正著陸器狀態(tài)的估計(jì)值；根據(jù)著陸器狀態(tài)估計(jì)以及著陸器狀態(tài)方差修改傳播后的采樣點(diǎn)ξi，得到修正值的采樣點(diǎn)本發(fā)明具有更高的狀態(tài)估計(jì)精度。本發(fā)明適用于火星著陸器大氣進(jìn)入段的導(dǎo)航。
【專利說明】一種基于采樣點(diǎn)繼承策略的火星著陸器大氣進(jìn)入段導(dǎo)航方 法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及導(dǎo)航制導(dǎo)與控制【技術(shù)領(lǐng)域】，具體涉及一種火星著陸器大氣進(jìn)入段導(dǎo)航 方法。

【背景技術(shù)】
[0002] 為了最大化探測(cè)任務(wù)的科學(xué)回報(bào)，未來的火星探測(cè)任務(wù)，將向著高精度、定點(diǎn)著陸 發(fā)展，這就需要提高著陸器的導(dǎo)航、控制精度。而大氣進(jìn)入是著陸過程中對(duì)最終著陸精度影 響較大的階段，也是火星著陸探測(cè)任務(wù)中最具挑戰(zhàn)性的階段之一，在這一階段，動(dòng)力學(xué)模型 具有很強(qiáng)的非線性，著陸器進(jìn)入點(diǎn)、氣動(dòng)力參數(shù)和大氣密度等參數(shù)存在著不確定性，諸多因 素使大氣進(jìn)入階段狀態(tài)估計(jì)成為難點(diǎn)。
[0003] 目前能夠在大氣進(jìn)入段實(shí)時(shí)應(yīng)用的估計(jì)手段仍然是擴(kuò)展卡爾曼濾波器和 Unscented Kalman Filter (UKF)濾波器。由于進(jìn)入段動(dòng)力學(xué)的強(qiáng)非線性，粒子濾波等非 參數(shù)估計(jì)方法無疑是解決狀態(tài)估計(jì)問題的有效的手段，但是有限的星載計(jì)算資源限制了 粒子數(shù)目，也就無法發(fā)揮粒子濾波的效能。正是出于這樣的考慮，一些學(xué)者致力于減少 粒子數(shù)目，以達(dá)到減少計(jì)算量的目的。德州農(nóng)工大學(xué)的Parikshit Dutta提出一種結(jié)合 Frobenius-Perron算法（F-P算法）與貝葉斯公式的濾波算法，首先利用F-P算法及非線 性動(dòng)力學(xué)獲得先驗(yàn)概率密度，之后利用觀測(cè)量及貝葉斯公式對(duì)先驗(yàn)密度修正，獲得后驗(yàn)概 率密度。在獲得概率密度時(shí)同樣要使用大量的采樣點(diǎn)作為初始條件對(duì)動(dòng)力學(xué)公式進(jìn)行積 分，但由于F-P算法可根據(jù)采樣點(diǎn)的隨時(shí)間演化規(guī)律確定各采樣點(diǎn)的權(quán)重，使得其相比于 粒子濾波，所需要的采樣點(diǎn)數(shù)量大為降低，計(jì)算量有大幅度下降，但仍沒有達(dá)到實(shí)時(shí)估計(jì)的 要求。Abhishek Haider等針對(duì)高超音速飛行器狀態(tài)估計(jì)問題，提出利用Liouville方程 遞推系統(tǒng)狀態(tài)概率密度分布，提高計(jì)算效率。此外，Parikshit Dutta還提出結(jié)合F-P算法 和Karhunen-LoSve展開式的濾波算法。該算法首先利用F-P算法及動(dòng)力學(xué)獲得先驗(yàn)概率 密度，再通過采樣點(diǎn)及Karhunen-LMve展開式對(duì)狀態(tài)量及觀測(cè)量不確定性進(jìn)行更新。在算 法實(shí)施中，采樣點(diǎn)的數(shù)量仍是影響濾波精度的關(guān)鍵因素，為獲得較為精確的結(jié)果，需要較多 的采樣點(diǎn)支持。雖然相比于粒子濾波，該算法的計(jì)算量有所下降，但距離實(shí)際應(yīng)用仍有一定 距離。
[0004] Gau s s-Hermi t e濾波方法在解決非線性問題時(shí)有較高的估計(jì)精度，但其計(jì)算相對(duì) 復(fù)雜，不適合高維問題。近年來提出的Sparse-grid濾波方法基于Sparse-grid積分，可以 按照指定的精度逼近多項(xiàng)式概率密度函數(shù)積分，且計(jì)算量大幅度優(yōu)于粒子濾波等方法。


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

[0005] 本發(fā)明為了解決利用已有的濾波方法進(jìn)行著陸器狀態(tài)估計(jì)時(shí)精度不高的問題，提 出了一種基于采樣點(diǎn)繼承策略的火星著陸器大氣進(jìn)入段導(dǎo)航方法。
[0006] 本發(fā)明的一種基于采樣點(diǎn)繼承策略的火星著陸器大氣進(jìn)入段導(dǎo)航方法，步驟如 下：
[0007] 步驟一：在濾波起始周期，根據(jù)著陸器狀態(tài)X，以及著陸器狀態(tài)方差Px，利用無跡卡 爾曼濾波的采樣點(diǎn)生成規(guī)則給出采樣點(diǎn)和采樣點(diǎn)權(quán)值；
[0008] 具體過程如下：
[0009] 2n+l個(gè)采樣點(diǎn)為：
[0010]

【權(quán)利要求】
1. 一種基于采樣點(diǎn)繼承策略的火星著陸器大氣進(jìn)入段導(dǎo)航方法，其特征在于它的步驟 如下： 步驟一：在濾波起始周期，根據(jù)著陸器狀態(tài)X，以及著陸器狀態(tài)方差Px，利用無跡卡爾曼 濾波的采樣點(diǎn)生成規(guī)則給出采樣點(diǎn)和采樣點(diǎn)權(quán)值； 具體過程如下： 2n+l個(gè)采樣點(diǎn)為：
這里n為系統(tǒng)的維數(shù)，即著陸器狀態(tài)個(gè)數(shù)；T為可調(diào)的比例參數(shù)，用來控制樣本點(diǎn)到均 值的距離； 相應(yīng)的權(quán)值為：
步驟二：根據(jù)無跡卡爾曼濾波規(guī)則由火星大氣進(jìn)入段動(dòng)力學(xué)方程傳播步驟一中獲得的 采樣點(diǎn)，并根據(jù)傳播后的采樣點(diǎn)和步驟一生成的權(quán)值給出著陸器狀態(tài)預(yù)測(cè)值i和著陸器狀 態(tài)方差Px，具體過程如下： 根據(jù)火星大氣進(jìn)入段動(dòng)力學(xué)方程和由式（1)得到的采樣點(diǎn)，利用龍格庫塔法進(jìn)行數(shù)值 積分，得到一個(gè)濾波周期At時(shí)長(zhǎng)后的采樣點(diǎn)預(yù)測(cè)值Ii，即傳播后的采樣點(diǎn)，記這個(gè)過程為
計(jì)算著陸器狀態(tài)預(yù)測(cè)值i I
更新著陸器狀態(tài)方差Px
步驟三：濾波器的觀測(cè)量為y，根據(jù)無跡卡爾曼濾波規(guī)則，由采樣點(diǎn)權(quán)值和傳播后的采 樣點(diǎn)計(jì)算觀測(cè)量預(yù)測(cè)值:P和方差Py，以及協(xié)方差Pxy，具體過程如下： 根據(jù)傳播后的采樣點(diǎn)Ii計(jì)算采樣點(diǎn)對(duì)應(yīng)觀測(cè)量Yi
這里觀測(cè)方程h(i)由觀測(cè)量定義得到；進(jìn)而得到觀測(cè)量預(yù)測(cè)值i'和觀測(cè)量方差Py

協(xié)方差Pxy為
步驟四：測(cè)量更新，即當(dāng)敏感器測(cè)量到新的數(shù)據(jù)時(shí)，修正著陸器狀態(tài)的估計(jì)值，具體過 程如下： 根據(jù)高斯假設(shè)Kalman濾波器規(guī)則計(jì)算濾波增益
著陸器狀態(tài)估計(jì)無+
著陸器狀態(tài)方差估計(jì)
其中，R為測(cè)量噪聲方差，K為濾波增益，為狀態(tài)和觀測(cè)量的協(xié)方差的轉(zhuǎn)置； 步驟五：采樣點(diǎn)繼承：即根據(jù)步驟四得到著陸器狀態(tài)r以及著陸器狀態(tài)方差G1，修改 步驟二中傳播后的采樣點(diǎn)Ii，得到修正值的采樣點(diǎn)f; 具體過程如下： 首先定義部分變量：
1為傳播后的采樣點(diǎn)Ii組成的矩陣；€為采樣點(diǎn)的修正值，即這里要求取的變量， 為名+組成的矩陣；W為采樣點(diǎn)對(duì)應(yīng)權(quán)值組成的向量，W是權(quán)值向量為對(duì)角線元素的對(duì)角 陣；根據(jù)以上定義有：
假設(shè)測(cè)量更新后采樣點(diǎn)集1+通過1的旋轉(zhuǎn)變換和尺度變換得到，即：
其中，Q為一正交矩陣，滿足QQt =I，其階數(shù)與系統(tǒng)維數(shù)相同，表示對(duì)采樣點(diǎn)的旋轉(zhuǎn)變 換，S為n階對(duì)角矩陣，代表對(duì)采樣點(diǎn)的尺度變換； 將式（21)分別帶入到式（20a)和（20b)當(dāng)中，得到兩個(gè)約束：

這里F和P已經(jīng)在步驟四中計(jì)算得出，因此式（22a)和式（22b)以及QQt=I作為求 解Q和S的約束方程；為了盡量保留非線性動(dòng)力學(xué)造成的非高斯分布信息，變換矩陣Q和S的解還應(yīng)該使trace(L-I) (L-I)T最??；根據(jù)以上約束，Q和S的解通過非線性約束尋優(yōu)數(shù) 值算法獲得； 獲得變換矩陣Q和S后，根據(jù)式（21)計(jì)算采樣點(diǎn)修正值會(huì)+，并將作為下一濾波周 期開始時(shí)的采樣點(diǎn)，即令下一周期中的= € ;完成采樣點(diǎn)繼承后，順序執(zhí)行步驟二至步驟 五，直到著陸過程結(jié)束。
2. 根據(jù)權(quán)利要求1所述的一種基于采樣點(diǎn)繼承策略的火星著陸器大氣進(jìn)入段導(dǎo)航方 法，其特征在于步驟五中Q和S的解通過非線性約束尋優(yōu)數(shù)值算法獲得是通過Matlab軟件 中的fmincon函數(shù)計(jì)算得出的。
3. 根據(jù)權(quán)利要求1或2所述的一種基于采樣點(diǎn)繼承策略的火星著陸器大氣進(jìn)入段導(dǎo)航 方法，其特征在于步驟一至步驟五中所述的無跡卡爾曼濾波能夠替換為除無跡卡爾曼濾波 以外的基于積分點(diǎn)的卡爾曼濾波。
4. 根據(jù)權(quán)利要求1或2所述的一種基于采樣點(diǎn)繼承策略的火星著陸器大氣進(jìn)入段導(dǎo) 航方法，其特征在于步驟一所述的著陸器狀態(tài)X為X=[r0XVYV]T，其中r為著陸 器與火星質(zhì)心之間的距離，9和A分別為著陸器經(jīng)度、緯度，V為著陸器相對(duì)火星大氣的速 度，Y和V分別為著陸器航跡角和航向角。
5. 根據(jù)權(quán)利要求4所述的一種基于采樣點(diǎn)繼承策略的火星著陸器大氣進(jìn)入段導(dǎo)航方 法，其特征在于步驟一所述的n為系統(tǒng)的維數(shù)中的n= 6。
6. 根據(jù)權(quán)利要求5所述的一種基于采樣點(diǎn)繼承策略的火星著陸器大氣進(jìn)入段導(dǎo)航方 法，其特征在于步驟二所述的火星大氣進(jìn)入段動(dòng)力學(xué)方程為：

其中，$是傾側(cè)角，m為著陸器質(zhì)量，D和L表不飛行器的氣動(dòng)阻力和氣動(dòng)升力，wm表 示火星自轉(zhuǎn)角速度，g為火星重力加速度，且滿足：
U為火星的萬有引力常數(shù)；著陸器所受的氣動(dòng)阻力和升力表達(dá)式如下：
其中，Cd和Q分別是著陸器阻力系數(shù)和升力系數(shù)，S是著陸器的氣動(dòng)參考面積，q是動(dòng) 壓，P是火星大氣密度，采用指數(shù)模型：
其中，P^是火星表面大氣的密度值，Rm為火星半徑，hs為參考高度。
7.根據(jù)權(quán)利要求6所述的一種基于采樣點(diǎn)繼承策略的火星著陸器大氣進(jìn)入段導(dǎo)航方 法，其特征在于步驟三所述的濾波器觀測(cè)量y為y=[qQY0入]1，其中Q=kqP°_5v3_15 為熱流密度。
【文檔編號(hào)】G01C21/20GK104266650SQ201410578128
【公開日】2015年1月7日 申請(qǐng)日期:2014年10月25日 優(yōu)先權(quán)日:2014年10月25日
【發(fā)明者】田陽, 崔祜濤, 徐田來 申請(qǐng)人:哈爾濱工業(yè)大學(xué)