粒子濾波算法在靜止目標(biāo)定位時(shí)的數(shù)學(xué)建模

江春冬，盧　茹，杜太行

（河北工業(yè)大學(xué)控制科學(xué)與工程學(xué)院，天津300130）

?

粒子濾波算法在靜止目標(biāo)定位時(shí)的數(shù)學(xué)建模

江春冬，盧茹，杜太行

（河北工業(yè)大學(xué)控制科學(xué)與工程學(xué)院，天津300130）

摘要：粒子濾波算法在目標(biāo)定位中主要用于目標(biāo)跟蹤，對(duì)靜止目標(biāo)定位的應(yīng)用研究鮮有報(bào)道，尤其是針對(duì)具體的無(wú)線電移動(dòng)監(jiān)測(cè)車，已知數(shù)據(jù)只有車的位置坐標(biāo)和目標(biāo)示向度情況，數(shù)學(xué)模型的建立還沒(méi)有文獻(xiàn)可供參考。在熟悉粒子濾波機(jī)理的基礎(chǔ)上，參考粒子濾波在目標(biāo)跟蹤時(shí)建立數(shù)學(xué)模型的方法，結(jié)合無(wú)線電移動(dòng)監(jiān)測(cè)車對(duì)靜止目標(biāo)定位的實(shí)際需要，建立粒子濾波算法在靜止目標(biāo)定位時(shí)的數(shù)學(xué)模型，模型中融合分類和擇優(yōu)的措施以提高定位精度。最后在LabVIEW平臺(tái)下對(duì)所建立的模型進(jìn)行仿真實(shí)驗(yàn)，結(jié)果表明所建立的模型準(zhǔn)確可行。

關(guān)鍵詞：定位；數(shù)學(xué)模型；粒子濾波；靜止目標(biāo)

0　引言

無(wú)線電干擾源是一種靜止的信號(hào)源，對(duì)其測(cè)向定位是無(wú)線電管理的一項(xiàng)重要工作。其中，完成機(jī)動(dòng)干擾源定位查找或?qū)嵤┨囟▍^(qū)域干擾信號(hào)監(jiān)測(cè)的無(wú)線電移動(dòng)監(jiān)測(cè)車是無(wú)線電監(jiān)測(cè)測(cè)向工作中的重要技術(shù)裝備之一[1-2]。但由于車的天線情況及使用環(huán)境所限，監(jiān)測(cè)車接收到的測(cè)向信號(hào)多徑現(xiàn)象嚴(yán)重，再使用現(xiàn)有的車載線性測(cè)向定位方法，定位結(jié)果往往誤差過(guò)大，影響對(duì)實(shí)際無(wú)線電干擾源的判斷。

鑒于此，考慮將研究較熱的適用于非線性非高斯問(wèn)題的粒子濾波[3-6]算法應(yīng)用到車載系統(tǒng)中，取代原有的線性定位方法。雖然粒子濾波算法在目標(biāo)跟蹤方面已有不少研究成果[7-8]，但對(duì)靜止目標(biāo)定位的研究非常少。雖然二者表面看來(lái)相同，選擇合適的參照物即可，但實(shí)際存在如下問(wèn)題：1）文獻(xiàn)中目標(biāo)跟蹤的問(wèn)題，目標(biāo)的運(yùn)動(dòng)軌跡基本已知，位置、速度加速度變化規(guī)律可循；而使用移動(dòng)監(jiān)測(cè)車對(duì)靜止干擾源定位時(shí)，車的行進(jìn)路線要根據(jù)實(shí)際的道路情況而定。2）目標(biāo)跟蹤問(wèn)題，選擇的狀態(tài)量是目標(biāo)某一時(shí)刻的狀態(tài)；而對(duì)靜止目標(biāo)來(lái)說(shuō)，如果仍舊選擇目標(biāo)某一時(shí)刻的狀態(tài)為狀態(tài)量，狀態(tài)就是不變的，沒(méi)有了定位的必要。若要選擇車的狀態(tài)為狀態(tài)量，車的任一時(shí)刻狀態(tài)又是非連續(xù)的。

因此，使用粒子濾波算法進(jìn)行無(wú)線電干擾源測(cè)向定位時(shí)，如何選擇合適的物理量為狀態(tài)變量，并建立合適的狀態(tài)空間模型，是必須解決的問(wèn)題。本文在已知某一時(shí)刻移動(dòng)監(jiān)測(cè)車位置坐標(biāo)和相應(yīng)干擾源信號(hào)測(cè)向角的情況下，建立粒子濾波算法進(jìn)行定位時(shí)的數(shù)學(xué)模型。

