一種基于期望最大化去偏轉(zhuǎn)換量測濾波的目標(biāo)跟蹤算法

張連仲，王寶寶，王超塵

（1. 江蘇自動(dòng)化研究所，連云港 222006；2. 南京理工大學(xué)自動(dòng)化學(xué)院，南京 210094）

雷達(dá)作為一種傳統(tǒng)的目標(biāo)探測設(shè)備被廣泛應(yīng)用于目標(biāo)跟蹤。由于雷達(dá)傳感器的測量信息是在球面坐標(biāo)下給出的，這就導(dǎo)致了量測模型為非線性[1]。對于非線性系統(tǒng)的目標(biāo)跟蹤傳統(tǒng)算法有兩種，一種是擴(kuò)展卡爾曼濾波(EKF)，另一種是去偏轉(zhuǎn)換量測卡爾曼濾波(DCMKF)。后者是將目標(biāo)的坐標(biāo)系轉(zhuǎn)換到笛卡爾坐標(biāo)系，且利用了統(tǒng)計(jì)方法計(jì)算出轉(zhuǎn)換測量誤差的均值和協(xié)方差，然后利用標(biāo)準(zhǔn)卡爾曼濾波器對目標(biāo)進(jìn)行濾波[2]。

由于DCMKF 不存在線性近似，故其濾波精度較高，近年來已經(jīng)廣泛應(yīng)用于目標(biāo)跟蹤工程，并涌現(xiàn)出了許多關(guān)于該算法的改進(jìn)方法[3-7]。然而傳統(tǒng)非線性目標(biāo)跟蹤算法是假定在量測噪聲為高斯分布條件下的，當(dāng)量測噪聲出現(xiàn)異常時(shí)，傳統(tǒng)算法的跟蹤精度嚴(yán)重下降。文獻(xiàn)[4]提出了一種魯棒的去偏轉(zhuǎn)換量測卡爾曼濾波算法(RDCMKF)，該算法在量測數(shù)據(jù)丟失和目標(biāo)被遮擋兩種情況下可以得到較好的跟蹤精度，但是對于量測出現(xiàn)野值的情況下不能得到理想的跟蹤結(jié)果。文獻(xiàn)[5]提出了一種改進(jìn)的去偏轉(zhuǎn)換量測卡爾曼濾波算法(MDCMKF)，該算法每一時(shí)刻進(jìn)行一次狀態(tài)預(yù)測更新和兩次測量更新，使得改進(jìn)的噪聲轉(zhuǎn)換誤差協(xié)方差更加精確、噪聲更小，但是該算法在野值的環(huán)境下跟蹤精度仍然不佳。

為了解決野值干擾下的雷達(dá)目標(biāo)跟蹤問題，本文提出了一種期望最大化去偏轉(zhuǎn)換量測卡爾曼濾波算法（EMDCMKF），該算法首先對目標(biāo)量測進(jìn)行去偏轉(zhuǎn)換，對轉(zhuǎn)換后的量測噪聲協(xié)方差進(jìn)行解耦合，之后給其添加自適應(yīng)因子，利用期望最大化方法估計(jì)自適應(yīng)因子矩陣，從而修正了量測噪聲協(xié)方差，使其更接近于實(shí)際環(huán)境。仿真結(jié)果表明，提出的算法相較于傳統(tǒng)算法具有更好的跟蹤精度。

1 雷達(dá)量測模型

圖1 雷達(dá)量測坐標(biāo)系Fig.1 Radar measurement coordinate system

在實(shí)際目標(biāo)跟蹤測量系統(tǒng)中，雷達(dá)獲得的目標(biāo)信息Zk∈RP有斜距rk、高低角η k和方位角θk，雷達(dá)與目標(biāo)相對位置關(guān)系如圖1 所示。根據(jù)圖1，雷達(dá)量測方程表示如下：

其中（x k,y k,zk）T是目標(biāo)在直角坐標(biāo)系中相對應(yīng)的位置。分別表示斜距、方位角和高低角量測誤差，它們相互獨(dú)立，并且都是滿足均值為零、標(biāo)準(zhǔn)差分別為σ r,σ θ,ση的高斯白噪聲。

