專利名稱：一種粒子狀態(tài)估計方法
技術(shù)領(lǐng)域：
本發(fā)明屬于目標(biāo)跟蹤技術(shù)領(lǐng)域，具體涉及一種粒子狀態(tài)估計方法。
背景技術(shù)：
在機動目標(biāo)跟蹤領(lǐng)域，粒子濾波得到了深入的研究，如果不考慮其它因素，目標(biāo)跟蹤就是單純的濾波估計問題。粒子濾波由于適用于任何能用狀態(tài)空間模型表示的非線性系統(tǒng)，以及傳統(tǒng)卡爾曼濾波無法表示的非線性系統(tǒng)，精度可以逼近最優(yōu)估計，所以被大量應(yīng)用與目標(biāo)跟蹤領(lǐng)域。但是其本身也存在著一些問題。粒子濾波需要在一定的假設(shè)條件下，才能獲得比較良好的濾波效果。首先，狀態(tài)方程Xk+1 = fk(xk, Uk)(其中χ代表狀態(tài)，k代表時刻，U代表過程噪聲)和量測方程k = hk(Xk，Vk) (Ζ代表觀測狀態(tài)，V代表觀測噪聲)已知； 其次能夠從過程噪聲分布Uk和先驗分布中進行采樣；再者用于點估計的似然函數(shù)Pkbklxk) 已知。傳統(tǒng)的粒子狀態(tài)估計方法示意圖如

