CN104050686A - 一种新型的密集空间目标跟踪方法 - Google Patents

Abstract

本发明提出了一种新型的密集空间目标跟踪方法，提出了把密集的多个跟踪目标合成一组看作一个整体，使用组重心测量值来更新无迹卡尔曼滤波算法中的状态估计值，然后把无迹卡尔曼滤波算法的状态估计嵌进粒子滤波中，利用粒子滤波算法去估计组内的目标的分布，从而实现对每个目标的跟踪。本发明提出的新的算法避免了传统跟踪算法中所使用的数据关联方法，减少了滤波算法的复杂性，提高了滤波的有效性，并且提高目标跟踪的准确性。本发明适用于跟踪不规则分布的空间目标。

Description

一种新型的密集空间目标跟踪方法

技术领域

本发明涉及一种无线传感器网络领域和空间多目标跟踪领域，具体来说是一种基于无迹卡尔曼滤波和粒子滤波的密集空间目标跟踪方法。

背景技术

空间多目标跟踪技术很早就被提出来了，随着数据关联等技术在多目标跟踪理论方面取得了开创性的突破，由数据关联技术和滤波技术相结合提出了很多新的目标跟踪算法。目前常见的几种典型的数据关联方法有最邻近法、联合概率数据关联法以及多假设跟踪法等，但是传统的数据关联法在解决复杂环境下的多目标跟踪问题存在很多难题，所以卡尔曼滤波算法和粒子滤波算法等一类新的目标跟踪算法被提了出来。

无迹卡尔曼滤波算法是选择有限个近似高斯分布的离散点，然后对每个离散点进行非线性变换，得到变换后的点，将它们的均值和方差经过加权处理，就可得到非线性系统状态均值和协方差。

粒子滤波是采用蒙特卡罗采样的贝叶斯滤波方法，它将复杂的目标状态分布表示为一组加权值，通过寻找在粒子滤波中最大加权值的粒子来确定目标最可能所处的状态分布。粒子滤波的优点是有很好的鲁棒性和抗干扰性，在非线性复杂的噪声环境下能体现粒子滤波的优越性。但是粒子滤波存在两个显著的问题，采样过程中粒子的退化，即重要性权重的方差随时间的推移随机递增，使得粒子的权值聚集到少数粒子上；采用大量粒子模拟状态变量的后验概率密度函数会造成计算的复杂性。

利用传感器对密集空间目标进行跟踪一直是一个难题，传感器受到分辨率和数据更新率以及能量的限制。然而在密集空间目标跟踪时分辨率和数据更新率是很重要的两个参数。利用传感器进行密集空间目标跟踪时要解决的主要问题是：问题一、在多个目标落在同一分辨单元时，只有一个检测值或者返回值；问题二、在测量时，单个目标可能导致多个测量值，尤其是更新缓慢的传感器。针对无迹卡尔曼滤波和粒子滤波的特点，本发明提出了基于这两种滤波的新的目标跟踪算法来解决以上难题。

发明内容

本发明为了解决了上述难题，提出一种新型的密集空间目标跟踪方法，本方法基于卡尔曼滤波和粒子滤波，不是对单个目标进行跟踪，而是把所有的跟踪目标作为一组视为一个整体，利用无迹卡尔曼滤波得到这个组的行为状态估计，再把这个状态估计代入粒子滤波中，利用粒子滤波去获取这组内目标的分布情况。

一种新型的密集空间目标的跟踪方法，具体步骤如下：

(1)建立传感器的测量模型，并选择传感器的参数；

(2)根据一组目标的重心位置对无迹卡尔曼滤波进行初始化；

(3)通过传感器测量得到的组目标的径向距离和方位角来更新无迹卡尔曼滤波，估计得到组重心位置、速度和加速度(在笛卡尔坐标系中)；

(4)把无迹卡尔曼滤波得到的状态估计带进粒子滤波，去更新粒子的当前状态；

(5)粒子重要性权值的更新；

(6)计算加权粒子的后验概率密度，得到组内目标分布情况，实现目标跟踪；

