基于單軸磁傳感器的旋轉(zhuǎn)彈姿態(tài)解算算法

嚴 浩，袁 磊

(1.北京理工大學(xué)機電學(xué)院，北京 100081；2.北京特種車輛研究所，北京 100072)

0 引言

在彈體設(shè)計以及彈體空氣動力學(xué)研究中，彈體的飛行姿態(tài)參數(shù)測量是必不可少的，準確地對彈體飛行姿態(tài)進行測試對推動彈體的設(shè)計進程有重大意義。使用陀螺儀進行高速旋轉(zhuǎn)彈體姿態(tài)測量，測量結(jié)果誤差大、成本高。磁傳感器具有成本低、體積小、抗過載高、靈敏度高的優(yōu)點，多用于航向估計，使用磁傳感器對旋轉(zhuǎn)彈體測姿有研究價值。采用機載磁傳感器精確測量旋轉(zhuǎn)體的姿態(tài)一直被認為是一項艱巨的任務(wù)[1]。文獻[2]建立了數(shù)學(xué)模型，提出了利用地磁信息求解地磁偏角的方法，但是數(shù)學(xué)模型應(yīng)用范圍受限較大，且沒有進行誤差分析。文獻[3]在Harkins等人研究基礎(chǔ)上，提出了磁傳感器選擇、安裝與調(diào)試原則，但是并未進行展開研究。文獻[4]推導(dǎo)了磁傳感器輸出值的特征比值與彈體俯仰角的數(shù)學(xué)關(guān)系，研究了姿態(tài)角的解算與修正算法，沒有進行實際實驗驗證。文獻[5]從旋轉(zhuǎn)體轉(zhuǎn)速和地磁數(shù)據(jù)采樣率兩個方面進行了零點交叉法測量磁方位角的誤差分析。文獻[6]著重分析地磁場和傳感器安裝位置的空間關(guān)系，進行炮彈姿態(tài)角和磁方位角的推導(dǎo)和解算，解算誤差為±0.4°。文獻[7]采用非正交地磁傳感器組合測量并計算得到地磁方位角和滾轉(zhuǎn)角信息，解算誤差為±0.2°。文獻[8]提出了采用地磁傳感器和光電傳感器相結(jié)合的方法解算彈體的滾轉(zhuǎn)姿態(tài)信息，解算角度誤差在4°以內(nèi)。本文針對使用陀螺儀對高速旋轉(zhuǎn)彈體測姿誤差大，存在漂移的問題，提出基于單軸磁傳感器的旋轉(zhuǎn)彈姿態(tài)解算算法。

1 旋轉(zhuǎn)彈姿態(tài)解算原理

如圖1所示，O-NED為地理坐標系，其中M為地磁場矢量，H為地磁場水平分量，d為地磁方位角，I為地磁傾角。O-xbybzb為彈體坐標系，是與彈體固連的動坐標系。O-xbybzb的原點O為彈體質(zhì)心，Oxb為彈體對稱軸，指向彈體頭部；Oyb軸位于炮彈橫截面內(nèi)且垂直于Oxb軸；Ozb軸由Oxb軸和Oyb軸右手定則規(guī)定。彈體坐標系可以由地理坐標系經(jīng)平移旋轉(zhuǎn)變換而來，旋轉(zhuǎn)過程的三個旋轉(zhuǎn)角分別為偏航角ψ、俯仰角θ、滾轉(zhuǎn)角φ。

圖1 地理坐標系和彈體坐標系Fig.1 Geographical coordinate system and projectile coordinate system

由圖1所示坐標系O-NED和O-xbybzb的關(guān)系，彈體旋轉(zhuǎn)滾轉(zhuǎn)角φ為0時，設(shè)這時與O-xbybzb重合的坐標系為準彈體坐標系O-xByBzB，準彈體坐標系可以由地理坐標系平移后經(jīng)偏航角ψ和俯仰角θ旋轉(zhuǎn)而來。此時地磁場矢量M和準彈體坐標系O-xByBzB以及彈體坐標系O-xbybzb的空間關(guān)系如圖2所示。其中MxByB是M在xBOyB平面的投影，γ為M與xBOyB平面的夾角，α為投影MxByB與OxB軸的夾角。在彈體上安裝磁傳感器，在彈體旋轉(zhuǎn)初始狀態(tài)即φ為0時，傳感器敏感軸OS在xBOzB平面內(nèi)且與OxB軸的夾角為β，稱為傳感器安裝角。

