基于System Generator的简化粒子滤波算法设计及硬件实现

王佳辉， 王义平， 薛雅丽

(南京航空航天大学，南京 210016)

0 引言

粒子滤波算法[1]在处理非线性、非高斯系统的状态估计方面有明显的优势，可有效克服扩展卡尔曼滤波等方法在处理非线性问题上的缺点。目前国内外对粒子滤波的理论研究已较为成熟，如文献[2]提出的复合粒子滤波，文献[3]提出的自适应粒子滤波，文献[4]提出的智能粒子滤波，都具有很好的性能，但这些文献主要集中在粒子滤波算法仿真方面，而对算法硬件实现方面的研究较少，一定程度上限制了粒子滤波在工程方面的应用。文献[5]中采用DSP实现粒子滤波，文献[6]在ARM平台实现粒子滤波目标跟踪算法，但由于每个粒子所需的数学运算相同，因而粒子滤波更适合在具有并行能力的FPGA上运行。在FPGA上设计滤波算法具有较好的实时性，但对非专业人士来说，在FPGA上实现算法设计具有较大难度。美国Xilinx公司提供了设计工具System Generator[7](SysGen)，使用者能在Simulink下通过SysGen提供的各个模块完成整个FPGA的设计。文献[8]给出了使用SysGen设计粒子滤波一个周期的迭代过程，本文将针对纯方位跟踪问题，对通用粒子滤波进行适当简化，通过SysGen设计完整的粒子滤波过程，并对FPGA上设计实现的硬件结构进行分析和仿真。硬件仿真表明， SysGen上设计简化后的粒子滤波并在FPGA上实现是完全可行的，且具有较好的跟踪精度和运算速度。

1 纯方位跟踪数学模型

纯方位跟踪问题通过测量传感器与目标间的方位角zk估计当前时刻目标的位置及速度，图1所示为在直角坐标系下，时刻k以及时刻k+1的位置和观测值。

图1 纯方位跟踪问题Fig.1 Bearings-only tracking

目标运动的离散方程

Xk=ΦXk-1+Γwk

(1)

观测方程为

(2)

纯方位跟踪的粒子滤波算法就是利用粒子滤波算法由所测得运动目标的方位信息z来实时估计目标航迹X。

2 简化粒子滤波算法

对于纯方位跟踪问题,粒子滤波已取得较好的跟踪结果。然而，通用粒子滤波算法运算量大、运行周期长。为了便于硬件实现，本文针对上述问题，设计了简化粒子滤波算法。

2.1 通用粒子滤波算法

对于状态方程

(3)

式中：xk表示k时刻系统状态；yk表示观测向量；wk,uk分别表示系统过程和测量噪声。

通用粒子滤波算法的步骤如下所述。

forj=1∶M

1) 采样步骤(S)，通过p(xk|xjk-1)获得采样更新xjk；

end

本文的简化算法主要针对两方面进行，分别为针对重采样及权值计算的简化。

2.2 重采样算法的简化

随着时间的推移，较少粒子占据着大量权重，导致较差的后验分布，从而影响估计的性能。而重采样通过复制权值高的粒子覆盖权值低的粒子，使得更多的粒子集中在较高概率分布的区域。

重采样算法无法和其他模块并行运算，需要等权值计算完毕才能进行，因而对重采样进行简化，对于提升滤波速度有着重要意义。

文献[9]给出了SR，RR，RSR以及PR等重采样算法，表1所示为上述3种重采样算法及本文给出的简化重采样算法(SPR)计算量及时延的比较。表中，M表示粒子数量，L1,L2,L3,L4表示对应重采样算法自身的时延。

表1 重采样算法计算量及时延的比较

由表1可以看出，RSR算法及本文提出的简化重采样算法具有较少的计算量和较短的时延，而RSR算法硬件实现复杂，且需要乘法和除法运算。因而本文提出的简化重采样算法在计算量、时延和硬件实现难度等方面具有一定的优势。

在降低计算量的同时也要保证性能，表2所示为SR及SPR算法在常用一维非线性模型下100次仿真后的均方根误差(RMSE)，由此可以看出，SPR算法性能略有下降，但仍有较好的跟踪精度。

表2 SR及SPR算法均方根误差(RMSE)比较

具体算法流程如下所述。

设置阈值: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 权值计算的简化

由于本文采用采样重要性重采样(SIRF)算法即每个滤波周期都进行重采样，相较于序贯重要性采样(SISF)算法[10]具有固定周期，实现简单等优点,且重采样后粒子权值均相等，因而估计模块可简化为求重采样后粒子状态的平均值，而不需要对权值求和。由于重采样算法的简化，权值只在判断是否保留粒子时起作用，不再需要对权值进行标准化，这些对硬件资源占用及实现难度都将是一大优化。综上，本文提出了简化粒子滤波算法，算法流程如下所述。

forj=1∶M

m=r(j)

end

r(1∶M)=1∶M

3 基于SysGen的简化粒子滤波算法实现

在FPGA上运行的纯方位跟踪粒子滤波算法的架构如图2所示。

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

图3为粒子滤波算法的时序图。

图3 SIRF时序图Fig.3 SIRF timing diagram

本文设计主要分控制模块、采样模块、权值计算模块、重采样模块和状态估计模块进行。

3.1 控制模块设计

采用状态机设计控制模块，主要有初始化、采样、权值计算、重采样4个状态。

图4为状态转移图。

图4 控制模块状态转移图Fig.4 Control module state transition diagram

采用SysGen中的Mcode模块描述上述状态机，生成如图5所示控制模块。其中，start和over信号由各个模块内部的计数器产生。

