基于FPGA的高斯粒子滤波硬件实现方法

摘要

本发明公开了一种基于FPGA的高斯粒子滤波硬件实现方法，所述方法包括如下步骤：采用调节粒子生成模块CPG根据Cholesky分解得到协方差矩阵S和均值μ绘制调节粒子；采用粒子生成模块PG生成粒子群；采用粒子更新模块PU计算粒子群的权值；采用中央处理单元模块CU更新更新权值后粒子群的均值和协方差；采用重采样模块RS采集更新后的粒子群；采用协方差计算模块CC计算采样的粒子群的均值和协方差；采用Cholesky分解所述的均值和协方差得到协方差矩阵S和均值μ。使用模块级流水线设计，该设计大大简化了设计流程。模块级流水线通过分布式控制器来实现同步执行，该控制器控制各个处理模块的数据生成和传输。

说明书

技术领域

本发明涉及一种基于FPGA实现的高斯粒子滤波算法的硬件实现方法，采用数据流结构的块级流水线设计方法，属于非线性系统滤波和电子技术领域。 

背景技术

近几年发展起来的粒子滤波算法是一种基于Monte Carlo思想的非线性非高斯系统滤波与预测方法，它采用后验概率密度函数的随机采样点集以及相应的权值来表示状态向量的变化，从而完全突破了卡尔曼滤波理论框架，对系统的过程噪声和量测噪声的形式没有任何限制。 

但是相比于Kalman滤波，粒子滤波算法比较复杂，运算量比较大，从而使得粒子滤波的实时性很差，阻碍了其实际应用。目前，绝大部分粒子滤波文献是关于其理论研究和算法仿真的，而关于其硬件实现的则很少；而粒子滤波从理论、算法研究走向实际应用的过程中，硬件实现是一个关键环节。总体来看，目前粒子滤波硬件实现方法的研究还不够深入，因此，研究粒子滤波算法的硬件实现方法有着极其重要的理论和现实意义。 

粒子滤波算法具有如下两个独特的执行特性：(1)可以表示为数据流图，以致于节点(或模块)可以并发执行。虽然每个模块的复杂性不同，但是数据流图都可以清楚地表示数据依赖关系；(2)数据流图中的每个模块执行每个迭代周期的一组数据。 

因此，本发明应用模块级两级流水线方案有效实现了高斯粒子滤波算法(GPF)，应用FPGA设计粒子滤波算法的各个模块，从而为工程应用中复杂粒子滤波算法的高效计算和硬件实现问题，提供一种新颖的解决思路。 

发明内容

本发明针对现有技术存在的缺陷提出了一种可重构模式粒子滤波算法，并给出了具体的实现方法基于FPGA的高斯粒子滤波硬件实现方法。 

本发明基于FPGA的高斯粒子滤波硬件实现方法包括如下步骤： 

(1)采用调节粒子生成模块CPG根据Cholesky分解得到协方差矩阵S和均值μ绘制调节粒子； 

(2)采用粒子生成模块PG根据步骤(1)生成的调节粒子生成粒子群； 

(3)采用粒子更新模块PU计算步骤(2)所述粒子群的权值； 

(4)采用中央处理单元模块CU更新步骤(3)更新权值后粒子群的均值和协方差； 

(5)采用重采样模块RS采集步骤(4)更新后的粒子群； 

(6)采用协方差计算模块CC计算步骤(5)采样的粒子群的均值和协方差； 

(7)采用Cholesky分解步骤(6)所述的均值和协方差得到协方差矩阵S和均值μ，返回步骤(1)。 

将步骤(2)所述生成的粒子群经过均值计算/生成输出模块MC/OG处理后输出至中央处理单元模块CU更新粒子群的均值和协方差。 

所述步骤(3)所述的粒子更新模块PU采用算术运算有乘法、分类、除法、三角函数artan()和指数函数exp()；应用坐标旋转数字计算方法CORDIC展开用作artan()和exp()的结构算子。 

所述协方差矩阵S是一个4×4的三角阵，维数是10。 

所述均值μ的维数是4。 

