許立松,馬國(guó)梁,聞 泉,王雨時(shí)

(南京理工大學(xué) 1.機(jī)械工程學(xué)院;2.能源與動(dòng)力工程學(xué)院,江蘇 南京 210094)

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

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

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

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

1 組合濾波模型

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

為了描述火箭彈的飛行姿態(tài),引入飛行力學(xué)中的姿態(tài)角概念:俯仰角?,偏航角ψ,滾轉(zhuǎn)角γ。地面坐標(biāo)系A(chǔ)xyz和彈體坐標(biāo)系Ox1y1z1定義方式如文獻(xiàn)[16]所述。地面坐標(biāo)系A(chǔ)xyz先繞Ay軸順時(shí)針旋轉(zhuǎn)ψ角,形成坐標(biāo)系A(chǔ)x0yz0,坐標(biāo)系A(chǔ)x0yz0再繞Az0軸順時(shí)針旋轉(zhuǎn)?角形成坐標(biāo)系A(chǔ)x1y0z0,坐標(biāo)系A(chǔ)x1y0z0再繞Ax1軸順時(shí)針旋轉(zhuǎn)γ角形成坐標(biāo)系A(chǔ)x1y1z1,坐標(biāo)系A(chǔ)x1y1z1平移至彈丸質(zhì)心形成彈體坐標(biāo)系Ox1y1z1。用俯仰角?和偏航角ψ可以確定彈軸在地面坐標(biāo)系中的指向。

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

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

(1)

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

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

(2)

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

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

(3)

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

(4)

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

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

Z=(vxvyvzxyz)T+V

(5)

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

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

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

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

(6)

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

(7)

(8)

(9)

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

①計(jì)算相應(yīng)雅可比矩陣。

②時(shí)間更新:

(10)

(11)

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

(12)

(13)

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

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

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

(14)

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

(15)

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

②按下式計(jì)算Sigma點(diǎn)向量:

(16)

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

(17)

(18)

(19)

(20)

(21)

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

(22)

(23)

(24)

⑤完成后,返回②,計(jì)算下一時(shí)刻的狀態(tài)估計(jì)值。可以看出UKF與傳統(tǒng)Kalman濾波器的區(qū)別主要在于狀態(tài)協(xié)方差陣的計(jì)算方法不同。

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

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

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

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

4 仿真分析

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

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

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

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

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

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

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

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

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

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

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

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

圖7 滾轉(zhuǎn)角對(duì)比曲線(10～10.1 s)

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

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

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

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

圖11 EKF和UKF濾波后滾轉(zhuǎn)角估計(jì)誤差曲線

表1 濾波估計(jì)最大誤差

表2 濾波估計(jì)誤差均方根

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

5 結(jié)束語(yǔ)

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

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