安亮亮，王良明，趙丹丹

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

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

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

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

1 坐標(biāo)系描述

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

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

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

地磁場(chǎng)強(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)而來(lái)，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只相差一個(gè)轉(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 基于零交叉點(diǎn)法的地磁方位角和滾轉(zhuǎn)角測(cè)量

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

則靜力矩分量可以寫成：

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

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

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

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

EKF的基本思路是對(duì)非線性函數(shù)的Taylor展開(kāi)式進(jìn)行一階線性化截?cái)?，忽略其余高階項(xiàng)，從而將非線性問(wèn)題轉(zhuǎn)化為線性問(wèn)題。

4.1 狀態(tài)方程

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

方程組(17)可以寫成如下?tīng)顟B(tài)方程：

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

4.2 量測(cè)方程

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

地磁單位矢量為

彈軸單位矢量為

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

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

式中，d為量測(cè)噪聲，

4.3 雅克比矩陣

對(duì)非線性函數(shù)的Taylor展開(kāi)式進(jìn)行一階線性化截?cái)?，則取前兩項(xiàng)為：

其中，Δt為采樣間隔。

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

同理，通過(guò)量測(cè)方程(22)可得：

式中，

5 仿真驗(yàn)證及實(shí)驗(yàn)

5.1 仿 真

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

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

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

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

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

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

5.2 實(shí)彈實(shí)驗(yàn)及結(jié)果分析

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

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

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

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

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

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

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

圖9 地磁方位角測(cè)量值和計(jì)算值對(duì)照Fig.9 Contrast of magnetic azimuth between measured and calculated value

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

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

6 結(jié) 論

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