1　粒子濾波算法及其實(shí)現(xiàn)步驟

粒子濾波算法的基本思想是依據(jù)系統(tǒng)狀態(tài)向量的經(jīng)驗(yàn)條件分布在狀態(tài)空間產(chǎn)生一組稱為粒子的隨機(jī)樣本集合，然后根據(jù)觀測(cè)量不斷調(diào)整粒子的權(quán)重和目標(biāo)狀態(tài)位置，最后通過(guò)調(diào)整后的粒子信息修正最初的經(jīng)驗(yàn)條件分布，估計(jì)出目標(biāo)的狀態(tài)或位置[9-10]。

根據(jù)粒子濾波算法的基本思想可知，基本的粒子濾波算法實(shí)現(xiàn)步驟應(yīng)包括：

1）選擇合適的狀態(tài)變量和觀測(cè)變量，建立系統(tǒng)的狀態(tài)空間數(shù)學(xué)模型。這一步在眾多文獻(xiàn)中并沒(méi)有把它歸入到粒子濾波方法實(shí)現(xiàn)流程里，但這一步卻是至關(guān)重要的。一般情況下選擇目標(biāo)狀態(tài)為狀態(tài)量。

2）針對(duì)k時(shí)刻狀態(tài)，建立初始粒子群，即進(jìn)行序貫重要性采樣。

3）針對(duì)k時(shí)刻狀態(tài)，對(duì)2）建立的每一個(gè)粒子，進(jìn)行權(quán)值計(jì)算，并歸一化。

4）針對(duì)k時(shí)刻狀態(tài)，對(duì)計(jì)算完權(quán)值的粒子進(jìn)行取舍，為保證粒子群大小不變，對(duì)權(quán)值較小的粒子進(jìn)行重采樣，然后重復(fù)執(zhí)行3）、4），直到結(jié)束條件滿足。

5）輸出k時(shí)刻的狀態(tài)。

6）對(duì)每一時(shí)刻進(jìn)行如上處理，得到各時(shí)刻的狀態(tài)集合。

2　粒子濾波算法在目標(biāo)跟蹤時(shí)的數(shù)學(xué)模型

設(shè)二維空間內(nèi)，選擇觀測(cè)點(diǎn)為坐標(biāo)原點(diǎn)，目標(biāo)的運(yùn)動(dòng)規(guī)律為：在x、y兩個(gè)方向上，后一時(shí)刻的位置為前一時(shí)刻的位置加上前一時(shí)刻該方向上速度，即：

式中：ν——速度；

x、y——方向位置；

k、k-1——時(shí)刻。

系統(tǒng)會(huì)受到外界干擾，影響系統(tǒng)運(yùn)動(dòng)軌跡。

2.1建立系統(tǒng)的狀態(tài)空間模型

選擇x，y方向的位置和速度為狀態(tài)變量，即k時(shí)刻的狀態(tài)量為Xk=[xk，νxk，yk，νyk]；選擇目標(biāo)與觀測(cè)站間的角度為觀測(cè)量，k時(shí)刻的值用Zk表示，加上系統(tǒng)噪聲，可得k時(shí)刻的狀態(tài)方程和觀測(cè)方程[11-12]分別為

式中，系統(tǒng)矩陣Φ為

2.2粒子濾波實(shí)現(xiàn)模型

1）初始化采樣，根據(jù)式（2），可得k時(shí)刻的粒子集[13]，表示為{X（i）k}Ni=1，展開(kāi)表示為

式中：μ——系統(tǒng)噪聲；

C——噪聲系數(shù)，選擇為常量；

N——粒子數(shù)。

2）權(quán)值計(jì)算，選擇先驗(yàn)概率密度函數(shù)為重要性函數(shù)，根據(jù)觀測(cè)方程，并在一定假設(shè)條件下，得權(quán)值計(jì)算公式為

式中x（i）k、y（i）k為式（3）中包含系統(tǒng)噪聲的粒子，δ2為觀測(cè)噪聲的方差。

計(jì)算出每一粒子權(quán)值后，對(duì)權(quán)值進(jìn)行歸一化處理[13]：

3）重采樣，對(duì)歸一化的權(quán)值選取一定的閾值，并采用系統(tǒng)重采樣方法進(jìn)行重采樣，摒棄權(quán)值較小的粒子，突出權(quán)值較大的粒子的作用，獲得重采樣后的粒子群。粒子群中粒子的數(shù)目不變[14]。

4）狀態(tài)輸出，結(jié)束條件滿足時(shí)，輸出當(dāng)前時(shí)刻的狀態(tài)。

3　粒子濾波算法在目標(biāo)靜止時(shí)的數(shù)學(xué)模型

