CN108279412A - 一种目的地约束下目标跟踪装置及方法 - Google Patents

Abstract

本发明提供一种目的地约束下目标跟踪装置及方法，包括：中央处理模块；观测雷达模块，其用于在跟踪过程中获得目标位置量测信息；滤波模块，其用于对目标运动状态进行估计，本发明包括不同两种形式，即增广量测形式和序贯处理形式。采用本发明提供的目的地约束下目标跟踪装置及方法能够为约束先验信息不完整情况下的目标跟踪问题提供了新的解决途径，合理利用其运动轨迹中隐含的线性等式约束信息，避免了信息浪费，提高了估计精度。

Description

一种目的地约束下目标跟踪装置及方法

技术领域

本发明涉及目标跟踪领域，尤其涉及一种目的地约束下目标跟踪装置及方法。

背景技术

约束状态估计是在目标状态服从等式或不等式约束条件的情况下，对其进行状态估计的方法。在很多实际目标跟踪场景中，目标运动轨迹不完全由目标本身的运动速度决定，而是受其所处物理环境或是其自身运动特点影响或限制，并不是无约束的自由运动。合理利用这些实际场景中蕴含的约束信息可以有效的提高估计性能和滤波精度。

在实际应用中存在这样一种线性等式约束条件，其先验信息是不完整的，只有目标运动的目的地，即约束直线上的一个点是确定已知的，我们把这种约束称为目的地约束。这种约束常见于水平笛卡尔坐标平面上的反辐射导弹跟踪问题，由于其导引头的导引策略，在中途不改变打击目标的情况下，反辐射导弹在笛卡尔坐标系内的运动轨迹可以视作是指向静止打击目标的一条直线。而作为防守方，静止打击目标(例如雷达)的坐标我们是先验可知的。这种明显的运动特点可以作为先验约束信息引入跟踪系统用来提高估计精度。其他应用还包括贵重财产的反炮击保护等。这类约束状态估计问题中只包含具体约束条件的部分先验信息，而传统的约束状态估计方法，如D.Simon,“Kalman filtering withstate constraints:a survey oflinear and nonlinear algorithms,”IET ControlTheory&Applications,2010,4(8),pp.1303-1318(状态约束下的卡尔曼滤波)中提到的方法，均是针对有完整先验信息的约束条件的，无法直接用于处理这种目的地约束下的目标跟踪问题。

目前针对目的地约束目标跟踪方法的研究，如G.Zhou and K.Li,“Stateestimationwith destination constraints,”Proceedings of 19th InternationalConference on Information Fusion,pp.292–297,2016(目的地约束下的状态估计)中利用量测点和目的地连线来构造近似的目的地约束，并用受测量噪声影响的伪量测来将其引入跟踪系统中，得到了较大的性能提升。然而这种方法需要获取伪量测噪声的统计特性，相对复杂，而且伪量测中包含的先验信息来源于真实约束的近似，约束效果不理想。因此，需要提供一种更加准确高效的目的地约束下目标跟踪方法。

发明专利内容

有鉴于此，本发明旨在提出一种目的地约束下目标跟踪方法，以进行更加准确高效的目标跟踪。

为达到上述目的，本发明的技术方案是这样实现的：

一种目的地约束下目标跟踪装置，包括：

中央处理模块；

观测雷达模块，其用于在跟踪过程中获得目标位置量测信息；

滤波模块，其用于对目标运动状态进行估计；

所述中央处理模块还包括：

目的地记录单元，其用于记录目的地坐标(x_n,y_n)，其中x_n和y_n为目的地x，y方向坐标；

转换单元，其用于将极坐标下的目标位置量测信息，转换到笛卡尔坐标系下，目标运动状态方程为：

x_k+1＝Φ_kx_k+Γ_kv_k

其中x_k是k时刻状态向量；x_k+1是k+1时刻状态向量，下标为对应时刻；Φ_k是状态转移矩阵；v_k是过程噪声向量，假设过程噪声是零均值方差已知的高斯白噪声，其协方差矩阵为cov(v_k)＝Q_k≥0；Γ_k是噪声分布矩阵；

伪量测单元，其用于根据目的地坐标(x_n,y_n)，和状态分量间确定的约束关系来构造伪量测：

其中x_n和y_n为目的地x，y方向坐标；和为目标运动状态中沿x，y方向速度分量；λ_k为伪量测；

量测单元，其用于构建量测和目标状态之间的关系的量测方程；

运动模型单元，其用于构建沿直线运动目标跟踪的运动模型的量测矩阵，所述运动模型的量测矩阵包括：近匀速模型(NCV)的量测矩阵和近匀加速模型(NCA)的量测矩阵。

进一步的，所述目的地约束下目标跟踪方法包括如下步骤：

步骤1：在所述中央处理模块的目的地记录单元中记录目的地坐标(x_n,y_n)，其中x_n和y_n为目的地x，y方向坐标；

