閆小龍，陳國光，田曉麗, 董曉芬，趙福春

(1 中北大學，太原 030051； 2 山東特種工業(yè)集團有限公司，山東淄博 255201)

0 引言

在制導(dǎo)火箭彈的飛行過程中，為了準確對火箭彈的飛行軌跡進行控制，姿態(tài)角的獲取是非常重要的。如今使用陀螺儀、星敏傳感器、GPS以及磁強計來測量滾轉(zhuǎn)姿態(tài)角[1]。磁強計以成本低，受外界干擾小而被廣泛應(yīng)用。然而，這種傳感器測姿時存在較多的誤差因素，限制了滾轉(zhuǎn)角測量的精度。因此，在正確測量火箭彈滾轉(zhuǎn)角前，必須對磁強計及測量電路進行校準補償。目前，研究人員已經(jīng)提出了很多的解決方案。黃琳等人使用UKF濾波方法離線校準磁傳感器的靈敏度偏差，從而給出每個磁傳感器對應(yīng)的補償值[2]。Da Forno等人分別使用UKF與EKF對磁強計的靈敏度和基線偏移進行在線校準[3]。盧兆興等人提出使用自適應(yīng)遺傳算法校準磁強計的靈敏度[4]。文中提出的算法對雙軸磁強計的測量幅值、雙軸基線偏移、靈敏度、非正交角進行在線校準。磁強計五個參數(shù)的校準使測量值更為準確；兩步快速算法使得在線快速校準成為可能。

1 滾轉(zhuǎn)姿態(tài)角測量模型

使用雙軸磁強計測量制導(dǎo)火箭彈滾轉(zhuǎn)角時，在火箭彈橫截面上安裝了雙軸磁強計測量組件。通過角度關(guān)系的轉(zhuǎn)換，可以得到火箭彈的滾轉(zhuǎn)姿態(tài)角。

圖1是基于磁強計的角度測量模型。紅色方塊代表雙軸磁強計測量組件。其中，O-xynz為測量坐標系，OX為彈軸矢量，Oy和Oz是磁強計的兩個理論正交的敏感軸。δ是非正交誤差角，OZm為載體零位參考軸，OZ為慣性空間零位參考軸，Hyz是火箭彈當?shù)氐卮艌鍪噶吭贠yz上的投影，由全球地磁場模型給出，γ為載體旋轉(zhuǎn)姿態(tài)角。φ0是磁強計的安裝誤差角，φ是制導(dǎo)火箭彈的滾轉(zhuǎn)姿態(tài)角。

(1)

記φs=φ-φ0，則

(2)

火箭彈的滾轉(zhuǎn)姿態(tài)角可得

γ=αH-φ0-φs

(3)

通過濾波方法估計參數(shù)實際上是將參數(shù)估計的問題轉(zhuǎn)化為狀態(tài)估計的問題，可以將濾波過程中關(guān)注的參數(shù)增加到狀態(tài)向量中，在濾波過程中實時估計，根據(jù)濾波的狀態(tài)向量與設(shè)定真值進行對比，從而判斷濾波過程及結(jié)果的正確性。

濾波的系統(tǒng)狀態(tài)方程與量測方程如式(4)和式(5)所示：

Xk+1=f(Xk,uk)+Wk

(4)

Hk+1=h(Xk+1)+Vk+1

(5)

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

濾波過程中被估計的參數(shù)向量為：

(6)

為了準確而精簡地描述火箭彈的滾轉(zhuǎn)運動，考慮到火箭彈在飛行過程中所需要的飛行參數(shù)以及在飛行過程中的加速度變化很小，所以這里假設(shè)火箭彈的滾轉(zhuǎn)運動為加速度緩慢變化的加速運動。

(7)

在公式(7)中，Φ,Φ′,Φ″分別代表火箭彈的滾轉(zhuǎn)角位移，角速度和角加速度，下標“i”表示第i個采樣周期對應(yīng)的數(shù)據(jù)。

2 濾波器初值的兩步快速自適應(yīng)調(diào)整

在卡爾曼濾波算法中，狀態(tài)向量初值的準確性決定著濾波器的收斂速度[6]。然而，由于不同的磁強計，不同的安裝方法及安裝工藝導(dǎo)致狀態(tài)向量中的參數(shù)具有不同的真值，所以狀態(tài)向量的初值是不確定的。為了加快濾波器的收斂速度，需要將濾波器的初值盡可能快的精確到一個較小的范圍內(nèi)。

2.1 第一步：磁強計校準參數(shù)的快速調(diào)整

捷聯(lián)在彈體截面上的雙軸磁強計在火箭彈飛行時，Y軸和Z軸的軌跡方程為：

(8)

在這一步驟中，校正參數(shù)擬合的精度越高，意味著UKF收斂速度越快，但是擬合精度的提高與擬合時間的消耗并非成正比關(guān)系。因此，當擬合精度提高的速度接近濾波器收斂的速度時需要適當?shù)呐袛鄺l件來終止這一過程。

(Λf)i=(Xf)i-(Xf)i-1

(9)

擬合參數(shù)的協(xié)方差矩陣為:

