一种基于自适应粒子滤波的实时目标跟踪方法和介质

本发明属于非线性滤波及电子，具体涉及一种基于自适应粒子滤波的实时目标跟踪方法和介质。

背景技术：

1、粒子滤波(particle filter)又叫序贯蒙特卡洛方法，是一种基于蒙特卡洛模拟近似实现的贝叶斯滤波方法，属于非线性滤波方法。其基本思想是通过一组加权样本(粒子)集递归估计随机事件的后验密度，从含有噪声或不完整的观测序列，估计出系统的状态。

2、粒子滤波器是卡尔曼滤波器(kalman filter)的一般化方法，卡尔曼滤波器建立在线性的状态空间和高斯分布的噪声上；而粒子滤波器的状态空间模型可以是非线性，且噪声分布可以是任何形式。相比于其他滤波算法，粒子滤波在解决非线性、非高斯的动态状态空间模型(dynamic state-space model)上，具备更好的稳定性和算法性能。因此，粒子滤波被广泛应用于各种领域，比如目标跟踪定位等[1,2,3]。

3、标准的粒子滤波算法由四个步骤组成：采样、权值估计、重采样和输出估计，如代码1所示。可以看到，在所有滤波时刻，粒子滤波算法采用固定的粒子数目。即传统粒子滤波算法在对目标进行跟踪定位时，粒子数目在算法初始化阶段即确定，并且在整个跟踪定位阶段保持不变。并且，粒子滤波的滤波效果即定位精度极大地依赖于选取的粒子数目。简而言之，采用的粒子数目越多，定位精度越高。然而，使用大量的粒子数目将给粒子滤波带来非常大的计算量，导致其计算速度过慢，从而限制其在实时系统中的应用[4]。

4、

5、

6、此外，对粒子滤波算法的特性而言，根据粒子滤波系统中观测量的变化，在每次迭代过程中表示后验概率密度分布所需要的加权粒子数是实时变化的，因此使用固定数量的粒子集会导致在某些迭代过程中造成不必要的计算资源浪费。并且，粒子滤波的滤波效果并不随着粒子数的增加而一直增加，当粒子数增加到一定程度，其定位精度会收敛到固定值。而用于状态估计的粒子数目越多，其带来的计算复杂度越高，从而影响对机器人进行跟踪定位的实时性。

7、为了实现用于实时目标跟踪的粒子滤波算法，人们提出了自适应粒子滤波方法[4,5,6,7]，即根据观测值动态改变每个时刻所使用的粒子数目进行状态估计，从而减少平均使用的粒子数目，提高粒子滤波算法的速度。已有的自适应粒子滤波方法主要有两类：一种是koller等人提出的基于似然函数的自适应粒子滤波方法[6]，该方法直到非归一化似然值总和超过预先指定的阈值才生成样本集。该方法虽然在算法上容易实现，但是增强了粒子之间的相关性，影响了粒子滤波算法的并行计算特性，使得粒子滤波算法在硬件中难以并行实现；另一种是由fox等人提出的基于kld采样的自适应粒子滤波方法[5]，该方法基于样本的近似值和真实后验值之间的误差来自适应变化粒子数。该方法比之前的自适应方法有更好的近似值以及使用更少的粒子数，但是基于kld采样的自适应方法需要对提议分布预先进行离散近似，并且对概率间的kld距离的计算过于复杂，对硬件实现和加速带来了极大的挑战。此外，bolic等人提出了一种简化的自适应粒子滤波方法[7]，该方法随机改变每个步骤使用的粒子数目，并不能保证算法的有效性和定位精度。

8、总的而言，已有的自适应粒子滤波算法相比标准滤波方法，能够减少平均使用的粒子数目，但是他们引入的额外计算影响了粒子滤波的并行执行[6]，并带来了额外复杂的计算量[5]，或者不能保证算法的有效性[7]，因此其带来的速度提升非常有限，从而限制了他们在实时目标跟踪系统中的应用。

9、参考文献：

10、[1]zl 201910432983.9一种自适应粒子滤波地波雷达目标一体化探测方法

11、[2]zl 201310357510.x一种基于自适应粒子滤波和稀疏表示的目标跟踪算法

12、[3]t.c.chau,w.luk,p.y.cheung,a.eele,and j.maciejowski,“adaptivesequential monte carlo approach for real-time applications,”in 22ndinternational conference on field programmable logic andapplications(fpl).ieee,2012,pp.527–530.

13、[4]c.kwok,d.fox,and m.meila,“real-time particle filters,”advances inneural informationprocessing systems,vol.15,2002.

14、[5]d.fox,“adapting the sample size in particle filters through kld-sampling,”the international journal ofrobotics research,vol.22,no.12,pp.985–1003,2003.

15、[6]d.koller andr.fratkina,“using learning for approximation instochastic processes.”in icml,1998,pp.287–29.

16、[7]m.bolic,s.hong,and p.m.djuric,“performance and complexity analysisof adaptive particle filtering for tracking applications,”in conferencerecord of the thirty-sixth asilomar conference on signals,systems andcomputers,2002.,vol.1.ieee,2002,pp.853–857

