超視距目標(biāo)跟蹤的卡爾曼濾波算法研究

朱嘉穎，游世勛

哈爾濱工程大學(xué)信息與通信工程學(xué)院，黑龍江 哈爾濱 150001

日益復(fù)雜的電磁作戰(zhàn)環(huán)境對(duì)雷達(dá)系統(tǒng)的探測(cè)及跟蹤技術(shù)提出了新的要求。特別是在對(duì)非線性目標(biāo)進(jìn)行無源探測(cè)和超視距跟蹤時(shí)，由于目標(biāo)定位數(shù)據(jù)的高度非線性，跟蹤算法初期的收斂性能會(huì)大打折扣?？柭鼮V波是公認(rèn)速度快且性能穩(wěn)定的濾波算法之一，近幾十年來，學(xué)者們針對(duì)非線性系統(tǒng)對(duì)其進(jìn)行了一系列改進(jìn)。其中擴(kuò)展卡爾曼濾波(EKF)、無跡卡爾曼濾波(UKF)和容積卡爾曼濾波(CKF)在許多工程場(chǎng)合得到了成熟運(yùn)用。這些算法常被用來處理探測(cè)得到的非線性量測(cè)，以增強(qiáng)雷達(dá)目標(biāo)跟蹤的精度。李炳榮等[1]提出一種采用修正增益的擴(kuò)展卡爾曼濾波算法，提高了單站目標(biāo)無源跟蹤的穩(wěn)定性和跟蹤精度。曲長(zhǎng)文[2]和LIU Changyun[3]均對(duì)UKF算法進(jìn)行改進(jìn)，提高了濾波精度和收斂速度。胡振濤[4]和Liu H[5]通過改進(jìn)CKF算法提高了目標(biāo)跟蹤算法性能。文獻(xiàn)LIU Changyun[6]、武勇[7]和BAO Shuida[8]對(duì)多種濾波算法進(jìn)行結(jié)合，以提高算法魯棒性。而Chavez-Garcia[9]又提出一種新的多傳感器多目標(biāo)跟蹤算法。

以上文獻(xiàn)均把研究重點(diǎn)落在了算法性能的改進(jìn)方面。另一方面，濾波初值的選取是否得當(dāng)同樣對(duì)目標(biāo)跟蹤效果發(fā)揮著重要作用[10]，但少有深入研究。文獻(xiàn)[1]、[2]、[4]、[8]中均未提到目標(biāo)初始狀態(tài)的確定。文獻(xiàn)[3]設(shè)定目標(biāo)狀態(tài)初始值為其期望值。文獻(xiàn)[5]在仿真時(shí)直接給定初始值且與目標(biāo)真實(shí)初始狀態(tài)相同。文獻(xiàn)[7]仿真時(shí)設(shè)定初始值與實(shí)際值有一定偏差。實(shí)際上，在工程中目標(biāo)真實(shí)運(yùn)動(dòng)狀態(tài)是無法直接得到的，均需利用量測(cè)來進(jìn)行估計(jì)。

目前，目標(biāo)跟蹤時(shí)大致獲取目標(biāo)初始位置普遍采用三點(diǎn)法(TPM)，但該方法對(duì)探測(cè)精度影響十分敏感，特別是在惡劣的電磁環(huán)境下對(duì)超視距目標(biāo)進(jìn)行跟蹤時(shí)，易導(dǎo)致量測(cè)不可靠(本文定義距離誤差大于0.5 km或角度誤差大于0.1°的量測(cè)為不可靠量測(cè))，此時(shí)利用TPM估算出的目標(biāo)運(yùn)動(dòng)初始狀態(tài)與真實(shí)狀態(tài)差距很大甚至遠(yuǎn)超出目標(biāo)物理性能限制，從而導(dǎo)致目標(biāo)跟蹤初期收斂速度變慢且濾波精度較差。這種現(xiàn)狀下，雷達(dá)往往放棄精準(zhǔn)跟蹤而等待目標(biāo)靠近，如此便失去了該段時(shí)間來對(duì)目標(biāo)進(jìn)行評(píng)估反應(yīng)。若在超視距目標(biāo)跟蹤時(shí)探測(cè)效果不理想的情況下依舊能得到較好的目標(biāo)跟蹤效果，提高跟蹤算法的前期收斂速度和精度，便可贏得前期這段寶貴時(shí)間來對(duì)目標(biāo)進(jìn)行預(yù)先識(shí)別和評(píng)估。為達(dá)到這一目的，本文對(duì)現(xiàn)有方法進(jìn)行改進(jìn)，提出了一種自適應(yīng)濾波初值確定方法，即投影修正法(PCM )。

