基于地磁傳感器測量的滾轉彈藥角速度估計*

馬云建，辛長范，賈意弦，王　堯，張　毅（中北大學機電工程學院，太原　030051）

基于地磁傳感器測量的滾轉彈藥角速度估計*

馬云建，辛長范，賈意弦，王堯，張毅
（中北大學機電工程學院，太原030051）

為了實時獲得滾轉彈藥的飛行角速度，提出了一種基于地磁組件的角速度測量方法。該方法采用地磁傳感器獲得大地磁場強度在彈體三軸的投影及其變化率，并結合剛體轉動運動模型，利用卡爾曼濾波器來獲得滾轉彈藥的角速度。文中所提出的算法不同于其他的角速率估計器，不使用彈體的姿態，使得該算法適用于滾轉彈藥。仿真結果表明：該算法能很好的估計出滾轉彈藥的角速度，誤差在允許的范圍內，有較高的應用價值。

滾轉彈藥；角速度估計；地磁傳感器；卡爾曼濾波器

0　引言

在現代戰爭中，對炮彈的打擊精度要求很高，為了實現精確打擊，角速率在彈體姿態控制系統的實現上是一個非常重要的信息，它一般用于反旋轉、速率阻尼和在姿態估計算法中的姿態傳播［1］。一般來講，這些信息由彈載速率陀螺儀來提供，即使是高精度的速率陀螺儀有時也是極不可靠的，而且價格非常貴，因此迫切需要研究一種新的估計彈體角速率的算法。在通常情況下，彈體初始姿態以及方向信息的獲取，可以通過太陽磁傳感器。但是當有日食時，地磁場的觀測值就顯得非常有價值了。并且地磁傳感器具有體積小、精度高、易于集成和價格便宜的特點。

文中主要介紹了一種快速的角速率估計器，用一系列的地磁觀測值作為它唯一的信源［2］。與以前介紹的方法相反，這里提出的估計器并不使用TAM觀測值噪聲的數值微分。因此，避免了測量噪聲的放大。與之相應的，考慮到有效測量噪聲是有色噪聲的事實，使用擴展的卡爾曼濾波方法，直接通過一個測量方程處理磁場觀測量。磁場矢量相對于較短的飛行距離時，可以忽略地磁場模型。因此，忽略了模型誤差。此外，這種算法不使用彈體姿態，使得這種算法非常適用于滾轉彈藥。

1　地磁傳感器組件測量角速度原理

以國際地磁與高空物理協會公布的國際地磁參考場（IGRF）作為全球地磁場的基準模型［3］。地磁場的大小和方向可用磁場強度HT表示，標準單位為特斯拉（T）。水平方向的強度H是HT在水平面上的投影；磁偏角D是H與正北方向的夾角，以磁場東偏為正；磁傾角Ic是HT與水平面的傾角，以向下為正。可以在北向、東向和向下3個方向上分解，3個分量分別為HX、HY、HZ。地磁分量模型［4］見圖1所示。

圖1中地磁場強度的3個分量HX、HY、HZ與磁偏角D和磁傾角Ic的關系為：

一般測量的地磁場強度是在導航坐標系上描述的，它在地面坐標系上的投影為：

即：

滾轉彈藥角速度的估計系統主要由三軸正交地磁傳感器和信號轉換與處理系統構成。將三軸地磁傳感器沿彈體坐標系裝在彈上，地磁傳感器用來感知彈體飛行過程中在彈體坐標系上的地磁場強度的分量及其變化率［5］，通過信號處理和角速度估計算法解算出彈體的角速度。

彈藥發射前必須根據IGRF計算出當地的地磁場強度，地磁組件事先通過發射后結合慣性飛行段裝定的彈道參數對地磁進行標定和對準［6］。在飛行過程中，三軸地磁傳感器實時敏感地磁強度在彈體坐標系上的分量，通過角速度估計算法實時輸出角速度信息，對制導彈藥的姿態進行實時調整和修正。

2　滾轉彈藥角速度數學模型

由向量在地面坐標系與彈體坐標系投影關系，可得地磁場強度在彈體上的投影為：

系統的觀測模型為：

式中：ω是彈體角速度矢量。

在較短的飛行距離上，地磁矢量ME是不變的，所以上式的左邊可以忽略，即=0，可以得出：

即：

式（2）形成了基本的濾波觀測式，它敘述了彈體參考系下彈體角速度相對于地磁場的時間導數，它與彈體角速度及彈體坐標系下地磁矢量MB有關。

根據文獻［7］可知彈箭相對于地面坐標系的姿態運動學方程為：

由于自旋滾轉彈藥的角速度ωx1比ωy1和ωz1要大很多，所以可以忽略ωy1和ωz1，即 ˙γ=ωx1。而彈體的滾轉角速度可以利用單軸陀螺測得，那么根據式（3）可知，只要測量獲得彈體上的地磁分量就可以得出橫向轉動角速度ωy1和ωz1。

3　卡爾曼濾波器設計

文中所設計的卡爾曼濾波器為一個自帶初始化條件的擴展卡爾曼濾波器。濾波器的狀態矢量x為彈體角速率在彈體系下的3個分量。

假定彈體在不受外部力矩的情況下，處于旋轉運動的狀態，其動力學方程可由下列歐拉方程來表示。

式中J是轉動慣量矩陣。此方程為一個非線性的微分方程，可寫成下列形式：