下面具体阐述无迹卡尔曼滤波算法过程，并把状态估计用进粒子滤波中，来更新粒子的当前状态，使得粒子滤波得到更新，得到加权粒子新的后验概率密度，并且对粒子滤波中粒子权值的进行更新，最后得到组内目标的概率密度函数，实现目标的跟踪。

本发明系统模型为：

x_k＝f(x_k-1,θ_k-1)+w_k-1           (1)

z_k＝h_k(x_k,θ_k)+v_k             (2)其中，x_k∈R^n×1为系统状态矢量，f_k为n为向量函数，z_k∈R^mx1为系统观测矢量，h_k为m维向量函数，w_k为n维随机过程噪声，v_k为m维随机测量噪声。

滤波前作如下假设，过程噪声和量测噪声为互不相关的零均值白噪声，即w_k～N(0,Q_k)，v_k～N(0,R_k)。

无迹卡尔曼滤波初始化：k＝0，得到公式(3)和公式(4)：

${\hat{x}}_{0|0}={\stackrel{\‾}{x}}_0=E\left(x_0\right)---\left(3\right)$

cov(x₀)＝P₀                (4)

在k-1时刻的状态为求更新后的状态估计

首先计算在k-1时刻采样的样本点集σ点(i＝1,…2n)：

${\mathrm{\χ}}_{k-1|k-1}^0={\hat{x}}_{k-1|k-1}---\left(5\right)$

${\mathrm{\χ}}_{k-1|k-1}^i={\hat{x}}_{k-1|k-1}+{\left(\sqrt{(n+\mathrm{\λ})p_{k-1|k-1}}\right)}_i,i=1,\·\·\·n---\left(6\right)$

${\mathrm{\χ}}_{k-1|k-1}^i={\hat{x}}_{k-1|k-1}-{\left(\sqrt{(n+\mathrm{\λ})p_{k-1|k-1}}\right)}_i,i=n+1,\·\·\·2n---\left(7\right)$

其中，λ＝α²(n+κ)-n，α是决定σ点的分布，通常设为一个较小的正数(1＞α≥1e^-4)。κ为第二个尺度参数，通常设为0或3-n。

通过计算样本点集σ点中的i＝1,2…,2n来计算p_k|k-1，计算如下：

${\mathrm{\χ}}_k^i=f_k({\mathrm{\χ}}_{k-1|k-1}^i,{\mathrm{\θ}}_{k-1|k-1}^i),i=\mathrm{1,2}\·\·\·,2n---\left(8\right)$

${\hat{x}}_{k|k-1}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}^m{\mathrm{\χ}}_k^i---\left(9\right)$

$p_{k|k-1}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}^c({\mathrm{\χ}}_k^i-{\hat{x}}_{k|k-1}){({\mathrm{\χ}}_k^i-{\hat{x}}_{k|k-1})}^T+Q_{k-1}---\left(10\right)$

其中w^m、w^c表达式如下：

w^m＝λ/(n+λ)               (11)

w^c＝λ(n+λ)+(1-α²+β)          (12)

等式(12)中的β表示状态的分布，对于高斯分布β＝2是最优的，如果状态变量是单变量，则最佳的选择是β＝0。

通过公式(9)和公式(10)得到的p_k|k-1通过量测方程对x_k的传播为：

${\mathrm{\χ}}_k^i={\hat{x}}_{k|k-1}+{\left(\sqrt{(n+\mathrm{\λ})p_{k|k-1}}\right)}_i,i=1,2\·\·\·,n---\left(13\right)$

${\mathrm{\χ}}_k^i={\hat{x}}_{k|k-1}-{\left(\sqrt{(n+\mathrm{\λ})p_{k|k-1}}\right)}_i,i=n+1,\·\·\·,2n---\left(14\right)$

${\hat{z}}_{k|k-1}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}^m{\mathrm{\χ}}_k^i,i=\mathrm{1,2}\·\·\·,2n---\left(15\right)$