本发明采用分布式控制器，使用模块级流水线设计方法设计了样本重要性重采样粒子滤波器(GPF)，以二维纯方位目标跟踪为处理对象，主要估计的未知状态是笛卡尔坐标系(X_n＝[x,V_x,y,V_y]^T)中跟踪对象的位置和速度，其中x,y是指目标的位置坐标，V_x,V_y分别是x，y方向上的速度分量。整个粒子滤波器由若干个处理模块组成，每个模块处理各种复杂的算术运算，同时每个处理模块具有用于控制其操作的局部控制器。分布式控制能够高效地处理各个数据模块之间的数据依赖关系。本发明提出的硬件设计方法可以扩展到不同粒子滤波的动态重新配置。对于每个粒子滤波器，首先定义每个处理模块的操作，然后定义数据流结构。从处理模块设计和数据流结构设计出发，最后推导和设计缓冲控制器和全局控制器。 

本发明最重要的部分是数据中心，它负责处理模块之间大量的数据传输。整个滤波器使用模块级流水线设计，该设计大大简化了设计流程。模块级流水线通过分布式控制器来实现同步执行，该控制器控制各个处理模块的数据生成和传输。 

附图说明

图1：GPF的数据流图； 

图2：GPF算法的各个模块之间的数据关系； 

图3：GPF算法的各个模块之间的信号流向关系； 

图4：GPF的模块级时序图。 

具体实施方式

图1所示为GPF的数据流图，根据该数据流图本发明设计了GPF的各个处理模块，包括粒子生成模块PG、粒子更新模块PU、均值计算/生成输出模块MC/OG、重采样模块RS等，如图1所示为GPF的数据流图。 

粒子生成模块PG：在粒子生成模块PG中，有四个连接输入向量的缓冲区和4个连接输出向量(x,V_x,y,V_y)的缓冲区。存储在输入缓冲区的输入向量是通过重采样模块RS生成的。在接下来的迭代中，粒子生成模块PG的输出再次使用在重采样模块RS中。此外，另外还有两个被用于PU1处理模块的(x,y)的缓冲区。所有输入输出都是M维向量。 

在粒子生成的算术运算步骤中，输出通过并行操作进行计算。在粒子生成模块PG中有两个噪声发生器。我们利用高效的FPGA实现BOX-Muller的方法得到噪声样本。噪声的生成是一个查找表和算术逻辑的组合。 

粒子更新模块PU：粒子更新主要的算术运算有乘法、分类、除法、三角函数artan()和指数函数exp()。应用坐标旋转数字计算方法(CORDIC)展开用作artan()和exp()的结构算子。根据运算单元的维数，将粒子更新(PU)运算在功能上分成两个个处理模块：PU1和PU2。 

PU1处理模块有2个接收来自粒子生成模块PG的(x,y)输入缓冲区和1个将输出t_PU1输送到PU2处理模块的缓冲区。PU1处理模块计算artan(y/x)并生成M维临时数据(t_PU1)。对于artan()运算，为了区别开(-x,y)和(x,-y)，用一个常值π/2和一个多路复用器来调整角度。由于粒子生成模块PG和PU1处理模块之间没有数据依赖关系，一旦PU1处理模块的输入缓冲区得到数据，PU1处理模块就直接计算其输出。 

PU2处理模块有两个输入缓冲区。一个是来自PU1处理模块的(t_PU1)，另一个是外部观测输入(z(n))。在迭代n期间，z(n)的值不变。PU2处理模块有两个输出缓冲区。分别是输出(t_PU2)和(sum)。PU2处理模块计算其余的权重，这些权重是非标准化的并被指定为输出流(t_PU2)。 

均值计算/生成输出模块MC/OG：均值计算模块MC用来计算向量μ的均值，即生成 滤波输出。利用粒子生成模块PG生成的数据以及PU2处理模块计算的权重和sum来设计模块。然后执行这个模块且通过sum的值来标准化输出为： 

$>{\mu }_x=1/\mathrm{sum}\underset{m=1}{\overset{M}{\Sigma }}x\left(m\right)t_{\mathrm{PU}2}\left(m\right)$>