動力學模型的狀態傳播方程采用標準的四階龍格-庫塔數值積分算法來計算，由tk時刻的角速度可以計算出tk+1時刻的角速度，積分步長取0.001 s。對于誤差協方差矩陣Pk的計算采用線性化的動力學模型來近似表示為：

式中：I是一個3×3的單位矩陣；Δt是采樣間隔；通過計算得到Jacobian矩陣Fk為：

下標k與時間常數tk有關。

3.1測量模型

其中測量噪聲vk服從下列正態分布：

其方差RTAM為已知量。通過連續兩次對測量值進行差分即可得到基于式（2）的濾波觀測方程為：

其中：

式（10）是時變的觀測矩陣。

式（11）即為有效的測量矢量，nk是有效的觀測噪聲，且：

由于通過上述差分運算，使得服從正態分布的測量噪聲變為有色噪聲，在濾波時需要對有色噪聲進行適當處理。

3.2有色噪聲模型

由式（12）可知，有效測量噪聲nk的自相關函數在一個積分步長之后即變為零，所以，其可用一階馬爾柯夫模型來表示。

式中：α與關聯時間有關；η為服從正態分布的白噪聲。

由式（13）可得到數學期望為：

定義自相關矩陣為：

運用式（8）和式（12）可得：

由式（15）可得：

即：

由式（17）和式（19）可得：

3.3狀態增加和實現

通過將原有的狀態矢量與有效測量噪聲合并后得到增廣狀態矢量：

擴展后的狀態轉移矩陣為：

同時觀測矩陣為：

相應的處理噪聲協方差矩陣為：

其中Qk值可通過試算試驗進行調整，直到滿足要求為止。

通過上述方程可實現對有色噪聲的處理，但也帶來了另一個問題，即測量模型具有奇異性。通過在測量模型上再增加一個微小的噪聲，可解決此奇異性問題，而模型的結構仍基本保持不變。

4　數值仿真與分析

為了評估上述濾波器的特性，對300次蒙特卡洛仿真下的統計結果進行分析。仿真初始條件為，自轉速率ωx為7 r/s，并假定彈體的轉動慣量是精確的，沒有誤差，傳感器也進行了良好校準，沒有偏置及非正交測量誤差，典型的仿真曲線如圖2～圖4所示。

圖2　角速度分量ωx的性能（實線為真實值，虛線為估計值）

圖3　角速度分量ωy的性能（實線為真實值，虛線為估計值）

圖4　角速度分量ωz的性能（實線為真實值，虛線為估計值）

表1　蒙特卡洛研究的結果

通過300次蒙特卡洛仿真得到角速度的1-σ誤差小于0.5°/s，下表為統計后的各軸角速度的均值誤差及1-σ誤差。

5　結論

文中建立了通過三軸地磁傳感器測量數據計算彈體角速率的卡爾曼濾波算法，此方法除了彈體轉動慣量外，不需任何原始數據。通過仿真表明，當自轉速率為7 r/s時，角速度估計誤差小于0.5°/s，可以滿足有控火箭彈的測量要求。

［1］ LEFFERTS E J，MARKLY F L，SHUSTER M D.Kalman filtering for spacecraft attitude estimation［J］.Journal of Guidance，Control，and Dynamics，1982，5（5）：417 -429.

［2］STEYN W H.A multi-mode attitude determination and control system for small SATELLITES-pHd dissertation. South Africa.Univ.of Stellenbosch［R］.1995.

［3］彭富清.地磁模型與地磁導航［J］.海洋測繪，2006，26（2）：73-75.

［4］韓艷.制導炮彈飛行姿態的陀螺-磁阻傳感器組合測量方法研究［D］.南京：南京理工大學，2010.

［5］HARKINS T E，WILSON M J.Measuring in-flight angular motion with a low-cost magnetometer：ARL-TR-4244［R］. Aberaeen：Army Research Lab.，2007.

［6］茍秋雄，劉明喜，李虎軍.基于磁阻傳感器的末制導迫擊炮滾轉姿態初始對準技術研究［J］.彈箭與制導學報，2008，28（3）：45-48.

［7］ 孟秀云.導彈制導與控制系統原理［M］.北京：北京理工大學出版社，2003：24-28.

Angular Velocity Estimation of Rolling-ammunition Based on Magnetometer

MA Yunjian，XIN Changfan，JIA Yixian，WANG Yao，ZHANG Yi
（School of Mechatronics Engineering，North University of China，Taiyuan 030051，China）

In order to obtain real-time flight ammunition’s roll angular velocity，angular velocity measurement method was proposed based on geomagnetic components.The method uses a geomagnetic sensor to obtain its projected rate of change in the earth’s magnetic field strength tri-axial projectile，combined with rotational movement of rigid model，using the extended Kalman filter to get the roll angular velocity of ammunition.The proposed algorithm is different from other angular rate estimators，it does not use missile attitude，makes the algorithm applicable to rolling ammunition.The simulation results show that：the algorithm can efficiently estimate angular velocity of rolling ammunition，the error is in the allowable range，it has higher application value.

roll ammunition；angular velocity estimation；geomagnetic sensor；Kalman filter

TJ765

A

10.15892/j.cnki.djzdxb.2016.01.018

2015-02-01

馬云建（1989-），男，安徽渦陽人，碩士研究生，研究方向：基于DSP的控制系統設計。