基于System Generator的簡(jiǎn)化粒子濾波算法設(shè)計(jì)及硬件實(shí)現(xiàn)

王佳輝， 王義平， 薛雅麗

(南京航空航天大學(xué)，南京 210016)

0 引言

粒子濾波算法[1]在處理非線性、非高斯系統(tǒng)的狀態(tài)估計(jì)方面有明顯的優(yōu)勢(shì)，可有效克服擴(kuò)展卡爾曼濾波等方法在處理非線性問(wèn)題上的缺點(diǎn)。目前國(guó)內(nèi)外對(duì)粒子濾波的理論研究已較為成熟，如文獻(xiàn)[2]提出的復(fù)合粒子濾波，文獻(xiàn)[3]提出的自適應(yīng)粒子濾波，文獻(xiàn)[4]提出的智能粒子濾波，都具有很好的性能，但這些文獻(xiàn)主要集中在粒子濾波算法仿真方面，而對(duì)算法硬件實(shí)現(xiàn)方面的研究較少，一定程度上限制了粒子濾波在工程方面的應(yīng)用。文獻(xiàn)[5]中采用DSP實(shí)現(xiàn)粒子濾波，文獻(xiàn)[6]在ARM平臺(tái)實(shí)現(xiàn)粒子濾波目標(biāo)跟蹤算法，但由于每個(gè)粒子所需的數(shù)學(xué)運(yùn)算相同，因而粒子濾波更適合在具有并行能力的FPGA上運(yùn)行。在FPGA上設(shè)計(jì)濾波算法具有較好的實(shí)時(shí)性，但對(duì)非專業(yè)人士來(lái)說(shuō)，在FPGA上實(shí)現(xiàn)算法設(shè)計(jì)具有較大難度。美國(guó)Xilinx公司提供了設(shè)計(jì)工具System Generator[7](SysGen)，使用者能在Simulink下通過(guò)SysGen提供的各個(gè)模塊完成整個(gè)FPGA的設(shè)計(jì)。文獻(xiàn)[8]給出了使用SysGen設(shè)計(jì)粒子濾波一個(gè)周期的迭代過(guò)程，本文將針對(duì)純方位跟蹤問(wèn)題，對(duì)通用粒子濾波進(jìn)行適當(dāng)簡(jiǎn)化，通過(guò)SysGen設(shè)計(jì)完整的粒子濾波過(guò)程，并對(duì)FPGA上設(shè)計(jì)實(shí)現(xiàn)的硬件結(jié)構(gòu)進(jìn)行分析和仿真。硬件仿真表明， SysGen上設(shè)計(jì)簡(jiǎn)化后的粒子濾波并在FPGA上實(shí)現(xiàn)是完全可行的，且具有較好的跟蹤精度和運(yùn)算速度。

1 純方位跟蹤數(shù)學(xué)模型

純方位跟蹤問(wèn)題通過(guò)測(cè)量傳感器與目標(biāo)間的方位角zk估計(jì)當(dāng)前時(shí)刻目標(biāo)的位置及速度，圖1所示為在直角坐標(biāo)系下，時(shí)刻k以及時(shí)刻k+1的位置和觀測(cè)值。

圖1 純方位跟蹤問(wèn)題Fig.1 Bearings-only tracking

目標(biāo)運(yùn)動(dòng)的離散方程

Xk=ΦXk-1+Γwk

(1)

觀測(cè)方程為

(2)

純方位跟蹤的粒子濾波算法就是利用粒子濾波算法由所測(cè)得運(yùn)動(dòng)目標(biāo)的方位信息z來(lái)實(shí)時(shí)估計(jì)目標(biāo)航跡X。

2 簡(jiǎn)化粒子濾波算法

對(duì)于純方位跟蹤問(wèn)題,粒子濾波已取得較好的跟蹤結(jié)果。然而，通用粒子濾波算法運(yùn)算量大、運(yùn)行周期長(zhǎng)。為了便于硬件實(shí)現(xiàn)，本文針對(duì)上述問(wèn)題，設(shè)計(jì)了簡(jiǎn)化粒子濾波算法。

2.1 通用粒子濾波算法

對(duì)于狀態(tài)方程

(3)

式中：xk表示k時(shí)刻系統(tǒng)狀態(tài)；yk表示觀測(cè)向量；wk,uk分別表示系統(tǒng)過(guò)程和測(cè)量噪聲。

通用粒子濾波算法的步驟如下所述。

forj=1∶M

1) 采樣步驟(S)，通過(guò)p(xk|xjk-1)獲得采樣更新xjk；

