基于星敏感器的衛(wèi)星姿態(tài)估計方法研究

，，

(1.火箭軍指揮學(xué)院， 武漢 430012；2.空軍駐湖南地區(qū)軍事代表室，長沙 410100；3.空軍駐河南地區(qū)軍事代表室，鄭州 450006)

0 引言

現(xiàn)代小衛(wèi)星技術(shù)已有20余年的發(fā)展歷史,經(jīng)過不斷的研究試驗,在技術(shù)上得到了全面迅速的提高。小衛(wèi)星有質(zhì)量小、體積小、成本低、研制周期短、功能密度大及發(fā)射靈活等特點,在通信、對地觀測、行星探測、科學(xué)技術(shù)驗證等多個領(lǐng)域得到了廣泛應(yīng)用。限于質(zhì)量與功耗的約束常采用無陀螺定姿方案,利用衛(wèi)星姿態(tài)動力學(xué)方程結(jié)合星敏感器的測量信息實現(xiàn)姿態(tài)確定,常用算法是將姿態(tài)動力學(xué)方程中的干擾力矩項增廣為狀態(tài)變量增廣卡爾曼濾波器[1]。干擾力矩是隨衛(wèi)星軌道運行作周期性變化的量,在使用過程中發(fā)現(xiàn),在干擾力矩變化較快的時間段,姿態(tài)估計準確度較低,而在干擾力矩變化較為平緩的時間段,姿態(tài)估計結(jié)果良好。為克服干擾力矩變化給姿態(tài)估計結(jié)果精度帶來的影響,本文從增廣卡爾曼濾波的基本原理出發(fā),詳細分析了干擾力矩變化影響姿態(tài)估計精度的原因[2],并從不同側(cè)面提出了兩種不同的改進算法:1)增廣自適應(yīng)卡爾曼濾波器(Adaptive Augmented Extended Kalman Filter, AAEKF)算法,依據(jù)前一時間段的干擾力矩估計結(jié)果,估算干擾力矩的變化率,依據(jù)估計出的變化率自適應(yīng)調(diào)整系統(tǒng)的偽隨機噪聲,使濾波器能更好地跟蹤上干擾力矩的變化,同時姿態(tài)估計精度也得到提高。2)增廣強跟蹤卡爾曼濾波器(Augmented Strong Tracking Extended Kalman Filter, ASTEKF)算法,將干擾力矩視為模型中的參數(shù),干擾力矩變化過快導(dǎo)致算法無法準確辨識干擾力矩大小,致使動力學(xué)模型中干擾力矩參數(shù)失配,姿態(tài)估計精度降低,將強跟蹤的思想引入到增廣卡爾曼濾波器中,可以有效地降低因為模型參數(shù)失配對姿態(tài)估計精度的影響。文中通過仿真試驗的方式,對三種動力學(xué)姿態(tài)估計器的性能作了詳細的分析,驗證了改進后的動力學(xué)姿態(tài)估計器提高姿態(tài)估計精度的能力。

1 無陀螺衛(wèi)星姿態(tài)確定系統(tǒng)描述

1.1 姿態(tài)確定系統(tǒng)的狀態(tài)方程

無陀螺衛(wèi)星姿態(tài)確定系統(tǒng)由于沒有陀螺儀提供角速率信息,必須在運動學(xué)方程的基礎(chǔ)上引入姿態(tài)動力學(xué)方程作為對角速率信息的補充。因此姿態(tài)確定系統(tǒng)的狀態(tài)方程包含有運動學(xué)方程和動力學(xué)方程,具體形式如下所示[1，3]。

Tc是動量輪以外的控制力矩,Td是干擾力矩,h是動量輪產(chǎn)生的動量矩,J是衛(wèi)星的慣量矩陣。

1.2 姿態(tài)確定系統(tǒng)的觀測方程

姿態(tài)確定系統(tǒng)從星敏感器獲得矢量觀測信息,至少需要2個不平行的觀測矢量才能確定衛(wèi)星的姿態(tài)。假定星敏感器本體系坐標軸方向和衛(wèi)星本體系坐標軸方向一致,則系統(tǒng)觀測方程可以寫為

CS=A(q)·CI+υ

其中:

是衛(wèi)星軌道坐標系到星敏感器本體系轉(zhuǎn)換矩陣的四元數(shù)形式,CI是參考矢量在衛(wèi)星軌道坐標系中的表示,CS是參考矢量在星敏感器本體系中的表示,υ是測量白噪聲。

2 動力學(xué)姿態(tài)估計器的設(shè)計

2.1 傳統(tǒng)增廣卡爾曼濾波

非線性系統(tǒng)的狀態(tài)方程和測量方程如下所示,狀態(tài)方程是連續(xù)形式的,而測量方程是離散形式[4-5]。