1 傳統(tǒng)卡爾曼濾波算法

傳統(tǒng)卡爾曼濾波的系統(tǒng)方程如下：

X(k+1)=FX(k)+ΓW(k)

Z(k)=ΗX(k)+V(k)

(1)

式(1)為狀態(tài)轉(zhuǎn)移方程和量測(cè)方程。式中：k為離散時(shí)間，目標(biāo)在時(shí)刻k的狀態(tài)為X(k)；W(k)是均值為零、協(xié)方差為Q的高斯白噪聲；F為狀態(tài)轉(zhuǎn)移矩陣；Γ為噪聲驅(qū)動(dòng)矩陣。式中：Η為量測(cè)矩陣；V(k)為協(xié)方差為R的觀測(cè)噪聲序列。以下為濾波遞推過程：

1)分別預(yù)測(cè)目標(biāo)狀態(tài)及噪聲協(xié)方差矩陣：

2)卡爾曼濾波增益的求取

3)結(jié)合量測(cè)值Z(k+1)估計(jì)下一時(shí)刻的目標(biāo)狀態(tài)向量和噪聲協(xié)方差矩陣：

(2)

卡爾曼濾波算法是一種高斯噪聲背景下的最優(yōu)估計(jì)跟蹤算法。但在實(shí)際的跟蹤系統(tǒng)中，無論是目標(biāo)的運(yùn)動(dòng)狀態(tài)還是量測(cè)信息，往往都具有某些非線性特性，這種情況下，線性卡爾曼濾波的作用效果將受到限制，由此非線性的卡爾曼濾波算法應(yīng)運(yùn)而生。

2 非線性卡爾曼濾波算法

非線性系統(tǒng)描述為

式中：f(·)是非線性狀態(tài)轉(zhuǎn)移函數(shù)；h(·)是非線性觀測(cè)函數(shù)；W(k)和V(k)都是均值為零的高斯白噪聲，W(k)的協(xié)方差為Q，V(k)的協(xié)方差為R。

以下簡(jiǎn)單介紹3種常用的非線性卡爾曼濾波算法，分別為擴(kuò)展卡爾曼濾波、無跡卡爾曼濾波和容積卡爾曼濾波算法。

2.1 擴(kuò)展卡爾曼濾波

EKF的主要思路是充分利用非線性模型的特性，對(duì)其進(jìn)行局部線性化，進(jìn)而將非線性模型近似為線性模型。

將非線性系統(tǒng)函數(shù)展開成Taylor級(jí)數(shù)如下：

只保留等式右側(cè)的線性部分即可得到近似的線性系統(tǒng)模型如式(3)所示。其中F(k)和H(k)分別為f(·)和h(·)在k時(shí)刻的雅可比矩陣，n為狀態(tài)向量維數(shù)。

(3)

變換之后再通過線性卡爾曼濾波完成對(duì)目標(biāo)的濾波估計(jì)。

2.2 無跡卡爾曼濾波

UKF仍采用卡爾曼濾波的基本框架，其主要思想是以UT變換為基礎(chǔ)，選取采樣策略來模擬逼近非線性分布。其中UT變換指在確保不改變采樣點(diǎn)原分布均值和協(xié)方差的前提下，依照某種策略隨機(jī)選取Sigma 點(diǎn)集，并針對(duì)各點(diǎn)進(jìn)行非線性變換，變換后狀態(tài)分布的均值和協(xié)方差即可通過求取轉(zhuǎn)換后點(diǎn)集的統(tǒng)計(jì)變量來得到。具體變換過程如下：

無跡卡爾曼濾波算法通過對(duì)將非線性變換作用于sigma點(diǎn)集，再通過變換后的點(diǎn)集求取變換后的均值和協(xié)方差，求得的非線性變換后的均值和協(xié)方差至少具有2階精度(Taylor函數(shù)展開式)，可明顯降低非線性系統(tǒng)中的濾波誤差。

2.3 容積卡爾曼濾波

CKF是一種濾波精度更高的非線性濾波算法，該算法同無跡卡爾曼濾波一樣，基本思想也是通過采樣的方式來逼近狀態(tài)分布，同樣采用經(jīng)典卡爾曼濾波算法的算法迭代框架。但使其區(qū)別于無跡卡爾曼濾波的是對(duì)采樣點(diǎn)的選取遵循球面徑向積分原則：

對(duì)Pk|k進(jìn)行cholesky分解得到Sk|k，即

Pk|k=Sk|k(Sk|k)T

本文結(jié)合以上3種非線性卡爾曼濾波算法，對(duì)初值確定方法進(jìn)行仿真驗(yàn)證。