end

本文的簡(jiǎn)化算法主要針對(duì)兩方面進(jìn)行，分別為針對(duì)重采樣及權(quán)值計(jì)算的簡(jiǎn)化。

2.2 重采樣算法的簡(jiǎn)化

隨著時(shí)間的推移，較少粒子占據(jù)著大量權(quán)重，導(dǎo)致較差的后驗(yàn)分布，從而影響估計(jì)的性能。而重采樣通過(guò)復(fù)制權(quán)值高的粒子覆蓋權(quán)值低的粒子，使得更多的粒子集中在較高概率分布的區(qū)域。

重采樣算法無(wú)法和其他模塊并行運(yùn)算，需要等權(quán)值計(jì)算完畢才能進(jìn)行，因而對(duì)重采樣進(jìn)行簡(jiǎn)化，對(duì)于提升濾波速度有著重要意義。

文獻(xiàn)[9]給出了SR，RR，RSR以及PR等重采樣算法，表1所示為上述3種重采樣算法及本文給出的簡(jiǎn)化重采樣算法(SPR)計(jì)算量及時(shí)延的比較。表中，M表示粒子數(shù)量，L1,L2,L3,L4表示對(duì)應(yīng)重采樣算法自身的時(shí)延。

表1 重采樣算法計(jì)算量及時(shí)延的比較

由表1可以看出，RSR算法及本文提出的簡(jiǎn)化重采樣算法具有較少的計(jì)算量和較短的時(shí)延，而RSR算法硬件實(shí)現(xiàn)復(fù)雜，且需要乘法和除法運(yùn)算。因而本文提出的簡(jiǎn)化重采樣算法在計(jì)算量、時(shí)延和硬件實(shí)現(xiàn)難度等方面具有一定的優(yōu)勢(shì)。

在降低計(jì)算量的同時(shí)也要保證性能，表2所示為SR及SPR算法在常用一維非線性模型下100次仿真后的均方根誤差(RMSE)，由此可以看出，SPR算法性能略有下降，但仍有較好的跟蹤精度。

表2 SR及SPR算法均方根誤差(RMSE)比較

具體算法流程如下所述。

設(shè)置閾值:T

n=0，r=1

forj=1∶M

if(wjk>T)

n=n+1

end

end

forj=1∶M

r=mod(r,n)+1

end

2.3 權(quán)值計(jì)算的簡(jiǎn)化

由于本文采用采樣重要性重采樣(SIRF)算法即每個(gè)濾波周期都進(jìn)行重采樣，相較于序貫重要性采樣(SISF)算法[10]具有固定周期，實(shí)現(xiàn)簡(jiǎn)單等優(yōu)點(diǎn),且重采樣后粒子權(quán)值均相等，因而估計(jì)模塊可簡(jiǎn)化為求重采樣后粒子狀態(tài)的平均值，而不需要對(duì)權(quán)值求和。由于重采樣算法的簡(jiǎn)化，權(quán)值只在判斷是否保留粒子時(shí)起作用，不再需要對(duì)權(quán)值進(jìn)行標(biāo)準(zhǔn)化，這些對(duì)硬件資源占用及實(shí)現(xiàn)難度都將是一大優(yōu)化。綜上，本文提出了簡(jiǎn)化粒子濾波算法，算法流程如下所述。

forj=1∶M

m=r(j)

end

r(1∶M)=1∶M

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

在FPGA上運(yùn)行的純方位跟蹤粒子濾波算法的架構(gòu)如圖2所示。

圖2 粒子濾波算法SIRF原理Fig.2 Principle of particle filter algorithm SIRF

圖3為粒子濾波算法的時(shí)序圖。

圖3 SIRF時(shí)序圖Fig.3 SIRF timing diagram

本文設(shè)計(jì)主要分控制模塊、采樣模塊、權(quán)值計(jì)算模塊、重采樣模塊和狀態(tài)估計(jì)模塊進(jìn)行。

3.1 控制模塊設(shè)計(jì)

采用狀態(tài)機(jī)設(shè)計(jì)控制模塊，主要有初始化、采樣、權(quán)值計(jì)算、重采樣4個(gè)狀態(tài)。

圖4為狀態(tài)轉(zhuǎn)移圖。

圖4 控制模塊狀態(tài)轉(zhuǎn)移圖Fig.4 Control module state transition diagram

采用SysGen中的Mcode模塊描述上述狀態(tài)機(jī)，生成如圖5所示控制模塊。其中，start和over信號(hào)由各個(gè)模塊內(nèi)部的計(jì)數(shù)器產(chǎn)生。

