專利名稱：一種基于改進(jìn)mmupf濾波的火星探測(cè)器自主天文導(dǎo)航方法
技術(shù)領(lǐng)域：
本發(fā)明涉及航天導(dǎo)航技術(shù)領(lǐng)域，可應(yīng)用火星探測(cè)器或其他繞行星飛行探測(cè)器導(dǎo)航參數(shù)的精確確定，特別涉及一種基于改進(jìn)MMUPF濾波方法(改進(jìn)多模型UPF濾波方法)的火星探測(cè)器自主天文導(dǎo)航方法，適用于轉(zhuǎn)移軌道上的深空探測(cè)器的導(dǎo)航定位。

背景技術(shù)：
行星探測(cè)器在其轉(zhuǎn)移軌道運(yùn)行中，所受的太陽(yáng)及各行星引力的大小會(huì)隨其位置的變化而變化，當(dāng)距離某顆行星較遠(yuǎn)時(shí)，其引力可忽略不計(jì)，但當(dāng)距離較近時(shí)，則行星的引力就會(huì)增大，并對(duì)探測(cè)器的軌道運(yùn)動(dòng)產(chǎn)生較大的影響，因此在行星探測(cè)器的軌道運(yùn)動(dòng)中，其軌道動(dòng)力學(xué)模型不是一成不變的，而是連續(xù)變化的。
由于行星探測(cè)器在其轉(zhuǎn)移軌道運(yùn)行中動(dòng)力學(xué)模型建立不準(zhǔn)確，測(cè)量的量測(cè)量也存在的噪聲，所以要采用濾波的方法，減小模型誤差和量測(cè)噪聲。
以往的濾波方法，如IMM和MMPF方法，由于行星探測(cè)器在其轉(zhuǎn)移軌道運(yùn)行中，軌道動(dòng)力學(xué)模型連續(xù)變化，為了獲得好的濾波效果，預(yù)先設(shè)置大量的子模型，且在濾波過(guò)程中保持不變，從而估計(jì)出導(dǎo)航參數(shù)，但大量的子模型濾波增加了計(jì)算量，直接影響了導(dǎo)航的實(shí)時(shí)性。
綜上，由于行星探測(cè)器在轉(zhuǎn)移軌道運(yùn)行中的軌道動(dòng)力學(xué)模型是連續(xù)變化，而目前所采用的濾波方法，要設(shè)置大量子模型，對(duì)應(yīng)動(dòng)力學(xué)模型連續(xù)變化的問(wèn)題，但是由于是子模型之間的切換，并不是實(shí)時(shí)連續(xù)變化，也因此導(dǎo)航精度較低；由于設(shè)置了大量的子模型，導(dǎo)致計(jì)算量大，影響了導(dǎo)航的實(shí)時(shí)性。所以目前在轉(zhuǎn)移軌道運(yùn)行的深空探測(cè)器采用目前的濾波方法導(dǎo)航精度不高、實(shí)時(shí)性差。


