Kalman彈道濾波器的快速收斂性研究

范　旭，陳國光，白敦卓，楊智杰

(1.中北大學(xué)機電工程學(xué)院，太原 030051；2.豫西工業(yè)集團有限公司，南陽 473000)

0　引言

對于彈道測量，其測量數(shù)據(jù)中包含著干擾或噪聲引起的誤差，直接用測量數(shù)據(jù)進行分析計算將會帶來很大的誤差甚至是錯誤，而必須從量測數(shù)據(jù)中濾取所需要的信息，這種數(shù)據(jù)處理方法稱為濾波。目前，工程技術(shù)中使用較多的是最小二乘濾波、最大似然估計和Kalman濾波。其中，Kalman濾波在控制領(lǐng)域，在火箭、炮彈、導(dǎo)彈彈道和飛行狀態(tài)以及衛(wèi)星軌道觀測的數(shù)據(jù)處理上得到廣泛的應(yīng)用。Kalman最初提出的濾波基本理論只適用于線性系統(tǒng)，并且要求量測也必須是線性的。如果模型是非線性的，通常在推導(dǎo)濾波方程時，增加線性化步驟。在狀態(tài)估計時，對系統(tǒng)方程在前一狀態(tài)值處做實時的線性Taylor近似。在預(yù)測步驟中，對量測方程在相應(yīng)的預(yù)測位置也進行線性Taylor近似,得到的Kalman濾波稱為擴展Kalman濾波。處理非線性模型的這一思想是很自然的，且濾波過程簡單有效[1]。

常規(guī)的濾波初值選取方法雖然可使濾波過程及結(jié)果收斂，但濾波前期的殘差較大且濾波收斂速度慢，需要計算一段時間才可應(yīng)用，影響彈道修正的實時應(yīng)用并且使修正過程出現(xiàn)不必要的大幅度修正。為解決此類問題，需提出較好的狀態(tài)初值估計方法，使濾波過程快速收斂滿足彈道修正的實時性需要。

1　外彈道濾波模型

彈道濾波是根據(jù)在一段彈道上測得的彈箭飛行彈道數(shù)據(jù)(坐標、速度、加速度、姿態(tài)角和姿態(tài)變化率)，利用數(shù)學(xué)方法從這些數(shù)據(jù)中提取當(dāng)前飛行狀態(tài)及彈道數(shù)學(xué)模型中的參數(shù)的最優(yōu)估計。而彈道預(yù)測則是利用這種最優(yōu)估計和彈道數(shù)學(xué)模型計算彈道，求取所需點的彈道參數(shù)。彈道濾波是彈道預(yù)測的基礎(chǔ)，而彈道預(yù)測是彈道修正的重要依據(jù)，所以濾波的收斂速度及準確度也是彈道修正的重要研究方向。

1.1　濾波狀態(tài)模型

地面炮位偵查雷達對空中飛行的彈箭進行跟蹤和探測，地面計算機對雷達實測到的彈道參數(shù)進行數(shù)據(jù)處理，獲得較準確的彈道參數(shù)，與目標坐標比較，得到彈道偏差，并由雷達向彈載計算機傳輸指令，使修正控制機構(gòu)實現(xiàn)一維或二維彈道修正。根據(jù)上述彈道修正彈的執(zhí)行過程，在雷達量測數(shù)據(jù)的濾波過程中，需要用到彈道模型，考慮彈道參數(shù)獲取的快速性、實時性等，本文采用質(zhì)點彈道模型作為彈道濾波的狀態(tài)方程[2]：

(1)

式中，x、y、z、vx、vy、vz分別為彈箭在地面坐標系中的坐標分量和速度分量，C為彈道系數(shù)，Hτ(y)為空氣密度函數(shù)，G(vτ)為阻力函數(shù)，彈丸速度

為地面標準虛溫值，τ為彈丸所處高度y處的虛溫值，X=(vx,vy,vz,x,y,z)T作為濾波的狀態(tài)變量。

則系統(tǒng)狀態(tài)方程為：

(2)

式中，W(k)是均值為0的Gauss白噪聲，且服從方差為Q的正態(tài)分布，W～N(0,Q)。

