ASUKF方法在航天器自主導(dǎo)航中的應(yīng)用

李璟璟，張迎春，2，李化義，陳雪芹

(1.哈爾濱工業(yè)大學(xué) 衛(wèi)星技術(shù)研究所，黑龍江 哈爾濱 150001;2.深圳航天東方紅海特衛(wèi)星有限公司，廣東 深圳 518057)

在航天器自主導(dǎo)航技術(shù)中，系統(tǒng)的測(cè)量數(shù)據(jù)通常都會(huì)帶有隨機(jī)噪聲誤差，由于實(shí)際衛(wèi)星導(dǎo)航系統(tǒng)大部分都是非線性系統(tǒng)，因此需要用非線性濾波方法得到系統(tǒng)狀態(tài)變量的最優(yōu)估計(jì).EKF方法［1］雖然在工程中有廣泛的應(yīng)用，但是其對(duì)非線性方程的可微要求以及一階近似的處理方法，限制了應(yīng)用的范圍和精度的提高.Julier等［2-3］提出的UKF方法，通過廣義狀態(tài)點(diǎn)對(duì)系統(tǒng)狀態(tài)的概率分布進(jìn)行近似，避免了雅可比矩陣的求取，提高了濾波精度.

然而，UKF對(duì)于系統(tǒng)先驗(yàn)噪聲的分布有更為苛刻的要求.其良好的估計(jì)性能建立在精確已知系統(tǒng)先驗(yàn)噪聲分布的基礎(chǔ)之上［4］.但是，在軌航天器面臨的實(shí)際空間環(huán)境是錯(cuò)綜復(fù)雜的，在航天器運(yùn)行的過程中，噪聲統(tǒng)計(jì)特性往往會(huì)發(fā)生變化.另外，由于受到空間各種不確定因素的影響，測(cè)量系統(tǒng)往往也會(huì)受到不同程度干擾［5］.當(dāng)存在如上所述的不確定噪聲及干擾的作用時(shí)，UKF算法的估計(jì)性能會(huì)嚴(yán)重下降，甚至出現(xiàn)發(fā)散.因此，如何提高濾波器的自適應(yīng)能力成為航天器自主導(dǎo)航需要解決的問題之一.

針對(duì)這個(gè)問題，SONG［6］等通過對(duì)噪聲方差進(jìn)行估計(jì)得到了自適應(yīng)UKF算法并將其應(yīng)用到移動(dòng)機(jī)器人的狀態(tài)及參數(shù)估計(jì)中.齊俊桐［7］等提出了基于MIT規(guī)則的自適應(yīng)UKF方法并將其應(yīng)用在旋翼飛行機(jī)器人的容錯(cuò)控制中.然而，這些方法都需要進(jìn)行大量的偏微分計(jì)算，當(dāng)系統(tǒng)維數(shù)較大時(shí)，必然會(huì)影響計(jì)算速度，增加計(jì)算機(jī)負(fù)荷.Kim［8］等通過自適應(yīng)因子對(duì)EKF迭代過程中的方差陣和增益陣進(jìn)行調(diào)整，得到自適應(yīng)EKF算法并將其用在INS-GPS系統(tǒng)的狀態(tài)估計(jì)中.該方法提高了EKF的自適應(yīng)能力，但是仍然保持在一階估計(jì)精度.裴?？。?］等提出了基于神經(jīng)網(wǎng)絡(luò)輔助的自適應(yīng)SSUKF方法并應(yīng)用到車載組合導(dǎo)航系統(tǒng)的信息融合中.該方法與UKF相比具有較好的自適應(yīng)能力和較短的計(jì)算時(shí)間，但由于神經(jīng)網(wǎng)絡(luò)需要一個(gè)預(yù)先訓(xùn)練的過程，這極大地影響了該方法在航天領(lǐng)域中的應(yīng)用.