中央处理单元模块CU：中央处理单元模块CU的输入输出在采样周期期间一次生成。在该单元模块中采用时间复用运算符执行除法和平方根等操作。为了同步，在读取缓冲控制器输出之前，输出缓冲在处理模块中。该模块有2个来自协方差计算模块CC的(μ,Var)输入缓冲区，还有一个输出(S)给调节粒子生成模块CPG的缓冲区。这些输出都是序列化的。 

协方差计算模块CC：在协方差计算模块CC处理模块中，计算局部协方差4×4矩阵Var。这个模块生成标准化局部协方差，它有6个分别来自粒子生成模块PGP的(x,V_x,y,V_y)和PU2处理模块的(w,sum)输入缓冲区。还有一个输出给中央处理单元模块CU的Var缓冲区，这些输出也都是序列化的。 

调节粒子生成模块CPG：在调节粒子生成模块CPG中，通过Cholesky分解得到协方差矩阵S和均值μ。矩阵S是一个4×4的三角阵，所以来自中央处理单元模块CU的数据数量是10(不是16)。所有的乘法器是流水线执行的，它们并发操作产生M个调节粒子。因为输出是通过不同数量的运算符计算得到的，为了在输出的同时获得所有调节粒子，必须为每个不同的状态引入额外的延迟，因此调节粒子生模块CPG需要4个随机数生成器。 

调节粒子生成模块CPG有2个来自中央处理单元模块CU的(μ,S)的输入缓冲区和4个输出给粒子生成模块PG的缓冲区。平均值μ的维数是4，协方差的维数是10。这些数据顺序生成，并行使用，保存在互连总线上。输出数据的维数是M。 

值得注意的是，GPF的循环可以融合，除了Cholesky分解和协方差计算，其他步骤都可以在一个M次循环中执行。Cholesky分解和最终的协方差计算是连续的，并且它们的复杂性是固定的，不依赖于粒子数。 

二维纯方位目标跟踪问题的GPF的结构如图2所示，该图显示了各处理模块和缓冲区之间的数据连接关系，它是一个模块级流水线结构。 

GPF的各个模块之间的信号流向关系如图3所示，由图可以看出，只有当所有模块的处理完成时才开始执行Cholesky分解，中央处理单元CU结合各个PE的局部结果后才执行Cholesky分解操作。最终均值和协方差的估计值计算需要所有权重之和，因此，只有所有粒子的权重计算完成才开始该步骤。 

GPF的模块级时序图如图4所示，由图可以看出采样周期为M/K+L_GPF+L_CU，其中L_GPF是各个处理模块的启动延迟总和，L_CU是中央处理单元模块CU的延迟，中央处理单元模块CU对均值和协方差的局部估计进行求和并通过权重和对它们进行归一化，在4维纯方位目标跟踪中这一看似简单的步骤数学算法非常复杂。协方差矩阵的最终估计被传递到Cholesky分解模块，该模块输出一个较低的对角式4×4分解矩阵，该分解矩阵用于下一循环的采样步骤。因此在GPF的硬件实现时，各个PE和中央处理单元模块CU之间的唯一通信是均值和协方差的局部估计和Cholesky分解矩阵之间的交换。 

控制器设计 