步骤2：在目标跟踪过程中，通过所述观测雷达模块获得目标位置量测信息，所述目标位置量测信息包括：目标相对雷达坐标系原点的距离量测和方位角量测将所述观测雷达模块获得的所述目标位置量测信息反馈至所述中央处理模块；

步骤3：在所述中央处理模块的转换单元中将极坐标下的目标位置量测信息，转换到笛卡尔坐标系下，在笛卡尔坐标系下，目标运动状态方程为：

x_k+1＝Φ_kx_k+Γ_kv_k

其中x_k是k时刻状态向量；x_k+1是k+1时刻状态向量，下标为对应时刻；Φ_k是状态转移矩阵；v_k是过程噪声向量，假设过程噪声是零均值方差已知的高斯白噪声，其协方差矩阵为cov(v_k)＝Q_k≥0；Γ_k是噪声分布矩阵；

步骤4：在所述中央处理模块的伪量测单元中，根据目的地坐标(x_n,y_n)，和状态分量间确定的约束关系来构造伪量测：

其中x_n和y_n为目的地x，y方向坐标；和为目标运动状态中沿x，y方向速度分量；λ_k为伪量测；

步骤5：得到笛卡尔坐标系下的伪量测后，在所述中央处理模块的量测单元中，构建量测和目标状态之间的关系的量测方程；

步骤6：在所述中央处理模块的运动模型单元中，构建沿直线运动目标的运动模型的量测矩阵，所述运动模型的量测矩阵包括：近匀速模型(NCV)的量测矩阵和近匀加速模型(NCA)的量测矩阵；

步骤7：通过所述滤波模块进行滤波初始化：采用两点差分法，即利用最初两个时刻k＝1、k＝2时刻的笛卡尔坐标系下位置量测值得到k＝2时刻的状态估计：

其对应的初始状态协方差矩阵为

其中和是笛卡尔坐标下的目标沿x，y方向的位置量测信息，是通过无偏量测转换方法将雷达位置量测转换到笛卡尔坐标系下得到的转换量测；转换公式为：

其中 是从雷达获取的距离，方位角量测； 是转换后得到的沿x，y方向的笛卡尔坐标量测，是转换后的量测向量；μ_θ是去偏系数，可通过方位角测量噪声方差求得：

对应的协方差矩阵为

其中R_k,xx，R_k,yy分别为x，y方向笛卡尔坐标量测对应的量测噪声方差，R_k,xy为x，y方向笛卡尔坐标量测对应的量测噪声之间的互协方差：

上标“c”代表与转换量测相关的向量、矩阵和函数；

步骤8：通过所述滤波模块从k＝3时刻开始滤波，进而得到k时刻的约束状态估计结果。

进一步的，所述步骤5中，得到笛卡尔坐标系下的伪量测后，在所述中央处理模块的量测单元中，构建量测和目标状态之间的关系的量测方程，具体为：

步骤5(1)：

将伪量测增广到量测向量中，得到量测方程为：

对应的量测噪声协方差矩阵为：

其中是k时刻量测向量；和分别是雷达获取的距离量测和方位角量测；是表征量测向量与状态向量间关系的函数；是测量噪声向量；和分别是距离和方位角量测对应的量测噪声；σ_r和σ_θ分别是距离和方位角量测对应的量测噪声标准差；R_k,rr和R_k,θθ分别是距离和方位角量测对应的量测噪声方差，R_k,rθ是距离和方位角量测之间的互协方差；R_k,λλ是伪量测对应的量测噪声方差，由于伪量测是一个常数，因此R_k,λλ＝0；R_k,rλR_k,θλ是伪量测与位置量测之间的互协方差，均为零；上标“a”代表增广的向量、矩阵和函数。

进一步的，所述步骤6中所述的近匀速模型(NCV)的量测矩阵和近匀加速模型(NCA)的量测矩阵，具体为：

步骤6(1)：

此时对应的状态向量分别为和x_k、y_k分别为沿x，y方向的位置分量，分别为沿x，y方向的速度分量；其中和分别为x，y方向的加速度分量；T为雷达扫描周期。

进一步的，经过所述步骤7的初始化滤波，在所述步骤8中，通过所述滤波模块从k＝3时刻开始滤波，其具体步骤如下：

步骤8(1)：

首先根据k-1时刻的约束状态估计对k时刻状态进行一步预测：

计算状态一步预测：

计算状态一步预测协方差：

然后进行无迹变换：

计算在附近选取的2n+1个δ采样点

根据量测方程，计算量测预测对应的2n+1个后验δ采样点

根据采样点，计算预测量测均值

计算预测量测对应的协方差矩阵

计算量测和状态向量的交互协方差

计算滤波增益

最后更新状态估计及协方差：

计算状态估计

计算状态估计协方差

P_k|k＝P_k|k-1-K_kP_k,zz(K_k)^T

其中n为状态向量维数，i＝0,1,...,2n，上标“a”代表增广的向量、矩阵和函数；

