兩步自適應(yīng)快速濾波算法測(cè)量火箭彈滾轉(zhuǎn)角*

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

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

0 引言

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

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

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

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

(1)

記φs=φ-φ0，則

(2)

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

γ=αH-φ0-φs

(3)

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

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

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

(4)

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

(5)

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

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

(6)

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

(7)

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

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

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

2.1 第一步：磁強(qiáng)計(jì)校準(zhǔn)參數(shù)的快速調(diào)整

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

(8)

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

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

(9)

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

(10)

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

(11)

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

(12)

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

(13)

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

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

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

(14)

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

(15)

3 具有加性噪聲的增強(qiáng)無(wú)跡卡爾曼濾波算法

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

(16)

(17)

(18)

(19)

(20)

時(shí)間傳播方程為：

(21)

(22)

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

(23)

(24)

量測(cè)估計(jì)方程為：

(25)

(26)

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

(27)

計(jì)算UKF增益：

(28)

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

(29)

4 算法模擬仿真

測(cè)量火箭彈的滾轉(zhuǎn)運(yùn)動(dòng)，對(duì)磁強(qiáng)計(jì)和測(cè)量電路進(jìn)行仿真建模。火箭彈在實(shí)際飛行過(guò)程中，磁強(qiáng)計(jì)和測(cè)量電路的工作參數(shù)不能準(zhǔn)確的獲取。而在模擬過(guò)程中，這些誤差參數(shù)都可以進(jìn)行人為調(diào)整。只有這樣才能將最終的數(shù)據(jù)處理結(jié)果與正確的數(shù)據(jù)進(jìn)行對(duì)比，用來(lái)評(píng)估算法的優(yōu)勢(shì)。

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

參數(shù)參數(shù)值標(biāo)稱值真實(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)恒定值。開(kāi)始小計(jì)算量的UKF濾波。通過(guò)對(duì)比可以看出，TAUKF較UKF在收斂速度上有著較明顯的優(yōu)勢(shì)。

在滾轉(zhuǎn)角信息的輸出結(jié)果看來(lái)，提出的TAUKF算法同樣在收斂速度上要遠(yuǎn)遠(yuǎn)的高于現(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左右的信號(hào)處理時(shí)間。

5 結(jié)論

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