3 初值確定方法

在研究目標(biāo)跟蹤濾波算法時(shí)，現(xiàn)有大多文獻(xiàn)設(shè)定目標(biāo)初始狀態(tài)已知，忽略了確定初值的過程，或直接采用三點(diǎn)法粗略估計(jì)目標(biāo)初始狀態(tài)。事實(shí)上，初值確定的合理性明顯影響著目標(biāo)跟蹤的收斂速度和跟蹤精度。

3.1 三點(diǎn)法

設(shè)通過處理量測(cè)信息依次得到目標(biāo)在跟蹤初期的3組位置信息分別為s0,s1和s2，則目標(biāo)在前2個(gè)時(shí)刻的速度和初始加速度分別為：

3.2 投影修正法

無源探測(cè)技術(shù)的高隱蔽性和遠(yuǎn)距離作用范圍使其得到極大重視，但探測(cè)精度不高的問題也一直制約其發(fā)展，特別是在進(jìn)行機(jī)動(dòng)目標(biāo)超視距跟蹤時(shí)，探測(cè)量測(cè)的誤差進(jìn)一步增大。由式(2)可知，在濾波迭代過程中，若量測(cè)擾動(dòng)過大時(shí)，會(huì)帶來很大的新息誤差， 導(dǎo)致目標(biāo)狀態(tài)估計(jì)誤差明顯增大，濾波前期調(diào)整過程更加漫長(zhǎng)，無法快速收斂于真實(shí)值。經(jīng)過長(zhǎng)時(shí)間的迭代處理后，通過不斷調(diào)整卡爾曼濾波增益，最終實(shí)現(xiàn)目標(biāo)跟蹤濾波的收斂。

為解決上述問題，本文提出利用投影修正法對(duì)TPM粗略估算的目標(biāo)初始狀態(tài)進(jìn)行自適應(yīng)修正，使其滿足目標(biāo)的物理特性，確定更合理的目標(biāo)初值，從而減小濾波前期的慢收斂/發(fā)散現(xiàn)象。

在三維空間建立大地坐標(biāo)系，如圖1所示，將目標(biāo)速度‖vmax‖，分別向x-y-z3個(gè)坐標(biāo)軸投影。圖中灰色陰影部分為由于探測(cè)所得量測(cè)誤差導(dǎo)致的目標(biāo)速度方向波動(dòng)范圍，且陰影中速度矢量長(zhǎng)度不超過目標(biāo)運(yùn)動(dòng)最大速率。

圖1 速度分解示意

如果三點(diǎn)法估算出的速度矢量超出這一范圍，則需修正。分析圖1中幾何關(guān)系，求得目標(biāo)最大速度‖vmax‖在3個(gè)坐標(biāo)軸上的分量分別為：

vmax,x=‖vmax‖sinθ1cosθ2

vmax,y=‖vmax‖sinθ1sinθ2

vmax,z=‖vmax‖cosθ1

對(duì)3個(gè)分量值分別求取關(guān)于俯仰角θ1和方位角θ2的偏導(dǎo)數(shù)，得到如下結(jié)果：

在修正速度分量前需先對(duì)其有效性進(jìn)行判定，為防止因量測(cè)誤差擾動(dòng)而造成誤判，本文規(guī)定，i方向上的有效(無需修正)的速度分量vi和最大速度分量vmax,i間需滿足如下條件：

‖vi+Δvi(Δθ1,Δθ2)‖≤‖vmax,i‖

則速度分量的判決邊界為

Vi=‖vmax,i‖-max(‖Δvi‖)=

又由于

所以

Vi≈

依次對(duì)三點(diǎn)法估計(jì)得到的3個(gè)初始速度分量進(jìn)行有效性判決后，修正不可靠分量，這樣不僅充分利用了量測(cè)中攜帶的目標(biāo)運(yùn)動(dòng)信息，同時(shí)也得到了與先驗(yàn)知識(shí)匹配性更高的速度初值。具體修正方案如式(4)：

(4)

式中vc,i表示速度修正矢量vc的i方向分量，且i∈{x,y,z}。

同理可得到對(duì)初始加速度的修正方案：

式中：Ai為加速度在i方向上的判決邊界；ac,i表示加速度修正矢量ac的i方向分量，i∈{x,y,z}。算法實(shí)現(xiàn)框圖如圖2所示。

圖2 基于投影修正法的卡爾曼濾波流程圖

投影修正法的實(shí)現(xiàn)步驟如下：