圖2 彈體坐標系、準彈體坐標系和地磁場空間關(guān)系Fig.2 The spatial relationship of projectile body coordinate system, quasi-projectile coordinate system and geomagnetic field

在彈體飛行過程中，由于旋轉(zhuǎn)角速度遠大于偏航角和俯仰角變化率，因此假設(shè)在彈體飛行過程中，彈體旋轉(zhuǎn)一周，γ和α不變，則磁敏感軸OS上的地磁強度變化和傳感器安裝角β及滾轉(zhuǎn)角φ有關(guān)。在安裝角度β已經(jīng)固定的情況下，彈體旋轉(zhuǎn)一周的OS軸地磁強度隨φ變化。

由圖1、圖2所定義的坐標系和傳感器OS軸以及相互之間的位置關(guān)系，可以求得傳感器敏感軸上的地磁強度為：

MOS=MxBcosβ-MyBsinβsinφ+MzBsinβcosφ

(1)

式(1)中，MxB=Mcosγcosα，MyB=Mcosγsinα，MzB=Msinγ。

當(dāng)β=0°或180°，即OS與Oxb軸重合時，有MOS=±Mxb=±Mcosγcosα，故彈體旋轉(zhuǎn)一周，磁傳感器輸出的信號為一個大小和γ、α有關(guān)的常數(shù)，沒有零點。

當(dāng)β=90°，即OS⊥Oxb軸時，有：

MOS=-Mcosγsinαsinφ+Msinγcosφ

(2)

當(dāng)γ=0°或180°且α=0°或180°，即M與Oxb重合時，MOS=0，MOS不隨φ的變化而變化，彈體旋轉(zhuǎn)一周傳感器敏感軸上磁場強度皆為0；當(dāng)γ=0°或180°且α≠0°或180°時，MOS=±Msinαcosφ，彈體旋轉(zhuǎn)一周傳感器敏感軸上磁場強度隨φ呈正弦變化，幅值和α有關(guān)；當(dāng)γ≠0°或180°且α=0°或180°時，即M在xbOzb平面內(nèi)時，MOS=Msinγcosφ彈體旋轉(zhuǎn)一周傳感器敏感軸上磁場強度隨φ呈余弦變化，幅值和γ有關(guān)；當(dāng)γ≠0°或180°且α≠0°或180°時，有：

MOS=Asin(φ+φ0)

(3)

式(3)中，

彈體旋轉(zhuǎn)一周傳感器敏感軸上磁場強度隨φ呈幅值為A，初始相位角為φ0的正弦變化。故當(dāng)磁傳感器安裝角度與彈軸成90°且當(dāng)?shù)卮艌龇较虿慌c彈軸方向重合時，彈體旋轉(zhuǎn)一周，磁傳感器輸出的信號總有零點，但是零點的相位差不變且為90°。

當(dāng)β≠90°，0°或180°時，有：

MOS=A0+Asin(φ+φ0)

(4)

式(4)中，

令Mos=0，可以解得：

(5)

式(5)在一個2π周期內(nèi)有兩解的必要條件為：

(6)

故當(dāng)傳感器安裝角度不與彈軸垂直或重合，且滿足式(6)要求時，傳感器輸出信號在一個弧度為2π的旋轉(zhuǎn)周期內(nèi)存在兩個零點φs1、φs2，這時相位差為：

Δφs=φs2-φs1

(7)

從式(5)及式(7)可以看出，Δφs與γ、α、β有關(guān)，即存在函數(shù)關(guān)系f：

Δφs=f(γ,α,β)

(8)

式(8)數(shù)值解可由式(5)、(7)解出，f為用數(shù)值解擬合的函數(shù)關(guān)系。如果已知γ、α、β其中的兩個量，由Δφs可以求出第三個量。

實際上，γ和α是未知的。對于彈體姿態(tài)常用歐拉角描述，即前文提到的偏航角ψ、俯仰角θ、滾轉(zhuǎn)角φ。在當(dāng)?shù)氐卮怒h(huán)境參數(shù)M、D、I已知的情況下，φ為0°時彈體三軸上的磁強強度為：

將上式代入式(1)可以得到與前述相似的結(jié)論。即對于高速旋轉(zhuǎn)彈，解出φs′：

(9)

式(9)中，

令：

(10)

則在滿足|H|<1以及M、D、I不變的情況下，存在Δφs′：

Δφs′=φs2′-φs1′

(11)

從式(11)和式(9)可以看出，Δφs′與ψ、θ、β有關(guān)，即存在函數(shù)關(guān)系h：

Δφs′=h(ψ,θ,β)

(12)