發(fā)明內(nèi)容
本發(fā)明要解決的技術(shù)問(wèn)題是克服現(xiàn)有濾波方法的精度低、計(jì)算量大的不足，提供一種精度高、自主性強(qiáng)的轉(zhuǎn)移軌道上火星探測(cè)器自主天文導(dǎo)航方法。
本發(fā)明解決其技術(shù)問(wèn)題所采用的技術(shù)方案為一種基于改進(jìn)MMUPF濾波方法的火星探測(cè)器自主天文導(dǎo)航方法，首先對(duì)處于轉(zhuǎn)移軌道上的深空探測(cè)器進(jìn)行精確建模，其中系統(tǒng)模型采用四體Singer模型，然后以星光角距作為量測(cè)量；最后采用一種新的濾波方法改進(jìn)MMUPF濾波方法進(jìn)行導(dǎo)航參數(shù)的最優(yōu)估計(jì)，解決當(dāng)距離不同時(shí)，行星引力大小和軌道動(dòng)力學(xué)模型變化連續(xù)變化的問(wèn)題，確定火星探測(cè)器或轉(zhuǎn)移軌道上的行星探測(cè)器的導(dǎo)航參數(shù)，顯著提高導(dǎo)航精度； 具體包括以下步驟 1.建立基于軌道動(dòng)力學(xué)的轉(zhuǎn)移軌道上火星探測(cè)器導(dǎo)航系統(tǒng)狀態(tài)方程 式中，μs、μm和μe分別為太陽(yáng)、火星和地球的引力常數(shù)；rse和rsm分別為太陽(yáng)相對(duì)地球和火星的位置矢量，rps，rpe和rpm分別為太陽(yáng)、地球和火星的質(zhì)心到探測(cè)器的位置矢量；a為可用一階Singer模型表示的攝動(dòng)力； 具體的狀態(tài)方程為 式中(x1，y1，z1)，(x2，y2，z2)和(x，y，z)分別為火星、地球和探測(cè)器的位置坐標(biāo)，ax，ay，az為可用一階Singer模型表示的三個(gè)方向的攝動(dòng)力，βx，βy，βz為時(shí)間相關(guān)常數(shù)的倒數(shù)； 可簡(jiǎn)寫為 狀態(tài)變量為X＝[x y，z，vx，vy，vz，ax，ay，az]T，f(X(t)，t)為系統(tǒng)非線性連續(xù)狀態(tài)轉(zhuǎn)移函數(shù)，狀態(tài)噪聲為w＝[wx，wy，wz，wvx，wvy，wvz，wax，way，waz]T。
2.建立以星光角距為量測(cè)量的量測(cè)方程； 如圖2所示，從火星探測(cè)器上觀測(cè)到得導(dǎo)航恒星星光的矢量方向與太陽(yáng)、地球和行星球心的矢量方向之間的夾角，表達(dá)式如下 可簡(jiǎn)寫為 Z(t)＝h(X(t)，t)+v(t)(5) 式中，Z＝[αs，αe，αm]T，測(cè)量噪聲

是太陽(yáng)、地球、火星與導(dǎo)航星之間星光角距的量測(cè)誤差，αs，αe，αm分別為太陽(yáng)、地球、火星與導(dǎo)航星之間的星光角距，s為導(dǎo)航星在日心慣性坐標(biāo)系中的位置矢量，rps，rpe和rpm分別為太陽(yáng)、地球和火星的質(zhì)心到探測(cè)器的位置矢量，h(X(t)，t)為非線性連續(xù)量測(cè)函數(shù)，αs，αe，αm，

s，rps，rpe，rpm均為時(shí)間t的變量。
3.對(duì)以上狀態(tài)方程及量測(cè)方程進(jìn)行離散化； X(k+1)＝F(X(k)，k)+w(k)(6) Z(k)＝H(X(k)，k)+v(k) (7) 式中，k＝1，2，…，F(xiàn)(X(k)，k)為f(X(t)，t)離散后的非線性狀態(tài)轉(zhuǎn)移函數(shù)，H(X(k)，k)為h(X(t)，t)離散后的非線性量測(cè)函數(shù)，w(k)、v(k)互不相關(guān)。
4.采用改進(jìn)MMUPF濾波算法，并輸出導(dǎo)航信息。
用模型參數(shù)替代模型編號(hào)作為增補(bǔ)的狀態(tài)量增補(bǔ)到狀態(tài)變量中，首先對(duì)每個(gè)粒子進(jìn)行模型更新，然后按照粒子濾波方法(UPF)對(duì)該粒子的狀態(tài)(包含增補(bǔ)狀態(tài))進(jìn)行狀態(tài)更新，而后進(jìn)行重采樣，最后輸出經(jīng)MMUPF濾波后的導(dǎo)航參數(shù)。其中步驟4所用的改進(jìn)MMUPF濾波算法，其實(shí)現(xiàn)步驟為 ①T＝0時(shí)，初始化X＝x0，生成N個(gè)服從先驗(yàn)分布p(x0)的粒子x0i，i＝1，2，…N，設(shè)置每個(gè)粒子的初始UPF權(quán)值w0i均設(shè)為1/N，i＝1，2，…N；其中N為大于10的整數(shù)，需滿足導(dǎo)航精度和實(shí)時(shí)性的要求，通常為10~150時(shí)。
②T＝k時(shí)濾波過(guò)程 A.模型更新 根據(jù)先驗(yàn)概率密度p(xs(k)|γ(k)，γ(k+1)，Z(k))采樣，該先驗(yàn)概率密度可近似表示為擴(kuò)維的粒子集{xs(k)，xM(k)，xM(k+1|k)}；其中xM(k+1|k)可由xM(k)經(jīng)過(guò)馬爾科夫移動(dòng)產(chǎn)生根據(jù)模型編號(hào)為j的xM(k+1|k)，計(jì)算滿足條件的模型編號(hào)l，使得模型編號(hào)為j的xM(k+1|k)更新為模型編號(hào)是l的xM(k+1|k)，其中j，l分別表示第j個(gè)模型和第l個(gè)模型；其中sl為(0，1]間的服從均勻分布的隨機(jī)數(shù)，

