CN103345577B - 变分贝叶斯概率假设密度多目标跟踪方法 - Google Patents

Abstract

本发明公开了一种基于变分贝叶斯近似的概率假设密度多目标跟踪方法，它属于制导和智能信息处理技术领域，主要解决现有随机集滤波方法难以实现未知量测噪声环境下的变数目多目标跟踪问题。该方法通过引入变分贝叶斯近似技术，联合估计目标状态和量测噪声协方差的后验概率假设密度，并采用高斯混合逆伽马分布递推闭合解，从而实现未知量测噪声环境下数目变化的多目标跟踪，且具有良好的跟踪效果和鲁棒性，可以满足实际工程系统的设计需求，具有良好的工程应用价值。

Description

变分贝叶斯概率假设密度多目标跟踪方法

技术领域

本发明属于智能信息处理技术领域，涉及未知量测噪声的变数目多目标跟踪方法。具体地说是一种基于变分贝叶斯近似和概率假设密度滤波的多目标跟踪方法，可用于各种交通管制、机器人导航和精确制导等系统中的目标检测与跟踪。

背景技术

早期的多目标跟踪算法主要是通过目标与量测之间的数据关联技术实现跟踪，如最近邻算法、联合概率数据关联算法、多假设跟踪算法等。但这些算法大多是针对已知数目的多目标跟踪问题，且计算复杂度比较高，难以有效实现杂波环境下数目变化的多目标跟踪。

近年来，随机有限集(Randomfiniteset，RFS)理论得到了广泛关注，在不需要复杂数据关联的情况下，可以快速实现对未知数目的多目标跟踪，且可以实时地估计目标数目。目前，基于随机有限集理论的高斯混合概率假设密度(GM-PHD)滤波方法已被证明是实现数目变化的多目标跟踪有效方法。但在滤波过程中，该类方法需要事先假定已知目标的量测噪声，限制了这类方法的应用范围。通常在真实跟踪场景中，量测噪声是未知且时变的，因此，如果能正确估计未知的量测噪声，将有助于对真实跟踪场景的分析，可以改善算法对真实跟踪场景的适应能力，提高杂波环境下多扩展目标的跟踪性能。

发明内容

针对上述问题，本发明提出一种基于变分贝叶斯近似的概率假设密度多目标跟踪方法，以解决真实跟踪场景中未知量测噪声环境下数目变化的多目标跟踪问题，具有良好的跟踪效果和鲁棒性，可以满足实际工程系统的设计需求。

实现本发明的关键技术是：在概率假设密度滤波框架下，引入变分贝叶斯近似技术，联合估计目标状态和量测噪声协方差的后验概率假设密度，采用高斯混合逆伽马(Invertedgamma，IG)分布递推闭合解，实现未知量测噪声变数目的多目标跟踪。

为实现上述目标，具体实现步骤如下：

(1)初始化目标状态和量测噪声协方差的联合后验概率假设密度v₀(x，R)：

$v_0(x,R)=\underset{i=1}{\overset{J_0}{\Σ}}\[w_0^{\left(i\right)}N\left(x;m_0^{\left(i\right)},P_0^{\left(i\right)}\right)\underset{l=1}{\overset{d}{\Π}}IG({\left({\mathrm{\σ}}_{0,l}^{\left(i\right)}\right)}^2;{\mathrm{\α}}_{0,l}^{\left(i\right)},{\mathrm{\β}}_{0,l}^{\left(i\right)})\]$

其中，和为第i个高斯分量参数，J₀为高斯分量个数；IG(·；)表示逆伽马分布，表示量测噪声标准差，和为逆伽马分布的两个参数。

(2)当k≥1，预测目标状态和量测噪声协方差的联合概率假设密度v_k|k-1(x，R)：

v_k|k-1(x，R)＝v_S，k|k-1(x，R)+b_k|k-1(x，R)+γ_k(x，R)

其中，v_S，k|k-1(x，R)为生存目标状态和量测噪声协方差的联合预测概率假设密度，b_k|k-1(x，R)和γ_k(x，R)分别表示衍生目标和新生目标的联合预测概率假设密度。

(3)更新目标状态和量测噪声协方差的联合概率假设密度v_k|k(x，R)：

(3a)设定逆伽马分布参数，其中l＝1，...，d，d为量测噪声协方差R的维度；

(3b)计算量测噪声协方差其中，n＝1，...，N为迭代次序，N为最大迭代次数；

(3c)如果n≤N，更新计算目标状态和协方差矩阵并判断是否小于很小的常量ε，如果小于ε，停止迭代，否则，更新参数返回步骤(3b)；

(4)修剪和融合高斯-逆伽马混合分量；

(5)计算目标数目，提取多目标状态。若下一时刻观测信息到达，转到步骤(2)进行迭代；否则，目标跟踪过程结束。

本发明具有以下优点：

(1)本发明引入了变分贝叶斯近似技术，通过估计多目标状态和量测噪声的联合后验概率假设密度，有效估计了各个目标在不同时刻的真实量测噪声，为复杂环境下多目标跟踪场景的分析提供了帮助，保证PHD滤波算法可以有效地实现对未知量测噪声环境下数目变化的多目标跟踪；

(2)本发明给出了高斯-逆伽马混合形式的闭合递推解，并对高斯-逆伽马混合分量进行了删减和合并，有效降低了计算复杂度，提高了运行效率。

附图说明

图1是本发明的整体流程图；

图2是采用本发明方法的多目标状态估计和真实轨迹效果图；

图3是采用本发明方法与传统GM-PHD方法估计目标数的比较效果图；

图4是采用本发明方法与传统GM-PHD方法的OSPA统计距离比较效果图；

图5是采用本发明方法估计量测噪声标准差的效果图；

图6是不同量测噪声环境下采用本发明方法和传统GM-PHD方法的平均OSPA距离比较效果图；

