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

摘要

本发明提供了一种基于道路网的机动目标跟踪算法，借助先验道路信息库，给出一种利用道路信息实现的跟踪算法模型自适应策略，采用变结构的多模型方法实现地面多机动目标跟踪，可以提高机动目标跟踪的状态估计精度，降低目标跟丢率，同时避免固定多模型算法采用过多模型带来的计算负担，大大减少运行时间，本方案在地面目标跟踪问题上有其实用价值。

主权项

一种基于道路网的机动目标跟踪算法，其特征在于，包括以下步骤：A.构建道路信息库；B.基于道路信息库的地面目标跟踪：B‑1.根据道路信息库确定总目标运动模型集其中，Nr为总模型集的模型个数，每一个运动模型Mp包括该模型的状态转移矩阵Fp和过程噪声协方差矩阵Qp的信息，确定各模型间的模型转移概率矩阵[πpq]，其中p＝1，2，..，Nr，q＝1，2，..，Nr；B‑2.初始化，令观测时刻k＝0，即为初始时刻；定义初始时刻的目标后验强度D0(x)为：D0(x)=Σj=1J0w0(j)N(x|m0(j),P0(j))]]>其中，j＝1，2，...，J0，J0为初始时刻的目标后验强度的高斯分量数，表示均值为协方差矩阵为的高斯分布，和分别为高斯分量j的权重、均值和协方差矩阵，为初始时刻目标个数期望值；B‑3.对总目标运动模型集中的每一个运动模型分别定义一个GMPHD滤波器，将每个GMPHD滤波器初始时刻的目标后验强度函数D0，p(x)都取为D0(x)，即：D0,p(x)=D0(x)=Σj=1J0w0(j)N(x|m0(j),P0(j))]]>其中，D0，p(x)为基于模型Mp(p＝1，2，..，Nr)定义的GMPHD滤波器的初始目标后验强度函数；B‑4.对高斯分量标号j(j＝1，2，..，J0)，定义模型标记向量其中，(p＝1，2，..，Nr，j＝1，2，..，J0)定义为I0,p(j)=1p∈M0(j)0p∉M0(j)]]>为k＝0时刻标号为j的高斯分量的当前运动模型子集，作为初始时刻，各标号高斯分量的当前运动模型子集(j＝1，2，..，J0)取为总目标运动模型集M，从而对j＝1，2，..，J0，模型标记向量都为全1向量[1，1，...，1]；B‑5.对每个高斯分量标号j(j＝1，2，..，J0)，定义模型权重向量其中，(p＝1，2，..，Nr，j＝1，2，..，J0)为模型p在当前运动模型子集中的模型概率(如果模型p不在模型集合中，其概率为0)，作为初始时刻，各标号高斯分量的当前运动模型子集(j＝1，2，..，J0)取为总目标运动模型集M且当前运动模型子集中各模型概率取相等，从而对j＝1，2，..，J0，模型权重向量都为C.令k＝k+1，即为计算下一时刻；D.假设k‑1时刻，即k时刻的前一时刻，基于模型Mp定义的GMPHD滤波器(p＝1，2，..，Nr)的目标后验强度函数Dk‑1，p(x)如下：Dk-1,p(x)=Σj=1Jk-1wk-1,p(j)N(x|mk-1,p(j),Pk-1,p(j))]]>其中，j＝1，2，...，Jk‑1，Jk‑1为k‑1时刻目标后验强度的高斯分量数，表示均值为协方差矩阵为的高斯分布，和分别为滤波器后验强度函数的第j个高斯分量的权重、均值和协方差矩阵；假设k‑1时刻各高斯分量标号j＝1～Jk‑1对应的模型标记向量和模型权重向量分别为和其中，(p＝1，2，..，Nr，j＝1，2，..，Jk‑1)定义为：Ik-1,p(j)=1p∈Mk-1(j)0p∉Mk-1(j)]]>为k‑1时刻标号为j的高斯分量的当前运动模型子集，(p＝1，2，..，Nr，j＝1，2，..，Jk‑1)为模型p在模型集合中的模型概率；k‑1时刻Nr个滤波器的目标后验强度函数组成的集合记为k‑1时刻的后验强度函数集合{Dk‑1(x)}，其中，{Dk‑1(x)}表示集合具体步骤如下：D‑1.新生目标强度确定：新生目标强度为高斯和形式：Dγ,k(x)=Σj=1Jγ,kwγ,k(j)N(x|mγ,k(j),Pγ,k(j))]]>其中，j＝1，2，...，Jγ，k，Jγ，k为新生目标强度的高斯分量数，表示均值为协方差矩阵为的高斯分布，和分别为各高斯分量的权重、均值和协方差矩阵，新生目标强度的具体参数一般事先已经给定；定义新生目标各高斯分量j＝1，2，..，Jγ，k的模型标记向量和模型权重向量分别为和其中，(p＝1，2，..，Nr，j＝1，2，..，Jγ，k)定义为：Iγ,k,p(j)=1p∈Mγ,k(j)0p∉Mγ,k(j)]]>为新生目标强度标号为j的高斯分量的当前运动模型子集，(p＝1，2，..，Nr，j＝1，2，..，Jγ，k)为模型p在模型集合中的模型概率；对新生目标，各标号高斯分量的当前运动模型子集(j＝1，2，..，Jγ，k)取为总目标运动模型集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的模型标记向量(j＝1，2，...，Jk‑1)和k‑1时刻的模型权重向量(j＝1，2，...，Jk‑1)，计算各高斯分量在k时刻模型为q(q＝1，2，..，Nr)的情况下k‑1时刻模型为p(p＝1，2，..，Nr)的概率规则为：如果则μk-1,k,pq(j)=1μk-1,k,q(j)πpq(j)μk-1,p(j)]]>其中，为k时刻模型q的模型权重，计算公式为μk-1,k,q(j)=Σp=1Nrπpq(j)μk-1,p(j)]]>如果则其中，j＝1，2，..，Jk‑1，p＝1，2，..，Nr，q＝1，2，..，Nr。以上步骤后，对标号j的权重(q＝1，2，..，Nr)进行归一化，令归一化结果构成模型权重向量其中，q＝1，2，..，Nr，j＝1，2，...，Jk‑1。同理，对新生目标强度Dγ，k(x)的各高斯分量分别计算更新模型权重向量其中，j＝1，2，...，Jγ，k，具体操作类似，不再细述；D‑4.混合条件PHD初始化：计算k时刻滤波器(q＝1，2，..，Nr)的重初始化目标强度其中D~k-1,q(x)=Σj=1Jk-1w~k-1,q(j)N(x|m~k-1,q(j),P~k-1,q(j))]]>其中，表示均值为协方差矩阵为的高斯分布，和分别为重初始化目标强度高斯分量j的权重、均值和协方差矩阵，计算规则为，如果模型标记向量中，则w~k-1,q(j)=Σp=1Nrwk-1,p(j)μk-1,pq(j)]]>m~k-1,q(j)=Σp=1Nrmk-1,p(j)μk-1,pq(j)]]>P~k-1,q(j)=Σp=1Nr[Pk-1,p(j)+(mk-1,p(j)-m~k-1,q(j))(mk-1,p(j)-m~k-1,q(j))T]μk-1,pq(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=1Jk-1wS,k|k-1,q(j)N(x|mS,k|k-1,q(j),PS,k|k-1,q(j))]]>其中，表示均值为协方差矩阵为的高斯分布，和分别为存活目标强度DS，k|k‑1，q(x)高斯分量j的权重、均值和协方差矩阵，计算规则为，如果模型标记向量中，则wS,k|k-1,q(j)=pS,kw~k-1,q(j)]]>mS,k|k-1,q(j)=Fqm~k-1,q(j)]]>PS,k|k-1,q(j)=FqP~k-1,q(j)(Fq)T+Qq]]>如果则和取全零矩阵，其中，pS，k为目标存活概率，是已知参数，Fq为滤波器对应模型Mq的状态转移矩阵，Qq为模型Mq的过程噪声协方差矩阵，上述计算完毕后，将各模型预测强度改写为如下形式：Dk|k-1,q(x)=Σj=1Jk|k-1wk|k-1,q(j)N(x|mk|k-1,q(j),Pk|k-1,q(j))]]>其中，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个高斯分量。生成预测强度的模型标记向量(j＝1～Jk|k‑1)，其中，标号为1～Jk‑1的来自步骤D‑2的标号为Jk‑1+1～Jk|k‑1的来自D‑2的生成预测强度的模型权重向量(j＝1，2，...，Jk|k‑1)，其中，标号为1～Jk‑1的来自步骤D‑3的标号为Jk‑1+1～Jk|k‑1的来自步骤D‑3的D‑6.条件PHD更新：计算k时刻滤波器(q＝1，2，..，Nr)的后验强度函数Dk，q(x)Dk,q(x)=(1-pD,k)Dk|k-1,q(x)+Σi=1Nz,xDD,k,q(x;zi)]]>其中，为k时刻的量测值集合，Zk量测个数为Nz，k，pD，k为目标检测概率，是已知参数，且DD,k,q(x;zi)=Σj=1Jk|k-1wD,k,q(j)(zi)N(x|mD,k,q(j)(zi),PD,k,q(j)(zi))]]>其中，表示均值为协方差矩阵为的高斯分布，和分别为后验强度函数Dk，q(x)高斯分量j的权重、均值和协方差矩阵，计算规则为，如果模型标记向量中，则wD,k,q(j)(zi)=pD,kwk|k-1,q(j)g(zi|mk|k-1,q(j))κk(zi)+pD,kΣl=1Jk|k-1wk|k-1,q(l)g(zi|mk|k-1,q(l))]]>mD,k,q(j)(zi)=mk|k-1,q(j)+Kk,q(j)(zi-Hqmk|k-1,q(j))]]>PD,k,q(j)(zi)=(I-Kk,q(j)Hq)Pk|k-1,q(j)]]>Kk,q(j)=Pk|k-1,q(j)(Hq)T(Sk,q(j))-1]]>Sk,q(j)=HqPk|k-1,q(j)(Hq)T+Rq]]>g(z|mk|k-1,q(j))=N(z|Hqmk|k-1,q(j),HqPk|k-1,q(j)(Hq)T+Rq)]]>如果则和取全零矩阵，以上步骤以后，将k时刻模型q的后验强度改写为以下形式：Dk,q(x)=Σj=1Jkwk,q(j)N(x|mk,q(j),Pk,q(j))]]>其中，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的预测强度模型标记向量(j＝1～Jk|k‑1)生成后验强度的模型标记向量(j＝1～Jk)，对应关系为其中，i＝0，1，2，...，Nz，k，l＝1，2，...，Jk|k‑1；根据步骤D‑5的预测强度模型权重向量(j＝1，2，...，Jk|k‑1)生成后验强度的模型标记向量(j＝1～Jk)，其中，μk,q(i×Jk|k-1+l)=μk|k-1,q(l)g(zi|mk|k-1,q(l))ck(l)]]>ck(l)=Σq=1Nrμk|k-1,q(l)g(zi|mk|k-1,q(l))]]>其中，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=1Jkwk,M(j)N(x|mk,M(j),Pk,M(j))]]>其中，表示均值为协方差矩阵为的高斯分布，和分别为多模型混合估计Dk，M(x)高斯分量j的权重、均值和协方差矩阵，计算公式为：wk,M(j)=Σq=1Nrwk,q(j)μk,q(j)]]>mk,M(j)=Σq=1Nrmk,q(j)μk,q(j)]]>Pk,M(j)=Σq=1Nr[Pk,q(j)+(mk,M(j)-mk,q(j))(mk,M(j)-mk,q(j))T]μk,q(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＝1～Jk)和模型标记向量(j＝1～Jk)的相应项也进行合并，其中，模型权重向量的合并操作为将被合并的高斯分量对应的模型权重进行求和，得到合并后高斯分量的权重，模型标记向量的合并操作为将被合并的高斯分量对应的模型标记进行求并集，得到合并后高斯分量的模型标记；对修改后的模型权重向量进行归一化，使对剪枝合并后的各高斯分量标号j，F.数目与状态估计；G.终止的判断条件。