若滿足|H|<1，則θ和Δφs′、ψ、β之間存在唯一函數(shù)關(guān)系g：

θ=g(Δφs′,ψ,β)

(13)

將式(13)中的ψ、β視作常數(shù)，則可以通過式(12)解出數(shù)值解Δφs′，利用Δφs′和求解數(shù)值解Δφs′過程中使用的θ數(shù)據(jù)，擬合出函數(shù)g。

2 旋轉(zhuǎn)彈姿態(tài)解算算法

2.1 假設(shè)條件

目前國內(nèi)外主流火炮多采用線膛炮管設(shè)計，打出的炮彈都是旋轉(zhuǎn)彈[9]。旋轉(zhuǎn)彈的旋轉(zhuǎn)速度較高，且在旋轉(zhuǎn)一周的過程中，彈體滾轉(zhuǎn)角的變化遠遠大于偏航角和俯仰角的變化。火炮射擊射程較近，例如制式155 mm榴彈射程在60 km以內(nèi)，可以忽略在該射程內(nèi)地磁場強度矢量的變化[10]。

基于以上情況，提出以下假設(shè)條件：整個彈道過程中，偏航角ψ保持不變；彈體旋轉(zhuǎn)一周過程中，旋轉(zhuǎn)角速度是個常數(shù)且俯仰角不變；整個彈道過程中，地磁場強度矢量不變。

2.2 姿態(tài)角和地磁零點相位差Δφs擬合

基于地磁信號零點相位差計算姿態(tài)的原理，根據(jù)當(dāng)?shù)氐卮怒h(huán)境參數(shù)，由火炮發(fā)射初始諸元，計算磁傳感器在不同安裝角度下旋轉(zhuǎn)彈姿態(tài)角隨地磁信號零點相位差的關(guān)系。

某地地磁環(huán)境參數(shù)D=-10.64°，I=65.9°，M=56 868.1 nT，由試驗場炮位已知發(fā)射方位角可以確定ψ=148.62°。根據(jù)前述章節(jié)，可以求出：不同安裝角度β，初始相位角φ0′和俯仰角θ的關(guān)系如圖3所示；H和俯仰角θ的關(guān)系如圖4所示；Δφs和俯仰角θ的關(guān)系如圖5所示。

圖3 不同安裝角度β下φ0′隨θ變化曲線Fig.3 Variation curve of φ0 with θ at different installation angle β

圖4 不同安裝角度β下H隨θ變化曲線Fig.4 Variation curve of E with θ at different installation angle β

圖5 不同安裝角度β下Δφs隨θ變化曲線Fig.5 Variation curve of Δφs with θ at different installation angle β

由圖3可知，不同安裝角度，旋轉(zhuǎn)彈上磁傳感器輸出的地磁信號初始相位隨俯仰角θ變化曲線重合，地磁信號初始相位和安裝角度β無關(guān)。

從圖4、圖5可以看出，Δφs和俯仰角θ的關(guān)系在一定區(qū)域內(nèi)是單調(diào)變化的，也就是說，利用零點相位差Δφs求解彈體姿態(tài)存在盲區(qū)。明顯看出圖4中H值落在(-1,1)之間時，圖5中Δφs和俯仰角θ有確定的關(guān)系，此時對應(yīng)的θ范圍為姿態(tài)可求解區(qū)域。

圖6所示是β=60°時θ隨Δφs變化曲線。從圖6可知，只要知道旋轉(zhuǎn)彈彈體在旋轉(zhuǎn)一周內(nèi)的地磁信號的兩個零點之間的相位差Δφs，就能求得在這個旋轉(zhuǎn)周期內(nèi)的姿態(tài)角θ。

圖6 β=60°時θ隨Δφs變化曲線Fig.6 Variation curve of θ with Δφs at β=60°

2.3 安裝角度β修正

實際工程中，地磁信號測試裝置中的地磁傳感器理論上應(yīng)與彈軸成β角安裝，實際安裝的角度為β′，如果不進行安裝角度修正，還以β角進行計算，則會形成誤差。假設(shè)彈體的俯仰角θ=15°，理論安裝角β=60°，未進行安裝角β修正的情況下，真實安裝角β′和俯仰角θ解算誤差的變化關(guān)系曲線如圖7所示。

圖7 θ=15°，β=60°時β′隨θ解算誤差變化曲線Fig.7 Variation curve of β′ with the solution error of θ at θ=15°, β=60°