本文針對(duì)受到不確定因素影響的星敏/地平儀自主導(dǎo)航系統(tǒng)，在現(xiàn)有自適應(yīng)估計(jì)方法的基礎(chǔ)上，做了進(jìn)一步的改進(jìn).通過對(duì)狀態(tài)方程進(jìn)行Unscented變換，對(duì)觀測(cè)方程圍繞狀態(tài)估計(jì)點(diǎn)進(jìn)行線性化，并引入自適應(yīng)因子，在航天器自主導(dǎo)航系統(tǒng)中的仿真表明，ASUKF方法對(duì)不確定的噪聲和干擾影響具有一定的自適應(yīng)能力，并較AEUKF節(jié)約了計(jì)算時(shí)間.

1 ASUKF濾波方法

ASUKF濾波方法通過超球面分布變換以及自適應(yīng)過程的實(shí)現(xiàn)在自適應(yīng)調(diào)節(jié)的同時(shí)降低計(jì)算量.

1.1 Unscented變換

UT變換是UKF算法的核心，其基本原理是用采樣點(diǎn)的分布來近似隨機(jī)變量的概率分布，由被估計(jì)量的“先驗(yàn)”均值和方差，產(chǎn)生一批離散的與被估計(jì)量具有相同的概率統(tǒng)計(jì)特性的采樣點(diǎn)，即Sigma點(diǎn).它不必對(duì)狀態(tài)方程和觀測(cè)方程線性化，而是在狀態(tài)矢量xk附近按一定規(guī)則選取有限的采樣點(diǎn)，其均值和協(xié)方差分別為和，再根據(jù)生成的Sigma點(diǎn)，計(jì)算“后驗(yàn)”的均值和方差，將狀態(tài)矢量的統(tǒng)計(jì)特性通過非線性系統(tǒng)傳播.UT變換是UKF實(shí)現(xiàn)的基礎(chǔ)，是其區(qū)別于其他非線性估計(jì)方法的本質(zhì)特點(diǎn).

對(duì)于狀態(tài)變量xk，均值，方差，UT變換過程［10］為

將選取的狀態(tài)矢量通過非線性函數(shù)f(·)，即χi，k/k－1=f(χi，k－1)，然后加權(quán)近似求解系統(tǒng)輸出的統(tǒng)計(jì)特性:

式中:

1.2 超球面分布變換(SSUT)

UKF方法通過UT變換得到了關(guān)于其均值對(duì)稱分布的2n+1個(gè)采樣點(diǎn).在狀態(tài)向量維數(shù)較高時(shí)，其計(jì)算效率較低.采用SSUT算法，可以在保證濾波精度的同時(shí)，將采樣點(diǎn)個(gè)數(shù)由2n+1個(gè)縮減為n+2個(gè)，在一定程度上節(jié)約了計(jì)算時(shí)間，提高了計(jì)算效率［11-12］，降低了星載計(jì)算機(jī)的負(fù)荷［13］.

SSUT算法通過n+1個(gè)分布在以隨機(jī)狀態(tài)均值為原點(diǎn)的超球面上的相等權(quán)值的采樣點(diǎn)來近似狀態(tài)的概率分布.這樣，由超球面分布采樣點(diǎn)和狀態(tài)均值點(diǎn)構(gòu)成n+2個(gè)Unscented變換采樣點(diǎn).均值為0，均方差陣為單位陣的n維隨機(jī)變量的超球面分布采樣點(diǎn)算法如下［12］:

1)選擇權(quán)值W0，滿足0≤W0≤1.

2)確定權(quán)值Wi:

3)初始化向量序列:

4)擴(kuò)展向量序列.對(duì)于j=2，3，…，n，遞推計(jì)算:

1.3 基于自適應(yīng)因子的自適應(yīng)方法

在EKF濾波過程中，系統(tǒng)新息為

引入對(duì)角矩陣βk=diag(β1，k，…，βn，k)輸出預(yù)測(cè)誤差相關(guān)函數(shù)形式［8］為

為使估計(jì)器能夠準(zhǔn)確的進(jìn)行狀態(tài)估計(jì)，需使輸出預(yù)測(cè)誤差序列{，…，}達(dá)到不相關(guān)零均值高斯序列.則根據(jù)方程，只需

為使上式成立，可引入自適應(yīng)因子αk［8］為

其中，新息方差陣:

估計(jì)的新息方差陣:

同時(shí)引入 λk≥1，對(duì)進(jìn)行自適應(yīng)校正，有=λkPk/k－1，其中，Pk/k－1為系統(tǒng)不完全方差陣.通過λk和αk修正可得新的先驗(yàn)狀態(tài)估計(jì)誤差方差矩陣為

在更新迭代過程中

M為所選取的累加窗口.通過對(duì)M的選擇，系統(tǒng)能夠有效利用測(cè)量量的歷史信息，從而對(duì)當(dāng)前的迭代矩陣進(jìn)行調(diào)整，實(shí)現(xiàn)對(duì)不確定噪聲和干擾的自適應(yīng).當(dāng)系統(tǒng)先驗(yàn)信息不精確時(shí)，λk和αk的取值一般可遵從如下規(guī)則:

2)調(diào)節(jié)增益陣Kk:取λk=1，此時(shí)自適應(yīng)因子主要調(diào)節(jié)的是系統(tǒng)的增益矩陣，主要適用于觀測(cè)方程存在不確定先驗(yàn)統(tǒng)計(jì)信息的情況.

1.4 ASUKF狀態(tài)估計(jì)方法

當(dāng)存在不確定性因素影響時(shí)，非線性系統(tǒng)可表示為

式中:Δf(xk)為不確定過程噪聲方差陣、未知輸入干擾或系統(tǒng)模型不確定性引起的系統(tǒng)狀態(tài)方程不確定項(xiàng)，Δg(xk)為不確定測(cè)量噪聲方差陣和未知輸入干擾引起的系統(tǒng)測(cè)量方程不確定項(xiàng)，Δf(xk)、Δg(xk)與Qk、Rk以及xk相互獨(dú)立.EKF方法將f(xk)、g(xk)分別圍繞濾波值和估計(jì)值進(jìn)行泰勒展開，并略去二階以上項(xiàng)，在卡爾曼濾波框架下進(jìn)行狀態(tài)估計(jì).其缺點(diǎn)是需要計(jì)算狀態(tài)方程和觀測(cè)方程的雅可比矩陣，計(jì)算量大，精度不夠高.UKF方法通過Unscented變換，利用確定性采樣形式，在避免了計(jì)算雅可比矩陣的同時(shí)，提高了估計(jì)精度.但是UKF的顯著缺點(diǎn)是對(duì)系統(tǒng)先驗(yàn)信息的要求非常嚴(yán)格，當(dāng)先驗(yàn)信息與實(shí)際不符時(shí)，容易導(dǎo)致UKF的發(fā)散.為了綜合2種估計(jì)方法的優(yōu)點(diǎn)，首先將狀態(tài)方程進(jìn)行Unscented變換處理，對(duì)觀測(cè)方程求取雅可比矩陣，同時(shí)迭代估計(jì)過程中引入自適應(yīng)調(diào)節(jié)因子，得到AEUKF方法，為了進(jìn)一步降低計(jì)算時(shí)間，將狀態(tài)方程用SSUT變換取代Unscented變換，最終得到ASUKF方法.該方法在降低算法復(fù)雜度的同時(shí)，減緩了由于先驗(yàn)信息不準(zhǔn)確造成的估計(jì)發(fā)散問題.由于AEUKF與ASUKF的區(qū)別僅在于采樣方法的不同，因而只給出ASUKF算法如下:

1)對(duì)于狀態(tài)變量xk，均值，方差，進(jìn)行SSUT變換:

2)預(yù)測(cè)過程:

3)更新過程:

2 組合導(dǎo)航系統(tǒng)模型

在J2000.0地心赤道坐標(biāo)系下，衛(wèi)星自主導(dǎo)航系統(tǒng)建立為如下模型［14-16］:

式中:x=［x y z vxvyvz］T，分別為衛(wèi)星在3個(gè)坐標(biāo)軸上的位置和速度，Re為地球半徑，μ為地心引力常數(shù)，J2為地球引力二階帶諧項(xiàng)系數(shù)，ΔFx、ΔFy、ΔFz為地球非球形攝動(dòng)的高階攝動(dòng)項(xiàng)和日、月攝動(dòng)以及太陽光壓攝動(dòng)和大氣攝動(dòng)等其他攝動(dòng)力影響.

