三维空间下快速UK-GMPHD多目标跟踪方法

三维空间下快速uk-gmphd多目标跟踪方法
技术领域
1.本发明属于单传感器多目标跟踪领域，涉及一种三维空间下快速uk-gmphd多目标跟踪方法。在三维空间内，通过对高斯混合概率假设密度滤波器(gmphd)进行改进，实现一种非线性观测条件下快速的无迹卡尔曼高斯混合概率假设密度滤波器(uk-gmphd)多目标跟踪方法，能够在复杂环境下保持对多目标跟踪精度的同时，大幅度提高跟踪效率。


背景技术：

2.多目标跟踪是指从传感器获取的数据中联合估计出目标的数量、状态等信息。目前，较流行的多目标跟踪方法分为两种：一种是基于数据关联(da)，另一种是基于随机有限集(rfs)。
3.随机有限集(rfs)理论的提出为解决多目标跟踪的问题提供了新的理论基础，受到国内外学者的重视。该理论是将目标状态和传感器观测信息分别建模为一个有限集，其具有较好的数学理论。一种基于随机有限集的工程实现是概率假设密度(phd)滤波器，通过使用一阶矩来近似表达服从高斯分布的多目标rfs的概率密度函数，从而避免使用数据关联方法来解决目标动力学状态估计问题。但是在非线性观测条件下，存在原始的单传感器高斯混合概率假设密度滤波方法(gnphd)在跟踪大量目标时出现跟踪效率低问题。


技术实现要素：

