基于SNVA的機(jī)動(dòng)目標(biāo)狀態(tài)估計(jì)

喬雪,白亞騰

(中國(guó)礦業(yè)大學(xué) 信息與電氣工程學(xué)院,江蘇 徐州,221116)

摘要:利用位置預(yù)測(cè)估計(jì)值與位置濾波估計(jì)值之間的偏差進(jìn)行加速度方差自適應(yīng)調(diào)節(jié),提出一種基于狀態(tài)噪聲方差自適應(yīng)(SNVA)的機(jī)動(dòng)目標(biāo)狀態(tài)估計(jì)方法。采用SNVA對(duì)目標(biāo)加速度噪聲方差進(jìn)行自適應(yīng)調(diào)整,實(shí)現(xiàn)了對(duì)當(dāng)前統(tǒng)計(jì)模型的改進(jìn);利用擴(kuò)展卡爾曼濾波算法對(duì)目標(biāo)狀態(tài)進(jìn)行估計(jì)。仿真結(jié)果表明,基于SNVA的擴(kuò)展卡爾曼濾波算法對(duì)機(jī)動(dòng)目標(biāo)速度估計(jì)的絕對(duì)誤差小于0.1 m/s,加速度估計(jì)的絕對(duì)誤差小于0.1 m/s2,能夠?qū)C(jī)動(dòng)目標(biāo)的狀態(tài)進(jìn)行準(zhǔn)確的估計(jì)。

關(guān)鍵詞:機(jī)動(dòng)目標(biāo);當(dāng)前統(tǒng)計(jì)模型;擴(kuò)展卡爾曼濾波;狀態(tài)估計(jì)

收稿日期:2014-02-24

基金項(xiàng)目:教育部博士學(xué)科點(diǎn)專項(xiàng)科研

作者簡(jiǎn)介:喬雪(1990- ),女,碩士研究生,研究方向?yàn)閺?fù)雜系統(tǒng)分析與控制。E-mail:1219771581@qq.com。

中圖分類號(hào):TJ765.4文獻(xiàn)標(biāo)識(shí)碼:A

ManeuveringTargetStateEstimationBasedonSNVA

QIAOXue,BAIYa-teng

(SchoolofInformationandElectricalEngineering,ChinaUniversityofMining&Technology,Xuzhou221116,China)

Abstract:By using the difference between the location forecast estimation and the corrected location estimation,the acceleration variance was adjusted adaptively.A maneuvering target state estimation method based on state noise variance adaptive(SNVA)was proposed.The SNVA was used to adaptively adjust the system noise variance in target state estimation system,which can improve the current statistical model.The extended Kalman filter algorithm was used to estimate the target state.Simulation results show that the extended Kalman filter algorithm based on SNVA can estimate the target state accurately.The absolute estimation error of velocity is less than 0.1 m/s,and the absolute estimation error of acceleration is less than 0.1 m/s2.The target state can be accurately estimated by the proposed method.

Keywords:maneueringtarget;currentstatisticalmodel;extendedKalmanfilter;stateestimation

對(duì)于機(jī)動(dòng)目標(biāo)狀態(tài)估計(jì)問(wèn)題的研究是目前自動(dòng)控制領(lǐng)域的熱點(diǎn)課題之一。將濾波算法與機(jī)動(dòng)目標(biāo)模型[1-2]相結(jié)合可以對(duì)機(jī)動(dòng)目標(biāo)的狀態(tài)進(jìn)行估計(jì)。在濾波算法的發(fā)展歷史中有很多學(xué)者提出了不同的濾波算法,其中,卡爾曼濾波理論是由Kalman于1960年提出的。對(duì)于線性系統(tǒng),運(yùn)用卡爾曼濾波算法可以得到系統(tǒng)狀態(tài)的最小均方差估計(jì)。對(duì)發(fā)生機(jī)動(dòng)運(yùn)動(dòng)的非線性系統(tǒng)通常采用擴(kuò)展卡爾曼濾波(extendedKalmanfilter,EKF)算法[4-5]。EKF算法首先將非線性狀態(tài)方程通過(guò)泰勒級(jí)數(shù)展開(kāi)進(jìn)行線性化處理,線性化后所得到的濾波算法通常被用于對(duì)機(jī)動(dòng)目標(biāo)的狀態(tài)進(jìn)行估計(jì)。

