[发明专利]一种基于道路网的机动目标跟踪算法

权利要求书

1.一种基于道路网的机动目标跟踪算法，其特征在于，包括以下步骤：

A.构建道路信息库；

A-1.生成数字地图：将真实道路表示为一系列直线路段的组合，弯曲的道路用多个小的直线路段近似，用道路编号、起始点、结束点联合表示路段，连接的起始点和结束点表示道路交叉点，整个数字道路网由一系列首尾相连的直线路段组成；将地面空间定义分成开放区域和道路区域两大部分，道路路段和道路交叉点属于道路区域，其余区域为开放区域；

A-2.选定总的模型集合，根据路段和地形约束形成对应于各道路路段、道路交叉点和开放区域的目标运动模型集；

B.基于道路信息库的地面目标跟踪：

B-1.根据道路信息库确定总目标运动模型集其中，N_r为总模型集的模型个数，每一个运动模型M_p包括该模型的状态转移矩阵F_p和过程噪声协方差矩阵Q_p的信息，确定各模型间的模型转移概率矩阵[π_pq]，其中p＝1，2，..，N_r，q＝1，2，..，N_r；

B-2.初始化，令观测时刻k＝0，即为初始时刻；定义初始时刻的目标后验强度D₀(x)为：

其中，j＝1，2，...，J₀，J₀为初始时刻的目标后验强度的高斯分量数，表示均值为协方差矩阵为的高斯分布，和分别为高斯分量j的权重、均值和协方差矩阵，为初始时刻目标个数期望值；

B-3.对总目标运动模型集中的每一个运动模型分别定义一个GMPHD滤波器，将每个GMPHD滤波器初始时刻的目标后验强度函数D_0，p(x)都取为D₀(x)，即：

其中，D_0，p(x)为基于模型M_p(p＝1，2，..，N_r)定义的GMPHD滤波器的初始目标后验强度函数；

B-4.对高斯分量标号j(j＝1，2，..，J₀)，定义模型标记向量其中， (p＝1，2，..，N_r，j＝1，2，..，J₀)定义为

为k＝0时刻标号为j的高斯分量的当前运动模型子集，作为初始时刻，各标号高斯分量的当前运动模型子集取为总目标运动模型集M，从而对j＝1，2，..，J₀，模型标记向量都为全1向量[1，1，...，1]；

B-5.对每个高斯分量标号j(j＝1，2，..，J₀)，定义模型权重向量其中，为模型p在当前运动模型子集中的模型概率，如果模型p不在模型集合中，其概率为0，作为初始时刻，各标号高斯分量的当前运动模型子集取为总目标运动模型集M且当前运动模型子集中各模型概率取相等，从而对j＝1，2，..，J₀，模型权重向量都为

C.令k＝k+1，即为计算下一时刻；

D.假设k-1时刻，即k时刻的前一时刻，基于模型M_p定义的GMPHD滤波器的目标后验强度函数D_k-1，p(x)如下：

其中，j＝1，2，...，J_k-1，J_k-1为k-1时刻目标后验强度的高斯分量数，表示均值为协方差矩阵为的高斯分布，和分别为滤波器后验强度函数的第j个高斯分量的权重、均值和协方差矩阵；假设k-1时刻各高斯分量标号j＝1～J_k-1对应的模型标记向量和模型权重向量分别为和其中，定义为：

为k-1时刻标号为j的高斯分量的当前运动模型子集，为模型p在模型集合中的模型概率；k-1时刻N_r个滤波器的目标后验强度函数组成的集合记为k-1时刻的后验强度函数集合{D_k-1(x)}，其中，{D_k-1(x)}表示集合具体步骤如下：

D-1.新生目标强度确定：新生目标强度为高斯和形式：

其中，j＝1，2，...，J_γ，k，J_γ，k为新生目标强度的高斯分量数，表示均值为协方差矩阵为的高斯分布，和分别为各高斯分量的权重、均值和协方差矩阵，新生目标强度的具体参数事先已经给定；定义新生目标各高斯分量j＝1，2，..，J_γ，k的模型标记向量和模型权重向量分别为和其中，定义为：

为新生目标强度标号为j的高斯分量的当前运动模型子集，为模型p在模型集合中的模型概率；对新生目标，各标号高斯分量的当前运动模型子集取为总目标运动模型集M且当前运动模型子集中各模型概率取相等，从而对j＝1，2，..，J_r，k，模型标记向量都为全1向量[1，1，...，1]，模型权重向量都为

D-2.运动模型集确定：对j＝1，2，..，J_k-1，根据k-1时刻标号j的模型权重向量得到最大模型权重对应的运动模型取运动模型的状态转移矩阵对基于运动模型的滤波器的目标后验强度中第j个高斯分量进行一步预测，判定预测值在步骤A-1所述数字地图中的坐标，并根据步骤A-2确定的“区域-区域运动模型集”关系得到预测坐标所在区域的预设目标运动模型集，将其作为k时刻标号j对应的当前运动模型子集，记为根据计算模型标记向量具体为将的项置1，其余清0，其中，j＝1，2，...，J_k-1，p＝1，2，..，N_r； 对k时刻新生目标强度D_γ，k(x)的各高斯分量j＝1～J_γ，k，判定其均值在步骤A-1所述数字地图中的坐标，并根据A-2确定的“区域-区域运动模型集”关系得到均值坐标所在区域的预设目标运动模型集，将其作为标号为j的新生目标高斯分量在时刻k对应的当前运动模型子集，记为根据对运动模型标记向量进行更新，具体为将的项置1，其余清0，其中，j＝1，2，...，J_γ，k，p＝1，2，..，N_r；

