基于分治采樣粒子濾波的三維跟蹤算法*

鄧 琪，陳 剛*，魯華祥，張珊珊，陳艾東，郭祉薇

（1.中國科學(xué)院半導(dǎo)體研究所高速電路與神經(jīng)網(wǎng)絡(luò)實驗室，北京 100083；2.中國科學(xué)院大學(xué)，北京 100089；3.北京聯(lián)合大學(xué)，北京 100101）

0 引言

機動目標跟蹤是雷達跟蹤、視頻監(jiān)控、移動機器人等許多實際應(yīng)用中的一項基礎(chǔ)性和關(guān)鍵性任務(wù)，具有廣泛的軍事和民用背景［1］。目前國內(nèi)外大量研究都集中在二維平面，當(dāng)目標在三維空間各方向都出現(xiàn)強度不一致的高機動時，通過二維模型和算法的擴展已無法準確描述目標運動，因此，三維空間機動目標跟蹤已成為該類問題的難點之一［2］。

機動目標跟蹤本質(zhì)是利用傳感器的離散量測值來估計目標的連續(xù)狀態(tài)，主要包括目標機動模型的建模、機動檢測或機動辨識以及濾波算法［3］。粒子濾波（Particle Filter，PF）由于適用于強非線性非高斯系統(tǒng)，常被作為機動目標跟蹤過程中的濾波算法。但粒子濾波本身存在粒子退化問題和粒子匱乏現(xiàn)象，且算法復(fù)雜度很大程度上依賴于粒子數(shù)目［4］。為解決該問題，多數(shù)研究都致力于改進粒子重采樣步驟，通常基于傳統(tǒng)重采樣機制進行改進，如分層重采樣［5］、自適應(yīng)重采樣［6］、確定性重采樣［7］等；另一種新的發(fā)展方向是通過引入群智能優(yōu)化思想增加樣本多樣性，如遺傳算法［8］、螢火蟲算法［9］、蝙蝠算法［10］等優(yōu)化重采樣。以上方法通過改進粒子重采樣環(huán)節(jié)能一定程度上增加樣本多樣性，降低粒子退化程度。但當(dāng)處理三維空間中的機動目標跟蹤問題時，由于目標不同方向上機動方式和強度不一致，狀態(tài)空間會出現(xiàn)某些區(qū)域的粒子分布稀疏，難以均勻覆蓋，致使算法迭代時粒子加劇退化，導(dǎo)致樣本多樣性降低。隨著目標運動模型復(fù)雜度增加，算法性能下降的更為明顯，常需增加粒子數(shù)量來保證覆蓋范圍，運算時間較長，無法保證跟蹤實時性和良好的跟蹤精度。

針對以上問題，本文提出一種基于分治采樣［11］的粒子濾波三維機動目標跟蹤算法（Partition Sampling Particle Filter，PS-PF），通過分解運動空間，子空間內(nèi)獨立抽樣粒子，降維處理目標機動，解決三維空間中粒子分布稀疏的問題，減緩粒子退化問題和粒子匱乏現(xiàn)象對目標跟蹤造成的影響。同時為方便數(shù)據(jù)處理，加入量測信息預(yù)處理模塊，實現(xiàn)量測信息的坐標轉(zhuǎn)換。通過在多個運動模型上的仿真比較，結(jié)果表明本文所提出的算法在跟蹤性能上具有顯著優(yōu)勢。

1 三維空間運動模型

1.1 狀態(tài)模型

考慮目標的狀態(tài)模型如下［12］：

1.2 觀測模型

笛卡爾坐標系中，目標狀態(tài)在各維度中相互獨立，為便于處理，考慮目標觀測模型如下：

2 觀測數(shù)據(jù)預(yù)處理

實際應(yīng)用中，機動目標的觀測數(shù)據(jù)由雷達等傳感器測量獲得，所得到的觀測數(shù)據(jù)多基于球坐標，包括徑向距離r，方位角b 和俯仰角e，可表示為［13］：

