基于地磁方位角的旋轉(zhuǎn)彈丸姿態(tài)解算方法

安亮亮，王良明，趙丹丹

（1.南京理工大學(xué) 能源與動力工程學(xué)院，南京210094；2.遼沈工業(yè)集團(tuán)有限公司，沈陽 110045）

地球磁場有著豐富的參數(shù)信息，利用地磁場來實現(xiàn)彈體的姿態(tài)測量具有全天候、能耗低、抗沖擊能力強(qiáng)、無累積誤差、可靠性高等優(yōu)良特征[1]。近年來，隨著集成電路技術(shù)的成熟以及反衛(wèi)星武器的不斷發(fā)展，地磁探測技術(shù)不斷受到重視，廣泛應(yīng)用于航空航天、軍事等領(lǐng)域[2-3]。

采用地磁傳感器組合測量彈丸的飛行姿態(tài)參數(shù)，對于分析彈箭動力學(xué)、為導(dǎo)航制導(dǎo)系統(tǒng)提供支持等都有重要意義。常用的地磁傳感器測姿方法大多依賴于坐標(biāo)系轉(zhuǎn)移矩陣，即載體坐標(biāo)系與導(dǎo)航坐標(biāo)系的相關(guān)轉(zhuǎn)移。由于三姿態(tài)角的解算方程并不獨立，需要假定某一姿態(tài)角為已知量，或者通過與內(nèi)置彈載器件如陀螺儀、加速度計等進(jìn)行數(shù)據(jù)融合來解算姿態(tài)角[4-6]，這些方法無疑大大增加了成本，而且由于載體小尺寸高轉(zhuǎn)速的特點，也會存在傳感器布局困難、姿態(tài)測量精度受限制等缺點。美國的研究人員最早提出了一種利用兩個磁傳感器輸出信號所對應(yīng)的零交叉點相位信息來測量旋轉(zhuǎn)彈丸地磁方位角的方法[7]。南京理工大學(xué)在此基礎(chǔ)上提出了一種基于非正交磁傳感器組的極值比值法并做了相關(guān)研究[8-9]。文獻(xiàn)[10]提出了一種比極值比值法精度更高的積分比方法，利用在高自旋環(huán)境中收集的所有地磁數(shù)據(jù)來計算彈丸姿態(tài)。文獻(xiàn)[11]也在此基礎(chǔ)上提出了一種相移比方法，簡化了極值比-螺距角曲線的校準(zhǔn)，并充分利用了測量數(shù)據(jù)。

在以上研究的基礎(chǔ)上，本文通過分析穩(wěn)定彈丸的外力矩情況，推導(dǎo)出了包含俯仰和偏航參數(shù)的彈丸繞心動力學(xué)方程組，然后基于磁方位角的物理原理，借助擴(kuò)展卡爾曼濾波器（EKF）估計出了彈丸的俯仰角和偏航角。最后通過模擬仿真驗證了該方法的有效性，并將該方法應(yīng)用到實際工程項目中，也取得了預(yù)期的效果。

1 坐標(biāo)系描述

1.1 地理坐標(biāo)系O-NED

描述地磁場時，通常將地磁場磁感應(yīng)強(qiáng)度M投影到北東地坐標(biāo)系O - NED。如圖1所示，以北半球為例，磁感應(yīng)強(qiáng)度M所處垂面為磁子午面，X軸沿地理子午線向北為正，Y軸沿緯度線向東為正，Z軸垂直向下為正。

圖1 地磁場參數(shù)Fig.1 Geomagnetic parameters

地磁場強(qiáng)度M在X、Y、Z軸上的投影x、y、z分別稱為北向、東向以及垂直磁強(qiáng)度。M在水平面XOY面上的投影H 稱為水平磁強(qiáng)度。磁子午面與地理子午面的夾角D 稱為磁偏角，規(guī)定H 向東偏為正。M 與水平面的夾角I 稱為磁傾角，在北半球，M指向地平線之下，磁傾角為正。