(10)

2.2 第二步：滾轉(zhuǎn)運動的快速調(diào)整

(11)

結(jié)合式(7)有下列關(guān)系：

(12)

同樣，在考慮解算速度要求高于解算精度要求的前提下，當滾轉(zhuǎn)角參數(shù)殘差最小時取為濾波初值，對Φ,Φ′,Φ″進行參數(shù)擬合，定義殘差為：

(13)

當ε取到最小值時，火箭彈的滾轉(zhuǎn)參數(shù)Φ,Φ′,Φ″取得最優(yōu)值。

與第一步相同，在第二步中，參數(shù)擬合的精度與擬合的時間并非成正比。當擬合精度提高的速度接近濾波收斂的速度時需要一個類似的停止策略。

(Λs)i=(Xs)i-(Xs)i-1

(14)

擬合參數(shù)的協(xié)方差為：

(15)

3 具有加性噪聲的增強無跡卡爾曼濾波算法

為了提高現(xiàn)有濾波算法的性能，文中提出了快速收斂噪聲隱含的擴維無跡卡爾曼濾波算法。文中提出新的TAUKF算法，該算法在UKF中包含模型誤差預(yù)測，增加了系統(tǒng)狀態(tài)下的驅(qū)動噪聲，以擴大系統(tǒng)狀態(tài)的輸入信息。所提出的算法克服了傳統(tǒng)UKF系統(tǒng)模型誤差對濾波性能的影響，增強了濾波過程的魯棒性。擴展系統(tǒng)狀態(tài)，增加了UKF模型中狀態(tài)向量的維數(shù)?？紤]到驅(qū)動噪聲對系統(tǒng)運動模型的影響，噪聲被加到系統(tǒng)狀態(tài)中。因此，擴展狀態(tài)向量和協(xié)方差矩陣可以表示為：

(16)

(17)

(18)

(19)

(20)

時間傳播方程為：

(21)

(22)

狀態(tài)估計方程為：

(23)

(24)

量測估計方程為：

(25)

(26)

互協(xié)方差矩陣為：

(27)

計算UKF增益：

(28)

最后，更新狀態(tài)矩陣和協(xié)方差矩陣：

(29)

4 算法模擬仿真

測量火箭彈的滾轉(zhuǎn)運動，對磁強計和測量電路進行仿真建模?；鸺龔椩趯嶋H飛行過程中，磁強計和測量電路的工作參數(shù)不能準確的獲取。而在模擬過程中，這些誤差參數(shù)都可以進行人為調(diào)整。只有這樣才能將最終的數(shù)據(jù)處理結(jié)果與正確的數(shù)據(jù)進行對比，用來評估算法的優(yōu)勢。

分別使用傳統(tǒng)UKF方法和文中提出的TAUKF算法對磁場測量組件進行參數(shù)校準和火箭彈的角度測量。

參數(shù)參數(shù)值標稱值真實值Y00.50.513Z00.50.406kyz11.19δ/(°)010.6Rk00.003Qk00.003fs/Hz500

由圖2(圖中FS為“Full Scale”的縮寫，代表滿量程)我們可以看到，TAUKF在0.11 s左右完成了濾波算法的初值擬合，并將濾波算法的初值調(diào)整到非常接近真值的精度。在后續(xù)濾波階段大約0.15 s左右，濾波器狀態(tài)向量中校正參數(shù)接近真值，校正參數(shù)值設(shè)為最優(yōu)恒定值。開始小計算量的UKF濾波。通過對比可以看出，TAUKF較UKF在收斂速度上有著較明顯的優(yōu)勢。

在滾轉(zhuǎn)角信息的輸出結(jié)果看來，提出的TAUKF算法同樣在收斂速度上要遠遠的高于現(xiàn)有的UKF算法。并且在500 Hz采樣頻率的前提下，提出的TAUKF算法可以在接收數(shù)據(jù)0.17 s左右就可以提供0.023 rad精度的角位移，0.07 rad/s精度的角速度，以及0.08 rad/s2精度的角加速度。而傳統(tǒng)的UKF算法想要提供該精度下的滾轉(zhuǎn)角信息則需要49 s左右的信號處理時間。

5 結(jié)論

在該研究中，提出了一種TAUKF濾波算法，用于在線快速估計火箭彈的滾轉(zhuǎn)姿態(tài)角。有效地解決了旋轉(zhuǎn)導(dǎo)彈在發(fā)射后不能及時獲取滾轉(zhuǎn)姿態(tài)而無法機動的問題。該算法通過對磁場測量組件的校正參數(shù)的確定，以及實時的量測結(jié)果，計算火箭彈的滾轉(zhuǎn)姿態(tài)角。在滾轉(zhuǎn)姿態(tài)角的實時處理過程中，該算法對濾波器的初值進行自適應(yīng)獲取，極大的加快了濾波器的收斂速度與精度。并且在磁場測量組件的校正參數(shù)確定后，對濾波器的狀態(tài)向量的校正參數(shù)賦予確定值，在很大程度上減少了彈載計算機的運算量，為彈上其他信息的計算留出空間。