基于無跡卡爾曼濾波的火箭彈姿態(tài)估計

許立松,馬國梁,聞 泉,王雨時

(南京理工大學 1.機械工程學院;2.能源與動力工程學院,江蘇 南京 210094)

隨著外彈道學的發(fā)展,對制導彈藥的姿態(tài)測量提出了新的要求。大量程微機電(micro electro mechanical systems,MEMS)陀螺的出現(xiàn)為解決高轉速測量問題提供了途徑,通過MEMS陀螺的角速率測量信息進行捷聯(lián)解算可以得到彈丸飛行姿態(tài),但隨著時間增加會出現(xiàn)誤差累積的問題,因此一般將GPS與慣性器件進行組合[1-2],通過Kalman濾波獲得飛行姿態(tài)的估計值。

Kalman濾波方法于1960年被提出,在其基礎上發(fā)展了擴展卡爾曼濾波(extended Kalman filter,EKF)、無跡卡爾曼濾波(unscented Kalman filter,UKF)、粒子濾波(particle filter,PF)等一系列濾波方法[3-4]。迭代方法[5]和自適應算法[6]的加入進一步提升了濾波效果,神經(jīng)網(wǎng)絡[7]等人工智能技術也在濾波領域發(fā)揮了很大的作用。

國外一直致力于制導彈藥姿態(tài)估計方面的研究[8-11]。文獻[11]采用了EKF方法對雙旋彈丸進行姿態(tài)估計;文獻[12]針對炮射彈丸將卡爾曼濾波與多維標度(multi-dimensional scaling,MDS)相結合,提高了位置測量的魯棒性。國內(nèi)也有相應的研究,文獻[13]利用UKF方法辨識彈道模型,預報彈道落點;文獻[14]將UKF方法加以改進,降低了彈丸射程修正的誤差;文獻[15]結合EKF建立了固定鴨舵雙旋彈的實時位置和速度估計方法?？梢钥吹?多數(shù)研究集中在彈丸位置及速度參數(shù)的估計,部分彈丸姿態(tài)估計的研究主要使用EKF方法。使用EKF需要對系統(tǒng)進行線性化處理,而本文所述的火箭彈滾轉角速率較高,是狀態(tài)變化較快的強非線性系統(tǒng),因而線性化模型導致的精度丟失會導致濾波誤差的進一步加大。UKF方法實際上屬于粒子濾波器的一種,也被稱為Sigma點濾波器。UKF方法最大的特點是采用無跡變換來估計非線性變換的均值和方差,其計算復雜程度與擴展卡爾曼濾波相當,有研究表明,在對飛行器氣動參數(shù)辨識效果的比較中,UKF在辨識精度和收斂性上都優(yōu)于EKF。在以非線性系統(tǒng)模型為基礎的濾波算法中,UKF的計算量相對較小,隨著現(xiàn)今嵌入式系統(tǒng)計算能力的提升,基于UKF算法的實時嵌入式濾波有望實現(xiàn)工程化應用,因而選用UKF濾波方法進行組合濾波,并與EKF方法的濾波效果相對比,進行更深層次的討論。

本文以某尾翼穩(wěn)定火箭彈為研究對象,測量器件包括三軸角速率陀螺、三軸加速度計和GPS組件,陀螺和加速度計以捷聯(lián)方式安裝于彈體,能夠在彈丸飛行過程中測得彈體坐標系下的三軸角速率及三軸加速度。針對其姿態(tài)估計的濾波問題,以姿態(tài)角、位置、速度參數(shù)作為狀態(tài)變量,建立了火箭彈運動參數(shù)的捷聯(lián)慣性解算模型,以GPS的位置和速度測量值作為輸出變量,構成組合濾波模型。分別通過EKF和UKF濾波方法對飛行姿態(tài)進行估計,仿真結果表明,UKF組合濾波方法的濾波性能更加優(yōu)良,其俯仰角、偏航角估計誤差均方根相比EKF降低了一半,滾轉角估計誤差均方根降低了三分之二,滿足姿態(tài)估計的需求。

1 組合濾波模型

1.1 姿態(tài)角及坐標系定義

為了描述火箭彈的飛行姿態(tài),引入飛行力學中的姿態(tài)角概念:俯仰角?,偏航角ψ,滾轉角γ。地面坐標系Axyz和彈體坐標系Ox1y1z1定義方式如文獻[16]所述。地面坐標系Axyz先繞Ay軸順時針旋轉ψ角,形成坐標系Ax0yz0,坐標系Ax0yz0再繞Az0軸順時針旋轉?角形成坐標系Ax1y0z0,坐標系Ax1y0z0再繞Ax1軸順時針旋轉γ角形成坐標系Ax1y1z1,坐標系Ax1y1z1平移至彈丸質(zhì)心形成彈體坐標系Ox1y1z1。用俯仰角?和偏航角ψ可以確定彈軸在地面坐標系中的指向。

1.2 捷聯(lián)慣性解算模型