其中，vr、vb、ve分別為徑向距離、方位角和俯仰角的觀測噪聲。

基于球坐標的觀測數(shù)據(jù)具有很強的非線性，需要對觀測數(shù)據(jù)進行坐標轉(zhuǎn)換。假設(shè)兩坐標系間存在坐標變換φ=h-1，其中h=［hr，hb，he］T，當(dāng)已知系統(tǒng)觀測值Z=［r，b，e］T，可以得出笛卡爾坐標系中的觀測值：

轉(zhuǎn)換后笛卡爾坐標系中的真實觀測數(shù)據(jù)可表示為：

這里vx、vy、vz為轉(zhuǎn)換后的觀測噪聲，其協(xié)方差矩陣為Rc =J（Z）×R×J（Z），J（Z）為觀測量的Jacobian行列式，R 為轉(zhuǎn)換前觀測噪聲協(xié)方差矩陣。

3 分治采樣粒子濾波算法

3.1 粒子濾波

粒子濾波以貝葉斯估計為解決框架，基于蒙特卡洛方法，通過尋找一組在狀態(tài)空間傳播的隨機樣本對概率密度函數(shù)進行近似，以樣本均值代替積分運算，從而獲得狀態(tài)最小方差。該算法不需要對系統(tǒng)做任何先驗假設(shè)，適用于強非線性非高斯系統(tǒng)，具有很好的算法可擴展性和普適性。

粒子濾波算法描述如下［3-4］：

3.1.1 初始化

k=0 時，由先驗概率p（X0）產(chǎn)生粒子集，其中。

3.1.2 更新

權(quán)值計算，根據(jù)權(quán)值更新公式計算粒子新的權(quán)值：

狀態(tài)估計輸出：

3.1.3 重采樣

3.1.4 預(yù)測

3.2 分治采樣方法

對于粒子濾波算法，狀態(tài)空間維數(shù)越高意味著所需粒子數(shù)更多，從而保證粒子的空間覆蓋率足夠廣。隨著狀態(tài)維數(shù)增加，算法復(fù)雜度呈指數(shù)增長。分治采樣方法是將系統(tǒng)狀態(tài)空間分割成若干個獨立子空間，每個子空間用各自最佳的策略獨立抽樣粒子，并在低維空間預(yù)測最優(yōu)子狀態(tài)，最后輸出為所有預(yù)測子狀態(tài)的加權(quán)和。

注意到在笛卡爾坐標系中，目標在運動空間各方向的運動狀態(tài)相互獨立，不受其他方向上的運動影響，即運動空間是三維正交的。根據(jù)運動分解與合成思想，目標整體運動狀態(tài)可以表示為各方向運動的疊加。圖1 為三維空間分治采樣方法結(jié)構(gòu)圖。

引入分治法思想，對三維運動空間進行分割，假設(shè)分別在一維子空間中采樣粒子點，各方向分別采樣Nx、Ny、Nz個隨機樣本，那么粒子總數(shù)為（Nx+Ny+Nz），總體粒子多樣性能達到Nx×Ny×Nz種。若直接在三維空間中采樣同樣數(shù)量的粒子，粒子多樣性只有（Nx+Ny+Nz）種，顯然前者采樣粒子的空間覆蓋范圍更廣，能一定程度上降低粒子退化現(xiàn)象、提高預(yù)測精度。

下面對算法復(fù)雜度進行分析。根據(jù)本文第1 節(jié)分析，狀態(tài)向量維數(shù)為9，觀測向量維數(shù)為3。設(shè)粒子采樣總數(shù)為N 個，濾波算法中狀態(tài)一步預(yù)測時間為Tf，量測一步預(yù)測時間為Th。當(dāng)采用分治采樣方法，狀態(tài)空間分解為3 個獨立子空間，各子空間內(nèi)抽取樣本數(shù)相同，狀態(tài)一步預(yù)測時間降低為Tf/9，量測一步預(yù)測時間降低為Th/3。對于同一個模型，傳統(tǒng)采樣濾波方法的復(fù)雜度為N×（Tf+Th），分治采樣濾波方法的復(fù)雜度為N×（Tf+3Th）/9。兩種方法的復(fù)雜度同階，但分治采樣方法運算時間上要小于傳統(tǒng)方法。當(dāng)Tf＞＞Th，算法復(fù)雜度由狀態(tài)一步預(yù)測時間主導(dǎo)，隨著目標狀態(tài)模型復(fù)雜性上升，算法運算時間能削減至原算法的12%。