图7是不同杂波率环境下采用本发明方法和传统GM-PHD方法的OSPA距离比较效果图。

具体实施方式

一、基础理论介绍

1.变分贝叶斯近似技术

假设单个目标的状态方程和量测方程分别表示为：

x_k+1＝Fx_k+Gw_k1)

y_k＝h(x_k)+v_k2)

其中，表示k时刻目标的状态向量，F为一步转移矩阵，函数h(·)表示观测模型，w_k和v_k分别表示状态噪声和量测噪声，对应的协方差分别表示为Q_k和R_k。在真实跟踪场景中，R_k通常是未知且变化的，需要适时估计。

假设目标动态模型和量测噪声协方差无关，则目标状态x_k和量测噪声协方差R_k的联合后验概率分布可表示为：

p(x_k，R_k|y_1：k-1)＝∫p(x_k|x_k-1)p(R_k|R_k-1)p(x_k-1，R_k-1|y_1：k-1)dx_k-1dR_k-1

其中，p(x_k|x_k-1)和p(R_k|R_k-1)分别表示目标状态和量测噪声的转移概率分布，由于很难直接获得p(R_k|R_k-1)，导致无法直接计算出p(x_k，R_k|y_1：k)。

根据变分贝叶斯近似技术有p(x_k，R_k|y_1：k)≈Q_x(x_k)Q_R(R_k)，通过最小化p(x_k，R_k|y_1：k)和Q_x(x_k)Q_R(R_k)之间的Kullback-Leibler(KL)距离：

$KL\[Q_x\left(x_k\right)Q_R\left(R_k\right)\left|\right|p(x_k,R_k|y_{1:k})\]=\∫Q_x\left(x_k\right)Q_R\left(R_k\right)\·log\left(\frac{Q_x\left(x_k\right)Q_R\left(R_k\right)}{p(x_k,R_k|y_{1:k})}\right){\mathrm{dx}}_k{\mathrm{dR}}_k$

可得 $Q_x\left(x_k\right)=N(x_k;{\hat{x}}_k,P_k),Q_R\left(R_k\right)=\underset{l=1}{\overset{d}{\Π}}IG({\mathrm{\σ}}_{k,l}^2;{\mathrm{\α}}_{k,l},{\mathrm{\β}}_{k,l}),$ 其中，IG(·；α，β)表示参数为α和β的逆伽马分布，和P_k表示k时刻的状态估计及其协方差估计。

2.概率假设密度滤波

假设在k时刻，多目标状态集为观测集为其中，N_k和M_k分别表示k时刻目标数及量测数。若k-1时刻多目标的状态随机集为X_k-1，则k时刻的状态随机集X_k及量测随机集Z_k可分别表示为：

$\begin{array}{c}{X}_k=\left(\underset{x\∈X_{k-1}}{\∪}{S}_{k|k-1}\left(x\right)\right)\∪\left(\underset{x\∈X_{k-1}}{\∪}{B}_{k|k-1}\left(x\right)\right)\∪{\mathrm{\Γ}}_k\\ Z_k=K_k\∪\left(\underset{x\∈X_k}{\∪}{\mathrm{\Θ}}_k\left(x\right)\right)\end{array}$

其中，S_k|k-1(x)表示k-1时刻目标在k时刻存活的随机集，B_k|k-1(x)表示k-1时刻目标在k时刻衍生目标的随机集，Γ_k表示k时刻新生目标的随机集，Θ_k(x)表示源于真实目标的量测随机集，K_k表示由杂波引起的量测随机集。

根据贝叶斯估计理论，可推导出多目标联合后验概率密度分布的最优贝叶斯递推计算公式为：

p_k|k-1(X_k|Z_1：k-1)＝∫f_k|k-1(X_k|X)p_k-1(X|Z_1：k-1)μ_s(dX)

$p_k\left(X_k\right|Z_{1:k})=\frac{g_k\left(Z_k\right|X_k)p_{k|k-1}\left(X_k\right|Z_{1:k-1})}{\∫g_k\left(Z_k\right|X)p_{k|k-1}\left(X\right|Z_{1:k-1}){\mathrm{\μ}}_s\left(dX\right)}$

其中，μ_s表示状态空间的近似Lebesgue测度，p_k|k-1(X_k|Z_1：k-1)和p_k(X_k|Z_1：k)分别表示多目标联合预测概率密度分布和后验概率密度分布，f_k|k-1(·)表示状态转移概率密度函数，g_k(·)表示似然函数。

近年来，在此基础上，Mahler等学者提出了概率假设密度(PHD)滤波算法，通过递推计算多目标状态随机集合后验概率分布的一阶矩，可以实现对数目变化的多目标跟踪。PHD函数v_k(x)是状态空间上多峰函数，峰值个数等于目标数峰值的位置可近似认为各个目标的状态期望值。

假设v_k|k-1(x)和v_k|k(x)分别为k时刻的预测PHD函数和后验PHD函数，则PHD的预测方程和更新方程可分别表示为：

v_k|k-1(x)＝∫p_S，k|k-1(x′)f_k|k-1(x|x′)v_k-1(x′)d(x′)+∫β_k|k-1(x|x′)v_k-1(x′)d(x′)+γ_k(x)

$v_k\left(x\right)=\[1-p_{D,k}\left(x\right)\]v_{k|k-1}\left(x\right)+\underset{z\∈Z_k}{\Σ}\frac{p_{D,k}\left(x\right)g_k\left(z\right|x)v_{k|k-1}\left(x\right)}{{\mathrm{\κ}}_k\left(z\right)+\∫p_{D,k}\left(x\right)g_k\left(z\right|x)v_{k|k-1}\left(x\right)dx}$