关于无迹变换，代表(n+λ)P_k|k-1矩阵的第j行，λ是一个尺度参数，λ＝α²(n+κ)-n，n+λ≠0；和分别是根据δ采样点计算均值和协方差时对应的权值，可以通过以下公式得到：

其中α，β和κ是经验参数，α用来决定δ采样点在随机量均值附近的散布情况，β用来引入随机量分布的先验知识，κ是比例参数。

进一步的，所述步骤5中，得到笛卡尔坐标系下的伪量测后，在所述中央处理模块的量测单元中，构建量测和目标状态之间的关系的量测方程，具体为：

步骤5(2)：

得到笛卡尔坐标系下的量测后，量测和目标状态之间的关系由量测方程给出：

其中是笛卡尔坐标下的量测向量，是表征笛卡尔坐标量测与状态关系的量测矩阵；是笛卡尔坐标系下量测对应的测量噪声，对应的测量噪声协方差矩阵为

进一步的，所述步骤6中所述的近匀速模型(NCV)的量测矩阵和近匀加速模型(NCA)的量测矩阵，具体为：

步骤6(2)：

此时对应的状态向量分别为和

进一步的，经过所述步骤7的滤波初始化，在所述步骤8中，通过所述滤波模块从k＝3时刻开始滤波，其具体步骤如下：

步骤8(2)：

根据状态方程，计算状态一步预测

计算状态一步预测协方差

根据量测方程，计算量测预测

计算量测预测对应的协方差

计算滤波增益

计算状态估计

计算状态估计协方差

进而得到的是k时刻的无约束的滤波结果。

进一步的，所述目的地约束下目标跟踪方法还包括步骤9：

在所述步骤8(2)的基础上进行第二次滤波，根据所述步骤4中给出的伪量测方式写出新的量测方程为：

其中为只包含伪量测的量测向量，其对应的测量噪声协方差矩阵为 为表征量测与状态关系的函数；

系统状态方程保持不变，根据序贯滤波思想，将所述步骤8(2)中滤波得到的状态估计和协方差作为一步预测结果代入到滤波过程中，即

状态一步预测

状态一步预测协方差

然后进行无迹变换：

计算基于选择的2n+1个δ采样点

根据量测方程，计算量测预测对应的2n+1个后验δ采样点

根据采样点，计算预测量测均值

计算预测量测对应的协方差矩阵

计算量测和状态向量的交互协方差

计算滤波增益

最后更新状态估计及协方差：

计算状态估计

计算状态估计协方差

其中n为状态向量维数，i＝0,1,...,2n，

和分别是根据δ采样点计算均值和协方差时对应的权值，可以通过以下公式得到：

其中α，β和κ是经验参数，α用来决定δ采样点在随机量均值附近的散布情况，β用来引入随机量分布的先验知识，κ是比例参数；

进而得到k时刻的约束状态估计结果。

相对于现有技术，本发明所述的目的地约束下目标跟踪方法具有以下优势：

(1)本发明包括不同两种形式，即增广量测形式和序贯处理形式，为约束先验信息不完整情况下的目标跟踪问题提供了新的解决途径，合理利用其运动轨迹中隐含的线性等式约束信息，避免了信息浪费，提高了估计精度；

(2)由于目的地约束中包含关于目标状态的有用信息，增大了滤波器可利用的信息量，从而提高了滤波精度，进而采用本方法在引入目的地约束后进行目标追踪能够明显减小误差，优化效果十分明显；

(3)序贯处理形式将第一次滤波过程的估计结果作为第二次滤波过程中的一步预测，提高了预测精度，进而得到了更好的滤波精度；

(4)本发明针对构造出的伪量测提出了有效的滤波方法，将伪量测中包含的与目标状态有关的约束先验信息引入跟踪系统，这些信息完全来源于目标真实直线轨迹，不涉及近似，因此更加准确，能够有效提升估计精度；并且避免了在伪量测中引入测量噪声，不再需要获取伪量测噪声的统计特性，提高效率的同时简化了滤波模块设计。

附图说明

构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1是本发明目标追踪装置的结构框图；

图2是本发明增广量测形式的目的地约束滤波模块的功能框图；

图3是本发明序贯处理形式的目的地约束滤波模块的功能框图；

图4示出了仿真实验中构造的笛卡尔坐标系下直线匀速运动目标真实轨迹；

图5示出了利用无约束无迹卡尔曼滤波和本发明的两种形式分别对仿真数据估计得到的位置均方根误差对比结果；

图6示出了利用无约束无迹卡尔曼滤波和本发明的两种形式分别对仿真数据估计得到的速度均方根误差对比结果。

具体实施方式

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

以下结合附图，对本发明专利上述的和另外的技术特征和优点作更详细的说明。

实施例一

本发明提供一种目的地约束下目标跟踪装置，所述目标追踪装置包括：

中央处理模块；