代表了馬爾科夫鏈的概率分布函數(shù)P(γ(k-1)≤γi|γ(k)＝γj)；根據(jù)上述準(zhǔn)則對(duì)每個(gè)粒子進(jìn)行模型更新，將xM，k-1i更新為xM，ki； B.狀態(tài)更新，用UKF方法進(jìn)行狀態(tài)更新，包括時(shí)間更新和量測(cè)更新； C.計(jì)算權(quán)值，計(jì)算歸一化的UPF權(quán)值wki和有效粒子尺寸 D.重采樣 根據(jù)歸一后的權(quán)重對(duì)粒子集重采樣，使得重采樣后的樣本集

的近似分布為后驗(yàn)概率密度p(xk|zk)。并將權(quán)值wki重新置為1/N。
E.結(jié)果輸出

式中，xk+1為k+1時(shí)刻的狀態(tài)估計(jì)值，包含導(dǎo)航所需的位置、速度、攝動(dòng)加速度，Pk+1為k+1時(shí)刻的估計(jì)方差矩陣，

為第i個(gè)粒子經(jīng)過(guò)模型更新、狀態(tài)更新以及重采樣后的狀態(tài)估計(jì)，Pk+1i為第i個(gè)粒子經(jīng)過(guò)模型更新和狀態(tài)更新后的狀態(tài)估計(jì)方差，wk+1i為第i個(gè)粒子的UPF歸一化權(quán)值。
本發(fā)明的原理是由于行星探測(cè)器在其轉(zhuǎn)移軌道運(yùn)行中，所受的太陽(yáng)及各行星引力的大小會(huì)隨其位置的變化而變化，當(dāng)距離某顆行星較遠(yuǎn)時(shí)，其引力可忽略不計(jì)，但當(dāng)距離較近時(shí)，則行星的引力就會(huì)增大，并對(duì)探測(cè)器的軌道運(yùn)動(dòng)產(chǎn)生較大的影響，因此在行星探測(cè)器的軌道運(yùn)動(dòng)中，其軌道動(dòng)力學(xué)模型不是一成不變的，而是連續(xù)變化的。改進(jìn)MMUPF的基本思想是用一組帶有不同參數(shù)值的濾波器的估計(jì)值通過(guò)加權(quán)得到對(duì)系統(tǒng)狀態(tài)的最優(yōu)估計(jì)，從而達(dá)到對(duì)于未知或不確定性系統(tǒng)參數(shù)的系統(tǒng)進(jìn)行自適應(yīng)的目的，設(shè)置多個(gè)子模型，用模型參數(shù)替代模型編號(hào)，建立利用模型參數(shù)作為狀態(tài)變量的多模型狀態(tài)方程，準(zhǔn)確描述行星探測(cè)器在不同位置時(shí)的運(yùn)動(dòng)狀態(tài)；由于利用模型參數(shù)替代模型編號(hào)，不必對(duì)子模型整體進(jìn)行更新，只更新與狀態(tài)變量有關(guān)的模型參數(shù)，節(jié)省了計(jì)算量，提高了導(dǎo)航精度，并且加入了基于星光角距的量測(cè)信息，如圖2所示，s為導(dǎo)航星在日心慣性坐標(biāo)系中的位置矢量，rps，rpe和rpm分別為太陽(yáng)、地球和火星的質(zhì)心到探測(cè)器的位置矢量，αs，αe，αm分別為利用星敏感器獲得的太陽(yáng)、地球、火星與導(dǎo)航恒星之間的星光角距，利用UPF濾波方法對(duì)利用粒子濾波對(duì)該原狀態(tài)變量和含有模型參數(shù)的增補(bǔ)狀態(tài)進(jìn)行更新，對(duì)狀態(tài)進(jìn)行最優(yōu)估計(jì)，對(duì)系統(tǒng)進(jìn)行自適應(yīng)最優(yōu)估計(jì)。
本發(fā)明與現(xiàn)有技術(shù)相比的優(yōu)點(diǎn)在于本發(fā)明所采用的改進(jìn)MMUPF濾波方法與UPF、IMMUKF和傳統(tǒng)的MMUPF濾波方法相比，不必對(duì)子模型整體進(jìn)行更新，只更新與狀態(tài)變量有關(guān)的模型參數(shù)，在計(jì)算量沒(méi)有增加的基礎(chǔ)上提高導(dǎo)航精度，在相同導(dǎo)航精度的前提下，實(shí)時(shí)性也有提高，實(shí)現(xiàn)轉(zhuǎn)移軌道上火星探測(cè)器的實(shí)時(shí)準(zhǔn)確定位。