描述彈丸運動的基本參數(shù)除了姿態(tài)角外,還包括飛行速度及彈丸的位置坐標,共9個運動參數(shù),根據(jù)飛行力學及慣性導航知識[16],以差分形式寫出捷聯(lián)慣性解算方程組為

(1)

式中:Ts為采樣周期;上標k表示離散時刻序號;ωx1,ωy1,ωz1分別為彈體坐標系下三軸角速率;ax1,ay1,az1分別為彈體坐標系下三軸加速度分量;vx,vy,vz分別為彈丸對地飛行速度在地面發(fā)射坐標系下的三軸分量;x,y,z分別為彈丸質(zhì)心位置在地面發(fā)射坐標系下的三軸分量。

根據(jù)測量特性,認為三軸陀螺和加速度計的測量誤差由系統(tǒng)誤差和隨機誤差構成,以x1軸陀螺為例,測量模型可表示為

(2)

最終取狀態(tài)向量為

X=(?ψγvxvyvzxyz
εgx1εgy1εgz1εax1εay1εaz1)T

(3)

式中:εgx1,εgy1,εgz1為三軸角速率陀螺的系統(tǒng)誤差分量;εax1,εay1,εaz1為三軸加速度計的系統(tǒng)誤差分量。以x1軸陀螺為例,認為系統(tǒng)誤差變化緩慢甚至不隨時間變化時,其變化方程可描述為

(4)

對于y1,z1軸陀螺和三軸加速度計的系統(tǒng)誤差,狀態(tài)方程形式與式(4)一致,不再贅述。

根據(jù)Kalman濾波理論,式(1)和式(4)構成了狀態(tài)方程,而地面坐標系下的速度測量值和位置測量值都可以由GPS測量值通過坐標轉換得到,觀測向量可表示為

Z=(vxvyvzxyz)T+V

(5)

式中:V=(ηvxηvyηvzηxηyηz)T為觀測噪聲向量,各元素分別表示地面坐標系下速度分量和位置分量的隨機觀測噪聲。

2 EKF濾波方法的實現(xiàn)步驟

組合測姿系統(tǒng)的狀態(tài)方程形式上較為復雜,采用擴展卡爾曼濾波方法,需先將其線性化,并計算雅可比矩陣。

對于此非線性系統(tǒng),其狀態(tài)方程和觀測方程可寫為

(6)

式中:U為控制向量,W=(ηgx1ηgy1ηgz1ηax1ηay1ηaz1)T為過程噪聲,V為觀測噪聲向量,下標k表示離散時刻序號,其線性化表示為

(7)

(8)

(9)

EKF的實現(xiàn)步驟如下。

①計算相應雅可比矩陣。

②時間更新:

(10)

(11)

③測量更新并計算后驗狀態(tài)估計和狀態(tài)協(xié)方差陣:

(12)

(13)

重復以上步驟,即可計算下一時刻的狀態(tài)估計值。

3 UKF濾波方法實現(xiàn)步驟

根據(jù)UKF基本理論,對于狀態(tài)方程組(1)和方程組(4),定義增廣狀態(tài)向量為

(14)

式中:W為過程噪聲向量,各元素分別表示三軸陀螺和三軸加速度計的隨機測量噪聲。令過程噪聲協(xié)方差陣為Q,觀測噪聲協(xié)方差陣為R,狀態(tài)協(xié)方差陣初值記為P0。用χ表示Sigma點對應的變量,令χX為狀態(tài)向量對應的Sigma點向量,χW為過程噪聲向量對應的Sigma點向量,χV為觀測噪聲向量對應的Sigma點向量,對照增廣狀態(tài)向量的定義方式,定義增廣Sigma點向量為

(15)

UKF算法的實現(xiàn)步驟如下。

②按下式計算Sigma點向量:

(16)

式中:α,κ為可以進行設置的濾波器參數(shù);N為增廣狀態(tài)向量維數(shù)。

(17)

(18)

(19)

(20)

(21)

⑤求取濾波器增益矩陣,計算后驗狀態(tài)估計和狀態(tài)協(xié)方差陣:

(22)

(23)

(24)

⑤完成后,返回②,計算下一時刻的狀態(tài)估計值?？梢钥闯鯱KF與傳統(tǒng)Kalman濾波器的區(qū)別主要在于狀態(tài)協(xié)方差陣的計算方法不同。

UKF算法中用到的有關參數(shù)選擇情況如下。

選擇κ≥0可以保證協(xié)方差陣的正半定性,本文中取κ=0;

選擇0≤α≤1,α控制Sigma點的分布范圍,一般選一個較小正數(shù),以避免強非線性時的局部采樣效應,本文中取α=0.001。

本文假設測量噪聲滿足高斯分布,參數(shù)的優(yōu)化取值為2,而參數(shù)λ=α2(N+κ)-N。

4 仿真分析

以某火箭彈為研究對象,進行六自由度彈道仿真計算,并分別使用慣性解算、EKF組合濾波和UKF組合濾波的方法對全彈道過程進行姿態(tài)估計。火箭彈初始質(zhì)量50 kg,推力持續(xù)2.5 s。主要仿真條件:初速為44.969 7 m/s,射角為45°,初始滾轉速率為31.73 rad/s,初始偏航速率為2 rad/s。