观测雷达模块，其用于在跟踪过程中获得目标位置量测信息；

滤波模块，其用于对目标运动状态进行估计。

所述中央处理模块还包括：

目的地记录单元，其用于记录目的地坐标(x_n,y_n)，其中x_n和y_n为目的地x，y方向坐标；

转换单元，其用于中将极坐标下的目标位置量测信息，转换到笛卡尔坐标系下，目标运动状态方程为：

x_k+1＝Φ_kx_k+Γ_kv_k

其中x_k是k时刻状态向量；x_k+1是k+1时刻状态向量，下标为对应时刻；Φ_k是状态转移矩阵；v_k是过程噪声向量，假设过程噪声是零均值方差已知的高斯白噪声，其协方差矩阵为cov(v_k)＝Q_k≥0；Γ_k是噪声分布矩阵；

伪量测单元，其用于根据目的地坐标(x_n,y_n)，和状态分量间确定的约束关系来构造伪量测：

其中x_n和y_n为目的地x，y方向坐标；和为目标运动状态中沿x，y方向速度分量；λ_k为伪量测；

量测单元，其用于构建量测和目标状态之间的关系的量测方程；

运动模型单元，其用于构建沿直线运动目标跟踪的运动模型的量测矩阵，所述运动模型的量测矩阵包括：近匀速模型(NCV)的量测矩阵和近匀加速模型(NCA)的量测矩阵。

实施例二

一种目的地约束下目标跟踪方法，所述目的地约束下目标跟踪方法基于上述所述目标追踪装置实现，所述目的地约束下目标跟踪方法包括如下步骤：

步骤1：在所述中央处理模块的目的地记录单元中记录目的地坐标(x_n,y_n)，其中x_n和y_n为目的地x，y方向坐标；

步骤2：在目标跟踪过程中，通过所述观测雷达模块获得目标位置量测信息，所述目标位置量测信息包括：目标相对雷达坐标系原点的距离量测和方位角量测将所述观测雷达模块获得的所述目标位置量测信息反馈至所述中央处理模块；

步骤3：在所述中央处理模块的转换单元中将极坐标下的目标位置量测信息，转换到笛卡尔坐标系下，在笛卡尔坐标系下，目标运动状态方程为：

x_k+1＝Φ_kx_k+Γ_kv_k

其中x_k是k时刻状态向量；x_k+1是k+1时刻状态向量，下标为对应时刻；Φ_k是状态转移矩阵；v_k是过程噪声向量，假设过程噪声是零均值方差已知的高斯白噪声，其协方差矩阵为cov(v_k)＝Q_k≥0；Γ_k是噪声分布矩阵；

步骤4：在所述中央处理模块的伪量测单元中，根据目的地坐标(x_n,y_n)，和状态分量间确定的约束关系来构造伪量测：

其中x_n和y_n为目的地x，y方向坐标；和为目标运动状态中沿x，y方向速度分量；λ_k为伪量测；

步骤5：得到笛卡尔坐标系下的伪量测后，在所述中央处理模块的量测单元中，构建量测和目标状态之间的关系的量测方程；

步骤6：在所述中央处理模块的运动模型单元中，构建沿直线运动目标的运动模型的量测矩阵，所述运动模型的量测矩阵包括：近匀速模型(NCV)的量测矩阵和近匀加速模型(NCA)的量测矩阵；

步骤7：通过所述滤波模块进行滤波初始化：采用两点差分法，即利用最初两个时刻k＝1、k＝2时刻的笛卡尔坐标系下位置量测值得到k＝2时刻的状态估计：

其对应的初始状态协方差矩阵为

其中和是笛卡尔坐标下的目标沿x，y方向的位置量测信息，是通过无偏量测转换方法将雷达位置量测转换到笛卡尔坐标系下得到的转换量测；转换公式为：

其中 是从雷达获取的距离，方位角量测；是转换后得到的沿x，y方向的笛卡尔坐标量测，是转换后的量测向量；μ_θ是去偏系数，可通过方位角测量噪声方差求得：

对应的协方差矩阵为

其中R_k,xx，R_k,yy分别为x，y方向笛卡尔坐标量测对应的量测噪声方差，R_k,xy为x，y方向笛卡尔坐标量测对应的量测噪声之间的互协方差：

上标“c”代表与转换量测相关的向量、矩阵和函数；

步骤8：通过所述滤波模块从k＝3时刻开始滤波，进而得到k时刻的约束状态估计结果。

进一步的，步骤1中所述的目标位置量测信息包括目标相对雷达坐标系原点的距离量测和方位角量测至于所述观测雷达模块如何获得这些信息、以及本发明实施方式的方法如何从所述观测雷达模块处获得这些信息，本领域技术人员可以采取各种方式来实现，无论采用哪种方法都在本发明的保护范围内。

进一步的，步骤2中所述的极坐标下的目标位置量测信息，包括距离信息和方位角信息。