采用直接敏感地平技術(shù)中的星光角距α作為觀測(cè)量，可建立觀測(cè)方程為

式中:r是航天器在地心慣性坐標(biāo)系中的位置矢量，由地平敏感器獲得，s是導(dǎo)航星星光方向的單位矢量，由星敏感器識(shí)別，ε為測(cè)量方程的殘差.

當(dāng)航天器在軌運(yùn)行時(shí)，由于空間環(huán)境的復(fù)雜性，使實(shí)際的噪聲統(tǒng)計(jì)特性往往產(chǎn)生變化，同時(shí)，各種空間干擾也會(huì)對(duì)測(cè)量方程的輸出產(chǎn)生一定的影響.考慮測(cè)量方程在測(cè)量過程中噪聲發(fā)生變化，同時(shí)受到不確定干擾的情況.對(duì)于不確定噪聲，假設(shè)在某時(shí)間段噪聲統(tǒng)計(jì)特性跳變?yōu)棣k，Δvk～N(0，ΔRk)，對(duì)于不確定常值干擾Δb，假設(shè)可用如下方程描述:

式中:B為干擾幅值.

3 ASUKF方法在天文導(dǎo)航系統(tǒng)中的仿真驗(yàn)證

對(duì)于方程(19)、(20)組成的天文導(dǎo)航系統(tǒng)，文獻(xiàn)［15］利用UKF方法對(duì)系統(tǒng)進(jìn)行導(dǎo)航參數(shù)估計(jì)，并通過仿真證明了UKF方法用于自主導(dǎo)航系統(tǒng)的有效性.在實(shí)際系統(tǒng)中，系統(tǒng)建模誤差、先驗(yàn)噪聲信息估計(jì)不足以及空間環(huán)境干擾都可引起方程(18)中Δ項(xiàng)的變化.單純采用UKF方法很容易使估計(jì)過程出現(xiàn)較大的偏差.而采用ASUKF方法，可以使濾波器對(duì)系統(tǒng)的不確定性具有一定的自適應(yīng)能力.分別在理想情況下和存在不確定性影響的情況下進(jìn)行仿真.系統(tǒng)在理想情況下的仿真，是為了驗(yàn)證ASUKF方法的精度;在不確定環(huán)境影響下的仿真，可以驗(yàn)證自適應(yīng)方法的有效性.

選取仿真場(chǎng)景［16］為:半長軸a=7 136.635，偏心率e=0.001 809軌道傾角i=65°，升交點(diǎn)赤經(jīng)Ω=30°，近地點(diǎn)角距ω=30°，星敏感器的視場(chǎng)為20°×20°，星敏感器精度3″，紅外地平儀精度為0.02°，導(dǎo)航星選用均勻分布在天球上的40顆星.假設(shè)系統(tǒng)的測(cè)量噪聲方差陣為:Rk=4×10－6，仿真中取M=20.

分別采用UKF、AEUKF和ASUKF濾波算法進(jìn)行自主導(dǎo)航解算，圖1和圖2分別給出了3個(gè)軸的位置和速度的估計(jì)誤差仿真結(jié)果.由圖可見，3種濾波方法的估計(jì)誤差都能夠較好地趨于收斂.

圖1 位置估計(jì)誤差Fig.1 Estimate error of position

圖2 速度估計(jì)誤差Fig.2 Estimate error of velocity

為了進(jìn)一步比較估計(jì)效果，分別對(duì)3種方法進(jìn)行100次Monte Carlo仿真，將濾波穩(wěn)定后的數(shù)據(jù)求取均方根誤差均值，同時(shí)求取濾波過程所用的時(shí)間平均值.結(jié)果如表1所示，可見3種方法精度的數(shù)量級(jí)相同.數(shù)值上UKF精度略高，這是因?yàn)锳EUKF和ASUKF方法觀測(cè)方程采用了雅可比矩陣而不是采用UT變換.同樣由于觀測(cè)方程采用了雅可比矩陣并且ASUKF采用了SSUT變換，ASUKF的計(jì)算時(shí)間最少.這表明了ASUKF方法的有效性.自適應(yīng)因子變化過程如圖3所示.通過圖3可以看到，在噪聲統(tǒng)計(jì)特性準(zhǔn)確及沒有干擾的情況下，AEUKF和ASUKF方法的自適應(yīng)因子幅值變化比較平緩.

