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

(1.西北工業(yè)大學(xué) 航天學(xué)院, 陜西 西安 710072; 2.西安機(jī)電信息技術(shù)研究所 機(jī)電動(dòng)態(tài)控制重點(diǎn)實(shí)驗(yàn)室, 陜西 西安 710065)

0 引言

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

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

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

1 基本原理

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

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

1.2 簡化SHAKF算法

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

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)轉(zhuǎn)移矩陣；Γ(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 改進(jìn)型SHAKF算法

改進(jìn)型SHAKF算法具體步驟如下：

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

(9)

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

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

(10)

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

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

(11)

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

3)重復(fù)步驟1、步驟2，完成改進(jìn)型SHAKF迭代算法。

3 仿真驗(yàn)證

3.1 仿真條件

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

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

3.2 對(duì)比驗(yàn)證

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

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

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

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

4 實(shí)驗(yàn)驗(yàn)證

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

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

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

5 結(jié)論

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

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

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

參考文獻(xiàn)(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] 王佳偉，史凱，徐國泰，等. 基于擴(kuò)展卡爾曼濾波的高轉(zhuǎn)速修正引信滾轉(zhuǎn)角測量方法[J]. 西北工業(yè)大學(xué)學(xué)報(bào)，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] 張常云. 自適應(yīng)濾波方法研究[J]. 航空學(xué)報(bào)，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] 韓子鵬，趙子華，劉世平，等. 彈箭外彈道學(xué)[M]. 北京: 北京理工大學(xué)出版社，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.