z(tk)=h(x(tk),u(tk),β(tk))+v(tk)

其中,x(t)是狀態(tài)矢量,z(t)是測量矢量,u(t)是輸入矢量,β是系統(tǒng)參數(shù),Γ(t)是系統(tǒng)噪聲分布矩陣,w(t)是系統(tǒng)噪聲矢量,v(t)是測量噪聲矢量。滿足如下條件

E[w(t)]=0

E[v(tk)]=0

E[w(t)vT(tk)]=0

E[w(t)wT(τ)]=Q(t)δ(t-τ)

E[v(tk)vT(tj)]=R(tk)δkj

若將非線性系統(tǒng)參數(shù)β(t)增廣為狀態(tài)矢量,則增廣后的狀態(tài)矢量為

其中,nβ是隨機噪聲,定義為增廣狀態(tài)的隨機變化率,方差根據(jù)實際情況可調(diào),滿足

結(jié)合上式,系統(tǒng)狀態(tài)方程可以改寫為

對于上面的非線性狀態(tài)方程,無法直接利用普通的卡爾曼濾波算法,需對非線性模型進行一階泰勒展開,并將連續(xù)狀態(tài)模型離散化,則卡爾曼濾波器的計算流程如下面各式所示[4-5]。

預(yù)測方程:

xa(t0|0)=xa(t0)

式中:

Φa(tk|k-1)=eFa(tk)Δt

校正方程:

K(tk)=Pa(tk|k-1)HT(tk)[H(tk)Pa(tk|k-1)·

HT(tk)+R(tk)]-1

xa(tk|k)=xa(tk|k-1)+K(tk)[z(tk)-

h(xa(tk|k-1),u(tk))]

Pa(tk|k) =[I-K(tk)H(tk)]Pa(tk|k-1)

= [I-K(tk)H(tk)]Pa(tk|k-1)·

[I-K(tk)H(tk)]T+

K(tk)Ra(tk)KT(tk)

2.2 增廣自適應(yīng)卡爾曼濾波器

依據(jù)文獻[2,6]中的論述,增廣卡爾曼濾波對增廣狀態(tài)的估計原理實際上是極小方差準則下的隨機逼近,與增廣狀態(tài)的逼近能力與增廣狀態(tài)隨機變化率nβ的方差大小有關(guān)。增大隨機變化率的方差,可以增強卡爾曼濾波器對時變增廣狀態(tài)的跟蹤能力,但是也會增大濾波器的帶寬,使濾波結(jié)果不夠平滑;反之減小隨機變化率的方差可以得到較為平滑的結(jié)果,但又無法跟蹤上增廣狀態(tài)隨時間的變化。若能夠在濾波過程中依據(jù)實時狀態(tài)對增廣狀態(tài)的隨機變化率進行在線調(diào)整,即在增廣狀態(tài)變化較慢的時間段采用較小的隨機變化率,在增廣狀態(tài)變化較快的時間段采用較大的隨機變化率,不僅能夠較好地跟蹤上增廣狀態(tài)的變化,也能有效控制估計結(jié)果的隨機噪聲。

具體的調(diào)整機制是采用增廣狀態(tài)的實時估計結(jié)果,近似計算出最近一個時間段內(nèi)增廣狀態(tài)的變化率,將增廣狀態(tài)的隨機變化率方差寫成近似估計出的增廣狀態(tài)的變化率的單調(diào)遞增函數(shù),本文的單調(diào)遞增函數(shù)選取F[x]=xxT,具體形式如下:

Qnβ(tk)=Qnβ(t0)F[εκβ(tk)]

其中，Δt是濾波間隔時間,m是估計增廣狀態(tài)變化率的時間窗內(nèi)濾波次數(shù),ε是壓縮因子。給定Qnβ(t0)值之后可依據(jù)濾波結(jié)果計算出Qnβ(tk),用Qnβ(tk)代替2.1節(jié)中增廣卡爾曼濾波算法中的Qnβ進行運算即可。

2.3 增廣強跟蹤卡爾曼濾波器

當模型參數(shù)發(fā)生變化時,濾波器過程參數(shù)會與模型參數(shù)失配,造成濾波器狀態(tài)估值偏離系統(tǒng)真實狀態(tài)。反映在輸出殘差序列上就是殘差序列不再相互正交,此時只要在線適當調(diào)整增益矩陣,使得輸出殘差序列仍相互正交,則可強迫濾波器保持對實際系統(tǒng)狀態(tài)的跟蹤[7-8]。增廣狀態(tài)β可視為模型參數(shù),增廣狀態(tài)的跟蹤偏差可視為模型參數(shù)失配,將強跟蹤思想引入增廣卡爾曼濾波器中,形成增廣強跟蹤卡爾曼濾波器,可以更好地對增廣狀態(tài)進行估計。增廣強跟蹤卡爾曼濾波器和普通增廣卡爾曼濾波器(Augmented Extended Kalman Filter, AEKF)的實施過程相比,運算流程基本相同,只是在狀態(tài)協(xié)方差預(yù)測過程中乘以一個時變的漸消矩陣LMD(k+1),借以實時改變增益矩陣。

