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

，，

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

0 引言

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

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

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

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

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

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

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

CS=A(q)·CI+υ

其中:

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

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

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

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

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

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

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β是隨機(jī)噪聲,定義為增廣狀態(tài)的隨機(jī)變化率,方差根據(jù)實(shí)際情況可調(diào),滿足

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

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

預(yù)測(cè)方程:

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

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

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

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

2.3 增廣強(qiáng)跟蹤卡爾曼濾波器

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

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

確定時(shí)變漸消矩陣的一步算法為

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)擴(kuò)展后的先驗(yàn)知識(shí)來確定,0<ρ≤1為遺忘因子,Kb≥1為弱化因子,γ(k)為第k步輸出殘差,n為擴(kuò)展后的系統(tǒng)狀態(tài)維數(shù)。

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

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

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

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

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

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

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

參照文獻(xià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

測(cè)量噪聲為

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 結(jié)論

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

[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ù)辨識(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)漸消因子的擴(kuò)展卡爾曼濾波器[J].自動(dòng)化學(xué)報(bào),1991,17(6):689-695.

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

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

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