閆小龍，于天鵬，范 旭，朱宜家，陳國(guó)光，田曉麗

(1.中北大學(xué)，太原 030051； 2.淮海工業(yè)集團(tuán)， 山西 長(zhǎng)治 046000； 3.中國(guó)兵器工業(yè)第五九研究所， 重慶 400039)

1 引言

在制導(dǎo)彈箭的飛行過(guò)程中，彈體姿態(tài)的快速準(zhǔn)確測(cè)量直接決定了制導(dǎo)控制的精準(zhǔn)程度，而彈箭往往又存在大過(guò)載、寬范圍滾轉(zhuǎn)等特性。磁強(qiáng)計(jì)以抗過(guò)載能力強(qiáng)、無(wú)初始對(duì)需求、無(wú)累計(jì)誤差、體積小、重量輕、成本低等優(yōu)點(diǎn)成為制導(dǎo)彈箭姿態(tài)測(cè)量的最佳選擇方案[1-5]。

磁強(qiáng)計(jì)通常捷聯(lián)于彈體，依據(jù)完善的當(dāng)?shù)氐卮艌?chǎng)信息得出磁強(qiáng)計(jì)各敏感軸的分量，從而獲取彈體姿態(tài)[6]。但是，由于磁強(qiáng)計(jì)制造工藝、電路貼裝工藝、獲取信號(hào)的調(diào)理電路等問(wèn)題，磁強(qiáng)計(jì)的測(cè)量輸出值會(huì)受到不同因素的影響，比如磁強(qiáng)計(jì)敏感軸的非正交誤差、比例因子誤差、基線偏置誤差等，這些誤差都會(huì)對(duì)彈體的測(cè)量帶來(lái)不同程度的偏差[7-9]。因此，在正確的測(cè)量彈體姿態(tài)前必須對(duì)磁強(qiáng)計(jì)進(jìn)行校準(zhǔn)，以確保測(cè)量結(jié)果的準(zhǔn)確。在磁強(qiáng)計(jì)測(cè)姿的研究歷程中，極大極小值校正法、橢圓/橢球校正法、遞推最小二乘的羅差校正法以及基于無(wú)磁轉(zhuǎn)臺(tái)的誤差分離標(biāo)定法是最常見(jiàn)的校準(zhǔn)方法[10-12]，隨著磁強(qiáng)計(jì)在制導(dǎo)彈箭領(lǐng)域的運(yùn)用，一些自適應(yīng)的在線校正濾波方法也不斷被提出，比如非線性的卡爾曼濾波方法(EKF、UKF)等[13]。這些方法都有著不錯(cuò)的校準(zhǔn)效果，但是針對(duì)彈箭使用的磁強(qiáng)計(jì)測(cè)姿系統(tǒng)來(lái)說(shuō)，磁強(qiáng)計(jì)在發(fā)射初期非常容易受到各種鐵磁物質(zhì)的干擾，且全彈道時(shí)長(zhǎng)又相對(duì)較短。因此，如何快速準(zhǔn)確的對(duì)彈載磁強(qiáng)計(jì)測(cè)姿系統(tǒng)進(jìn)行在線校準(zhǔn)是一個(gè)非常具有挑戰(zhàn)性的問(wèn)題[14-15]。

本文旨在提出一種針對(duì)旋轉(zhuǎn)彈體的磁強(qiáng)計(jì)在線校準(zhǔn)測(cè)量方案。在彈箭發(fā)射后對(duì)彈體捷聯(lián)磁強(qiáng)計(jì)獲取的數(shù)據(jù)在時(shí)域內(nèi)進(jìn)行基于能量的初始概略評(píng)估，快速獲取磁強(qiáng)計(jì)校準(zhǔn)參數(shù)初始估計(jì)值后，將彈體的滾轉(zhuǎn)姿態(tài)二階擬合參數(shù)與磁強(qiáng)計(jì)校準(zhǔn)參數(shù)一同作為無(wú)跡卡爾曼濾波器的狀態(tài)量初值，在快速得到彈體滾轉(zhuǎn)姿態(tài)信息的同時(shí)進(jìn)一步對(duì)磁強(qiáng)計(jì)校準(zhǔn)參數(shù)進(jìn)行精準(zhǔn)獲取，為了保證彈體姿態(tài)的求解效率，在判定磁強(qiáng)計(jì)校準(zhǔn)參數(shù)收斂穩(wěn)定后對(duì)無(wú)跡卡爾曼濾波器進(jìn)行簡(jiǎn)化重構(gòu)，極大地降低姿態(tài)信息求解的空間及時(shí)間復(fù)雜度。所提出的算法將有效的提高磁強(qiáng)計(jì)校準(zhǔn)精度及姿態(tài)求解速度。