表1 均方根誤差Table 1 RMS error

圖3 自適應(yīng)因子Fig.3 Adaptive factor

在實(shí)際的空間環(huán)境中，航天器面臨的噪聲特性和干擾幅值往往是不確定的范圍，為驗(yàn)證ASUKF方法的自適應(yīng)能力，假設(shè)在系統(tǒng)估計(jì)過程的第1 500 s～2 000 s，系統(tǒng)觀測(cè)噪聲擴(kuò)大為原來的100倍，同時(shí)常值干擾幅值為0.05，干擾作用時(shí)間為3 s.分別采用3種方法對(duì)航天器的位置和速度進(jìn)行估計(jì)，估計(jì)誤差分別如圖4和圖5所示.由圖可見，在受干擾的時(shí)間段內(nèi)，UKF濾波方法的估計(jì)誤差變化較為劇烈，而AEUKF和ASUKF由于自適應(yīng)因子的調(diào)節(jié)作用，基本不受未知噪聲和擾動(dòng)的影響.

表2 均方根誤差Table 2 RMS Error

圖4 位置估計(jì)誤差Fig.4 Estimate error of position

圖5 速度估計(jì)誤差Fig.5 Estimate error of velocity

圖6 自適應(yīng)因子Fig.6 Adaptive factor

表2給出了存在如上所述的噪聲和干擾時(shí)，分別進(jìn)行100次Monte Carlo仿真所得到的均方根誤差均值以及濾波過程所用的平均時(shí)間.

通過以上仿真可以看出，當(dāng)系統(tǒng)噪聲統(tǒng)計(jì)特性發(fā)生變化，同時(shí)存在常值干擾時(shí)，用UKF方法進(jìn)行的位置和速度估計(jì)，顯然受到較大的影響，在受到干擾的時(shí)間段內(nèi)，估計(jì)精度較差.而AEUKF和ASUKF方法基本不受未知噪聲和擾動(dòng)的影響，能夠快速達(dá)到穩(wěn)定，顯示出良好的自適應(yīng)能力.自適應(yīng)因子變化情況如圖6所示，由圖可見，隨著噪聲和干擾的變化，兩者自適應(yīng)因子也較快地做了相應(yīng)的調(diào)整，以使估計(jì)過程適應(yīng)誤差陣及觀測(cè)量的變化.從濾波時(shí)間上看，不論是否存在未知噪聲及干擾，ASUKF方法的計(jì)算時(shí)間最短，這對(duì)于提高星載計(jì)算機(jī)計(jì)算效率具有一定的意義.

4 結(jié)束語

本文以自主導(dǎo)航系統(tǒng)為背景，提出了一種具有一定自適應(yīng)能力的狀態(tài)估計(jì)算法-ASUKF算法.該算法結(jié)合EKF和UKF方法的優(yōu)點(diǎn)，對(duì)系統(tǒng)狀態(tài)方程進(jìn)行SSUT變換，對(duì)系統(tǒng)測(cè)量方程求取雅可比矩陣.考慮到系統(tǒng)測(cè)量過程中噪聲和干擾的不確定性，在迭代過程中引入了自適應(yīng)因子，利用測(cè)量量的歷史數(shù)據(jù)對(duì)方差陣或增益陣進(jìn)行實(shí)時(shí)調(diào)整，使濾波器具有了自適應(yīng)能力.與傳統(tǒng)的UKF方法以及僅具有自適應(yīng)調(diào)節(jié)因子的AEUKF方法相比，ASUKF方法明顯提高了計(jì)算效率，對(duì)不確定的噪聲和干擾具有較強(qiáng)的自適應(yīng)能力.這對(duì)航天器高精度自主導(dǎo)航的工程實(shí)現(xiàn)具有一定的參考價(jià)值.

［1］LERRO D，BAR-SHAOM Y K.Tracking with debiased consistent converted measurements vs.EKF［J］.IEEE Transaction on Aerospace and Electron System，1993，29: 1015-1022.

