粒子濾波算法的FPGA設(shè)計(jì)與實(shí)現(xiàn)

毛麗民，劉叔軍，朱培逸，張水平

粒子濾波算法是一種基于蒙特卡羅方法和遞推貝葉斯估計(jì)的濾波方法，主要用于非線性、非高斯系統(tǒng)，發(fā)展于20世紀(jì)90年代.粒子濾波雖然在理論上取得了很多研究成果，但在硬件實(shí)現(xiàn)方面，僅僅處于初級(jí)階段，在實(shí)際信號(hào)處理領(lǐng)域還沒(méi)有得到廣泛的應(yīng)用，主要是因?yàn)榱Ｗ訛V波算法的結(jié)構(gòu)復(fù)雜、運(yùn)算量大，需要很大的存儲(chǔ)空間［1］.雖然有些改進(jìn)的粒子濾波算法在一定程度上提高了粒子濾波算法精度，但使得算法更加復(fù)雜，實(shí)現(xiàn)更加困難.

FPGA不僅存儲(chǔ)容量大，而且能真正實(shí)現(xiàn)粒子濾波的并行運(yùn)算，提高運(yùn)算速度［2］.本文基于FPGA的粒子濾波算法硬件實(shí)現(xiàn)，以二維純方位目標(biāo)追蹤為例驗(yàn)證了基于FPGA的粒子濾波算法，不僅在一定程度上簡(jiǎn)化粒子濾波算法，節(jié)省了硬件存儲(chǔ)資源，而且提高了算法的運(yùn)行速度.

1 粒子濾波算法簡(jiǎn)介

粒子濾波算法原理如圖1所示［3］.圖中粒子濾波算法主要由時(shí)間更新、觀測(cè)更新和重采樣3步組成.k-1時(shí)刻的先驗(yàn)概率由N個(gè)權(quán)值為1/N的粒子近似表示.在時(shí)間更新中，使用狀態(tài)轉(zhuǎn)移方程預(yù)測(cè)每個(gè)粒子在k時(shí)刻的狀態(tài).使用觀測(cè)值更新粒子權(quán)值.在重采樣過(guò)程中權(quán)值小的粒子將被拋棄，保留權(quán)值較大的粒子，權(quán)值被重新設(shè)為1/N.

圖1 SIR算法示意圖

2 ISE System Generator與Matlab聯(lián)合開(kāi)發(fā)FPGA

ISE System Generator提供了一種軟/硬件相結(jié)合的設(shè)計(jì)環(huán)境，與傳統(tǒng)的編寫(xiě)代碼的開(kāi)發(fā)模式不同，而且提供了豐富的模塊庫(kù)，只要能用Verilog語(yǔ)言實(shí)現(xiàn)功能都用系統(tǒng)提供的模塊實(shí)現(xiàn)，并且模塊中包含很多已經(jīng)成熟的算法，可以減少工作量，提高開(kāi)發(fā)效率［4］.

3 簡(jiǎn)化粒子濾波算法FPGA實(shí)現(xiàn)

基于FPGA的純方位目標(biāo)跟蹤算法，如圖2所示，主要由4部分組成:粒子存儲(chǔ)及采樣模塊、粒子權(quán)值和閾值計(jì)算模塊、粒子的重采樣模塊、粒子的狀態(tài)估計(jì)及輸出模塊.每個(gè)變量都采用定點(diǎn)數(shù)表示，本文采用18位表示位置信息，用16位表示相位信息［5］.

圖2 粒子濾波算法的實(shí)現(xiàn)框圖

3.1 存儲(chǔ)采樣模塊

存儲(chǔ)模塊主要用于存儲(chǔ)采樣前粒子的狀態(tài)，其具體實(shí)現(xiàn)框圖如圖3所示.

圖3 存儲(chǔ)模塊實(shí)現(xiàn)框圖

圖3 中xk RAM用于存儲(chǔ)粒子X(jué)方向的位置，vxk RAM用于存儲(chǔ)粒子X(jué)方向的速度.采樣模塊就主要將噪聲生成模塊產(chǎn)生的噪聲和速度值相加;同時(shí)，噪聲向右移一位后加上位置的更新值.其實(shí)現(xiàn)框圖如圖4所示［6］.

圖4 采樣更新單元結(jié)構(gòu)

