基于雙旋迫擊炮彈平臺的改進型Sage-Husa自適應濾波滾轉角測量算法

王佳偉， 祁克玉， 楊愷華， 梁軻， 閆杰

(1.西北工業(yè)大學 航天學院, 陜西 西安 710072; 2.西安機電信息技術研究所 機電動態(tài)控制重點實驗室, 陜西 西安 710065)

0 引言

基于中大口徑迫擊炮彈應用平臺，采用慣性器件/衛(wèi)星定位來組合測量彈體姿態(tài)和彈道參數(shù)，可將修正引信與彈體通過解耦部件連接、形成反向相對旋轉狀態(tài)，構成雙旋受控迫擊炮彈。在氣動力作用下，修正引信頭部的兩對翼面分別產生滾轉力矩和修正彈道所需控制力，這是目前國際上二維彈道修正引信技術中最具代表性的總體設計方案[1-3]。修正引信的實時滾轉角直接影響修正方向，是彈道控制模塊的重要輸入信息，其準確測量是二維彈道修正技術有待解決的關鍵技術。

陀螺/衛(wèi)星定位組合測量滾轉角最早于1995年由Lucia提出[4]，近年來不少文獻進行了進一步闡述：Park等[5]和Lee等[6]繼承并改良了Lucia的算法，但卻是在修正引信轉速不大于3 r/s的微旋狀態(tài)且恒定不變假設條件下提出的，與實際彈道中修正引信從起控至修正末段高達每秒數(shù)十轉的轉速跨度不符；Maley[7]提出了將擴展卡爾曼濾波(EKF)與磁傳感器輔助測量算法相結合的方法，并從理論仿真層面進行了驗證，但目前尚未有進一步的實驗室硬件測試驗證結果；文獻[8]中所涉及的傳統(tǒng)EKF滾轉角測量算法對于真實彈道環(huán)境下較大轉速變化范圍的滾轉角測量具有顯著效果，但對于彈道末段彈丸章動所引發(fā)的陀螺徑向兩軸測量誤差卻無法消除，從而嚴重影響了滾轉角測量精度。

Sage-Husa自適應卡爾曼濾波(SHAKF)可以在線估計量測噪聲并具有較好的穩(wěn)定性[9]，只是每次濾波都需要預估系統(tǒng)噪聲，增加了迭代濾波的復雜度[10]?？紤]到軸向陀螺測量修正引信轉速的精度較高、可以作為補償系統(tǒng)噪聲的先驗知識，本文提出了一種基于雙旋迫擊炮彈平臺的改進型SHAKF滾轉角測量算法，既能夠提高濾波實時性，又能夠大幅度降低量測噪聲對滾轉角測量精度的影響。

1 基本原理

1.1 陀螺/衛(wèi)星定位組合算法

遺憾的是，該方法僅局限于不大于3 r/s微旋恒定轉速條件下的滾轉角測量，基于理論解析得到的滾轉角又極易受到量測噪聲影響，因此需要設計一種針對轉速較大范圍變化情況的自適應濾波算法。

1.2 簡化SHAKF算法

設線性離散系統(tǒng)的數(shù)學模型為

X(k)=Φ(k,k-1)X(k-1)+Γ(k,k-1)W(k-1)，

(1)

Z(k)=H(k)X(k)+V(k)，

(2)

式中：X(k)為系統(tǒng)狀態(tài)向量；Z(k)為系統(tǒng)觀測向量；Φ(k,k-1)為系統(tǒng)狀態(tài)轉移矩陣；Γ(k,k-1)為系統(tǒng)噪聲矩陣；H(k)為系統(tǒng)量測矩陣；W(k)和V(k)分別為系統(tǒng)噪聲向量和量測噪聲向量；k表示第k次濾波迭代。

簡化SHAKF算法[12]為

(k)=Φ(k,k-1)(k-1)+K(k)V(k)，

(3)

V(k)=Z(k)-H(k)(k,k-1)，

(4)

K(k)=P(k,k-1)HT(k)·
[H(k)P(k,k-1)HT(k)+R(k)]-1，

(5)

P(k,k-1)=Φ(k,k-1)P(k-1)ΦT(k,k-1)+
Γ(k,k-1)Q(k)ΓT(k,k-1),

(6)

P(k)=[I-K(k)H(k)]P(k,k-1)[I-
K(k)H(k)]T+K(k)R(k-1)KT(k),

(7)

R(k)=(1-dk)R(k-1)+
dk{[I-H(k)K(k-1)]V(k)·
VT(k)[I-H(k)K(k-1)]T+
H(k)P(k-1)HT(k)},

(8)

式中：dk=(1-b)/(1-bk+1)，0

2 改進型SHAKF算法

改進型SHAKF算法具體步驟如下：

1)X(k)、Z(k)、Φ(k,k-1)和H(k)參照文獻[8]，Γ(k,k-1)改寫為

(9)

2)濾波迭代中增加轉速判據(jù)：根據(jù)(3)式～(8)式的濾波流程，當進行到第k次濾波時，針對計算得到的量測噪聲進行濾波狀態(tài)異常判斷，異常判據(jù)為