當(dāng)使用移動(dòng)監(jiān)測(cè)車進(jìn)行測(cè)向定位時(shí)，干擾源目標(biāo)信號(hào)靜止，移動(dòng)監(jiān)測(cè)車移動(dòng)，移動(dòng)監(jiān)測(cè)車某一時(shí)刻能夠給出的信息是它的坐標(biāo)及干擾源的示向度，這些量都包含干擾噪聲。簡(jiǎn)單的原理圖如圖1所示，虛線為車行路線，線上圓點(diǎn)為記錄信息點(diǎn)，星形為待定位的干擾源。（xT，yT）表示靜止目標(biāo)的位置坐標(biāo)，（xk，yk）表示k時(shí)刻移動(dòng)監(jiān)測(cè)車的位置坐標(biāo)，θk表示移動(dòng)監(jiān)測(cè)車在k時(shí)刻觀測(cè)到的示向角。k=1，2，…，n，n為記錄數(shù)據(jù)個(gè)數(shù)。

圖1　定位原理圖

3.1系統(tǒng)狀態(tài)量選擇

仿照目標(biāo)跟蹤時(shí)模型狀態(tài)量的選擇[11-12]，首先以目標(biāo)狀態(tài)為狀態(tài)量，但這個(gè)目標(biāo)比較特殊：它靜止且未知，無(wú)法由前一時(shí)刻直接推出，根據(jù)已知數(shù)據(jù)，只能由兩個(gè)記錄點(diǎn)在理論上的兩條直線相交獲得。這同時(shí)又引入了另一個(gè)問(wèn)題，就是由于觀測(cè)的位置坐標(biāo)和示向角度誤差較大，由兩個(gè)記錄點(diǎn)計(jì)算得到的目標(biāo)位置誤差較大，由此定位出來(lái)的結(jié)果誤差也較大。

因?yàn)楸O(jiān)測(cè)車是移動(dòng)的，選擇監(jiān)測(cè)車的位置坐標(biāo)為狀態(tài)量會(huì)出現(xiàn)兩個(gè)問(wèn)題：1）監(jiān)測(cè)車的運(yùn)動(dòng)軌跡并無(wú)規(guī)律，沿著實(shí)際路線不定時(shí)地記錄數(shù)據(jù)，后一數(shù)據(jù)與前一數(shù)據(jù)沒(méi)有太多的關(guān)系，可以認(rèn)為狀態(tài)量是離散的；2）觀測(cè)角度本身參與了狀態(tài)計(jì)算，再以此作為權(quán)值計(jì)算依據(jù)，合理性有待考查。

因?yàn)檐嚨奈恢脿顟B(tài)量是離散的，其實(shí)際位置坐標(biāo)是根據(jù)實(shí)際情況記錄的，無(wú)法根據(jù)前一狀態(tài)進(jìn)行估計(jì)。而且因?yàn)椴皇侵苯俞槍?duì)目標(biāo)的，在計(jì)算過(guò)程中還要進(jìn)行轉(zhuǎn)換。綜合分析后，以第1種方案建立系統(tǒng)的狀態(tài)方程。

3.2系統(tǒng)狀態(tài)模型建立

需要定位的目標(biāo)是未知的，其狀態(tài)只能由已知數(shù)據(jù)進(jìn)行計(jì)算。根據(jù)車測(cè)數(shù)據(jù)，首先取m組數(shù)據(jù)進(jìn)行狀態(tài)計(jì)算。理論上，m組數(shù)據(jù)兩兩相交會(huì)得到Cm2個(gè)狀態(tài)，但由于實(shí)際數(shù)據(jù)不變，只需取相鄰(記錄時(shí))兩個(gè)數(shù)據(jù)或者相隔一定區(qū)間的數(shù)據(jù)計(jì)算狀態(tài)就可以了。取任意2組數(shù)據(jù)進(jìn)行狀態(tài)計(jì)算公式表示為

式中xTk，yTk分別為第k次計(jì)算時(shí)目標(biāo)狀態(tài)在兩個(gè)方向上的分量，（x1k，y1k），θ1k（x2k，y2k），θ2k分別為第k次計(jì)算時(shí)所使用的兩組數(shù)據(jù)位置及角度。狀態(tài)變量Xk=[xTk，yTk]。

觀測(cè)量仍舊選擇觀測(cè)角度，其模型可描述為

式中νk為k時(shí)刻的觀測(cè)噪聲。

3.3粒子濾波實(shí)現(xiàn)模型

1）初始化采樣