2 期望最大化去偏轉(zhuǎn)換卡爾曼濾波器

2.1 去偏轉(zhuǎn)換量測卡爾曼濾波（DCMKF）

雷達(dá)量測數(shù)據(jù)是建立在球坐標(biāo)系下的，可以通過坐標(biāo)轉(zhuǎn)換矩陣將其變換到笛卡爾坐標(biāo)系下：

結(jié)合式(1)(2)可推導(dǎo)出去偏轉(zhuǎn)換量測偏差μk和協(xié)方差陣Rk：

式中μk和Rk具體計(jì)算公式可參考文獻(xiàn)[5]，因此式(2)可以修正為

在量測噪聲協(xié)方差矩陣kR中，非對角元素對kR的影響比較小，可將其簡化為對角陣如式(6)所示。相比于可減少濾波器的計(jì)算量。

其中M為轉(zhuǎn)換矩陣，具體值為：

去偏轉(zhuǎn)換量測卡爾曼濾波算法(DCMKF)是在卡爾曼濾波框架的一種擴(kuò)展，它的主要步驟如下：

1） 一步預(yù)測。假定系統(tǒng)的狀態(tài)方程為：

去偏轉(zhuǎn)換更新后的量測方程為：

狀態(tài)和協(xié)方差的一步預(yù)測：

2） 測量更新。將去偏轉(zhuǎn)換更新后的量測和協(xié)方差帶入卡爾曼濾波中，得到：

DCMKF 算法可以解決高斯噪聲下的目標(biāo)跟蹤問題，然而當(dāng)實(shí)際環(huán)境中存在野值干擾時(shí)，DCMKF 算法的跟蹤精度下降，目標(biāo)狀態(tài)估計(jì)的效果變差。

2.2 期望最大化去偏轉(zhuǎn)換量測卡爾曼濾波

雷達(dá)量測噪聲出現(xiàn)野值時(shí)，其噪聲分布發(fā)生了變化，不再是滿足均值為零、標(biāo)準(zhǔn)差分別為σ r,σ θ,ση的高斯白噪聲，同樣經(jīng)過解耦去偏轉(zhuǎn)換后的量測噪聲協(xié)方差發(fā)生了變化。如果依然使用解耦去偏轉(zhuǎn)換量測后的來進(jìn)行跟蹤建模，會(huì)導(dǎo)致卡爾曼濾波器跟蹤精度下降。鑒于上述情況，引入自適應(yīng)因子kα來調(diào)整。由于去偏轉(zhuǎn)換后量測噪聲會(huì)產(chǎn)生均值kμ，均值μk已在量測方程修正中減去，為考慮算法復(fù)雜度，這里我們不考慮μk變化帶來的影響，因此將去偏轉(zhuǎn)換后的量測噪聲建模為：

其中N(·;·,· )是高斯分布，是去偏轉(zhuǎn)換后的量測噪聲，αk是自適應(yīng)因子，可以表示為：

之后采用期望最大化方法估計(jì)狀態(tài)和自適應(yīng)因子，將該方法分為兩個(gè)步驟。

2)通過以下公式計(jì)算：

通過式(18)和(19)，式(17)可以改寫為式(23)。

式(23)中aii表示kα的第i行i列的元素。通過式(18)-(23)不斷迭代更新kα，從而更新Kk，假設(shè)迭代次數(shù)為n，給出初始自適應(yīng)因子矩陣設(shè)置為單位矩陣。將利用式(23)獲取的帶入式(12)中，得到Kk的表達(dá)式為：

最后，利用迭代更新后得到的Kk通過式(13)(14)獲取目標(biāo)狀態(tài)和協(xié)方差更新。

3 仿真實(shí)驗(yàn)

本文通過對一個(gè)運(yùn)動(dòng)目標(biāo)的跟蹤實(shí)例來驗(yàn)證提出算法的有效性。假定該目標(biāo)做勻速運(yùn)動(dòng)，目標(biāo)初始狀態(tài)和協(xié)方差分別為：