vT(k)v(k)>δtr(E[v(k)vT(k)]),

(10)

式中：v(k)為新息序列；δ為儲備系數(shù)，取值范圍δ>1；tr表示矩陣的跡；E為求矩陣期望。若(10)式成立則說明實際誤差將超過理論預估的δ倍，即濾波發(fā)散。由(10)式可以推導得到濾波異常判據(jù)的表達式如下：

vT(k)v(k)>δ[H(k)P(k,k-1)HT(k)+R(k)].

(11)

由于(11)式中P(k,k-1)包含轉速補償信息，此濾波異常判據(jù)體現(xiàn)了當前濾波過程中對新舊轉速信息的利用程度：δ=1為最嚴格濾波異常判據(jù)，本文仿真驗證選取范圍為3≤δ≤5. 若(11)式成立，則認為實際余項與原假設不相容，此時通過(8)式計算R(k)以代替R(k-1)；反之，若(11)式不成立，則省去(8)式的計算、繼續(xù)使用R(k-1)。

3)重復步驟1、步驟2，完成改進型SHAKF迭代算法。

3 仿真驗證

3.1 仿真條件

下面利用雙旋迫擊炮彈模型對改進型SHAKF滾轉角測量算法進行仿真驗證。根據(jù)文獻[8]仿真條件可知：射角采用最大射程角，彈丸初速為名義初速，在炮兵標準氣象條件下彈丸全彈道保持穩(wěn)定飛行。迫擊炮彈尾翼產生的導轉力矩和動壓變化使得彈丸轉速呈倒U型變化趨勢且保持低速右旋(由彈尾向彈頭方向看)；修正引信頭部安裝了與彈丸尾翼反向的導轉翼面，在反向導轉力矩作用下在全彈道內與彈丸旋轉方向相反且轉速較高，在修正階段轉速在15～25 r/s之間波動。全彈道范圍彈丸和修正引信無控飛行的轉速變化曲線如圖2所示。

表1給出了應用于仿真驗證的各主要測量參數(shù)。

3.2 對比驗證

在3.1節(jié)所述仿真條件下，分別采用EKF滾轉角測量算法與本文提出的改進型SHAKF測量算法進行比較，得到全彈道滾轉角測量誤差曲線如圖3所示。

表1 測量模型參數(shù)

從圖3可見：采用改進型SHAKF算法的收斂速度雖然與原EKF測量算法并無太大差別，但是其滾轉角測量誤差卻顯著降低：對濾波收斂后的滾轉角測量誤差進行統(tǒng)計，傳統(tǒng)EKF算法的滾轉角測量誤差均值為2.86°，標準差為1.67°；而采用SHAKF算法的測量誤差均值為0.26°，標準差為0.35°.

圖4所示為滾轉角測量誤差對比的局部放大圖。由圖4可見，除去彈道15～40 s區(qū)間外，采用新算法的滾轉角測量誤差在升弧段和降弧段都遠低于原EKF算法的結果，但都存在一個明顯的趨勢項。究其原因，是由于其各自所對應的轉速與彈道中段相比較高，單位時間內修正引信滾轉角增量也過大，同時其間的彈丸章動也帶來更大的量測誤差，這些都會導致預測協(xié)方差和濾波器的增益不能隨新息同步改變，從而失去對突變狀態(tài)變量的跟蹤能力，使估計誤差略有增加。以圖2為例，修正引信的轉速在彈道升弧段和降弧段都超過了18 r/s，這意味著僅在一個濾波步長周期內預估狀態(tài)變量的增量就達到6.5°. 在仿真層面，通過提高SHAKF算法的濾波頻率，或者減小濾波判據(jù)因子δ以提高濾波條件，二者都可以實現(xiàn)更高的滾轉角測量精度，但不可避免地會降低濾波實時性并在硬件實現(xiàn)過程中大大增加微處理器的運算處理負擔。對比采用原EKF算法的滾轉角測量誤差，改進型SHAKF算法對修正引信滾轉角的估計效果是令人滿意的，考慮到目前的解算誤差滿足修正系統(tǒng)精度要求，可暫不改變仿真步長。

4 實驗驗證

下面以高精度微機電系統(tǒng)傳感器三軸轉臺作為硬件實驗驗證平臺，驗證改進型SHAKF滾轉角測量算法。三軸轉臺如圖5所示，滾轉角測量硬件電路如圖6所示。

為了模擬驗證外彈道環(huán)境下修正引信的滾轉角測量精度，通過設置三軸轉臺的俯仰和方位控制單元來模擬彈丸的俯仰、偏航姿態(tài)，設置滾轉控制單元，將滾轉速率由30 r/s均勻減小至1 r/s，以模擬彈丸的時變轉速。固連在滾轉軸上的滾轉角測量硬件模塊會采集并存儲陀螺輸出信號，最后將采用改進型SHAKF滾轉角測量算法計算出的實時滾轉角與理論值進行對比，并分析解算誤差。三軸轉臺滾轉角速率輸出以及對應角速率條件下的滾轉角解算誤差曲線如圖7所示。