圖1為本發(fā)明的流程圖。
圖2為本發(fā)明中的量測(cè)信息——星光角距示意圖。
圖3為本發(fā)明中的濾波方法——改進(jìn)MMUPF原理圖。

具體實(shí)施例方式 如圖1所示，本發(fā)明以火星探測(cè)器為例，具體實(shí)施方法如下 建立基于軌道動(dòng)力學(xué)的轉(zhuǎn)移軌道上火星探測(cè)器導(dǎo)航系統(tǒng)狀態(tài)方程。
根據(jù)天文導(dǎo)航系統(tǒng)的需要，采用四體Singer模型，該模型是在四體模型的基礎(chǔ)上利用一階Singer模型對(duì)其他攝動(dòng)項(xiàng)進(jìn)行建模。該模型(系統(tǒng)狀態(tài)方程)在日心慣性坐標(biāo)系(J2000.0)下，可以表示為 具體可以寫為 式中，μs、μm和μe分別為太陽(yáng)、火星和地球的引力常數(shù)；rse和rsm分別為地球和火星的位置矢量，rps，rpe和rpm分別為太陽(yáng)、地球和火星的質(zhì)心到探測(cè)器的位置矢量；(x1，y1，z1)，(x2，y2，z2)和(x，y，z)分別為火星、地球和探測(cè)器的位置。(vx，vy，vz)是探測(cè)器的速度，ax，ay，az分別為可用一階Singer模型表示的部分?jǐn)z動(dòng)項(xiàng)之和。βx，βy，βz分別為時(shí)間相關(guān)常數(shù)的倒數(shù)，(dx/dt，dy/dt，dz/dt)為探測(cè)器位置的微分，即探測(cè)器的速度；(dvx/dt，dvy/dt，dvz/dt)為探測(cè)器速度的微分，即探測(cè)器的加速度；(dax/dt，day/dt，daz/dt)為攝動(dòng)項(xiàng)的微分，即攝動(dòng)項(xiàng)對(duì)時(shí)間的變化率 a為可用一階Singer模型表示的攝動(dòng)力； 可簡(jiǎn)寫為 狀態(tài)變量為X＝[x，y，z，vx，vy，vz，ax，ay，az]T，f(X(t)，t)為系統(tǒng)非線性連續(xù)狀態(tài)轉(zhuǎn)移函數(shù)，狀態(tài)噪聲為w＝[wx，wy，wz，wvx，wvy，wvz，wax，way，waz]T。
2、建立以星光角距為量測(cè)量的量測(cè)方程。
從探測(cè)器上觀測(cè)到得導(dǎo)航恒星星光的矢量方向與太陽(yáng)、地球和行星球心的矢量方向之間的夾角，表達(dá)式如下 式中，αs，αe，αm分別為太陽(yáng)、地球、火星與導(dǎo)航星之間的星光角距，s為導(dǎo)航星在日心慣性坐標(biāo)系中的位置矢量，rps，rpe和rpm分別為太陽(yáng)、地球和火星的質(zhì)心到探測(cè)器的位置矢量，αs，αe，αm，