在目標(biāo)跟蹤系統(tǒng)中，粒子群的初始化采樣是根據(jù)狀態(tài)方程在某一時(shí)刻狀態(tài)加上系統(tǒng)噪聲獲得的，而靜止目標(biāo)是計(jì)算出來(lái)的，計(jì)算的依據(jù)本身包含噪聲，不夠準(zhǔn)確，若在此基礎(chǔ)上，再附加噪聲已經(jīng)沒(méi)有意義。于是用前面m個(gè)數(shù)據(jù)計(jì)算出的各種狀態(tài)組成具有N個(gè)粒子的粒子群，作為初始粒子群。初始粒子群表示為{X（i）0}Ni=1。

2）k時(shí)刻采樣

k取（1，2，…，n），從第m+1個(gè)數(shù)據(jù)開(kāi)始算起。

3）k時(shí)刻權(quán)值計(jì)算并歸一化

利用新的觀測(cè)值來(lái)計(jì)算每一個(gè)粒子的權(quán)值，δ2為觀測(cè)噪聲的方差。

歸一化的公式形式同式（6）。

4）k時(shí)刻重采樣

對(duì)計(jì)算完歸一化權(quán)值的粒子集進(jìn)行重采樣，重采樣的方法采用系統(tǒng)重采樣，保留權(quán)值大的粒子，舍去權(quán)值小的粒子。

5）k時(shí)刻的狀態(tài)輸出

利用式（7）輸出估算的目標(biāo)狀態(tài)。

從式（11）可以看出，初始化的粒子群是后面每一個(gè)時(shí)刻的粒子集，其好壞對(duì)定位結(jié)果影響很大。為了使定位結(jié)果有保證，對(duì)給出的m個(gè)數(shù)據(jù)組成初始粒子集進(jìn)行了處理：1）粒子不能選擇的太少，最好超過(guò)50個(gè)；2）在計(jì)算目標(biāo)前，先根據(jù)觀測(cè)角度進(jìn)行排序，然后用相隔一定區(qū)間的數(shù)據(jù)進(jìn)行計(jì)算；3）利用一定的閾值，將明顯偏離的數(shù)據(jù)去除。經(jīng)過(guò)這樣的處理，定位準(zhǔn)確度會(huì)有所提高。

4　仿真試驗(yàn)

基于LabVIEW平臺(tái)對(duì)建立的模型進(jìn)行仿真實(shí)驗(yàn)。

以移動(dòng)監(jiān)測(cè)車的初始位置為坐標(biāo)原點(diǎn)，設(shè)目標(biāo)干擾源的坐標(biāo)為（2070m，1630m）。程序中可以自定義仿真時(shí)測(cè)量數(shù)據(jù)組數(shù)、測(cè)量數(shù)據(jù)步長(zhǎng)、坐標(biāo)及示向角度的噪聲，給出一定的壞點(diǎn)，同時(shí)目標(biāo)干擾源位置可隨機(jī)產(chǎn)生，也可給定。設(shè)定好各種數(shù)據(jù)后，通過(guò)點(diǎn)擊“計(jì)算”，獲得仿真數(shù)據(jù)。

該仿真實(shí)驗(yàn)測(cè)量的是200組數(shù)據(jù)，仿真數(shù)據(jù)步長(zhǎng)為20m，粒子群的個(gè)數(shù)選擇100個(gè)，即取101個(gè)數(shù)據(jù)產(chǎn)生粒子群。

圖2為用剩余的99組數(shù)據(jù)進(jìn)行定位實(shí)驗(yàn)，得到移動(dòng)監(jiān)測(cè)車運(yùn)行路線與定位目標(biāo)。99組數(shù)據(jù)并沒(méi)有完全用上，根據(jù)規(guī)則，去除了明顯不合理的數(shù)據(jù)，實(shí)際參與計(jì)算的數(shù)據(jù)組為66組。

圖3為迭代次數(shù)與定位目標(biāo)間的關(guān)系，從圖可以看出，隨著迭代次數(shù)的增加，定位結(jié)果更趨準(zhǔn)確。

表1給出了定位結(jié)果，并給出定位結(jié)果與目標(biāo)干擾源相比較得到的誤差。可見(jiàn)使用所建立的針對(duì)靜止目標(biāo)進(jìn)行定位的粒子濾波數(shù)學(xué)模型是可行的，定位精度比較高，能夠滿足實(shí)際定位的需要。

圖2　粒子濾波定位結(jié)果

圖3　迭代次數(shù)與定位目標(biāo)結(jié)果圖

表1　定位結(jié)果與誤差

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