2 磁強(qiáng)計(jì)測(cè)姿誤差模型

將磁強(qiáng)計(jì)捷聯(lián)于旋轉(zhuǎn)彈體后，由于磁強(qiáng)計(jì)本身的制造工藝誤差、調(diào)理電路封裝誤差、磁強(qiáng)計(jì)相對(duì)彈體安裝誤差以及彈體干擾等原因使得磁強(qiáng)計(jì)獲取的磁場(chǎng)信息與當(dāng)?shù)卮艌?chǎng)信息不符，誤差模型可以描述為

(1)

圖1是基于磁強(qiáng)計(jì)的角度測(cè)量模型。圖1中Ox′、Oy′、Oz′分別為彈體的x、y、z軸，其中Ox′方向?yàn)閺椉^部指向，Ox、Oy、Oz分別為捷聯(lián)磁強(qiáng)計(jì)的x、y、z軸，為了在可行的操作范圍內(nèi)降低磁強(qiáng)計(jì)的校準(zhǔn)難度，在磁強(qiáng)計(jì)安裝于彈體時(shí)認(rèn)為彈體y軸與磁強(qiáng)計(jì)y′軸是重合的。其中δz為磁強(qiáng)計(jì)z軸與彈體z′軸的誤差角，δxz為磁強(qiáng)計(jì)x軸與彈體x′y′平面的誤差?yuàn)A角，δxy為磁強(qiáng)計(jì)x軸在彈體x′y′平面的投影與彈體x′軸的誤差?yuàn)A角。

圖1 捷聯(lián)磁強(qiáng)計(jì)非正交誤差模型示意圖Fig.1 Magnetorometer non-fixed error model

因此，可以磁強(qiáng)計(jì)的非正交誤差矩陣可以描述為

(2)

結(jié)合磁強(qiáng)計(jì)誤差模型，并且忽略軟鐵誤差與噪聲的前提下，對(duì)于旋轉(zhuǎn)制導(dǎo)彈箭的三軸磁強(qiáng)計(jì)輸出模型可以表示為

(3)

式中:sx、sy、sz分別為三通道敏感軸的比例因子；Bbx、Bby、Bbz分別為三通道敏感軸的偏置誤差；Bex、Bey、Bez分別為地磁場(chǎng)be在彈體三軸方向上磁分量；ω為彈體飛行滾轉(zhuǎn)角速度；t為飛行時(shí)間；By、Bz為磁強(qiáng)計(jì)彈體橫截面內(nèi)兩敏感軸的輸出值；φ0為磁強(qiáng)計(jì)的初始相位角。

則式(3)可以描述為

(4)

依據(jù)磁強(qiáng)計(jì)與彈體捷聯(lián)關(guān)系，求解上式可得彈體實(shí)時(shí)滾轉(zhuǎn)姿態(tài)角：

(5)

式中：sy、sz、Bbz、Bby、δz為磁強(qiáng)計(jì)待校準(zhǔn)參數(shù)值。

通常情況下，在彈箭發(fā)射及飛行過(guò)程中，如果不對(duì)滾轉(zhuǎn)姿態(tài)進(jìn)行控制，則滾轉(zhuǎn)運(yùn)動(dòng)可以近似的認(rèn)為是勻加速的，且加速度是非常小的，因此滾轉(zhuǎn)運(yùn)動(dòng)可以近似的描述為。

(6)

式中：Φ0為彈箭的初始角位移；Φ′為彈箭的滾轉(zhuǎn)角速度；Φ″為彈箭的滾轉(zhuǎn)角加速度。表達(dá)式中“i”表示第i個(gè)采樣周期對(duì)應(yīng)的數(shù)據(jù)。

在彈道的參數(shù)處理過(guò)程中，通常通過(guò)實(shí)時(shí)濾波的方法將參數(shù)估計(jì)的問(wèn)題轉(zhuǎn)換成參數(shù)實(shí)時(shí)濾波的問(wèn)題，從而增加參數(shù)的時(shí)效性與準(zhǔn)確性。在此建立濾波的系統(tǒng)狀態(tài)方程與量測(cè)方程：

