基于卡爾曼濾波的機(jī)動(dòng)與非機(jī)動(dòng)目標(biāo)的定位跟蹤

徐 楊，張正華

（揚(yáng)州大學(xué) 信息工程學(xué)院，江蘇 揚(yáng)州 225000）

在我國(guó)成為世界第二大經(jīng)濟(jì)體的同時(shí)，我國(guó)居民對(duì)交通的需求也越來(lái)越高，然而我國(guó)目前的交通狀況卻不容樂(lè)觀：目前我國(guó)道路交通事故死亡人數(shù)仍居世界第一位；我國(guó)道路交通事故的車輛事故率約為發(fā)達(dá)國(guó)家的十幾倍[1]；因此，智能交通系統(tǒng)的實(shí)施勢(shì)在必行。

所謂智能交通是指一個(gè)基于現(xiàn)代電子信息技術(shù)面向交通運(yùn)輸?shù)姆?wù)系統(tǒng)。它的突出特點(diǎn)是以信息的收集、處理、發(fā)布、交換、分析、利用為主線，為交通參與者提供多樣性的服務(wù)。而目標(biāo)跟蹤是智能交通系統(tǒng)的核心，通過(guò)對(duì)機(jī)動(dòng)車輛的實(shí)時(shí)監(jiān)測(cè)和跟蹤，不僅可以得到單個(gè)車輛的速度，還可以得到車輛的流量和密度等，因此可以實(shí)現(xiàn)對(duì)交通事故或者車輛故障等突發(fā)狀況的提前預(yù)判，在很大程度上避免交通事故的發(fā)生。

實(shí)現(xiàn)智能交通的前提是對(duì)運(yùn)動(dòng)的車輛進(jìn)行跟蹤。關(guān)于卡爾曼濾波的一個(gè)典型應(yīng)用就是從一組對(duì)物體位置的觀察序列（可能有偏差）推算出目標(biāo)的運(yùn)動(dòng)狀態(tài)，即物體位置的坐標(biāo)及速度，這一組序列從可行性的角度來(lái)說(shuō)，應(yīng)該是有限的并且有噪聲包含其中[2-3]。在我們身邊的很多工程應(yīng)用(如雷達(dá)、傳感器監(jiān)控)中都很容易發(fā)現(xiàn)它的身影。卡爾曼濾波應(yīng)用廣泛且功能強(qiáng)大的原因在于，它不僅可以估計(jì)信號(hào)的過(guò)去和當(dāng)前狀態(tài)，甚至能估計(jì)將來(lái)的狀態(tài)，即使并不知道模型的確切性質(zhì)。一般情況下，我們可以利用卡爾曼濾波對(duì)復(fù)雜運(yùn)動(dòng)情況下的目標(biāo)進(jìn)行實(shí)時(shí)精確的跟蹤。

1 卡爾曼濾波在目標(biāo)跟蹤中的應(yīng)用介紹

在理論上，經(jīng)典的維納濾波理論和方法有不可避免的局限性，而卡爾曼的濾波的優(yōu)點(diǎn)之一，就是突破了維納濾波的這種局限性，因?yàn)槠湓谝肓讼到y(tǒng)的狀態(tài)變量和狀態(tài)空間的基礎(chǔ)上，進(jìn)一步提出了狀態(tài)空間方法(基于時(shí)域的)，這一方法的提出不僅代表了現(xiàn)代控制理論的開(kāi)端，而且還延伸出了一套遞推濾波算法，這套算法，不僅容易在計(jì)算機(jī)上實(shí)時(shí)實(shí)現(xiàn)，而且適合多變量的處理，是適用于線性系統(tǒng)的最優(yōu)估計(jì)理論[4-5]。Kalman濾波基本公式：

我們可以得知，卡爾曼濾波算法在對(duì)運(yùn)動(dòng)目標(biāo)的動(dòng)態(tài)跟蹤方法中，能夠?qū)崟r(shí)地處理數(shù)據(jù)，提高了初始階段的跟蹤精度。作為一個(gè)對(duì)單個(gè)目標(biāo)的跟蹤算法，它是多目標(biāo)跟蹤算法，信息融合與跟蹤的基礎(chǔ)，也是實(shí)現(xiàn)智能交通的第一步[6-7]。

2 仿真分析

2.1 非機(jī)動(dòng)目標(biāo)

1)情景假設(shè)

