一种基于道路网的机动目标跟踪算法的制作方法

本发明属于目标跟踪领域，涉及一种基于道路网的机动目标跟踪算法，具体涉及一种利用道路信息辅助地面目标跟踪的变结构多模型概率假设密度算法。

背景技术：

地面目标跟踪在军事和民用领域有极为广阔的应用前景。地面目标具有运动能力强、运动状态多的特点，基于单模型的滤波器无法满足对其机动的跟踪，传统的方法是利用多模型滤波方法，例如交互式多模型-多假设跟踪算法和交互式多模型-联合概率数据关联算法对地面目标进行跟踪。然而，这种固定模型集算法的性能很大程度上取决于它使用的模型集，为了覆盖目标的全部运动，要求运动模型集中包含目标所有可能的运动模型，并且所有模型在整个跟踪过程中都有效，这一方面导致了计算量的增加，另一方面过多模型的竞争还可能导致目标估计精度的降低。考虑到地面目标的运动受到道路和地形条件的限制，基于道路地理信息辅助的地面目标跟踪算法被提出。该类方法根据道路信息调整目标运动模型，每一跟踪时刻在目标所有可能的运动模型中选择与当前目标真实运动状态匹配的模型子集进行跟踪滤波。这种变模型的结构解决了固定模型方法模型过多的问题，但是该方法依然是基于以数据关联为核心的经典多目标跟踪框架，该框架下，需要通过数据关联完成观测数据和目标间的分配，再通过滤波技术完成目标状态估计，随着杂波和目标数目的增加，会导致计算量随指数增长，并且对出生、死亡、衍生、合并等行为难以给出满意的结果。

针对经典多目标跟踪框架的问题，基于随机有限集的跟踪算法被提出，该理论很好的解决了数据关联的难题，将所有个体目标状态集合和每次观测所获得的观测值集合建模为随机有限集，自然并入目标起始和终止机制，实现目标数目及其状态的同时估计。概率假设密度滤波器作为随机有限集最优多目标贝叶斯滤波器的一种原理性近似滤波器，具有序列蒙特卡洛和高斯混合两种实现方式，高斯混合实现方式能够提供解析解，具有计算量小和实现简单的特点，是多目标跟踪领域的研究热点。然而，与其他滤波方法一样，基于随机有限集的多目标跟踪算法同样要对目标运动模式进行建模，建立在单模型基础上的高斯混合概率假设密度滤波器并不能有效识别目标机动。对此，一些基于多模型的方法被提出，例如使用多个模型并行滤波估计，使用最佳拟合高斯匹配目标动态模型等，但是这些方法依然是固定模型集方法，具有固定模型集方法的固有弊端，同时无法考虑和利用目标运动的外部条件，比如目标的周围环境信息等。

技术实现要素：

本发明所要解决的，就是针对上述目标跟踪和道路辅助存在的问题，提出一种基于道路网的机动目标跟踪算法。该算法基于高斯混合概率假设密度(gmphd)滤波框架，借助先验道路信息库，采用变结构的多模型方法实现地面多机动目标跟踪的目标跟踪算法。首先，给出一种先验道路信息库的构建方法，以道路网为基础将传感器威力区分成道路区域，道路交点区域，非道路区域等，根据各区域对目标运动方式的限制给定各区域相应的目标运动模型集合，构建道路信息库；其次，给出一种利用道路信息实现的跟踪算法模型自适应策略，实现目标在不同区域之间的运动模型集转换；最后，给出具体的滤波跟踪步骤。

本发明具体采用如下技术方案：

a.构建道路信息库：

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

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

a-3.形成道路地图查找表：根据雷达探测距离精度将整个地面空间按照位置进行矩形分块，以位置为索引建立查找表，查找表内保存各小矩形块的路段信息和对应的模型集信息；道路匹配时，根据目标位置直接匹配查找表进行信息提取；本步骤为可选步骤；

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

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

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

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

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

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

b-4.对高斯分量标号j(j＝1，2，..，j0)，定义模型标记向量其中，定义为

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

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

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

d.假设k-1时刻，即k时刻的前一时刻，基于模型mp定义的gmphd滤波器(p＝1，2，..，nr)的目标后验强度函数dk-1，p(x)如下：

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

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

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

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

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

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

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

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

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

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

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

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

d-5.条件phd预测：计算k时刻滤波器(q＝1，2，..，nr)的预测强度函数dk|k-1，q(x)

dk|k-1，q(x)＝ds，k|k-1，q(x)+dγ，k(x)

其中，存活目标强度ds，k|k-1，q(x)为

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

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

其中，jk|k-1＝jk-1+jγ，k，dk|k-1，q(x)中标号为1～jk-1的高斯分量对应ds，k|k-1，q(x)中的jk-1个高斯分量，dk|k-1，q(x)标号中标号为jk-1+1～jk|k-1的高斯分量对应dγ，k(x)中标号为1～jγ，k的jγ，k个高斯分量。生成预测强度的模型标记向量其中，标号为1～jk-1的来自步骤d-2的标号为jk-1+1～jk|k-1的来自d-2的生成预测强度的模型权重向量其中，标号为1～jk-1的来自步骤d-3的标号为jk-1+1～jk|k-1的来自步骤d-3的

d-6.条件phd更新：计算k时刻滤波器(q＝1，2，..，nr)的后验强度函数dk，q(x)

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

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

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

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