1.2　濾波量測方程

設(shè)雷達測量值為斜距r、方位角β和高低角ε，雷達坐標系為球坐標系，可得雷達測量值與地面坐標系的關(guān)系[3]：

(3)

令量測變量為Z，即Z=(r,β,ε)T，則得量測方程為：

(4)

式中，V是雷達的量測噪聲，假定為零均值Gauss白噪聲，且服從方差為R的正態(tài)分布。即V～N(0,R)，h(X)為三維矢量函數(shù)。

2　Kalman彈道濾波方程

Kalman濾波方程組分為兩大部分，第一部分為Kalman濾波方程，這一部分負責(zé)向下一時刻推算軌跡狀態(tài)；第二部分為Kalman濾波器的增益矩陣遞推公式，這一部分用于反饋先驗估計，并對預(yù)測進行修正[4]。

設(shè)k時刻的被估計狀態(tài)Xk受系統(tǒng)噪聲W驅(qū)動，驅(qū)動機理由下述狀態(tài)方程描述：

Xk=Φk/k-1Xk-1+Γk-1Wk-1

(5)

量測方程為：

Zk=HkXk+Vk

(6)

式中，Φk/k-1為tk-1時刻的一步轉(zhuǎn)移矩陣,Γk-1為系統(tǒng)噪聲驅(qū)動陣,Hk為量測陣,Vk為量測噪聲序列,Wk-1為系統(tǒng)激勵噪聲序列。

狀態(tài)的一步預(yù)測為：

(7)

量測預(yù)測方程為：

(8)

(9)

濾波增益為：

(10)

一步預(yù)測均方誤差矩陣為：

(11)

估計均方誤差為：

Pk=(I-KkHk)Pk/k-1

(12)

3　濾波初值的選取方法

3.1　瞬時量測數(shù)據(jù)直接應(yīng)用

工程運算中，對于濾波初值的選取一般為濾波時刻的量測數(shù)據(jù)，而協(xié)方差矩陣P0也是基于量測誤差和時間間隔T計算得出，進而形成主對角線矩陣[5-7]。設(shè)濾波開始時間為t，將量測數(shù)據(jù)rt、βt、εt轉(zhuǎn)化為x、y、z三方向數(shù)據(jù)即xt、yt、zt。而vx、vy、vz可根據(jù)時間間隔T與xt、yt、zt得到。

(13)

此種方法存在較大的弊端。首先，這種實時應(yīng)用的初值具有較大的隨機性，量測值受量測誤差和系統(tǒng)誤差影響，使位置狀態(tài)初值以及計算速度的結(jié)果存在波動，及每次量測的狀態(tài)初值均不相同使每次濾波過程不盡相同存在偶然性，不能滿足實時性應(yīng)用的要求。其次，這種求得速度初值的方法所得到的速度分量與實際彈道諸元相差較多，進而使協(xié)方差矩陣P0的數(shù)值過大，影響濾波收斂的快速性。針對這兩類問題，提出如下方法。

3.2　濾波初始狀態(tài)中速度分量與位置分量的估計方法

在實際情況下，彈體在空中的運動受到重力、空氣阻力及橫風(fēng)等外界因素影響，其運動軌跡為曲線[8]。在曲線擬合的過程中，可選擇線性回歸模型，指數(shù)類或冪指數(shù)類等及其組合形式作為擬合算法的基礎(chǔ)。其中，線性回歸模型即多項式擬合方法計算復(fù)雜度低，計算速度快。同時，分析真實的彈道曲線，可發(fā)現(xiàn)彈道曲線形狀與變化規(guī)律與拋物線類似，從實際情況與模型的相似性出發(fā)，可選擇多項式進行曲線擬合。從上述兩點分析，將采用線性回歸模型進行彈道擬合并分析。在曲線擬合的計算過程中，常用到最小二乘法、Gauss消去法、三次樣條法和追趕法等方法，來對系數(shù)矩陣進行求解，進而得到多項式系數(shù)?；趶椀狼€的形狀和擬合彈道多項式計算的快速實時性，選取最小二乘法進行彈道多項式擬合。

設(shè)擬合多項式為n次冪多項式，其表達形式為：

