卡爾曼彈道濾波狀態(tài)初值的最優(yōu)估計方法研究*

范 旭,陳國光,白敦卓,朱宜家

(1 中北大學機電工程學院,太原 030051;2 豫西工業(yè)集團有限公司,河南南陽 473000)

0 引言

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

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

1 外彈道濾波模型

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

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

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

(1)

則有系統(tǒng)狀態(tài)方程:

(2)

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

1.2 濾波量測方程

設雷達測量值為斜距r、方位角β和高低角ε,雷達坐標系為球坐標系,可得雷達測量值與地面坐標系的關系[3]:

β=arctan(z/x)

(3)

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

(4)

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

2 卡爾曼彈道濾波方程

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

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

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

(5)

量測方程:

Zk=HkXk+Vk

(6)

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

狀態(tài)的一步預測

(7)

量測預測方程:

(8)

(9)

濾波增益:

(10)

一步預測均方誤差矩陣:

(11)

估計均方誤差

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

(12)

3 濾波初值的選取方法

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

(13)

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

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

在實際情況下,彈體在空中的運動受到重力,空氣阻力及橫風等外界因素影響,其運動軌跡為曲線[8]。分析真實的彈道曲線,可發(fā)現(xiàn)彈道曲線形狀與變化規(guī)律與拋物線類似,從實際情況與模型的相似性出發(fā),可選擇多項式進行曲線擬合。從上述兩點分析,將采用線性回歸模型進行彈道擬合并分析。在曲線擬合的計算過程中,常用到最小二乘法,高斯消去法,三次樣條法和追趕法等方法,來對系數(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)

將上述條件方程轉化為矩陣表達形式如下:

(17)

即:AX=b。

由數(shù)值分析方法可知,當系數(shù)矩陣A非奇異時,則上式有唯一解向量X。X向量中的參數(shù)就是多項式各次冪的系數(shù)。將擬合數(shù)據(jù)中xm=t代入完整多項式就可估計出濾波時刻彈箭所在的位置分量x,y,z。在已知完整多項式的情況下,對所在時刻的速度v估計應為所在時刻的多項式導數(shù),即當x=xm時所對應的f′(xm)。

多項式f(x)的導數(shù)f′(x)表達式為:

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

(18)

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

4 仿真校驗

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

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

多項式的擬合精度受擬合數(shù)據(jù)點個數(shù)及擬合模型與實際軌跡契合度等方面影響。所以將從采用數(shù)據(jù)點的時間長度,多項式階數(shù)兩方面來分析其變化對所估初值精度的影響。

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

圖3中可看出隨數(shù)據(jù)點數(shù)的增加,基于二次多項式的估計殘差逐漸減小,而到使用濾波前5 s數(shù)據(jù)時殘差卻有所增長。其原因在于,當數(shù)據(jù)時間長度在1 s和4.5 s之間時,隨著數(shù)據(jù)點的增多,真實彈道與拋物線的相似度逐漸提高,使模型誤差逐漸減小,殘差隨之下降。但真實彈道雖與拋物線相似,但其軌跡為非對稱性,并不是二次多項式完全契合的拋物線,所以當數(shù)據(jù)時間長度再次增加時,這種差異性會逐漸凸顯,造成殘差增大。所以擬合數(shù)據(jù)時間長度選取為4.5 s。

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

5 結論

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