17、[8]l.martino,v.elvira,and f.louzada,“effective sample size forimportance sampling basedon discrepancy measures,”signal processing,vol.131,pp.386–401,2017.

技术实现思路

1、本发明提供一种基于自适应粒子滤波的实时目标跟踪方法和介质，在每个跟踪步骤均根据权重粒子集，计算并使用能表征当前观测值下的状态后验概率分布的最有效的粒子数目，可以避免计算资源在冗余粒子上的浪费，并能保证粒子滤波对实时目标的跟踪精度。

2、为实现上述技术目的，本发明采用如下技术方案：

3、一种基于自适应粒子滤波的实时目标跟踪方法，包括：

4、步骤1，设定粒子滤波的初始粒子数量n(t＝0)和目标跟踪过程中的粒子目标数量；

5、步骤2，初始化目标跟踪所有粒子t＝0时刻的状态，权重相等均为1，其中状态包括位置和速度；

6、步骤3，基于粒子t-1时刻的状态并根据状态转移方程预测粒子t时刻的状态；

7、步骤4，根据粒子t时刻的预测位置和传感器的位置以及传感器的观测值，计算粒子t时刻的权重；

8、步骤5，根据所有粒子t时刻的归一化权重计算t时刻的有效粒子数量；

9、步骤6，将计算所得t时刻的有效粒子数量与粒子目标数量比较，并根据比较结果调整t时刻的粒子数量为n*(t)；

10、步骤7，根据所有粒子t时刻的归一化权重和调整后的粒子数量n*(t)进行粒子重采样，得到t时刻的粒子集，且所有粒子权重相同；

11、步骤8，利用新粒子集的粒子平均状态对目标的状态进行估计，并输出的当前t时刻的目标状态；

12、步骤9，更新下一时刻t+1的粒子数为n(t+1)＝n*(t)，更新时刻t＝t+1，然后基于新粒子集返回步骤3，继续步骤3-9对目标的后续时刻进行状态跟踪。

13、进一步地，步骤4中粒子t时刻的权重计算方法为：

14、

15、式中，为计算得到第i个粒子t时刻的权重；yj,t为第j个传感器t时刻接收到的目标信号强度值，作为目标跟踪粒子滤波的观测值；s为传感器的设置数量；为第i个粒子t时刻的预测位置；sj为第j个传感器的位置；p0为目标发射信号的功率；α＞1指信号的路径损耗指数；||·||是l2范数计算。

16、进一步地，步骤5中有效粒子数量的计算方法为：

17、

18、

19、式中，ess(t)为t时刻的有效粒子数量，为经步骤4计算得到的第i个粒子t时刻的权重，为第i个粒子t时刻的归一化权重，n(t)为t时刻的粒子数量。

20、进一步地，步骤6中所述根据比较结果调整t时刻的粒子数量，具体为：

21、若计算所得t时刻的有效粒子数量大于设定粒子目标数量的上限，则

22、调整t时刻的粒子数量为n*(t)＝n(t)-m；其中n(t),n*(t)为调整前、后的粒子数量，m为粒子集数目调整值；

23、若计算所得t时刻的有效粒子数量小于设定粒子目标数量的下限，则调整t时刻的粒子数量为n*(t)＝n(t)+m；

24、否则，保持t时刻的粒子数量不变。

25、进一步地，步骤8中所述利用新粒子集的粒子平均状态对目标的状态进行估计，公式表示为：

26、

27、式中，为新粒子集中第i个粒子t时刻的预测状态，n*(t)为t时刻调整后的粒子数量，xt为估计得到的目标状态。

28、进一步地，初始化的目标跟踪粒子集，状态的先验密度符合高斯分布：

29、p(x0)＝n(x0；0；d0)；

30、其中，d0是对角矩阵：σr和σv分别表示位置先验方差和速度先验方差。

31、进一步地，用于步骤3粒子状态预测的状态转移方程形式为：

32、

33、其中，为转移矩阵，转移矩阵a中的元素t是系统观测之间的时间间隔；是第i个粒子t-1时刻的状态；ut是高斯矢量的实现，i是单位矩阵；是预测得到第i个粒子t时刻的状态。

34、一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述任一项技术方案所述的基于自适应粒子滤波的实时目标跟踪方法。

35、与现有技术相比，本发明具有以下有益效果：

36、1、本发明选取能够表征当前观测值下的状态后验概率分布的有效粒子数量，并基于有效粒子数量实时调整粒子集中的粒子数量，既能保证目标跟踪精度，又减少粒子数目从而提高了计算速度，适合实时系统；

37、2、本发明目标跟踪所采用的自适应粒子滤波方法，在判断粒子数目时，只需要计算有效样本数目，避免了现有自适应粒子滤波方法的复杂计算量，更加适用于实时处理和硬件实现；

38、3、本发明目标跟踪所采用的自适应粒子滤波方法，粒子数量的调整只在重采样步骤即可完成，不影响粒子滤波的采样、权值计算和状态估计这三个步骤的并行计算，因此在硬件并行实现时更有优势，更加适合硬件计算系统。