假設(shè)有一二坐標(biāo)雷達(dá)對(duì)一平面上運(yùn)動(dòng)的目標(biāo)進(jìn)行觀測(cè)，目標(biāo)在 t=0～600 s沿 x軸作恒速直線運(yùn)動(dòng)，運(yùn)動(dòng)速度為15 m/s，目標(biāo)的起始點(diǎn)為（-10 000 m,2 000 m）。雷達(dá)掃描周期T=2 s，x和y獨(dú)立地進(jìn)行觀測(cè)，觀測(cè)噪聲標(biāo)準(zhǔn)差均為100 m。要求濾波誤差標(biāo)準(zhǔn)差的方差壓縮系數(shù)為0.5。

2)結(jié)果分析

我們已經(jīng)把卡爾曼濾波的算法敘述的很清楚，由5個(gè)公式就很容易實(shí)現(xiàn)卡爾曼濾波算法。在計(jì)算機(jī)仿真中，我們采用Matlab編寫程序，利用蒙特卡洛的方法對(duì)跟蹤濾波器進(jìn)行仿真分析，次數(shù)為1 000次。以下給出仿真圖和結(jié)果分析。

圖1是目標(biāo)的真實(shí)軌跡和測(cè)量軌跡，測(cè)量軌跡是真實(shí)軌跡數(shù)據(jù)添加方差和均值固定的隨機(jī)測(cè)量噪聲得到的，目標(biāo)沿y=2 000 m做勻速直線運(yùn)動(dòng)。給出的測(cè)量軌跡用于濾波后與濾波軌跡作比較分析。從圖中可以看出，測(cè)量軌跡圍繞真實(shí)軌跡作上下浮動(dòng)。

圖1 目標(biāo)的真實(shí)軌跡與量測(cè)軌跡Fig.1 The real trajectory and measured trajectory

圖2 是1 000次濾波數(shù)據(jù)曲線。從圖中可以看出，濾波開(kāi)始時(shí)誤差較大，但是隨著時(shí)間的推移，濾波誤差降低，估計(jì)值逐步逼近真實(shí)軌跡。

圖3是y方向?yàn)V波估計(jì)誤差均值及誤差標(biāo)準(zhǔn)差。濾波開(kāi)始時(shí)誤差較大，隨著采樣點(diǎn)的增加，誤差逐漸減小，誤差的標(biāo)準(zhǔn)差也具有同樣的特性。達(dá)到了濾波誤差標(biāo)準(zhǔn)差的方差壓縮系數(shù)為0.5的要求。

圖2 1 000次濾波數(shù)據(jù)曲線Fig.2 The 1 000 filtering data curve

圖3 y方向?yàn)V波估計(jì)誤差均值及誤差標(biāo)準(zhǔn)差Fig.3 The average value of y direction filtering estimation error and the standard deviation

圖4 是x方向?yàn)V波估計(jì)誤差均值及誤差標(biāo)準(zhǔn)差。與y方向的估計(jì)誤差均值相比，x方向的估計(jì)誤差均值波動(dòng)較大，這是由于在x方向上有速度分量的緣故，同時(shí)其方向上濾波估計(jì)誤差也有一定波動(dòng)。也達(dá)到了其壓縮系數(shù)為0.5的要求。

圖4 x方向?yàn)V波估計(jì)誤差均值及誤差標(biāo)準(zhǔn)差Fig.4 The average value of x direction filtering estimation error and the standard deviation

圖5 是單次濾波速度估計(jì)與1 000次濾波速度估計(jì)。單次濾波速度與實(shí)際值有差距，但是1 000次濾波取均值后速度濾波已經(jīng)于實(shí)際值，但是濾波開(kāi)始時(shí)仍有很大偏差，這隨著采樣點(diǎn)的增加，誤差逐漸減小。

圖5 單次濾波速度估計(jì)與1 000次濾波速度估計(jì)Fig.5 The estimation of single filtering speed and 1 000 filtering speed

2.2 機(jī)動(dòng)目標(biāo)

1)情景假設(shè)

假定有一二座標(biāo)雷達(dá)對(duì)一平面上運(yùn)動(dòng)的目標(biāo)進(jìn)行觀測(cè)，目標(biāo)在0-400 s沿著y軸作恒速直線運(yùn)動(dòng)，運(yùn)動(dòng)速度為-15 m/s，目標(biāo)的起始點(diǎn)為(2 000 m,10 000 m)，在 t=400～600 s向軸x方向做的慢轉(zhuǎn)彎，加速度為0.075 m/s，完成慢轉(zhuǎn)彎后加速度將降為零，從t=610 s開(kāi)始做90°的快轉(zhuǎn)彎，加速度為0.3 m/s，在660 s結(jié)束轉(zhuǎn)彎，加速度降至零。雷達(dá)掃描周期T=2 s，X和Y獨(dú)立地進(jìn)行觀測(cè)，觀測(cè)噪聲的標(biāo)準(zhǔn)差均為100 m。

2)結(jié)果分析