在對(duì)機(jī)動(dòng)目標(biāo)的狀態(tài)進(jìn)行估計(jì)之前,首先應(yīng)該建立機(jī)動(dòng)目標(biāo)的模型。周宏仁提出的“當(dāng)前”統(tǒng)計(jì)(currentstatistical,CS)模型本質(zhì)上是一種時(shí)間相關(guān)模型,機(jī)動(dòng)目標(biāo)“當(dāng)前”加速度的預(yù)測(cè)值即為均值?！爱?dāng)前”統(tǒng)計(jì)模型是目前最常用、效果較好的一類實(shí)用模型,但是該模型也存在一些局限性,主要表現(xiàn)在以下2個(gè)方面:①濾波估計(jì)的快速性和準(zhǔn)確性均會(huì)受限于預(yù)先給出的加速度預(yù)測(cè)極限值;②如果目標(biāo)實(shí)際機(jī)動(dòng)的狀態(tài)與估計(jì)狀態(tài)不符,則會(huì)造成濾波發(fā)散,對(duì)機(jī)動(dòng)目標(biāo)的狀態(tài)估計(jì)結(jié)果造成影響。

因此,針對(duì)上述局限性①的問(wèn)題,本文在基于CS模型和EKF算法的機(jī)動(dòng)目標(biāo)狀態(tài)估計(jì)基礎(chǔ)上,提出一種基于狀態(tài)噪聲方差自適應(yīng)(statenoisevarianceadaptive,SNVA)的機(jī)動(dòng)目標(biāo)狀態(tài)估計(jì)方法。該方法的基本思想是:通過(guò)濾波對(duì)機(jī)動(dòng)目標(biāo)的位置進(jìn)行估計(jì),并求得位置濾波估計(jì)值與預(yù)測(cè)值之間的偏差,進(jìn)而可以計(jì)算偏差在采樣時(shí)間內(nèi)的變化率,用所得的位置偏差變化率對(duì)當(dāng)前時(shí)刻的加速度方差進(jìn)行修正。采用這種方法是為了避免由于對(duì)加速度的極限值設(shè)定不當(dāng)而造成估計(jì)精度降低的問(wèn)題。與仿真結(jié)果的對(duì)比證明了這種方法在對(duì)強(qiáng)機(jī)動(dòng)目標(biāo)的狀態(tài)進(jìn)行估計(jì)時(shí)比傳統(tǒng)的EKF濾波算法更加有效。

1基于“當(dāng)前”統(tǒng)計(jì)模型的機(jī)動(dòng)目標(biāo)狀態(tài)估計(jì)

假設(shè)機(jī)動(dòng)目標(biāo)系統(tǒng)的離散狀態(tài)空間模型為

X(k)=Φ[X(k-1),k-1]+

Γ[X(k-1),k-1]W(k-1)

(1)

Y(k)=H[X(k),k]+V(k)

(2)

式中:X(k)是n維狀態(tài)向量;Y(k)是m維觀測(cè)向量;W(k-1)和V(k)分別是k-1時(shí)刻和k時(shí)刻的系統(tǒng)噪聲向量;Γ[X(k-1),k-1]是n×p維系統(tǒng)噪聲輸入矩陣;H[X(k),k]是m×n維觀測(cè)矩陣。

①系統(tǒng)狀態(tài)一步預(yù)測(cè)估計(jì)方程。

(3)

(4)

Γ(k,k-1)=Γ[X(k-1/k-1),k-1]

(5)

(6)

②一步預(yù)測(cè)估計(jì)誤差方程。

P(k|k-1)=Φ(k,k-1)P(k-1/k-1)×

ΦT(k,k-1)+Γ(k,k-1)Q(k)ΓT(k,k-1)

(7)

式中:P(k-1/k-1)是后驗(yàn)估計(jì)誤差協(xié)方差矩陣,Q(k)為系統(tǒng)狀態(tài)噪聲的方差陣,計(jì)算公式為

(8)