(7)

式中：Xk+1為系統(tǒng)狀態(tài)向量；f(Xk，uk)為系統(tǒng)線性函數(shù)；h(Xk+1)為系統(tǒng)的非線性函數(shù)；Wk為系統(tǒng)方程高斯白噪聲；uk為確定性控制向量；Hk+1為系統(tǒng)的量測(cè)向量；Vk+1為測(cè)量方程的高斯白噪聲。

3 基于磁強(qiáng)計(jì)能量捕獲的校準(zhǔn)參數(shù)估計(jì)

3.1 磁強(qiáng)計(jì)能量捕獲

在利用磁強(qiáng)計(jì)測(cè)量彈體飛行滾轉(zhuǎn)姿態(tài)前，磁強(qiáng)計(jì)必須進(jìn)過(guò)精確校準(zhǔn)才能準(zhǔn)確獲取彈體滾轉(zhuǎn)角。由式(4)可知，磁強(qiáng)計(jì)的校準(zhǔn)參數(shù)均包含在磁強(qiáng)計(jì)敏感軸測(cè)量模型中。依據(jù)磁強(qiáng)計(jì)測(cè)量磁場(chǎng)機(jī)理，磁強(qiáng)計(jì)敏感軸敏感當(dāng)?shù)卮艌?chǎng)在軸上的分量，并以模擬量或數(shù)字量的形式輸出，其本質(zhì)反應(yīng)的是當(dāng)?shù)卮艌?chǎng)在軸方向上的分量。當(dāng)旋轉(zhuǎn)彈體在飛行過(guò)程中，彈體繞彈軸旋轉(zhuǎn)，其捷聯(lián)在彈體上的磁強(qiáng)計(jì)y軸與z軸敏感到固定磁場(chǎng)的磁分量為正弦變化的。因此，依據(jù)獲取參數(shù)對(duì)兩個(gè)敏感軸的磁能曲線進(jìn)行擬合，可以快速的得到磁強(qiáng)計(jì)校準(zhǔn)參數(shù)的初始估計(jì)值。y軸與z軸的實(shí)時(shí)磁分量可以描述為

(8)

根據(jù)法拉第電磁感應(yīng)產(chǎn)生的電動(dòng)勢(shì)可知：

(9)

針對(duì)制導(dǎo)彈箭來(lái)說(shuō)，一般發(fā)射過(guò)程會(huì)賦予彈體一個(gè)預(yù)設(shè)的角速度，這個(gè)角速度通常由身管膛線或發(fā)射架體的導(dǎo)轉(zhuǎn)機(jī)構(gòu)來(lái)確定的，因此彈體的初始角速度可以描述為

(10)

式中:vi為彈體在身管內(nèi)的移動(dòng)速度、r為身管半徑，θ為身管纏線角。

依據(jù)磁場(chǎng)能量模型，結(jié)合式(8)～式(10)可得ti時(shí)刻由于磁場(chǎng)變化產(chǎn)生的能量可以表示為

(11)

式中，k為能量與磁場(chǎng)強(qiáng)度的修正系數(shù)。

3.2 磁強(qiáng)計(jì)校準(zhǔn)參數(shù)估計(jì)

由式(11)可以看出彈體捷聯(lián)磁強(qiáng)計(jì)兩個(gè)正交的敏感軸捕獲磁場(chǎng)能量可以描述為正弦與余弦的函數(shù)關(guān)系，依據(jù)磁強(qiáng)計(jì)的實(shí)時(shí)采樣值與彈體炮口初速估計(jì)值對(duì)兩敏感軸磁能模型進(jìn)行三角函數(shù)擬合。

令：

(12)

則有

(13)

(14)

求解上式即可得到Ay、Φy、Ky、Az、Φz(mì)、Kz的最優(yōu)擬合值，結(jié)合式(12)即可求得磁強(qiáng)計(jì)在彈體截面內(nèi)兩個(gè)敏感軸的校準(zhǔn)參數(shù)初值sy、sz、Bby、Bbz、δz。

4 二次重構(gòu)無(wú)跡卡爾曼濾波算法求解彈體滾轉(zhuǎn)姿態(tài)

當(dāng)獲取到磁強(qiáng)計(jì)的校準(zhǔn)參數(shù)后，由式(5)可計(jì)算出彈體的滾轉(zhuǎn)姿態(tài)角，因此，狀態(tài)向量初值為