由圖7中可以看到：在修正引信轉速自30 r/s至1 r/s范圍的動態(tài)變化過程中，除了濾波迭代初期的震蕩之外，僅有的滾轉角解算失準皆出現(xiàn)在轉速狀態(tài)突變所對應的時刻(其收斂過程都不會超過200 ms)；改進型SHAKF滾轉角測量算法始終能夠保持較高的測量精度，且解算絕對誤差不超過4.2°，由此可見該算法完全能夠實現(xiàn)時變轉速條件下的滾轉角準確測量，并具有比傳統(tǒng)EKF算法更高的測量精度和自適應性。

5 結論

本文針對傳統(tǒng)EKF測量算法在彈道末段無法消除徑向安裝陀螺測量噪聲導致滾轉角測量誤差偏大問題，利用軸向陀螺的實測轉速作為系統(tǒng)噪聲補償，提出了一種基于雙旋迫擊炮彈平臺的改進型SHAKF滾轉角測量算法。所得主要結論如下：

1)通過與傳統(tǒng)EKF算法進行仿真比對驗證，結果表明：在相同仿真條件下，采用新算法的滾轉角測量誤差顯著降低，同時其濾波器的自適應性對于彈道升弧段和降弧段滾轉角新息改變的跟蹤能力也遠遠優(yōu)于EKF算法，滾轉角測量誤差均值為0.26°，標準差為0.35°.

2)以高精度三軸轉臺作為硬件實驗驗證平臺，模擬驗證外彈道環(huán)境下修正引信的滾轉角測量精度，結果表明：改進型SHAKF滾轉角測量算法始終能夠保持較高的測量精度，且解算絕對誤差不超過4.2°.

參考文獻(References)

[1] Widmayer R S. Terminal trajectory correction capability for standard spinning projectiles[C]∥2nd International Symposium on Ballistics. Daytona, FL，US: International Ballistics Committee, 1976.

[2] Philippe W. Stability analysis for canard guided dual-spin stabilized projectiles[C]∥AIAA Atmospheric Flight Mechanics Conference and Exhibit. Chicago,IL, US: AIAA, 2009.

[3] Philippe W, Friedrich L, Denis B,et al. Wind tunnel tests and open-loop trajectory simulations for a 155 mm canards guided spin stabilized projectile[C]∥AIAA Atmospheric Flight Mechanics Conference and Exhibit. Honolulu, HI, US: AIAA, 2008: 1-17.

[4] Lusia D J. Estimation of the local vertical state for a guided munition shell with an embedded GPS/micro-mechanical inertial navigation system[D]. Cambridge, MA, US: MIT, 1995: 33-51.

[5] Park H Y, Kim K J, Lee J G, et al. Roll angle estimation for smart munition[C]∥Proceedings of IFAC Symposium on Automatic Control in Aerospace. Toulouse, France: Pleiades Publishing，2007: 6-12.

[6] Lee H S, Kim K J, Park H Y. Roll estimation of a smart munition using a magnetometer based on an unscented Kalman filter[C]∥AIAA Guidance Navigation and Control Conference and Exhibit. Honolulu, HI, US: AIAA, 2008.

[7] Maley J M. Efficient attitude estimation for a spin-stabilized projectile[J]. Journal of Guidance, Control, and Dynamics, 2016, 39(2): 339-350.

[8] 王佳偉，史凱，徐國泰，等. 基于擴展卡爾曼濾波的高轉速修正引信滾轉角測量方法[J]. 西北工業(yè)大學學報，2016，34(6): 938-944.

WANG Jia-wei, SHI Kai, XU Guo-tai, et al. Roll estimation of high rotation speed correction fuze based on extended Kalman filter[J]. Journal of Northwestern Polytechnical University, 2016, 34(6): 938-944.(in Chinese)

[9] Sage A P, Husa G W. Adaptive filtering with unknown prior statistics[C]∥Proceedings of the 10th Joint Automatic Control Conference. Boulder, CO, US: Elsevier，1969: 760-769.

[10] 張常云. 自適應濾波方法研究[J]. 航空學報，1998, 19(7): 96-99.

ZHANG Chang-yun. Approach to adaptive filtering algorithm[J]. Acta Aeronautica et Astronautica Sinica, 1998, 19(7): 96-99. (in Chinese)

[11] 韓子鵬，趙子華，劉世平，等. 彈箭外彈道學[M]. 北京: 北京理工大學出版社，2008: 127-149.

HAN Zi-peng, ZHAO Zi-hua, LIU Shi-ping, et al. Exterior ballistics of projectiles and rockets[M]. Beijing: Beijing Institute of Technology Press, 2008: 127-149. (in Chinese)

[12] Narasimhappa M, Rangababu P, Sabat S L, et al. A modified Sage-Husa adaptive Kalman filter for denoising fiber optic gyroscope signal[C]∥Proceedings of 2012 Annual IEEE. Kochi, India: IEEE Computer Society，2012: 1266-1271.