(9)

式中:amax,a-max分別為預(yù)先設(shè)定的加速度上、下限。

至此,根據(jù)式(1)和式(2)以及擴(kuò)展卡爾曼濾波方程,就可以得到目標(biāo)的狀態(tài)估計(jì)。圖1給出了擴(kuò)展卡爾曼濾波算法的結(jié)構(gòu)框圖,由于系統(tǒng)的量測(cè)方程在直角坐標(biāo)下是非線性的,則應(yīng)將量測(cè)方程中的觀測(cè)矩陣進(jìn)行線性化處理:

(10)

圖1中,Q(k)為系統(tǒng)狀態(tài)噪聲W(k)的方差,R(k)為觀測(cè)噪聲V(k)的方差,K(k)為卡爾曼濾波增益。

圖1　擴(kuò)展卡爾曼濾波算法的結(jié)構(gòu)框圖

在利用擴(kuò)展卡爾曼濾波進(jìn)行狀態(tài)估計(jì)的過(guò)程中,一方面要求預(yù)先給定機(jī)動(dòng)目標(biāo)加速度的極限值amax和a-max,這個(gè)參數(shù)不能自適應(yīng)調(diào)節(jié),因此這種狀態(tài)估計(jì)算法的估計(jì)能力有限;另一方面,如果缺乏必要的先驗(yàn)信息,機(jī)動(dòng)目標(biāo)加速度的極限值很難確定,往往與實(shí)際系統(tǒng)模型不匹配,因此造成狀態(tài)估計(jì)的精度大大降低。

2狀態(tài)噪聲方差的自適應(yīng)性

由式(6)可知,在CS模型中狀態(tài)噪聲方差取決于機(jī)動(dòng)目標(biāo)加速度的極限值amax和a-max。當(dāng)加速度極限值的取值較大時(shí),較大的噪聲方差能夠保證對(duì)目標(biāo)機(jī)動(dòng)的快速響應(yīng),但是會(huì)造成估計(jì)精度的降低;當(dāng)加速度極限值的取值較小時(shí),狀態(tài)估計(jì)精度有所提高,但是系統(tǒng)對(duì)機(jī)動(dòng)目標(biāo)的響應(yīng)速度較慢。因此,系統(tǒng)不能實(shí)時(shí)反映目標(biāo)的真實(shí)機(jī)動(dòng)情況。為了擴(kuò)大基于CS模型的狀態(tài)估計(jì)算法在機(jī)動(dòng)目標(biāo)狀態(tài)估計(jì)中的應(yīng)用范圍,提高濾波精度,將上述狀態(tài)估計(jì)算法進(jìn)行改進(jìn),提出基于狀態(tài)噪聲方差自適應(yīng)(state noise variance adaptive,SNVA)的狀態(tài)估計(jì)算法。該算法利用位置觀測(cè)值和位置濾波估計(jì)值之間偏差的變化率對(duì)狀態(tài)噪聲方差進(jìn)行實(shí)時(shí)調(diào)整,從而避免加速度上、下限對(duì)濾波估計(jì)的影響。

在第k次采樣周期T(T是指相鄰2次實(shí)測(cè)之間的時(shí)間間隔)內(nèi)有:

(11)

(12)

(13)

(14)

(15)

(16)

式中:μ是一個(gè)正常數(shù)。聯(lián)立式(11)～式(16),可得加速度狀態(tài)噪聲方差:

(17)

圖2　基于SNVA的機(jī)動(dòng)目標(biāo)狀態(tài)估計(jì)結(jié)構(gòu)圖

3仿真研究

3.1　目標(biāo)機(jī)動(dòng)狀態(tài)估計(jì)

實(shí)驗(yàn)主要考察導(dǎo)彈在鉛垂面攻擊目標(biāo)的情況,vM,vT分別為導(dǎo)彈和目標(biāo)的瞬時(shí)速率;θ為彈道傾角;θT為目標(biāo)的高低角;xM,yM和xT,yT分別表示導(dǎo)彈和目標(biāo)在x和y方向上的位移;比例系數(shù)K的取值在10到15之間。導(dǎo)彈與目標(biāo)在地面坐標(biāo)系中的位置關(guān)系如圖3所示,下標(biāo)d表示地面坐標(biāo)系。

