基于地磁傳感器測量的滾轉(zhuǎn)彈藥角速度估計(jì)*

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

基于地磁傳感器測量的滾轉(zhuǎn)彈藥角速度估計(jì)*

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

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

滾轉(zhuǎn)彈藥；角速度估計(jì)；地磁傳感器；卡爾曼濾波器

0　引言

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

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

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

以國際地磁與高空物理協(xié)會(huì)公布的國際地磁參考場（IGRF）作為全球地磁場的基準(zhǔn)模型［3］。地磁場的大小和方向可用磁場強(qiáng)度HT表示，標(biāo)準(zhǔn)單位為特斯拉（T）。水平方向的強(qiáng)度H是HT在水平面上的投影；磁偏角D是H與正北方向的夾角，以磁場東偏為正；磁傾角Ic是HT與水平面的傾角，以向下為正?？梢栽诒毕颉|向和向下3個(gè)方向上分解，3個(gè)分量分別為HX、HY、HZ。地磁分量模型［4］見圖1所示。

圖1中地磁場強(qiáng)度的3個(gè)分量HX、HY、HZ與磁偏角D和磁傾角Ic的關(guān)系為：

一般測量的地磁場強(qiáng)度是在導(dǎo)航坐標(biāo)系上描述的，它在地面坐標(biāo)系上的投影為：

即：

滾轉(zhuǎn)彈藥角速度的估計(jì)系統(tǒng)主要由三軸正交地磁傳感器和信號(hào)轉(zhuǎn)換與處理系統(tǒng)構(gòu)成。將三軸地磁傳感器沿彈體坐標(biāo)系裝在彈上，地磁傳感器用來感知彈體飛行過程中在彈體坐標(biāo)系上的地磁場強(qiáng)度的分量及其變化率［5］，通過信號(hào)處理和角速度估計(jì)算法解算出彈體的角速度。

彈藥發(fā)射前必須根據(jù)IGRF計(jì)算出當(dāng)?shù)氐牡卮艌鰪?qiáng)度，地磁組件事先通過發(fā)射后結(jié)合慣性飛行段裝定的彈道參數(shù)對(duì)地磁進(jìn)行標(biāo)定和對(duì)準(zhǔn)［6］。在飛行過程中，三軸地磁傳感器實(shí)時(shí)敏感地磁強(qiáng)度在彈體坐標(biāo)系上的分量，通過角速度估計(jì)算法實(shí)時(shí)輸出角速度信息，對(duì)制導(dǎo)彈藥的姿態(tài)進(jìn)行實(shí)時(shí)調(diào)整和修正。

2　滾轉(zhuǎn)彈藥角速度數(shù)學(xué)模型

由向量在地面坐標(biāo)系與彈體坐標(biāo)系投影關(guān)系，可得地磁場強(qiáng)度在彈體上的投影為：

系統(tǒng)的觀測模型為：

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

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

即：

式（2）形成了基本的濾波觀測式，它敘述了彈體參考系下彈體角速度相對(duì)于地磁場的時(shí)間導(dǎo)數(shù)，它與彈體角速度及彈體坐標(biāo)系下地磁矢量MB有關(guān)。

根據(jù)文獻(xiàn)［7］可知彈箭相對(duì)于地面坐標(biāo)系的姿態(tài)運(yùn)動(dòng)學(xué)方程為：

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

3　卡爾曼濾波器設(shè)計(jì)

文中所設(shè)計(jì)的卡爾曼濾波器為一個(gè)自帶初始化條件的擴(kuò)展卡爾曼濾波器。濾波器的狀態(tài)矢量x為彈體角速率在彈體系下的3個(gè)分量。

假定彈體在不受外部力矩的情況下，處于旋轉(zhuǎn)運(yùn)動(dòng)的狀態(tài)，其動(dòng)力學(xué)方程可由下列歐拉方程來表示。