f(x)=A1·xn+A2·xn-1+…+An·x+An+1

(14)

取m次量測數(shù)據(jù)(xi,yi)(m>n)，根據(jù)最小二乘法估計準則可得目標函數(shù)：

(15)

因目標函數(shù)為實際問題，其極小值必然存在，由極小值存在條件：

(16)

將上述條件方程轉(zhuǎn)化為矩陣表達形式：

(17)

即AX=b。

由數(shù)值分析方法可知，當(dāng)系數(shù)矩陣A非奇異時，則式(17)有唯一解向量X，X向量中的參數(shù)A1、A2、…、An+1，就是多項式各次冪的系數(shù)。在已知完整多項式的情況下，對所在時刻的速度v估計應(yīng)為所在時刻的多項式導(dǎo)數(shù)，即當(dāng)x=xm時所對應(yīng)的f′(xm)。

多項式f(x)的導(dǎo)數(shù)f′(x)表達式為：

f′(x)=nA1·xn-1+(n-1)A2·xn-2+…+An

(18)

將xm=t代入，便可獲得濾波的速度分量估計初值vx、vy、vz。所以根據(jù)多項式擬合得到的曲線方程，代入濾波起始時間就可估計出彈箭所在的速度分量。以濾波開始時刻的量測數(shù)據(jù)求出的(x,y,z)和速度分量估計初值(vx,vy,vz)作為濾波分量的估計初值(x,y,z,vx,vy,vz)。

4　仿真校驗

為驗證此種初始狀態(tài)選取方法的可行性和實用性，需進行計算機仿真計算。以某型火箭彈為例，通過6D彈道方程求解出彈道數(shù)據(jù)，將彈道數(shù)據(jù)轉(zhuǎn)化為極坐標的雷達量測數(shù)據(jù)。并通過隨機發(fā)生器產(chǎn)生均值為0的Gauss白噪聲作為要引入的雷達噪聲，加入到原始量測數(shù)據(jù)，產(chǎn)生受噪聲影響的雷達量測數(shù)據(jù)。

在仿真計算中，取雷達數(shù)據(jù)刷新間隔為0.1s，徑向距離上的方差σr為10m，方向角的噪聲方差σr為0.0015rad，俯仰角的噪聲方差σr為0.0015rad，即量測噪聲矩陣R的主對角元素值。模擬在火箭彈發(fā)射3.5s后雷達開始跟蹤目標測量，量測數(shù)據(jù)在40s開始對彈道數(shù)據(jù)進行濾波處理，持續(xù)監(jiān)控20s左右，至60s時停止濾波處理。

從圖1可看出，所估速度殘差隨著擬合多項式階數(shù)的增加先減少而后略有增加。雖然擬合數(shù)據(jù)時間長度不同，但一次多項式基本對應(yīng)殘差最大值，造成這種情況的原因在于直線模型與真實三方向軌跡情況存在很大差異，具有較大的模型誤差，估計效果差。由一次多項式至二次多項式時，殘差變化較大，證明擬合模型與真實軌跡狀況的匹配度較高，估計速度殘差減小。由二次多項式至五次多項式時，估計殘差卻略有增長，原因在于擬合數(shù)據(jù)是帶有量測噪聲的彈道測量數(shù)據(jù)。隨著多項式階數(shù)的增加，在擬合過程中數(shù)據(jù)點對擬合結(jié)果的影響將增大，也就是將量測噪聲所引起的誤差逐漸加入到了估計結(jié)果中，使得估計位置殘差略有增長但較為平緩。由圖1可知，隨著擬合數(shù)據(jù)時間長度的增加，所估速度殘差趨勢基本為先減小后升高。在二次多項式的擬合模型下，所估速度殘差最小并且數(shù)據(jù)長度為3s和5s時，基于二次多項式的速度估計殘差變化不大且準確性較高。

從圖2 ～圖4 可看出，應(yīng)用多項式估計后，三方向速度濾波初值估計殘差大幅度減少，濾波過程平滑，波動較小，可快速收斂。