P(k+1,k)=LMD(k+1)Φ(k+1,k)P(k)·

ΦΤ(k+1,k)+ΔtΓ(k)Q(k)ΓΤ(k)

LMD(k+1)= diag[λ1(k+1),…,λi(k+1),

…,λn(k+1)]i=1,2,…,n

確定時變漸消矩陣的一步算法為

V0(k+1) =E[γ(k+1)γT(k+1)]

N(k+1)=V0(k+1)-KaR(k+1)-

H(k+1,k)Γ(k)Q(k)ΓT(k)H(k+1,k)

M(k+1)=Φ(k+1,k)P(k)Φ(k+1,k)·

HT(k+1,k)H(k+1,k)

其中，ai的值可以依據(jù)系統(tǒng)擴展后的先驗知識來確定,0<ρ≤1為遺忘因子,Kb≥1為弱化因子,γ(k)為第k步輸出殘差,n為擴展后的系統(tǒng)狀態(tài)維數(shù)。

3 仿真結(jié)果及分析

依據(jù)1.1節(jié)中的衛(wèi)星姿態(tài)確定系統(tǒng)的狀態(tài)方程,將未知的干擾力矩項增廣為狀態(tài)矢量,則動力學(xué)姿態(tài)估計系統(tǒng)的狀態(tài)方程可以改寫為

對應(yīng)到上面的增廣卡爾曼濾波公式中有

f(xa(t),u(t))=

對于姿態(tài)確定系統(tǒng)的測量方程,則有

z(tk)=CS;h(x(tk),u(tk),β)=A(q)·CI

已知完備的衛(wèi)星姿態(tài)確定系統(tǒng)狀態(tài)方程和測量方程,便可以運用上面的增廣卡爾曼濾波算法對星敏感器的測量數(shù)據(jù)進行濾波,得到衛(wèi)星姿態(tài)的實時信息。干擾力矩、姿態(tài)角速率、姿態(tài)四元數(shù)信息都在衛(wèi)星軌道坐標系中定義,星敏感器本體系坐標軸方向和衛(wèi)星本體系坐標軸方向一致。三軸穩(wěn)定小衛(wèi)星的轉(zhuǎn)動慣量矩陣為

參照文獻[9-10],干擾力矩基本都是周期函數(shù)形式,采用如下形式來模擬干擾力矩

Td(t)=[ -Tdx0sin(ωφt),Tdy0sin(ωθt),

-Tdz0sin(ωψt)]

其中，Tdx0=Tdy0=Tdz0為1×10-5N·m。其中ωφ=6ωe、ωθ=4ωe、ωψ=8ωe,ωe為衛(wèi)星的公轉(zhuǎn)角速率,假定衛(wèi)星在650km的圓軌道,則ωe為0.06148839356680(°)/s。濾波器初始狀態(tài)為:

q0=[0.0,0.0,0.0,1.0]

ω0=[0.0,0.0,0.0]

Td=[0.0,0.0,0.0]

系統(tǒng)噪聲為

E[w(t)wT(t)]=Q(t)=εI3×3

測量噪聲為

E[v(tk)vT(tk)]=R(tk)=1×10-10I3×3

3.1 增廣卡爾曼濾波器姿態(tài)估計結(jié)果

仿真所用干擾力矩如圖1所示。圖2、圖3所示為采用不同的系統(tǒng)噪聲(增廣狀態(tài)隨機變化率)時,普通增廣卡爾曼濾波器的干擾力矩和歐拉角估計誤差。從圖中可以看出,干擾力矩和歐拉角估計誤差呈周期性變化,且其變化周期剛好與圖1中顯示的干擾力矩的變化周期一致。在干擾力矩的峰值處(變化率最小處)估計誤差最小,相反在干擾力矩過零處(變化率最大處)估計誤差最大。較大的系統(tǒng)噪聲雖然可以抑制估計結(jié)果的振蕩誤差,但同時隨機誤差也會增大;采用較小的系統(tǒng)噪聲,隨機誤差會減小,但估計結(jié)果會有很大的振蕩誤差。

圖1 仿真所用的干擾力矩Fig.1 Perturbed moment for simulation

圖2 AEKF干擾力矩估計誤差Fig.2 Perturbed moment estimation error of AEKF

圖3 AEKF歐拉角估計誤差Fig.3 Eulerian angle estimation error of AEKF