-s，rps，rpe，rpm均為時(shí)間t的變量。
令Z＝[αs，αe，αm]T，測(cè)量噪聲其中

是太陽(yáng)、地球、火星與導(dǎo)航星之間星光角距的量測(cè)誤差，則量測(cè)方程可表示為 Z(t)＝h(X(t)，t)+v(t)(12) 式中，h(X(t)，t)為非線性連續(xù)量測(cè)函數(shù)。
3、對(duì)以上狀態(tài)方程及兩個(gè)量測(cè)方程進(jìn)行離散化 X(k+1)＝F(X(k)，k)+w(k)(13) Z(k)＝H(X(k)，k)+v(k)(14) 式中，k＝1，2，…，F(xiàn)(X(k)，k)為f(X(t)，t)離散后的非線性狀態(tài)轉(zhuǎn)移函數(shù)，H(X(k)，k)為h(X(t)，t)離散后的非線性量測(cè)函數(shù)，w(k)、v(k)互不相關(guān)。
4、采用改進(jìn)MMUPF濾波算法，估計(jì)并輸出導(dǎo)航信息。
本發(fā)明所采用的MMUPF方法用模型參數(shù)作為增補(bǔ)的狀態(tài)量，即令X＝[xs，xM]，其中xS為原始狀態(tài)量，而xM＝[γ1，γ2，…，γM]為模型參數(shù)，包括模型的模型誤差和量測(cè)誤差，這樣k時(shí)刻的后驗(yàn)概率密度p(xs(k)|γ(k)，Z(k))就可由一組粒子{xs(k)，xM(k)}表示，其中γ(k)代表模型參數(shù)。第j個(gè)模型的條件概率密度p(Xs(k)|γ(k)＝j(luò)，Z(k))就可近似為一個(gè)子粒子集{xs(k)，xM(k)＝j(luò)}中的粒子數(shù)。并對(duì)該增補(bǔ)狀態(tài)通過(guò)粒子濾波的方法進(jìn)行狀態(tài)的更新，具體算法步驟如圖3所示 (1)T＝0時(shí)，初始化X＝x0 生成N個(gè)服從先驗(yàn)分布p(x0)分布的粒子x0i，t＝1，…，N，其中N為大于10的整數(shù)，需滿足導(dǎo)航精度和實(shí)時(shí)性的要求，通常為10~150時(shí)，粒子數(shù)N越大，導(dǎo)航精度越高，但是實(shí)時(shí)性受到限制，本實(shí)施例中，N＝20滿足導(dǎo)航精度中的位置精度實(shí)現(xiàn)22km、速度精度實(shí)現(xiàn)0.8m/s。所選粒子均值x0i和方差P0i滿足 設(shè)置每個(gè)樣本的初始UPF權(quán)值w0i均設(shè)為1/N，i＝1，2，…N。在初始先驗(yàn)分布p(x0)未知的情況下，通常取為初始狀態(tài)量x0為中心的均勻分布或高斯分布，通常初始分布的選取對(duì)結(jié)果的影響較小。
(2)T＝k時(shí)濾波過(guò)程 ①模型更新 根據(jù)先驗(yàn)概率密度p(xs(k)|γ(k)，γ(k+1)，Z(k))采樣，該先驗(yàn)概率密度可近似表示為擴(kuò)維的粒子集{xs(k)，xM(k)，xM(k+1|k)}；其中xM(k+1|k)可由xM(k)經(jīng)過(guò)馬爾科夫移動(dòng)產(chǎn)生如果xM(k+1|k)的模型編號(hào)j，則當(dāng)l滿足時(shí)，令xM(k+1|k)為模型編號(hào)是l的xM，其中j，l分別表示第j個(gè)模型和第l個(gè)模型；其中sl為(0，1]間的服從均勻分布的隨機(jī)數(shù)，

代表了馬爾科夫鏈的概率分布函數(shù)P(γ(k-1)≤γl|γ(k)＝γj)；根據(jù)上述準(zhǔn)則對(duì)每個(gè)粒子進(jìn)行模型更新，將xM，ki更新為xM，k+1i； ②狀態(tài)更新 用UKF方法對(duì)第i個(gè)粒子進(jìn)行更新，具體算法如下 a)采樣 在第k時(shí)刻的第i個(gè)粒子