從圖5 ～圖7 可看出，應(yīng)用多項式法估計狀態(tài)初值與量測數(shù)據(jù)直接應(yīng)用相比，其三方向上濾波前期的殘差幅值大幅度縮減，濾波收斂速度增加，殘差波動平緩，證明此種方法具有較高的實用性和精度。

5　結(jié)論

本文針對雷達量測的火箭彈彈道數(shù)據(jù)，通過對濾波前期的量測數(shù)據(jù)進行坐標轉(zhuǎn)換，對所量測的三方向數(shù)據(jù)進行多項式擬合，進而估計濾波起始時刻速度分量，以其作為濾波初始狀態(tài)。選取濾波前期擬合數(shù)據(jù)長度和擬合多項式階數(shù)作為變量，通過計算機仿真分析其對彈道參數(shù)估計精度的影響，最后選取最優(yōu)組合確定其為濾波初始狀態(tài)估計的最優(yōu)方法。通過濾波仿真，驗證此種初始狀態(tài)選取方法的可行性。由仿真結(jié)果可看出，量測數(shù)據(jù)直接應(yīng)用后，三方向濾波收斂時間在濾波開始后5.5s左右，而應(yīng)用此種估計方法的濾波過程基本在3.5s收斂，濾波收斂時間減少1/3且濾波幅值大幅度減少，滿足快速準確的要求，實用性強。

[1]秦永元, 張洪鉞, 汪叔華.卡爾曼濾波與組合導(dǎo)航原理[M].西安:西北工業(yè)大學(xué)出版社, 1998.

QIN Yong-yuan, ZHANG Hong-yue, WANG Shu-hua. Kalman filter and integrated navigation principle[M]. Xi’an: Northwestern Polytechnical University Press, 1998.

[2]韓子鵬.彈箭外彈道學(xué)[M].北京:北京理工大學(xué)出版社, 2014.

HAN Zi-peng. Exterior ballistics of rockets[M]. Beijing: Beijing Institute of Technology Press, 2014.

[3]李巖, 任睿, 王旭剛. 兩種卡爾曼濾波模型在修正彈彈道數(shù)據(jù)處理中的應(yīng)用比較[J].彈道學(xué)報, 2011, 23(1):27-30.

LI Yan, REN Rui,WANG Xu-gang. Application comparison of two Kalman filtering models used in trajectory data processing of correction projectile[J]. Journal of Ballistics, 2011, 23(1):27-30.

[4]閆小龍, 陳國光, 楊東. 抗野值卡爾曼濾波在火箭彈落點估計中的應(yīng)用[J].彈箭與制導(dǎo)學(xué)報, 2016, 36(3):94-98.

YAN Xiao-long, CHEN Guo-guang, YANG Dong. Application of Kalman filter restraining outliers in estimation of rockets impact point[J]. Journal of Projectiles, Rockets, Missiles and Guidance, 2016, 36(3):94-98.

[5]吳漢洲, 宋衛(wèi)東, 徐敬青. 基于多項式擬合的擴展卡爾曼濾波算法[J].計算機應(yīng)用, 2016,36(5):1455-1457+1463.

WU Han-zhou, SONG Wei-dong, XU Jing-qing. Ext-ended Kalman filtering algorithm based on polynomial fitting[J]. Journal of Computer Applications, 2016, 36(5):1455-1457+1463.

[6]Yan X L, Chen G G, Bai D Z. Application of adaptive Kalman filter in rocket impact point estimation[J]. Journal of Measurement Science and Instrumentation, 2015, 6(3):212-217.

[7]余小琴, 沈文苗. 擴展卡爾曼濾波算法初值選取方法[J].聲學(xué)與電子工程, 2012(1): 12-13+17.

YU Xiao-qin, SHEN Wen-miao. Extended Kalman filter algorithm initial value selection method[J]. Acoustics and Electronics Engineering, 2012(1):12-13+17.

[8]秦偉, 田曉麗, 劉超. 基于卡爾曼濾波的導(dǎo)彈姿態(tài)最優(yōu)控制法[J].機械工程與自動化,2016(1):183-185.

QIN Wei, TIAN Xiao-li, LIU Chao. Kalman filter based optimal control approach for attitude control of a missile[J]. Mechanical Engineering and Automation, 2016(1): 183-185.