滤波器应用缓冲控制器实现整体操作，决定控制器结构和整体实现的参数如下：L_maxi、L_i、nr_i、nw_i、M_i、C_i、P_i、F_i和D_i。其中L_maxi是指处理模块之间的逻辑延迟；实际的L_i的范围为0＜L_i＜L_maxi；nr_i是写入缓冲区和读取缓冲区之间的偏移量；nw_i是读取前一时刻缓冲控制器和写入当前缓冲控制器之间的偏移量；C_i、P_i和F_i分别是指处理模块i的数据使用率、数据生成速度和处理速度；D_i是指处理模块i生成数据的延迟系数；参数M_i是数据生成模块的数据流维度。控制器有两个逻辑部分：读和写。写逻辑部分由参数L_i和nw_ij配置，而读逻辑部分由D_ij和nr_ij配置，其中i和j分别表示生成和消耗的处理模块。注意，这些参数由数据流结构和处理模块实现细节得到。当缓冲区控制器激活时，写和读逻辑并发执行。写部分的开始表明数据已经到达了连接到该缓冲区的处理模块。在等待L_i周期后，由生成处理模块计算的实际数据在缓冲控制器端是有效的。写逻辑部分不会写这L_i个来自生成模块的无效数据。这将保证正确地接收到有效的数据流，如果生成模块是纯粹的流水线操作的硬件。然而，无论流水线深度多少(即处理模块生成的延迟数据)处理块需要有限数量的计算时间是可能的。为了支持这种类型的处理模块，我们又使用一个参数nw_ij。在这个等待周期(L_i+nw_ij)后，数据被写入缓冲区。一旦数据样本被写入缓冲区， 开始读取过程的读逻辑部分。参数nr_ij代表从缓冲区中的数据写入和读取之间的偏移量。这个参数是支持数据依赖性。即使没有数据依赖关系，生成模块数据生成速率不同于消耗模块数据消耗速率是可能的。为了支持连接缓冲控制器的两个处理模块的速率不匹配问题，我们使用另一个参数D_ij。在这等待期间(max(nr_ij,D_ij))后，从缓冲区读取数据。因此，写逻辑部分由(L_i,nw_ij)配置，而读逻辑部分由(nr_ij,D_ij)配置。相同的缓冲控制器通过修改这些参数来支持不同的数据传输特性。缓冲区控制器激活受三个关键的同步信号：start_time_ij，write_begin_ij和read_begin_ij的支配。其中指标ij代表缓冲控制器位于处理模块i和j之间。 

FPGA实现 

GPF的数据流图如图3所示，图中给出了处理模块和缓冲区之间的连接关系。表1列出了每个处理模块的主要参数，处理模块的实际速度范围为206MHz～351MHz之间，由于受到CORDIC方法的速度限制，同时为了简化控制器设计，选取206MHz为全局时钟。该表还给出了FPGA实现时各模块所占用的FPGA资源。 

由表1和表2推导出GPF所有缓冲控制器的参数(见表3)，该表给出了每个缓冲控制器的开始时间，写开始时间和读开始时间。表2列出了实现GPF的处理模块之间的数据依赖关系。在该表中，多个源节点和目的节点的出现表明有一个以上的具有不同特性的数据连接。在该表中，nr₇＝49。这个值是nr₂+L_PU1+nw₃+nr₃+L_PU2+nw₄+nr₄的和。这是因为该缓冲区已经完成对粒子生成模块PG成的数据的写操作，但是为了数据同步，均值计算模块MC的读数据将会延迟执行。对于E4和E5，nw₄和nw₅分别是2和M+1。在E11链接中，CPG的读操作将延迟nr₁₁，而nr₁₁＝nr₆+L_CU+nw₁₀+nr₁₀＝78。这样就使得μ和S在CPG模块同步。在E12链接中，它对应于CU处理摸了生成第一个数据所花费的时间。对于E8、E9和E11，nw_i＝M。由于不存在速率失配，D的值都是1。同样给出了缓冲控制器的缓冲区大小。因素4意味着数据矢量(即(x,V_x,y,V_y))是由一个缓冲控制器控制。同步使用的缓冲区的数量约为4M，其中M为滤波器所用的粒子数。需要注意的是每个缓冲控制器需要的实际缓冲区大小被限制为min(nr_i,M_i)。因此，E7实际使用的缓冲区比表中所示最大尺寸要小。 

表1处理模块信息表 

节点 L C P F FPGA(％) CPG 11 206MHz 206MHz 206MHz 3.3 PG 8 206MHz 206MHz 206MHz 2.1 PU1 23 206MHz 206MHz 206MHz 2.1 PU2 20 206MHz 206MHz 206MHz 1.7 RS 19 206MHz 206MHz 206MHz 3.0 MC/OG 8 206MHz 206MHz 206MHz 2.2 CC 8 206MHz 206MHz 206MHz 2.4 CU 1 206MHz 206MHz 206MHz 1.6

表2GPF的链接信息表(EIT) 

表3GPF的缓冲控制器参数 

启动 时刻 写开始 读开始 start1 0 12 13 start2 13 22 23 start3 23 44 45 start4 45 67 68 start5 45 M+66 M+67 start7 13 22 69 start8 68 M+76 M+77 start9 68 M+76 M+77 start11 68 M+76 M+154 start12 M+77 M+153 M+154 reset M+154 - -