附近選取一系列樣本點(diǎn)，這些樣本點(diǎn)的均值和協(xié)方差分別為

和P(k|k)。設(shè)狀態(tài)變量為n×1維，那么第i個(gè)粒子對(duì)應(yīng)的2n+1個(gè)樣本點(diǎn)χ0，k…χη+n，k及其UKF加權(quán)值W0…Wη+n分別如下 W0＝τ/(n+τ) Wη＝1/[2(n+τ)](16) Wη+n＝1/[2(n+τ)] 式中，η＝1，2，...，τ∈R；當(dāng)P(k|k)＝ATA時(shí)，

取A的第η行，當(dāng)P(k|k)＝AAT時(shí)，

取A的第η列。
b)時(shí)間更新 第i個(gè)粒子的第Γ個(gè)采樣點(diǎn)的狀態(tài)量一步預(yù)測(cè)χk+1|k為 由式(17)得第i個(gè)粒子的所有采樣點(diǎn)狀態(tài)量的一步預(yù)測(cè)加權(quán)后結(jié)果

為 式中，WΓi為第i個(gè)粒子的第Γ個(gè)采樣點(diǎn)的權(quán)值； 由式(17)和式(18)得狀態(tài)量的估計(jì)方差一步預(yù)測(cè)Pk+1-為 式中，Qk+1為k+1時(shí)刻狀態(tài)模型噪聲協(xié)方差陣； 由式(17)可以得出第i個(gè)粒子的第Γ個(gè)采樣點(diǎn)對(duì)應(yīng)得量測(cè)估計(jì)值ZΓ，k+1|ki 由式(20)可以得出第i個(gè)粒子的所有采樣點(diǎn)量測(cè)估計(jì)加權(quán)值
c)量測(cè)更新 由式(20)、式(21)可得第i個(gè)粒子的量測(cè)方差陣

為 式中，Rk+1分別為量測(cè)噪聲協(xié)方差； 由式(17)、式(18)、式(20)、式(21)可得第i個(gè)粒子的狀態(tài)變量量測(cè)量方差陣
由式(22)、式(23)第i個(gè)粒子的濾波增益Kk+1為 第i個(gè)粒子狀態(tài)量的估計(jì)值

和估計(jì)方差Pk+1為 式中，Qk+1和Rk+1分別為系統(tǒng)和量測(cè)噪聲協(xié)方差。當(dāng)x(k)假定為高斯分布時(shí)，通常選取n+τ＝3。
③計(jì)算權(quán)值 并計(jì)算歸一化的UPF權(quán)值wki
計(jì)算有效粒子尺寸Neff 如果Neff小于既定門限，一般門限值取為2N/3，則執(zhí)行步驟D，否則直接執(zhí)行步驟E。
④重采樣 根據(jù)歸一后的權(quán)重對(duì)粒子集重采樣，使得重采樣后的樣本集

的近似分布為后驗(yàn)概率密度p(xk|zk)。并將權(quán)值wki重新置為1/N。
⑤結(jié)果輸出

式中，xk+1為k+1時(shí)刻的狀態(tài)估計(jì)值，包含導(dǎo)航所需的位置、速度、攝動(dòng)加速度，Pk+1為k+1時(shí)刻的估計(jì)方差矩陣，