圖5 控制模塊Fig.5 Control module

3.2 采樣模塊設(shè)計(jì)

本文采用一塊a,b雙口RAM存儲(chǔ)粒子濾波采樣前和采樣后的狀態(tài)值:a口的地址輸入為讀地址信號(hào)，讀取采樣前的粒子狀態(tài);b口的地址輸入為寫地址信號(hào)，存儲(chǔ)采樣后的粒子狀態(tài)。為了配合重采樣模塊，在讀取的粒子狀態(tài)后增加一個(gè)選擇器，重采樣時(shí)，復(fù)制權(quán)值大的粒子進(jìn)入采樣更新過(guò)程。具體實(shí)現(xiàn)如圖6所示。其中,x方向的采樣更新模塊如圖7所示，y方向同理，高斯噪聲模塊直接由SysGen中的參考模塊給出,采樣模塊共有15個(gè)時(shí)延。

圖6 采樣存儲(chǔ)模塊Fig.6 Sampling storage module

圖7 采樣更新模塊Fig.7 Sampling update module

3.3 權(quán)值計(jì)算模塊設(shè)計(jì)

對(duì)于本文給出的簡(jiǎn)化粒子濾波算法，權(quán)值只在判斷粒子是否保留或者丟棄時(shí)起作用，因而可以將權(quán)值計(jì)算公式簡(jiǎn)化為

(4)

函數(shù)arctan()通過(guò)SysGen中參考模塊CORDIC 6.0模塊實(shí)現(xiàn)，并且為減少計(jì)算量，當(dāng)殘差高八位非零時(shí),直接將權(quán)值置零，具體實(shí)現(xiàn)見圖8,該模塊具有52個(gè)時(shí)延。

圖8中，指數(shù)模塊計(jì)算指數(shù)時(shí)小數(shù)部分通過(guò)sinh和cosh函數(shù)相加得到，整數(shù)部分直接通過(guò)查找表得到，兩者相乘得到最終的計(jì)算結(jié)果,具體實(shí)現(xiàn)如圖9所示，指數(shù)模塊具有24個(gè)時(shí)延。

3.4 重采樣模塊設(shè)計(jì)

將計(jì)算得到的粒子權(quán)值存入MEM1內(nèi)，通過(guò)計(jì)數(shù)器順序讀取，與閾值比較大小，分別由計(jì)數(shù)器Counter_u,Counter_d控制保留粒子以及復(fù)制粒子，Counter_u從低往高、Counter_d從高往低計(jì)數(shù)，并作為地址從雙口存儲(chǔ)器MEM2的b口存入粒子序號(hào)。存儲(chǔ)結(jié)束后，MEM2低位存儲(chǔ)著保留粒子序號(hào)，高位存儲(chǔ)著丟棄粒子序號(hào)。存儲(chǔ)丟棄粒子序號(hào)，是為了在重采樣后將需要復(fù)制的粒子存入丟棄粒子序號(hào)中。

分類完成后，用Counter_res從雙口存儲(chǔ)器a口讀取并復(fù)制保留粒子序號(hào)，用Counter_d讀取丟棄粒子序號(hào)分別作為采樣模塊雙口RAM的讀地址和寫地址。根據(jù)Counter_res的工作方式如下所述。

輸入：粒子數(shù)M,有效粒子數(shù)Ms

輸出：讀地址，復(fù)制信號(hào)

初始化

讀地址：read_addr=0

中間變量：temp=0

復(fù)制信號(hào)：rep=0

iftemp>M

read_addr=read_addr+1

temp=read_addr+Ms

rep=0

else

read_addr=read_addr

temp=temp+Ms

rep=1

end

實(shí)現(xiàn)的功能為MEM2按順序從地址0到Ms-1被反復(fù)讀取「M/Ms?+1次,「x?表示不大于x的最大整數(shù)。

簡(jiǎn)化重采樣具體實(shí)現(xiàn)如圖10所示，重采樣模塊具有M+4個(gè)時(shí)延。

圖10 重采樣模塊Fig.10 Resampling module

3.5 狀態(tài)估計(jì)模塊設(shè)計(jì)

由于重采樣后各粒子權(quán)值均相等，因而狀態(tài)估計(jì)可簡(jiǎn)化為求取粒子權(quán)值的平均值。狀態(tài)估計(jì)模塊主要通過(guò)累加器及乘法器實(shí)現(xiàn)，具體設(shè)計(jì)見圖11。