D-3.模型混合概率初始化：根据步骤D-2的模型标记向量和k-1时刻的模型权重向量计算各高斯分量在k时刻模型为q(q＝1，2，..，N_r)的情况下k-1时刻模型为p(p＝1，2，..，N_r)的概率规则为：如果则

其中，为k时刻模型q的模型权重，计算公式为

如果则其中，j＝1，2，..，J_k-1，p＝1，2，..，N_r，q＝1，2，..，N_r；

以上步骤后，对标号j的权重进行归一化，令归一化结果构成模型权重向量其中，q＝1，2，..，N_r，j＝1，2，...，J_k-1；同理，对新生目标强度D_γ，k(x)的各高斯分量分别计算更新模型权重向量其中，j＝1，2，...，J_γ，k，具体操作类似，不再细述；

D-4.混合条件PHD初始化：计算k时刻滤波器的重初始化目标强度其中

其中，表示均值为协方差矩阵为的高斯分布，和分别为重初始化目标强度高斯分量j的权重、均值和协方差矩阵，计算规则为，如果模型标记向量中，则

如果则和取全零矩阵，其中，q＝1，2，..，N_r，j＝1，2，...，J_k-1；

D-5.条件PHD预测：计算k时刻滤波器的预测强度函数D_k|k-1，q(x)

D_k|k-1，q(x)＝D_{s，k|k-1，q}(x)+D_γ，k(x)

其中，存活目标强度D_{S，k|k-1，q}(x)为

其中，表示均值为协方差矩阵为的高斯分布，和分别为存活目标强度D_{S，k|k-1，q}(x)高斯分量j的权重、均值和协方差矩阵，计算规则为，如果模型标记向量中，则

如果则和取全零矩阵，其中，p_S，k为目标存活概率，是已知参数，F_q为滤波器对应模型M_q的状态转移矩阵，Q_q为模型M_q的过程噪声协方差矩阵，上述计算完毕后，将各模型预测强度改写为如下形式：

其中，J_k|k-1＝J_k-1+J_γ，k，D_k|k-1，q(x)中标号为1～J_k-1的高斯分量对应D_{S，k|k-1，q}(x)中的J_k-1个高斯分量，D_k|k-1，q(x)标号中标号为J_k-1+1～J_k|k-1的高斯分量对应D_γ，k(x)中标号为1～J_γ，k的J_r，k个高斯分量； 生成预测强度的模型标记向量其中，标号为1～J_k-1的来自步骤D-2的标号为J_k-1+1～J_k|k-1的来自D-2的生成预测强度的模型权重向量其中，标号为1～J_k-1的来自步骤D-3的标号为J_k-1+1～J_k|k-1的来自步骤D-3的

D-6.条件PHD更新：计算k时刻滤波器的后验强度函数D_k，q(x)

其中，为k时刻的量测值集合，Z_k量测个数为N_z，k，p_D，k为目标检测概率，是已知参数，且

其中，表示均值为协方差矩阵为的高斯分布，和分别为后验强度函数D_k，q(x)高斯分量j的权重、均值和协方差矩阵，计算规则为，如果模型标记向量中，则

如果则和取全零矩阵，以上步骤以后，将k时刻模型q的后验强度改写为以下形式：

其中，J_k＝J_k|k-1+N_z，k×J_k|k-1为k时刻滤波器后验强度函数D_k，q(x)的高斯分量数，改写后标号为1～J_k|k-1的高斯分量对应(1-p_D，k)D_k|k-1，q(x)的J_k|k-1个高斯分量，标号为(i×J_k|k-1+1)～((i+1)×J_k|k-1)的高斯分量对应D_D，k，q(x；z_i)的J_k|k-1个高斯分量，其中，i＝1，2，...，N_z，k；根据步骤D-5的预测强度模型标记向量生成后验强度的模型标记向量对应关系为其中，i＝0，1，2，...，N_z，k，l＝1，2，...，J_k|k-1；根据步骤D-5的预测强度模型权重向量生成后验强度的模型标记向量其中，

其中，i＝0，1，2，...，N_z，k，l＝1，2，...，J_k|k-1；

D-7.PHD混合估计：对k时刻全部滤波器的后验强度D_k，q(x)(q＝1，2，..，N_r)进行混合估计，得到k时刻的多模型混合估计D_k，M(x)：

其中，表示均值为协方差矩阵为的高斯分布，和分别为多模型混合估计D_k，M(x)高斯分量j的权重、均值和协方差矩阵，计算公式为：

E.剪枝与合并：设置修剪阈值r，删除k时刻多模型混合估计D_k，M(x)中权值低于该门限的高斯分量，得到剪枝后的目标强度D_k，M(x)，同时对N_r个滤波器的后验强度D_k，q(x)(q＝1，2，..，N_r)分别进行与混合估计D_k，M(x)相同的剪枝操作，对后验强度的模型权重向量和模型标记向量的相应项也进行删除；设置合并阈值U，计算D_k，M(x)各高斯分量距离，把距离小于阈值U的高斯分量进行合并，得到合并后的目标强度D_k，M(x)，同时对N_r个模型下的后验强度D_k，q(x)(q＝1，2，..，N_r)分别进行与D_k，M(x)相同的合并操作，对后验强度的模型权重向量和模型标记向量的相应项也进行合并，其中，模型权重向量的合并操作为将被合并的高斯分量对应的模型权重进行求和，得到合并后高斯分量的权重，模型标记向量的合并操作为将被合并的高斯分量对应的模型标记进行求并集，得到合并后高斯分量的模型标记；对修改后的模型权重向量进行归一化，使对剪枝合并后的各高斯分量标号j，

F.数目与状态估计；

G.终止的判断条件。