龍達峰,孫俊麗,溫晶晶

(1 惠州學院,廣東惠州 516007;2 中北大學信息與通信工程學院,太原 030051;3 晉西工業(yè)集團有限責任公司,太原 030027)

0 引言

由于旋轉彈發(fā)射高過載、高速旋轉和彈載空間狹小等惡劣彈載環(huán)境的限制,適用于旋轉彈用的測量傳感器主要有MEMS陀螺儀、地磁傳感器和GPS等[1-4],而且現(xiàn)有導彈彈載姿態(tài)測量系統(tǒng)直接移植于高速旋轉彈導航應用時會出現(xiàn)各種問題。特別是彈丸發(fā)射時高達10 000g的極大過載,使得彈載姿態(tài)測量傳感器容易出現(xiàn)短時的飽和失真、性能退化、甚至失效等,造成彈載測量系統(tǒng)的性能嚴重退化、參數(shù)不全或是導航初始參數(shù)獲取困難等問題[5]。針對彈載姿態(tài)測量系統(tǒng)移植應用問題,目前普遍采用對彈體本身進行局部改造以實現(xiàn)測量系統(tǒng)的移植應用,但這些改造方會影響到依靠旋轉穩(wěn)定工作方式彈丸的氣動力特性,且造成彈載裝置結構變得非常復雜,從而極大地增加了旋轉彈智能化改造難度[6]。因此,研制具有易移植、小體積和高精度的旋轉彈用姿態(tài)測量系統(tǒng)成為其制導化改造的難點所在。此外,在彈載慣導測量系統(tǒng)中,若對低精度MEMS陀螺測量輸出角速率的積分解算存在嚴重的誤差累積?？紤]到基于地磁信息的磁測姿態(tài)系統(tǒng)是通過測量彈體內地磁場矢量信息來確定彈體姿態(tài)方法,其測量誤差不隨時間累積[7-8]。因此,文中提出了一種利用地磁傳感器測量信息對慣性測量系統(tǒng)進行組合濾波方法,由地磁傳感器、MEMS陀螺和GPS測量彈體姿態(tài)信息,再根據(jù)所建立的組合濾波模型,由改進EKF濾波算法完成旋轉彈飛行姿態(tài)準確估計。

1 旋轉彈飛行姿態(tài)測量方案

旋轉彈姿態(tài)傳感器方案包括三軸地磁傳感器和三軸MEMS陀螺儀,分別用于測量地磁信息和彈體角速率信息。文中選取地面發(fā)射坐標系作為導航參考坐標系,因此彈體磁測姿態(tài)解算公式為:

(1)

通過對組合濾波模型的可觀測性研究分析發(fā)現(xiàn)[9-10],若僅僅利用MEMS陀螺輸出角速率和地磁傳感器輸出信息進行彈體姿態(tài)濾波估計,3個姿態(tài)角是不完全可觀測的,解決的方法是借助于其它輔助測量方法獲得其中任一個姿態(tài)角后再進行組合濾波。考慮到旋轉彈外彈道飛行的特殊性,即便是彈丸受控飛行期間,彈丸飛行也是小攻角狀態(tài),因此在不考慮彈丸的攻角情況下,完全可以利用GPS所測量的彈體速度信息來估算彈體的偏航角和俯仰角,用于代替偏航角不變假設,其估算原理如圖1所示。

圖1 彈丸偏航角和俯仰角估算原理

由圖1所示速度投影關系可知,利用GPS測量所得的速度信息來估算彈體偏航角ψm和俯仰角θm姿態(tài)公式為:

(2)

式(2)是假設攻角為零的情況下才能完全成立。然而,若旋轉彈飛行時攻角較小情況下,仍然可以利用式(2)進行計算,所估算所得偏航角和俯仰角會將存在一定的誤差,但其可以當作是測量噪聲帶來的誤差來進行處理,可以通過后續(xù)的姿態(tài)濾波算法對其進行誤差參數(shù)估計與數(shù)據(jù)補償。

綜合上述測量方案,文中采用以磁測信息、彈體速度估算所得偏航角和俯仰角共同作為系統(tǒng)觀測信息,對慣性導航系統(tǒng)進行姿態(tài)組合濾波估計,最終實現(xiàn)旋轉彈飛行姿態(tài)的準確測量。

2 旋轉彈彈體姿態(tài)估計算法

2.1 彈體姿態(tài)組合濾波器結構器設計

為了提高彈體姿態(tài)算法的實時性,彈體姿態(tài)濾波器采用圖2所示3個濾波器組成的多速率組合式實時濾波結構。其中磁傳感器濾波器和陀螺濾波器分別用于完成彈載地磁傳感器和MEMS陀螺角速率測量輸出的數(shù)據(jù)的濾波與誤差補償;旋轉彈飛行姿態(tài)估計濾波器用于實現(xiàn)彈體偏航角、俯仰角和滾轉角三維姿態(tài)角的快速濾波估計。

圖2 多速率組合式的實時濾波器方案

2.2 地磁傳感器誤差估計與補償

彈載地磁傳感器用于測量彈體系坐標內的地磁信息,其測量誤差主要有零偏、靈敏度誤差和交叉耦合誤差等[11-12]。因此,采用公式(3)所示的磁傳感器誤差模型進行地磁傳感器測量誤差參數(shù)在線估計和磁測數(shù)據(jù)補償。

(3)

對測量誤差模型(3)進行進一步的變換,可推導得到三軸磁傳感器測量數(shù)據(jù)的補償公式為:

(4)

2.3 旋轉彈彈體姿態(tài)估計濾波器

由捷聯(lián)慣導原理可知,彈體飛行姿態(tài)歐拉方程式為[13-14]:

(5)