4.本发明的第一个目的是在非线性观测条件下，针对原始的单传感器高斯混合概率假设密度滤波方法(gnphd)在跟踪大量目标时出现跟踪效率低问题，为了加强实际工程应用，提出了一种三维空间下快速uk-gmphd多目标跟踪方法，简称f-uk-gmphd，有效提高多目标跟踪效率。
5.为了达到上述目的，本发明方法采用的技术方案如下：
6.步骤(1)、构建单传感器多目标跟踪场景，设置相关参数，包括目标运动的过程噪声和传感器的观测噪声；
7.步骤(2)、对多目标的状态、观测进行建模；
8.步骤(3)、基于步骤(1)、(2)，对传感器进行f-uk-gmphd滤波，得到后验高斯分量；
9.步骤(4)、重复步骤(3)得到一次蒙特卡洛下，对所有时刻的多目标估计结果，实现对多目标跟踪，并使用性能评价指标——最优子模式分配(ospa)对本发明方法的性能进行评估；
10.步骤(5)、重复步骤(3)-(4)进行下一次蒙特卡洛的滤波。
11.本发明的第二个目的是提供一种计算机可读存储介质，其上存储有计算机程序，当所述计算机程序在计算机中执行时，令计算机执行所述的方法。
12.本发明的第三个目的是提供一种计算设备，包括存储器和处理器，所述存储器中存储有可执行代码，所述处理器执行所述可执行代码时，实现所述的方法。
13.本发明的有益效果是：
14.本发明提出一种在三维空间下的非线性观测条件的快速uk-gmphd多目标跟踪方法(简称f-uk-gmphd)。通过对原始uk-gmphd的更新步骤进行改进，并省去合并操作来提高运行效率。因此，本发明提出的滤波器能够在复杂跟踪环境中显著提升计算效率。本发明提出了一套完整的处理方法和流程，配置结构明了，在保证跟踪精度的同时，有效提高多目标跟踪效率，可广泛应用于多目标跟踪领域。
附图说明
15.图1是本发明方法核心部分具体实现的流程图；
16.图2是目标真实运动轨迹图(目标数为50)；
17.图3是使用f-uk-gmphd滤波方法的目标跟踪图；
18.图4是在100次蒙特卡洛下，本发明方法与原始单传感器uk-gmphd滤波的ospa平均值的比较图。
19.图5是在100次蒙特卡洛下，本发明方法与原始单传感器uk-gmphd滤波的目标估计数目的比较图。
具体实施方式
20.以下结合技术方案和附图，详细叙述本发明的具体实施方式。
21.三维空间下的快速uk-gmphd多目标跟踪方法，包括以下步骤：
22.步骤(1)、构建单传感器多目标跟踪场景，设置相关参数，包括目标运动的过程噪声和传感器的观测噪声，具体是：
23.目标在三维空间内做非机动运动，其状态表示为x＝[p
x
，v
x
，py，vy，pz，vz]
t
，其中p
x
，py，pz分别为目标在x、y、z方向的位置，v
x
、vy、vz分别为目标在x、y、z方向的速度，上标t表示转置；
[0024]
设置目标运动的过程噪声协方差为σ
x
(k)2表示k时刻环境对目标在x方向速度的噪声方差，σy(k)2表示k时刻环境对目标在y方向速度的噪声方差，σz(k)2表示k时刻环境对目标在z方向速度的噪声方差；
[0025]
传感器在空间位置可表示为obs＝[p
x，obs
，p
y，obs
，p
z，obs
]
t
，p
x，obs
，p
y，obs
，p
z，obs
分别表示传感器在x、y、z方向的位置，上标t表示转置；设置传感器的观测噪声协方差为其中δ
d2
、δ
θ2
、δ
α2
分别表示传感器距离，方位角和俯仰角的噪声方差；
[0026]
步骤(2)、对多目标的状态、观测进行建模，具体是：
[0027]
2-1目标状态建模
[0028]
对于任意存在目标i，其k时刻状态表示为x
i，k
。在k-1时刻，m(k-1)个目标的状态可以表示为x
k-1
＝{x
1，k-1
，x
2，k-1
，
…
，x
m(k-1)，k-1
}。到k时刻，这些目标可能死亡或继续存在，继续存在的目标将发展为他们的新状态，并且可能出现新的目标，故m(k)个目标新的状态可以
表示为xk＝{x
1，k
，x
2，k
，
…
，x
m(k)，k
}。
[0029]
离散型系统的目标i运动状态方程可描述如下：
[0030]
x
i，k 1
＝f
·
x
i，k
g
·qi，k
ꢀꢀꢀ
(1)
[0031]
其中，x
i，k 1
和x
i，k
分别表示k 1和k时刻目标i的状态；f表示状态转移矩阵；g表示噪声驱动矩阵；q
i，k
表示k时刻目标i的过程噪声协方差。
[0032]
2-2传感器观测建模
[0033]
k时刻对目标状态xi形成的观测方程如下：
[0034]zi，k
＝g(x
i，k
) ε
ꢀꢀꢀ
(2)
[0035]
其中，z
i，k
表示对目标状态为x
i，k
的观测向量；g是非线性观测函数；ε表示传感器的观测噪声，一般假设观测噪声服从均值为0，协方差为r已知的高斯随机过程。
[0036]
在k时刻，传感器除了可能获取到目标的量测，还可能会受到环境杂波的干扰，从而产生伪量测。假设形成的杂波观测数量服从强度为λ(也称杂波期望数)的泊松过程，其位置分布在观测区域内呈均匀分布态势。因此，杂波方程可表示如下：
[0037][0038]
其中，ρ(n
c，k
)表示k时刻杂波数量为n
c，k
的泊松分布的概率函数；表示观测到第i个杂波，观测取值为的概率密度；v表示观测空间的体积。
[0039]
在k时刻传感器接收到的量测值由目标生成的量测(式(2))和杂波(式(3))共同组成，并且目标产生的量测与杂波是无法区分的。故可以将k时刻传感器接收到的量测值表示为zk＝{z
1，k
，z
2，k
，
…
，z
n(k)，k
}，n(k)表示k时刻传感器接收到的量测数目。
[0040]
步骤(3)、基于步骤(1)、(2)，对传感器进行f-uk-gmphd滤波，得到后验高斯分量，具体是：
[0041]
3-1：f-uk-gmphd预测
[0042]
假设在k-1时刻，多目标phd可表示为如下高斯混合的形式：
[0043][0044]
其中d
k-1
(x)表示k-1时刻目标状态为x的phd，j
k-1
是k-1时刻的高斯分量个数；和分别为第i个高斯分量的权重、状态均值和状态协方差；表示目标状态为x的空间分布，其服从状态均值为状态协方差为的高斯分布。
[0045]
那么k时刻的预测phd可表示为：
[0046]dk|k-1
(x)＝d
s，k|k-1
(x) γk(x)
ꢀꢀꢀ
(5)
[0047]
[0048][0049][0050][0051]
其中d
k|k-1
(x)表示k时刻目标状态为x的预测phd；d
s，k|k-1
(x)表示k时刻幸存目标状态为x的预测phd；γk(x)表示k时刻新生目标状态为x的预测phd；p
s，k
表示幸存概率；g表示噪声驱动矩阵；q
k-1
表示在k-1时刻的过程噪声协方差；j
γ，k
是新生目标高斯分量的数量；和分别代表k时刻第i个新生目标高斯分量的权重、状态均值和状态协方差；表示目标状态为x的空间分布，其服从状态均值为状态协方差为的高斯分布。分别代表k时刻第i个幸存目标高斯分量的预测状态均值和预测状态协方差。表示目标状态为x的空间分布，其服从状态均值为状态协方差为的高斯分布。
[0052]
由于k时刻预测phd d
k|k-1
(x)也为高斯混合形式，故式(5)可改写为如下：
[0053][0054]
其中，
[0055]jk|k-1
＝j
k-1
j
γ，k
ꢀꢀꢀ
(11)
[0056][0057]
表示目标状态为x的空间分布，其服从状态均值为状态协方差为的高斯分布；分别表示k时刻第i个预测高斯分量的权重、状态均值和状态协方差。
[0058]
3-2：获取sigma点集及相应权值
[0059]
3-2-1获取sigma点集
[0060]
对j
k|k-1
个预测高斯分量计算获取得到j
k|k-1
组sigma点集，每组个数为2n
x
1个，然后计算各组sigma点集内每个sigma点的预测状态均值
[0061][0062]
其中j表示第j个sigma点，j＝1，
…
，2n
x
1；n
x
表示状态维数；λu＝α
u2
(n
x
ku)-n
x
，表示一个比例因子，用来降低总的预测误差；αu表示采样点的分布状态；κu表示预设参数，可以取值为0，并通过调整κu使得矩阵(n
x
λu)p0为半正定矩阵；p0表示初始协方差；
[0063]
3-2-2计算sigma点相应的权值
[0064][0065]
其中为sigma点集内状态均值对应的权值，为sigma点集内状态协方差对应的权值，上标j表示第j个sigma点；βu是一个非负数(即βu≥0)，一般可选取为2。
[0066]
3-3：f-uk-gm-phd更新
[0067]
基于k时刻获得的多目标预测phd d
k|k-1
(x)，接下来将对其进行更新，得到后验phd dk(x)，phd更新公式如下：
[0068][0069]
其中φ表示一个系数(0≤φ≤1，pd为检测概率；d
d，k
(x；z)表示量测z更新预测phd。
[0070]
公式(15)中φ
·
(1-pd)
·dk|k-1
(x)表示漏检分量的更新，表示k时刻量测zk更新预测phd。
[0071][0072][0073][0074][0075]
其中，
[0076][0077][0078][0079][0080]
[0081][0082][0083][0084]
上述中，表示k时刻第l个后验高斯分量的权重、状态均值和状态协方差，jk表示更新后的高斯分量数；表示目标状态为x的空间分布，其服从状态均值为状态协方差为的高斯分布；pd为检测概率；表示量测z的空间分布，其服从均值为协方差为的高斯分布；kk表示杂波强度；g(
·
)为非线性观测函数；z
m，k
初始为一个空集合，用来记录被更新使用过的量测，即每更新完一个预测phd，将权重最大的phd分量对应的量测z
ψ，k
加入z
m，k
集合；表示k时刻第i个量测z的似然，ψ表示量测索引；表示k时刻第l个后验高斯分量的增益；z
ψ，k
表示k时刻第ψ个量测；表示第ψ个预测观测的均值；r表示量测噪声协方差；为sigma点集内第j个sigma点的状态均值对应的权值；为sigma点集内第j个sigma点状态协方差对应的权值；表示第i个预测观测的均值；表示第i个sigma点集内第j个预测状态均值；表示第i个sigma点集内第j个预测观测；表示第i个预测观测的协方差；表示第i个状态测量的协方差。
[0085]
注意，的后验高斯分量不属于d
d，k
(x；z)，最后更新完的高斯分量数jk≤j
k|k-1
。
[0086]
为了方便理解，对上述phd的更新步骤做一些说明。与传统的phd更新不同的是，针对漏检分量的更新，原始phd将漏检分量被保留下来这会增加一定的计算量，乘以一个权重φ以便在后续剪枝步骤减少高斯分量数。针对量测更新，传统的phd量测更新是一个量测会更新出一组后验phd分量，分别带有一定的权重，再进行归一化处理。本发明更新则是使用每个量测更新某个phd预测分量后，将权重最大的phd分量保留，同时删除更新后的权重最大的phd分量所对应的量测z
ψ，k
(由于一个目标最多产生一个量测)。为了加快更新效率，在使用量测更新预测phd时，还可引入马氏距离对整个观测空间进行量测分割，删除一些不可能的量测(离目标较远的量测)。
[0087]
3-4：剪枝
[0088]
经步骤3-1至3-3完成k时刻更新后，得到目标后验phd的后验高斯分量个数面临着随着时间推移不断增加的计算问题。传统phd采用剪枝与合并操作降低高斯分量个数，但由于合并涉及遍历，并且每次都会进行马氏距离的运算，涉及到矩阵逆的计算，当目标较多时，计算效率低。故为了加快计算效率，本发明仅采用剪枝操作，即通过对权重小于阈值τh的高斯分量进行剪枝，保留权重较大的高斯分量：
[0089][0090]
其中修剪阈值0＜τh≤1。
[0091]
剪枝后的高斯分量可表示如下：
[0092][0093]
其中，表示k时刻剪枝后的第i个高斯分量的权重、状态均值和状态协方差；表示k时刻剪枝后的高斯分量的个数；表示目标状态为x的空间分布，其服从状态均值为状态协方差为的高斯分布；
[0094]
3-5：新生目标自动起始
[0095]
3-5-1设置参数：最大速度vmax和最小速度vmin
[0096]
3-5-2k时刻用于新生的量测表示为(具体由z
k-z
m，k
计算得出)，k-1时刻用于新生的量测则为n(k)表示k时刻量测数目。
[0097]
当以下公式成立时，新生目标起始：
[0098][0099]
其中h
p
(
·
)表示映射函数，即将量测映射到笛卡尔坐标系。
[0100]
3-5-3重复步骤3-5-2，遍历所有量测集合和z
k-1
，得到的新生目标高斯分量为：
[0101][0102]
3-5-4将作为k时刻的后验多目标phd，同时将新生γk(x)传递到k 1时刻进行下一时刻phd预测与更新。
[0103][0104]
3-6：多目标状态估计
[0105]
剪枝合并得到后验phd dk(x)，使用如下公式可得到最终目标的个数估计值：
[0106][0107]
其中round(
·
)表示取整操作。
[0108]
取前个权重大于某阈值τm的状态均值作为k时刻多目标状态
[0109]
步骤(4)、重复步骤(3)得到一次蒙特卡洛下，对所有时刻的多目标估计结果，实现对多目标跟踪，并使用性能评价指标——最优子模式分配(ospa)对本发明方法的性能进行评估。此处为常规技术，故不详述。
[0110]
步骤(5)、重复步骤(3)-(4)进行下一次蒙特卡洛的滤波。此处为常规技术，故不详述。
[0111]
本发明仿真实验如下：
[0112]
仿真条件：本发明在intel(r)core(tm)i5-6300hq cpu@2.30ghz内存8.00gb的电脑上，使用matlabr2018b软件完成仿真。
[0113]
仿真场景设置：传感器的三维空间位置为obs＝[0，0，0]
t
，探测空间为[0，6km]
×
[0，6km]
×
[0，15km]。过程噪声协方差设置为观测噪声协方差设置为检测概率pd＝0.9，幸存概率p
s，k
＝0.99，修剪阈值τh＝0.001，杂波期望数λ＝100，跟踪步长100次。
[0114]
仿真结果分析：图2为目标真实运动轨迹图(目标数为50)。经过本发明方法后的结果如图3、图4、图5、表1所示。图3为跟踪场景，该场景模拟多个目标的运动轨迹并且给出f-uk-gmphd跟踪效果；图4是100次蒙特卡洛仿真后与原始单传感器uk-gmphd滤波的ospa平均值的比较图；图5是100次蒙特卡洛仿真后与原始单传感器uk-gmphd滤波的目标估计个数的比较图；表1是在100次蒙特卡洛下平均跑一次蒙特卡洛所需时间。从图4比较得出本发明方法与传统uk-gmphd相比具有相对较大的ospa，但是从表1可以比较得出本发明方法较传统uk-gmphd计算效率极大提升，从而说明本发明算法在保证跟踪精度的同时，有效快速提高对多目标跟踪效率，达到稳定快速的跟踪效果。
[0115]
表1在100次蒙特卡洛下，本发明方法(f-uk-gmphd)与原始phd滤波(uk-gmphd)平均跑一次蒙特卡洛所需时间的对比
[0116]