雷達(dá)測量系統(tǒng)的采樣周期為T=0 .02s 。目標(biāo)運(yùn)動(dòng)的真實(shí)軌跡如圖2 所示。

圖2 目標(biāo)運(yùn)動(dòng)的真實(shí)軌跡Fig.2 The true trajectory of the target movement

采用等速(CV)模型對目標(biāo)進(jìn)行跟蹤，CV 模型為：

其中：

量測方程系數(shù)矩陣和噪聲協(xié)方差為：

本文通過位置和速度的平均均方根誤差（ARMSE）來評估濾波器的精度，其定義如下：

其中w.p. 表示相應(yīng)情況發(fā)生的概率。

在仿真實(shí)驗(yàn)中為了確保EMDCMKF 算法的實(shí)時(shí)性和穩(wěn)定性，需要確定合理的迭代次數(shù)。圖3 和圖4分別給出了迭代次數(shù)分別為1、2、3 次時(shí)EMDCMKF的位置均方根誤差和速度均方根誤差，表1 給出了不同迭代次數(shù)的均方根誤差和算法運(yùn)行時(shí)間?？梢钥闯?，雖然迭代1 次的均方根誤差最低，效果最好，但是第2、3 次之后算法的效果趨于穩(wěn)定，為保證算法的合理性，同時(shí)保證算法運(yùn)行效率，將迭代次數(shù)設(shè)置為2。

圖3 不同迭代次數(shù)位置均方根誤差Fig.3 Root mean square error of position at different iteration times

圖4 不同迭代次數(shù)速度均方根誤差Fig.4 Root mean square error of speed at different iteration times

表1 不同迭代次數(shù)EMDCMKF 算法性能Tab.1 Performance of EMDCMKF algorithm with different iteration times

圖5 和圖6 分別顯示了量測噪聲出現(xiàn)野值的情況下，EMDCMKF 和EKF、DCMKF 算法的位置和速度均方根誤差，表2 給出了三種算法的ARMSE 以及運(yùn)行時(shí)間?？梢钥闯?，量測噪聲出現(xiàn)野值時(shí)，EKF 和DCMKF 跟蹤性能較差，因?yàn)槌霈F(xiàn)野值時(shí)噪聲分布發(fā)生了變化，傳統(tǒng)算法不能夠?qū)α繙y噪聲進(jìn)行修正。提出的EMDCMKF 位置ARMSE 相比于EKF 和DCMKF分別減少了45.1%和52.5%，速度ARMSE 相比于EKF和DCMKF 分別減少了52.6%和66.8%，提出算法的跟蹤精度明顯優(yōu)于兩種傳統(tǒng)算法。但是由于EMDCMKF 存在迭代更新步驟，算法復(fù)雜度高于其他兩種算法，所以運(yùn)行時(shí)間略長。

圖5 三種算法的位置均方根誤差Fig.5 Root mean square error of position of the three algorithms

圖6 三種算法的速度均方根誤差Fig.6 Root mean square error of speed of the three algorithms

表2 三種算法性能Tab.2 Performance of the three algorithms

4 結(jié) 論

在實(shí)際雷達(dá)測量系統(tǒng)目標(biāo)跟蹤過程中往往會(huì)出現(xiàn)野值干擾，導(dǎo)致傳統(tǒng)的濾波器跟蹤精度下降。針對此問題，本文提出了一種EMDCMKF 算法。該算法給量測噪聲添加自適應(yīng)因子，利用期望最大化方法估計(jì)自適應(yīng)因子矩陣，修正了量測噪聲協(xié)方差。仿真結(jié)果表明，與擴(kuò)展卡爾曼濾波(EKF)和去偏轉(zhuǎn)換量測卡爾曼濾波(DCMKF)相比，在量測噪聲存在野值的情況下，本文提出的EMDCMKF 可以得到更好的跟蹤結(jié)果，有效地克服了野值干擾。但是由于EMDCMKF算法復(fù)雜度高，導(dǎo)致算法運(yùn)行時(shí)間長于傳統(tǒng)算法，因此后續(xù)的工作是考慮降低算法的復(fù)雜度以提高算法的效率和實(shí)時(shí)性。