进一步的，步骤4中所述伪量测λ_k不受量测噪声影响，与速度，位置分量间是非线性关系，描述了来自所有可能方向的，指向同一目的地的直线轨迹共同满足的约束关系。

实施例三

如上述所述的目的地约束下目标跟踪方法，本实施例与其不同之处在于，所述步骤5中，得到笛卡尔坐标系下的伪量测后，在所述中央处理模块的量测单元中，构建量测和目标状态之间的关系的量测方程，具体为：

步骤5(1)：

将伪量测增广到量测向量中，得到量测方程为：

对应的量测噪声协方差矩阵为：

其中是k时刻量测向量；和分别是雷达获取的距离量测和方位角量测；是表征量测向量与状态向量间关系的函数；是量测噪声向量；和分别是距离和方位角量测对应的量测噪声；σ_r和σ_θ分别是距离和方位角量测对应的量测噪声标准差；R_k,rr和R_k,θθ分别是距离和方位角量测对应的量测噪声方差，R_k,rθ是距离和方位角量测之间的互协方差；R_k,λλ是伪量测对应的量测噪声方差，由于伪量测是一个常数，因此R_k,λλ＝0；R_k,rλR_k,θλ是伪量测与位置量测之间的互协方差，均为零；上标“a”代表增广的向量、矩阵和函数。

进一步的，R_k,rθ是距离和方位角量测之间的互协方差，本发明中假设距离量测和方位角量测之间是不相关的，即R_k,rθ＝0。

实施例四

如上述所述的目的地约束下目标跟踪方法，本实施例与其不同之处在于，所述步骤5中，得到笛卡尔坐标系下的伪量测后，在所述中央处理模块的量测单元中，构建量测和目标状态之间的关系的量测方程，具体为：

步骤5(2)：

得到笛卡尔坐标系下的量测后，量测和目标状态之间的关系由量测方程给出：

其中是笛卡尔坐标下的量测向量，是表征笛卡尔坐标量测与状态关系的量测矩阵；是笛卡尔坐标系下量测对应的测量噪声，对应的测量噪声协方差矩阵为

实施例五

如上述所述的目的地约束下目标跟踪方法，本实施例与其不同之处在于，步骤6中所述的近匀速模型(NCV)的量测矩阵和近匀加速模型(NCA)的量测矩阵，具体为：

步骤6(1)：

此时对应的状态向量分别为和x_k、y_k分别为沿x，y方向的位置分量，分别为沿x，y方向的速度分量；其中和分别为x，y方向的加速度分量；T为雷达扫描周期。

实施例六

如上述所述的目的地约束下目标跟踪方法，本实施例与其不同之处在于，步骤6中所述的近匀速模型(NCV)的量测矩阵和近匀加速模型(NCA)的量测矩阵，具体为：

步骤6(2)：

此时对应的状态向量分别为和

实施例七

如上述所述的目的地约束下目标跟踪方法，本实施例与其不同之处在于，由于增广量测向量与笛卡尔坐标系下目标状态之间是非线性关系，因此在滤波过程中需要采用非线性滤波方法，常用的非线性滤波方法包括转换量测卡尔曼滤波方法，无迹卡尔曼滤波方法，扩展卡尔曼滤波方法以及粒子滤波方法等。在本实施方式中，优选地，采用无迹卡尔曼滤波方法。

经过步骤7的初始化滤波，在所述步骤8中，通过所述滤波模块从k＝3时刻开始滤波，其具体步骤如下：

步骤8(1)：

首先根据k-1时刻的约束状态估计对k时刻状态进行一步预测：

计算状态一步预测：

计算状态一步预测协方差：

然后进行无迹变换：

计算在附近选取的2n+1个δ采样点

根据量测方程，计算量测预测对应的2n+1个后验δ采样点

根据采样点，计算预测量测均值

计算预测量测对应的协方差矩阵

计算量测和状态向量的交互协方差

计算滤波增益

最后更新状态估计及协方差：

计算状态估计

计算状态估计协方差

P_k|k＝P_k|k-1-K_kP_k,zz(K_k)^T

其中n为状态向量维数，i＝0,1,...,2n，上标“a”代表增广的向量、矩阵和函数；

关于无迹变换，代表(n+λ)P_k|k-1矩阵的第j行，λ是一个尺度参数，λ＝α²(n+κ)-n，n+λ≠0；和分别是根据δ采样点计算均值和协方差时对应的权值，可以通过以下公式得到：

其中α，β和κ是经验参数，α用来决定δ采样点在随机量均值附近的散布情况，β用来引入随机量分布的先验知识，κ是比例参数。

进一步的，所述k时刻的约束状态估计即为滤波结果。

实施例八

如上述所述的目的地约束下目标跟踪方法，本实施例与其不同之处在于，由于从雷达处获取的位置量测与笛卡尔坐标系下目标状态之间是非线性关系，因此在处理位置量测时需要采用非线性滤波方法，常用的非线性滤波方法包括转换量测卡尔曼滤波方法，无迹卡尔曼滤波方法，扩展卡尔曼滤波方法以及粒子滤波方法等。本实施方式中，优选地，采用转换量测卡尔曼滤波方法。