1.2 速度坐標(biāo)系O-X2Y2Z2

速度坐標(biāo)系 O - X2Y2Z2的 O X2軸沿質(zhì)心速度矢量v 的方向，O Y2軸垂直于速度向上，O Z2軸按右手法則與 O X2Y2面垂直并向右為正。在速度坐標(biāo)系中，速度矢量在鉛直面的投影分量與水平面的夾角稱為速度高低角θa，也稱為彈道傾角；在水平面的投影分量與鉛直面的夾角稱為速度方向角ψ2，也稱為彈道偏角。

1.3 彈軸坐標(biāo)系O-ξηζ

Oξ軸為彈軸且向前為正，Oη軸垂直于Oξ軸指向上方，Oζ軸按右手法則垂直于Oξη平面指向右。Oξ軸在鉛直面的投影與水平面的夾角稱為俯仰角φa，在水平面的投影與鉛直面的夾角稱為偏航角φ2。

1.4 第二彈軸坐標(biāo)系 O-ξη2ζ2

第二彈軸坐標(biāo)系是由速度坐標(biāo)系 O - X2Y2Z2旋轉(zhuǎn)而來，Oξ軸仍為彈軸，先由速度坐標(biāo)系 O - X2Y2Z2繞OZ2軸旋轉(zhuǎn)δ1角到達(dá) O - ξ′ η2Z2位置，再由 O - ξ′ η2Z2繞 O η2軸旋轉(zhuǎn)δ2角到達(dá) O - ξη2Z2位置，如圖2所示。其中，δ1和δ2分別為高低攻角和方向攻角。彈軸坐標(biāo)系O- ξηζ與第二彈軸坐標(biāo)系 O - ξη2Z2的Oξ軸都是彈丸的縱軸，故坐標(biāo)平面Oηζ與坐標(biāo)平面 O η2ζ2只相差一個轉(zhuǎn)角β。

圖2 第二彈軸坐標(biāo)系與速度坐標(biāo)系的關(guān)系Fig.2 Relationship between the second projectile axial coordinate system and the velocity coordinate system

2 基于零交叉點法的地磁方位角和滾轉(zhuǎn)角測量

與彈軸夾角為λ的地磁傳感器隨彈體在地磁場中一起旋轉(zhuǎn)時，沿傳感器敏感軸的瞬時場強(qiáng)為[7]：

旋轉(zhuǎn)彈丸在地磁場中飛行時，傳感器的輸出信號周期性地變化。當(dāng)傳感器軸與地磁場正交時，傳感器的輸出為零，即所謂的零交叉點。通過對兩個地磁傳感器的零交叉點進(jìn)行判別和運(yùn)算，就可以確定彈體飛行過程中相對于地磁場的方位角。

地磁傳感器組合使用兩個單軸地磁傳感器，一個與旋轉(zhuǎn)軸夾角為90°，另一個與旋轉(zhuǎn)軸夾角為60°，如圖3所示。

圖3 地磁傳感器組合安裝示意圖Fig.3 Installation diagram of geomagnetic sensor assembly

傾角分別為90°和60°的兩個磁傳感器S1、S2共面，當(dāng)彈體滾轉(zhuǎn)一周時，就會產(chǎn)生4個連續(xù)的零交叉點，對應(yīng)于各自傳感器的相位分別用和表示，則有比率：

磁方位角σM與比率Φ之間存在著對應(yīng)關(guān)系，其對應(yīng)關(guān)系規(guī)律只與磁傳感器S2的安裝傾角有關(guān)，如圖4所示，圖中λ代表斜置傳感器的傾斜角。

圖4 比率Φ與磁方位角σMFig.4 Ratio (Φ) versus magnetic azimuth (σM)

比率Φ以σM= 90°為對稱軸左右對稱，因此除了對稱軸之外，每個比率Φ都對應(yīng)兩個磁方位角σM，可以通過核查當(dāng) S2與磁場正交時沿 S1的磁場強(qiáng)度值的正負(fù)來確定取哪個值。