圖4、圖5所示為增廣強跟蹤卡爾曼濾波器采用不同系統(tǒng)噪聲時，干擾力矩和歐拉角估計誤差與AEKF的估計結(jié)果一樣,也呈周期性變化。但是ASTEKF結(jié)果受系統(tǒng)噪聲的影響較小,且小系統(tǒng)噪聲的估計不僅隨機噪聲較小,誤差的偏離也得到了抑制。因此在使用ASTEKF時,不用擔心過小的系統(tǒng)噪聲會跟蹤不上干擾力矩的變化帶來的偏差。

圖6、圖7所示為增廣自適應(yīng)卡爾曼濾波器的干擾力矩和歐拉角估計誤差,與AEKF不同,在估計誤差的峰值處都呈現(xiàn)平臺狀,有效地抑制了峰值處干擾力矩變化帶來的估計誤差。

圖4 ASTEKF干擾力矩估計誤差Fig.4 Perturbed moment estimation error of ASTEKF

圖5 ASTEKF歐拉角估計誤差Fig.5 Eulerian angle estimation error of ASTEKF

圖6 AAEKF干擾力矩估計誤差Fig.6 Perturbed moment estimation error of AAEKF

圖7 AAEKF歐拉角估計誤差Fig.7 Eulerian angle estimation error of AAEKF

圖8、圖9所示為三種算法估計結(jié)果的比較。對于AEKF而言,系統(tǒng)噪聲不能太大,也不能太小,這里顯示估計性能最好時的結(jié)果,此時Q=10-14·I3。對于ASTEKF而言,系統(tǒng)噪聲盡可能地取小,這里選取Q=10-16I3。而AAEKF濾波過程中自適應(yīng)調(diào)整系統(tǒng)噪聲,因此無需選定一個系統(tǒng)噪聲。從圖中可以看出,AEKF和ASTEKF的最優(yōu)性能與AAEKF的性能相當。三種濾波器的可操作性依次是AAEKF優(yōu)于ASTEKF,ASTEKF又優(yōu)于AEKF。

圖8 三種算法干擾力矩估計誤差Fig.8 Perturbed moment estimation error of three algorithms

圖9 三種算法歐拉角估計誤差Fig.9 Eulerian angle estimation error of three algorithms

4 結(jié)論

對于AEKF而言,系統(tǒng)噪聲的選取對濾波結(jié)果有著重要影響,必須對干擾力矩的實際情況有充分的了解才能選取合適的系統(tǒng)噪聲,實用性較差；ASTEKF對系統(tǒng)噪聲選取的依賴性較小,較小的系統(tǒng)噪聲就可以跟蹤上干擾力矩的變化,濾波結(jié)果的隨機誤差也得到了控制;AAEKF方法無需采用固定的系統(tǒng)噪聲,通過實際的估計過程實時地給出系統(tǒng)噪聲,同時抑制了估計結(jié)果隨機誤差和振蕩型偏差,具有較強的實用性。

[1] Psiaki M L, Martel F, Pal P K.Three-axis attitude determination via Kalman filtering of magnetometer data[J].Journal of Guidance, 1990, 13(3): 506-514.

[2] George J, Crassidis J L.Sensitivity analysis of disturbance accommodating control with Kalman filter estimation[C]//Proceedings of the AIAA Guidance, Navigation and Control Conference and Exhibit.Hilton Head, 2007.

[3] Lefferts E J, Markley F L, M.D.Shuster.Kalman Filtering for Spacecraft Attitude Estimation[J].Journal of Guidance Control & Dynamics, 1982, 5(4):536-542.

[4] Ljung L.Asymptotic behavior of the extended Kalman filter as a parameter estimator for linear systems[J].IEEE Transactions on Automatic Control, 1979, 24(1): 36-50.

[5] 蔡金獅.飛行器系統(tǒng)參數(shù)辨識學(xué)[M].北京:國防工業(yè)出版社,2003.

[6] Merwe R V D.Sigma-point Kalman filters for probabilistic inference in dynamic state-space models[D].University of Stellenbosch, 2004.

[7] 周東華,序裕庚,張鐘俊.一種帶多重次優(yōu)漸消因子的擴展卡爾曼濾波器[J].自動化學(xué)報,1991,17(6):689-695.

[8] 周東華,葉銀忠.現(xiàn)代故障診斷與容錯控制[M].北京:清華大學(xué)出版社,2000.

[9] 屠善澄.衛(wèi)星姿態(tài)動力學(xué)與控制[M].北京:中國宇航出版社,1998.

[10] 章仁為.衛(wèi)星軌道姿態(tài)動力學(xué)與控制[M].北京:北京航空航天大學(xué)出版社,1998.