其中，i＝0，1，2，...，nz，k，l＝1，2，...，jk|k-1；

d-7.phd混合估计：对k时刻全部滤波器的后验强度dk，q(x)(q＝1，2，..，nr)进行混合估计，得到k时刻的多模型混合估计dk，m(x)：

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

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

f.数目与状态估计：将步骤e后的目标强度dk，m(x)进行权值累加，得到时刻k的目标数目nk，选出权值最高的nk个高斯分量，作为时刻k的多目标状态估计；

g.kend为终止时刻，如果k＝kend，退出，否则返回步骤c。

本发明的有益效果是：构建道路信息库，给出一种利用道路信息实现的跟踪算法模型自适应策略，使用结构可变的多模型高斯混合概率假设密度滤波器进行地面目标跟踪，一方面满足了目标多机动方式的要求，提高了机动目标跟踪的状态估计精度，降低了目标跟丢率，另一方面避免了采用过多模型带来的计算负担，大大减少了运行时间，另外，本专利的道路知识辅助框架具有可扩展性，支持在道路信息库中加入更多有用先验信息，比如道路的开放性、可见性等，适当修改后继算法的相应步骤，可以很方便的利用这些信息辅助跟踪。本发明在地面目标跟踪问题上有其实用价值。

附图说明

图1为本发明所述方法的流程图；

图2为实施例1的仿真场景图；

图3为实施例1的仿真结果图；

图4为实施例2的仿真场景图；

图5为实施例2的仿真结果图。

具体实施方式

实施例1

仿真场景：考虑一段道路区域有一个目标的场景，如图2所示。整个监视区域为[500m，4500m]×[0m，2000m]，整个区域每个观测时刻的平均杂波数为5。道路端点数为7，分别为点a(600，1043.7)，点b(1500，1043.7)，点c(1500，1800)，点d(1500，100)，点e(3218.8，1043.7)，点f(3655.5，1800)，点g(4400，1043.7)，道路路段数为6，分别为ab、cb、db、eb、fe、ge。目标个数为1，目标在1s出现，初始状态为[1500；500；0；10]，1～50s进行匀速直线运动，51～60s进行角速度为的匀速转弯运动，61～220s进行匀速直线运动，221～230s进行角速度为的匀速转弯运动，231～300s进行匀速直线运动。采样周期为2s，仿真步数为150。

系统目标运动模型为

每个目标状态xk＝[x，y，vx，vy]^t，包括目标的位置和速度，量测zk＝[zx，zy]^t为位置变量。对于模型r，为模型状态转移矩阵，wk为过程噪声，其噪声标准差为0.1m，vk为量测噪声，其噪声标准差为10m。仿真测试阶段采用7个运动模型，包括1个cv模型和6个ct模型，在cv模型中

在ct模型中

其中，t为量测数据时间间隔，仿真中为2，w表示ct模型中的转弯角速度。仿真中取运动模型f1为f2～f7分别为w＝0.1，-0.1，0.2，-0.2，0.3，-0.3rad/s的模型转移概率矩阵为

目标检测概率为0.95，存活概率为0.99，新生目标的强度函数为其中对于道路辅助设定，假设目标在道路路段内的运动模型为f1，目标在道路交叉点b的运动模型集为{f1，f4，f5}，在交叉点e的运动模型集为{f1，f2，f3，f6，f7}。

将本发明提供的基于道路辅助的变结构算法与固定结构算法进行比较，通过100次蒙特卡洛仿真实验，用表征跟踪误差的优化子模式分配(ospa)距离进行性能比较，如图3。可以看出本发明算法的跟踪误差比使用固定模型的算法imm-gmphd要小，本发明算法在目标行驶在道路路段内的时段的跟踪误差很小，只有在目标运行到道路交叉点时误差较大。通过100次蒙特卡洛仿真实验，本发明运行时间为64s，imm-gmphd算法运行时间为147s，本发明运行时间约为固定模型集方法imm-gmphd的40％。

实施例2

仿真场景：考虑一段道路区域有三个目标的场景。采用与实施例1相同的仿真场景，目标个数变为3。目标1在1s出现，初始状态为[1500；500；0；10]，1～50s进行匀速直线运动，51～60s进行角速度为的匀速转弯运动，61～220s进行匀速直线运动，221～230s进行角速度为的匀速转弯运动，231～300s进行匀速直线运动。目标2在41s出现，初始状态为[1500；1507.3；0；-10]，41～82s进行匀速直线运动，83～92s进行角速度为的匀速转弯运动，93～174s进行匀速直线运动。目标3在1s出现，初始状态为[3541.9；1603.3；-5；-8.7]，1～60s进行匀速直线运动，61～68s进行角速度为的匀速转弯运动，69～140s进行匀速直线运动。采样周期为2s，仿真步数为150。仿真场景见图4。新生目标的强度函数设为其中，其余仿真参数设置与实施例1相同。通过100次蒙特卡洛仿真实验，性能比较如图5，可以看出本发明算法能够实现对多机动目标的稳定跟踪，并且跟踪误差比使用固定模型的算法imm-gmphd要小，仅在有新目标出现的仿真时刻1和仿真时刻21有较大误差。在此实施例，100次蒙特卡洛仿真实验，本发明运行时间同样约为固定模型集方法imm-gmphd的40％。