垂直軸傳感器S1的安裝傾角λ= 90°，則由式(1)可以得到沿傳感器S1敏感軸的瞬時場強(qiáng)為；

當(dāng)磁傳感器S1的敏感軸與地磁場正交時，傳感器輸出信號為零,即MS1=0。此時，sin(σM) = 0或者sin(φS1) = 0。而sin(σM) = 0意味著σM=0°或 180°，這種情況會在實驗中規(guī)避。所以，當(dāng)σM≠0°和 180°時，只存在 s in(φS1) = 0一種情況，則φS1= 0°或 180°，即相位組為(0°,180°)。

對于斜置軸傳感器S2有λ= 60°，則沿傳感器敏感軸的瞬時場強(qiáng)為：

當(dāng)磁傳感器S2的敏感軸與地磁場正交時，傳感器輸出信號為零，即MS2=0。代入式(4)，計算得到：

而在磁方位角σM已經(jīng)確定的情況下，相位組可以計算出來。

零交叉點法相對于傳統(tǒng)的坐標(biāo)系轉(zhuǎn)移方法，其優(yōu)勢在于將三姿態(tài)角信息(滾轉(zhuǎn)角、俯仰角、偏航角)轉(zhuǎn)換成兩姿態(tài)角信息(滾轉(zhuǎn)角、地磁方位角),將滾轉(zhuǎn)角信息和地磁方位角信息分離為俯仰角和偏航角的單獨解算提供了可能性。文獻(xiàn)[7]詳細(xì)討論了零交叉點法解算得出的地磁方位角及滾轉(zhuǎn)角的誤差，其誤差在10-3弧度的量級，滿足二次數(shù)據(jù)處理的要求。

3 俯仰角和偏航角的解算方法

由于彈丸在空中的瞬時姿態(tài)可以由地磁方位角表示，而彈丸在空中的地磁參數(shù)是已知的（通過彈丸位置信息計算得到），所以彈丸的地磁方位角只包含俯仰角和偏航角兩個未知信息。

彈丸的空中運(yùn)動可分解為質(zhì)心運(yùn)動和繞質(zhì)心運(yùn)動。質(zhì)心運(yùn)動規(guī)律由質(zhì)心運(yùn)動定律確定，繞質(zhì)心運(yùn)動則由動量矩定理來描述，其中，繞質(zhì)心運(yùn)動決定了彈丸的姿態(tài)運(yùn)動規(guī)律。假設(shè)無風(fēng)，參考外彈道學(xué)中的彈丸繞質(zhì)心動力方程組，結(jié)合實際問題組成新的方程組，如下：

式中，Mη、Mζ為外力矩在彈軸系的分量，A、C為轉(zhuǎn)動慣量系數(shù)，ωη、ωξ、ωζ為角速度分量，mz′為靜力矩動導(dǎo)數(shù)，其中，

研究對象為高速旋轉(zhuǎn)彈丸，只考慮靜力矩。靜力矩的向量形式如下：

靜力矩在彈軸坐標(biāo)系上的投影分量形式為

式中，vrζ和 vrη分別為相對速度 vr在彈軸坐標(biāo)系上的分量，又記 vrζ2和vrη2分別為相對速度vr在第二彈軸坐標(biāo)系上的分量，它們之間的關(guān)系為

各坐標(biāo)系內(nèi)的方位角存在以下關(guān)系[9]：

如圖2所示，自速度坐標(biāo)系向第二彈軸坐標(biāo)系的轉(zhuǎn)動關(guān)系可以得到則式(10)可以改寫為：

則靜力矩分量可以寫成：

將式(12)代入式(14)，得到：

將式(16)代入方程組(6)，可以得到：

式中：彈丸的速度v 可以通過彈道雷達(dá)測量得到；彈道傾角θa和彈道偏角ψ2也可以通過速度分量計算出來；彈丸縱軸角速度ωξ由零交叉點法計算得到。

4 姿態(tài)解算方法的EKF濾波器構(gòu)建