$p_{z_k}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}^c({\mathrm{\χ}}_k^i-{\hat{z}}_{k|k-1})({\mathrm{\χ}}_k^i-{\hat{z}}_{k|k-1})+R_k---\left(16\right)$

$p_{x_k{z}_k}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}^c({\mathrm{\χ}}_k^i-{\hat{x}}_{k|k-1}){({\mathrm{\χ}}_k^i-{\hat{z}}_{k|k-1})}^T---\left(17\right)$

通过公式(16)和公式(17)得到就可以更新状态估计

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})---\left(18\right)$

$K_k=p_{x_k{z}_k}{p}_{z_k}^{-1}---\left(19\right)$

接下来具体阐述把无迹卡尔曼滤波得到的状态估计代入粒子滤波中，去更新粒子的当前状态。本发明使用粒子滤波去估计组内目标的概率密度函数，粒子滤波中的每一个粒子状态都是由权值和支持点决定的，假设X_k＝{x₁,…,x_k}为直到k时刻的所有状态的集合，即为无迹卡尔曼滤波的状态估计。每个粒子由一组支持点组成，可表示为：

$X_k^i=\{({\stackrel{*}{x}}_{j,k}^i,{\stackrel{*}{y}}_{j,k}^i),j=1\·\·\·M\}---\left(20\right)$

其中，M为每个粒子拥有的支持点数，表示为第i个粒子的第k次迭代中的第j个支持点的x坐标，同理，表示这种计算中的y坐标。

对公式(20)中的的更新是粒子在状态空间的简单传播来完成的，这必须更新所有的粒子才能完成的。假设每一个粒子对应一个跟踪目标，粒子的运行的近似组的速度是由组重心来决定的，所以，每个粒子的支持点的更新映射为：

$X_k^i\→X_{k+1}^i,i=1\·\·\·N---\left(21\right)$

其中，N为总粒子数。定义重心估计采样周期为ΔT，为状态估计得到的目标速度和加速度。公式(21)中支持点的更新在笛卡尔坐标系中表示为：

${\stackrel{*}{x}}_{j,k+1}^i={\stackrel{*}{x}}_{j,k}^i+\mathrm{\ΔT}{\stackrel{.}{x}}_k+\frac{\mathrm{\Δ}{T}^2}{2}{\stackrel{..}{x}}_k+\mathrm{\ξ}---\left(22\right)$

${\stackrel{*}{y}}_{j,k+1}^i={\stackrel{*}{y}}_{j,k}^i+\mathrm{\ΔT}{\stackrel{.}{y}}_k+\frac{\mathrm{\Δ}{T}^2}{2}{\stackrel{..}{y}}_k+\mathrm{\ξ}---\left(23\right)$

由于考虑到组内的有些单个目标不可能一直随组的重心做同步运动，所以在公式(22)和公式(23)加入了小噪声ξ。

从公式(20)到公式(23)能够看出每个粒子可以设想为一个2维密度概率函数，它的支持点更新来实现滤波的更新。加权粒子的后验概率密度函数为：

$p\left(X_k\right|z_{1,k})=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i\mathrm{\δ}(X_k-X_k^i)---\left(24\right)$

公式(24)中为粒子权重，z_k是当前组目标跟踪的测量值。本发明中的先验概率用作重要采样分布，所以权值更新等式如下：

$w_{k+1}^i=w_k^{i}p\left(z_k\right|X_k^i)---\left(25\right)$

其中为粒子滤波中的后验概率，本发明采用最佳拟合测量法来计算由于每个粒子是一个2维的概率密度函数，因此可以使用2维直方图的相关性来求解，用ρ表示这两者的相关性，可以得到：

$p\left(z_k\right|X_k^i)\≈{\left(\frac{\mathrm{\ρ}+1}{2}\right)}^a---\left(26\right)$

a为上述中的加速度，常用的范围在[1,4]。把公式(26)带进(25)式可以实现权值更新。

在完成以上工作后，本发明还要将支持点转换到极坐标中，用来量化公式(2)中定义的映射，得到组内粒子的分布，用来表示得到公式(27)：