其中，β_k|k-1(x)和γ_k(x)分别为衍生目标状态集合和新生目标状态集合的预测PHD，p_S，k|k-1(x)为目标从k-1时刻到k时刻的存活概率，p_D，k(x)为检测概率，κ_k(z)＝λ_kc_k(z)为杂波集合的PHD，其中，杂波数目服从λ_k的泊松分布，c_k(z)为观测空间的杂波概率密度分布。

二、本发明基于变分贝叶斯近似的概率假设密度多目标跟踪方法

1.变分贝叶斯概率假设密度滤波迭代过程

假设状态向量x和量测噪声协方差R相互独立，且p_S，k(x，R)＝p_S，k，p_D，k(x，R)＝p_D，k，则x和R的联合概率假设密度迭代公式可表示为：

v_k|k-1(x，R)＝∫(p_S，k|k-1(x′，R′)f_k|k-1(x，R|x′，R′)+β_k|k-1(x，R|x′，R′))v_k-1(x′，R′)d(x′)d(R′)+γ_k(x，R)

＝∫(p_S，k|k-1f_k|k-1(x|x′)p_k|k-1(R|R′)+β_k|k-1(x|x′)p_k|k-1(R|R′))v_k-1(x′，R′)d(x′)d(R′)+γ_k(x，R)

＝∫(p_S，k|k-1f_k|k-1(x|x′)+β_k|k-1(x|x′))p_k|k-1(R|R′)v_k-1(x′，R′)d(x′)d(R′)+γ_k(x，R)

$v_{k|k}(x,R)=(1-p_{D,k})v_{k|k-1}(x,R)+\underset{y\∈Y_k}{\Σ}\frac{p_{D,k}{v}_{D,k}(x,R|y)}{{\mathrm{\κ}}_k\left(y\right)+\∫\∫p_{D,k}{v}_{D,k}(x^{\′},R^{\′}|y)d\left(x^{\′}\right)d\left(R^{\′}\right)}$

其中，v_D，k(x，R|y)＝g_k(y|x，R)v_k|k-1(x，R)，由于R和p_k|k-1(R|R′)未知，所以无法直接获得g_k(y|x，R)，进而无法直接计算v_D，k(x，R|y)，但根据变分贝叶斯近似原理有v_D，k(x，R|y)≈D_x，k(x)D_R，k(R)，它们之间的KL距离可表示为：

$KL\[D_{x,k}\left(x\right)D_{R,k}\left(R\right)\left|\right|v_{D,k}(x,R|y)\]=\∫D_{x,k}\left(x\right)D_{R,k}\left(R\right)\·log\left(\frac{D_{x,k}\left(x\right)D_{R,k}\left(R\right)}{v_{D,k}(x,R|y)}\right){\mathrm{dx}}_k{\mathrm{dR}}_k$

其中，D_x，k(x)和D_R，k(R)可通过最小化KL距离获得， $D_{R,k}\left(R\right)=\∫\left(\underset{l=1}{\overset{d}{\Π}}IG\right({\left({\mathrm{\σ}}_{k,l}\right)}^2;{\mathrm{\α}}_{k,l},{\mathrm{\β}}_{k,l}\left)\right){\mathrm{d\σ}}_{k,l}.$

2.具体实施步骤

参照图1，本发明的具体实施步骤包括如下：

步骤1.令初始时刻k＝0，初始化参数J₀和d，并根下式计算初始化目标状态和量测噪声协方差的联合后验概率假设密度v₀(x，R)：

$v_0(x,R)=\underset{i=1}{\overset{J_0}{\Σ}}\[w_0^{\left(i\right)}N\left(x;m_0^{\left(i\right)},P_0^{\left(i\right)}\right)\underset{l=1}{\overset{d}{\Π}}IG({\left({\mathrm{\σ}}_{0,l}^{\left(i\right)}\right)}^2;{\mathrm{\α}}_{0,l}^{\left(i\right)},{\mathrm{\β}}_{0,l}^{\left(i\right)})\]$

步骤2.当k≥1，预测目标状态和量测噪声协方差的联合概率假设密度：

v_k|k-1(x，R)＝v_S，k|k-1(x，R)+b_k|k-1(x，R)+γ_k(x，R)

(2.1)预测存活目标高斯分量的均值和协方差

$\begin{array}{c}{m}_{S,k|k-1}^{\left(i\right)}=F_{S,k-1}^{\left(i\right)}{m}_{S,k-1}^{\left(i\right)}\\ P_{S,k|k-1}^{\left(i\right)}=Q_{k-1}+F_{S,k-1}^{\left(i\right)}{P}_{S,k-1}^{\left(i\right)}{\left(F_{S,k-1}^{\left(i\right)}\right)}^T\end{array}$

其中，Q_k-1和分别为过程噪声协方差和状态转移矩阵。

预测逆伽马分布参数和即 ${\mathrm{\α}}_{S,k|k-1,l}^{\left(i\right)}={\mathrm{\ρ}}_l{\mathrm{\α}}_{S,k-1,l}^{\left(i\right)},{\mathrm{\β}}_{S,k|k-1,l}^{\left(i\right)}={\mathrm{\ρ}}_l{\mathrm{\β}}_{S,k-1,l}^{\left(i\right)},$ 其中，ρ_l表示遗忘因子，且ρ_l∈(0，1]。和分别为k-1时刻第i个分量逆伽马分布的自由度和尺度参数，l＝1，...，d，d表示量测噪声协方差R的维度。计算存活目标状态和量测噪声协方差的联合预测概率假设密度v_S，k|k-1(x，R)：

$v_{S,k|k-1}(x,R)=p_{S,k}\underset{i=1}{\overset{J_{k-1}}{\Σ}}\[w_{k-1}^{\left(i\right)}N\left(x;m_{S,k|k-1}^{\left(i\right)},P_{S,k|k-1}^{\left(i\right)}\right)\underset{l=1}{\overset{d}{\Π}}IG({\left({\mathrm{\σ}}_{S,k|k-1,l}^{\left(i\right)}\right)}^2;{\mathrm{\α}}_{S,k|k-1,l}^{\left(i\right)},{\mathrm{\β}}_{S,k|k-1,l}^{\left(i\right)})\]$