EKF的基本思路是對非線性函數(shù)的Taylor展開式進(jìn)行一階線性化截斷，忽略其余高階項，從而將非線性問題轉(zhuǎn)化為線性問題。

4.1 狀態(tài)方程

根據(jù)動力學(xué)方程組(17)取狀態(tài)變量

方程組(17)可以寫成如下狀態(tài)方程：

非線性方程(18)只是對彈丸運(yùn)動的近似描述，存在一定誤差，特引入一個高斯白噪聲V，且 V ～N(0,R)。

4.2 量測方程

射向記為αN，不考慮子午收斂角的影響，將磁場矢量和彈軸矢量分別投影到地面坐標(biāo)系中。

地磁單位矢量為

彈軸單位矢量為

式中，由于研究對象為旋轉(zhuǎn)彈丸，射程較近，所以全彈道過程中地磁變化很小，磁偏角D及磁傾角I可以視為定值。

地磁方位角σM是地磁矢量與彈軸矢量的夾角，根據(jù)向量夾角余弦公式可以得到：

式中，d為量測噪聲，

4.3 雅克比矩陣

對非線性函數(shù)的Taylor展開式進(jìn)行一階線性化截斷，則取前兩項為：

其中，Δt為采樣間隔。

由狀態(tài)方程(18)可得雅克比矩陣：

同理，通過量測方程(22)可得：

式中，

5 仿真驗證及實驗

5.1 仿 真

仿真以155 mm彈丸為模擬對象，假設(shè)彈丸以初速800 m/s發(fā)射,射角60°,射向100°。加入角速度高低分量2 rad/s和方向分量2 rad/s模擬初始擾動，仿真出全彈道數(shù)據(jù)；再通過彈丸的空中位置和相關(guān)坐標(biāo)系的轉(zhuǎn)移，模擬出全彈道的地磁參數(shù)；然后根據(jù)零交叉點法計算出彈丸的地磁方位角作為真值，滾轉(zhuǎn)角真值則以彈道數(shù)據(jù)為準(zhǔn)。

參照地磁傳感器的性能參數(shù)設(shè)置誤差，地磁方位角真值加入高斯白噪聲V～N(0,(0.5°)2)作為量測值；滾轉(zhuǎn)角真值加入高斯白噪聲d～N(0,(1.5°)2)模擬帶誤差的滾轉(zhuǎn)角計算值。圖5為模擬的量測值與真值的誤差,可以看出,地磁方位角的最大誤差在±2°，滾轉(zhuǎn)角的最大誤差在±6°左右，遠(yuǎn)大于文獻(xiàn)[7]提供的誤差。

圖5 加入的噪聲Fig.5 Added noise

采用設(shè)計好的 EKF濾波器進(jìn)行俯仰角和偏航角信息的估計,濾波結(jié)果與真值進(jìn)行比較，如圖6(a)所示，由圖中可以看出，估計值與真值非常接近，整體規(guī)律一致，只在局部稍有不同，而且俯仰角和偏航角分別繞彈道傾角和彈道偏角周期性擺動且幅值不斷衰減。圖6(b)進(jìn)一步展示了濾波的估計誤差，由圖中看出，俯仰角和偏航角的濾波誤差整體趨勢比較穩(wěn)定，最大誤差不超過0.2°。

從仿真過程及濾波結(jié)果來看，基于地磁方位角的姿態(tài)解算方法能夠比較準(zhǔn)確地估計出彈丸的俯仰角和偏航角，估計效果令人滿意。即使量測值的誤差較大，地磁方位角誤差±2°，滾轉(zhuǎn)角的模擬計算值誤差為±6°，采用文中提出的解算方法仍然能夠估計出精度較高的姿態(tài)角，最大誤差不超過0.2°。

圖6 解算結(jié)果與真值比較Fig.6 Calculation results versus true values

5.2 實彈實驗及結(jié)果分析

