CN106952290A - 一种用于三维空间跟踪转弯机动目标的方法及系统 - Google Patents
一种用于三维空间跟踪转弯机动目标的方法及系统 Download PDFInfo
- Publication number
- CN106952290A CN106952290A CN201710224049.9A CN201710224049A CN106952290A CN 106952290 A CN106952290 A CN 106952290A CN 201710224049 A CN201710224049 A CN 201710224049A CN 106952290 A CN106952290 A CN 106952290A
- Authority
- CN
- China
- Prior art keywords
- target
- edge distribution
- current moment
- turning rate
- steering angle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 42
- 238000009826 distribution Methods 0.000 claims description 191
- 238000005259 measurement Methods 0.000 claims description 114
- 239000011159 matrix material Substances 0.000 claims description 60
- 238000012545 processing Methods 0.000 claims description 22
- 230000007704 transition Effects 0.000 claims description 14
- 238000006243 chemical reaction Methods 0.000 claims description 12
- 238000001914 filtration Methods 0.000 claims description 12
- 230000008569 process Effects 0.000 claims description 11
- 230000004083 survival effect Effects 0.000 claims description 8
- 238000001514 detection method Methods 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 6
- 238000000605 extraction Methods 0.000 claims description 4
- 238000010586 diagram Methods 0.000 description 8
- 239000000203 mixture Substances 0.000 description 4
- 238000004088 simulation Methods 0.000 description 3
- 238000002474 experimental method Methods 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 238000000342 Monte Carlo simulation Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000007619 statistical method Methods 0.000 description 1
- 230000017105 transposition Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/20—Special algorithmic details
- G06T2207/20076—Probabilistic image processing
Landscapes
- Radar Systems Or Details Thereof (AREA)
Abstract
本发明提供了一种用于三维空间跟踪转弯机动目标的方法及系统,旨在解决现有技术中难以对三维空间中转弯率变化的机动目标进行有效跟踪的问题。本发明所提供的方法或系统通过预测、转弯率和转向角估计、更新、新目标生成及目标状态提取这些模块,将转弯率和转向角估计与序贯贝叶斯滤波器相结合,确定出各目标在当前时刻的状态估计与误差估计,从而实现了对三维空间中转弯率变化的机动目标进行有效跟踪。
Description
技术领域
本发明属于多传感器信息融合技术领域,尤其涉及一种用于三维空间跟踪转弯机动目标的方法及系统。
背景技术
贝叶斯滤波技术主要用于协助解决测量数据具有不确定性情况下的多传感器信息的融合与处理,它是一种强大的统计方法工具。然而,目前的多目标贝叶斯滤波方法难以对三维空间中转弯率变化的机动目标进行有效跟踪。
发明内容
本发明提供了一种用于三维空间跟踪转弯机动目标的方法及系统,旨在解决现有技术中难以对三维空间中转弯率变化的机动目标进行有效跟踪的问题。
为解决上述技术问题,本发明是这样实现的,本发明提供了一种用于三维空间跟踪转弯机动目标的方法,所述方法包括:
步骤1、根据前一时刻各目标的边缘分布、存在概率、转弯率和转向角,以及当前时刻与前一时刻的时间差,得到当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角;
所述步骤1具体包括:
以k-1表示前一时刻,k表示当前时刻,tk-1表示前一时刻的时间,tk表示当前时刻的时间,以N(x;mi,k-1,Pi,k-1)、ρi,k-1、ωi,k-1和θi,k-1分别表示k-1时刻目标i的边缘分布、存在概率、转弯率和转向角;其中,i为索引号,且i=1,2,…Nk-1,N表示高斯分布,x表示目标的状态向量,mi,k-1和Pi,k-1分别表示k-1时刻目标i的状态均值和协方差,Nk-1表示k-1时刻目标的总数;
由k-1时刻目标i的边缘分布N(x;mi,k-1,Pi,k-1)、存在概率ρi,k-1、转弯率ωi,k-1和转向角θi,k-1,预测得到当前时刻目标i的预测边缘分布和预测存在概率分别为N(x;mi,k|k-1,Pi,k|k-1)和ρi,k|k-1;其中, 和分别表示目标i位置的x分量、y分量和z分量,和分别表示目标i速度的x分量、y分量和z分量;其中,mi,k|k-1=Fi,k|k-1mi,k-1,Pi,k|k-1=Qi,k-1+Fi,k|k-1Pi,k-1(Fi,k|k-1)T,ρi,k|k-1=pS,k(tk-tk-1)ρi,k-1,Fi,k|k-1表示状态转移矩阵,且Fi,k|k-1=A+B·C·D·B-1·(I-A);其中, I表示单位矩阵,Δtk=tk-tk-1表示k时刻与k-1时刻的时间差,方位角高低角Qi,k-1表示k-1时刻目标i的过程噪声协方差矩阵,pS,k(tk-tk-1)表示目标的幸存概率,且T表示采样周期,δ表示预设的常数;
k时刻目标i的预测转弯率和预测转向角分别为ωi,k|k-1=ωi,k-1和θi,k|k-1=θi,k-1;
步骤2、根据前一时刻各目标的边缘分布和存在概率,当前时刻与前一时刻的时间差,以及当前时刻的测量集合,估计当前时刻各目标对应于各测量的转弯率与转向角;
步骤3、根据估计得到的当前时刻各目标对应于各测量的转弯率与转向角,前一时刻各目标的边缘分布,当前时刻各目标的预测存在概率,当前时刻与前一时刻的时间差,以及当前时刻的测量集合,得到当前时刻各目标对应于各测量的边缘分布和存在概率;
根据所述当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角,所述当前时刻各目标对应于各测量的边缘分布、存在概率、转弯率和转向角得到当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角;
步骤4、利用当前时刻的各测量生成新生目标的边缘分布,为所述新生目标指定存在概率、转弯率和转向角;同时,将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角,分别与所述的当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角进行合并,生成合并后的当前时刻各目标的边缘分布、存在概率、转弯率和转向角;
所述步骤4具体包括:
利用当前时刻的M个测量生成当前时刻新生目标的边缘分布为当前时刻各新生目标指定存在概率为转弯率为和转向角为其中,表示第j个新生目标的边缘分布的协方差,为第j个新生目标的边缘分布的均值,由当前时刻的第j个测量生成,并且ργ表示为当前时刻各新生目标所指定的存在概率,和分别表示测量yj,k的x分量、y分量和z分量,上标T表示矩阵或向量的转置;
将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角,分别与所述的当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角进行合并,生成合并后的当前时刻各目标的边缘分布为存在概率为转弯率为和转向角为其中Nk=Nk-1+M;
步骤5、从所述合并后的当前时刻各目标中,将存在概率小于第一阈值的目标裁减掉,并且将裁减后余下目标的边缘分布、存在概率、转弯率和转向角作为下一时刻递归滤波的输入;同时,从裁减后余下目标的边缘分布中提取存在概率大于第二阈值的边缘分布作为当前时刻的输出,并且将各个输出边缘分布的均值与方差分别作为当前时刻目标的状态估计与误差估计。
进一步地,所述步骤2具体包括:
步骤A、设测量其中,和分别表示测量yj,k的x分量、y分量和z分量,上标T表示矩阵或向量的转置;利用mi,k-1和yj,k通过转换得到向量[xi,j yi,jzi,j]T,转换公式为其中,方向角高低角
步骤B、利用转换后的向量[xi,j yi,j zi,j]T得到转弯率ωi,j和转向角θi,j,其中,
步骤C、根据所述转弯率ωi,j、最大转弯率ωmax和最小转弯率ωmin得到目标i对应于测量yj,k的转弯率其中,ωmax和ωmin是两个已知的参数;由所述转向角θi,j,得到目标i对应于测量yj,k的转向角
在完成对M个测量的处理后,得到各目标对应于各测量的转弯率为和转向角为其中,i=1,…,Nk-1,j=1,…,M。
进一步地,所述步骤3具体包括:
步骤D、由k-1时刻目标i的边缘分布N(x;mi,k-1,Pi,k-1),以及所述的转弯率和转向角得到k时刻目标i对应于测量yj,k的预测边缘分布为其中,i=1,…,Nk-1,j=1,…,M,表示状态向量的均值,且 表示状态向量的方差,且其中,为状态转移矩阵,且其中, I表示单位矩阵,Δtk=tk-tk-1为k时刻与k-1时刻的时间差,方向角高低角
步骤E、利用贝叶斯规则对测量yj,k进行处理,得到目标i对应于测量yj,k的存在概率滤波增益均值向量协方差矩阵其中,Hk表示观测矩阵,Rk表示观测噪声方差矩阵,pD,k表示目标的检测概率,λc,k表示杂波密度,I表示单位矩阵;
在完成对M个测量的处理后,得到各目标对应于各测量的边缘分布为和存在概率为其中,i=1,…,Nk-1,j=1,…,M;
步骤F、取其中则k时刻已存在目标i的边缘分布取为存在概率为转弯率为和转向角为其中i=1,…,Nk-1,当q=M+1时,
处理完后,得到各已存在目标的边缘分布为N(x;mi,k,Pi,k)、存在概率为ρi,k、转弯率为ωi,k和转向角为θi,k,其中,i=1,…,Nk-1,j=1,…,M。
本发明还提供了一种用于跟踪三维空间转弯机动目标的系统,所述系统包括:
预测模块,用于根据前一时刻各目标的边缘分布、存在概率、转弯率和转向角,以及当前时刻与前一时刻的时间差,得到当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角;
所述预测模块具体用于:
以k-1表示前一时刻,k表示当前时刻,tk-1表示前一时刻的时间,tk表示当前时刻的时间,以N(x;mi,k-1,Pi,k-1)、ρi,k-1、ωi,k-1和θi,k-1分别表示k-1时刻目标i的边缘分布、存在概率、转弯率和转向角;其中,i为索引号,且i=1,2,…Nk-1,N表示高斯分布,x表示目标的状态向量,mi,k-1和Pi,k-1分别表示k-1时刻目标i的状态均值和协方差,Nk-1表示k-1时刻目标的总数;
由k-1时刻目标i的边缘分布N(x;mi,k-1,Pi,k-1)、存在概率ρi,k-1、转弯率ωi,k-1和转向角θi,k-1,预测得到当前时刻目标i的预测边缘分布和预测存在概率分别为N(x;mi,k|k-1,Pi,k|k-1)和ρi,k|k-1;其中, 和分别表示目标i位置的x分量、y分量和z分量,和分别表示目标i速度的x分量、y分量和z分量;其中,mi,k|k-1=Fi,k|k-1mi,k-1,Pi,k|k-1=Qi,k-1+Fi,k|k-1Pi,k-1(Fi,k|k-1)T,ρi,k|k-1=pS,k(tk-tk-1)ρi,k-1,Fi,k|k-1表示状态转移矩阵,且Fi,k|k-1=A+B·C·D·B-1·(I-A);其中, I表示单位矩阵,Δtk=tk-tk-1表示k时刻与k-1时刻的时间差,方位角高低角Qi,k-1表示k-1时刻目标i的过程噪声协方差矩阵,pS,k(tk-tk-1)表示目标的幸存概率,且T表示采样周期,δ表示预设的常数;
k时刻目标i的预测转弯率和预测转向角分别为ωi,k|k-1=ωi,k-1和θi,k|k-1=θi,k-1;
转弯率与转向角估计模块,用于根据前一时刻各目标的边缘分布和存在概率,当前时刻与前一时刻的时间差,以及当前时刻的测量集合,估计当前时刻各目标对应于各测量的转弯率与转向角;
更新模块,用于根据估计得到的当前时刻各目标对应于各测量的转弯率与转向角,前一时刻各目标的边缘分布,当前时刻各目标的预测存在概率,当前时刻与前一时刻的时间差,以及当前时刻的测量集合,得到当前时刻各目标对应于各测量的边缘分布和存在概率;
根据所述当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角,所述当前时刻各目标对应于各测量的边缘分布、存在概率、转弯率和转向角得到当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角;
新目标生成模块,用于利用当前时刻的各测量生成新生目标的边缘分布,为所述新生目标指定存在概率、转弯率和转向角;同时,将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角,分别与所述的当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角进行合并,生成合并后的当前时刻各目标的边缘分布、存在概率、转弯率和转向角;
所述新目标生成模块具体用于:
利用当前时刻的M个测量生成当前时刻新生目标的边缘分布为当前时刻各新生目标指定存在概率为转弯率为和转向角为其中,表示第j个新生目标的边缘分布的协方差,为第j个新生目标的边缘分布的均值,由当前时刻的第j个测量生成,并且ργ表示为当前时刻各新生目标所指定的存在概率,和分别表示测量yj,k的x分量、y分量和z分量,上标T表示矩阵或向量的转置;
将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角,分别与所述的当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角进行合并,生成合并后的当前时刻各目标的边缘分布为存在概率为转弯率为和转向角为其中Nk=Nk-1+M;
目标状态提取模块,用于从所述合并后的当前时刻各目标中,将存在概率小于第一阈值的目标裁减掉,并且将裁减后余下目标的边缘分布、存在概率、转弯率和转向角作为下一时刻递归滤波的输入;同时,从裁减后余下目标的边缘分布中提取存在概率大于第二阈值的边缘分布作为当前时刻的输出,并且将各个输出边缘分布的均值与方差分别作为当前时刻目标的状态估计与误差估计。
进一步地,所述转弯率与转向角估计模块具体用于:
设测量其中,和分别表示测量yj,k的x分量、y分量和z分量,上标T表示矩阵或向量的转置;利用mi,k-1和yj,k通过转换得到向量[xi,j yi,j zi,j]T,转换公式为其中,方向角高低角
利用转换后的向量[xi,j yi,j zi,j]T得到转弯率ωi,j和转向角θi,j,其中,
根据所述转弯率ωi,j、最大转弯率ωmax和最小转弯率ωmin得到目标i对应于测量yj,k的转弯率其中,ωmax和ωmin是两个已知的参数;由所述转向角θi,j,得到目标i对应于测量yj,k的转向角
在完成对M个测量的处理后,得到各目标对应于各测量的转弯率为和转向角为其中,i=1,…,Nk-1,j=1,…,M。
进一步地,所述更新模块具体用于:
由k-1时刻目标i的边缘分布N(x;mi,k-1,Pi,k-1),以及所述的转弯率和转向角得到k时刻目标i对应于测量yj,k的预测边缘分布为其中,i=1,…,Nk-1,j=1,…,M,表示状态向量的均值,且 表示状态向量的方差,且其中,为状态转移矩阵,且其中, I表示单位矩阵,Δtk=tk-tk-1为k时刻与k-1时刻的时间差,方向角高低角
利用贝叶斯规则对测量yj,k进行处理,得到目标i对应于测量yj,k的存在概率滤波增益均值向量协方差矩阵其中,Hk表示观测矩阵,Rk表示观测噪声方差矩阵,pD,k表示目标的检测概率,λc,k表示杂波密度,I表示单位矩阵;
在完成对M个测量的处理后,得到各目标对应于各测量的边缘分布为和存在概率为其中,i=1,…,Nk-1,j=1,…,M;
取其中则k时刻已存在目标i的边缘分布取为存在概率为转弯率为和转向角为其中i=1,…,Nk-1,当q=M+1时,
处理完后,得到各已存在目标的边缘分布为N(x;mi,k,Pi,k)、存在概率为ρi,k、转弯率为ωi,k和转向角为θi,k,其中,i=1,…,Nk-1,j=1,…,M。
本发明与现有技术相比,有益效果在于:
本发明提供的用于三维空间跟踪转弯机动目标的方法或系统,通过前一时刻各目标的边缘分布、存在概率、转弯率和转向角,以及当前时刻与前一时刻的时间差对当前时刻各目标的参数进行预测,得到当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角;通过前一时刻各目标的相关参数等和当前时刻的测量对当前时刻各目标的参数进行估计,得到当前时刻各目标对应于各测量的边缘分布、存在概率、转弯率和转向角;根据当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角以及估计得到的当前时刻各目标对应于各测量的边缘分布、存在概率、转弯率和转向角等参数,得到当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角;然后利用当前时刻的各测量生成新生目标的参数,将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角与当前时刻各已存在目标的更新边缘分布、存在概率、转弯率和转向角分别进行合并,从而得到当前时刻各目标的边缘分布、存在概率、转弯率和转向角;最后通过对当前时刻各目标的边缘分布、存在概率、转弯率和转向角的处理,确定出各个目标在当前时刻的状态估计与误差估计,从而实现了对三维空间中转弯率变化的机动目标进行有效跟踪。
附图说明
图1是本发明实施例提供的一种用于跟踪三维空间转弯机动目标的方法流程图;
图2是本发明实施例提供的一种用于跟踪三维空间转弯机动目标的系统示意图;
图3是本发明实施例提供的传感器在70个扫描周期的测量数据示意图;
图4是本发明实施例提供的根据跳变马尔科夫系统模型高斯混合概率假设密度滤波方法处理得到的结果示意图;
图5是本发明实施例提供的利用本发明提供的一种用于三维空间跟踪转弯机动目标方法处理得到的结果示意图;
图6是本发明实施例提供的利用本发明一种用于三维空间跟踪转弯机动目标方法以及跳变马尔科夫系统模型高斯混合概率假设密度滤波方法在经过100次实验得到的平均OSPA距离示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白,以下结合附图及实施例,对本发明进行进一步详细说明。应当理解,此处所描述的具体实施例仅仅用以解释本发明,并不用于限定本发明。
作为本发明的第一个实施例,如图1所示,本发明提供的一种本发明提供了一种用于三维空间跟踪转弯机动目标的方法,该方法包括以下步骤:
步骤S101:根据前一时刻各目标的边缘分布、存在概率、转弯率和转向角,以及当前时刻与前一时刻的时间差,得到当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角。
所述步骤S101具体包括:
以k-1表示前一时刻,k表示当前时刻,tk-1表示前一时刻的时间,tk表示当前时刻的时间,以N(x;mi,k-1,Pi,k-1)、ρi,k-1、ωi,k-1和θi,k-1分别表示k-1时刻目标i的边缘分布、存在概率、转弯率和转向角;其中,i为索引号,且i=1,2,…Nk-1,N表示高斯分布,x表示目标的状态向量,mi,k-1和Pi,k-1分别表示k-1时刻目标i的状态均值和协方差,Nk-1表示k-1时刻目标的总数;
由k-1时刻目标i的边缘分布N(x;mi,k-1,Pi,k-1)、存在概率ρi,k-1、转弯率ωi,k-1和转向角θi,k-1,预测得到当前时刻目标i的预测边缘分布和预测存在概率分别为N(x;mi,k|k-1,Pi,k|k-1)和ρi,k|k-1;其中, 和分别表示目标i位置的x分量、y分量和z分量,和分别表示目标i速度的x分量、y分量和z分量;其中,mi,k|k-1=Fi,k|k-1mi,k-1,Pi,k|k-1=Qi,k-1+Fi,k|k-1Pi,k-1(Fi,k|k-1)T,ρi,k|k-1=pS,k(tk-tk-1)ρi,k-1,Fi,k|k-1表示状态转移矩阵,且Fi,k|k-1=A+B·C·D·B-1·(I-A);其中, I表示单位矩阵,Δtk=tk-tk-1表示k时刻与k-1时刻的时间差,方位角高低角Qi,k-1表示k-1时刻目标i的过程噪声协方差矩阵,pS,k(tk-tk-1)表示目标的幸存概率,且T表示采样周期,δ表示预设的常数;
k时刻目标i的预测转弯率和预测转向角分别为ωi,k|k-1=ωi,k-1和θi,k|k-1=θi,k-1。
步骤S102:根据前一时刻各目标的边缘分布和存在概率,当前时刻与前一时刻的时间差,以及当前时刻的测量集合,估计当前时刻各目标对应于各测量的转弯率与转向角。
步骤S103:根据估计得到的当前时刻各目标对应于各测量的转弯率与转向角,前一时刻各目标的边缘分布,当前时刻各目标的预测存在概率,当前时刻与前一时刻的时间差,以及当前时刻的测量集合,得到当前时刻各目标对应于各测量的边缘分布和存在概率;根据所述当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角,所述当前时刻各目标对应于各测量的边缘分布、存在概率、转弯率和转向角得到当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角。
步骤S104:利用当前时刻的各测量生成新生目标的边缘分布,为所述新生目标指定存在概率、转弯率和转向角;同时,将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角,分别与所述的当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角进行合并,生成合并后的当前时刻各目标的边缘分布、存在概率、转弯率和转向角。
步骤S104具体包括:
利用当前时刻的M个测量生成当前时刻新生目标的边缘分布为当前时刻各新生目标指定存在概率为转弯率为和转向角为其中,表示第j个新生目标的边缘分布的协方差,为第j个新生目标的边缘分布的均值,由当前时刻的第j个测量生成,并且ργ表示为当前时刻各新生目标所指定的存在概率,和分别表示测量yj,k的x分量、y分量和z分量,上标T表示矩阵或向量的转置;
将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角,分别与所述的当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角进行合并,生成合并后的当前时刻各目标的边缘分布为存在概率为转弯率为和转向角为其中Nk=Nk-1+M,具体合并过程如下:
将当前时刻各新生目标的边缘分布与当前时刻各已存在目标的边缘分布进行合并,生成合并后的当前时刻各目标的边缘分布为
将当前时刻各新生目标的存在概率与当前时刻各已存在目标的存在概率进行合并,生成合并后的当前时刻各目标的存在概率为
将当前时刻各新生目标的转弯率与当前时刻各已存在目标的转弯率进行合并,生成合并后的当前时刻各目标的转弯率为
将当前时刻各新生目标的转向角与当前时刻各已存在目标的转向角进行合并,生成合并后的当前时刻各目标的转向角为
步骤S105:从所述合并后的当前时刻各目标中,将存在概率小于第一阈值的目标裁减掉,并且将裁减后余下目标的边缘分布、存在概率、转弯率和转向角作为下一时刻递归滤波的输入;同时,从裁减后余下目标的边缘分布中提取存在概率大于第二阈值的边缘分布作为当前时刻的输出,并且将各个输出边缘分布的均值与方差分别作为当前时刻目标的状态估计与误差估计。
步骤S102具体包括以下步骤:
步骤A、设测量其中,和分别表示测量yj,k的x分量、y分量和z分量,上标T表示矩阵或向量的转置;利用mi,k-1和yj,k通过转换得到向量[xi,j yi,jzi,j]T,转换公式为其中,方向角高低角
步骤B、利用转换后的向量[xi,j yi,j zi,j]T得到转弯率ωi,j和转向角θi,j,其中,
步骤C、根据所述转弯率ωi,j、最大转弯率ωmax和最小转弯率ωmin得到目标i对应于测量yj,k的转弯率其中,ωmax和ωmin是两个已知的参数;由所述转向角θi,j,得到目标i对应于测量yj,k的转向角
在完成对M个测量的处理后,得到各目标对应于各测量的转弯率为和转向角为其中,i=1,…,Nk-1,j=1,…,M。
步骤S103具体包括以下步骤:
步骤D、由k-1时刻目标i的边缘分布N(x;mi,k-1,Pi,k-1),以及所述的转弯率和转向角得到k时刻目标i对应于测量yj,k的预测边缘分布为其中,i=1,…,Nk-1,j=1,…,M,表示状态向量的均值,且 表示状态向量的方差,且其中,为状态转移矩阵,且其中, I表示单位矩阵,Δtk=tk-tk-1为k时刻与k-1时刻的时间差,方向角高低角
步骤E、利用贝叶斯规则对测量yj,k进行处理,得到目标i对应于测量yj,k的存在概率滤波增益均值向量协方差矩阵其中,Hk表示观测矩阵,Rk表示观测噪声方差矩阵,pD,k表示目标的检测概率,λc,k表示杂波密度,I表示单位矩阵;
在完成对M个测量的处理后,得到各目标对应于各测量的边缘分布为和存在概率为其中,i=1,…,Nk-1,j=1,…,M;
步骤F、取其中则k时刻已存在目标i的边缘分布取为存在概率为转弯率为和转向角为其中i=1,…,Nk-1,当q=M+1时,
处理完后,得到各已存在目标的边缘分布为N(x;mi,k,Pi,k)、存在概率为ρi,k、转弯率为ωi,k和转向角为θi,k,其中,i=1,…,Nk-1,j=1,…,M。
为了能够产生仿真数据,在本实施例的仿真实验中设定了部分参数的具体数据大小,包括幸存概率pS,k=1.0、检测概率pD,k=0.95、杂波密度λc,k=1.95×10-8m-3、过程噪声的标准差σv=1ms-2、观测噪声的标准差σw=1m和传感器的扫描周期T=1s。同时,考虑三维空间[-400m,400m]×[-400m,400m]×[-400m,400m]中运动的目标,目标的状态由位置和速度构成,表示为其中,x、y和z分别表示位置分量,和分别表示速度分量,上标T表示向量的转置;过程噪声方差矩阵为,其中,Δtk=tk-tk-1为当前时刻与前一时刻的时间差,σv为过程噪声标准差;观测噪声方差矩阵σw为观测噪声的标准差;参数δ取为δ=2.5,最大转弯率和最小转弯率分别取为ωmax=8度/秒和ωmin=-8度/秒。
综上所述,本发明第一个实施例所提供的用于三维空间跟踪转弯机动目标的方法,通过前一时刻各目标的边缘分布、存在概率、转弯率和转向角,以及当前时刻与前一时刻的时间差对当前时刻各目标的参数进行预测,得到当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角;通过前一时刻各目标的相关参数等和当前时刻的测量对当前时刻各目标的参数进行估计,估计得到当前时刻的各目标对应于各测量的边缘分布、存在概率、转弯率和转向角;根据当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角以及估计得到的当前时刻的各目标对应于各测量的边缘分布、存在概率、转弯率和转向角等参数,得到当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角;然后利用当前时刻的各测量生成新生目标的参数,将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角与当前时刻各已存在目标的更新边缘分布、存在概率、转弯率和转向角分别进行合并,从而得到当前时刻各目标的边缘分布、存在概率、转弯率和转向角;最后通过对当前时刻各目标的边缘分布、存在概率、转弯率和转向角的处理,确定出各个目标在当前时刻的状态估计与误差估计,从而实现了对三维空间中转弯率变化的机动目标进行有效跟踪。该方法将转弯率估计与序贯贝叶斯滤波器相结合,在保证数据处理的实时性的同时,有效地解决了三维空间转弯机动目标的跟踪问题,具有很强的实用性。
作为本发明的第二个实施例,如图2所示,本发明还提供了一种用于三维空间跟踪转弯机动目标的系统,该系统包括:
预测模块10,用于根据前一时刻各目标的边缘分布、存在概率、转弯率和转向角,以及当前时刻与前一时刻的时间差,得到当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角。
转弯率与转向角估计模块20,用于根据前一时刻各目标的边缘分布和存在概率,当前时刻与前一时刻的时间差,以及当前时刻的测量集合,估计当前时刻各目标对应于各测量的转弯率与转向角。
更新模块30,用于根据估计得到的当前时刻各目标对应于各测量的转弯率与转向角,前一时刻各目标的边缘分布,当前时刻各目标的预测存在概率,当前时刻与前一时刻的时间差,以及当前时刻的测量集合,得到当前时刻各目标对应于各测量的边缘分布和存在概率;根据所述当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角,所述当前时刻各目标对应于各测量的边缘分布、存在概率、转弯率和转向角得到当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角。
新目标生成模块40,用于利用当前时刻的各测量生成新生目标的边缘分布,为所述新生目标指定存在概率、转弯率和转向角;同时,将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角,分别与所述的当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角进行合并,生成合并后的当前时刻各目标的边缘分布、存在概率、转弯率和转向角。
目标状态提取模块50,用于从所述合并后的当前时刻各目标中,将存在概率小于第一阈值的目标裁减掉,并且将裁减后余下目标的边缘分布、存在概率、转弯率和转向角作为下一时刻递归滤波的输入;同时,从裁减后余下目标的边缘分布中提取存在概率大于第二阈值的边缘分布作为当前时刻的输出,并且将各个输出边缘分布的均值与方差分别作为当前时刻目标的状态估计与误差估计。
上述预测模块10具体用于:以k-1表示前一时刻,k表示当前时刻,tk-1表示前一时刻的时间,tk表示当前时刻的时间,以N(x;mi,k-1,Pi,k-1)、ρi,k-1、ωi,k-1和θi,k-1分别表示k-1时刻目标i的边缘分布、存在概率、转弯率和转向角;其中,i为索引号,且i=1,2,…Nk-1,N表示高斯分布,x表示目标的状态向量,mi,k-1和Pi,k-1分别表示k-1时刻目标i的状态均值和协方差,Nk-1表示k-1时刻目标的总数;
由k-1时刻目标i的边缘分布N(x;mi,k-1,Pi,k-1)、存在概率ρi,k-1、转弯率ωi,k-1和转向角θi,k-1,预测得到当前时刻目标i的预测边缘分布和预测存在概率分别为N(x;mi,k|k-1,Pi,k|k-1)和ρi,k|k-1;其中, 和分别表示目标i位置的x分量、y分量和z分量,和分别表示目标i速度的x分量、y分量和z分量;其中,mi,k|k-1=Fi,k|k-1mi,k-1,Pi,k|k-1=Qi,k-1+Fi,k|k-1Pi,k-1(Fi,k|k-1)T,ρi,k|k-1=pS,k(tk-tk-1)ρi,k-1,Fi,k|k-1表示状态转移矩阵,且Fi,k|k-1=A+B·C·D·B-1·(I-A);其中, I表示单位矩阵,Δtk=tk-tk-1表示k时刻与k-1时刻的时间差,方位角高低角Qi,k-1表示k-1时刻目标i的过程噪声协方差矩阵,pS,k(tk-tk-1)表示目标的幸存概率,且T表示采样周期,δ表示预设的常数,该δ为预先给定的已知常数;
k时刻目标i的预测转弯率和预测转向角分别为ωi,k|k-1=ωi,k-1和θi,k|k-1=θi,k-1。
上述转弯率与转向角估计模块20具体用于:设测量其中,和分别表示测量yj,k的x分量、y分量和z分量,上标T表示矩阵或向量的转置;利用mi,k-1和yj,k通过转换得到向量[xi,j yi,j zi,j]T,转换公式为其中,方向角高低角
利用转换后的向量[xi,j yi,j z,ij]T得到转弯率ωi,j和转向角θi,j,其中,
根据所述转弯率ωi,j、最大转弯率ωmax和最小转弯率ωmin得到目标i对应于测量yjk的转弯率其中,ωmax和ωmin是两个已知的参数;由所述转向角θi,j,得到目标i对应于测量yj,k的转向角
在完成对M个测量的处理后,得到各目标对应于各测量的转弯率为和转向角为其中,i=1,…,Nk-1,j=1,…,M。
上述更新模块30具体用于:由k-1时刻目标i的边缘分布N(x;mi,k-1,Pi,k-1),以及所述的转弯率和转向角得到k时刻目标i对应于测量yj,k的预测边缘分布为其中,i=1,…,Nk-1,j=1,…,M,表示状态向量的均值,且 表示状态向量的方差,且其中,为状态转移矩阵,且其中, I表示单位矩阵,Δtk=tk-tk-1为k时刻与k-1时刻的时间差,方向角高低角
利用贝叶斯规则对测量yj,k进行处理,得到目标i对应于测量yj,k的存在概率滤波增益均值向量协方差矩阵其中,Hk表示观测矩阵,Rk表示观测噪声方差矩阵,pD,k表示目标的检测概率,λc,k表示杂波密度,I表示单位矩阵;
在完成对M个测量的处理后,得到各目标对应于各测量的边缘分布为和存在概率为其中,i=1,…,Nk-1,j=1,…,M;
取其中则k时刻已存在目标i的边缘分布取为存在概率为转弯率为和转向角为其中i=1,…,Nk-1,当q=M+1时,
处理完后,得到各已存在目标的边缘分布为N(x;mi,k,Pi,k)、存在概率为ρi,k、转弯率为ωi,k和转向角为θi,k,其中,i=1,…,Nk-1,j=1,…,M。
上述新目标生成模块40具体用于:利用当前时刻的M个测量生成当前时刻新生目标的边缘分布为当前时刻各新生目标指定存在概率为转弯率为和转向角为其中,表示第j个新生目标的边缘分布的协方差,为第j个新生目标的边缘分布的均值,由当前时刻的第j个测量生成,并且ργ表示为当前时刻各新生目标所指定的存在概率,和分别表示测量yj,k的x分量、y分量和z分量,上标T表示矩阵或向量的转置;
将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角,分别与所述的当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角进行合并,生成合并后的当前时刻各目标的边缘分布为存在概率为转弯率为和转向角为其中Nk=Nk-1+M。
需要说明的是,在本实施例中,如图3所示,为实验中通过传感器得到的在70个扫描周期的仿真观测数据。为了处理仿真数据,本实施例中对相关参数进行了预设,取幸存概率pS,k=0.6、检测概率pD,k=0.95、杂波密度λc,k=1.95×10-8m-3、观测噪声标准差σw=1m、过程噪声标准差σv=1ms-2、第一阈值为10-3、第二阈值为0.5、新生目标的权重wγ=0.1,新生目标的存在概率pγ=0.1,新生目标的协方差为图4为根据跳变马尔科夫系统模型高斯混合概率假设密度滤波方法对图3中的数据处理得到的结果示意图。图5所示为利用本发明提供的一种用于三维空间跟踪转弯机动目标方法对图4得到的结果进行处理后的示意图。图6所示为利用本发明所提供的方法以及跳变马尔科夫系统模型高斯混合概率假设密度滤波方法在经过100次蒙特卡罗(Monte Carlo)实验得到的,平均最优亚模式分配(Optimal Sub Pattern Assignment,OSPA)距离示意图。通过实验结果进行比较可以得出,本发明所提供的方法或系统可以获得较为精确和可靠的目标状态估计、且其OSPA距离处于合理范围内。
综上所述,本发明第二个实施例提供的用于三维空间跟踪转弯机动目标的系统,通过预测模块、转弯率和转向角估计模块、更新模块、新目标生成模块及目标状态提取模块,将转弯率和转向角估计与序贯贝叶斯滤波器相结合,确定出各个目标在当前时刻的状态估计与误差估计,从而实现了对三维空间中转弯率变化的机动目标进行有效跟踪。在保证数据处理的实时性的同时,有效地解决了三维空间转弯机动目标的跟踪问题,具有很强的实用性。
以上所述仅为本发明的较佳实施例而已,并不用以限制发明,凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等,均应包含在本发明的保护范围之内。
Claims (6)
1.一种用于三维空间跟踪转弯机动目标的方法,其特征在于,所述方法包括以下步骤:
步骤1、根据前一时刻各目标的边缘分布、存在概率、转弯率和转向角,以及当前时刻与前一时刻的时间差,得到当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角;
所述步骤1具体包括:
以k-1表示前一时刻,k表示当前时刻,tk-1表示前一时刻的时间,tk表示当前时刻的时间,以N(x;mi,k-1,Pi,k-1)、ρi,k-1、ωi,k-1和θi,k-1分别表示k-1时刻目标i的边缘分布、存在概率、转弯率和转向角;其中,i为索引号,且i=1,2,…Nk-1,N表示高斯分布,x表示目标的状态向量,mi,k-1和Pi,k-1分别表示k-1时刻目标i的状态均值和协方差,Nk-1表示k-1时刻目标的总数;
由k-1时刻目标i的边缘分布N(x;mi,k-1,Pi,k-1)、存在概率ρi,k-1、转弯率ωi,k-1和转向角θi,k-1,预测得到当前时刻目标i的预测边缘分布和预测存在概率分别为N(x;mi,k|k-1,Pi,k|k-1)和ρi,k|k-1;其中, 和分别表示目标i位置的x分量、y分量和z分量,和分别表示目标i速度的x分量、y分量和z分量;其中,mi,k|k-1=Fi,k|k-1mi,k-1,Pi,k|k-1=Qi,k-1+Fi,k|k-1Pi,k-1(Fi,k|k-1)T,ρi,k|k-1=pS,k(tk-tk-1)ρi,k-1,Fi,k|k-1表示状态转移矩阵,且Fi,k|k-1=A+B·C·D·B-1·(I-A);其中, I表示单位矩阵,Δtk=tk-tk-1表示k时刻与k-1时刻的时间差,方位角高低角Qi,k-1表示k-1时刻目标i的过程噪声协方差矩阵,pS,k(tk-tk-1)表示目标的幸存概率,且T表示采样周期,δ表示预设的常数;
k时刻目标i的预测转弯率和预测转向角分别为ωi,k|k-1=ωi,k-1和θi,k|k-1=θi,k-1;
步骤2、根据前一时刻各目标的边缘分布和存在概率,当前时刻与前一时刻的时间差,以及当前时刻的测量集合,估计当前时刻各目标对应于各测量的转弯率与转向角;
步骤3、根据估计得到的当前时刻各目标对应于各测量的转弯率与转向角,前一时刻各目标的边缘分布,当前时刻各目标的预测存在概率,当前时刻与前一时刻的时间差,以及当前时刻的测量集合,得到当前时刻各目标对应于各测量的边缘分布和存在概率;
根据所述当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角,所述当前时刻各目标对应于各测量的边缘分布、存在概率、转弯率和转向角得到当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角;
步骤4、利用当前时刻的各测量生成新生目标的边缘分布,为所述新生目标指定存在概率、转弯率和转向角;同时,将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角,分别与所述的当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角进行合并,生成合并后的当前时刻各目标的边缘分布、存在概率、转弯率和转向角;
所述步骤4具体包括:
利用当前时刻的M个测量生成当前时刻新生目标的边缘分布为当前时刻各新生目标指定存在概率为转弯率为和转向角为其中,表示第j个新生目标的边缘分布的协方差,为第j个新生目标的边缘分布的均值,由当前时刻的第j个测量生成,并且j=1,…,M,ργ表示为当前时刻各新生目标所指定的存在概率,和分别表示测量yj,k的x分量、y分量和z分量,上标T表示矩阵或向量的转置;
将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角,分别与所述的当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角进行合并,生成合并后的当前时刻各目标的边缘分布为存在概率为转弯率为和转向角为其中Nk=Nk-1+M;
步骤5、从所述合并后的当前时刻各目标中,将存在概率小于第一阈值的目标裁减掉,并且将裁减后余下目标的边缘分布、存在概率、转弯率和转向角作为下一时刻递归滤波的输入;同时,从裁减后余下目标的边缘分布中提取存在概率大于第二阈值的边缘分布作为当前时刻的输出,并且将各个输出边缘分布的均值与方差分别作为当前时刻目标的状态估计与误差估计。
2.如权利要求1所述的方法,其特征在于,所述步骤2具体包括:
步骤A、设测量其中,和分别表示测量yj,k的x分量、y分量和z分量,上标T表示矩阵或向量的转置;利用mi,k-1和yj,k通过转换得到向量[xi,j yi,jzi,j]T,转换公式为其中,方向角高低角
步骤B、利用转换后的向量[xi,j yi,j zi,j]T得到转弯率ωi,j和转向角θi,j,其中,
步骤C、根据所述转弯率ωi,j、最大转弯率ωmax和最小转弯率ωmin得到目标i对应于测量yj,k的转弯率其中,ωmax和ωmin是两个已知的参数;由所述转向角θi,j,得到目标i对应于测量yj,k的转向角
在完成对M个测量的处理后,得到各目标对应于各测量的转弯率为和转向角为其中,i=1,…,Nk-1,j=1,…,M。
3.如权利要求2所述的方法,其特征在于,所述步骤3具体包括:
步骤D、由k-1时刻目标i的边缘分布N(x;mi,k-1,Pi,k-1),以及所述的转弯率和转向角得到k时刻目标i对应于测量yj,k的预测边缘分布为其中,i=1,…,Nk-1,j=1,…,M,表示状态向量的均值,且 表示状态向量的方差,且其中,为状态转移矩阵,且其中, I表示单位矩阵,Δtk=tk-tk-1为k时刻与k-1时刻的时间差,方向角高低角
步骤E、利用贝叶斯规则对测量yj,k进行处理,得到目标i对应于测量yj,k的存在概率滤波增益均值向量协方差矩阵其中,Hk表示观测矩阵,Rk表示观测噪声方差矩阵,pD,k表示目标的检测概率,λc,k表示杂波密度,I表示单位矩阵;
在完成对M个测量的处理后,得到各目标对应于各测量的边缘分布为和存在概率为其中,i=1,…,Nk-1,j=1,…,M;
步骤F、取其中则k时刻已存在目标i的边缘分布取为存在概率为转弯率为和转向角为其中i=1,…,Nk-1,当q=M+1时,
处理完后,得到各已存在目标的边缘分布为N(x;mi,k,Pi,k)、存在概率为ρi,k、转弯率为ωi,k和转向角为θi,k,其中,i=1,…,Nk-1,j=1,…,M。
4.一种用于跟踪三维空间转弯机动目标的系统,其特征在于,所述系统包括:
预测模块,用于根据前一时刻各目标的边缘分布、存在概率、转弯率和转向角,以及当前时刻与前一时刻的时间差,得到当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角;
所述预测模块具体用于:
以k-1表示前一时刻,k表示当前时刻,tk-1表示前一时刻的时间,tk表示当前时刻的时间,以N(x;mi,k-1,Pi,k-1)、ρi,k-1、ωi,k-1和θi,k-1分别表示k-1时刻目标i的边缘分布、存在概率、转弯率和转向角;其中,i为索引号,且i=1,2,…Nk-1,N表示高斯分布,x表示目标的状态向量,mi,k-1和Pi,k-1分别表示k-1时刻目标i的状态均值和协方差,Nk-1表示k-1时刻目标的总数;
由k-1时刻目标i的边缘分布N(x;mi,k-1,Pi,k-1)、存在概率ρi,k-1、转弯率ωi,k-1和转向角θi,k-1,预测得到当前时刻目标i的预测边缘分布和预测存在概率分别为N(x;mi,k|k-1,Pi,k|k-1)和ρi,k|k-1;其中, 和分别表示目标i位置的x分量、y分量和z分量,和分别表示目标i速度的x分量、y分量和z分量;其中,mi,k|k-1=Fi,k|k-1mi,k-1,Pi,k|k-1=Qi,k-1+Fi,k|k-1Pi,k-1(Fi,k|k-1)T,ρi,k|k-1=pS,k(tk-tk-1)ρi,k-1,Fi,k|k-1表示状态转移矩阵,且Fi,k|k-1=A+B·C·D·B-1·(I-A);其中, I表示单位矩阵,Δtk=tk-tk-1表示k时刻与k-1时刻的时间差,方位角高低角Qi,k-1表示k-1时刻目标i的过程噪声协方差矩阵,pS,k(tk-tk-1)表示目标的幸存概率,且T表示采样周期,δ表示预设的常数;
k时刻目标i的预测转弯率和预测转向角分别为ωi,k|k-1=ωi,k-1和θi,k|k-1=θi,k-1;
转弯率与转向角估计模块,用于根据前一时刻各目标的边缘分布和存在概率,当前时刻与前一时刻的时间差,以及当前时刻的测量集合,估计当前时刻各目标对应于各测量的转弯率与转向角;
更新模块,用于根据估计得到的当前时刻各目标对应于各测量的转弯率与转向角,前一时刻各目标的边缘分布,当前时刻各目标的预测存在概率,当前时刻与前一时刻的时间差,以及当前时刻的测量集合,得到当前时刻各目标对应于各测量的边缘分布和存在概率;
根据所述当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角,所述当前时刻各目标对应于各测量的边缘分布、存在概率、转弯率和转向角得到当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角;
新目标生成模块,用于利用当前时刻的各测量生成新生目标的边缘分布,为所述新生目标指定存在概率、转弯率和转向角;同时,将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角,分别与所述的当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角进行合并,生成合并后的当前时刻各目标的边缘分布、存在概率、转弯率和转向角;
所述新目标生成模块具体用于:
利用当前时刻的M个测量生成当前时刻新生目标的边缘分布为当前时刻各新生目标指定存在概率为转弯率为和转向角为其中,表示第j个新生目标的边缘分布的协方差,为第j个新生目标的边缘分布的均值,由当前时刻的第j个测量生成,并且j=1,…,M,ργ表示为当前时刻各新生目标所指定的存在概率,和分别表示测量yj,k的x分量、y分量和z分量,上标T表示矩阵或向量的转置;
将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角,分别与所述的当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角进行合并,生成合并后的当前时刻各目标的边缘分布为存在概率为转弯率为和转向角为其中Nk=Nk-1+M;
目标状态提取模块,用于从所述合并后的当前时刻各目标中,将存在概率小于第一阈值的目标裁减掉,并且将裁减后余下目标的边缘分布、存在概率、转弯率和转向角作为下一时刻递归滤波的输入;同时,从裁减后余下目标的边缘分布中提取存在概率大于第二阈值的边缘分布作为当前时刻的输出,并且将各个输出边缘分布的均值与方差分别作为当前时刻目标的状态估计与误差估计。
5.如权利要求4所述的系统,其特征在于,所述转弯率与转向角估计模块具体用于:
设测量其中,和分别表示测量yj,k的x分量、y分量和z分量,上标T表示矩阵或向量的转置;利用mi,k-1和yj,k通过转换得到向量[xi,j yi,j zi,j]T,转换公式为其中,方向角高低角
利用转换后的向量[xi,j yi,j zi,j]T得到转弯率ωi,j和转向角θi,j,其中,
根据所述转弯率ωi,j、最大转弯率ωmax和最小转弯率ωmin得到目标i对应于测量yj,k的转弯率其中,ωmax和ωmin是两个已知的参数;由所述转向角θi,j,得到目标i对应于测量yj,k的转向角
在完成对M个测量的处理后,得到各目标对应于各测量的转弯率为和转向角为其中,i=1,…,Nk-1,j=1,…,M。
6.如权利要求5所述的系统,其特征在于,所述更新模块具体用于:
由k-1时刻目标i的边缘分布N(x;mi,k-1,Pi,k-1),以及所述的转弯率和转向角得到k时刻目标i对应于测量yj,k的预测边缘分布为其中,i=1,…,Nk-1,j=1,…,M,表示状态向量的均值,且 表示状态向量的方差,且其中,为状态转移矩阵,且其中, I表示单位矩阵,Δtk=tk-tk-1为k时刻与k-1时刻的时间差,方向角高低角
利用贝叶斯规则对测量yj,k进行处理,得到目标i对应于测量yj,k的存在概率滤波增益均值向量协方差矩阵其中,Hk表示观测矩阵,Rk表示观测噪声方差矩阵,pD,k表示目标的检测概率,λc,k表示杂波密度,I表示单位矩阵;
在完成对M个测量的处理后,得到各目标对应于各测量的边缘分布为和存在概率为其中,i=1,…,Nk-1,j=1,…,M;
取其中则k时刻已存在目标i的边缘分布取为存在概率为转弯率为和转向角为其中i=1,…,Nk-1,当q=M+1时,
处理完后,得到各已存在目标的边缘分布为N(x;mi,k,Pi,k)、存在概率为ρi,k、转弯率为ωi,k和转向角为θi,k,其中,i=1,…,Nk-1,j=1,…,M。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710224049.9A CN106952290B (zh) | 2017-04-07 | 2017-04-07 | 一种用于三维空间跟踪转弯机动目标的方法及系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710224049.9A CN106952290B (zh) | 2017-04-07 | 2017-04-07 | 一种用于三维空间跟踪转弯机动目标的方法及系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN106952290A true CN106952290A (zh) | 2017-07-14 |
CN106952290B CN106952290B (zh) | 2019-05-10 |
Family
ID=59475471
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710224049.9A Expired - Fee Related CN106952290B (zh) | 2017-04-07 | 2017-04-07 | 一种用于三维空间跟踪转弯机动目标的方法及系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN106952290B (zh) |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108917745A (zh) * | 2018-05-16 | 2018-11-30 | 深圳大学 | 一种三维空间转弯机动目标的状态估计方法及系统 |
CN108921870A (zh) * | 2018-07-03 | 2018-11-30 | 深圳大学 | 三维空间转弯机动目标的轨迹起始方法及装置 |
CN109800721A (zh) * | 2019-01-24 | 2019-05-24 | 深圳大学 | 一种多目标跟踪方法及系统 |
WO2019218241A1 (zh) * | 2018-05-16 | 2019-11-21 | 深圳大学 | 一种三维空间转弯机动目标的状态估计方法及系统 |
WO2020006672A1 (zh) * | 2018-07-03 | 2020-01-09 | 深圳大学 | 三维空间转弯机动目标的轨迹起始方法及装置 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102385690A (zh) * | 2010-09-01 | 2012-03-21 | 汉王科技股份有限公司 | 基于视频图像的目标跟踪方法及系统 |
CN103390107A (zh) * | 2013-07-24 | 2013-11-13 | 深圳大学 | 一种基于狄拉克加权和的目标跟踪方法与目标跟踪系统 |
CN103902829A (zh) * | 2014-04-11 | 2014-07-02 | 深圳大学 | 传递边缘分布及存在概率的目标跟踪方法与目标跟踪系统 |
CN104318059A (zh) * | 2014-09-24 | 2015-01-28 | 深圳大学 | 用于非线性高斯系统的目标跟踪方法和跟踪系统 |
CN104867163A (zh) * | 2015-05-28 | 2015-08-26 | 深圳大学 | 一种传递边缘分布的测量驱动目标跟踪方法与跟踪系统 |
CN106127776A (zh) * | 2016-06-28 | 2016-11-16 | 北京工业大学 | 基于多特征时空上下文机器人目标识别与运动决策方法 |
-
2017
- 2017-04-07 CN CN201710224049.9A patent/CN106952290B/zh not_active Expired - Fee Related
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102385690A (zh) * | 2010-09-01 | 2012-03-21 | 汉王科技股份有限公司 | 基于视频图像的目标跟踪方法及系统 |
CN103390107A (zh) * | 2013-07-24 | 2013-11-13 | 深圳大学 | 一种基于狄拉克加权和的目标跟踪方法与目标跟踪系统 |
CN103902829A (zh) * | 2014-04-11 | 2014-07-02 | 深圳大学 | 传递边缘分布及存在概率的目标跟踪方法与目标跟踪系统 |
CN104318059A (zh) * | 2014-09-24 | 2015-01-28 | 深圳大学 | 用于非线性高斯系统的目标跟踪方法和跟踪系统 |
CN104867163A (zh) * | 2015-05-28 | 2015-08-26 | 深圳大学 | 一种传递边缘分布的测量驱动目标跟踪方法与跟踪系统 |
CN106127776A (zh) * | 2016-06-28 | 2016-11-16 | 北京工业大学 | 基于多特征时空上下文机器人目标识别与运动决策方法 |
Cited By (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108917745A (zh) * | 2018-05-16 | 2018-11-30 | 深圳大学 | 一种三维空间转弯机动目标的状态估计方法及系统 |
CN108917745B (zh) * | 2018-05-16 | 2019-10-11 | 深圳大学 | 一种三维空间转弯机动目标的状态估计方法及系统 |
WO2019218241A1 (zh) * | 2018-05-16 | 2019-11-21 | 深圳大学 | 一种三维空间转弯机动目标的状态估计方法及系统 |
CN108921870A (zh) * | 2018-07-03 | 2018-11-30 | 深圳大学 | 三维空间转弯机动目标的轨迹起始方法及装置 |
CN108921870B (zh) * | 2018-07-03 | 2019-08-09 | 深圳大学 | 三维空间转弯机动目标的轨迹起始方法及装置 |
WO2020006672A1 (zh) * | 2018-07-03 | 2020-01-09 | 深圳大学 | 三维空间转弯机动目标的轨迹起始方法及装置 |
CN109800721A (zh) * | 2019-01-24 | 2019-05-24 | 深圳大学 | 一种多目标跟踪方法及系统 |
CN109800721B (zh) * | 2019-01-24 | 2020-10-23 | 深圳大学 | 一种多目标跟踪方法及系统 |
Also Published As
Publication number | Publication date |
---|---|
CN106952290B (zh) | 2019-05-10 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106952290B (zh) | 一种用于三维空间跟踪转弯机动目标的方法及系统 | |
CN104318059B (zh) | 用于非线性高斯系统的目标跟踪方法和跟踪系统 | |
CN105719312B (zh) | 基于序贯贝叶斯滤波的多目标跟踪方法及跟踪系统 | |
WO2017124299A1 (zh) | 基于序贯贝叶斯滤波的多目标跟踪方法及跟踪系统 | |
Bilik et al. | Maneuvering target tracking in the presence of glint using the nonlinear Gaussian mixture Kalman filter | |
CN107462882B (zh) | 一种适用于闪烁噪声的多机动目标跟踪方法及系统 | |
CN106772353B (zh) | 一种适用于闪烁噪声的多目标跟踪方法及系统 | |
WO2018010099A1 (zh) | 一种用于跟踪转弯机动目标的方法及其系统 | |
CN103902829B (zh) | 传递边缘分布及存在概率的目标跟踪方法与目标跟踪系统 | |
CN104867163A (zh) | 一种传递边缘分布的测量驱动目标跟踪方法与跟踪系统 | |
CN106168943A (zh) | 一种用于跟踪转弯机动目标的方法及其系统 | |
CN104833967A (zh) | 一种基于滚动时域估计的雷达目标跟踪方法 | |
Liu et al. | Extended target tracking with multipath detections, terrain-constrained motion model and clutter | |
CN104777465B (zh) | 基于b样条函数任意扩展目标形状及状态估计方法 | |
Su et al. | A variational Bayesian approach for partly resolvable group tracking | |
Lian et al. | Joint spatial registration and multi-target tracking using an extended PM-CPHD filter | |
CN105548985A (zh) | 基于RAV-Jerk模型的机动目标跟踪方法 | |
CN113761662B (zh) | 一种滑翔类目标的轨迹预测管道的生成方法 | |
Bugallo et al. | Particle filtering in high-dimensional systems with Gaussian approximations | |
WO2018184181A1 (zh) | 一种用于三维空间跟踪转弯机动目标的方法及系统 | |
Alotaibi et al. | Object Tracking Incorporating Transfer Learning into an Unscented Kalman Filter | |
CN109800721B (zh) | 一种多目标跟踪方法及系统 | |
Zhang et al. | State estimation with a heading constraint | |
Bogomolov | Test Results of the Long Baseline Navigation Solutions under a Large a Priori Position Uncertainty | |
Oliveira et al. | GNSS-denied joint cooperative terrain navigation and target tracking using factor graph geometric average fusion |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20190510 Termination date: 20200407 |