一種基于預(yù)測(cè)值量測(cè)轉(zhuǎn)換的卡爾曼濾波跟蹤算法

王 旭1，程 婷，吳小平1，何子述

(1.西南電子電信技術(shù)研究所，成都610041；2.電子科技大學(xué) 電子工程學(xué)院，成都611731)

1 引 言

在雷達(dá)目標(biāo)跟蹤中，系統(tǒng)的量測(cè)信息通常在球坐標(biāo)系得到的，包括目標(biāo)的距離、方位角和俯仰角量測(cè)信息，而目標(biāo)運(yùn)動(dòng)狀態(tài)方程是建立在直角坐標(biāo)系的，顯然這些量測(cè)信息與目標(biāo)狀態(tài)向量呈非線性關(guān)系。因此，基于該信息的目標(biāo)跟蹤是一個(gè)非線性估計(jì)問題，無法直接采用經(jīng)典的卡爾曼濾波。

針對(duì)這一問題可采用擴(kuò)展卡爾曼濾波算法[1]，但該算法僅在量測(cè)誤差較小時(shí)能獲得較好的跟蹤效果，量測(cè)誤差增大時(shí)跟蹤精度下降甚至發(fā)散?；跇颖军c(diǎn)的算法，例如不敏卡爾曼濾波方法[2]和粒子濾波算法[3-4]，雖然能獲得較好的跟蹤效果，但是算法的運(yùn)算量較擴(kuò)展卡爾曼濾波算法大，尤其是粒子濾波算法。基于量測(cè)轉(zhuǎn)換的卡爾曼濾波(Converted Measurement Kalman Filter,CMKF)算法是解決這一問題的另一種思路，即將量測(cè)值從極坐標(biāo)轉(zhuǎn)換到直角坐標(biāo)系下，轉(zhuǎn)換為線性估計(jì)問題，再采用標(biāo)準(zhǔn)卡爾曼濾波算法進(jìn)行狀態(tài)估計(jì)。利用極坐標(biāo)和直角坐標(biāo)系之間的變換關(guān)系可以直接實(shí)現(xiàn)量測(cè)轉(zhuǎn)換[5]，但這種轉(zhuǎn)換方式是有偏的[6]。文獻(xiàn)[6]提出了去偏量測(cè)轉(zhuǎn)換(Debiased Converted Measurement,DCM)，采用加性去偏的量測(cè)轉(zhuǎn)換方式，即將傳統(tǒng)量測(cè)轉(zhuǎn)換結(jié)果減去其均值，從而保證了轉(zhuǎn)換量測(cè)的均值是無偏的，但是在求取量測(cè)轉(zhuǎn)換誤差統(tǒng)計(jì)特性時(shí)以量測(cè)值為條件。文獻(xiàn)[7]指出傳統(tǒng)量測(cè)轉(zhuǎn)換方法的偏差實(shí)質(zhì)是乘性的，因此提出通過乘以一個(gè)偏移因子實(shí)現(xiàn)去偏，提出了無偏量測(cè)轉(zhuǎn)換(Unbiased Converted Measurement,UCM)方法，但該方法在計(jì)算量測(cè)轉(zhuǎn)換誤差統(tǒng)計(jì)特性時(shí)存在兼容性問題[8]?；诖?，文獻(xiàn)[8]提出了改進(jìn)的UCM(Modified Unbiased Converted Measurement,MUCM)算法,雖然其克服了UCM的問題，但是在該算法中求取量測(cè)轉(zhuǎn)換誤差統(tǒng)計(jì)特性時(shí)以量測(cè)值為條件，這使得后續(xù)卡爾曼濾波算法中的增益矩陣與量測(cè)誤差相關(guān)，最終導(dǎo)致狀態(tài)估計(jì)結(jié)果的有偏性。上述對(duì)目標(biāo)位置量測(cè)信息的量測(cè)轉(zhuǎn)換方法，也是同時(shí)基于目標(biāo)位置和徑向速度量測(cè)信息進(jìn)行目標(biāo)跟蹤的基礎(chǔ)[9]。文獻(xiàn)[10]中將DCM和序貫濾波相結(jié)合依次處理目標(biāo)位置量測(cè)信息和徑向速度量測(cè)信息。文獻(xiàn)[11]中采用DCM處理目標(biāo)位置量測(cè)信息，并利用靜態(tài)融合方法處理徑向速度量測(cè)信息?？梢姡繙y(cè)轉(zhuǎn)換方法的研究對(duì)于同時(shí)具有位置和徑向速度量測(cè)時(shí)的目標(biāo)跟蹤問題也具有實(shí)質(zhì)性意義。