经过步骤7的初始化滤波，在所述步骤8中，通过所述滤波模块从k＝3时刻开始滤波，其具体步骤如下：

步骤8(2)：

根据状态方程，计算状态一步预测

计算状态一步预测协方差

根据量测方程，计算量测预测

计算量测预测对应的协方差

计算滤波增益

计算状态估计

计算状态估计协方差

以上步骤得到的是k时刻的无约束的滤波结果。

进一步的，上标“u”代表无约束滤波过程中涉及的矩阵和向量。

进一步的，所述步骤8(2)给出的序贯处理形式目的地滤波方法，具有较高的稳定性和精度。

实施例九

如上述实施例八所述的目的地约束下目标跟踪方法，本实施例与其不同之处在于，所述目的地约束下目标跟踪方法还包括步骤9：

在所述步骤8(2)的基础上进行第二次滤波，根据所述步骤4中给出的伪量测方式写出新的量测方程为：

其中为只包含伪量测的量测向量，其对应的测量噪声协方差矩阵为 为表征量测与状态关系的函数；

系统状态方程保持不变，根据序贯滤波思想，将所述步骤8(2)中滤波得到的状态估计和协方差作为一步预测结果代入到滤波过程中，即

状态一步预测

状态一步预测协方差

然后进行无迹变换：

计算基于选择的2n+1个δ采样点

根据量测方程，计算量测预测对应的2n+1个后验δ采样点

根据采样点，计算预测量测均值

计算预测量测对应的协方差矩阵

计算量测和状态向量的交互协方差

计算滤波增益

最后更新状态估计及协方差：

计算状态估计

计算状态估计协方差

其中n为状态向量维数，i＝0,1,...,2n，

和分别是根据δ采样点计算均值和协方差时对应的权值，可以通过以下公式得到：

其中α，β和κ是经验参数，α用来决定δ采样点在随机量均值附近的散布情况，β用来引入随机量分布的先验知识，κ是比例参数；

进而得到k时刻的约束状态估计结果。

进一步的，本实施例中所述伪量测形式与所述步骤4中给出的伪量测方式相同，不受量测噪声影响，与状态分量间是非线性关系。

进一步的，本实施例中上标“p”代表只与伪量测相关，与雷达位置量测无关的向量、矩阵和函数。

实施例十

如上述所述的目的地约束下目标跟踪方法，本实施例与其不同之处在于，本发明包含两种不同形式，即增广量测形式和序贯处理形式；

所述增广量测形式通过引入增广量测进行滤波，即将位置量测和伪量测在同一量测向量中同时处理；

所述序贯处理形式首先处理位置量测然后再单独处理伪量测；

结合图2和图3所示，图2、图3示出了两种形式分别对应的滤波模块功能框图。其中增广量测形式的目的地滤波方法主要通过以下步骤实现：

(1)获得目标的位置量测信息；

(2)根据目的地坐标和状态分量构造伪量测并将其增广到量测方程中；

(3)根据状态方程和增广后的量测方程进行滤波，得到约束滤波结果。

序贯处理形式的目的地滤波方法主要通过以下步骤实现：

(1)获得目标的位置量测信息；

(2)利用位置量测进行第一次滤波，得到无约束滤波结果；

(3)根据目的地坐标和状态分量构造伪量测，利用伪量测在无约束滤波结果的基础上进行第二次滤波，得到约束滤波结果。

实施例十一

如上述所述的目的地约束下目标跟踪方法，本实施例与其不同之处在于，为验证本发明的效果，利用仿真数据进行蒙特卡洛实验，仿真试验中的目标沿直线匀速运动，运动轨迹如图4所示。此时假设真实轨迹的先验信息是不完整的，因此无法采用传统约束估计方法来进行估计。这里采用一种未引入任何约束的标准无迹卡尔曼滤波方法和目的地约束滤波方法进行对比，仿真中设置雷达采样间隔为1s，仿真目标200s的运动，重复进行200次蒙特卡洛实验。

结合图5、图6所示，从图5和图6中可以明显看到引入目的地约束后误差明显减小，优化效果十分明显。这是由于目的地约束中包含关于目标状态的有用信息，增大了滤波器可利用的信息量，从而提高了滤波精度。而两种不同形式的目的地约束滤波方法中序贯处理形式相对性能较高，这是由于序贯处理将第一次滤波过程的估计结果作为第二次滤波过程中的一步预测，提高了预测精度，进而得到了更好的滤波精度。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种目的地约束下目标跟踪装置，其特征在于，包括：

中央处理模块；

观测雷达模块，其用于在跟踪过程中获得目标位置量测信息；

滤波模块，其用于对目标运动状态进行估计；

所述中央处理模块还包括：