本文設(shè)計(jì)的噪聲生成模塊是根據(jù)查找表的噪聲生成方式實(shí)現(xiàn)的，具體實(shí)現(xiàn)結(jié)構(gòu)如圖5所示.該圖中，噪聲生成模塊主要由一個(gè)線性反饋移位寄存器(LFSR)與兩個(gè)高斯白噪聲生成器組成，其中LFSR用于生成L-bits的偽隨機(jī)數(shù)，高斯白噪聲生成器［1］用來(lái)產(chǎn)生零均值方差為1的高斯白噪聲.圖5表示噪聲生成模塊所生成的X方向的噪聲隨機(jī)數(shù).

圖5 噪聲生成模塊

高斯白噪聲生成器的內(nèi)部結(jié)構(gòu)如圖6所示:主要是由4個(gè)Box-Muller和3個(gè)加法器組成，用來(lái)生成零均值方差為1的高斯白噪聲［7］.

圖6 高斯白噪聲生成器內(nèi)部結(jié)構(gòu)

3.2 權(quán)值及閥值計(jì)算模塊

本文選取重要密度函數(shù)為先驗(yàn)概率密度［8］，權(quán)值更新公式簡(jiǎn)化為:

由于零均值的高斯白噪聲為觀測(cè)噪聲，所以權(quán)值的計(jì)算公式為:

如果 N^eff值較小，k-1時(shí)刻進(jìn)行粒子重采樣，則=1/N;如果 N^eff值較大，k-1時(shí)刻不進(jìn)行重采樣，則=1/N，將權(quán)值歸一化處理，

可以忽略，所以權(quán)值計(jì)算公式簡(jiǎn)化為

圖7為簡(jiǎn)化粒子的權(quán)值計(jì)算實(shí)現(xiàn)框圖.式中exp()與arctan()將用到Xilinx提供的高效IP核.位置信息通過(guò)IP核計(jì)算預(yù)測(cè)的觀測(cè)值，然后與觀測(cè)值比較，得到差值，依據(jù)公式3需要對(duì)差值進(jìn)行平方.當(dāng)差值大于255時(shí)，將該粒子的權(quán)值設(shè)為零，此時(shí)簡(jiǎn)化了硬件結(jié)構(gòu).delay是arctan IP核的運(yùn)行時(shí)間，delay1是指數(shù)計(jì)算時(shí)間，exp指數(shù)運(yùn)算的實(shí)現(xiàn)框圖如圖8所示［9］.

圖7 權(quán)值計(jì)算模塊框圖

圖8 指數(shù)運(yùn)算實(shí)現(xiàn)框圖

在計(jì)算中，指數(shù)計(jì)算由ex=shx+chx表示，其中sh與ch的范圍為［-π/4，π/4］.因此，將指數(shù)運(yùn)算的數(shù)值分為整數(shù)部分和小數(shù)部分.小數(shù)的指數(shù)運(yùn)算由IP core cordic獲取，整數(shù)使用查找表方法.最后將整數(shù)與小數(shù)相乘得到粒子的權(quán)值.

3.3 重采樣模塊

圖9為簡(jiǎn)化重采樣模塊結(jié)構(gòu)圖.因?yàn)閬G棄的粒子與選取的粒子數(shù)目是未知的，故將Single Port RAM2和Single Port RAM1的深度都設(shè)為256.

圖9 簡(jiǎn)化重采樣模塊結(jié)構(gòu)圖

3.4 狀態(tài)估計(jì)輸出模塊

狀態(tài)估計(jì)值輸出模塊(圖10)在使用FPGA實(shí)現(xiàn)時(shí)，與存儲(chǔ)采樣模塊、權(quán)值及閥值計(jì)算模塊并行執(zhí)行，在存儲(chǔ)采樣模塊與權(quán)值和閥值計(jì)算模塊獲得新的粒子以及權(quán)值后通過(guò)乘累加器來(lái)實(shí)現(xiàn).

圖10 狀態(tài)估計(jì)值輸出結(jié)構(gòu)

要得到最終的狀態(tài)估計(jì)值，還需對(duì)每個(gè)粒子及其權(quán)值的乘累加值進(jìn)行比例調(diào)整.比例調(diào)整就是將各個(gè)狀態(tài)值除以Wk和權(quán)值.二維目標(biāo)跟蹤中，有4個(gè)狀態(tài)值，因此需要四次除法過(guò)程，但除法器占用Slice資源比較多.因此采用4個(gè)乘法器，先求出權(quán)值和的倒數(shù)l/Wk，最后將每個(gè)狀態(tài)值輸出相乘.