選取彈體偏航角、俯仰角和滾轉角作為系統(tǒng)的狀態(tài)變量Xa=[ψ,θ,γ]T;以彈體飛行姿態(tài)歐拉方程式(5)作為狀態(tài)方程,將其可簡寫為如下一般形式:

(6)

v(t)

(7)

式中,ci,si(i=ψ,θ,γ)分別為余弦函數(shù)cosi和正弦函數(shù)sini的簡寫形式。由于上述觀測方程是非線性方程,將其簡寫為如下形式:

Za(t)=h[Xa(t),t]+v(t)

(8)

因此,由狀態(tài)方程(5)和觀測方程(7)共同構成彈體姿態(tài)組合濾波模型。

2.4 改進EKF的彈體姿態(tài)濾波算法

考慮到上述所構建的姿態(tài)組合濾波模型是強非線性特征,濾波模型的線性與離散化處理時,傳統(tǒng)EKF濾波算法實質上是基于一階近似的線性化方法,在Taylor級數(shù)展開關系式中并沒有取其余高階項,只取一階項,分析發(fā)現(xiàn)其會帶來較大的系統(tǒng)誤差,使得其濾波估計精度有所下降。因此,文中在進行模型的離散化及線性處理時,采用二階近似方法來降低線性化帶來的系統(tǒng)誤差,即取到Taylor級數(shù)展開的二階項,通過這樣的方法以降低線性化帶來的系統(tǒng)誤差,為進一步提高EKF算法精度。相比于線性一階逼近的EKF濾波算法,文中所采用基于二階近似的改進EKF濾波算法主要不同在于如下兩個方程:

(9)

式中：ei為第i個標準基向量,即第i個元素為1,其余為0;tr(·)表示取矩陣的跡,也即為取矩陣主對角元素之和;Hx為觀測方程的雅可比矩陣;而Hxx,i為觀測方程Hessian矩陣,其計算公式為:

(10)

算法除上述公式不同,二階近似改進EKF濾波算法與傳統(tǒng)EKF濾波算法基本相同,其主要流程也包括初值選取、狀態(tài)預測與量測更新3個濾波算法步驟,但由于進行Taylor展開時取二階近似,所以狀態(tài)預測和量測更新方程有所不同。因此,基于二階近似改進EKF濾波算法具體步驟如下:

1)濾波初始參數(shù):

2)時間更新過程:

3)量測更新過程:

因此,通過上述的初值選取、狀態(tài)預測與量測更新3個濾波算法步驟,最終完成彈載飛行姿態(tài)參數(shù)的快速濾波估計。

3 算法的仿真驗證

3.1 仿真參數(shù)設置

文中選取152 mm高速旋轉彈為仿真對象,根據(jù)旋轉彈外彈道數(shù)學模型,首先利用Matlab程序計算機仿真生成外彈道飛行數(shù)據(jù)以及各彈載姿態(tài)傳感器測量輸出的理想數(shù)據(jù)。然后,再根據(jù)所建立的彈載傳感器的測量誤差模型,并按如表1所示的彈載傳感器誤差參數(shù)設置,仿真生成彈載傳感器的實際測量輸出數(shù)據(jù),在此基礎上進行彈體飛行姿態(tài)估計算法的仿真驗證。

表1 彈載傳感器誤差參數(shù)設置

3.2 數(shù)值仿真及結果分析

根據(jù)上述仿真設置的條件,旋轉彈外彈道飛行軌跡如圖3所示。

圖3 彈丸外彈道曲線

采用前述基于改進EKF的彈體姿態(tài)濾波算法完成旋轉彈飛行偏航角、俯仰角和滾轉角三維姿態(tài)的濾波估計,彈體姿態(tài)濾波估計結果如圖4～圖9所示。

圖4 偏航角濾波估計結果

圖5 俯仰角濾波估計結果

圖6 滾轉角濾波估計結果

圖8 俯仰角估計誤差曲線

圖4～圖6所示分別為彈體偏航角、俯仰角和滾轉角的理論值與濾波估計值的結果對比,圖中藍色實線為旋轉彈理想彈道條件下的彈體姿態(tài)角,而紅色虛線所示為彈體姿態(tài)算法濾波估計所得三維姿態(tài)角,與之相對應的彈體偏航角、俯仰角和滾轉角姿態(tài)角估計誤差如圖7～圖9所示。

圖9 滾轉角估計誤差曲線

從上述姿態(tài)估計結果來看,其彈體三維姿態(tài)在5 s內基本能夠得到很好的收斂,在濾波穩(wěn)定后,經統(tǒng)計得彈體偏航角、俯仰角和滾轉角姿態(tài)估計誤差分別為0.806 8°、0.839 2°和3.926 4°(1σ估計誤差),結果表明彈體姿態(tài)濾波算法可以快速完成彈體飛行姿態(tài)準確估計。

4 結論

針對旋轉彈飛行姿態(tài)測量問題,文中采用地磁傳感器、MEMS陀螺和GPS姿態(tài)組合測量方案,建立了基于多速率組合式彈體姿態(tài)濾波模型,并采用了基于改進EKF的彈體姿態(tài)濾波算法完成彈體的姿態(tài)估計方法。最后選取152 mm旋轉彈為研究仿真對象,利用計算機仿真方法進行了所述算法的仿真驗證。仿真結果表明,彈體三維姿態(tài)在5 s內能夠實現(xiàn)很好的收斂,在濾波穩(wěn)定后,彈體三維姿態(tài)在5 s內能夠得到很好的收斂,經統(tǒng)計得彈體偏航角、俯仰角和滾轉角3個姿態(tài)角估計誤差分別為0.806 8°、0.839 2°和3.926 4°,驗證了文中所述算法可以快速完成彈體飛行姿態(tài)準確估計。