現(xiàn)有量測(cè)轉(zhuǎn)換方法主要的區(qū)別在于量測(cè)轉(zhuǎn)換方式以及計(jì)算量測(cè)轉(zhuǎn)換誤差統(tǒng)計(jì)特性的方法?？紤]到傳統(tǒng)量測(cè)轉(zhuǎn)換的偏差實(shí)質(zhì)為乘性的,本文將采用UCM的量測(cè)轉(zhuǎn)換方式，同時(shí)為克服UCM中計(jì)算量測(cè)轉(zhuǎn)換誤差統(tǒng)計(jì)特性方法的不兼容問題，對(duì)MUCM方法進(jìn)行改進(jìn)，提出以目標(biāo)預(yù)測(cè)值為條件計(jì)算量測(cè)轉(zhuǎn)換誤差的統(tǒng)計(jì)特性，克服濾波參數(shù)與量測(cè)誤差之間的相關(guān)性，從而保證最終目標(biāo)狀態(tài)估計(jì)結(jié)果的無偏性。將該量測(cè)轉(zhuǎn)換方法與卡爾曼濾波算法相結(jié)合，最終獲得了一種基于預(yù)測(cè)值量測(cè)轉(zhuǎn)換的卡爾曼濾波跟蹤算法。仿真結(jié)果表明了本文算法的有效性。

2 問題描述

考慮一個(gè)三維目標(biāo)跟蹤場(chǎng)景，令r、β和ε分別表示在球坐標(biāo)系下的真實(shí)目標(biāo)距離、方位角和俯仰角，用rm、βm和εm表示相應(yīng)的量測(cè)值，則真實(shí)值和量測(cè)值之間有下面的關(guān)系：

(1)

(2)

顯然，上述量測(cè)信息與目標(biāo)運(yùn)動(dòng)狀態(tài)之間呈非線性關(guān)系，量測(cè)轉(zhuǎn)換方法旨在笛卡爾坐標(biāo)系下建立如下量測(cè)方程：

Zc(k)=H(k)X(k)+Vc(k) 。

(3)

式中：Zc(k)為k時(shí)刻笛卡爾坐標(biāo)系下的轉(zhuǎn)換量測(cè)值;H(k)為量測(cè)矩陣;X(k)為目標(biāo)在k時(shí)刻的狀態(tài)向量;Vc(k)表示k時(shí)刻在笛卡爾坐標(biāo)系下的轉(zhuǎn)換量測(cè)誤差，通常為零均值高斯白噪聲，其相關(guān)矩陣為

(4)

利用非線性量測(cè)信息，不同的量測(cè)轉(zhuǎn)換方法將獲得不同的轉(zhuǎn)換量測(cè)值，并計(jì)算相應(yīng)的量測(cè)轉(zhuǎn)換誤差的統(tǒng)計(jì)特性。

3 基于預(yù)測(cè)值量測(cè)轉(zhuǎn)換的跟蹤算法

由于本文量測(cè)轉(zhuǎn)換方法是在MUCM算法基礎(chǔ)上提出的，因此為確保結(jié)構(gòu)的完整性，先對(duì)MUCM方法進(jìn)行簡(jiǎn)單回顧,隨后分析MUCM方法的不足之處，并給出基于預(yù)測(cè)值量測(cè)轉(zhuǎn)換的方法。

3.1 修正的UCM方法

在傳統(tǒng)的量測(cè)轉(zhuǎn)換方法中，根據(jù)球坐標(biāo)和直角坐標(biāo)系之間的轉(zhuǎn)換關(guān)系，容易得到[5]

(5)

將式(1)代入到式(5)中，然后對(duì)笛卡爾坐標(biāo)系下的轉(zhuǎn)換量測(cè)值x、y和z求數(shù)學(xué)期望可以得到[7]

