安全保持模式下的kalman濾波器設(shè)計

安全保持模式下的kalman濾波器設(shè)計

1磁強計測量模型衛(wèi)星安全保持模型主要用于消除星箭分離引起的角速帶偏差，并通過控制星星旋轉(zhuǎn)來實現(xiàn)對日衛(wèi)星的定向，以確保衛(wèi)星的能源供應(yīng)，便于接收地球。在這個階段中的姿態(tài)確定精度要求不高,但是要求測量部件的可靠性高、功耗低。針對這類在低軌運行并具有豐富的地磁場資源可以利用的小衛(wèi)星而言,只利用三軸磁強計作為測量部件進行姿態(tài)確定,可以避免在安全保持模式下,陀螺出現(xiàn)故障時角速度的獲取問題,這無疑是一個較好的選擇。本文首先推導(dǎo)了衛(wèi)星姿態(tài)動力學(xué)、運動學(xué)和磁強計測量模型的線性化方程,采用四元數(shù)和角速度作為狀態(tài)變量描述衛(wèi)星的姿態(tài)。在推導(dǎo)中對四元數(shù)歸一化進行了研究,將誤差四元數(shù)降階為3個獨立的變量,從而避免誤差協(xié)方差陣的奇異問題。考慮到衛(wèi)星姿態(tài)的動力學(xué)和磁強計測量模型均為非線性方程的情況,選用增廣型Kalman濾波器(EKF)。EKF是一種非線性濾波法,其本質(zhì)特性是應(yīng)用模型及輸出方程的一階Taylor級數(shù)展開式,然后應(yīng)用線性濾波的方法,其模型誤差假設(shè)為已知的高斯過程。此算法僅依賴于一個三軸磁強計的測量數(shù)據(jù),通過對測得的一系列數(shù)據(jù)進行遞歸處理,就可以估計出衛(wèi)星的姿態(tài)角和角速度。最后用C++語言編程進行了數(shù)學(xué)仿真試驗,結(jié)果表明,姿態(tài)角的確定精度≤5°,姿態(tài)角速率的確定精度≤0.005(°)/s,能夠滿足此類任務(wù)的要求。2衛(wèi)星成像運動學(xué)和動態(tài)模型2.1單元角速度矢量討論衛(wèi)星的姿態(tài)時,首先要選定參考坐標(biāo)系,本文涉及3個坐標(biāo)系:當(dāng)?shù)氐乩碜鴺?biāo)系、衛(wèi)星軌道坐標(biāo)系和衛(wèi)星星體坐標(biāo)系。采用四元數(shù)描述衛(wèi)星姿態(tài),其姿態(tài)運動學(xué)方程與動力學(xué)方程為:˙ˉqbo=12ˉqbo?ˉωbo(1)˙ωbi=Ι-1[Τc+Τd-[ωbi×]Ιωbi](2)ˉqbo是由軌道坐標(biāo)系旋轉(zhuǎn)到星體坐標(biāo)系的四元數(shù)矢量,定義為:ˉqbo=[q0q1q2q3]Τ=[q0qΤ]Τˉωbo表示擴展的星體相對軌道坐標(biāo)系的角速度,定義為:ˉωbo=[0(ωbo)Τ]Τ?ωbo為星體相對軌道坐標(biāo)系的角速度矢量;“?”表示四元數(shù)乘法;ωbi為星體相對慣性坐標(biāo)系的角速度矢量;I為衛(wèi)星慣性陣;Tc為施加于衛(wèi)星的控制力矩矢量;Td為施加于衛(wèi)星的干擾力矩矢量;[ωbi×]具有如下形式:[ωbi×]=[0-ωbizωbiyωbiz0-ωbix-ωbiyωbix0](3)2.2軌道坐標(biāo)系的相互影響定義真實四元數(shù)ˉqbo與估計四元數(shù)?ˉqbo之間的誤差四元數(shù)為:Δˉqbo=[Δq0Δq1Δq2Δq3]Τ=[Δq0ΔqΤ]Τ(4)在標(biāo)稱姿態(tài)附近對式(1)和(2)進行線性化,有Δ˙ˉqbo=12Δˉqbo??ˉωbo-12?ˉωbo?Δˉqbo+12Δˉqbo?Δ?ˉωbo(5)Δ˙ωbi=Ι-1[ΔΤc+ΔΤd-Δωbi×(Ιωorb)-ωorb×(ΙΔωbi)](6)ωorb=[0-ω00]為衛(wèi)星標(biāo)稱角速度矢量(標(biāo)稱狀態(tài)下,軌道坐標(biāo)系與衛(wèi)星本體坐標(biāo)系重合);此時ω0=√μ/r3,μ為地球引力常數(shù),γ為衛(wèi)星地心距。考慮到在每一個采樣周期內(nèi)星體相對于軌道坐標(biāo)系的攝動是小轉(zhuǎn)角情況,在線性化過程中認為誤差四元數(shù)是小量,則Abo(Δˉqbo)可簡化為:Abo(Δˉqbo)=[12Δq3-2Δq2-2Δq312Δq12Δq2-2Δq11]=Ι-2[Δq×](7)其中,Δωbo=Δωbi-[Abo(Δˉqbo)-Ι3×3]Abo(?ˉqbo)ωoi(8)姿態(tài)角速度估計誤差ΔˉωΙ可表示為:Δωbi=ωbi-?ωbi(9)將(7),(8)和(9)代入(5)中,并略去二階以上小量,可得到:Δ˙q=[Δ˙q1Δ˙q2Δ˙q3]=-[?ωbi×]Δq+12Δωbi(10)Δ˙q0=0(11)由上式可見,在處理四元數(shù)歸一化的同時將誤差四元數(shù)自然降階為3個獨立的變量,從而避免了誤差協(xié)方差陣奇異問題。式(6)和(10)就構(gòu)成了關(guān)于Δq和Δωbi的線性濾波狀態(tài)方程。定義誤差狀態(tài)向量為:ΔX=[ΔqΤΔωbiΤ]Τ6×1(12)忽略ΔTC,ΔTd的影響,則狀態(tài)方程可描述為:Δ˙X=FΔX+Wa。其中,F6×6=[-[?ωbi×]3×312E3×303×3F′3×3](13)F′3×3=Ι-1[[(Ιωorb)×]-[ωorb×]Ι](14)Wa為高斯白噪聲。3aboaoiacki的計量方程根據(jù)地理坐標(biāo)系、慣性坐標(biāo)系、軌道坐標(biāo)系及衛(wèi)星本體系之間的轉(zhuǎn)換矩陣,即可確定衛(wèi)星所在軌道位置的地磁場磁通密度Bb:Bb=AboAoiAigBg(15)式中,Aig,Aoi和Abo分別為地理坐標(biāo)系到慣性坐標(biāo)系的轉(zhuǎn)換矩陣、慣性坐標(biāo)系到軌道坐標(biāo)系的轉(zhuǎn)換矩陣及軌道系到本體系的坐標(biāo)轉(zhuǎn)換矩陣。定義測量殘差為測量值和估計值之差,推導(dǎo)從略。Ζ=[ΔBxbΔBybΔBzb]=2[?Bb×]Δq+vm=ΗΔX+Va(16)Η=[2[?Bb×]3×303×3]3×6(17)Va為測量噪聲矢量。4強計姿態(tài)確定算法根據(jù)上述推導(dǎo),用誤差四元數(shù)和誤差角速度作為狀態(tài)變量,由式(13)和(16)得到三軸磁強計姿態(tài)確定模型為:{ΔX˙=FΔX+WaΖ=ΗΔX+Va(18)EKF姿態(tài)確定算法過程包括3部分:測量、濾波估計和校正。基于上述模型,對衛(wèi)星姿態(tài)進行估計的過程如下:(1)bi-tk狀態(tài)轉(zhuǎn)移陣tk設(shè)*-,*+分別表示狀態(tài)變量更新前后的值。在磁強計測量期間,計算姿態(tài)變量的預(yù)報值:q?bo-(tk+1)=Φ(ω?bo,Δt)qbo+(tk)(19)ω?bi-(tk+1)=ωbi+(tk)(20)其中Φ(ω?bo,Δt)為四元數(shù)狀態(tài)轉(zhuǎn)移陣。誤差協(xié)方差陣Ρ=E{x?x?Τ}的預(yù)報計算為:Ρ-(tk+1)=φ(Δt)Ρ+(tk)φΤ(Δt)+Γ(Δt)Q(tk)ΓΤ(Δt)(21)式中φ(Δt)為濾波狀態(tài)轉(zhuǎn)移矩陣,Q(tk)為系統(tǒng)狀態(tài)噪聲矩陣,φ(Δt)=eF(t)Δt≈Ι+F(tk)Δt(22)Γ(Δt)=∫0Δtφ(τ)G(τ)dτ(23)Q(tk)為系統(tǒng)噪聲矩陣。(2)觀察指標(biāo)矩陣計算在tk+1時刻,根據(jù)磁強計給出的測量值,計算觀測矩陣H,并對狀態(tài)變量進行更新計算,獲得tk+1時刻的狀態(tài)更新值x+(tk+1)。K(tk+1)=P-(tk+1)HT(tk+1)[H(tk+1)P-(tk+1)HT(tk+1)+R(tk+1)]-1(24)P+(tk+1)=[I-K(tk+1)H(tk+1)]P-(tk+1)[I-K(tk+1)H(tk+1)]T+K(tk+1)R(tk+1)KT(tk+1)(25)x+(tk+1)=x-(tk+1)+K(tk+1)[z(tk+1)-H(tk+1)x-(tk+1)](26)其中R(tk+1)為系統(tǒng)觀測噪聲矩陣。R(tk+1)=diag(σm2,σm2,σm2)(27)(3)qbo-tk+1.2狀態(tài)變量的校正在獲得tk+1時刻的狀態(tài)變量x+(tk+1)后,按下式對姿態(tài)四元數(shù)、角速度進行校正:qbo+(tk+1)=q?bo-(tk+1)?Δq?bo(tk+1)(28)ωbi+(tk+1)=ω?bi-(tk+1)+Δω?bi(tk+1)(29)在校正了姿態(tài)、漂移后,狀態(tài)變量x重新設(shè)置為零。5初始誤差情況下的仿真數(shù)值仿真的思路是:給定小衛(wèi)星的初始姿態(tài)歐拉角和角速度,通過歐拉角求得初始的姿態(tài)四元數(shù),用解微分方程的算法求解動力學(xué)和運動學(xué)方程,得到衛(wèi)星仿真時段的姿態(tài)理論值,作為衡量濾波器性能的標(biāo)準(zhǔn)。地磁場模型采用球諧波模型,為了得到更加精確的地磁場測量值,磁強計的測量值是由10階的地磁場模型經(jīng)計算和坐標(biāo)轉(zhuǎn)化得到。仿真的衛(wèi)星參數(shù)選為:太陽同步軌道,軌道高度650km,軌道傾角為100.50793°,衛(wèi)星慣量特性Ixx=90.0kg·m2,Iyy=250.0kg·m2,Izz=250.0kg·m2。仿真的系統(tǒng)噪聲設(shè)為{10-10,10-10,10-10,10-10,10-10,10-10};三軸的測量噪聲方差均設(shè)為(1.69×10-12)T,仿真步長0.5s。仿真給出了2組初始偏差情況下的濾波誤差。(1)初始的歐拉角的姿態(tài)誤差分別{10°,10°,-10°},角速度偏差為{0.2(°)/s,-0.2(°)/s,0.2(°)/s}時,選的誤差方差陣的初值為{10-2,10-2,10-2,10-2,10-2,10-2},仿真結(jié)果表明姿態(tài)角的濾波收斂時間為1000s左右,誤差在4°左右,見圖1～3,姿態(tài)角速率濾波的收斂時間為150s左右,誤差在0.003(°)/s左右。見圖4～6:(2)初始的歐拉角的姿態(tài)誤差分別為{30°,30°,-30°},角速度為{0.4(°)/s,-0.4(°)/s,0.4(°)/s},誤差方差陣的初值為{10-4,10-4,10-4,10-4,10-4,10-4}時,仿真結(jié)果表明姿態(tài)角的濾波收斂時間為1500s左右,誤差在2°左右,見圖7～9,姿態(tài)角速率濾波的收斂時間為500s左右,誤差在0.003(°)/s左右。見圖10～12:由仿真曲線可以看出,在較小的初始偏差(三軸姿態(tài)誤差為10°,姿態(tài)角速度誤差為0.2(°)/s的時候,該濾波器能在1000s內(nèi)完成收斂,完成安全保持模式下的定姿要求;在較大的初始偏差(三軸姿態(tài)誤差為30°,姿態(tài)角速度誤差為0.4(°)/s的時候,該濾波器也能在1500s內(nèi)完成收斂,較好的完成任務(wù),允許有一定的初始估值偏差。6軸整體誤差仿真在濾波器算法的結(jié)構(gòu)形式確定后,需要調(diào)整參數(shù)以保證算法收斂到足夠的精度。為了進一步了解誤差方差陣的初值、測量噪聲、系統(tǒng)噪聲對濾波器的影響,對第一組初始值進行以下的仿真分析:(1)在其它值不變,將誤差方差陣的初值分別改為10-1,10-2,10-3,10-4,10-6進行仿真發(fā)現(xiàn)濾波器的初始收斂速度有了明顯改善,但同時穩(wěn)態(tài)估計精度有所下降,濾波器將失去“最小均方誤差”的最佳性。當(dāng)所設(shè)計的濾波器不能在規(guī)定的誤差范圍內(nèi)工作時,就必須在二者中折中選擇。(2)在其它值不變,將磁強計測量噪聲分別改為10-15T,10-16T,10-17T,10-18T,10-19T進行仿真發(fā)現(xiàn)測量噪聲越小,定姿的精度越高,收斂時間越短,但毛刺會增多。(3)在其它值不變,將系統(tǒng)噪聲數(shù)量級分別改為10-6,10-8,10-10,10-12,10-14進行仿真發(fā)現(xiàn)三軸姿態(tài)誤差和姿態(tài)角速度誤差由發(fā)散到收斂再到發(fā)散。由(2)、(3)分析得出:系統(tǒng)噪聲取的過小或者測量噪聲取得