計算結果表明該彈丸最大轉速達到30 r/s,最大馬赫數(shù)3.2。在生成各測量器件的仿真測量值過程中,慣性器件的采樣周期選為2 ms,假定隨機噪聲均滿足高斯分布。其中x1軸陀螺隨機誤差的標準差為5(°)/s,系統(tǒng)誤差10(°)/s,y1、z1軸陀螺隨機誤差的標準差為5(°)/s,系統(tǒng)誤差3(°)/s,x軸加速度計隨機誤差的標準差為10×10-3g,系統(tǒng)誤差為100×10-3g,y、z軸加速度計隨機誤差的標準差為5×10-3g,系統(tǒng)誤差為50×10-3g。地面坐標系下三軸速度測量值的隨機誤差的標準差為0.1 m/s,三軸位置測量值的隨機誤差的標準差為5 m,不考慮速度和位置的系統(tǒng)誤差。

首先考慮僅采用慣性解算的方式獲取俯仰角和偏航角,假設俯仰角、偏航角初始對準誤差均為2°,滾轉角的初始對準誤差為5°。通過慣性解算可以獲得全彈道的俯仰角、偏航角和滾轉角。

慣性解算結果與彈道仿真值的曲線對比如圖1和圖2所示。

圖1 偏航角慣性解算曲線與真值對比曲線

圖2 俯仰角慣性解算曲線與真值對比曲線

可以看出,由于積累誤差的影響,慣性解算在彈丸飛行幾秒內(nèi)就很快發(fā)散,與真值相差較大。

圖3～圖8為全彈道下各姿態(tài)角真值與EKF和UKF濾波估計值的對比曲線,為了便于觀察,將俯仰和偏航角的對比曲線分時間段展示。滾轉角變化劇烈,亦取其中一段時間內(nèi)的數(shù)據(jù)進行觀察。

圖3 偏航角對比曲線(0～5 s)

圖4 偏航角對比曲線(5 s以后)

圖5 俯仰角對比曲線(0～5 s)

圖6 俯仰角對比曲線(5 s以后)

圖7 滾轉角對比曲線(10～10.1 s)

圖8 偏航角對比曲線(10～10.1 s)

兩種濾波方法下的各姿態(tài)角估計誤差(Δψ,Δ?,Δγ)曲線如圖9～圖11所示。結合表1和表2中數(shù)據(jù)可以觀察到,在俯仰角和偏航角的濾波估計上,EKF和UKF方法的誤差都較小,最大誤差相當,因而從兩者與真值曲線的對比圖可看出差距不大,但從誤差曲線可以看到,UKF的估計誤差明顯降低,由表中數(shù)據(jù)可得其誤差均方根較EKF降低了一半。在滾轉角的估計上,EKF濾波結果已經(jīng)完全無法滿足測量需求,而UKF方法的估計精度大幅提升,其誤差均方根相比EKF降低了三分之二。圖中的俯仰角、偏航角的估計值曲線呈波動狀,而滾轉角的估計曲線較為平滑,原因在于快速變化的滾轉角會對俯仰角、偏航角的估計產(chǎn)生影響,從圖7與圖8可以看到,相同的時間段下,其變化的趨同性是很明顯的。當豎直坐標軸范圍較小,同時坐標橫軸跨度較大,曲線便呈現(xiàn)出了較強的波動性。

圖9 EKF和UKF濾波后偏航角估計誤差曲線

圖10 EKF和UKF濾波后俯仰角估計誤差曲線

圖11 EKF和UKF濾波后滾轉角估計誤差曲線

表1 濾波估計最大誤差

表2 濾波估計誤差均方根

產(chǎn)生這些差異的原因在于EKF對模型的線性化使得EKF的先驗估計與真值差距較大,在相同的測量噪聲設定下,使用無跡變換的UKF沒有狀態(tài)轉移模型上的精度損失,顯然具有更小的方差分布。隨著火箭彈轉速的提升,相同的濾波周期下帶來的是滾轉角更大的變化尺度,從而隨著前期的誤差積累集中爆發(fā)。UKF更高的模型精度同時意味著其收斂速度會優(yōu)于EKF,這一點在圖10也有很好的體現(xiàn),可以看到UKF的誤差拐點先于EKF出現(xiàn)。

5 結束語

本文以姿態(tài)角、位置、速度參數(shù)作為狀態(tài)變量,建立了彈丸運動參數(shù)的捷聯(lián)慣性解算模型,對比了EKF和UKF方法的濾波效果。仿真結果驗證了UKF組合濾波方法的有效性,其俯仰角、偏航角估計誤差均方根相比EKF降低了一半,滾轉角估計誤差均方根降低了三分之二。

本文的UKF組合濾波方法仍有一定的不足,其對滾轉角的跟蹤精度尚不能達到同其余兩軸相當?shù)某潭?如何進一步改進濾波效果并使其適用于運動狀態(tài)變化更快的制導彈藥及常規(guī)彈藥,都有待進一步研究。