图5 控制模块Fig.5 Control module

3.2 采样模块设计

本文采用一块a,b双口RAM存储粒子滤波采样前和采样后的状态值:a口的地址输入为读地址信号，读取采样前的粒子状态;b口的地址输入为写地址信号，存储采样后的粒子状态。为了配合重采样模块，在读取的粒子状态后增加一个选择器，重采样时，复制权值大的粒子进入采样更新过程。具体实现如图6所示。其中,x方向的采样更新模块如图7所示，y方向同理，高斯噪声模块直接由SysGen中的参考模块给出,采样模块共有15个时延。

图6 采样存储模块Fig.6 Sampling storage module

图7 采样更新模块Fig.7 Sampling update module

3.3 权值计算模块设计

对于本文给出的简化粒子滤波算法，权值只在判断粒子是否保留或者丢弃时起作用，因而可以将权值计算公式简化为

(4)

函数arctan()通过SysGen中参考模块CORDIC 6.0模块实现，并且为减少计算量，当残差高八位非零时,直接将权值置零，具体实现见图8,该模块具有52个时延。

图8中，指数模块计算指数时小数部分通过sinh和cosh函数相加得到，整数部分直接通过查找表得到，两者相乘得到最终的计算结果,具体实现如图9所示，指数模块具有24个时延。

3.4 重采样模块设计

将计算得到的粒子权值存入MEM1内，通过计数器顺序读取，与阈值比较大小，分别由计数器Counter_u,Counter_d控制保留粒子以及复制粒子，Counter_u从低往高、Counter_d从高往低计数，并作为地址从双口存储器MEM2的b口存入粒子序号。存储结束后，MEM2低位存储着保留粒子序号，高位存储着丢弃粒子序号。存储丢弃粒子序号，是为了在重采样后将需要复制的粒子存入丢弃粒子序号中。

分类完成后，用Counter_res从双口存储器a口读取并复制保留粒子序号，用Counter_d读取丢弃粒子序号分别作为采样模块双口RAM的读地址和写地址。根据Counter_res的工作方式如下所述。

输入：粒子数M,有效粒子数Ms

输出：读地址，复制信号

初始化

读地址：read_addr=0

中间变量：temp=0

复制信号：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

实现的功能为MEM2按顺序从地址0到Ms-1被反复读取「M/Ms⎤+1次,「x⎤表示不大于x的最大整数。

简化重采样具体实现如图10所示，重采样模块具有M+4个时延。

图10 重采样模块Fig.10 Resampling module

3.5 状态估计模块设计

由于重采样后各粒子权值均相等，因而状态估计可简化为求取粒子权值的平均值。状态估计模块主要通过累加器及乘法器实现，具体设计见图11。

图11 权值估计模块Fig.11 Weight estimating module

4 数值仿真

对于式(1)、式(2)所列的方程，给出初始仿真变量设置：M=2048,σw=0.000 1,σv=0.1°,x=0.5,y=0.8,vx=0.005,vy=-0.007。数值方面，位置速度信息均选为18位定点数(FIX_18_16)，权值信息选为16位无符号定点数(UFIX_16_15)。

用SysGen模块转换上述结构为HDL后，对整个设计进行综合及编译，并在Xilinx Zynq-7000(7z020 clg484-1)上实现完整的纯方位跟踪简化粒子滤波算法。表3所示为使用2048个粒子，基于简化粒子滤波算法，在FPGA上综合后各资源的占用情况。

表3 资源利用情况

表4所示为运行100次仿真后位置速度的均方根误差，图12所示为其中一次的跟踪结果及位置误差曲线。仿真结果表明该设计具有较好的跟踪精度。

表4 SPF均方根误差

运行一个周期的简化粒子滤波算法为TSPF=[(2048+15)+52+(2048+4)]Tclk=4167Tclk，即需要4167个时钟周期。在Zynq-7000使用100 MHz晶振的情况下，能获得的最大采样速率为24 kHz。表3中各资源利用率均在10 %左右，仍存在剩余，因此，如采用并行设计方案利用剩余资源，能够进一步提升运行速率。

图12 FPGA上运行简化粒子滤波跟踪结果Fig.12 Tracking result of SPF running on FPGA

5 结束语

本文给出了一种便于FPGA设计实现的简化粒子滤波算法，并通过美国Xilinx公司提供的设计工具Sys- Gen完成粒子滤波各个模块的设计，最后综合各个模块，完成硬件仿真。仿真结果表明，简化后的粒子滤波仍具有较好的跟踪精度，运行速率达到24 kHz，且硬件资源仍有剩余，存在进一步的速率提升空间。本文设计方法对于粒子滤波的FPGA实现具有一定的参考价值。

参 考 文 献

[1] 黄小平,王岩，缪鹏程.粒子滤波原理及应用——Matlab仿真[M].北京：电子工业出版社,2017.

[2] 蔡宗平,牛创,张雪影,等.复合 K 噪声下目标跟踪的改进粒子滤波算法研究[J].电光与控制,2016,23(5):1-5.

[3] 孙巧,张胜修,曹立佳,等.自适应模型更新的粒子滤波视觉跟踪[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利用粒子滤波算法实现目标跟踪[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] 纪志成,高春能，吴定会.FPGA 数字信号处理设计教程:System Generator 入门与提高[M].西安：西安电子科技大学出版社,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] 洪少华,史治国,陈抗生.用于纯方位跟踪的简化粒子滤波算法及其硬件实现[J].电子与信息学报,2009,31(1):96-100.