列出無(wú)跡卡爾曼濾波器方程：

(15)

更新?tīng)顟B(tài)矩陣和協(xié)方差矩陣：

(16)

(17)

鑒于校準(zhǔn)參數(shù)的最優(yōu)估計(jì)結(jié)果間存在耦合關(guān)系，將誤差估計(jì)定義為

(18)

式中，S是磁強(qiáng)計(jì)校準(zhǔn)參數(shù)的真實(shí)值。

(19)

5 數(shù)值仿真

為了驗(yàn)證算法的有效性，對(duì)該算法進(jìn)行數(shù)值仿真。建立彈體捷聯(lián)磁強(qiáng)計(jì)測(cè)量模型，并人為設(shè)置仿真誤差參數(shù)：Y敏感軸Bby誤差設(shè)置為1.4%，比例因子誤差設(shè)置為6%，Z敏感軸Bbz誤差設(shè)置為4.8%，比例因子誤差設(shè)置為-0.9%，非正交誤差角δz設(shè)置為12°，系統(tǒng)噪聲設(shè)置為0.2%，測(cè)量噪聲設(shè)置為0.2%，采樣頻率為500 Hz，如圖2所示。

圖2 產(chǎn)生的預(yù)設(shè)誤差樣本曲線Fig.2 Produced data sample

由濾波結(jié)果可以看出從彈道時(shí)5 s開(kāi)始比對(duì)結(jié)果，磁強(qiáng)計(jì)敏感軸Bby、Bbz校準(zhǔn)參數(shù)最終精度在0.1‰，比例因子sy/sz校準(zhǔn)參數(shù)最終精度在3‰，非正交誤差角δz校準(zhǔn)參數(shù)最終精度在4‰，滾轉(zhuǎn)姿態(tài)角估計(jì)誤差最終精度在0.23°左右，如圖3～圖6所示。在彈箭的閉環(huán)控制飛行中，0.23°的滾轉(zhuǎn)姿態(tài)誤差是非常小的，完全可以滿足彈箭控制飛行需求。

圖4 比例因子參數(shù)濾波結(jié)果曲線Fig.4 Scale factor parameter filtering result

圖5 非正交誤差角參數(shù)濾波結(jié)果曲線Fig.5 Non-orthogonal error angular parameter filter results

圖6 彈體滾轉(zhuǎn)姿態(tài)角濾波結(jié)果曲線Fig.6 Military roll gesture angle filter results

6 結(jié)論

提出了一種基于能量評(píng)估的無(wú)跡卡爾曼濾波算法(PUKF)，用于彈箭飛行過(guò)程中在線快速校準(zhǔn)磁強(qiáng)計(jì)并估計(jì)彈體滾轉(zhuǎn)角姿態(tài)。有效地解決了旋轉(zhuǎn)彈體在發(fā)射后不能及時(shí)獲取滾轉(zhuǎn)信息而無(wú)法進(jìn)行控制的問(wèn)題。與現(xiàn)有算法不同，所提出的算法通過(guò)磁強(qiáng)計(jì)敏感軸捕獲的磁分量建立磁能函數(shù)，結(jié)合彈體炮口轉(zhuǎn)速估計(jì)值對(duì)磁能三角函數(shù)進(jìn)行參數(shù)估計(jì)。將初步獲取的校準(zhǔn)參數(shù)估計(jì)值與磁強(qiáng)計(jì)測(cè)量值對(duì)彈體滾轉(zhuǎn)姿態(tài)進(jìn)一步解算，最終將磁強(qiáng)計(jì)校準(zhǔn)參數(shù)、彈體滾轉(zhuǎn)姿態(tài)一起作為無(wú)跡卡爾曼的濾波初值進(jìn)行濾波處理，獲取精確的磁強(qiáng)計(jì)校準(zhǔn)參數(shù)與彈體滾轉(zhuǎn)姿態(tài)角信息。通過(guò)校準(zhǔn)參數(shù)的三角函數(shù)擬合處理與無(wú)跡卡爾曼濾波算法相結(jié)合有效的減少了濾波收斂時(shí)間，使得彈箭出炮口較短時(shí)間內(nèi)較準(zhǔn)確的獲取彈體滾轉(zhuǎn)姿態(tài)用于航跡控制，提高了彈箭的作戰(zhàn)效能。