其中，为第i个分量的权值，表示第i个分量量测噪声预测标准差。

(2.2)预测衍生目标高斯分量的均值和协方差

$\begin{array}{c}{m}_{b,k|k-1}^{(i,j)}=F_{b,k-1}^{\left(j\right)}{m}_{b,k-1}^{(i,j)}+d_{b,k-1}^{\left(j\right)}\\ P_{b,k|k-1}^{(i,j)}=Q_{b,k-1}^{\left(j\right)}+F_{b,k-1}^{\left(j\right)}{P}_{b,k-1}^{\left(i\right)}{\left(F_{b,k-1}^{\left(j\right)}\right)}^T\end{array}$

其中，和分别为过程噪声协方差和状态转移矩阵，表示衍生目标的过程噪声。预测衍生目标逆伽马分布参数，衍生目标的预测PHD可表示为：

$b_{k|k-1}(x,R)=\underset{i=1}{\overset{J_{k-1}}{\Σ}}\underset{j=1}{\overset{J_{b,k}}{\Σ}}\[w_{k-1}^{\left(i\right)}{w}_{b,k}^{\left(j\right)}N\left(x;m_{b,k|k-1}^{\left(i,j\right)},P_{b,k|k-1}^{\left(i,j\right)}\right)\underset{l=1}{\overset{d}{\Π}}IG({\left({\mathrm{\σ}}_{b,k|k-1,l}^{\left(i,j\right)}\right)}^2;{\mathrm{\α}}_{b,k|k-1,l}^{\left(i,j\right)},{\mathrm{\β}}_{b,k|k-1,l}^{\left(i,j\right)})\]$

其中，为第j个衍生目标分量权值，表示第i个存活目标分量的第j个衍生目标分量的量测噪声预测标准差。

(2.3)计算新生目标状态和量测噪声协方差的联合预测概率假设密度γ_k(x，R)：

${\mathrm{\γ}}_k(x,R)=\underset{i=1}{\overset{J_{\mathrm{\γ},k}}{\Σ}}\[w_{\mathrm{\γ},k}^{\left(i\right)}N\left(x;m_{\mathrm{\γ},k}^{\left(i\right)},P_{\mathrm{\γ},k}^{\left(i\right)}\right)\underset{l=1}{\overset{d}{\Π}}IG({\left({\mathrm{\σ}}_{\mathrm{\γ},k,l}^{\left(i\right)}\right)}^2;{\mathrm{\α}}_{\mathrm{\γ},k,l}^{\left(i\right)},{\mathrm{\β}}_{\mathrm{\γ},k,l}^{\left(i\right)})\]$

其中，J_γ，k为新生目标高斯分量参数，和为新生目标量测噪声的逆伽马分布参数，表示第i个新生目标分量量测噪声预测标准差。

步骤3.更新目标状态和量测噪声协方差的联合概率假设密度v_k|k(x，R)：

$v_{k|k}(x,R)\≈(1-p_D)v_{k|k-1}(x,R)+\underset{y\∈Y_k}{\Σ}\underset{j=1}{\overset{J_{k|k-1}}{\Σ}}\[w_k^{\left(j\right)}N\left(x;m_{k|k}^{\left(j\right)},P_{k|k}^{\left(j\right)}\right)\underset{l=1}{\overset{d}{\Π}}IG({\left({\mathrm{\σ}}_{k,l}^{\left(j\right)}\right)}^2;{\mathrm{\α}}_{k,l}^{\left(j\right)},{\mathrm{\β}}_{k,l}^{\left(j\right)})\]$

(3.1)更新逆伽马分布参数， ${\mathrm{\α}}_{k,l}^{\left(j\right)}=0.5+{\mathrm{\α}}_{k|k-1,l}^{\left(j\right)},{\mathrm{\β}}_{k,l}^{\left(j\right)\left(0\right)}={\mathrm{\β}}_{k|k-1,l}^{\left(j\right)};$

(3.2)计算量测噪声协方差 $R_k^{\left(j\right)\left(n\right)}=diag\[\frac{{\mathrm{\β}}_{k,1}^{\left(j\right)\left(n\right)}}{{\mathrm{\α}}_{k,1}^{\left(j\right)}},...,\frac{{\mathrm{\β}}_{k,d}^{\left(j\right)\left(n\right)}}{{\mathrm{\α}}_{k,d}^{\left(j\right)}}\],n=1,...,N$ 为迭代次序，N为最大迭代次数；

(3.3)如n≤N，更新计算目标状态协方差矩阵和权值即

$m_{k|k}^{\left(j\right)\left(n\right)}=m_{k|k-1}^{\left(j\right)}+K_k^{\left(j\right)\left(n\right)}(y-{\hat{y}}_{k|k-1}^{\left(j\right)})$

$P_{k|k}^{\left(j\right)\left(n\right)}=\[I-K_k^{\left(j\right)\left(n\right)}H\]P_{k|k-1}^{\left(j\right)}$

$w_k^{\left(j\right)\left(n\right)}=\frac{p_{D,k}{w}_{k|k-1}^{\left(j\right)}N({\hat{y}}_{k|k-1}^{\left(j\right)},S_{k|k-1}^{\left(j\right)\left(n\right)})}{{\mathrm{\κ}}_k\left(y\right)+p_{D,k}\underset{i=1}{\overset{J_{k|k-1}}{\Σ}}{w}_{k|k-1}^{\left(i\right)}N({\hat{y}}_{k|k-1}^{\left(j\right)},S_{k|k-1}^{\left(j\right)\left(n\right)})}$

