一種多傳感器組合的姿態(tài)估計(jì)方法

一種多傳感器組合的姿態(tài)估計(jì)方法

1衛(wèi)星姿態(tài)和三軸角速率的實(shí)時(shí)、連續(xù)估計(jì)皮衛(wèi)星是指重量約1kg的小型衛(wèi)星。它主要基于微電器系統(tǒng)（ss）技術(shù)和精細(xì)制造技術(shù)。它具有短于技術(shù)和簡(jiǎn)單的功能特點(diǎn)。它具有結(jié)實(shí)性強(qiáng)、功能準(zhǔn)確、負(fù)載接口靈活等特點(diǎn)。由于體積小,重量輕,功耗指標(biāo)要求嚴(yán)格,許多常規(guī)姿態(tài)測(cè)量器件和定姿方法,不能滿足應(yīng)用要求。文獻(xiàn)中,提出并論證了采用三軸磁強(qiáng)計(jì)并結(jié)合濾波方法,解算衛(wèi)星三軸姿態(tài)和三軸角速率的可行性。該方法適用于具有一定傾角的近地軌道,在極區(qū)軌道段上性能下降,不能完全適用于皮星任務(wù)的太陽同步軌道。文獻(xiàn)中,對(duì)利用衛(wèi)星表面貼裝的太陽電池陣采樣數(shù)據(jù)敏感衛(wèi)星姿態(tài)的可行性和精度進(jìn)行了分析,但該方法在衛(wèi)星進(jìn)入太陽陰影時(shí)失效。本文提出的定姿方案,將上述兩種測(cè)姿手段相結(jié)合,并采用樣點(diǎn)卡爾曼濾波方法(UnscentedKalmanFiltering),獲得對(duì)三軸姿態(tài)角和角速率的實(shí)時(shí)、連續(xù)估計(jì)輸出,解決了無陀螺條件下姿態(tài)角速率的獲取問題。在Matlab環(huán)境下進(jìn)行的數(shù)值仿真結(jié)果表明,精度能夠滿足皮星任務(wù)要求。樣點(diǎn)卡爾曼濾波(UKF)方法,將樣點(diǎn)變換的非線性本質(zhì)以及卡爾曼濾波的遞推結(jié)構(gòu)相結(jié)合,更充分利用系統(tǒng)噪聲的統(tǒng)計(jì)特性,在濾波精度和收斂速度方面優(yōu)于EKF。缺點(diǎn)是運(yùn)算量大一倍,需要合理選擇狀態(tài)更新方程和濾波頻率,滿足星上實(shí)時(shí)運(yùn)算的要求。2元數(shù)姿態(tài)表示法剛體衛(wèi)星的姿態(tài)運(yùn)動(dòng),由姿態(tài)運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)方程組描述如下:式中,為姿態(tài)四元數(shù),ω為體軸系內(nèi)描述的角速度矢量。Ω(ω)代表由ω生成的4×4矩陣,[ω×]為叉乘矩陣,I為主軸系內(nèi)的慣量矩陣,hw代表偏置動(dòng)量輪角動(dòng)量,Tc為控制力矩,Td為擾動(dòng)力矩。選擇系統(tǒng)狀態(tài)為,則系統(tǒng)狀態(tài)方程及其離散形式定義為:式中,wk-1為過程噪聲矢量,協(xié)方差陣為Qk-1。過程噪聲主要由未建模的干擾力矩,以及選擇的具體數(shù)值算法產(chǎn)生。本文中選擇預(yù)報(bào)校正公式來將系統(tǒng)狀態(tài)進(jìn)行時(shí)間更新,具體如(5)～(9)式:式中,△T為濾波疊代時(shí)間步長(zhǎng),上標(biāo)avr表示求平均。為了保持四元數(shù)參數(shù)的規(guī)范性,采用增量轉(zhuǎn)動(dòng)的辦法處理四元數(shù)時(shí)間更新。四元數(shù)姿態(tài)表示法具有無奇點(diǎn)、無三角函數(shù)運(yùn)算、近似線性形式等優(yōu)點(diǎn),但在濾波疊代運(yùn)算中無法保證其規(guī)范性。強(qiáng)制進(jìn)行規(guī)范化處理,會(huì)使?fàn)顟B(tài)協(xié)方差陣失去非負(fù)定特性。文獻(xiàn)中提出一種解決方法:將四元數(shù)姿態(tài)參數(shù)降維,變換為三分量的“改良羅得里格斯參數(shù)(MRP)”,它與姿態(tài)四元數(shù)q及歐拉軸角參數(shù)(e,Φ)的關(guān)系為:用公式(11)評(píng)估小角度姿態(tài)誤差非常方便。降維后的MRP表示方法在q4=-1時(shí)出現(xiàn)奇異點(diǎn),需要采用基準(zhǔn)狀態(tài)與擾動(dòng)狀態(tài)相結(jié)合的方法回避這一困難,即:真實(shí)姿態(tài)運(yùn)動(dòng),分解為基準(zhǔn)狀態(tài)X和擾動(dòng)狀態(tài),再將增量四元數(shù)轉(zhuǎn)化為增量羅氏參數(shù)△p表示,轉(zhuǎn)換關(guān)系如下:式中,“”代表四元數(shù)乘法運(yùn)算,四元數(shù)求逆的定義為:。在一個(gè)濾波周期內(nèi),只要保證,即可以保證擾動(dòng)姿態(tài)(被估計(jì)狀態(tài))取值遠(yuǎn)離奇點(diǎn),避免了奇點(diǎn)問題。本文中,,濾波周期4s,滿足條件。綜合上述公式,對(duì)狀態(tài)參數(shù)的處理方法是:估計(jì)算法中直接操作擾動(dòng)狀態(tài),結(jié)合基準(zhǔn)運(yùn)動(dòng)狀態(tài),得到濾波輸出的估計(jì)狀態(tài),如下:3地磁矢量誤差來源姿態(tài)確定系統(tǒng)器件選擇時(shí),主要約束條件為:功耗、重量、價(jià)格。本文中擬采用的三軸磁強(qiáng)計(jì)與太陽電池陣組合定姿方案,就是綜合上述要求做出的選擇。方案中選用Honeywell-HMR2300型數(shù)字式三軸磁強(qiáng)計(jì),主要指標(biāo)如表1所示。對(duì)于測(cè)姿任務(wù),磁強(qiáng)計(jì)對(duì)地磁場(chǎng)強(qiáng)度矢量的測(cè)量精度為主要指標(biāo)。依據(jù)表1中的數(shù)據(jù),由磁強(qiáng)計(jì)本身測(cè)量噪聲引起的地磁矢量誤差,小于0.3°(1σ)。磁場(chǎng)參考矢量和測(cè)量矢量的其它誤差來源,在表2中列出。衛(wèi)星在軌飛行時(shí),短時(shí)間內(nèi)可將日光看作方向不變的平行光源。將此入射光作為參考方向,則衛(wèi)星表面貼裝的太陽電池陣列可起到姿態(tài)敏感器的作用,其輸出的歸一化電流信號(hào)中包含有衛(wèi)星的姿態(tài)信息,關(guān)系式如下:式中,m代表有正常輸出的太陽電池板編號(hào),總數(shù)為N。sB為體軸系內(nèi)度量的太陽單位矢量,pm為測(cè)量面法線單位矢量,αm為入射角,反映衛(wèi)星姿態(tài)信息。通常,當(dāng)同時(shí)有三個(gè)不共面的有效量測(cè)時(shí),sB矢量可求解得出。通過物理仿真實(shí)驗(yàn),此種途徑下單板測(cè)角精度1.15°(1σ),太陽矢量求解精度1.4°(1σ)。衛(wèi)星進(jìn)入太陽陰影段時(shí),將只用磁強(qiáng)計(jì)數(shù)據(jù)估計(jì)姿態(tài)。測(cè)量方程描述系統(tǒng)狀態(tài)與量測(cè)之間的關(guān)系,也為離散、非線性形式,如下:式中,為測(cè)量時(shí)刻的姿態(tài)四元數(shù)參數(shù),為方向余弦矩陣,磁場(chǎng)矢量和太陽矢量為準(zhǔn)慣性系中描述的兩個(gè)單位參考矢量,為已知值,vk為測(cè)量噪聲,協(xié)方差為Rk。在體軸系內(nèi)實(shí)際測(cè)量到的是磁場(chǎng)矢量bB和太陽矢量sB,真實(shí)觀測(cè)矢量為:4基于狀態(tài)濾波方法無角速率測(cè)量器件的衛(wèi)星,必須利用狀態(tài)估計(jì)的辦法,獲取角速率信息。解決此類非線性系統(tǒng)狀態(tài)估計(jì)問題,傳統(tǒng)的廣義卡爾曼濾波(EKF)方法表現(xiàn)出很多不足,例如:在設(shè)計(jì)EKF濾波器時(shí),要將系統(tǒng)狀態(tài)方程進(jìn)行一階線性化、離散化處理,容易造成濾波模型與實(shí)際系統(tǒng)模型有偏差,相應(yīng)要求濾波起始時(shí)刻的初始狀態(tài)偏差較小、濾波步長(zhǎng)較短等,但仍不能完全克服輸出有偏估計(jì)、濾波發(fā)散等問題。文獻(xiàn)中,提出一種新的非線性狀態(tài)估計(jì)方法:樣點(diǎn)卡爾曼濾波(UKF)。該方法仍采用Kalman濾波的遞推形式,但在濾波時(shí)間更新操作中,利用樣點(diǎn)變換(UnscentedTransformation)的優(yōu)異特性,更加充分的利用系統(tǒng)噪聲的統(tǒng)計(jì)特性,使濾波器以更高的階數(shù)和精度,捕獲到非線性傳播后的狀態(tài)均值和方差。文獻(xiàn)探討了將該方法應(yīng)用于有陀螺衛(wèi)星的姿態(tài)估計(jì)問題,其性能超過EKF方法,缺點(diǎn)是運(yùn)算量比EKF大一倍,這對(duì)于皮衛(wèi)星應(yīng)用的6狀態(tài)估計(jì)情形是可以承受的。EKF和UKF對(duì)狀態(tài)均值和方差的傳播效果示意如圖1。UKF姿態(tài)濾波算法結(jié)構(gòu)流程如圖2所示,包括三個(gè)主要環(huán)節(jié):(1)初始化:選定基準(zhǔn)狀態(tài)X,擾動(dòng)狀態(tài)x,協(xié)方差陣P的初值。(2)時(shí)間更新:在k～1時(shí)刻,計(jì)算擾動(dòng)狀態(tài)樣點(diǎn)集σk-1及權(quán)重系數(shù),結(jié)合當(dāng)前基準(zhǔn)狀態(tài),生成基準(zhǔn)狀態(tài)樣點(diǎn)集χk-1(i)并通過式(5)～(9)進(jìn)行時(shí)間更新,得到一步預(yù)測(cè)的樣點(diǎn)集,然后基準(zhǔn)狀態(tài)更新為,并在新基準(zhǔn)狀態(tài)附近,求時(shí)間更新后的擾動(dòng)狀態(tài)樣點(diǎn)集,同時(shí)計(jì)算預(yù)期量測(cè)樣點(diǎn)。得到新的擾動(dòng)狀態(tài)樣點(diǎn)集后,計(jì)算一步預(yù)測(cè)的狀態(tài)估計(jì),一步預(yù)測(cè)的狀態(tài)協(xié)方差Pk|k-1,一步預(yù)測(cè)量測(cè)估值。(3)量測(cè)更新:這部分與傳統(tǒng)KF方法基本相同。當(dāng)tk時(shí)刻新量測(cè)到來時(shí),獲得新息矢量vk,計(jì)算新息協(xié)方差,互相關(guān)協(xié)方差,以及當(dāng)前時(shí)刻濾波增益陣Kk。最后,得出tk時(shí)刻的擾動(dòng)狀態(tài)估計(jì)值xk和協(xié)方差陣Pk,疊加到當(dāng)前基準(zhǔn)狀態(tài)上,濾波結(jié)果為tk時(shí)刻的最優(yōu)狀態(tài)估計(jì)Xk。一個(gè)完整濾波計(jì)算周期結(jié)束。UKF算法的詳細(xì)描述可參考文獻(xiàn)。P0,Qk,Rk三個(gè)協(xié)方差陣,是對(duì)系統(tǒng)特性的描述,決定了濾波器的收斂速度、穩(wěn)健性和穩(wěn)態(tài)估計(jì)精度,取值時(shí)總希望盡可能接近系統(tǒng)真實(shí)水平。P0為初始狀態(tài)協(xié)方差,代表選取的濾波初值的精度水平。過程噪聲協(xié)方差陣Qk,體現(xiàn)兩方面誤差效應(yīng):(1)未建模干擾力矩引起的狀態(tài)預(yù)測(cè)誤差。干擾力矩模型越精確,這部分量值越小;(2)實(shí)際計(jì)算模型(式5～9)偏離真實(shí)系統(tǒng)模型(式3,4)造成的狀態(tài)一步預(yù)測(cè)誤差。在選定計(jì)算公式及濾波步長(zhǎng)后,可以用數(shù)值方法對(duì)Qk進(jìn)行評(píng)估。Rk為量測(cè)噪聲協(xié)方差陣,代表了敏感器件的測(cè)量精度,用第3小節(jié)中的器件精度指標(biāo)確定,具體取值見表3。濾波器實(shí)現(xiàn)時(shí),需要避免Pk陣失去非負(fù)定性(造成方差陣的三角分解無法進(jìn)行),同時(shí)也要避免濾波發(fā)散。產(chǎn)生這些異常的主要原因包括:(1)定點(diǎn)算法中,數(shù)值截?cái)嗾`差和舍入誤差隨時(shí)間積累。(2)系統(tǒng)模型與噪聲的統(tǒng)計(jì)模型不夠準(zhǔn)確,濾波遞推較長(zhǎng)時(shí)間后,新量測(cè)對(duì)估計(jì)輸出的修正作用減弱而陳舊量測(cè)的權(quán)重過強(qiáng)。對(duì)于前一種,可以對(duì)Pk陣進(jìn)行保護(hù)(元素值向主對(duì)角適度集中)。對(duì)于后一種,可采用衰減記憶法或限定記憶法,改進(jìn)濾波器結(jié)構(gòu),有效扼制濾波發(fā)散。當(dāng)保護(hù)措施無效而發(fā)散已經(jīng)出現(xiàn)時(shí),可選擇新的初始濾波狀態(tài),將濾波器重置。5初始誤差對(duì)姿態(tài)誤差的影響本節(jié),在Matlab環(huán)境下,對(duì)UKF姿態(tài)濾波進(jìn)行數(shù)值仿真,并給出相關(guān)結(jié)果。仿真程序中,通過對(duì)姿態(tài)微分方程進(jìn)行精確數(shù)值積分,獲得真實(shí)姿態(tài)變化規(guī)律。輸入濾波器的模擬量測(cè),則是通過10階IG-RF2000地磁模型和太陽歷表,加入測(cè)量噪聲后獲得,噪聲水平見前文所述。仿真實(shí)驗(yàn)相關(guān)參數(shù)設(shè)定見表3。仿真實(shí)驗(yàn)側(cè)重于三方面:(1)初始入軌后,實(shí)際角速率較大,各環(huán)節(jié)不確定性也最嚴(yán)重,此時(shí)重點(diǎn)考察濾波器對(duì)姿態(tài)角速度的捕獲能力,表現(xiàn)為抗初始誤差能力及收斂速度。在第一次濾波疊代之前先進(jìn)行兩次雙矢量解算,并用兩次姿態(tài)角的離散時(shí)間差分值近似角速度初值,代入濾波器。按照表3給出的測(cè)量誤差水平,這樣處理后能夠保證:初始姿態(tài)誤差最大3°,姿態(tài)角速率誤差最大±110%。表4中給出了各種初始誤差條件下的收斂能力及收斂速度。(2)姿態(tài)穩(wěn)定后,進(jìn)入對(duì)地定向狀態(tài),此時(shí)姿態(tài)參數(shù)只有小量值波動(dòng),濾波器輸出的穩(wěn)態(tài)角度、角速度估計(jì)精度是研究重點(diǎn),并且兩種測(cè)姿數(shù)據(jù)可同時(shí)獲得。選擇初始三軸角度誤差各1.57°(按表3數(shù)據(jù)進(jìn)行雙矢量定姿的1σ誤差),初始俯仰軸角速度誤差0.08(°)/s,偏航軸和滾動(dòng)軸角速度誤差0.02(°)/s,估計(jì)精度如圖3,4。從圖3,4中可以看出,組合兩種測(cè)量信息時(shí),穩(wěn)態(tài)精度很高:角度誤差<0.5°,三軸向角速度誤差<1×10-4(°)/s。圖4中的三分量誤差帶分別為初始角速率真值的5%,15%,5%(角速率分量主要在俯仰ωy方向上)。(3)進(jìn)入軌道陰影段,此時(shí)只有磁強(qiáng)計(jì)數(shù)據(jù)可用,濾波器輸出的姿態(tài)估計(jì)值精度有所下降,如圖5,6所示。圖5中,給出了按3-1-2轉(zhuǎn)動(dòng)次序(Ψ-φ-θ)描述的姿態(tài)角度估計(jì)誤差隨時(shí)間的變換規(guī)律。260s左右衛(wèi)星經(jīng)過極點(diǎn),從圖中可以明顯看出,極區(qū)段軌道上估計(jì)精度下降,特別是對(duì)偏航方向姿態(tài)角(Ψ角)的觀測(cè)性影響較大。320s左右飛出極區(qū)后,估計(jì)精度開始恢復(fù)正常。圖6中,給出體軸系中描述的姿態(tài)角速度估計(jì)誤差隨時(shí)間的變換規(guī)律?？梢钥闯?過極區(qū)對(duì)角速率估計(jì)影響不明顯,但偏航和滾動(dòng)角速度表現(xiàn)出較強(qiáng)的耦合性、較弱的觀測(cè)性,收斂速度較慢。由于俯仰方向角速度值絕對(duì)占優(yōu),偏航和滾動(dòng)角速率的弱觀測(cè)性,對(duì)整體角速率估計(jì)的影響處于可承受范圍內(nèi)。6姿態(tài)角速率估計(jì)值估計(jì)對(duì)于沒有精確、可靠角速率測(cè)量器件的微型衛(wèi)星,ADCS子系統(tǒng)的一個(gè)重要任務(wù)就是利用現(xiàn)有的姿測(cè)信息,構(gòu)造出對(duì)姿態(tài)角速率的觀測(cè)能力。本文提出一種利用三軸磁強(qiáng)計(jì)數(shù)據(jù)與衛(wèi)星表面貼裝的太陽電池陣列信號(hào)組合,并利用樣點(diǎn)