(6)

可見，該量測(cè)轉(zhuǎn)換方法是有偏的。根據(jù)式(6)，給出如下無偏轉(zhuǎn)換量測(cè)(UCM)Zc(k)[7]為

(7)

MUCM方法采用上述量測(cè)轉(zhuǎn)換方式，在計(jì)算量測(cè)轉(zhuǎn)換誤差統(tǒng)計(jì)特性時(shí)，令[8]

(8)

(9)

(10)

(11)

相應(yīng)地，基于量測(cè)值的轉(zhuǎn)換量測(cè)誤差的協(xié)方差矩陣Rc按下式計(jì)算[8]：

(12)

3.2 基于預(yù)測(cè)值的量測(cè)轉(zhuǎn)換方法

MUCM在計(jì)算轉(zhuǎn)換量測(cè)誤差的均值和協(xié)方差矩陣的時(shí)候都是利用量測(cè)值來計(jì)算的。這種計(jì)算方法會(huì)導(dǎo)致協(xié)方差矩陣和量測(cè)噪聲相關(guān)，進(jìn)一步會(huì)使卡爾曼增益依賴于量測(cè)噪聲，使得經(jīng)過卡爾曼增益加權(quán)后的量測(cè)噪聲的均值不為零，最終導(dǎo)致狀態(tài)估計(jì)無法保證無偏性。為了解決此問題，現(xiàn)考慮基于目標(biāo)預(yù)測(cè)位置計(jì)算量測(cè)轉(zhuǎn)換誤差的均值和方差。

目標(biāo)距離、方位角和俯仰角的預(yù)測(cè)值表示如下：

(13)

根據(jù)式(1)和式(13)，容易得到

(14)

(15)

利用式(14)和式(15)，基于目標(biāo)預(yù)測(cè)位置，可以得到轉(zhuǎn)換測(cè)量誤差的均值為

(16)

相應(yīng)的轉(zhuǎn)換量測(cè)誤差協(xié)方差陣RCp為

(17)

由式(17)推導(dǎo)求得的對(duì)應(yīng)的RCp的各元素具體如下：

(18)

(19)

(20)

(21)

(22)

(23)

基于式(7)的量測(cè)轉(zhuǎn)換結(jié)果，以目標(biāo)預(yù)測(cè)位置為條件計(jì)算出量測(cè)轉(zhuǎn)換誤差為零均值，方差滿足式(17)～(23)。由于式(7)所示的轉(zhuǎn)換結(jié)果已經(jīng)是直角坐標(biāo)系中目標(biāo)在3個(gè)方向的位置信息，顯然已經(jīng)與目標(biāo)狀態(tài)向量之間呈線性關(guān)系了，因此可以在此基礎(chǔ)上運(yùn)用卡爾曼濾波。將上述量測(cè)轉(zhuǎn)換方法與卡爾曼濾波算法相結(jié)合，即可獲得一種基于預(yù)測(cè)值量測(cè)轉(zhuǎn)換(Predicted Position based UCM,PPUCM)的卡爾曼濾波目標(biāo)跟蹤算法。

4 仿真分析

考慮一個(gè)在平面做勻速直線運(yùn)動(dòng)的目標(biāo)，雷達(dá)位于坐標(biāo)原點(diǎn)，其采樣間隔為1 s。目標(biāo)的初始位置為(5 5 5)km，初始速度為(25 25 0)m/s，現(xiàn)根據(jù)距離、方位角和俯仰角的量測(cè)精度，考慮3個(gè)不同的場(chǎng)景，具體參數(shù)如表1所示。這3個(gè)場(chǎng)景分別代表量測(cè)精度較好、一般和較差的情況。

表1 仿真場(chǎng)景參數(shù)Tab.1 The parameters of simulation scene

采用本文提出的基于預(yù)測(cè)值量測(cè)轉(zhuǎn)換的卡爾曼濾波算法對(duì)該目標(biāo)進(jìn)行跟蹤，并對(duì)其性能進(jìn)行統(tǒng)計(jì)，以下仿真結(jié)果均為100次蒙特卡洛仿真的統(tǒng)計(jì)結(jié)果。為說明本文算法的有效性，同時(shí)也采用UCM以及MUCM對(duì)其進(jìn)行跟蹤，并將三者的跟蹤精度在不同的仿真場(chǎng)景下進(jìn)行對(duì)比。圖1給出了3個(gè)場(chǎng)景下3種算法跟蹤結(jié)果的位置RMSE曲線。