目的地记录单元，其用于记录目的地坐标(x_n,y_n)，其中x_n和y_n为目的地x，y方向坐标；

转换单元，其用于将极坐标下的目标位置量测信息，转换到笛卡尔坐标系下，目标运动状态方程为：

x_k+1＝Φ_kx_k+Γ_kv_k

其中x_k是k时刻状态向量；x_k+1是k+1时刻状态向量，下标为对应时刻；Φ_k是状态转移矩阵；v_k是过程噪声向量，假设过程噪声是零均值方差已知的高斯白噪声，其协方差矩阵为cov(v_k)＝Q_k≥0；Γ_k是噪声分布矩阵；

伪量测单元，其用于根据目的地坐标(x_n,y_n)，和状态分量间确定的约束关系来构造伪量测：

其中x_n和y_n为目的地x，y方向坐标；和为目标运动状态中沿x，y方向速度分量；λ_k为伪量测；

量测单元，其用于构建量测和目标状态之间的关系的量测方程；

运动模型单元，其用于构建沿直线运动目标跟踪的运动模型的量测矩阵，所述运动模型的量测矩阵包括：近匀速模型(NCV)的量测矩阵和近匀加速模型(NCA)的量测矩阵。

2.一种基于权利要求1所述目标追踪装置实现的目的地约束下目标跟踪方法，其特征在于，所述目的地约束下目标跟踪方法包括如下步骤：

步骤1：在所述中央处理模块的目的地记录单元中记录目的地坐标(x_n,y_n)，其中x_n和y_n为目的地x，y方向坐标；

步骤2：在目标跟踪过程中，通过所述观测雷达模块获得目标位置量测信息，所述目标位置量测信息包括：目标相对雷达坐标系原点的距离量测和方位角量测将所述观测雷达模块获得的所述目标位置量测信息反馈至所述中央处理模块；

步骤3：在所述中央处理模块的转换单元中将极坐标下的目标位置量测信息，转换到笛卡尔坐标系下，在笛卡尔坐标系下，目标运动状态方程为：

x_k+1＝Φ_kx_k+Γ_kv_k

其中x_k是k时刻状态向量；x_k+1是k+1时刻状态向量，下标为对应时刻；Φ_k是状态转移矩阵；v_k是过程噪声向量，假设过程噪声是零均值方差已知的高斯白噪声，其协方差矩阵为cov(v_k)＝Q_k≥0；Γ_k是噪声分布矩阵；

步骤4：在所述中央处理模块的伪量测单元中，根据目的地坐标(x_n,y_n)，和状态分量间确定的约束关系来构造伪量测：

其中x_n和y_n为目的地x，y方向坐标；和为目标运动状态中沿x，y方向速度分量；λ_k为伪量测；

步骤5：得到笛卡尔坐标系下的伪量测后，在所述中央处理模块的量测单元中，构建量测和目标状态之间的关系的量测方程；

步骤6：在所述中央处理模块的运动模型单元中，构建沿直线运动目标的运动模型的量测矩阵，所述运动模型的量测矩阵包括：近匀速模型(NCV)的量测矩阵和近匀加速模型(NCA)的量测矩阵；

步骤7：通过所述滤波模块进行滤波初始化：采用两点差分法，即利用最初两个时刻k＝1、k＝2时刻的笛卡尔坐标系下位置量测值得到k＝2时刻的状态估计：

其对应的初始状态协方差矩阵为

其中和是笛卡尔坐标下的目标沿x，y方向的位置量测信息，是通过无偏量测转换方法将雷达位置量测转换到笛卡尔坐标系下得到的转换量测；转换公式为：

其中是从雷达获取的距离，方位角量测；是转换后得到的沿x，y方向的笛卡尔坐标量测，是转换后的量测向量；μ_θ是去偏系数，可通过方位角测量噪声方差求得：

对应的协方差矩阵为

其中R_k,xx，R_k,yy分别为x，y方向笛卡尔坐标量测对应的量测噪声方差，R_k,xy为x，y方向笛卡尔坐标量测对应的量测噪声之间的互协方差：

上标“c”代表与转换量测相关的向量、矩阵和函数；

步骤8：通过所述滤波模块从k＝3时刻开始滤波，进而得到k时刻的约束状态估计结果。

3.根据权利要求2所述的目的地约束下目标跟踪方法，其特征在于，所述步骤5中，得到笛卡尔坐标系下的伪量测后，在所述中央处理模块的量测单元中，构建量测和目标状态之间的关系的量测方程，具体为：

步骤5(1)：

将伪量测增广到量测向量中，得到量测方程为：

对应的量测噪声协方差矩阵为：