式中J是轉(zhuǎn)動(dòng)慣量矩陣。此方程為一個(gè)非線性的微分方程，可寫成下列形式：

動(dòng)力學(xué)模型的狀態(tài)傳播方程采用標(biāo)準(zhǔn)的四階龍格-庫塔數(shù)值積分算法來計(jì)算，由tk時(shí)刻的角速度可以計(jì)算出tk+1時(shí)刻的角速度，積分步長取0.001 s。對(duì)于誤差協(xié)方差矩陣Pk的計(jì)算采用線性化的動(dòng)力學(xué)模型來近似表示為：

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

下標(biāo)k與時(shí)間常數(shù)tk有關(guān)。

3.1測量模型

其中測量噪聲vk服從下列正態(tài)分布：

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

其中：

式（10）是時(shí)變的觀測矩陣。

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

由于通過上述差分運(yùn)算，使得服從正態(tài)分布的測量噪聲變?yōu)橛猩肼?，在濾波時(shí)需要對(duì)有色噪聲進(jìn)行適當(dāng)處理。

3.2有色噪聲模型

由式（12）可知，有效測量噪聲nk的自相關(guān)函數(shù)在一個(gè)積分步長之后即變?yōu)榱悖?，其可用一階馬爾柯夫模型來表示。

式中：α與關(guān)聯(lián)時(shí)間有關(guān)；η為服從正態(tài)分布的白噪聲。

由式（13）可得到數(shù)學(xué)期望為：

定義自相關(guān)矩陣為：

運(yùn)用式（8）和式（12）可得：

由式（15）可得：

即：

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

3.3狀態(tài)增加和實(shí)現(xiàn)

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

擴(kuò)展后的狀態(tài)轉(zhuǎn)移矩陣為：

同時(shí)觀測矩陣為：

相應(yīng)的處理噪聲協(xié)方差矩陣為：

其中Qk值可通過試算試驗(yàn)進(jìn)行調(diào)整，直到滿足要求為止。

通過上述方程可實(shí)現(xiàn)對(duì)有色噪聲的處理，但也帶來了另一個(gè)問題，即測量模型具有奇異性。通過在測量模型上再增加一個(gè)微小的噪聲，可解決此奇異性問題，而模型的結(jié)構(gòu)仍基本保持不變。

4　數(shù)值仿真與分析

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

圖2　角速度分量ωx的性能（實(shí)線為真實(shí)值，虛線為估計(jì)值）

圖3　角速度分量ωy的性能（實(shí)線為真實(shí)值，虛線為估計(jì)值）

圖4　角速度分量ωz的性能（實(shí)線為真實(shí)值，虛線為估計(jì)值）

表1　蒙特卡洛研究的結(jié)果

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

5　結(jié)論

文中建立了通過三軸地磁傳感器測量數(shù)據(jù)計(jì)算彈體角速率的卡爾曼濾波算法，此方法除了彈體轉(zhuǎn)動(dòng)慣量外，不需任何原始數(shù)據(jù)。通過仿真表明，當(dāng)自轉(zhuǎn)速率為7 r/s時(shí)，角速度估計(jì)誤差小于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］彭富清.地磁模型與地磁導(dǎo)航［J］.海洋測繪，2006，26（2）：73-75.

［4］韓艷.制導(dǎo)炮彈飛行姿態(tài)的陀螺-磁阻傳感器組合測量方法研究［D］.南京：南京理工大學(xué)，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］茍秋雄，劉明喜，李虎軍.基于磁阻傳感器的末制導(dǎo)迫擊炮滾轉(zhuǎn)姿態(tài)初始對(duì)準(zhǔn)技術(shù)研究［J］.彈箭與制導(dǎo)學(xué)報(bào)，2008，28（3）：45-48.

［7］ 孟秀云.導(dǎo)彈制導(dǎo)與控制系統(tǒng)原理［M］.北京：北京理工大學(xué)出版社，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的控制系統(tǒng)設(shè)計(jì)。