$X_k^i=\{({\stackrel{*}{r}}_{j,k}^i,{\stackrel{*}{\mathrm{\θ}}}_{j,k}^i),j=1\·\·\·M\}---\left(27\right)$

(27)式为一个概率密度函数，反映组内目标的分布情况，可以把它和传感器实际测量得到的概率密度函数相比较。

本发明针对密集空间目标的跟踪，解决了以往在跟踪密集空间目标常遇到的两大难题，提高了跟踪精度。本发明适用于非线性模型，使用多传感器的特定测量模型，能更加准确的模拟噪声的特征。提出无迹卡尔曼滤波和粒子滤波相结合不仅避免了粒子退化现象，还简化了计算复杂度。

附图说明

图1：为本发明流程图

图2：为测量模型图

具体实施方式

为使本发明的上述特征和优点能更加明显易懂，下面结合图1和具体实施方式对本发明作进一步详细说明。

首先将引入涉及到本方案的相关参数，并详细描述如下：

ΔT、Δθ、ΔR分别表示采样周期、波速宽度、大小变化范围；

r'、θ'分别表示是指到目标的径向距离和方位角；

Q、q、p分别为过程协方差、白噪声、状态协方差；

α、κ、β分别是决定σ点的分布，第二个尺度参数，状态的分布；

为状态估计得到的目标速度和加速度；

M、N分别为每个粒子拥有的支持点数和总粒子数；

X_k＝{x₁,…,x_k}为直到k时刻的所有状态的集合，为粒子权重，初始化值为

表示为第i个粒子的第k次迭代中的第j个支持点的x坐标，同理，表示这种计算中的y坐标；

分别表示这两个图像的相关性、粒子先验概率分布、极坐标中粒子分布；

本发明实例分别选择规则分布和不规则分布的目标各3组。规则分布的3组目标在极坐标中的组初始重心位置的径向距离都为20km，方位角分别为1.45rad、1.47rad和1.49rad，它们随着时间的变化做规则变化运动；不规则分布的3组目标在极坐标中的组初始重心位置的径向距离都为20km,方位角分别为1.5rad、1.53rad和1.55rad，它们随时间进行不规则运动。本发明具体实施步骤如下：

一、建立传感器的测量模型，并选择传感器的参数。传感器测量值是通过有一个旋转天线和固定扫描周期的2维雷达仿真得到的，在本发明中，使用了多个传感器。传感器的参数如表1，传感器的模型如图2。

表1传感器参数

参数名	参数值
		采样周期ΔT	2秒
波速宽度Δθ	1.1度
		大小变化范围ΔR	120米

通过图2，如果目标落入阴影区域，它将会被分配一个特定的角和变化范围，图中参数如表1，所以图2的量化公式为：

z_k(r,q)＝f(r',q')            (28)

二、根据一组目标的重心位置对无迹卡尔曼滤波进行初始化。把一组的重心作为初始位置，设测量到的重心位置为N_z，则测量值的噪声近似变化范围和角坐标分别为：无迹卡尔曼的非线性测量方程如下：

$\begin{array}{c}\stackrel{\‾}{r_k}=\frac{1}{N_z}\underset{l=1}{\overset{N_z}{\mathrm{\Σ}}}{r}_k^l---\left(29\right)\end{array}$

$\begin{array}{c}\stackrel{\‾}{{\mathrm{\θ}}_k}=\frac{1}{N_z}\underset{l=1}{\overset{N_z}{\mathrm{\Σ}}}{\mathrm{\θ}}_k^l---\left(30\right)\end{array}$

$H\left(\stackrel{\‾}{\mathrm{\θ}}\right)=a\tan \left(\frac{y}{x}\right)---\left(31\right)$

$H\left(\stackrel{\‾}{r}\right)=\sqrt{x^2+y^2}---\left(32\right)$

如图1中的100，对无迹卡尔曼滤波进行初始化：当k＝0时，

