三軸磁強(qiáng)計(jì)觀測(cè)的姿態(tài)確定方法

三軸磁強(qiáng)計(jì)觀測(cè)的姿態(tài)確定方法

0在線校正tam系統(tǒng)誤差三軸磁強(qiáng)計(jì)觀測(cè)（三軸磁強(qiáng)計(jì)，簡(jiǎn)單記錄為tam）觀測(cè)的近地衛(wèi)星姿態(tài)確定方法已經(jīng)開發(fā)。但是,大多數(shù)方法都采用簡(jiǎn)單的地磁觀測(cè)模型,僅僅考慮了隨機(jī)觀測(cè)噪聲的影響,而沒有完整考慮其它諸多系統(tǒng)誤差因素的影響。比較典型的系統(tǒng)誤差影響因素有TAM本身三個(gè)磁通線圈軸的非正交性、刻度因子調(diào)試誤差、零點(diǎn)常偏,以及剩磁場(chǎng)干擾和電磁設(shè)備工作時(shí)產(chǎn)生的附加干擾磁場(chǎng)。一些實(shí)際案例也證實(shí)對(duì)TAM進(jìn)行在線校正以確保獲得一定精度的姿態(tài)確定結(jié)果來說是非常必要的。Crassidis等人提出三個(gè)適合在線校正TAM系統(tǒng)誤差(3個(gè)非正交因素、3個(gè)刻度因子誤差和3個(gè)常偏置量)的參數(shù)濾波算法,即序貫中心化方法、普通EKF和簡(jiǎn)單UKF校正算法。事實(shí)上,后兩者的實(shí)質(zhì)區(qū)別在于新息協(xié)方差陣的計(jì)算有所不同,但UKF濾波器通常能夠取得相對(duì)其它兩個(gè)濾波器來說更高一些的估計(jì)精度和更快的收斂速度,而且對(duì)有色觀測(cè)噪聲也顯示出良好的魯棒性。需要指出的是,這三個(gè)校正算法僅僅利用了地磁觀測(cè)量的大小,而與其指向(或衛(wèi)星姿態(tài))無關(guān),屬于所謂的標(biāo)量校正算法。目前,利用非線性濾波理論解決姿態(tài)確定與TAM在線校正這一組合估計(jì)問題還是一個(gè)新鮮課題。本文作者曾經(jīng)討論了地磁姿態(tài)確定與地磁觀測(cè)常偏置量校正兩者組合的估計(jì)問題,該偏置量實(shí)際上是穩(wěn)態(tài)干擾磁場(chǎng)和電子設(shè)備零點(diǎn)常偏這兩個(gè)不具有獨(dú)立可觀性的偏置量的合成量。作者提出一個(gè)擴(kuò)維EKF濾波器,同時(shí)估計(jì)四元數(shù)、角速率常漂和地磁觀測(cè)常偏置量。本文進(jìn)一步引入更多系統(tǒng)誤差的影響,并基于一個(gè)完整的TAM觀測(cè)模型,利用擴(kuò)維UKF算法和雙重UKF算法給出幾個(gè)更加完善的地磁姿態(tài)確定算法。1k33+dk的校正一個(gè)完整的地磁觀測(cè)模型如下所示:bΜ,k=(Ι3×3+Dk)-1(ΟΤABR,kbR,k+bBiask+εk)(1)其中,I3×3表示單位矩陣;bM,k表示TAM觀測(cè)量;O表示正交安裝誤差矩陣;ABR,k表示星體系(B系)相對(duì)參考系(R系)的姿態(tài)矩陣;bR,k為依據(jù)地磁模型得到的參考矢量;bBiask表示常值偏置量;εk被認(rèn)為是零均值高斯地磁觀測(cè)噪聲,且方差為Σk=E{εkεkT}。TAM標(biāo)量校正觀測(cè)模型為:zk≡∥bΜ,k∥2-∥bR,k∥2=-bΜ,kΤ(2Dk+D2k)bΜ,k+2bΤΜ,k(Ι3×3+Dk)bBiask-∥bkBias∥2+veffk(2)其中,Dk矩陣的對(duì)角線元素表示刻度因子調(diào)試誤差,其它非對(duì)角線元素表示非正交性因素;veffk表示有效噪聲,且veffk≡2[(Ι3×3+Dk)bΜ,k-bBiask]εk-∥εk∥2(3)其統(tǒng)計(jì)模型可用高斯模型描述,且均值和方差分別為:μeffk=-tr(Σk)(4)(σeffk)2=νΤΣkν+2(trΣ2k)(5)且ν≡(Ι3×3+Dk)bΜ,k-bBiask(6)由于式(2)、(3)和(6)中的bM,k是未知量,所以必須用真實(shí)測(cè)量值?bM,k替代。2同時(shí)，修正三軸磁強(qiáng)計(jì)的狀態(tài)這部分基于擴(kuò)維估計(jì)方案和雙重濾波方案給出1個(gè)姿態(tài)確定擴(kuò)維UKF濾波器(記為AUKF)和2個(gè)雙重UKF濾波器(記為DUKF)。2.1dk模型和tam系統(tǒng)誤差參數(shù)狀態(tài)量定義為:xk≡[qΤk,βkΤ,ΘΤk]Τ(7)其中,qk表示四元數(shù);βk表示速率常漂;9維系統(tǒng)誤差參數(shù)Θk定義為:Θk≡[(bBiask)Τ[Dk,11Dk,22Dk,33Dk,12Dk,13Dk,23]Τ]Τ(8)離散四元數(shù)運(yùn)動(dòng)學(xué)方程與陀螺常漂及TAM系統(tǒng)誤差參數(shù)擴(kuò)展而成的系統(tǒng)方程如下所示:[qk+1βk+1Θk+1]=[Φk+1,k(?ωk-βk-ηυ,k)qkβk+ηu,kΘk](9)其中,?ωk表示角速率測(cè)量值;ηυ,k可看成是零均值高斯白噪聲,且方差陣為σ2υI3×3/Δt;ηu表示常漂量的一階Markov模型隨機(jī)驅(qū)動(dòng)噪聲,可看成是零均值高斯白噪聲,且方差陣為σ2uI3×3Δt。2.2基于地磁矢量觀測(cè)的k文獻(xiàn)提出一個(gè)經(jīng)典姿態(tài)確定UKF算法,即USQUE。為了解決四元數(shù)估計(jì)1度冗余性問題,USQUE采用了約束估計(jì)思想,即依據(jù)四元數(shù)后驗(yàn)估計(jì)利用四元數(shù)運(yùn)動(dòng)學(xué)方程預(yù)報(bào)一個(gè)參考四元數(shù),并利用一個(gè)非冗余的3分量姿態(tài)參數(shù)來表示并估計(jì)真實(shí)四元數(shù)與參考四元數(shù)之間的偏差四元數(shù)。該文基于TAM系統(tǒng)誤差得到完美校正的假設(shè)(即Θ=09×1)給出一個(gè)具體的地磁姿態(tài)確定算法。但在現(xiàn)實(shí)中,這種假設(shè)往往是不成立的。完整地磁矢量觀測(cè)模型(1)顯示,地磁矢量觀測(cè)與四元數(shù)表示的姿態(tài)矩陣ABR,k以及TAM系統(tǒng)誤差因素Θk都直接相關(guān),因而利用地磁觀測(cè)獲得的新息同樣可以對(duì)Θk進(jìn)行觀測(cè)更新估計(jì)。(關(guān)于這個(gè)擴(kuò)維狀態(tài)系統(tǒng)的可觀性分析請(qǐng)見3.3.1小節(jié))需要特別指出的是,在地磁矢量為唯一方向矢量觀測(cè)條件下,誤裝矩陣O是不可觀的,而要使之成為可觀,則必須引入其它高精度方向矢量觀測(cè)。為了分析簡(jiǎn)單起見,本文假設(shè)O=I3×3。通過直接將該參數(shù)擴(kuò)展到原狀態(tài)量中得到一個(gè)16維擴(kuò)維狀態(tài)量(7),其系統(tǒng)方程和觀測(cè)方程分別為式(9)和式(1)?；诖?依據(jù)USQUE不難得到同時(shí)估計(jì)參數(shù)矢量Θk的AUKF,其實(shí)際狀態(tài)量(15維)定義為Xaugk=[δΡkΤβΤkΘΤk]Τ其中,δPk為偏差四元數(shù)對(duì)應(yīng)的3維GRP姿態(tài)參數(shù)。然后,利用經(jīng)典UKF理論就可以給出相應(yīng)的AUKF,而四元素均值估計(jì)則由GRP姿態(tài)參數(shù)估計(jì)(轉(zhuǎn)換成偏差四元數(shù))與預(yù)報(bào)參考四元數(shù)合成得到。需要注意的是,每次觀測(cè)更新結(jié)束之后必須將δPk部分重置為零矢量。至于該濾波器的具體形式,這里就不再詳細(xì)給出了。2.3耦合或解耦的姿態(tài)確定模型DUKF包括一個(gè)姿態(tài)確定UKF濾波器和一個(gè)TAM系統(tǒng)誤差參數(shù)UKF濾波器。本文提出一個(gè)解耦DUKF濾波器和一個(gè)耦合DUKF濾波器。所謂耦合或解耦指的是TAM系統(tǒng)誤差校正與姿態(tài)信息(ABR,k(qk))有關(guān)或無關(guān)。具體來說就是,解耦DUKF的姿態(tài)確定USQUE采用簡(jiǎn)單地磁觀測(cè)模型,其參數(shù)校正UKF則選擇與姿態(tài)無關(guān)的標(biāo)量校正觀測(cè)模型(2)。耦合DUKF的姿態(tài)確定UKF同樣選擇USQUE,但采用完整地磁觀測(cè)模型(1),其參數(shù)校正UKF也采用同一完整觀測(cè)模型。為描述簡(jiǎn)潔起見,這里僅僅介紹一下耦合DUKF的參數(shù)UKF:(1)濾波器初始化k.0記初始參數(shù)均值和協(xié)方差估計(jì)分別為Θ0和PΘ0,令?Θ0|0=Θ0?ΡΘ0|0=ΡΘ0。(2)確定參數(shù)下的西格瑪點(diǎn)集參數(shù)均值和協(xié)方差時(shí)間更新為:?Θk|k-1=?Θk-1|k-1ΡΘk|k-1=ΡΘk-1|k-1然后,依據(jù)?Θk|k-1和PΘk|k-1,利用確定性UT采樣得到tk時(shí)刻的參數(shù)西格瑪點(diǎn)集{Θik|k-1}180,以及均值計(jì)算權(quán)值{W(m,i)k}18i=0和協(xié)方差計(jì)算權(quán)值{W(c,i)k}18i=0;(3)方法及估計(jì)結(jié)果預(yù)報(bào)觀測(cè)量西格瑪點(diǎn)集為zik=(Ι3×3+Dik)-1[ABR,k(?q-k|k-1)bR,k+bBias,ik],i=0,?,18其中,Dik表示由Θik|k-1最后6個(gè)分量組成的Dk矩陣;bBias,ik為Θik|k-1前3個(gè)分量組成的偏置矢量;?q依據(jù)前一時(shí)刻USQUE給出的四元數(shù)后驗(yàn)估計(jì)?qˉk-1|k-1和陀螺角速率觀測(cè)值利用離散四元數(shù)運(yùn)動(dòng)學(xué)方程(9)預(yù)報(bào)得到。為了利用三軸陀螺(相對(duì)TAM)的高頻采樣信息(假設(shè)采樣頻率相差KRIG倍),本文提出如下方法:?qˉj+1|j=Φj+1,j(?ωk-1+j-?βk-1+j|k-1)?q當(dāng)j=0時(shí),?q;當(dāng)j=KRIG}-1時(shí),令?q;?βk-1+j|k-1為USQUE給出的陀螺常漂估計(jì)?βk-1|k-1的時(shí)間更新,且?βk-1+j|k-1=?βk-1|k-1;?ωk-1+j表示地磁采樣間隔[tk-1,tk]內(nèi)第j個(gè)陀螺測(cè)量值。預(yù)報(bào)觀測(cè)量均值為?zk=18∑i=0W(m,i)kzik根據(jù)地磁粗糙測(cè)量zk=?bΜ,k,分別得到新息及其協(xié)方差υΘk=zk-?zkΡυυ,Θk=24∑i=0W(c,i)k(zik-?zk)(zik-?zk)Τ+Rk其中,Rk表示觀測(cè)噪聲(I3×3+Dk)-1εk的協(xié)方差陣,且Rk=(Ι3×3+?Dk)-1Σk(Ι3×3+?Dk)-Τ其中,?Dk為先驗(yàn)均值估計(jì)?Θk|k-1后6個(gè)分量組成的Dk矩陣;(·)-T表示逆矩陣轉(zhuǎn)置操作。相關(guān)協(xié)方差陣和增益矩陣分別為:Ρxz,Θk=18∑i=0W(c,i)k(Θik|k-1-?Θk|k-1)(zik-?zk)ΤΚΘk=Ρxz,Θk(Ρυυ,Θk)-1最后,參數(shù)估計(jì)觀測(cè)更新為?Θk|k=?Θk|k-1+ΚΘkυΘkΡΘk|k=ΡΘk|k-1-ΚΘkΡυυ,Θk(ΚΘk)Τ存儲(chǔ)?Θk|k和PΘk|k,以備USQUE和參數(shù)UKF算法下次遞推估計(jì)之用。3計(jì)算機(jī)模擬和結(jié)果分析3.1真實(shí)地磁場(chǎng)仿真本文考慮一個(gè)典型的近地剛體小衛(wèi)星。其近圓軌道(高度為823km、傾角為82deg)數(shù)據(jù)由商業(yè)軟件STK╋產(chǎn)生。衛(wèi)星處于無控狀態(tài),但受重力梯度力矩、剩磁力矩(剩磁矩為0.3A·m2)等內(nèi)/外界干擾力矩的影響,其初始旋轉(zhuǎn)角速率為2.0deg/s。真實(shí)地磁觀測(cè)量由真實(shí)地磁場(chǎng)矢量加上各種干擾因素的影響合成得到。本文采用一個(gè)10×10階的IGRF模型仿真出真實(shí)地磁場(chǎng)。各種具體干擾因素綜合形成的系統(tǒng)誤差影響假設(shè)為:bBiastrue=[5000nΤ3000nΤ6000nΤ]ΤDtrue=[0.050.050.050.050.100.050.050.050.05]另外,地磁矢量觀測(cè)還受到隨機(jī)觀測(cè)噪聲εk的影響,具體有TAM敏感器噪聲、地磁場(chǎng)模型化誤差以及定軌誤差等。實(shí)際觀測(cè)噪聲εk是一個(gè)有色噪聲,本文利用一階Markov模型來產(chǎn)生這一觀測(cè)噪聲,其時(shí)間常數(shù)對(duì)應(yīng)于18deg軌道弧長(zhǎng)(大概300s左右),有色噪聲大小與標(biāo)準(zhǔn)差為60nT的高斯白噪聲相同,采樣周期為10s。三軸陀螺真實(shí)角速率測(cè)量值由衛(wèi)星真實(shí)旋轉(zhuǎn)角速率以及常漂參數(shù)和隨機(jī)高斯噪聲的影響合成得到。真實(shí)常漂為0.1deg/h,而σ2υ=0.1(μrad)2/s,σu2=1×10-7(μrad)2/s3,采樣周期為1s。3.2啟動(dòng)濾波和參數(shù)調(diào)整(1)初始協(xié)方差陣的選擇仿真中選擇的初始四元數(shù)估計(jì)與真實(shí)四元數(shù)之間的偏差四元數(shù)的歐拉角大概為50deg左右。令δP0=03×1,其初始協(xié)方差陣則選擇一個(gè)相當(dāng)大的矩陣(盡管不具有物理意義,但能夠加快姿態(tài)估計(jì)收斂的速度)。陀螺常漂初始估計(jì)為0.2deg/h,其初始協(xié)方差估計(jì)選為Ρ0β=diag([0.2deg/h0.2deg/h0.2deg/h]2)(2)差陣選設(shè)置令初始參數(shù)均值估計(jì)Θ0=09×1;其協(xié)方差陣選為Ρ0Θ=diag([1e-5nΤ1e-5nΤ1e-5nΤ0.10.10.10.10.10.1]2)(3)參考地磁變量的生成與濾波器觀測(cè)噪聲的選擇利用已知軌道信息和一個(gè)8×8階IGRF模型產(chǎn)生所需的參考地磁矢量。濾波器采用的地磁觀測(cè)噪聲標(biāo)準(zhǔn)差經(jīng)過調(diào)試選為5×60nT(σ)。3.3模擬結(jié)果與分析3.3.1不同線性化系統(tǒng)及的異質(zhì)性這部分考察了基于地磁矢量觀測(cè)估計(jì)四元數(shù)、陀螺常漂參數(shù)以及TAM校正參數(shù)這一擴(kuò)維狀態(tài)的可觀性。連續(xù)四元數(shù)運(yùn)動(dòng)學(xué)方程與陀螺常漂及TAM系統(tǒng)誤差參數(shù)擴(kuò)展而成的狀態(tài)方程和觀測(cè)方程(1)構(gòu)成的系統(tǒng)是明顯的非線性系統(tǒng),而目前分析非線性系統(tǒng)可觀性的技術(shù)和方法還很不成熟,一個(gè)通用的處理方法是將連續(xù)非線性系統(tǒng)離散線性化,然后利用線性系統(tǒng)可觀性分析手段對(duì)非線性系統(tǒng)的局部可觀性能進(jìn)行分析。本文考慮的擴(kuò)維系統(tǒng)經(jīng)過離散線性化處理后的形式如下:Δxk+1=Ψ(tk+1,tk)Δxk+ζk,k=0,?,Ν-1(10)zk+1=ΗkΔxk+ξk,k=1,??Ν(11)式中,Δxk≡[ΔqkΤ,ΔβkΤ,ΔΘkΤ]T,表示真實(shí)狀態(tài)與估計(jì)狀態(tài)之間的誤差,其中,Δq表示四元數(shù)估計(jì)誤差的矢量部分;ζk和ξk分別表示離散后的系統(tǒng)噪聲和觀測(cè)噪聲;Ψ(tk+1,tk)表示狀態(tài)轉(zhuǎn)移矩陣,且Ψ(tk+1,tk)≈I15×15+FkΔt,其中,Δt表示離散間隔,I15×15表示單位矩陣,而線性化系統(tǒng)矩陣Fk不難從文獻(xiàn)得知:Fk=[-[ω?k×]3×3-12Ι3×303×9012×3012×3012×9]式(11)中,Hk表示觀測(cè)矩陣,根據(jù)文獻(xiàn),并經(jīng)過一定合理簡(jiǎn)化后得到:Ηk=[2[B^ref×]3×303×3Ι3×3-Σ]上式中,B^ref=ABR,k(q^ˉk)bR,k,而Σ=[Br,100Br,2Br,30Br,20Br,10Br,3Br,30Br,1Br,2]其中,Br=[Br,1Br,2Br,3]T表示參考地磁矢量觀測(cè)值。上述局部線性化離散系統(tǒng)(10)和(11)的可觀性可根據(jù)一段觀測(cè)時(shí)間區(qū)間上的Gramian矩陣的奇異性與否判斷得知,?=∑k=1ΝΨΤ(tk,t0)ΗkΤΗkΨ(tk,t0)若上述矩陣是非奇異矩陣,則該系統(tǒng)是可觀的。令Δt=10s,N=3600,通過研究發(fā)現(xiàn),各種已考慮到情況下的Gramian矩陣都是非奇異的,證實(shí)該擴(kuò)維狀態(tài)系統(tǒng)確實(shí)是可觀的。另外,通過對(duì)該矩陣的特征值分解,發(fā)現(xiàn)其最小特征值與最大特征值的比率約為3×10-10;若忽略掉9維TAM待校正參數(shù)相應(yīng)的行和列,發(fā)現(xiàn)剩余6維子系統(tǒng)矩陣的最小特征值與最大特征值之間的比率增大為5×10-6,這表明TAM待校正參數(shù)的可觀性相對(duì)其它6維姿態(tài)參數(shù)來說要弱一些。3.3.2不同濾波器下的姿態(tài)確定誤差這部分考察了經(jīng)典USQUE(未校正TAM系統(tǒng)誤差)、AUKF和解耦/耦合DUKF共4個(gè)姿態(tài)確定濾波器在地磁矢量觀測(cè)存在系統(tǒng)誤差和有色觀測(cè)噪聲情況下的姿態(tài)確定性能。本文利用文獻(xiàn)提供的姿態(tài)確定性能評(píng)估方法,即,利用真實(shí)四元數(shù)與估計(jì)四元數(shù)之間的偏差四元數(shù)的旋轉(zhuǎn)歐拉角(單位為deg)評(píng)估四元數(shù)估計(jì)好壞,δα=2arccos(δq4)其中,δq4表示標(biāo)量部分;利用陀螺常漂估計(jì)β^Est與真實(shí)值βTrue之間的誤差的模(單位deg/h)評(píng)估其估計(jì)好壞|Δβ|=|β^Est-βΤrue|圖1和圖2給出4個(gè)濾波器的一組典型姿態(tài)確定誤差結(jié)果。顯然,未校正TAM觀測(cè)系統(tǒng)誤差的USQUE的姿態(tài)確定誤差遠(yuǎn)大于AUKF、解耦DUKF以及耦合DUKF的估計(jì)誤差?，F(xiàn)實(shí)中,由于TAM觀測(cè)無可避免地存在某種程度的系統(tǒng)誤差或出現(xiàn)某些極端情況,因而校正TAM觀測(cè)量對(duì)于保障姿態(tài)確定算法給出一定精度的姿態(tài)確定結(jié)果來說意義重大。這部分還利用蒙特卡洛仿真方法比較了AUKF、解耦DUKF及耦合DUKF三者的估計(jì)性能。在20次獨(dú)立仿真過程中,這三者都取得了明顯收斂的結(jié)果。圖3、圖4分別給出了TAM待校正參數(shù)(bBias的第三個(gè)分量和D33分量)估計(jì)誤差的均方根RMS(RootMeanSquare)結(jié)果。不難看出,這三個(gè)算法之所以能夠取得遠(yuǎn)遠(yuǎn)好于經(jīng)典USQUE算法姿態(tài)確定性能