圖3　導(dǎo)彈與目標(biāo)在地面坐標(biāo)系中的關(guān)系

目標(biāo)的運(yùn)動(dòng)模型為

(18)

(19)

比例導(dǎo)引時(shí)導(dǎo)彈與目標(biāo)相對(duì)運(yùn)動(dòng)的方程組為

(20)

(21)

式中:r為導(dǎo)彈到目標(biāo)的相對(duì)距離,q是目標(biāo)線方位角,假設(shè)地面水平,則θT≈0。設(shè)目標(biāo)是機(jī)動(dòng)的,且存在未知的擾動(dòng)量ΔθT,ΔvT;將其代入式(18)和式(19),忽略由于ΔθT,ΔvT的存在而引起的Δr,可得到擾動(dòng)方程組:

(22)

令w1=ΔθT,w2=ΔvT,整理導(dǎo)彈與目標(biāo)運(yùn)動(dòng)方程組,得到系統(tǒng)的狀態(tài)方程和輸出方程:

(23)

(24)

式中:

其中:g為重力加速度,?為俯仰角,FP為發(fā)動(dòng)機(jī)推力,Fx為迎面阻力,Fy為升力,m為導(dǎo)彈的質(zhì)量,α為攻角,Mz為俯仰力矩,Jz為導(dǎo)彈繞Ozd軸轉(zhuǎn)動(dòng)的轉(zhuǎn)動(dòng)慣量,ωz為導(dǎo)彈繞Ozd軸的旋轉(zhuǎn)角速度。

根據(jù)所設(shè)計(jì)的模型采用Monte Carlo仿真方法進(jìn)行計(jì)算機(jī)仿真,取采樣次數(shù)N=100,得到機(jī)動(dòng)目標(biāo)沿x軸和y軸方向的速度估計(jì)誤差均值曲線,如圖4和圖5所示,圖中,Δv表示速度誤差,在實(shí)驗(yàn)中采樣周期T=2 s。

圖4　速度誤差曲線

由圖4可以看出,在100次的采樣結(jié)果中,x軸方向和y軸方向的速度濾波估計(jì)值與真實(shí)值之間的誤差均小于0.1 m/s。因此可以說(shuō)明,經(jīng)過(guò)SNVA自適應(yīng)處理后的機(jī)動(dòng)狀態(tài)估計(jì)方法能夠較為準(zhǔn)確地反映目標(biāo)的真實(shí)運(yùn)動(dòng)狀態(tài)。

在對(duì)目標(biāo)濾波加速度進(jìn)行的估計(jì)中,通過(guò)計(jì)算機(jī)選取400次的采樣結(jié)果,結(jié)合目標(biāo)的真實(shí)運(yùn)動(dòng)加速度求出加速度估計(jì)均方根誤差σa,并將結(jié)果擬合成曲線,如圖5所示。σa的具體計(jì)算公式如下[10]:

(25)

圖5　加速度均方根誤差曲線

由圖5可以看出,在對(duì)目標(biāo)x方向以及y方向上加速度進(jìn)行的估計(jì)中,濾波后的加速度估計(jì)值與目標(biāo)加速度真實(shí)值之間的均方根誤差穩(wěn)定在一個(gè)比較小的范圍內(nèi),說(shuō)明估計(jì)值與真實(shí)值之間的偏差較小。因此可以說(shuō)明,基于SNVA的狀態(tài)估計(jì)算法對(duì)機(jī)動(dòng)目標(biāo)的加速度能夠進(jìn)行比較準(zhǔn)確的估計(jì)。

綜合上述的仿真結(jié)果可知,在對(duì)2個(gè)方向上機(jī)動(dòng)目標(biāo)運(yùn)動(dòng)狀態(tài)進(jìn)行的估計(jì)中,濾波估計(jì)誤差都在誤差允許的范圍內(nèi),并沒(méi)有出現(xiàn)濾波發(fā)散現(xiàn)象。由此可以得出以下結(jié)論:基于SNVA的機(jī)動(dòng)目標(biāo)狀態(tài)估計(jì)能夠得到目標(biāo)機(jī)動(dòng)狀態(tài)的近似無(wú)偏估計(jì),并且估計(jì)精度高。

