基于星敏感器的衛星姿態估計方法研究

，，

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

0 引言

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

1 無陀螺衛星姿態確定系統描述

1.1 姿態確定系統的狀態方程

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

Tc是動量輪以外的控制力矩,Td是干擾力矩,h是動量輪產生的動量矩,J是衛星的慣量矩陣。

1.2 姿態確定系統的觀測方程

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

CS=A(q)·CI+υ

其中:

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

2 動力學姿態估計器的設計

2.1 傳統增廣卡爾曼濾波

非線性系統的狀態方程和測量方程如下所示,狀態方程是連續形式的,而測量方程是離散形式[4-5]。

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

其中,x(t)是狀態矢量,z(t)是測量矢量,u(t)是輸入矢量,β是系統參數,Γ(t)是系統噪聲分布矩陣,w(t)是系統噪聲矢量,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)增廣為狀態矢量,則增廣后的狀態矢量為

其中,nβ是隨機噪聲,定義為增廣狀態的隨機變化率,方差根據實際情況可調,滿足

結合上式,系統狀態方程可以改寫為

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

預測方程:

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 增廣自適應卡爾曼濾波器

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

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

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

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

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

當模型參數發生變化時,濾波器過程參數會與模型參數失配,造成濾波器狀態估值偏離系統真實狀態。反映在輸出殘差序列上就是殘差序列不再相互正交,此時只要在線適當調整增益矩陣,使得輸出殘差序列仍相互正交,則可強迫濾波器保持對實際系統狀態的跟蹤[7-8]。增廣狀態β可視為模型參數,增廣狀態的跟蹤偏差可視為模型參數失配,將強跟蹤思想引入增廣卡爾曼濾波器中,形成增廣強跟蹤卡爾曼濾波器,可以更好地對增廣狀態進行估計。增廣強跟蹤卡爾曼濾波器和普通增廣卡爾曼濾波器(Augmented Extended Kalman Filter, AEKF)的實施過程相比,運算流程基本相同,只是在狀態協方差預測過程中乘以一個時變的漸消矩陣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的值可以依據系統擴展后的先驗知識來確定,0<ρ≤1為遺忘因子,Kb≥1為弱化因子,γ(k)為第k步輸出殘差,n為擴展后的系統狀態維數。

3 仿真結果及分析

依據1.1節中的衛星姿態確定系統的狀態方程,將未知的干擾力矩項增廣為狀態矢量,則動力學姿態估計系統的狀態方程可以改寫為

對應到上面的增廣卡爾曼濾波公式中有

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

對于姿態確定系統的測量方程,則有

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

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

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

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

-Tdz0sin(ωψt)]

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

q0=[0.0,0.0,0.0,1.0]

ω0=[0.0,0.0,0.0]

Td=[0.0,0.0,0.0]

系統噪聲為

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

測量噪聲為

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

3.1 增廣卡爾曼濾波器姿態估計結果

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

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

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

圖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所示為三種算法估計結果的比較。對于AEKF而言,系統噪聲不能太大,也不能太小,這里顯示估計性能最好時的結果,此時Q=10-14·I3。對于ASTEKF而言,系統噪聲盡可能地取小,這里選取Q=10-16I3。而AAEKF濾波過程中自適應調整系統噪聲,因此無需選定一個系統噪聲。從圖中可以看出,AEKF和ASTEKF的最優性能與AAEKF的性能相當。三種濾波器的可操作性依次是AAEKF優于ASTEKF,ASTEKF又優于AEKF。

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

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

4 結論

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

[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] 蔡金獅.飛行器系統參數辨識學[M].北京:國防工業出版社,2003.

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

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

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

[9] 屠善澄.衛星姿態動力學與控制[M].北京:中國宇航出版社,1998.

[10] 章仁為.衛星軌道姿態動力學與控制[M].北京:北京航空航天大學出版社,1998.