其中， $S_{k|k-1}^{\left(j\right)\left(n\right)}={\mathrm{HP}}_{k|k-1}^{\left(j\right)}{H}^T+R_k^{\left(j\right)\left(n\right)},K_k^{\left(j\right)\left(n\right)}=P_{k|k-1}^{\left(j\right)}{H}^T{\[S_{k|k-1}^{\left(j\right)\left(n\right)}\]}^{-1},{\hat{y}}_{k|k-1}^{\left(j\right)}={\mathrm{Hm}}_{k|k-1}^{\left(j\right)},$ H表示观测矩阵，I表示单位矩阵，y表示量测；和分别为第j个分量的预测均值和预测协方差。

判断是否小于很小的常量ε，如果小于ε，停止迭代，否则，更新参数 ${\mathrm{\β}}_{k,l}^{\left(j\right)(n+1)}={\mathrm{\β}}_{k,l}^{\left(j\right)}+\frac{1}{2}{(y-{\mathrm{Hm}}_{k|k}^{\left(j\right)\left(n\right)})}_l^2+\frac{1}{2}{\left({\mathrm{HP}}_k^{\left(j\right)\left(n\right)}{H}^T\right)}_{ll},$ 其中，(·)_l表示向量的第l个分量，(·)_ll表示矩阵对角上第l个分量，返回步骤(3.2)。

(3.4)提取更新参数， $w_k^{\left(j\right)}=w_k^{\left(j\right)\left(n\right)},m_{k|k}^{\left(j\right)}=m_{k|k}^{\left(j\right)\left(n\right)},P_{k|k}^{\left(j\right)}=P_{k|k}^{\left(j\right)\left(n\right)},{\mathrm{\β}}_{k,l}^{\left(j\right)}={\mathrm{\β}}_{k,l}^{\left(j\right)\left(n\right)},$ 计算联合概率假设密度v_k|k(x，R)。

步骤4.修剪和融合高斯-逆伽马混合分量：

(4.1)参数设定。由步骤3获得的高斯-逆伽马混合分量为设定修剪阈值T₁和T₂，融合阈值U，最大高斯-逆伽马混合分量个数J_max；

(4.2)计算每个分量的量测噪声协方差，

(4.3)设s＝0， $I=\{i=1,...,J_k|{\mathrm{\&omega;}}_k^{\left(i\right)}>T_1,\left|\right|R_k^{\left(i\right)}|{|}_2<T_2\};$

(4.4)执行s＝s+1，取

$L=\{i\&Element;I|{(m_k^{\left(i\right)}-m_k^{\left(j\right)})}^T{\left(P_k^{\left(i\right)}\right)}^{-1}(m_k^{\left(i\right)}-m_k^{\left(j\right)})\&le;U\},$

${\widetilde{\mathrm{\&omega;}}}_k^{\left(s\right)}=\underset{i\&Element;L}{\&Sigma;}{\mathrm{\&omega;}}_k^{\left(i\right)},{\tilde{m}}_k^{\left(s\right)}=\frac{1}{{\widetilde{\mathrm{\&omega;}}}_k^{\left(i\right)}}\underset{i\&Element;L}{\&Sigma;}{\mathrm{\&omega;}}_k^{\left(i\right)}{m}_k^{\left(i\right)},$

${\widetilde{\mathrm{\&alpha;}}}_k^{\left(s\right)}=\frac{1}{{\widetilde{\mathrm{\&omega;}}}_k^{\left(s\right)}}\underset{i\&Element;L}{\&Sigma;}{\mathrm{\&omega;}}_k^{\left(i\right)}{\mathrm{\&alpha;}}_k^{\left(i\right)},{\widetilde{\mathrm{\&beta;}}}_k^{\left(s\right)}=\frac{1}{{\widetilde{\mathrm{\&omega;}}}_k^{\left(s\right)}}\underset{i\&Element;L}{\&Sigma;}{\mathrm{\&omega;}}_k^{\left(i\right)}{\mathrm{\&beta;}}_k^{\left(j\right)},$

${\tilde{P}}_k^{\left(s\right)}=\frac{1}{{\widetilde{\mathrm{\&omega;}}}_k^{\left(s\right)}}\underset{i\&Element;L}{\&Sigma;}{\mathrm{\&omega;}}_k^{\left(i\right)}(P_k^{\left(i\right)}+({\tilde{m}}_k^{\left(s\right)}-m_k^{\left(i\right)}\left){\left({\tilde{m}}_k^{\left(s\right)}-m_k^{\left(i\right)}\right)}^T\right),$

I＝I\L，直到为止；

(4.5)如果s＞J_max，按权值由大到小排列高斯-逆伽马混合分量，取前J_max个分量；

(4.6)输出修剪融合后的高斯-逆伽马混合分量

步骤5.提取多目标状态：

(5.1)根据权值计算目标数，即

(5.2)提取权值大于0.5的高斯-逆伽马分量作为目标状态。

步骤6.重复步骤2，继续跟踪未知量测噪声环境下数目变化的多目标。

本发明的效果可通过以下实验仿真进一步说明：

1.仿真条件及参数

假设多个目标在x-y平面上作匀速运动，目标状态表示为x＝[x，v_x，y，v_y]^T，其中，x和y分别为每个目标在笛卡尔坐标系中x方向和y方向上的坐标，v_x和v_y分别为每个目标在x方向和y方向上的速度。目标的状态方程如式1)所示，其中， $F=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right],G=\left[\begin{array}{cc}{T}^2/2& 0\\ T& 0\\ 0& T^2/2\\ 0& T\end{array}\right],$ T表示采样时间间隔。