由圖7可知，真實安裝角β′和俯仰角θ解算誤差變化關(guān)系是有規(guī)律的，當(dāng)真實角度在60°±10°變化時θ解算誤差在(-12°,11°)之間變化。安裝角度誤差的存在，將大大增加俯仰角θ的解算誤差，所以需要對傳感器安裝角度進行修正。

安裝角度的修正，是通過轉(zhuǎn)臺試驗實現(xiàn)，轉(zhuǎn)臺的俯仰角是某一固定值，通過提取在轉(zhuǎn)臺上勻速轉(zhuǎn)動一周的地磁信號，利用地磁信號零點信息求解俯仰角與轉(zhuǎn)臺俯仰角之間的誤差，進一步根據(jù)真實安裝角β′和俯仰角θ解算誤差變化關(guān)系確定真實傳感器真實安裝角β′。

2.4 地磁信號處理

地磁信號經(jīng)過信號去噪處理后，其零點提取流程如圖8所示。

圖8 地磁信號零點提取流程圖Fig.8 Flow chart of zero point extraction of geomagnetic signal

T包含了彈體整個彈道過程中地磁數(shù)據(jù)的零點信息，提取出來的地磁信號時間零點T數(shù)據(jù)用于求解彈體每個旋轉(zhuǎn)周期內(nèi)的零點相位差Δφs，根據(jù)之前的假設(shè)，有：

(14)

式(14)中，N是T數(shù)據(jù)的長度，Δφs包含了彈體整個彈道過程中每個旋轉(zhuǎn)周期內(nèi)的地磁數(shù)據(jù)零點相位差信息。

2.5 姿態(tài)角解算

由前文假設(shè)的條件，彈體每個旋轉(zhuǎn)周期轉(zhuǎn)速一定，只要求得每個旋轉(zhuǎn)周期的角速度，就能求出滾轉(zhuǎn)角。每個旋轉(zhuǎn)周期的轉(zhuǎn)速為：

(15)

式(15)中，w包含了個旋轉(zhuǎn)周期的旋轉(zhuǎn)角速度信息。則滾轉(zhuǎn)角為：

(16)

式(16)中，φ(τ)為某τ時刻彈體的旋轉(zhuǎn)彈滾轉(zhuǎn)角度值，n為時間T[2i+1]內(nèi)彈體的旋轉(zhuǎn)周數(shù)。

求解俯仰角θ，由修正后的安裝角β′重新擬合姿態(tài)角和相位零點差Δφs的關(guān)系：

θ=g(Δφs,β′,ψ)

(17)

式(17)中，依據(jù)姿態(tài)解算假設(shè)條件，ψ=ψ0，是由已知條件發(fā)射方位角確定的。β′是修正后的安裝角。將由式(14)求出的Δφs數(shù)據(jù)代入式(17)，可以求出：

θ[m]=g(Δφs[m],β′,ψ0)

(18)

式(18)中，θ[m]為彈體第m個旋轉(zhuǎn)周期的俯仰角，Δφs[m]為彈體第m個旋轉(zhuǎn)周期的地磁信息零點相位差。

3 火炮射擊實驗驗證

為獲取真實彈道地磁環(huán)境數(shù)據(jù)，本文進行了火炮射擊實驗，將地磁傳感器沿彈軸方向和與彈軸成60°方向安裝，對真實彈道環(huán)境地磁數(shù)據(jù)進行存儲測試。火炮射擊實驗選取的研究對象為155 mm牽引式加榴炮殺傷爆破砂彈，發(fā)射藥使用4號裝藥。實驗過程中使用DR582彈道雷達跟蹤彈丸位置，如圖9所示，以獲得彈道傾角信息。

圖9 DR582雷達Fig.9 DR582 radar

實驗中，存在磁干擾和測量噪聲，磁干擾主要為鐵磁干擾，設(shè)計磁阻傳感器置復(fù)位電路和使用非鐵磁性材料取代鐵磁性材料減少鐵磁干擾；對地磁數(shù)據(jù)使用小波變換進行去噪處理減小測量誤差[11]。

存儲測試系統(tǒng)所選地磁傳感器為Honeywell公司HMC150x系列磁阻傳感器，該系列傳感器具有尺寸小、頻響高、精度高等優(yōu)點。傳感器安裝于榴彈引信結(jié)構(gòu)中，存儲測試裝置如圖9所示。

圖10 存儲測試裝置Fig.10 Storage test device

3.1 實驗初始參數(shù)

實驗場地主要地磁參數(shù)如表1所示。

表1 實驗場地地磁參數(shù)Tab.1 Geomagnetic parameters of test site

火炮射擊試驗主要參數(shù)如表2所示。