1)根據(jù)對(duì)所測(cè)目標(biāo)的已有先驗(yàn)知識(shí)分析其物理性能，確定目標(biāo)運(yùn)動(dòng)最大速率‖vmax‖和最大加速度‖amax‖；

2)在有效范圍內(nèi)選取相應(yīng)修正值；

3)利用三點(diǎn)法粗略估計(jì)目標(biāo)初始運(yùn)動(dòng)狀態(tài)；

4)判別目標(biāo)初值有效性，并修正不可靠初值分量；

5)利用修正后的目標(biāo)初值開始迭代濾波。

并分別設(shè)定修正值‖vc‖和‖ac‖。再利用TPM求取目標(biāo)初始速度和加速度。最后對(duì)量測(cè)可靠性進(jìn)行判決，并對(duì)不可靠信息進(jìn)行修正，利用新的初始狀態(tài)進(jìn)行濾波。

4 仿真結(jié)果

本文采用Singer模型[11]，跟蹤模擬目標(biāo)運(yùn)動(dòng)軌跡，機(jī)動(dòng)時(shí)間設(shè)為[12]為60 s，對(duì)三維空間中的機(jī)動(dòng)目標(biāo)進(jìn)行跟蹤仿真。觀測(cè)站位置設(shè)置為[150,490,100]T，單位為km，距離量測(cè)相對(duì)誤差為0.001，角度量測(cè)誤差為0.1°。探測(cè)采樣周期T=0.5 s，采樣點(diǎn)數(shù)N=200。假設(shè)目標(biāo)與觀測(cè)站的相對(duì)速度不超過2馬赫(1馬赫≈340 m/s)[13]，相對(duì)加速度不超過10g(g=9.8 m/s2)，即0≤‖vc‖≤‖vmax‖=680，0≤‖ac‖≤‖amax‖=98。

場(chǎng)景1 假定目標(biāo)的初始位置在大地坐標(biāo)系下的坐標(biāo)為[630 km,778 km,316 km]T，距離觀測(cè)臺(tái)600 km，目標(biāo)初始速度矢量為[-100 m/s, 400 m/s, 200 m/s]T，加速度矢量為[5m/s2,-8m/s2,-5m/s2]T，并由此開始做勻加速運(yùn)動(dòng)。采用3種非線性卡爾曼濾波算法，并分別結(jié)合 PCM、TPM和真實(shí)值替代法(true value substituting method，TVSM)3種初值確定方法對(duì)目標(biāo)進(jìn)行跟蹤，x-y-z3個(gè)方向上的跟蹤絕對(duì)誤差(AD)隨采樣點(diǎn)數(shù)n的變化曲線如圖3所示。

(a)EKF在x方向的跟蹤誤差

(b)EKF在y方向的跟蹤誤差

(c)EKF在z方向的跟蹤誤差

(d)UKF在x方向的跟蹤誤差

(e)UKF在y方向的跟蹤誤差

(f)UKF在z方向的跟蹤誤差

(g)CKF在x方向的跟蹤誤差

(h)CKF在y方向的跟蹤誤差

(i)CKF在z方向的跟蹤誤差圖3 目標(biāo)與觀測(cè)臺(tái)相距600 km時(shí)跟蹤誤差對(duì)比

由圖3可以看到，在量測(cè)誤差影響下，當(dāng)n∈[0,80]時(shí)，直接采用TPM確定的初值誤差較大，導(dǎo)致對(duì)目標(biāo)的跟蹤濾波結(jié)果嚴(yán)重偏離真實(shí)位置，在各方向上的跟蹤誤差均明顯超過了PCM和TVSM。而采用PCM對(duì)初值進(jìn)行適當(dāng)修正后，跟蹤濾波誤差大幅減小，僅略高于真實(shí)值法。在n>80時(shí)，3種初值確定方法下的卡爾曼濾波均收斂至同一水平。由此可得出結(jié)論，PCM和TVSM相較TPM算法能明顯改善目標(biāo)跟蹤初期的跟蹤精度，在場(chǎng)景1設(shè)定的條件下，PCM和TVSM的收斂時(shí)間比TPM加快約80個(gè)采樣周期。而在工程應(yīng)用中，往往難以直接得到目標(biāo)實(shí)際初始狀態(tài)，所以與TVSM相比，在對(duì)機(jī)動(dòng)目標(biāo)進(jìn)行超視距跟蹤的現(xiàn)實(shí)場(chǎng)景下，PCM算法能夠發(fā)揮更有效的作用。