三、通过传感器测量得到的组目标的径向距离和方位角来更新无迹卡尔曼滤波，估计得到组中心位置的位置、速度和加速度(在笛卡尔坐标系中)。如图1中的101，由2维变量和替换了1维变量通过计算得到如图1中102。

本发明中无迹卡尔曼滤波中的重要参数值为表2：

表2

α	κ	β	γ
				0.0001	0	2	0.05

$r_k^i={\hat{r}}_{k|k-1}\±{\left(\sqrt{(n+\mathrm{\λ})p_{k|k-1}}\right)}_i,i=1,2\·\·\·,2n---\left(33\right)$

${\mathrm{\θ}}_k^i={\widehat{\mathrm{\θ}}}_{k|k-1}\±{\left(\sqrt{(n+\mathrm{\λ})p_{k|k-1}}\right)}_i,i=1,2\·\·\·,2n---\left(34\right)$

其中λ＝α²(n+κ)-n。

通过第二步测量模型方程可以定义测量矢量为：

$\stackrel{\‾}{z_k}=\left[\begin{array}{c}\stackrel{\‾}{r_k}\\ \stackrel{\‾}{{\mathrm{\θ}}_k}\end{array}\right]---\left(35\right)$

测量噪声的协方差近似为：

$R=\left[\begin{array}{cc}\frac{\mathrm{\Δ}{R}^2}{12N_z}& 0\\ 0& \frac{\mathrm{\Δ}{\mathrm{\Θ}}^2}{12N_z}\end{array}\right]---\left(36\right)$

过程噪声协方差定义如下：

$Q=\mathrm{\γ}\left[\begin{array}{cc}q& 0\\ 0& q\end{array}\right]---\left(37\right)$

假设白噪声的模型为：

$q=\left[\begin{array}{ccc}\frac{\mathrm{\Δ}{T}^5}{20}& \frac{\mathrm{\Δ}{T}^4}{s}& \frac{\mathrm{\Δ}{T}^3}{6}\\ \frac{\mathrm{\Δ}{T}^4}{8}& \frac{\mathrm{\Δ}{T}^3}{3}& \frac{\mathrm{\Δ}{T}^2}{2}\\ \frac{\mathrm{\Δ}{T}^3}{6}& \frac{\mathrm{\Δ}{T}^2}{2}& \mathrm{\ΔT}\end{array}\right]---\left(38\right)$

本发明是针对密集空间目标的跟踪，把密集的目标分成组来进行跟踪，本发明中使用一个简单的动态模型，这个模型中含有6种状态。把图1中106转换到笛卡尔坐标系中，状态矢量和转移矩阵表示为：

$s_k=\left[\begin{array}{c}{x}_k\\ {\stackrel{.}{x}}_k\\ {\stackrel{..}{x}}_k\\ y_k\\ {\stackrel{.}{y}}_k\\ {\stackrel{..}{y}}_k\end{array}\right]---\left(39\right)$

$\mathrm{\Φ}=\left[\begin{array}{cc}F& 0\\ 0& F\end{array}\right]---\left(40\right)$

其中 $F=\left[\begin{array}{ccc}1& \mathrm{\ΔT}& \frac{\mathrm{\Δ}{T}^2}{2}\\ 0& 1& \mathrm{\ΔT}\\ 0& 0& 1\end{array}\right].$

四、把无迹卡尔曼滤波得到的状态估计带进粒子滤波，去更新粒子的当前状态。如图1中的107，本发明使用粒子滤波去估计组内目标的概率密度函数，粒子滤波中的每一个粒子都是由权值和支持点决定的，每个粒子由一组支持点组成，可表示为：

$X_k^i=\{({\stackrel{*}{x}}_{j,k}^i,{\stackrel{*}{y}}_{j,k}^i),j=1\·\·\·M\}---\left(41\right)$

每个粒子的支持点的更新映射为

五、权值的更新。本发明中的先验概率用作重要采样分布，所以权值更新等式如下：

$w_{k+1}^i=w_k^{i}p\left(z_k\right|X_k^i)---\left(42\right)$