其中是k时刻量测向量；和分别是雷达获取的距离量测和方位角量测；是表征量测向量与状态向量间关系的函数；是量测噪声向量；和分别是距离和方位角量测对应的量测噪声；σ_r和σ_θ分别是距离和方位角量测对应的量测噪声标准差；R_k,rr和R_k,θθ分别是距离和方位角量测对应的量测噪声方差，R_k,rθ是距离和方位角量测之间的互协方差；R_k,λλ是伪量测对应的量测噪声方差，由于伪量测是一个常数，因此R_k,λλ＝0；R_k,rλR_k,θλ是伪量测与位置量测之间的互协方差，均为零；上标“a”代表增广的向量、矩阵和函数。

4.根据权利要求3所述的目的地约束下目标跟踪方法，其特征在于，所述步骤6中所述的近匀速模型(NCV)的量测矩阵和近匀加速模型(NCA)的量测矩阵，具体为：

步骤6(1)：

此时对应的状态向量分别为和x_k、y_k分别为沿x，y方向的位置分量，分别为沿x，y方向的速度分量；其中和分别为x，y方向的加速度分量；T为雷达扫描周期。

5.根据权利要求4所述的目的地约束下目标跟踪方法，其特征在于，经过所述步骤7的初始化滤波，在所述步骤8中，通过所述滤波模块从k＝3时刻开始滤波，其具体步骤如下：

步骤8(1)：

首先根据k-1时刻的约束状态估计对k时刻状态进行一步预测：

计算状态一步预测：

计算状态一步预测协方差：

然后进行无迹变换：

计算在附近选取的2n+1个δ采样点

根据量测方程，计算量测预测对应的2n+1个后验δ采样点

根据采样点，计算预测量测均值

计算预测量测对应的协方差矩阵

计算量测和状态向量的交互协方差

计算滤波增益

最后更新状态估计及协方差：

计算状态估计

计算状态估计协方差

P_k|k＝P_k|k-1-K_kP_k,zz(K_k)^T

其中n为状态向量维数，i＝0,1,...,2n，上标“a”代表增广的向量、矩阵和函数；

关于无迹变换，代表(n+λ)P_k|k-1矩阵的第j行，λ是一个尺度参数，λ＝α²(n+κ)-n，n+λ≠0；和分别是根据δ采样点计算均值和协方差时对应的权值，可以通过以下公式得到：

其中α，β和κ是经验参数，α用来决定δ采样点在随机量均值附近的散布情况，β用来引入随机量分布的先验知识，κ是比例参数。

6.根据权利要求2所述的目的地约束下目标跟踪方法，其特征在于，所述步骤5中，得到笛卡尔坐标系下的伪量测后，在所述中央处理模块的量测单元中，构建量测和目标状态之间的关系的量测方程，具体为：

步骤5(2)：

得到笛卡尔坐标系下的量测后，量测和目标状态之间的关系由量测方程给出：

其中是笛卡尔坐标下的量测向量，是表征笛卡尔坐标量测与状态关系的量测矩阵；是笛卡尔坐标系下量测对应的测量噪声，对应的测量噪声协方差矩阵为

7.根据权利要求6所述的目的地约束下目标跟踪方法，其特征在于，所述步骤6中所述的近匀速模型(NCV)的量测矩阵和近匀加速模型(NCA)的量测矩阵，具体为：

步骤6(2)：

此时对应的状态向量分别为和

8.根据权利要求7所述的目的地约束下目标跟踪方法，其特征在于，经过所述步骤7的滤波初始化，在所述步骤8中，通过所述滤波模块从k＝3时刻开始滤波，其具体步骤如下：

步骤8(2)：

根据状态方程，计算状态一步预测

计算状态一步预测协方差

根据量测方程，计算量测预测

计算量测预测对应的协方差

计算滤波增益

计算状态估计

计算状态估计协方差

进而得到的是k时刻的无约束的滤波结果。

9.根据权利要求8所述的目的地约束下目标跟踪方法，其特征在于，所述目的地约束下目标跟踪方法还包括步骤9：

在所述步骤8(2)的基础上进行第二次滤波，根据所述步骤4中给出的伪量测方式写出新的量测方程为：

其中为只包含伪量测的量测向量，其对应的测量噪声协方差矩阵为为表征量测与状态关系的函数；

系统状态方程保持不变，根据序贯滤波思想，将所述步骤8(2)中滤波得到的状态估计和协方差作为一步预测结果代入到滤波过程中，即

状态一步预测

状态一步预测协方差

然后进行无迹变换：

计算基于选择的2n+1个δ采样点

根据量测方程，计算量测预测对应的2n+1个后验δ采样点

根据采样点，计算预测量测均值

计算预测量测对应的协方差矩阵

计算量测和状态向量的交互协方差

计算滤波增益

最后更新状态估计及协方差：

计算状态估计

计算状态估计协方差

其中n为状态向量维数，i＝0,1,...,2n，

和分别是根据δ采样点计算均值和协方差时对应的权值，可以通过以下公式得到：

其中α，β和κ是经验参数，α用来决定δ采样点在随机量均值附近的散布情况，β用来引入随机量分布的先验知识，κ是比例参数；

进而得到k时刻的约束状态估计结果。