3.2　對(duì)比仿真

為了進(jìn)一步驗(yàn)證基于SNVA的狀態(tài)估計(jì)算法在機(jī)動(dòng)目標(biāo)狀態(tài)估計(jì)中的優(yōu)勢(shì),將SNVA算法和擴(kuò)展卡爾曼濾波算法同時(shí)應(yīng)用于3.1節(jié)的仿真實(shí)驗(yàn)中進(jìn)行對(duì)比。在利用擴(kuò)展卡爾曼濾波對(duì)目標(biāo)狀態(tài)進(jìn)行估計(jì)時(shí)預(yù)先設(shè)置機(jī)動(dòng)頻率為0.001 Hz,加速度的極限值amax=1.0 km/s2和a-max=-0.5 km/s2。實(shí)驗(yàn)中得到目標(biāo)的加速度估計(jì)以及加速度估計(jì)誤差曲線,如圖6和圖7所示。圖中ax和ay分別表示目標(biāo)在x軸和y軸方向上的加速度瞬時(shí)值,N表示采樣次數(shù),采樣周期T=2 s。

圖6　加速度估計(jì)曲線

由圖6可以看出,在對(duì)目標(biāo)x軸方向和y軸方向上的加速度進(jìn)行估計(jì)的過(guò)程中,每次采樣的基于SNVA的加速度估計(jì)值都比擴(kuò)展卡爾曼濾波的加速度估計(jì)值更加接近真實(shí)值。因此可以說(shuō)明,在機(jī)動(dòng)目標(biāo)的狀態(tài)估計(jì)問(wèn)題中,基于SNVA的狀態(tài)估計(jì)算法與擴(kuò)展卡爾曼濾波算法相比能更加準(zhǔn)確快速地對(duì)目標(biāo)狀態(tài)進(jìn)行估計(jì)。

目標(biāo)的加速度誤差估計(jì)曲線如圖7所示,圖中Δax和Δay分別表示目標(biāo)在x軸和y軸方向上的加速度誤差估計(jì),采樣周期T=2 s。

圖7　加速度估計(jì)誤差曲線

由圖7可以看出,在對(duì)目標(biāo)的加速度進(jìn)行估計(jì)的過(guò)程中,每個(gè)采樣周期內(nèi)基于SNVA算法的加速度估計(jì)誤差都明顯小于擴(kuò)展卡爾曼濾波的估計(jì)誤差,誤差均值在0附近波動(dòng),幅值不大,并沒(méi)有發(fā)生濾波發(fā)散。由此說(shuō)明,在機(jī)動(dòng)目標(biāo)的狀態(tài)估計(jì)問(wèn)題中,基于SNVA的狀態(tài)估計(jì)算法比擴(kuò)展卡爾曼濾波更能準(zhǔn)確地對(duì)目標(biāo)的狀態(tài)進(jìn)行估計(jì)。

4結(jié)論

本文提出的SNVA算法避免了在機(jī)動(dòng)目標(biāo)狀態(tài)估計(jì)問(wèn)題中預(yù)先設(shè)定的加速度極限值對(duì)于狀態(tài)估計(jì)的影響。該方法的特點(diǎn)主要體現(xiàn)在以下2個(gè)方面:

①當(dāng)目標(biāo)做機(jī)動(dòng)性較強(qiáng)的機(jī)動(dòng)運(yùn)動(dòng)時(shí),SNVA可以對(duì)目標(biāo)的狀態(tài)進(jìn)行近似無(wú)偏估計(jì);

②避免了預(yù)先對(duì)加速度極限值的設(shè)定,能夠?qū)崟r(shí)調(diào)整加速度極限范圍。

因此,在實(shí)際工程對(duì)跟蹤目標(biāo)的狀態(tài)估計(jì)的應(yīng)用中,SNVA的實(shí)用性較強(qiáng),估計(jì)精度高,跟蹤效果很好,并且大大拓寬了對(duì)機(jī)動(dòng)目標(biāo)的機(jī)動(dòng)跟蹤范圍。

