卡爾曼彈道濾波狀態初值的最優估計方法研究*

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

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

0 引言

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

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

1 外彈道濾波模型

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

1.1 濾波狀態模型

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

(1)

則有系統狀態方程:

(2)

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

1.2 濾波量測方程

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

β=arctan(z/x)

(3)

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

(4)

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

2 卡爾曼彈道濾波方程

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

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

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

(5)

量測方程:

Zk=HkXk+Vk

(6)

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

狀態的一步預測

(7)

量測預測方程:

(8)

(9)

濾波增益:

(10)

一步預測均方誤差矩陣:

(11)

估計均方誤差

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

(12)

3 濾波初值的選取方法

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

(13)

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

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

在實際情況下,彈體在空中的運動受到重力,空氣阻力及橫風等外界因素影響,其運動軌跡為曲線[8]。分析真實的彈道曲線,可發現彈道曲線形狀與變化規律與拋物線類似,從實際情況與模型的相似性出發,可選擇多項式進行曲線擬合。從上述兩點分析,將采用線性回歸模型進行彈道擬合并分析。在曲線擬合的計算過程中,常用到最小二乘法,高斯消去法,三次樣條法和追趕法等方法,來對系數矩陣進行求解,進而得到多項式系數。基于彈道曲線的形狀和擬合彈道多項式計算的快速實時性,選取最小二乘法進行進行彈道多項式擬合。

設擬合多項式的為n次冪多項式,其表達形式為

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

(14)

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

(15)

因目標函數為實際問題,其極小值必然存在,由極小值存在條件:

(16)

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

(17)

即:AX=b。

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

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

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

(18)

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

4 仿真校驗

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

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

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

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

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

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

5 結論

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