4 實(shí)驗(yàn)結(jié)果

System Generator把Xilinx Blockset的元件生成相應(yīng)的IP核庫(kù)組件，從系統(tǒng)設(shè)置的參數(shù)中推斷出電路及控制信號(hào)，并把Simulink的模型對(duì)應(yīng)生成ISE工程.為了確保多速率系統(tǒng)匯編的正確性，System Generator為執(zhí)行工具生成了一些約束文件.很多信息都能在System Generator模塊中配置相關(guān)參數(shù).

將各個(gè)功能模塊連接起來(lái)，在System Generator中配置好相關(guān)參數(shù)后，轉(zhuǎn)換成硬件描述語(yǔ)言VHDL，下載到 FPGA，然后對(duì)整個(gè)模塊進(jìn)行調(diào)試.系統(tǒng)的資源使用情況如表1所示.

表1 資源使用情況對(duì)比

最后將FPGA實(shí)驗(yàn)結(jié)果導(dǎo)入Matlab，與Matlab仿真結(jié)果進(jìn)行對(duì)比，對(duì)比結(jié)果如圖11所示.

圖11 粒子濾波算法的仿真結(jié)果圖

圖11 是系統(tǒng)運(yùn)行50個(gè)時(shí)刻的某一次仿真結(jié)果對(duì)比，選取粒子數(shù)為256個(gè).從圖中可以看出，F(xiàn)PGA實(shí)驗(yàn)結(jié)果與真實(shí)軌跡比較吻合，已能較好地反映真實(shí)軌跡的走勢(shì).

5 結(jié)語(yǔ)

粒子濾波算法是一種以貝葉斯估計(jì)為依據(jù)的濾波算法，廣泛用于非高斯、非線性、時(shí)變系統(tǒng)的濾波及參數(shù)估計(jì).但是，粒子濾波算法結(jié)構(gòu)復(fù)雜，運(yùn)算量大，限制了在實(shí)時(shí)系統(tǒng)中的實(shí)際應(yīng)用.本文在研究算法的基礎(chǔ)上，重點(diǎn)研究了基于FPGA的硬件實(shí)現(xiàn)方法，給出了一種簡(jiǎn)化粒子濾波算法及在FPGA上的實(shí)現(xiàn)方案，并給出了整體結(jié)構(gòu)及各個(gè)功能模塊;最后進(jìn)行下載調(diào)試，達(dá)到了預(yù)期效果.

［1］胡士強(qiáng).粒子濾波原理及其應(yīng)用［M］.北京：科學(xué)出版社，2010：1-7.

［2］劉剛，梁曉庚.遺傳重采樣粒子濾波的目標(biāo)跟蹤研究［J］. 計(jì)算機(jī)工程與應(yīng)用，2010，46(19)：196-199.

［3］高展宏.多媒體處理FPGA實(shí)現(xiàn)［M］.北京：電子工業(yè)出版社，2010：67-70.

［4］洪少華，史治國(guó).用于純方位跟蹤的簡(jiǎn)化粒子濾波算法及其硬件實(shí)現(xiàn)［J］.電子與信息學(xué)報(bào)，2009，31(1)：96-100.

［5］趙梅，張三同.輔助粒子濾波及其仿真應(yīng)用［J］.北京交通大學(xué)學(xué)報(bào)，2006，11(2)：24-28.

［6］王來(lái)雄，黃士坦.一種新的粒子濾波算法［J］.武漢大學(xué)學(xué)報(bào)，2009，39(1)：118-120.

［7］鄧文壇，張三同.一種改進(jìn)的粒子濾波跟蹤算法的研究［J］. 自動(dòng)化技術(shù)與應(yīng)用，2008，27(3)：84-87.

［8］唐濤，張涯輝.粒子濾波在被動(dòng)定位跟蹤中的應(yīng)用［J］. 儀器儀表學(xué)報(bào)，2005，26(8)：211-214.

［9］劉剛，梁曉庚.遺傳重采樣粒子濾波的目標(biāo)跟蹤研究［J］. 計(jì)算機(jī)工程與應(yīng)用，2010，46(19)：196-199.