場(chǎng)景2 假定目標(biāo)的初始位置在大地坐標(biāo)系下的坐標(biāo)為[630 km,778 km,316 km]T，距觀測(cè)臺(tái)600 km，目標(biāo)初始速度矢量為[100 m/s,400 m/s,100 m/s]T，加速度矢量為[10 m/s2,-10 m/s2,10 m/s2]T，并由此開始做勻加速運(yùn)動(dòng)。采用PCM確定初值，并分別結(jié)合3種非線性卡爾曼濾波算法對(duì)目標(biāo)進(jìn)行跟蹤，進(jìn)行100次Monte-Carlo仿真，求取跟蹤軌跡的相對(duì)均方根誤差。在PCM算法中選取不同的修正值時(shí)對(duì)應(yīng)的誤差變化趨勢(shì)如圖4。

(a)EKF在PRMS

(b)EKF在PRMS50

(c)EKF在PRMS100

(d)UKF的PRMS

(e)UKF的PRMS50

(f)UKF的PRMS100

(g)CKF的PRMS

(h)CKF的PRMS50

(i)CKF的PRMS100圖4 目標(biāo)與觀測(cè)臺(tái)相距600 km時(shí)的相對(duì)誤差對(duì)比

場(chǎng)景3 假定場(chǎng)景2中目標(biāo)初始位置在大地坐標(biāo)系下的坐標(biāo)變?yōu)閇750 km,1 130 km,580 km]T，與觀測(cè)臺(tái)之間的距離為1 000 km，并由此開始做勻加速運(yùn)動(dòng)。在相同條件下進(jìn)行仿真，結(jié)果見圖5。

(a)EKF在PRMS

(b)EKF在PRMS50

(c)EKF在PRMS100

(d)UKF的PRMS

(e)UKF的PRMS50

(f)UKF的PRMS100

(g)CKF的PRMS

(h)CKF的PRMS50

(i)CKF的PRMS100圖5 目標(biāo)與觀測(cè)臺(tái)相距1 000 km時(shí)的相對(duì)誤差對(duì)比

圖4、5表現(xiàn)了當(dāng)選取不同的修正值‖vc‖和‖ac‖時(shí)，得到目標(biāo)跟蹤相對(duì)誤差的變化趨勢(shì)。PRMS為濾波前200個(gè)周期(即n∈[0,200])的相對(duì)誤差平均值，同理PRMS50表示n∈[50,200]時(shí)求得的PRMS，PRMS100表示n∈[100,200]時(shí)求得的PRMS。其中，圖(a)～(c)、圖(d)～(f)和圖(g)～(i)分別為運(yùn)用EKF、UKF和CKF算法求得的結(jié)果。

從圖4和圖5中PRMS的變化趨勢(shì)不難看出，當(dāng)采用PCM分別結(jié)合EKF、UKF和CKF進(jìn)行目標(biāo)跟蹤時(shí)，零值修正(即‖vc‖=0，‖ac‖=0)可明顯減小濾波初期的相對(duì)誤差值。從PRMS50和PRMS100的變化趨勢(shì)可看出，隨著算法繼續(xù)迭代，濾波相對(duì)誤差對(duì)修正值‖vc‖和‖ac‖的敏感度逐漸下降，算法后期收斂精度基本不受影響。由此可見，在選取PCM修正值時(shí)，零值修正能在保證算法最終精度基本不變的前提下得到更好的初始濾波效果，即目標(biāo)跟蹤的收斂時(shí)間至少縮短了50個(gè)采樣周期。

5 結(jié)論

本文對(duì)超視距無源探測(cè)情況下，有界范圍內(nèi)的非線性目標(biāo)跟蹤問題進(jìn)行研究，通過改進(jìn)初值選取方法對(duì)跟蹤濾波收斂速度和精度進(jìn)行改善。

1)提出一種適用于工程實(shí)踐的初值確定方法——投影修正法(PCM)。

2)結(jié)合3種非線性卡爾曼濾波算法，對(duì)比以往的TPM和TVSM兩種初值確定方法進(jìn)行仿真驗(yàn)證。仿真結(jié)果表明PCM可明顯提高對(duì)非線性目標(biāo)進(jìn)行超視距跟蹤時(shí)的卡爾曼濾波算法收斂速度。此外，采取零值修正的PCM算法在相同條件下，能取得更快的濾波收斂速度，且整體濾波跟蹤精度不下降。

3)實(shí)際工程應(yīng)用中，在對(duì)非線性目標(biāo)進(jìn)行超視距無源探測(cè)和跟蹤時(shí)，采用零值修正的PCM進(jìn)行初值確定可提高目標(biāo)跟蹤前期精度和收斂速度，便于快速準(zhǔn)確地鎖定目標(biāo)，為探測(cè)方贏取更多反應(yīng)時(shí)間。