［2］JULIER S J，UHLMANN J K.Unscented filtering and nonlinear estimation［J］.Proceedings of IEEE，2004，92(3): 401-422.

［3］潘泉，楊峰，葉亮.一類非線性濾波器——UKF綜述［J］.控制與決策，2005，20(5):481-494.

PAN Quan，YANG Feng，YE Liang.Survey of a kind of nonlinear filters—UKF［J］.Control and Decision，2005，20(5):481-494.

［4］ZHAO Lin，WANG Xiaoxu.An adaptive UKF with noise Statistic Estimator［C］//4th IEEE Conference on Industrial Electronics and Applications，ICIEA 2009.Xi＇an，China，2009:614-618.

［5］高為廣，何海波，陳金平.自適應(yīng)UKF算法及其在GPS/ INS組合導(dǎo)航中的應(yīng)用［J］.北京理工大學(xué)學(xué)報(bào)，2008，28(6):505-509.

GAO Weiguang，HE Haibo，CHEN Jinping.An adaptive UKF algorithm and its application for GPS/INS integrated navigation system［J］.Transactions of Beijing Institute of Technology，2008，28(6):505-509.

［6］SONG Qi，HAN Jianda.An adaptive UKF algorithm for the state and parameter estimations of a mobile robot［J］.Acta Automatica Sinica，2008，34(1):72-79.

［7］齊俊桐，韓建達(dá).基于MIT規(guī)則的自適應(yīng)Unscented卡爾曼濾波及其在旋翼飛行機(jī)器人容錯(cuò)控制的應(yīng)用［J］.機(jī)械工程學(xué)報(bào)，2009，45(4):115-124.

QI Juntong，HAN Jianda.Adaptive UKF and its application in fault tolerant control of rotorcraft unmanned aerial vehicle［J］.Journal of Mechanical Engineering，2009，45(4): 115-124.

［8］KIM K H.The stability analysis of the adaptive fading extended Kalman filter using the innovation covariance［J］.International Journal of Control，Automation，and Systems，2009，7(1):49-56.

［9］裴?？?，居鶴華，崔平遠(yuǎn).基于自適應(yīng)SSUKF的組合導(dǎo)航信息融合方法［J］.系統(tǒng)工程與電子技術(shù)，2009，31 (5):1217-1221.

PEI Fujun，JU Hehua，CUI Pingyuan.Information fusion method based on adaptive SSUKF for integrated navigation system［J］.Systems Engineering and Electronics，2009，31 (5):1217-1221.

［10］JULIER S J.The scaled unscented transformation［C］// Proceedings of the American Control Conference.Anchorage，AK，2002.

［11］JULIER S J，UHLMANN J K.Reduced sigma point filters for the propagation of means and covariances through nonlinear transformations［C］//Proceedings of the American Control Conference.Anchorage，AK，2002.

［12］JULIER S J.The spherical simplex unscented transformation［C］//Proceedings of the American Control Conference.Denver，Colorado，2003.

［13］郭云飛，吳慶憲，姜長生.基于修正羅德里格參數(shù)的小衛(wèi)星SSUKF姿態(tài)確定［J］.上海航天，2008(6):12-25.

GUO Yunfei，WU Qingxian，JIANG Changsheng.Attitude estimation using modified rodrigues parameters based on SSUKF for micro-satellite［J］.Aerospace Shanghai，2008(6):12-25.

［14］劉林.航天器軌道理論［M］.北京:國防工業(yè)出版社，2000: 20-50.

［15］XIONG K，LIU L D，ZHANG H Y.Modified unscented Kalman filtering and its application in autonomous satellite navigation［J］.Aerospace Science and Technology，2009，13:238-246.

［16］張共愿，程詠梅，楊峰.預(yù)測(cè)—校正EKF算法在自主導(dǎo)航中的應(yīng)用［J］.宇航學(xué)報(bào)，2009，30(6):2227-2230.

ZHANG Gongyuan，CHENG Yongmei，YANG Feng.Applying of Forecast-revise EKF algorithm in autonomous navigation system［J］.Journal of Astronautics，2009，30(6): 2227-2230.