[发明专利]三维空间下快速UK-GMPHD多目标跟踪方法

权利要求书

1.三维空间下的快速UK-GMPHD多目标跟踪方法，包括以下步骤：

步骤(1)、构建单传感器多目标跟踪场景，设置相关参数，包括目标运动的过程噪声和传感器的观测噪声；

步骤(2)、对多目标的状态、观测进行建模；

步骤(3)、基于步骤(1)、(2)，对传感器进行F-UK-GMPHD滤波，得到后验高斯分量，具体是：

3-1：F-UK-GMPHD预测

假设在k-1时刻，多目标PHD表示为如下高斯混合的形式：

其中D_k-1(x)表示k-1时刻目标状态为x的PHD，J_k-1是k-1时刻的高斯分量个数；和分别为第i个高斯分量的权重、状态均值和状态协方差；表示目标状态为x的空间分布，其服从状态均值为状态协方差为的高斯分布；

那么k时刻的预测PHD表示为：

D_k|k-1(x)＝D_S,k|k-1(x)+γ_k(x) (5)

其中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的空间分布，其服从状态均值为状态协方差为的高斯分布；

由于k时刻预测PHDD_k|k-1(x)为高斯混合形式，故式(5)改写为如下：

其中，

J_k|k-1＝J_k-1+J_γ,k (11)

表示目标状态为x的空间分布，其服从状态均值为状态协方差为的高斯分布；分别表示k时刻第i个预测高斯分量的权重、状态均值和状态协方差；

3-2：获取Sigma点集及相应权值

3-2-1获取Sigma点集

对J_k|k-1个预测高斯分量计算获取得到J_k|k-1组Sigma点集，每组个数为2n_x+1个，然后计算各组Sigma点集内每个Sigma点的预测状态均值

其中j表示第j个Sigma点，j＝1,…,2n_x+1；n_x表示状态维数；λ_U＝α_U²(n_x+κ_U)-n_x，λ_U表示比例因子，α_U表示采样点的分布状态，κ_U表示预设参数，并通过调整κ_U使得矩阵(n_x+λ_U)P₀为半正定矩阵；P₀表示初始协方差；

3-2-2计算Sigma点相应的权值

其中为Sigma点集内状态均值对应的权值，为Sigma点集内状态协方差对应的权值，上标j表示第j个Sigma点；β_U是一个非负数；

3-3：F-UK-GM-PHD更新

根据公式(15)对多目标预测PHDD_k|k-1(x)进行更新，得到后验PHDD_k(x)：

其中φ表示一个系数，0≤φ≤1，p_d为检测概率，D_d,k(x；z)表示量测z更新预测PHD；

公式(15)中φ·(1-p_d)·D_k|k-1(x)表示漏检分量的更新，表示k时刻量测Z_k更新预测PHD；

其中，

其中表示k时刻第l个后验高斯分量的权重、状态均值和状态协方差；J_k表示更新后的高斯分量数；表示目标状态为x的空间分布，其服从状态均值为状态协方差为的高斯分布；p_d为检测概率；表示量测z的空间分布，其服从均值为协方差为的高斯分布；κ_k表示杂波强度；g(·)为非线性观测函数；Z_M,k表示权重最大的PHD分量对应的量测集合；表示k时刻第i个量测z的似然，Ψ表示量测索引；表示k时刻第l个后验高斯分量的增益；z_Ψ,k表示k时刻第Ψ个量测；表示第Ψ个预测观测的均值；R表示量测噪声协方差；为Sigma点集内第j个Sigma点的状态均值对应的权值；为Sigma点集内第j个Sigma点状态协方差对应的权值；表示第i个预测观测的均值；表示第i个Sigma点集内第j个预测状态均值；表示第i个Sigma点集内第j个预测观测；表示第i个预测观测的协方差；表示第i个状态测量的协方差；

3-4：剪枝

对目标后验PHD的后验高斯分量进行剪枝，保留权重较大的高斯分量：

其中τ_h表示修剪阈值，0τ_h≤1；

剪枝后的高斯分量表示如下：

其中,表示k时刻剪枝后的第i个高斯分量的权重、状态均值和状态协方差；表示k时刻剪枝后的高斯分量的个数；表示目标状态为x的空间分布，其服从状态均值为状态协方差为的高斯分布；

3-5：新生目标自动起始

3-5-1设置参数：最大速度VMAX和最小速度VMIN；

3-5-2k时刻用于新生的量测表示为k-1时刻用于新生的量测则为N(k)表示k时刻量测数目；

当以下公式成立时，新生目标自动起始；

其中h_p(·)表示映射函数，即将量测映射到笛卡尔坐标系；

3-5-3重复步骤3-5-2，遍历所有量测集合和Z_k-1，得到的新生目标高斯分量为：

3-5-4将作为k时刻的后验多目标PHD，同时将新生γ_k(x)传递到k+1时刻进行下一时刻PHD预测与更新；

3-6：多目标状态估计

剪枝合并得到后验PHDD_k(x)，使用如下公式得到最终目标的个数估计值:

其中round(·)表示取整操作；

取前个权重大于阈值τ_M的状态均值作为k时刻多目标状态

步骤(4)、重复步骤(3)得到一次蒙特卡洛下，对所有时刻的多目标估计结果，实现对多目标跟踪；

步骤(5)、重复步骤(3)-(4)进行下一次蒙特卡洛的滤波。