對于射程達(dá)到幾十公里的彈丸，以目前的技術(shù)水平無法直接觀測其俯仰和偏航姿態(tài)，但是根據(jù)彈丸的運(yùn)動規(guī)律和李雅普諾夫穩(wěn)定性可以檢驗實驗結(jié)果能否令人滿意。在飛行過程中，旋轉(zhuǎn)彈丸橫向姿態(tài)（俯仰、偏航）的慢速運(yùn)動項與速度方向是一致的，彈軸繞速度線呈周期性擺動且幅值不斷衰減，這樣才能保證彈丸的飛行穩(wěn)定性。因此可以通過雷達(dá)測量得到的彈丸速度角信息(即彈道角信息)來側(cè)面驗證姿態(tài)解算方法的可行性。

實彈實驗當(dāng)天，天氣晴朗，多云，無風(fēng)，在炮位架設(shè)測速雷達(dá)以便測量彈丸的速度。將實驗所使用的地磁傳感器組件與彈體一起標(biāo)定，以確保標(biāo)定實驗的準(zhǔn)確性。準(zhǔn)備完畢之后進(jìn)行發(fā)射實驗，射角為15.3°，射向為103.4°。

實驗結(jié)束之后，選擇其中一發(fā)彈丸的實驗數(shù)據(jù)作為分析對象。為詳細(xì)展示彈丸擺動規(guī)律，截取初始2 s的實驗數(shù)據(jù)。雷達(dá)測量彈丸初速為 725.16 m/s。彈丸飛行過程中，磁傳感器記錄下各軸方向的地磁強(qiáng)度變化，然后根據(jù)零交叉點法解算出彈丸飛行過程中的地磁方位角及轉(zhuǎn)速，解算結(jié)果如圖7所示。地磁方位角初始值為117.6°，彈丸轉(zhuǎn)速初始值為1486.39 rad/s。

然后采用設(shè)計好的EKF濾波器估計俯仰角和偏航角信息，并與測速雷達(dá)測量并計算得到的速度角信息對照，如圖8所示。估計出的俯仰角和偏航角分別繞彈道傾角和彈道偏角呈周期性擺動而且幅值不斷衰減，姿態(tài)變化規(guī)律穩(wěn)定，符合李雅普諾夫穩(wěn)定性定義。

圖7 地磁方位角及轉(zhuǎn)速解算結(jié)果Fig.7 Calculation of magnetic azimuth and rolling speed

圖8 俯仰角和偏航角的估計結(jié)果Fig.8 Estimation of pitch angle and yaw angle

再利用估計出的俯仰角、偏航角以及地磁參數(shù)計算出彈丸的地磁方位角，與零交叉點法解算的地磁方位角進(jìn)行對比，如圖9所示。從圖中可以看出，由俯仰角和偏航角反向計算出的地磁方位角與零交叉點法吻合，這表明了俯仰角和偏航角的估計結(jié)果比較準(zhǔn)確，達(dá)到預(yù)期效果。

圖9 地磁方位角測量值和計算值對照Fig.9 Contrast of magnetic azimuth between measured and calculated value

圖10 攻角δ1和δ2Fig.10 Attack anglesδ1and δ2

對于正常飛行的彈丸，可以近似認(rèn)為，估計得到的俯仰角與速度高低角θa的差值即為高低攻角δ1，偏航角與速度方位角ψ2的差值即為方向攻角δ2，如圖10所示。這樣對于測量彈丸的攻角信息也有幫助。

6 結(jié) 論

通過分析彈丸的旋轉(zhuǎn)運(yùn)動規(guī)律和受作用力矩情況，推導(dǎo)出了包含彈丸姿態(tài)角信息的動力學(xué)方程；再根據(jù)彈丸在地磁場的瞬時姿態(tài)建立起彈丸俯仰角、偏航角與地磁方位角的聯(lián)系；然后借助EKF估計出俯仰角和偏航角信息。仿真表明，文中所提出的解算方法精度較高，最大誤差不超過0.2°，證明了該方法的有效性。最后，將該方法應(yīng)用到實際工程中，取得了預(yù)期效果，也證明了該方法的可行性。