圖1所示，包括如下步驟初始化，從先驗概率密度pog中隨機抽取樣本點<，采樣初始粒子集 P(Xk)(右上角變量i代表粒子，粒子群總數(shù)為N)；重要性采樣，從建議分布重要性密度函數(shù)中采樣粒子《=q(Xk I X10-^Zu)；重要性加權(quán)，根據(jù)公式乂(Iftit) = 乂—!(^/(^[^^(、，-1)計算粒子重要性
N
加權(quán)，并且進行歸一化； Wk， ；=1狀態(tài)估計1丨=|；《#，由粒子權(quán)值及狀態(tài)估計目標(biāo)狀態(tài)值；
1=\重采樣步驟在迭代過程中，有效樣本數(shù)會減小即出現(xiàn)粒子退化現(xiàn)象，當(dāng)粒子退化到一定的程度時，粒子群就不能準(zhǔn)確的用來對狀態(tài)進行估值，所以增加重采樣步驟來增加粒子數(shù)量來解決退化問題。根據(jù)權(quán)值種，分別復(fù)制高權(quán)值，丟棄低權(quán)值的粒子，得到N個新粒子，然后歸一化。該步驟是一個可選擇步驟。先驗分布會受粒子濾波環(huán)境的隨機性、跟蹤目標(biāo)的機動性及觀測系統(tǒng)的偏差等條件的影響而很難獲得準(zhǔn)確的信息。在不準(zhǔn)確的觀察空間中采用權(quán)重加權(quán)的粒子重要性進行估計真實值，那么明顯不會獲得準(zhǔn)確的估計。同樣后驗概率更加難以得到，所以考慮從重要性函數(shù)中抽樣得到。在很多情況下，從最優(yōu)重要密度函數(shù)抽樣是比較困難的，一種比較簡便而且易于實現(xiàn)的方法就是選擇概率密度函數(shù)作為重要密度函數(shù)。即14^,1)=^( Κ—:)，很顯然，將上式帶入重要密度計算公式可得 ^=K-IPiyn IO。由于上式中的重要密度函數(shù)中未能利用最新的測量信息，以先驗密度函數(shù)進行抽樣得到權(quán)系數(shù)的方差比起最優(yōu)密度函數(shù)樹& I χ^—Ο作為重要密度得到的權(quán)系數(shù)其方差要大的多。因其形式更簡單且易于實現(xiàn)，在粒子濾波算法中已經(jīng)被廣泛使用。但是這種替代對于跟蹤系統(tǒng)而言并不適合，因為多維的運動狀態(tài)參量難以測量以及跟蹤探測系統(tǒng)難以實現(xiàn)同時刻大規(guī)模采樣。

發(fā)明內(nèi)容
本發(fā)明的目的是為了克服傳統(tǒng)粒子濾波初始階段由于先驗信息不足而對粒子最初先驗狀態(tài)估計偏差較大甚至錯誤估計導(dǎo)致濾波不穩(wěn)定甚至發(fā)散存在的問題，提出了一種粒子狀態(tài)估計方法。本發(fā)明的技術(shù)方案是一種粒子狀態(tài)估計方法，具體為對目標(biāo)觀測數(shù)據(jù)的狀態(tài)進行估計，得到狀態(tài)信息，將得到的狀態(tài)信息用于粒子濾波中的重要性采樣過程中，對粒子權(quán)值進行修正，得到估計狀態(tài)的粒子群。進一步的，對目標(biāo)觀測數(shù)據(jù)的狀態(tài)進行估計具體使用Kalman濾波器。本發(fā)明的有益效果本發(fā)明的方法通過對目標(biāo)觀測數(shù)據(jù)的狀態(tài)進行估計，得到狀態(tài)信息，將得到的狀態(tài)信息用于粒子濾波中的重要性采樣過程中，對粒子權(quán)值進行修正，進而能夠克服了由于濾波初始階段先驗信息不足而對粒子最初先驗狀態(tài)進行錯誤估計進而導(dǎo)致濾波不穩(wěn)定甚至發(fā)散的問題，提高了收斂的速度和目標(biāo)估計的準(zhǔn)確性。說明書附1為現(xiàn)有的粒子狀態(tài)估計方法原理示意圖。圖2為本發(fā)明的粒子狀態(tài)估計方法流程圖。圖3實驗一中采用現(xiàn)有的粒子狀態(tài)估計方法估計不準(zhǔn)確形成的發(fā)散形態(tài)示意圖。圖4實驗一中采用本發(fā)明的方法跟蹤仿真結(jié)果示意圖。圖5實驗二中采用現(xiàn)有的粒子狀態(tài)估計方法估計不準(zhǔn)確形成的發(fā)散形態(tài)示意圖。圖6實驗二中采用本發(fā)明的方法跟蹤仿真結(jié)果示意圖。
具體實施例方式下面結(jié)合附圖和具體的實施方式對本發(fā)明作進一步的闡述。本發(fā)明的粒子狀態(tài)估計方法，具體為對目標(biāo)觀測數(shù)據(jù)的狀態(tài)進行估計，得到狀態(tài)信息，將得到的狀態(tài)信息用于粒子濾波中的重要性采樣過程中，對粒子權(quán)值進行修正，得到估計狀態(tài)的粒子群。這里，對目標(biāo)觀測數(shù)據(jù)的狀態(tài)進行估計具體可以使用Kalman濾波器。這里借助 Kalman濾波器，是基于最優(yōu)估計理論的方法來對粒子最初先驗狀態(tài)進行估計，采用基于 Kalman濾波的參數(shù)估計方法，對粒子濾波中的粒子狀態(tài)采樣，使之能適用于實際濾波應(yīng)用環(huán)境。這樣就克服了由于濾波初始階段先驗信息不足而對粒子最初先驗狀態(tài)進行錯誤估計進而導(dǎo)致濾波不穩(wěn)定甚至發(fā)散的弊端?？梢钥闯觯@里也可以采用其它形式的濾波器對目標(biāo)觀測數(shù)據(jù)的狀態(tài)進行估計。這里，一方面根據(jù)已有信息建立初始的粒子種群，同時也開啟一個Kalman濾波器。然后探測目標(biāo)位置信息，以該信息進行粒子濾波器的重要性權(quán)值計算和Kalman濾波器的迭代估計，并在起始階段采用Kalman濾波的估值作為最終的結(jié)果，然后對粒子濾波器重要性權(quán)值進行評估分析，如果重要性權(quán)值不能合理反應(yīng)跟蹤系統(tǒng)的先驗情況，粒子都與觀察值相差甚遠，這時候繼續(xù)采用Kalman濾波器的估計值對初始粒子進行全部更新。若能合理反應(yīng)跟蹤系統(tǒng)的先驗情況，為了減少計算量就不需要Kalman濾波器而單獨采用粒子濾波器獨立濾波。通過相應(yīng)幾次迭代，當(dāng)采用Kalman濾波器數(shù)據(jù)修正之后獲得重要性權(quán)值比較符合運動目標(biāo)探測系統(tǒng)隨機性的時候，則粒子濾波可以實現(xiàn)較為穩(wěn)定跟蹤，此時就可以進行正常的粒子濾波器進行運動目標(biāo)參數(shù)估計了。為了便于對本發(fā)明的理解，對本實施例的方法作更進一步詳細(xì)的描述，具體如圖2 所不Si.粒子濾波器(PF)初始化，開啟一個基本粒子濾波器，進行取樣粒子、重要性采樣、權(quán)值歸一化、重采樣及狀態(tài)估計，獨立完成跟蹤目標(biāo)狀態(tài)的濾波預(yù)測；S2.在粒子濾波器對目標(biāo)狀態(tài)進行估計的同時開啟一個Kalman濾波器(KF)，同時對該目標(biāo)進行跟蹤濾波估計；S3.在濾波初始階段，利用Kalman濾波器濾波得到的狀態(tài)估計及狀態(tài)協(xié)方差構(gòu)造重要性密度函數(shù)，粒子濾波器利用該重要性密度函數(shù)進行重要性采樣，計算粒子權(quán)值并歸一化，完成對粒子參數(shù)的修正；這里的修正可以理解為采用Kalman濾波的狀態(tài)估計及狀態(tài)協(xié)方差構(gòu)造重要性密度函數(shù)進行重要性采樣得到的粒子權(quán)值與粒子濾波器只利用觀測進行重要性采樣得到的粒子權(quán)值進行比較，亦即是通過Kalman濾波器重新計算粒子權(quán)值并歸一化，而不是使用粒子濾波器直接利用觀測信息得到的重要性密度函數(shù)進行重要性采樣。S4.對粒子濾波權(quán)值進行評估，當(dāng)重要性權(quán)值符合運動目標(biāo)探測系統(tǒng)隨機性時，則粒子濾波可以實現(xiàn)較為穩(wěn)定跟蹤，撤銷Kalman濾波器，使用粒子濾波器獨立跟蹤；這里，所述的重要性權(quán)值符合運動目標(biāo)探測系統(tǒng)隨機性可以采用如下過程進行判斷，當(dāng)有效采樣大小Nrff < 0.9XN時，則重要性權(quán)值符合運動目標(biāo)探測系統(tǒng)隨機性，其中，
N # 二---，
V r./、2 為粒子權(quán)值，N為粒子群總數(shù)。
!=1 WkS5.重新采樣粒子，進入下一時刻的濾波。本發(fā)明方法的設(shè)計思想是由于粒子濾波在粒子濾波開始階段，先驗信息很難精確獲得，同時在跟蹤系統(tǒng)中也不可能實現(xiàn)對系統(tǒng)初始的探測信息進行大規(guī)模的采樣的缺點， 利用Kalman濾波器線性最優(yōu)的特點對粒子濾波器的初值進行修正。本發(fā)明方法的實質(zhì)是基于最優(yōu)估計理論的方法來對粒子最初先驗狀態(tài)進行估計， 采用基于Kalman濾波器濾波的參數(shù)估計方法，對粒子濾波中的粒子狀態(tài)采樣，使之能適用于實際濾波應(yīng)用環(huán)境。當(dāng)粒子濾波系統(tǒng)進入穩(wěn)定跟蹤之后，則不再利用Kalman濾波器進行修正，這樣就克服了由于濾波初始階段先驗信息不足而對粒子最初先驗狀態(tài)進行錯誤估計進而導(dǎo)致濾波不穩(wěn)定甚至發(fā)散的問題。為評估本發(fā)明方法的性能，進行了不同條件下的仿真實驗。假定目標(biāo)在二維平面上運動，χ方向和y方向的觀測互相獨立，且觀測噪聲均方差相同，對觀測的航跡信息進行濾波。仿真實驗一采樣周期0. 1秒，目標(biāo)χ軸方向初始速度為515m/s，y軸方向初始速度為Om/s，χ軸加速度為Om/s2，y軸加速度為9. 8m/s2，量測的標(biāo)準(zhǔn)偏差為50m。采用基本粒子濾波方法，當(dāng)目標(biāo)初始狀態(tài)估計不準(zhǔn)確時候，采用前三次探測的值來進行初始估計，其濾波圖像即二維運動目標(biāo)仿真結(jié)果如圖3所示，其中，縱軸表示目標(biāo)運行的Y軸坐標(biāo)，橫軸表示目標(biāo)運行的X軸坐標(biāo)，單位為米。目標(biāo)初始位置為(0，12100)，然后開始進行勻加速運動，該仿真用以模擬一個平拋運動，目標(biāo)受到向下的引力，向下方運行。 其中，實線代表了目標(biāo)運行的真實航跡，χ型曲線是加上了量測誤差之后的系統(tǒng)觀察值，而點線為濾波器對量測觀察值獲得之后濾波值。濾波曲線在觀察不久就偏離了真實航跡，表明本次濾波不成功，目標(biāo)跟蹤出現(xiàn)了發(fā)散。從圖3可以發(fā)現(xiàn)，初始階段粒子濾波僅能根據(jù)探測的點跡來估計運動狀態(tài)的初始情況，在這種情況下，估計值具有比較大偏差，因此隨著偏差的增大，粒子狀態(tài)愈來愈偏離真實目標(biāo)航跡。這樣就造成了跟蹤丟失的結(jié)果，難以滿足實際跟蹤系統(tǒng)需求。同樣在上述條件下，采用本發(fā)明所述的方法，首先同時啟動Kalman濾波器和粒子濾波進行跟蹤航跡的濾波。粒子濾波初始狀態(tài)采用Kalman濾波器計算的結(jié)果來產(chǎn)生，當(dāng)粒子濾波跟蹤穩(wěn)定之后，不再使用Kalman濾波的信息，而采用本身的跟蹤來進行。二維運動目標(biāo)仿真結(jié)果如圖4所示，其中，縱軸表示目標(biāo)運行的Y軸坐標(biāo)，橫軸表示目標(biāo)運行的X軸坐標(biāo)，單位為米。目標(biāo)初始位置為(0，12100)，然后開始進行勻加速運動， 該仿真用以模擬一個平拋運動，目標(biāo)受到向下的引力，向下方運行。其中，實線代表了目標(biāo)運行的真實航跡，χ型曲線是加上了量測誤差之后的系統(tǒng)觀察值，而點線為濾波器對量測觀察值獲得之后濾波值。從圖中可以看出濾波航跡在觀察不久就開始比觀測航跡更加接近于真實航跡，表明本次濾波成功的實現(xiàn)對目標(biāo)進行跟蹤，噪聲得到了抑制。從圖4可以看出，雖然開始階段由于Kalman濾波器也不能很快的就檢測出目標(biāo)的初始狀態(tài)，但是隨后當(dāng)檢測出來的目標(biāo)狀態(tài)與真實數(shù)據(jù)比較接近的時候，粒子濾波就可以采用該數(shù)值作為粒子初始估計狀態(tài)，然后開始生成分布粒子，從而在隨后的跟蹤中，慢慢收斂于真實航跡上，而不會出現(xiàn)跟蹤航跡的發(fā)散。仿真實驗二 采樣周期0. 1秒，目標(biāo)χ軸方向初始速度為515m/s，y軸方向初始速度為Om/s，χ軸加速度為-16m/s2，y軸加速度為9. 8m/s2，量測的標(biāo)準(zhǔn)偏差為50m。相對于仿真實驗一，仿真實驗二在兩個方向都具有了加速度變化。依然分別采用傳統(tǒng)粒子濾波方法和本發(fā)明的方法進行仿真。圖5是采用基本粒子濾波方法的仿真結(jié)果，其中，縱軸表示目標(biāo)運行的Y軸坐標(biāo)， 橫軸表示目標(biāo)運行的X軸坐標(biāo)，單位為米。目標(biāo)初始位置為(0，0)，然后開始進行勻加速運動，該仿真用以模擬一個轉(zhuǎn)彎運動，目標(biāo)受到一個轉(zhuǎn)彎的加速度，該加速度在X軸和Y軸上都有分量，引起目標(biāo)的一個360度的轉(zhuǎn)彎運動。其中，實線代表了目標(biāo)運行的真實航跡，χ 型曲線是加上了量測誤差之后的系統(tǒng)觀察值，而點線為濾波器對量測觀察值獲得之后濾波值。濾波航跡在觀察不久就開始落后與真實航跡，實際過程中，這樣大的誤差會導(dǎo)致預(yù)測不到目標(biāo)位置，造成跟蹤發(fā)散。在目標(biāo)上述運動條件下，由于初始狀態(tài)估計不準(zhǔn)確，隨著目標(biāo)運動時間的不斷延長，跟蹤的誤差積累愈大。最終目標(biāo)將飛出監(jiān)控探測預(yù)測范圍，從而不利于實際監(jiān)控過程中對目標(biāo)的預(yù)測濾波。圖6是采用本發(fā)明的方法的仿真結(jié)果，其中，縱軸表示目標(biāo)運行的Y軸坐標(biāo)，橫軸表示目標(biāo)運行的X軸坐標(biāo)，單位為米。目標(biāo)初始位置為(0，0)，然后開始進行勻加速運動，該仿真用以模擬一個轉(zhuǎn)彎運動，目標(biāo)受到一個轉(zhuǎn)彎的加速度，該加速度在X軸和Y軸上都有分量，引起目標(biāo)的一個360度的轉(zhuǎn)彎運動。其中，實線代表了目標(biāo)運行的真實航跡，χ型曲線是加上了量測誤差之后的系統(tǒng)觀察值，而點線為濾波器對量測觀察值獲得之后濾波值。從上圖中可以看出濾波曲線在觀察不久就開始比χ型曲線更加接近于真實航跡，表明本次濾波成功的實現(xiàn)對目標(biāo)進行跟蹤，噪聲得到了抑制。從圖6可以看出，本發(fā)明的方法可以實現(xiàn)對目標(biāo)的精確跟蹤，不會出現(xiàn)發(fā)散。本領(lǐng)域的普通技術(shù)人員將會意識到，這里所述的實施例是為了幫助讀者理解本發(fā)明的原理，應(yīng)被理解為本發(fā)明的保護范圍并不局限于這樣的特別陳述和實施例。本領(lǐng)域的普通技術(shù)人員可以根據(jù)本發(fā)明公開的這些技術(shù)啟示做出各種不脫離本發(fā)明實質(zhì)的其它各種具體變形和組合，這些變形和組合仍然在本發(fā)明的保護范圍內(nèi)。
權(quán)利要求
1.一種粒子狀態(tài)估計方法，具體為對目標(biāo)觀測數(shù)據(jù)的狀態(tài)進行估計，得到狀態(tài)信息， 將得到的狀態(tài)信息用于粒子濾波中的重要性采樣過程中，對粒子權(quán)值進行修正，得到估計狀態(tài)的粒子群。
2.根據(jù)權(quán)利要求1所述的粒子狀態(tài)估計方法，其特征在于，所述的對目標(biāo)觀測數(shù)據(jù)的狀態(tài)進行估計具體使用Kalman濾波器。
3.根據(jù)權(quán)利要求2所述的粒子狀態(tài)估計方法，其特征在于，具體包括如下步驟51.粒子濾波器初始化，開啟一個基本粒子濾波器，進行取樣粒子、重要性采樣、權(quán)值歸一化、重采樣及狀態(tài)估計，獨立的完成跟蹤目標(biāo)狀態(tài)的濾波預(yù)測；52.在粒子濾波器對目標(biāo)狀態(tài)進行估計的同時開啟一個Kalman濾波器，同時對該目標(biāo)進行跟蹤濾波估計；53.在濾波初始階段，利用Kalman濾波器濾波得到的狀態(tài)估計及狀態(tài)協(xié)方差構(gòu)造重要性密度函數(shù)，粒子濾波器利用該重要性密度函數(shù)進行重要性采樣，計算粒子權(quán)值并歸一化， 完成對粒子參數(shù)的修正；54.對粒子濾波權(quán)值進行評估，當(dāng)重要性權(quán)值符合運動目標(biāo)探測系統(tǒng)隨機性時，則粒子濾波可以實現(xiàn)較為穩(wěn)定跟蹤，撤銷Kalman濾波器，使用粒子濾波器獨立跟蹤；55.重新采樣粒子，進入下一時刻的濾波。
4.根據(jù)權(quán)利要求3所述的粒子狀態(tài)估計方法，其特征在于，步驟S4所述的重要性權(quán)值符合運動目標(biāo)探測系統(tǒng)隨機性采用如下過程進行判斷，當(dāng)有效采樣大小Nrff < 0. 9XN時，N # 二---，則重要性權(quán)值符合運動目標(biāo)探測系統(tǒng)隨機性，其中，eff為粒子權(quán)值，N為粒子t=l Wk群總數(shù)。
全文摘要
本發(fā)明公開了一種粒子狀態(tài)估計方法。本發(fā)明是針對傳統(tǒng)粒子濾波初始階段由于先驗信息不足而對粒子最初先驗狀態(tài)估計偏差較大甚至錯誤估計導(dǎo)致濾波不穩(wěn)定甚至發(fā)散存在的問題而提出的。本發(fā)明的方法通過對目標(biāo)觀測數(shù)據(jù)的狀態(tài)進行估計，得到狀態(tài)信息，將得到的狀態(tài)信息用于粒子濾波中的重要性采樣過程中，對粒子權(quán)值進行修正，進而能夠克服了由于濾波初始階段先驗信息不足而對粒子最初先驗狀態(tài)進行錯誤估計進而導(dǎo)致濾波不穩(wěn)定甚至發(fā)散的問題，提高了收斂的速度和目標(biāo)估計的準(zhǔn)確性。
文檔編號G06T7/20GK102542577SQ20111043563
公開日2012年7月4日 申請日期2011年12月22日 優(yōu)先權(quán)日2011年12月22日
發(fā)明者周雪, 方慶, 沈振, 賈海濤, 陸逸, 鞠初旭 申請人:電子科技大學(xué)