其中，通过计算转换得到先验概率 $p\left(z_k\right|X_k^i)\&ap;{\left(\frac{\mathrm{\&rho;}+1}{2}\right)}^a,(0<\mathrm{\&rho;}\&le;1).$

六、计算加权粒子的后验概率密度，得到组内目标分布情况，实现目标跟踪。把将支持点转换到极坐标中，用来量化公式(2)中定义的映射，得到组内粒子的分布，得到概率密度函数

本发明中粒子滤波的设计参数见表3：

表3

M	N	a	σp
				5	100	4	0.010

通过上述给定的参数，利用本发明去跟踪规则分布的密集目标和不规则分布的密集目标，得到的结果如下：

在跟踪规则分布的密集目标时，250秒就得到了粒子分布，提高了时效；本发明在跟踪不规则分布目标时更能体现它的优势，使最小距离测量平均误差和均方根误差达到0.06880km和0.05296km，明显提高密集目标跟踪的精度。

在此说明书中，本发明已参照特定的实施实例做了描述。但是，很显然仍可以做出各种修改和变换而不背离本发明的精神和范围。因此，说明书和附图应被认为是说明性的而非限制性的。

Claims (5)

1.一种新型的密集空间目标跟踪方法，是基于无迹卡尔曼滤波和粒子滤波的方法，包括以下步骤：

(1)建立传感器的测量模型，并选择传感器的参数；

(2)根据一组目标的重心位置对无迹卡尔曼滤波进行初始化；

(3)通过传感器测量得到的组目标的径向距离和方位角来更新无迹卡尔曼滤波，估计得到组重心位置、速度和加速度(在笛卡尔坐标系中)；

(4)把无迹卡尔曼滤波得到的状态估计带进粒子滤波，去更新粒子的当前状态；

(5)对粒子重要性权值进行更新；

(6)计算出加权粒子的后验概率密度，得到组内粒子分布情况，实现目标跟踪。

2.根据权利要求1所述的密集空间目标跟踪算法，其特征在于：步骤(1)选择的测量模型使用了多个传感器，如果目标落入特定阴影区域，它将会被分配一个特定的方位角Δθ和大小变化范围ΔR，测量模型量化公式如下：

z_k(r,θ)＝f(r',θ')

其中，r'、θ'分别表示是指到目标的径向距离和方位角。

3.根据权利要求1所述的密集空间目标跟踪算法，其特征在于：步骤(2)建立模型时，把密集的目标合成一组作为一个整体，把组的平均测量值提供给滤波器，如果测量到的重心位置为N_z，无迹卡尔曼的非线性测量方程如下：

$\left\{\begin{array}{c}\stackrel{\&OverBar;}{r_k}=\frac{1}{N_z}\underset{l=1}{\overset{N_z}{\mathrm{\&Sigma;}}}{r}_k^l\\ \stackrel{\&OverBar;}{{\mathrm{\&theta;}}_k}=\frac{1}{N_z}\underset{l=1}{\overset{N_z}{\mathrm{\&Sigma;}}}{\mathrm{\&theta;}}_k^l\end{array}\right.$

其中，表示到组重心的平均径向距离和方位角。

4.根据权利要求1所述的密集空间目标跟踪算法，其特征在于：步骤(4)把无迹卡尔曼滤波的状态估计代入粒子滤波中，更新粒子的状态，通过最优拟合测量法计算出粒子滤波的先验概率，实现粒子权值的更新。

5.根据权利要求1所述的密集空间目标跟踪算法，其特征在于：步骤(6)通过步骤(4)和步骤(5)可以计算出粒子的后验概率，通过测量方程可以把目标分布函数转换在笛卡尔坐标系中，在笛卡尔坐标中用 $X_k^i=\{({\stackrel{*}{r}}_{j,k}^i,{\stackrel{*}{\mathrm{\&theta;}}}_{j,k}^i),j=1\&CenterDot;\&CenterDot;\&CenterDot;M\}$ 表示跟踪目标的分布。