圖1 三維空間分治采樣方法結(jié)構(gòu)圖

3.3 基于分治采樣的粒子濾波跟蹤算法

基于分治采樣粒子濾波的三維機動目標跟蹤算法的基本思想，是將三維運動空間按照笛卡爾坐標系分解成3 個獨立的一維狀態(tài)子空間，使用粒子濾波算法在各子空間內(nèi)分別按照最優(yōu)采樣策略采取樣本點，增加粒子多樣性，并結(jié)合坐標系變換后的觀測數(shù)據(jù)預(yù)測子狀態(tài)，最后所有子狀態(tài)合成得到最終的輸出狀態(tài)?；诜种尾蓸拥牧Ｗ訛V波跟蹤算法步驟描述如下：

3.3.1 初始化

3.3.2 預(yù)測

3.3.3 輸出狀態(tài)合成

4 仿真與分析

為驗證算法有效性，進行兩個仿真實驗：三維勻速運動目標跟蹤和基于CS 模型的機動目標跟蹤。仿真實驗中將本文算法與標準粒子濾波進行對比，采用均方根誤差進行評價：

4.1 三維勻速運動目標跟蹤

設(shè)定傳感器的采樣間隔t=1 s，采樣時長T=200 s，給定目標的初始狀態(tài)X0=［-20 m，2 m/s，0 m/s2，10 m，1.5 m/s，0 m/s2，5 m，1 m/s，0 m/s2］T，運動模型為勻速直線運動模型（CV 模型）［14］，即：

過程噪聲與觀測噪聲都服從N（0，1）高斯分布。

圖2 三維勻速目標運動軌跡

目標運動軌跡如圖2 所示。圖3 為算法中采樣粒子數(shù)目與算法性能變化圖，增加粒子數(shù)目會提高算法跟蹤精度，但當(dāng)粒子數(shù)達到一定值后，算法性能將逐漸趨于平穩(wěn)，即增加粒子數(shù)不會帶來明顯的精度提升，而是收斂到所謂的最優(yōu)精度。

圖3 粒子數(shù)目與勻速目標跟蹤性能變化趨勢

由圖3 可知，標準粒子濾波在總粒子數(shù)達到160 后性能開始趨于穩(wěn)定，而基于分治采樣的算法在總粒子數(shù)為100 時就開始進入收斂狀態(tài)。為保證實驗結(jié)果可靠性，算法采用的粒子數(shù)目選擇在逼近最優(yōu)精度的平穩(wěn)狀態(tài)時所需粒子數(shù)進行重復(fù)實驗，標準粒子濾波算法選取粒子總數(shù)200 個，本文所提出的算法選取粒子總數(shù)150 個，每種算法進行1 000次蒙特卡羅仿真。

表1 三維勻速目標跟蹤結(jié)果對比

表1 為兩種算法在勻速運動目標跟蹤實驗中的數(shù)據(jù)結(jié)果，由于運動模型較為簡單，兩種算法在跟蹤精度上差距不大。其中標準粒子濾波每次需采樣200 個粒子才能達到較為穩(wěn)定的跟蹤效果。PS-PF 引入了降維處理思想，通過劃分獨立的運動空間，每個運動方向只需分別采樣50 個隨機樣本，共計150 個樣本點，在保證精度一定程度提升的同時，算法運行時間降低約65%。

4.2 基于當(dāng)前統(tǒng)計模型的三維機動運動目標跟蹤