從圖6可以看出，對(duì)應(yīng)的目標(biāo)有2次加速和2次勻速運(yùn)動(dòng)，符合情景假設(shè)中目標(biāo)真實(shí)軌跡變化。

圖6 真實(shí)軌跡與量測(cè)軌跡對(duì)比Fig.6 The comparison between real trajectory and measured trajectory

從圖7可以看出，機(jī)動(dòng)的卡爾曼濾波對(duì)觀測(cè)軌跡有很好的濾波效果，但在模型出現(xiàn)機(jī)動(dòng)的時(shí)候，會(huì)出現(xiàn)大的誤差。

從圖8可以看出，在一定的參數(shù)范圍內(nèi)，提高濾波次數(shù)，可以更直觀的看清楚濾波后的軌跡。

3 結(jié)束語(yǔ)

圖7 單次濾波與50次濾波均值曲線對(duì)比Fig.7 The comparison between the single filtering mean curve and the 50 times one

圖8 x濾波誤差均值與誤差標(biāo)準(zhǔn)差曲線Fig.8 The average value of X direction filtering estimation error and the curve of standard deviation

仿真實(shí)驗(yàn)表明，卡爾曼濾波算法在對(duì)運(yùn)動(dòng)目標(biāo)跟蹤中，可以實(shí)時(shí)地處理數(shù)據(jù)，利用卡爾曼濾波算法可以對(duì)運(yùn)動(dòng)目標(biāo)的位置，速度和加速度進(jìn)行精度較高的實(shí)時(shí)跟蹤。精確的實(shí)時(shí)跟蹤不僅可以應(yīng)用在智能交通系統(tǒng)中，對(duì)車輛進(jìn)行檢測(cè)和跟蹤，向總服務(wù)器實(shí)時(shí)反饋流量信息，從而對(duì)路段進(jìn)行管理，避免擁塞和堵車。此外，還可以應(yīng)用在軍用領(lǐng)域中，如電視跟蹤和紅外跟蹤等。

但是，從實(shí)驗(yàn)結(jié)果可以看出，卡爾曼濾波對(duì)非機(jī)動(dòng)目標(biāo)的跟蹤效果要優(yōu)于機(jī)動(dòng)目標(biāo)。即在目標(biāo)出現(xiàn)機(jī)動(dòng)的時(shí)刻，使用卡爾曼濾波可能會(huì)導(dǎo)致濾波發(fā)散，使濾波值和目標(biāo)真實(shí)值之間的誤差增大，這時(shí)卡爾曼濾波會(huì)不適宜于精度要求較高的場(chǎng)所。在以后的研究中，要對(duì)自適應(yīng)性的濾波技術(shù)以及多種濾波技術(shù)的結(jié)合如VD變維濾波和卡爾曼濾波的結(jié)合等進(jìn)行更進(jìn)一步的研究。

[1]楊飛.淺析我國(guó)智能交通系統(tǒng)的發(fā)展對(duì)策 [J].才智,2010(21):67-68.YANG Fei.Analyses countermeasures for the development of intelligent transportation systems in China[J].Intelligence,2010(21):67-68.

[2]Singer R.Estimating optimal tracking filter performance for manned maneuvering targets.IEEE Trans[J].on AES,1982:473-483.

[3]ZHU Hua-ping.Target tracking using Kalman Filter Embedded Trust Region[C]//Test and Measurement,2009.ICTM'09.International Conference on,2009:119-122.

[4]Wiener N.Extrapolation,interpolation and smoothing of stationary time series[M].New York:Wiely Press，1949.

[5]Chartchai Meesookho,Shrikanth Narayanan.Distributed Range Difference Based TargetLocalization in Sensor Network[J].Signals,Systems and Computers,2005:205-209.

[6]靳璐.機(jī)動(dòng)目標(biāo)跟蹤及無(wú)跡濾波 （UKF）的相關(guān)應(yīng)用研究[J].戰(zhàn)術(shù)導(dǎo)彈技術(shù)，2009(1):20-32.JIN LU.The tracking of the maneuverable target and research of UFK[J].Tactical Missile Technology,2009(1):20-32.

[7]宋亞姬.基于卡爾曼濾波的運(yùn)動(dòng)目標(biāo)檢測(cè)與跟蹤技術(shù)研究[D].長(zhǎng)春理工大學(xué),2011.

[8]苑柳青.基于粒子濾波的視頻運(yùn)動(dòng)目標(biāo)跟蹤方法研究[D].甘肅：蘭州理工大學(xué),2011.

[9]蔣志凱.數(shù)字濾波與卡爾曼濾波[M].北京：中國(guó)科學(xué)技術(shù)出版社，1993.