圖11 權(quán)值估計(jì)模塊Fig.11 Weight estimating module

4 數(shù)值仿真

對(duì)于式(1)、式(2)所列的方程，給出初始仿真變量設(shè)置：M=2048,σw=0.000 1,σv=0.1°,x=0.5,y=0.8,vx=0.005,vy=-0.007。數(shù)值方面，位置速度信息均選為18位定點(diǎn)數(shù)(FIX_18_16)，權(quán)值信息選為16位無(wú)符號(hào)定點(diǎn)數(shù)(UFIX_16_15)。

用SysGen模塊轉(zhuǎn)換上述結(jié)構(gòu)為HDL后，對(duì)整個(gè)設(shè)計(jì)進(jìn)行綜合及編譯，并在Xilinx Zynq-7000(7z020 clg484-1)上實(shí)現(xiàn)完整的純方位跟蹤簡(jiǎn)化粒子濾波算法。表3所示為使用2048個(gè)粒子，基于簡(jiǎn)化粒子濾波算法，在FPGA上綜合后各資源的占用情況。

表3 資源利用情況

表4所示為運(yùn)行100次仿真后位置速度的均方根誤差，圖12所示為其中一次的跟蹤結(jié)果及位置誤差曲線。仿真結(jié)果表明該設(shè)計(jì)具有較好的跟蹤精度。

表4 SPF均方根誤差

運(yùn)行一個(gè)周期的簡(jiǎn)化粒子濾波算法為TSPF=[(2048+15)+52+(2048+4)]Tclk=4167Tclk，即需要4167個(gè)時(shí)鐘周期。在Zynq-7000使用100 MHz晶振的情況下，能獲得的最大采樣速率為24 kHz。表3中各資源利用率均在10 %左右，仍存在剩余，因此，如采用并行設(shè)計(jì)方案利用剩余資源，能夠進(jìn)一步提升運(yùn)行速率。

圖12 FPGA上運(yùn)行簡(jiǎn)化粒子濾波跟蹤結(jié)果Fig.12 Tracking result of SPF running on FPGA

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

本文給出了一種便于FPGA設(shè)計(jì)實(shí)現(xiàn)的簡(jiǎn)化粒子濾波算法，并通過(guò)美國(guó)Xilinx公司提供的設(shè)計(jì)工具Sys- Gen完成粒子濾波各個(gè)模塊的設(shè)計(jì)，最后綜合各個(gè)模塊，完成硬件仿真。仿真結(jié)果表明，簡(jiǎn)化后的粒子濾波仍具有較好的跟蹤精度，運(yùn)行速率達(dá)到24 kHz，且硬件資源仍有剩余，存在進(jìn)一步的速率提升空間。本文設(shè)計(jì)方法對(duì)于粒子濾波的FPGA實(shí)現(xiàn)具有一定的參考價(jià)值。

參 考 文 獻(xiàn)

[1] 黃小平,王巖，繆鵬程.粒子濾波原理及應(yīng)用——Matlab仿真[M].北京：電子工業(yè)出版社,2017.

[2] 蔡宗平,牛創(chuàng),張雪影,等.復(fù)合 K 噪聲下目標(biāo)跟蹤的改進(jìn)粒子濾波算法研究[J].電光與控制,2016,23(5):1-5.

[3] 孫巧,張勝修,曹立佳,等.自適應(yīng)模型更新的粒子濾波視覺跟蹤[J].電光與控制,2017,24(2):1-5.

[4] YIN S,ZHU X P.Intelligent particle filter and its application to fault detection of nonlinear system[J].IEEE Transactions on Industrial Electronics,2015,62(6):3852-3861.

[5] 李雁斌,曹作良,劉常杰,等.基于DSP利用粒子濾波算法實(shí)現(xiàn)目標(biāo)跟蹤[J].光電子·激光，2009，20(6)：771-774.

[6] PAN L,LIU X M.The study of target tracking based on ARM embedded platform[J].Journal of Computers, 2012,7(8):2015-2023.

[7] VELMURUGAN R.Implementation strategies for particle filter based target tracking[D].Atlanta:Georgia Institute of Technology,2007.

[8] 紀(jì)志成,高春能，吳定會(huì).FPGA 數(shù)字信號(hào)處理設(shè)計(jì)教程:System Generator 入門與提高[M].西安：西安電子科技大學(xué)出版社,2008.

[9] LI T C,BOLIC M,DJURIC P M.Resampling methods for particle filtering:classification,implementation,and strategies[J].IEEE Signal Processing Magazine,2015,32(3):70-86.

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