賀豐收,繆禮鋒,張 瑩
(中航工業(yè)雷達(dá)與電子設(shè)備研究院,江蘇 無(wú)錫 214063)
道路約束條件下的地面運(yùn)動(dòng)目標(biāo)跟蹤*
賀豐收,繆禮鋒,張 瑩
(中航工業(yè)雷達(dá)與電子設(shè)備研究院,江蘇 無(wú)錫 214063)
有效使用道路信息能顯著提高地面運(yùn)動(dòng)目標(biāo)跟蹤性能。基于卡爾曼濾波理論,提出了道路約束條件下的濾波方法。首先建立了描述道路網(wǎng)中路段的數(shù)學(xué)模型,給出了將目標(biāo)分配于道路的定位算法;然后推導(dǎo)了道路約束條件下的過(guò)程噪聲矩陣;提出了路段切換時(shí)狀態(tài)修正公式;在道路約束最大后驗(yàn)概率估計(jì)準(zhǔn)則下,根據(jù)道路參數(shù)修正狀態(tài)與協(xié)方差。蒙特卡洛仿真結(jié)果證明了該算法的優(yōu)越性能,且定量分析表明其計(jì)算量是可接受的。
地面運(yùn)動(dòng)目標(biāo),跟蹤,道路約束,卡爾曼濾波
近年來(lái),由于軍事和民用需求的發(fā)展,地面動(dòng)目標(biāo)跟蹤得到了更多的關(guān)注。地面目標(biāo)跟蹤受到環(huán)境限制,較空中目標(biāo)跟蹤更為困難[1]。但地面目標(biāo)跟蹤可利用公路網(wǎng)絡(luò)、地形特征等非正態(tài)信息提高跟蹤質(zhì)量。Blackman明確指出[2]:“地面動(dòng)目標(biāo)跟蹤的關(guān)鍵在于使用道路網(wǎng)絡(luò),有效地使用道路網(wǎng)絡(luò)是提高地面目標(biāo)跟蹤性能最重要的途徑”。
國(guó)內(nèi)外學(xué)者就地面目標(biāo)跟蹤開(kāi)展了大量研究,見(jiàn)文獻(xiàn)[3-8]。這些文獻(xiàn)都沒(méi)有給出道路建模的細(xì)節(jié),且默認(rèn)目標(biāo)一直在道路上運(yùn)行即忽略了目標(biāo)至道路的分配算法;沒(méi)有考慮目標(biāo)穿過(guò)道路節(jié)點(diǎn)時(shí)的狀態(tài)修正處理,對(duì)約束濾波算法的計(jì)算復(fù)雜度也沒(méi)有進(jìn)行分析。本文根據(jù)道路約束的特點(diǎn),重點(diǎn)研究道路約束條件下的濾波方法。
1.1 運(yùn)動(dòng)與測(cè)量模型
考慮到地面目標(biāo)一般只在二維平面上運(yùn)動(dòng)且機(jī)動(dòng)相對(duì)較小,故用二維勻速模型對(duì)目標(biāo)進(jìn)行建模,系統(tǒng)動(dòng)態(tài)方程和測(cè)量方程的表達(dá)形式為:
相應(yīng)地vk=[vRvAz]T,其標(biāo)準(zhǔn)差為σR、σAz。
1.2 路段建模
首先對(duì)地面空間進(jìn)行定義,一般的地面空間分成兩大部分,開(kāi)放區(qū)域與道路網(wǎng)。其中開(kāi)放區(qū)域跟空中狀態(tài)一樣,對(duì)目標(biāo)運(yùn)動(dòng)模型沒(méi)有限制;而位于道路網(wǎng)的目標(biāo)運(yùn)動(dòng)只會(huì)沿道路平行方向,因此,運(yùn)動(dòng)的預(yù)測(cè)與濾波模型受到道路網(wǎng)的約束。用起始點(diǎn)、結(jié)束點(diǎn)與寬度聯(lián)合表示路段。連接的起始點(diǎn)和結(jié)束點(diǎn)稱(chēng)之為交叉點(diǎn)。彎曲的道路線可采用多個(gè)小的相鄰路段近似。如圖1所示:
圖1 路段示意圖
目標(biāo)時(shí)刻k的狀態(tài)變量在開(kāi)放區(qū)域可以表示為:
把目標(biāo)的狀態(tài)約束到其所在的道路段s時(shí),約束后的狀態(tài)變量可以表示為:
設(shè)定某一道路段的兩個(gè)起始點(diǎn)和結(jié)束點(diǎn)分別為(x1,y1)和(x2,y2),則當(dāng)目標(biāo)處在道路段上,以下兩個(gè)條件必須同時(shí)滿足:
1.2.1 目標(biāo)位置處在兩條端點(diǎn)連線上
1.2.2 速度和道路段方向平行
因此,這種約束可以表示為:
若采用矩陣的概念則可以表示成
道路與地理坐標(biāo)系x軸夾角為θs(見(jiàn)下頁(yè)圖2),其計(jì)算公式為:
1.3 路段分配
得到路段參數(shù)的前提是確定目標(biāo)所在的路段,需要將正確的路段分配給目標(biāo)。道路分配問(wèn)題的實(shí)質(zhì)即判決目標(biāo)是否位于某一道路段上。構(gòu)造假設(shè)檢驗(yàn):
如上式成立,則接受假設(shè)H0。門(mén)限χ22(1-α)是服從兩個(gè)自由度的卡方分布,其置信度為100(1-α)。如果存在多個(gè)道路段均滿足以上不等式,則將目標(biāo)分配到馬氏統(tǒng)計(jì)距離(不等式左側(cè))最小的道路段上。
需要注意的是,一般在航跡起始之后一段時(shí)間再進(jìn)行路段初始分配。在地面運(yùn)動(dòng)目標(biāo)航跡起始之后,如果目標(biāo)的狀態(tài)估計(jì)協(xié)方差滿足設(shè)定的門(mén)限條件,即可將目標(biāo)分配在道路段上。如果協(xié)方差較大,則狀態(tài)估計(jì)還不穩(wěn)定,帶有較大誤差,過(guò)早地啟動(dòng)道路分配可能會(huì)導(dǎo)致錯(cuò)誤的結(jié)果。一旦確定目標(biāo)所在道路段,則不必重復(fù)進(jìn)行路段分配,根據(jù)路段連接關(guān)系即可得到確定新的路段。在復(fù)雜道路條件下,如交叉、匯合、分支等復(fù)雜道路情況,可通過(guò)多假設(shè)方法[9]或者變結(jié)構(gòu)多模型[10]方法判斷所處的新路段。
2.1 道路約束下的過(guò)程噪聲
在對(duì)開(kāi)放空間目標(biāo)進(jìn)行濾波時(shí),由于狀態(tài)不受約束,所以過(guò)程噪聲在平面上的X,Y向可以認(rèn)為是大致相等的。但是對(duì)于處于道路網(wǎng)的地面目標(biāo)而言,其狀態(tài)是受約束的,其過(guò)程噪聲應(yīng)該充分考慮當(dāng)前狀態(tài)的特點(diǎn)。
行駛在道路上的目標(biāo)一般會(huì)在道路上,順著道路方向前進(jìn)。換句說(shuō)話其垂直于道路方向的過(guò)程噪聲不會(huì)很大,其標(biāo)準(zhǔn)差一般不會(huì)大于道路寬度的一半;而順著道路方向的過(guò)程噪聲一般不受影響。
可得:
令:
其中,
且與無(wú)關(guān)。
故根據(jù)式(13)可得:
可得到:
展開(kāi)后得:
引入道路約束后,垂直于道路方向上的過(guò)程噪聲分量方差在減小,減小的程度由W/σT2決定,W為道路寬度。當(dāng) W=σT2時(shí),σ⊥2=0.291 1σ2,當(dāng)W=3σT2時(shí),σ⊥2=0.973 4σ2,即當(dāng)W≥3σT2時(shí),道路寬度對(duì)過(guò)程噪聲的影響幾乎可以忽略。在目標(biāo)進(jìn)入寬度為W=σT2的道路時(shí),與沒(méi)有道路約束過(guò)程噪聲的概率密度函數(shù)如圖2所示:
圖2 過(guò)程噪聲概率密度函數(shù)對(duì)比圖
2.2 路段切換修正
圖3 路段切換預(yù)測(cè)示意圖
道路段長(zhǎng)度是有限的,因此,在目標(biāo)運(yùn)動(dòng)所在路段r運(yùn)動(dòng)至路段i時(shí),如果按照路段r的信息外推,則會(huì)出現(xiàn)如圖3所示的外推值x^r,嚴(yán)重影響外推精度。在式(24)所示的協(xié)方差矩陣修正后,協(xié)方差矩陣顯著縮小,較大的預(yù)測(cè)誤差將會(huì)導(dǎo)致目標(biāo)測(cè)量值不能落入關(guān)聯(lián)波門(mén)內(nèi),導(dǎo)致航跡中斷。因此,需要修正相應(yīng)的濾波狀態(tài)至對(duì)應(yīng)的路段。如圖3所示。
其中,
△θi為路段i與原路段r的夾角。
2.3 估計(jì)修正方法
狀態(tài)修正需要將目標(biāo)投影在道路上,可基于馬氏距離最小化來(lái)獲得這個(gè)狀態(tài)投影。為了獲得在道路段上的速度方向,需要引入速度方向和標(biāo)準(zhǔn)道路段矢量的正交關(guān)系。根據(jù)式(8)所示的限制關(guān)系,在道路限制條件下的最小化問(wèn)題可以表述為:
根據(jù)拉格朗日方法和狀態(tài)約束理論,道路約束條件下的最大后驗(yàn)概率狀態(tài)估計(jì)和協(xié)方差估計(jì)為:
其中Id為單位矩陣,而
3.1 仿真場(chǎng)景描述
為了評(píng)估上述算法的性能,考察道路約束條件對(duì)濾波精度的影響,構(gòu)造一個(gè)單目標(biāo)跟蹤道路交通網(wǎng)絡(luò)圖。目標(biāo)以20 m/s的速度在道路上做勻速運(yùn)動(dòng),道路寬度統(tǒng)一設(shè)為5 m。
圖4 仿真道路交通網(wǎng)示意圖
目標(biāo)途經(jīng)的道路與節(jié)點(diǎn)參數(shù)如表1所示:
表1 道路參數(shù)表
雷達(dá)位于坐標(biāo)原點(diǎn),采樣周期為5 s。對(duì)地慢動(dòng)目標(biāo)探測(cè)一般采用低重頻,不提供徑向速度測(cè)量(或有嚴(yán)重模糊的徑向速度測(cè)量),距離量測(cè)噪聲標(biāo)準(zhǔn)差σR和方位角量測(cè)噪聲標(biāo)準(zhǔn)差σAz分別為30 m和8 mrad。道路寬度W為5 m,在采樣周期為5 s時(shí),取策動(dòng)噪聲系數(shù)為σ=0.2。為重點(diǎn)說(shuō)明本文研究濾波算法性能,在本仿真場(chǎng)景中,不考慮地形遮擋影響,道路節(jié)點(diǎn)分支采用MHT方法[9-11]處理,本文不再贅述。蒙特卡洛仿真次數(shù)取500次。
3.2 仿真結(jié)果
分為4種情況進(jìn)行比較:①不引入任何約束條件進(jìn)行標(biāo)準(zhǔn)擴(kuò)展卡爾曼濾波(EKF),作為基準(zhǔn)參考值;②過(guò)程噪聲約束(Noise Constrain)卡爾曼濾波方法,垂直道路方向的過(guò)程噪聲取值為平行于道路方向過(guò)程噪聲的0.01;③采用估計(jì)修正(Estimation Modification)卡爾曼濾波方法,同時(shí)對(duì)濾波的狀態(tài)和協(xié)方差每個(gè)采樣周期進(jìn)行一次修正;④同時(shí)采用過(guò)程噪聲約束和狀態(tài)修正的道路約束(Road Constrain)卡爾曼濾波方法。通過(guò)比較這4種方法的性能表現(xiàn),評(píng)價(jià)本文提出方法對(duì)濾波性能的影響。其中,后三種方法都采用了道路切換修正方法。
從仿真結(jié)果來(lái)看,EKF方法在目標(biāo)真實(shí)軌跡附近有明顯的波動(dòng),而道路約束的RCKF方法目標(biāo)的真實(shí)軌跡吻合的非常好,準(zhǔn)確地定位在道路中心線上。下面的精度圖清楚地表明了道路約束方法在位置精度和速度精度上的巨大優(yōu)勢(shì)。
圖5 4種方法的跟蹤精度比較圖
由圖5可以看出,引入道路約束對(duì)跟蹤性能有明顯的提高,但不同的方法對(duì)性能的改善也是不一樣的。采用過(guò)程噪聲約束的NC方法可以大大的提高跟蹤的精度,尤其是在目標(biāo)受道路限制,運(yùn)動(dòng)方向發(fā)生突變時(shí),采用過(guò)程噪聲約束的方法可以非常平滑地過(guò)渡至新的運(yùn)動(dòng)狀態(tài),沒(méi)有出現(xiàn)大的誤差尖峰;而采用狀態(tài)修正的EM方法性能提高更為明顯,誤差水平更低;同時(shí)結(jié)合了過(guò)程噪聲約束與估計(jì)修正的RCKF方法精度最高。估計(jì)修正的EMKF方法優(yōu)于噪聲約束方法,其原因是估計(jì)修正方法不但可以改變協(xié)方差矩陣,而且可以直接修正目標(biāo)的位置和速度,結(jié)果反應(yīng)更加快速,更加準(zhǔn)確。RCKF相較于EMKF的性能提高有限,主要是因?yàn)镋MKF方法性能已經(jīng)很好,NC方法在此基礎(chǔ)上的改進(jìn)余地不大。另外,路段切換修正對(duì)精度的影響也是很重要的。道路約束的濾波方法必須結(jié)合路段切換修正,才能取得較好的效果。
為了定量比較道路約束方法帶來(lái)的性能變化,以EKF方法作為基準(zhǔn)衡量另外三種道路約束方法。精度改善的詳細(xì)數(shù)據(jù)見(jiàn)表2所示:
表2 精度和誤差比值表
3.3 計(jì)算量分析
根據(jù)標(biāo)準(zhǔn)卡爾曼濾波公式[12],統(tǒng)計(jì)得到標(biāo)準(zhǔn)卡爾曼濾波器總的計(jì)算量為:
3n3+2n2m+2nm2+nm+n2次乘加運(yùn)算,以及一個(gè)m×m維的矩陣求逆。其中,m為測(cè)量維數(shù),n為狀態(tài)維數(shù)。文獻(xiàn)[13]指出,逆矩陣計(jì)算量與同階矩陣相乘計(jì)算量相當(dāng),為m3次乘法與加法,因此,標(biāo)準(zhǔn)卡爾曼濾波總的計(jì)算量為:
過(guò)程噪聲修正的NC方法主要是對(duì)計(jì)算過(guò)程噪聲的系數(shù)進(jìn)行調(diào)整,因此,幾乎不影響計(jì)算量。估計(jì)修正的EM方法和RC方法在標(biāo)準(zhǔn)卡爾曼濾波之外,還要增加狀態(tài)和協(xié)方差修正環(huán)節(jié)。狀態(tài)修正式(23)的運(yùn)算量為:
協(xié)方差修正式(24)的運(yùn)算量為:
因此,估計(jì)修正的運(yùn)算量為:
取典型值n=4,m=2則CEM=710,CEKF=320。
實(shí)際工程中可以對(duì)卡爾曼濾波采取一些措施降低運(yùn)算量:考慮到其中的一些矩陣為對(duì)稱(chēng)矩陣,可以采用半角陣進(jìn)行存儲(chǔ)和運(yùn)算;其中的一些矩陣(如D陣)中含有較多的0元素,優(yōu)化與零元素有關(guān)的運(yùn)算;對(duì)運(yùn)算量大的代碼段進(jìn)行優(yōu)化設(shè)計(jì)。采取這些優(yōu)化措施后,在Intel Core(TM)i5-2400的3.1 GHz平臺(tái)上進(jìn)行測(cè)試,進(jìn)行5,000,000次濾波循環(huán)比較的結(jié)果如表3所示:
表3 計(jì)算量對(duì)比
在采用運(yùn)算優(yōu)化措施后,RCKF的運(yùn)算時(shí)間比EKF多出38.58%。考慮到如此顯著的性能提升,付出一些計(jì)算代價(jià)在大多數(shù)情況下是可以接受的。
本文提出了一種道路約束條件下的地面運(yùn)動(dòng)目標(biāo)的跟蹤算法。對(duì)道路采用路段模型進(jìn)行描述,構(gòu)建了路段方程,提出了道路分配算法;根據(jù)目標(biāo)在道路上運(yùn)動(dòng)的約束特點(diǎn),從過(guò)程噪聲,狀態(tài)與協(xié)方差估計(jì)修正,路段切換修正等方面進(jìn)行了分析與研究,并給出了相應(yīng)的濾波算法。蒙特卡洛仿真結(jié)果表明,融合了道路信息的地面目標(biāo)機(jī)動(dòng)不確定性大大降低,跟蹤性能得到了巨大的提升,證實(shí)了道路約束對(duì)地面運(yùn)動(dòng)目標(biāo)跟蹤的重要意義。各種道路約束方法對(duì)性能都有不同程度的改善,其中估計(jì)修正方法是性能提升最為明顯。本文主要應(yīng)用于地面單目標(biāo)跟蹤,下一步的研究方向?yàn)閺?fù)雜道路網(wǎng)條件下的地面多目標(biāo)跟蹤。
[1]Chong C Y,Garren D,Grayson T P.Ground Target Tracking-a Historical Perspective [C]//IEEE Aerospace Conference Proceedings,1998.
[2]Blackman S.Integration of a Road Network into a Radar Ground Moving Target Tracking(GMTT)System and Its Performance Evaluation[C]//Signal and Data Processing of Small Targets,2009.
[3]Mallick M,Scala L.IMM Estimator for Ground Target Tracking with Variable Measurement Sampling Intervals[C]//International Conference on Information Fusion,2006.
[4]Simon D,Chia T L.Kalman Filtering with State Equality Constrains[J].IEEE Trans on Aerospace and Electronic Systems,2002,36(1):26-46.
[5]Tahk M,Speyer J L.Target Tracking Problems Subject to KinematicConstraints[J].IEEE Trans on Automatic Control,1990,35(3):324-326.
[6]Herrero J G,Portas J B,Corredera J R.Using of Map Information for Tracking Targets on Airport Surface[J].IEEE Trans on Aerospace and Electronic Systems,2003,39(2):675-692.
[7]Kirubarajan T,Bar-Shalom Y,Pattipati K R.Ground Target Tracking with Variable Structure IMM Estimator[J].IEEE Trans on Aerospace and Electronic Systems,2000,36(1): 26-46.
[8]王建國(guó),何佩琨,龍騰.道路約束在卡爾曼濾波中的應(yīng)用[J].信號(hào)處理,2003,19(4):295-299.
[9]Reid D B.An Algorithm of Tracking Multiple Targets[J]. IEEE Trans on Automatic Control,1979,24(6):843-854.
[10]Li X R,Bar-ShalomY.Multiple-model Estimation with Variable Structure[J].IEEE Trans on Automatic Control,1996,41(4):478-493.
[11]Blackman S,Popoli R.Design and Analysis of Mordern Tracking Systems[M].Norwood,MA:Artech House,1999.
[12]周宏仁,敬忠良,王培德.機(jī)動(dòng)目標(biāo)跟蹤[M].北京:國(guó)防工業(yè)出版社,1991.
[13]Cormem T H,Leisern C E,Rivest R L.Introduction to Algorithms[M].New York:The Mit Press,2005.
Ground Moving Target Tracking Road Constraint
HE Feng-shou,MIAO Li-feng,ZHANG Ying
(Radar and Avionics Institute,Aviation Lndustry of China,Wuxi 214063,China)
The effective use of road data is probably the most important way that ground target tracking performance can be improved.This paper presents a road constraint algorithm based on Kalmanfilter theory for ground moving target tracking.Road segment model and road segment assignment algorithm are described.An process noise analysis is given about road constrain.Adapted algorithm to the adjacent road is introduced when the state passes a junction.According to the corresponding road parameters,target constrained estimation is modified under Maximum A Posteriori(MAP)solution.The simulation results show that the proposed algorithm is very effective.Quantitative analysis shows that time complexity of the algorithm is acceptable.
ground moving target,tracking,road constraint,Kalman filter
TP391
A
1002-0640(2015)03-0046-05
2014-01-15
2014-03-27
國(guó)家“八六三”基金資助項(xiàng)目(2013AA7042013)
賀豐收(1979- ),男,湖南常德人,高級(jí)工程師,碩士。研究方向:目標(biāo)跟蹤與數(shù)據(jù)融合。