(a)場(chǎng)景一

(b)場(chǎng)景二

(c)場(chǎng)景三圖1 3種基于量測(cè)轉(zhuǎn)換的卡爾曼濾波算法精度比較Fig.1 The precision comparison among three algorithms of measurement conversion based Kalman filter

由圖1可見，在3個(gè)仿真場(chǎng)景下，本文提出的方法PPUCM均能獲得最小的RMSE值，即能獲得更好的跟蹤效果，而UCM和MUCM兩者性能相當(dāng)，僅在場(chǎng)景三中量測(cè)精度較差時(shí)MUCM才體現(xiàn)出相對(duì)于UCM的優(yōu)勢(shì)。本文算法不僅在3個(gè)場(chǎng)景下均體現(xiàn)出更好的性能，而且跟蹤精度較傳統(tǒng)算法的改善程度隨著場(chǎng)景中量測(cè)誤差的增大而變得更加明顯。在場(chǎng)景三中，PPUCM的跟蹤精度較MUCM提升近20%，較UCM提升超過20%，其原因在于，PPUCM方法在量測(cè)轉(zhuǎn)換過程中以預(yù)測(cè)值為條件計(jì)算量測(cè)轉(zhuǎn)換誤差的均值和方差，避免了UCM計(jì)算中的不兼容性，也克服了MUCM計(jì)算中利用量測(cè)值為條件而導(dǎo)致的濾波增益與量測(cè)誤差相關(guān)的問題。進(jìn)一步，我們對(duì)比了3種算法的時(shí)效性，如表2所示。

表2 3種算法運(yùn)行時(shí)間比較Tab.2 The compare of run time among three algorithms

從表2可見，UCM的耗時(shí)最短，MCUM和PPUCM的耗時(shí)相當(dāng)，但3種算法的運(yùn)行時(shí)間在同一個(gè)量級(jí)上，這說明了本文算法能在不增加運(yùn)算量的同時(shí)有效提高目標(biāo)跟蹤精度。

5 結(jié)束語

實(shí)際目標(biāo)跟蹤系統(tǒng)中，量測(cè)數(shù)據(jù)通常是在球坐標(biāo)系中獲得的目標(biāo)量測(cè)位置，該信息與目標(biāo)運(yùn)動(dòng)狀態(tài)之間的非線性特性導(dǎo)致不能直接采用卡爾曼濾波進(jìn)行跟蹤。量測(cè)轉(zhuǎn)換方法是解決這類問題的一個(gè)有效途徑，但傳統(tǒng)的轉(zhuǎn)換方法在計(jì)算量測(cè)轉(zhuǎn)換誤差統(tǒng)計(jì)特性時(shí)以目標(biāo)量測(cè)值為條件，導(dǎo)致了濾波增益與量測(cè)誤差的相關(guān)性，使得最終估計(jì)結(jié)果為有偏估計(jì)。針對(duì)這種情況，本文提出基于目標(biāo)預(yù)測(cè)位置計(jì)算量測(cè)轉(zhuǎn)換誤差的均值和方差，克服了傳統(tǒng)量測(cè)轉(zhuǎn)換的缺陷，并與卡爾曼濾波算法相結(jié)合，實(shí)現(xiàn)了目標(biāo)的有效跟蹤。仿真表明，本文算法能在不增加運(yùn)算量的條件下獲得較傳統(tǒng)量測(cè)轉(zhuǎn)換方法更好的跟蹤效果，跟蹤精度改善度達(dá)20%，是一種有效的量測(cè)轉(zhuǎn)換目標(biāo)跟蹤方法。后續(xù)研究中，將進(jìn)一步考慮目標(biāo)的機(jī)動(dòng)特性和系統(tǒng)的徑向速度量測(cè)信息，發(fā)展可同時(shí)處理徑向速度量測(cè)的機(jī)動(dòng)目標(biāo)跟蹤算法。