為第i個(gè)粒子經(jīng)過(guò)模型更新、狀態(tài)更新以及重采樣后的狀態(tài)估計(jì)，Pk+1i為第i個(gè)粒子經(jīng)過(guò)模型更新和狀態(tài)更新后的狀態(tài)估計(jì)方差，wk+1i為第i個(gè)粒子的UPF歸一化權(quán)值。
本發(fā)明說(shuō)明書中未作詳細(xì)描述的內(nèi)容屬于本領(lǐng)域?qū)I(yè)技術(shù)人員公知的現(xiàn)有技術(shù)。
權(quán)利要求
1、一種基于改進(jìn)MMUPF濾波方法的火星探測(cè)器自主天文導(dǎo)航方法，其特征在于對(duì)處于轉(zhuǎn)移軌道上的深空探測(cè)器進(jìn)行建模，其中系統(tǒng)模型采用四體Singer模型，然后以星光角距作為量測(cè)量；采用改進(jìn)MMUPF濾波方法進(jìn)行導(dǎo)航參數(shù)的最優(yōu)估計(jì)，輸出導(dǎo)航信息，具體包括以下步驟
(1)建立基于軌道動(dòng)力學(xué)的轉(zhuǎn)移軌道上火星探測(cè)器導(dǎo)航系統(tǒng)狀態(tài)方程；火星探測(cè)器在轉(zhuǎn)移軌道上的狀態(tài)方程采用四體Singer模型，該模型在J2000.0日心慣性坐標(biāo)系下表示為
式中，μs、μm和μe分別為太陽(yáng)、火星和地球的引力常數(shù)；rse和rsm分別為地球和火星的位置矢量，rps，rpe和rpm分別為太陽(yáng)、地球和火星的質(zhì)心到探測(cè)器的位置矢量；a為可用一階Singer模型表示的攝動(dòng)力；
狀態(tài)方程為
式中(x1，y1，z1)，(x2，y2，z2)和(x，y，z)分別為火星、地球和探測(cè)器的位置坐標(biāo)，ax，ay，az為可用一階Singer模型表示的三個(gè)方向的攝動(dòng)力，βx，βy，βz為時(shí)間相關(guān)常數(shù)的倒數(shù)；
可簡(jiǎn)寫為
令狀態(tài)變量X(t)＝[x，y，z，vx，vy，vz，ax，ay，az]T，其中x，y，z為探測(cè)器三軸的位置坐標(biāo)，vx，vy，vz為探測(cè)器三軸的速度坐標(biāo)，ax，ay，az為探測(cè)器三軸所受攝動(dòng)加速度，f(X(t)，t)為系統(tǒng)非線性連續(xù)狀態(tài)轉(zhuǎn)移函數(shù)，狀態(tài)噪聲為w(t)＝[wx，wy，wz，wvx，wvy，wvz，wax，way，waz]T。
(2)建立以星光角距為量測(cè)量的量測(cè)方程；
以星光角距為量測(cè)量建立系統(tǒng)的量測(cè)方程為從火星探測(cè)器上觀測(cè)到得導(dǎo)航恒星星光的矢量方向與太陽(yáng)、地球和行星球心的矢量方向之間的夾角，表達(dá)式如下
可簡(jiǎn)寫為
Z(t)＝h(X(t)，t)+v(t)
式中，Z(t)＝[αs，αe，αm]T，測(cè)量噪聲
是太陽(yáng)、地球、火星與導(dǎo)航星之間星光角距的量測(cè)誤差，αs，αe，αm分別為太陽(yáng)、地球、火星與導(dǎo)航星之間的星光角距，s為導(dǎo)航星在日心慣性坐標(biāo)系中的位置矢量，由星敏感器識(shí)別，h(X(t)，t)為非線性連續(xù)量測(cè)函數(shù)，αs，αe，αm，
s，rps，rpe，rpm均為時(shí)間t的變量。
(3)對(duì)以上狀態(tài)方程及量測(cè)方程進(jìn)行離散化；
X(k+1)＝F(X(k)，k)+w(k)
Z(k)＝H(X(k)，k)+v(k)
式中，k＝1，2，…，F(xiàn)(X(k)，k)為f(X(t)，t)離散后的非線性狀態(tài)轉(zhuǎn)移函數(shù)，H(X(k)，k)為h(X(t)，t)離散后的非線性量測(cè)函數(shù)，w(k)、v(k)互不相關(guān)。
(4)采用改進(jìn)MMUPF濾波算法，估計(jì)并輸出導(dǎo)航信息，用模型參數(shù)替代模型編號(hào)作為增補(bǔ)的狀態(tài)量增補(bǔ)到狀態(tài)變量中，其中模型參數(shù)包括模型的模型誤差和量測(cè)誤差，k時(shí)刻的后驗(yàn)概率密度可由一組粒子表示，對(duì)每個(gè)粒子進(jìn)行模型更新，然后按照標(biāo)準(zhǔn)粒子濾波方法對(duì)該粒子的狀態(tài)進(jìn)行狀態(tài)更新，而后進(jìn)行重采樣，最后輸出經(jīng)MMUPF濾波后的導(dǎo)航參數(shù)。
2、根據(jù)權(quán)利要求1所述的一種基于改進(jìn)MMUPF濾波方法的火星探測(cè)器自主天文導(dǎo)航方法，其特征在于所述步驟(4)所用的改進(jìn)MMUPF濾波算法，其實(shí)現(xiàn)步驟為
(1)T＝0時(shí)，初始化X＝x0，生成N個(gè)服從先驗(yàn)分布p(x0)的粒子x0i，i＝1，2，…N，設(shè)置每個(gè)粒子的初始UPF權(quán)值w0i均設(shè)為1/N，i＝1，2，…N；其中N為大于10的整數(shù)，需滿足導(dǎo)航精度和實(shí)時(shí)性的要求，通常為10~150時(shí)。
(2)T＝k時(shí)濾波過(guò)程
A.模型更新
根據(jù)先驗(yàn)概率密度p(xs(k)|γ(k)，γ(k+1)，Z(k))采樣，該先驗(yàn)概率密度可近似表示為擴(kuò)維的粒子集{xs(k)，xM(k)，xM(k+1|k)}；其中xM(k+1|k)可由xM(k)經(jīng)過(guò)馬爾科夫移動(dòng)產(chǎn)生根據(jù)模型編號(hào)為j的xM(k+1|k)，計(jì)算滿足條件
的模型編號(hào)l，使得模型編號(hào)為j的xM(k+1|k)更新為模型編號(hào)是l的xM(k+1|k)，其中j，l分別表示第j個(gè)模型和第l個(gè)模型；其中sl為(0，1]間的服從均勻分布的隨機(jī)數(shù)，
代表了馬爾科夫鏈的概率分布函數(shù)P(γ(k-1)≤γl|γ(k)＝γj)；根據(jù)上述準(zhǔn)則對(duì)每個(gè)粒子進(jìn)行模型更新，將xM，k-1i更新為xM，ki；
B.狀態(tài)更新，用UKF方法進(jìn)行狀態(tài)更新，包括時(shí)間更新和量測(cè)更新；
C.計(jì)算權(quán)值，計(jì)算歸一化的UPF權(quán)值wki和有效粒子尺寸
D.重采樣
根據(jù)歸一后的權(quán)重對(duì)粒子集重采樣，使得重采樣后的樣本集
的近似分布為后驗(yàn)概率密度p(xk|zk)。并將權(quán)值wki重新置為1/N。
E.結(jié)果輸出
式中，xk+1為k+1時(shí)刻的狀態(tài)估計(jì)值，包含導(dǎo)航所需的位置、速度、攝動(dòng)加速度，Pk+1為k+1時(shí)刻的估計(jì)方差矩陣，
為第i個(gè)粒子經(jīng)過(guò)模型更新、狀態(tài)更新以及重采樣后的狀態(tài)估計(jì)，Pk+1i為第i個(gè)粒子經(jīng)過(guò)模型更新和狀態(tài)更新后的狀態(tài)估計(jì)方差，wk+1i為第i個(gè)粒子的UPF歸一化權(quán)值。
全文摘要
一種基于改進(jìn)MMUPF濾波方法的火星探測(cè)器自主天文導(dǎo)航方法，涉及一種火星探測(cè)器的自主導(dǎo)航方法。該方法先對(duì)處于轉(zhuǎn)移軌道上的深空探測(cè)器進(jìn)行精確建模，然后以星光角距作為量測(cè)量，最后采用改進(jìn)MMUPF濾波方法進(jìn)行導(dǎo)航參數(shù)的最優(yōu)估計(jì)，解決當(dāng)距離不同時(shí)，行星引力大小和軌道動(dòng)力學(xué)模型變化連續(xù)變化的問(wèn)題，顯著提高導(dǎo)航精度。可用于火星探測(cè)器或轉(zhuǎn)移軌道上的行星探測(cè)器的導(dǎo)航參數(shù)的確定。
文檔編號(hào)G01C21/20GK101672651SQ200910093148
公開(kāi)日2010年3月17日 申請(qǐng)日期2009年9月25日 優(yōu)先權(quán)日2009年9月25日
發(fā)明者寧曉琳, 吳偉仁, 辛 馬 申請(qǐng)人:北京航空航天大學(xué)