本文對(duì)粒子濾波算法在靜止目標(biāo)定位中的建模進(jìn)行了研究，主要根據(jù)實(shí)際的無(wú)線電移動(dòng)監(jiān)測(cè)車?yán)檬鞠蚨刃畔ふ腋蓴_源問(wèn)題而提出。對(duì)建立的模型利用LabVIEW平臺(tái)進(jìn)行了仿真研究，得到了較為準(zhǔn)確的定位結(jié)果。給出的模型為相應(yīng)的研究提供了一定的思路，它與定位方法有望應(yīng)用于實(shí)際的無(wú)線電移動(dòng)監(jiān)測(cè)車系統(tǒng)中。但在實(shí)際應(yīng)用時(shí)的影響因素還需要進(jìn)一步深入研究，對(duì)出現(xiàn)的問(wèn)題還需要進(jìn)行改進(jìn)和處理。

參考文獻(xiàn)

[1]張敬平.基于馬爾可夫鏈蒙特卡羅算法的無(wú)線電定位[J].上海海事大學(xué)學(xué)報(bào)，2010，31（1）：88-90.

[2]曹軼超.無(wú)線電定位算法研究及目標(biāo)運(yùn)動(dòng)分析[D].上海：東華大學(xué)，2009.

[3] ZUO J Y，JIA Y N，GAO Q X. Simplified unscented particle filter for nonlinear/non-gaussian bayesian estimation[J]. Journal of Systems Engineering and Electronics，2013，24（3）：537-544.

[4] CAO B，MA C W，LIU Z T. Improved particle filter based on fine resampling algorithm[J]. The Journal of China Universities of Posts and Telecommunications，2012，19（2）：100-106.

[5] BI J，GAN W, QI L T. A genetic resampling particle filter for freeway traffic-state estimation[J]. Chinese Physics B，2012（6）：599-603.

[6] SUNG Y H，CHUNG W J. Particle-filter-based walking prediction model for occlusion situations[J]. Journal of Measu rement Science and Instrumentation，2013，4（3）：263-266.

[7]陳志敏，薄煜明，吳盤龍，等.基于自適應(yīng)粒子群優(yōu)化的新型粒子濾波在目標(biāo)跟蹤中的應(yīng)用[J].控制與決策，2013，28（2）：193-200.

[8]沈玉娟，王健.基于粒子濾波跟蹤方法研究[J].儀表技術(shù)，2010（3）：55-57.

[9]朱志宇.粒子濾波算法及其應(yīng)用研究[M].北京：科學(xué)出版社，2010：87-102.

[10]陳莉.粒子濾波算法的研究[D].西安：西安電子科技大學(xué)，2012.

[11]曾鈺.機(jī)動(dòng)目標(biāo)無(wú)源測(cè)向定位系統(tǒng)的算法分析與仿真研究[D].太原：中北大學(xué)，2011.

[12]白劍鋒，南建國(guó)，鄔蒙.非線性粒子濾波目標(biāo)跟蹤應(yīng)用仿真[J].計(jì)算機(jī)工程與設(shè)計(jì)，2012，33（4）：1569-1573.

[13]單覃，周文婷，潘磊.淺析粒子濾波算法及其參數(shù)設(shè)置的影響[J].輕工科技，2012（12）：56-109.

[14]吳寶成.粒子濾波重采樣算法研究及其應(yīng)用[D].哈爾濱：哈爾濱工業(yè)大學(xué)，2006.

（編輯：劉楊）

Mathematical modeling for static target location with particle filter

JIANG Chundong，LU Ru，DU Taihang
（School of Control Science and Engineering，Hebei University of Technology，Tianjin 300130，China）

Abstract:Particle filter was used mostly in target tracking of position，but there were few reports about static target location especially the radio monitoring vehicles that their known data were only position coordinates of vehicles and azimuths of target. There are no corresponding reference documents to establish mathematical model. Based on particle filtering theory and referencing the method of mathematical modeling aim at target tracking and combined with the actual need of radio monitoring vehicles for static target location，established mathematical model aim at static target. This model combines classification and preferential measure to improve the location accuracy. Finally，the established model is simulated on LabVIEW and the results show that the modeling is accurate and feasible.

Keywords:location；mathematical model；particle filter；static target

作者簡(jiǎn)介：江春冬（1974-），女，吉林白城市人，講師，博士，研究方向?yàn)橹悄芩惴ā⒂?jì)算電磁場(chǎng)理論。

基金項(xiàng)目：工信部軟課題研究項(xiàng)目（12-MC-KY-14）

收稿日期：2015-02-21；收到修改稿日期：2015-04-16

doi：10.11857/j.issn.1674-5124.2016.02.026

文獻(xiàn)標(biāo)志碼：A

文章編號(hào)：1674-5124（2016）02-0115-04