表2 火炮射擊實驗初始諸元Tab.2 Initial elements of artillery firing test

3.2 傳感器測試數(shù)據(jù)

經(jīng)過火炮射擊實驗，測得的彈軸方向和與彈軸成60°方向的地磁數(shù)據(jù)信息，經(jīng)過信號處理后分別如圖11和圖12所示。圖13為60°方向地磁數(shù)據(jù)局部放大圖。

圖11 彈軸方向地磁數(shù)據(jù)Fig.11 Geomagnetic data at projectile axis direction

圖12 60°方向地磁數(shù)據(jù)Fig.12 Geomagnetic data in 60° direction

圖13 60°方向地磁數(shù)據(jù)局部放大Fig.13 Partial enlargement of geomagnetic data in 60° direction

從圖11可知，實驗測得的彈軸方向地磁數(shù)據(jù)不完整，這是測試系統(tǒng)中彈軸方向磁傳感器信號調(diào)理電路設(shè)計不合理造成的。從圖12、圖13可知，60°軸地磁傳感器大概在20 ms開始有數(shù)據(jù)，說明存儲測試裝置在觸發(fā)后經(jīng)過20 ms左右飛出炮膛。60°軸地磁傳感器數(shù)據(jù)較為理想，和理論推導(dǎo)幾乎一致，該數(shù)據(jù)可以用來解算彈體姿態(tài)。

3.3 姿態(tài)解算結(jié)果

假設(shè)彈丸剛飛出瞬間滾轉(zhuǎn)角為0°，則彈體旋轉(zhuǎn)滾轉(zhuǎn)角解算結(jié)果如圖14所示。利用地磁數(shù)據(jù)解算的俯仰角值和實驗中使用雷達彈道數(shù)據(jù)計算的俯仰角作對比，如圖15、圖16所示，俯仰角θ解算誤差隨時間變化曲線如圖18所示。

圖14 滾轉(zhuǎn)角φ解算結(jié)果Fig.14 Rolling angle φ solution result

圖15 俯仰角θ雷達數(shù)據(jù)和解算值Fig.15 Radar data and calculated value of pitch angle θ

圖16 俯仰角θ雷達數(shù)據(jù)和解算值局部放大Fig.16 Partial enlargement of pitch angle θ radar data and solution value

從圖14滾轉(zhuǎn)角φ解算結(jié)果可以看出，4號裝藥的155 mm榴彈在飛行過程中的旋轉(zhuǎn)速度較高，旋轉(zhuǎn)周期最快約為5 ms。從圖15、圖16俯仰角的解算結(jié)果可以看出，俯仰角變化率遠遠小于滾轉(zhuǎn)角變化率，這也符合了前文提出的姿態(tài)解算假設(shè)條件。

從圖16可以看出，解算出的俯仰角在某條曲線附近擺動，擺動頻率由快慢兩種頻率組成。這是因為在彈體實際飛行過程中存在章動運動，其在彈道初始段表現(xiàn)最為明顯，章動運動是一種二圓運動，勢必會引起偏航角和俯仰角會有兩種頻率的微小擺動。為了和雷達數(shù)據(jù)比較，取俯仰角的解算值擺動中心，將俯仰角解算值擺動中心和雷達數(shù)據(jù)作比較，結(jié)果如圖17、圖18所示。

從圖17姿態(tài)解算中心值與雷達數(shù)據(jù)對比和圖18的姿態(tài)解算誤差可看出，由于彈丸的章動運動，解算的彈丸俯仰角的最大誤差出現(xiàn)在彈道的初始段，這個時候彈丸剛飛出炮膛，章動較大。通過本文提出的姿態(tài)解算算法解算出的彈道初始段俯仰角解算誤差變化在0.15°以內(nèi)，具備較高的解算精度。

圖17 俯仰角θ雷達數(shù)據(jù)和解算中心值局部放大Fig.17 Partial enlargement of pitch angle θ lradar data and solution center value

4 結(jié)論

本文提出基于單軸磁傳感器的旋轉(zhuǎn)彈姿態(tài)解算算法。該算法同傳統(tǒng)的地磁匹配算法相比，僅需一個磁傳感器的地磁數(shù)據(jù)，且可顯著降低地磁數(shù)據(jù)采集分辨率的要求，提升工程適用性。算法通過對磁傳感器安裝角的修正，提高了姿態(tài)解算精度。炮射試驗驗證結(jié)果表明，解算誤差最大不超過0.15°，姿態(tài)解算誤差較小，精度有所提高，具有一定的工程應(yīng)用價值。