量测方程为y_k＝Hx_k+v_k，其中， $H=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right].$ 仿真场景中过程噪声协方差为其中σ_w1＝σ_w2＝0.5m，测噪声协方差为其中σ_v1和σ_v2未知。假设新生目标的联合后验概率假设密度表示为：

${\mathrm{\&gamma;}}_k(x,R)=0.2\&times;\underset{i=1}{\overset{2}{\&Sigma;}}\left(N\right(m_{\mathrm{\&gamma;}}^{\left(i\right)},P_{\mathrm{\&gamma;}}\left)\underset{l=1}{\overset{d}{\&Pi;}}IG\right({\left({\mathrm{\&sigma;}}_{\mathrm{\&gamma;},l}^{\left(i\right)}\right)}^2;{\mathrm{\&alpha;}}_{\mathrm{\&gamma;},l}^{\left(i\right)},{\mathrm{\&beta;}}_{\mathrm{\&gamma;},l}^{\left(i\right)}\left)\right)$

其中， $m_{\mathrm{\&gamma;}}^{\left(1\right)}=\&lsqb;0m,0m/s,0m,0m/s\&rsqb;,m_{\mathrm{\&gamma;}}^{\left(2\right)}=\&lsqb;-10m,0m/s,0m,0m/s\&rsqb;,$ P_γ＝diag[5，1，5，1]。初始逆伽马分布的2个参数都取1，目标的存活概率和检测概率分别取p_S，k＝0.99和p_D，k＝0.98。服从泊松分布的杂波均值为λ＝10，采样间隔为T＝1s，设置阈值T₁＝10^-5，T₂＝100，U＝4，J_max＝100。最大目标数目为N_max＝20，遗忘因子ρ＝0.9，蒙特卡洛仿真次数为300。

2.仿真内容及结果分析

仿真实验中，本发明方法与传统的高斯混合概率假设密度滤波(GM-PHD)方法进行对比，实验主要从以下三个方面开展：

实验1：固定量测噪声协方差

仿真场景中的真实量测噪声标准差为σ_x＝σ_y＝1，比较本发明方法GM-VBPHD与传统GM-PHD方法的跟踪性能，其中，本发明方法中的量测噪声标准差是未知的，GM-PHD方法中采用真实的量测噪声标准差。

图2是本发明方法的状态估计和真实轨迹对比的效果图。可以看出，采用本发明方法可以较准确地估计目标状态。

图3是采用本发明方法与传统GM-PHD方法估计目标数的对比效果图，其中，σ＝1为真实量测噪声标准差。可以看出，本发明方法和采用真实量测噪声的GM-PHD方法估计精度相当，而采用不准确的量测噪声标准差σ＝0.1、3和5时，GM-PHD方法的估计精度下降，尤其是当σ＝0.1时，目标漏估现象比较严重。

图4是采用本发明方法与传统GM-PHD方法的OSPA统计距离对比效果图。同样可以看出，在未知量测噪声的情况下，本发明方法的跟踪精度与采用真实量测噪声标准差(σ＝1)的GM-PHD方法相当。

实验2：不同量测噪声协方差

图5是采用本发明方法估计不同量测噪声标准差的效果图。可以看出，本发明方法可以较准确地估计不同量测噪声的标准差。

图6是不同量测噪声环境下采用本发明方法和传统GM-PHD方法的平均OSPA距离比较的效果图。可以看出，针对不同量测噪声环境下的多目标跟踪，本发明方法的跟踪精度与采用真实量测噪声协方差的GM-PHD方法相当。

实验3：不同杂波率

图7是不同杂波率环境下采用本发明方法和传统GM-PHD方法的OSPA距离比较效果图。可以看出，针对未知量测噪声不同杂波环境下的多目标跟踪，本发明方法的跟踪精度与采用真实量测噪声协方差的传统GM-PHD方法相当。

Claims (1)

1.基于变分贝叶斯近似的概率假设密度多目标跟踪方法，包括：

(1)初始化目标状态和量测噪声协方差的联合后验概率假设密度v₀(x，R)：

$v_0\left(x,R\right)\&ap;\underset{i=1}{\overset{J_0}{\mathrm{\&Sigma;}}}\&lsqb;w_0^{\left(i\right)}N\left(x;m_0^{\left(i\right)},P_0^{\left(i\right)}\right)\underset{l=1}{\overset{d}{\&Pi;}}IG\left({\left({\mathrm{\&sigma;}}_{0,l}^{\left(i\right)}\right)}^2;{\mathrm{\&alpha;}}_{0,l}^{\left(i\right)},{\mathrm{\&beta;}}_{0,l}^{\left(i\right)}\right)\&rsqb;$

其中，和为第i个高斯分量参数，J₀为高斯分量个数；IG(·；)表示逆伽马分布，表示量测噪声标准差，和为逆伽马分布的两个参数；

(2)当k≥1，预测目标状态和量测噪声协方差的联合概率假设密度：

v_k|k-1(x，R)＝v_S，k|k-1(x，R)+b_k|k-1(x，R)+γ_k(x，R)

其中，v_S，k|k-1(x，R)为生存目标状态和量测噪声协方差的联合预测概率假设密度，b_k|k-1(x，R)和γ_k(x，R)分别表示衍生目标和新生目标的联合预测概率假设密度；

(3)更新目标状态和量测噪声协方差的联合概率假设密度v_k|k(x，R)；

(4)修剪和融合高斯-逆伽马混合分量；

(5)计算目标数目，提取多目标状态：

(5a)根据权值计算目标数，即

(5b)提取权值大于0.5的高斯-逆伽马分量作为目标状态；

其中，步骤(2)所述的预测目标状态和量测噪声协方差的联合概率假设密度v_k|k-1(x，R)，按下述步骤计算得到：

(2.1)预测存活目标高斯分量的均值和协方差

$m_{S,k|k-1}^{\left(i\right)}=F_{S,k-1}^{\left(i\right)}{m}_{S,k-1}^{\left(i\right)}$