設(shè)定傳感器的采樣間隔t=1 s，采樣時長T=200 s，同樣給定目標初始狀態(tài)X0=［-20 m，2 m/s，0 m/s2，10 m，1.5 m/s，0 m/s2，5 m，1 m/s，0 m/s2］T，機動目標參數(shù)［9］如表2 所示。參數(shù)設(shè)置為機動頻率f=1/60，最大加速度αmax=1 m/s2，過程噪聲與觀測噪聲同樣都服從N（0，1）高斯分布。

表2 機動目標參數(shù)

三維機動目標運動軌跡如下頁圖4 所示，圖5為兩種算法中粒子數(shù)目與機動目標跟蹤性能變化趨勢圖，為保證實驗結(jié)果可靠性，算法選擇在最優(yōu)精度的平穩(wěn)狀態(tài)時所需粒子數(shù)進行重復(fù)實驗，標準粒子濾波算法選取1 000 個粒子，本文算法選取150 個粒子，每種算法進行1 000 次蒙特卡羅仿真。

圖4 三維機動目標運動軌跡

圖5 粒子數(shù)目與機動目標跟蹤性能變化趨勢

當(dāng)機動目標按表2 加速度變化的方式進行運動時，兩種算法的跟蹤結(jié)果對比如表3 所示，圖6為兩種算法各方向的誤差分布圖。標準粒子濾波算法在采樣1 000 個粒子點的情況下，目標各方向跟蹤位置的均方根誤差達到3.8 m 左右；而采用分治采樣策略的改進算法各方向只需采樣50 個粒子，在總粒子數(shù)為150 的情況下，目標位置的各方向均方根誤差能降低到1.9 m 左右，且算法的運行時間有了顯著降低。在該仿真實驗中，標準粒子濾波與分治采樣粒子濾波，二者粒子數(shù)比例為6.7∶1，跟蹤誤差為2∶1，運行時間為14∶1。顯然基于分治采樣的粒子濾波相較于標準粒子濾波，精度提升了一倍，算法時間減少到原來的7%，無論跟蹤精度還是運算時間上，都具有非常顯著的優(yōu)勢。

結(jié)合以上兩個仿真實驗結(jié)果分析，隨著運動模型復(fù)雜度的增加，標準粒子濾波算法所要采樣的粒子數(shù)也隨之增加，算法計算量很大程度上依賴于粒子數(shù)目，這將帶來了更大的計算量。當(dāng)粒子總數(shù)為1 000 個，算法運算時間長達11 s。且在復(fù)雜模型中，增加粒子數(shù)目并未使粒子的多樣性達到實際所需，并存在大量冗余且無效的粒子，跟蹤精度也有所下降?；诜种尾蓸拥母倪M粒子濾波通過劃分獨立子空間，在各子空間中分別抽取樣本點，以此增加樣本多樣性，對機動目標的運動實現(xiàn)降維處理，在提高跟蹤精度的同時大大降低了算法的單步迭代時間，更好地保證了目標跟蹤的實時性和有效性。綜上所述，本文所提出的基于分治采樣的改進粒子濾波跟蹤算法具有更高的綜合效率比。

表3 三維機動目標跟蹤結(jié)果對比

圖6 各方向目標跟蹤誤差

5 結(jié)論

針對粒子濾波三維運動空間中樣本多樣性較低，存在運算冗余等問題，本文提出一種基于分治采樣的粒子濾波跟蹤算法，通過分解獨立狀態(tài)子空間，各子空間內(nèi)分別采取最優(yōu)采樣策略采樣粒子，對目標運動進行降維處理，解決了標準粒子濾波在三維運動空間中粒子分布稀疏的問題，有效緩解了粒子退化問題，在提高樣本多樣性的同時降低了算法復(fù)雜度。仿真結(jié)果表明，該算法能大大降低同等精度條件下的算法運算時間，與標準粒子濾波相比具有更好的跟蹤性能，且運動模型越復(fù)雜越能突顯其優(yōu)勢，具有較好的應(yīng)用前景。