參考文獻(xiàn)

[1]蘭義華,任浩征,張勇,等.一種基于“當(dāng)前”模型的改進(jìn)卡爾曼濾波算法.山東大學(xué)學(xué)報(bào),2012,42(5):12-17.

LANYi-hua,RENHao-zheng,ZHANGYong,etal.AnimprovedKalmanfilteralgorithmbasedonthe“current”model.JournalofShandongUniversity,2012,42(5):12-17.(inChinese)

[2]LIUJian-shu,LIRen-hou,LIUYun-long.Interactingmultiplemodelalgorithmbasedonthecurrentstatisticalmodel.SystemsEngineeringandElectronics,2008,30(7):1 351-1 354.

[3]WANGJin-hua,CAOJie,WUDi,etal.Anobjecttrackingalgorithmbasedonthe"current"statisticalmodelandthemulti-featurefusion.JournalofSoftware,2012,7(9):2 000-2 008.

[4]常思江,王中原,牛春峰.基于卡爾曼濾波的彈箭飛行狀態(tài)估計(jì)方法.彈道學(xué)報(bào),2010,22(3):94-98.

CHANGSi-jiang,WANGZhong-yuan,NIUChun-feng.EstimationapproachofflightstateforguidedprojectilesbasedonKalmanfilter.JournalofBallistics,2010,22(3):94-98.(inChinese)

[5]史金光,劉猛,曹成壯,等.彈道修正落點(diǎn)預(yù)報(bào)方法研究.彈道學(xué)報(bào),2014,26(2):29-33.

SHIJin-guang,LIUMeng,CAOCheng-zhuang,etal.Researchonmethodoffallingpointpredictionfortrajectorycorrectionprojectile.JournalofBallistics,2014,26(2):29-33.(inChinese)

[6]周宏仁,敬忠良.機(jī)動(dòng)目標(biāo)跟蹤.北京:國(guó)防工業(yè)出版社,1991.

ZHOUHong-ren,JINGZhong-liang.Trackingofmaneuveringtargets.Beijing:NationalDefenceIndustryPress,1991.(inChinese)

[7]王以標(biāo),徐毓.基于當(dāng)前統(tǒng)計(jì)模型的改進(jìn)自適應(yīng)濾波算法.火力與指揮控制,2010,35(5):86-89.

WANGYi-biao,XUYu.Studyonimprovedadaptivefilteringalgorithmbasedoncurrentstatisticalmodel.FireControl&CommandControl,2010,35(5):86-89.(inChinese)

[8]徐琰珂,梁曉庚,賈曉洪.基于ANFIS的自適應(yīng)機(jī)動(dòng)目標(biāo)狀態(tài)估計(jì)算法.系統(tǒng)工程與電子技術(shù),2013,35(2):250-255.

XUYan-ke,LIANGXiao-geng,JIAXiao-hong.AdaptivemaneuveringtargetstateestimationalgorithmbasedonANFIS.SystemsEngineeringandElectronics,2013,35(2):250-255.(inChinese)

[9 ]余躍,劉藻珍.擴(kuò)展卡爾曼濾波器在導(dǎo)彈制導(dǎo)系統(tǒng)中的應(yīng)用與仿真//系統(tǒng)仿真技術(shù)及其應(yīng)用學(xué)術(shù)交流會(huì),合肥:中國(guó)科學(xué)技術(shù)大學(xué)出版社,2003:397-400.

YUYue,LIUZao-zhen.SimulationofhomingmissilesystemusingextendedKalmanfilter//AcademicCommunicationSystemSimulationTechnologyanditsApplication.Hefei:PressofUniversityofScienceandTechnologyofChina,2003:397-400.(inChinese)

[10] 楊宏,李亞安,李國(guó)輝.一種改進(jìn)擴(kuò)展卡爾曼濾波新方法.計(jì)算機(jī)工程與應(yīng)用,2010,46(19):18-20.

YANGHong,LIYa-an,LIGuo-hui.AnewmethodofimprovedextendedKalmanfilter.ComputerEngineeringandApplications,2010,46(19):18-20.(inChinese)