$P_{S,k|k-1}^{\left(i\right)}=Q_{k-1}+F_{S,k-1}^{\left(i\right)}{P}_{S,k-1}^{\left(i\right)}{\left(F_{S,k-1}^{\left(i\right)}\right)}^T$

其中，Q_k-1和分别为过程噪声协方差和状态转移矩阵；

预测逆伽马分布参数和即 ${\mathrm{\&alpha;}}_{S,k|k-1,l}^{\left(i\right)}={\mathrm{\&rho;}}_l{\mathrm{\&alpha;}}_{S,k-1,l}^{\left(i\right)},{\mathrm{\&beta;}}_{S,k|k-1,l}^{\left(i\right)}={\mathrm{\&rho;}}_l{\mathrm{\&beta;}}_{S,k-1,l}^{\left(i\right)},$ 其中，ρ_l表示遗忘因子，且ρ_l∈(0，1]，和分别为k-1时刻第i个分量逆伽马分布的自由度和尺度参数，l＝1，...，d，d表示量测噪声协方差R的维度；计算存活目标状态和量测噪声的联合预测概率假设密度v_S，k|k-1(x，R)：

$v_{S,k|k-1}(x,R)=p_{S,k}\underset{i=1}{\overset{J_{k-1}}{\&Sigma;}}\&lsqb;w_{k-1}^{\left(i\right)}N(x;m_{S,k|k-1}^{\left(i\right)},P_{S,k|k-1}^{\left(i\right)})\underset{l=1}{\overset{d}{\&Pi;}}IG\left({\left({\mathrm{\&sigma;}}_{S,k|k-1,l}^{\left(i\right)}\right)}^2;{\mathrm{\&alpha;}}_{S,k|k-1,l}^{\left(i\right)},{\mathrm{\&beta;}}_{S,k|k-1,l}^{\left(i\right)}\right)\&rsqb;$

其中，p_S，k表示目标存活概率，为第i个分量的权值，表示第i个分量量测噪声预测标准差；

(2.2)预测衍生目标高斯分量的均值和协方差

$m_{b,k|k-1}^{\left(i,j\right)}=F_{b,k-1}^{\left(j\right)}{m}_{b,k-1}^{\left(i,j\right)}+d_{b,k-1}^{\left(j\right)}$

$P_{b,k|k-1}^{(i,j)}=Q_{b,k-1}^{\left(j\right)}+F_{b,k-1}^{\left(j\right)}{P}_{b,k-1}^{\left(i\right)}{\left(F_{b,k-1}^{\left(j\right)}\right)}^T$

其中，和分别为过程噪声协方差和状态转移矩阵，表示衍生目标的过程噪声，预测衍生目标逆伽马分布参数，

衍生目标的预测PHD可表示为：

$b_{k|k-1}(x,R)=\underset{i=1}{\overset{J_{k-1}}{\&Sigma;}}\underset{j=1}{\overset{J_{b,k}}{\&Sigma;}}\&lsqb;w_{k-1}^{\left(i\right)}{w}_{b,k}^{\left(j\right)}N(x;m_{b,k|k-1}^{\left(i,j\right)},P_{b,k|k-1}^{\left(i,j\right)})\underset{l=1}{\overset{d}{\&Pi;}}IG\left({\left({\mathrm{\&sigma;}}_{b,k|k-1,l}^{\left(i,j\right)}\right)}^2;{\mathrm{\&alpha;}}_{b,k|k-1,l}^{\left(i,j\right)},{\mathrm{\&beta;}}_{b,k|k-1,l}^{\left(i,j\right)}\right)\&rsqb;$

其中，为第j个衍生目标分量权值，表示第i个存活目标分量的第j个衍生目标分量的量测噪声预测标准差；

(2.3)计算新生目标状态和量测噪声协方差的联合预测概率假设密度：

${\mathrm{\&gamma;}}_k(x,R)=\underset{i=1}{\overset{J_{\mathrm{\&gamma;},k}}{\&Sigma;}}\&lsqb;w_{\mathrm{\&gamma;},k}^{\left(i\right)}N(x;m_{\mathrm{\&gamma;},k}^{\left(i\right)},P_{\mathrm{\&gamma;},k}^{\left(i\right)})\underset{l=1}{\overset{d}{\&Pi;}}IG({\left({\mathrm{\&sigma;}}_{\mathrm{\&gamma;},k,l}^{\left(i\right)}\right)}^2;{\mathrm{\&alpha;}}_{\mathrm{\&gamma;},k,l}^{\left(i\right)},{\mathrm{\&beta;}}_{\mathrm{\&gamma;},k,l}^{\left(i\right)})\&rsqb;$

其中，J_γ，k为新生目标高斯分量参数，和为新生目标量测噪声的逆伽马分布参数，表示第i个新生目标分量量测噪声预测标准差；

其中，步骤(3)所述的更新目标状态和量测噪声协方差的联合概率假设密度v_k|k(x，R)，按下述步骤计算得到：

(3.1)更新逆伽马分布参数： ${\mathrm{\&alpha;}}_{k,l}^{\left(j\right)}=0.5+{\mathrm{\&alpha;}}_{k|k-1,l}^{\left(j\right)},{\mathrm{\&beta;}}_{k,l}^{\left(j\right)\left(0\right)}={\mathrm{\&beta;}}_{k|k-1,l}^{\left(j\right)};$

(3.2)计算量测噪声协方差：其中，n＝1，…，N为迭代次序，N为最大迭代次数；

(3.3)如n≤N，更新计算目标状态协方差矩阵和权值

$m_{k|k}^{\left(j\right)\left(n\right)}=m_{k|k-1}^{\left(j\right)}+K_k^{\left(j\right)\left(n\right)}(y-{\hat{y}}_{k|k-1}^{\left(j\right)})$

$P_{k|k}^{\left(j\right)\left(n\right)}=\&lsqb;I-K_k^{\left(j\right)\left(n\right)}H\&rsqb;P_{k|k-1}^{\left(j\right)}$

$w_k^{\left(j\right)\left(n\right)}=\frac{p_{D,k}{w}_{k|k-1}^{\left(j\right)}N\left({\hat{y}}_{k|k-1}^{\left(j\right)},S_{k|k-1}^{\left(j\right)\left(n\right)}\right)}{{\mathrm{\&kappa;}}_k\left(y\right)+p_{D,k}\underset{i=1}{\overset{J_{k|k-1}}{\&Sigma;}}{w}_{k|k-1}^{\left(i\right)}N({\hat{y}}_{k|k-1}^{\left(j\right)},S_{k|k-1}^{\left(j\right)\left(n\right)})}$

其中， $S_{k|k-1}^{\left(j\right)\left(n\right)}={\mathrm{HP}}_{k|k-1}^{\left(j\right)}{H}^T+R_k^{\left(j\right)\left(n\right)},K_k^{\left(j\right)\left(n\right)}=P_{k|k-1}^{\left(j\right)}{H}^T{\&lsqb;S_{k|k-1}^{\left(j\right)\left(n\right)}\&rsqb;}^{-1},{\hat{y}}_{k|k-1}^{\left(j\right)}={\mathrm{Hm}}_{k|k-1}^{\left(j\right)},$ H表示观测矩阵，I表示单位矩阵，y表示量测；和分别为第j个分量的预测均值和预测协方差，p_D，k表示目标的检测概率，κ_k(y)表示杂波概率假设密度；

判断是否小于很小的常量ε，如果小于ε，停止迭代；否则，更新参数即 ${\mathrm{\&beta;}}_{k,l}^{\left(j\right)(n+1)}={\mathrm{\&beta;}}_{k,l}^{\left(j\right)}+\frac{1}{2}{(y-{\mathrm{Hm}}_{k|k}^{\left(j\right)\left(n\right)})}_l^2+\frac{1}{2}{\left({\mathrm{HP}}_k^{\left(j\right)\left(n\right)}{H}^T\right)}_{ll},$ 其中，(·)_l表示向量的第l个分量，(·)_ll表示矩阵对角上第l个分量，返回步骤(3.2)；

(3.4)提取更新参数： $w_k^{\left(j\right)}=w_k^{\left(j\right)\left(n\right)},m_{k|k}^{\left(j\right)}=m_{k|k}^{\left(j\right)\left(n\right)},P_{k|k}^{\left(j\right)}=P_{k|k}^{\left(j\right)\left(n\right)},{\mathrm{\&beta;}}_{k,l}^{\left(j\right)}={\mathrm{\&beta;}}_{k,l}^{\left(j\right)\left(n\right)},$ 并计算联合概率假设密度v_k|k(x，R)；

其中，步骤(4)所述的修剪和融合高斯-逆伽马混合分量，按下述步骤计算得到：

(4.1)参数设定：由步骤(3)获得的高斯-逆伽马分量为设定修剪阈值T₁和T₂，融合阈值U，最大高斯-逆伽马分量个数J_max；

(4.2)计算每个分量的量测噪声协方差，

(4.3)设s＝0， $I=\{i=1,...,J_k|{\mathrm{\&omega;}}_k^{\left(i\right)}>T_1,\left|\right|R_k^{\left(i\right)}|{|}_2<T_2\};$

(4.4)执行s＝s+1，取

$L=\{i\&Element;I|{(m_k^{\left(i\right)}-m_k^{\left(j\right)})}^T{\left(P_k^{\left(i\right)}\right)}^{-1}(m_k^{\left(i\right)}-m_k^{\left(j\right)})\&le;U\},$

${\widetilde{\mathrm{\&omega;}}}_k^{\left(s\right)}=\underset{i\&Element;L}{\&Sigma;}{\mathrm{\&omega;}}_k^{\left(i\right)},{\tilde{m}}_k^{\left(s\right)}=\frac{1}{{\widetilde{\mathrm{\&omega;}}}_k^{\left(l\right)}}\underset{i\&Element;L}{\&Sigma;}{\mathrm{\&omega;}}_k^{\left(i\right)}{m}_k^{\left(i\right)},$

${\widetilde{\mathrm{\&alpha;}}}_k^{\left(s\right)}=\frac{1}{{\widetilde{\mathrm{\&omega;}}}_k^{\left(s\right)}}\underset{i\&Element;L}{\&Sigma;}{\mathrm{\&omega;}}_k^{\left(i\right)}{\mathrm{\&alpha;}}_k^{\left(i\right)},{\widetilde{\mathrm{\&beta;}}}_k^{\left(s\right)}=\frac{1}{{\widetilde{\mathrm{\&omega;}}}_k^{\left(s\right)}}\underset{i\&Element;L}{\&Sigma;}{\mathrm{\&omega;}}_k^{\left(i\right)}{\mathrm{\&beta;}}_k^{\left(i\right)},$

${\tilde{P}}_k^{\left(s\right)}=\frac{1}{{\widetilde{\mathrm{\&omega;}}}_k^{\left(s\right)}}\underset{i\&Element;L}{\&Sigma;}{\mathrm{\&omega;}}_k^{\left(i\right)}(P_k^{\left(i\right)}+({\tilde{m}}_k^{\left(s\right)}-m_k^{\left(i\right)}\left){\left({\tilde{m}}_k^{\left(s\right)}-m_k^{\left(i\right)}\right)}^T\right),$

I＝I\L，直到为止；

(4.5)如果s＞J_max，按权值由大到小排列高斯-逆伽马分量，取前J_max个分量；

(4.6)输出修剪融合后的高斯-逆伽马分量