基于带噪声目的地信息约束的跟踪方法与流程

本发明涉及运动目标状态估计领域，具体而言，涉及一种基于带噪声目的地信息约束的跟踪方法。

背景技术

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

在实际应用中存在这样一种线性等式约束条件，其先验信息是不完整的，只有目标运动的目的地，即约束直线上的一个点的先验信息是已知的，我们把这种约束称为目的地约束。这种约束可用来估计反辐射导弹在水平笛卡尔坐标平面上的运动状态，由于其导引头的导引策略，在中途不改变打击目标的情况下，反辐射导弹在笛卡尔坐标系内的运动轨迹可以视作是指向静止打击目标的一条直线。而作为防守方，静止打击目标(例如雷达)的坐标我们是先验可知的。这种明显的运动特点可以作为先验约束信息引入跟踪系统用来提高估计精度。其他应用还包括重要设施的反炮击保护等。针对这类目的地约束目标跟踪问题，目前已有一些研究成果，如g.zhouandk.li,“stateestimationwithdestinationconstraints,”proceedingsof19thinternationalconferenceoninformationfusion,pp.292–297,2016(目的地约束下的状态估计)中利用量测点和目的地连线来构造近似的目的地约束，并通过伪量测将约束先验信息引入跟踪系统中，相比无约束目标跟踪方法取得了明显的性能提升。又如y.huang,x.wang,et.al.,“stateestimationwithincompletelinearconstraint,”proceedingsof20thinternationalconferenceoninformationfusion,pp.1524–1529,2017(非完整线性约束下的状态估计)中根据无约束方法的位置滤波结果和目的地连线构造近似的完整线性约束条件，然后利用投影方法得到约束滤波结果，提高了跟踪精度。

在实现本发明的过程中，发明人发现以上方法均假设目的地坐标是确定已知的，在实际目标跟踪应用场景中，我们能够获得的目的地坐标不一定总是完全准确的，可能会因测量噪声或其他因素的影响导致其与真实目的地坐标间存在偏差。在这种情况下，采用以上方法直接利用目的地坐标构造约束条件会引入较大的约束误差，导致滤波性能恶化甚至出现发散现象。

技术实现要素：

本发明提供一种基于带噪声目的地信息约束的跟踪方法，用以克服现有技术在目的地信息受噪声影响的情况下直接利用目的地坐标构造约束条件会引入较大的约束误差，导致滤波性能恶化甚至出现发散现象的问题。

本发明提供一种基于带噪声目的地信息约束的跟踪方法，包括以下步骤:

在笛卡尔坐标系下，对运动目标的状态进行建模，得到运动目标的状态方程；

将运动目标的目的地的笛卡尔坐标增广到运动目标的状态向量中作为新的状态分量，根据增广状态向量得到运动目标的增广状态方程；

根据所述增广状态向量各状态分量之间确定的约束关系构造伪量测；利用所述增广状态向量在滤波过程中同时估计运动目标的位置、速度和目的地坐标；

将所述伪量测增广到运动目标的量测向量中，得到运动目标的增广量测方程；

根据所述增广状态方程和所述增广量测方程进行滤波，并根据滤波结果更新运动目标的状态估计和状态估计协方差。

进一步地，所述运动目标的状态方程为：

xk+1＝φkxk+γkvk

其中xk是运动目标的运动状态向量，包含雷达扫描次数为k时沿x，y方向的位置分量xk、yk和速度分量φk是状态转移矩阵；vk是过程噪声向量，假设过程噪声是零均值方差已知的高斯白噪声，其协方差矩阵为cov(vk)＝qk≥0；γk是噪声分布矩阵。

进一步地，对于沿直线运动目标跟踪所采用的运动模型为近匀速模型ncv或近匀加速模型nca，对应的状态转移矩阵和噪声分布矩阵分别为：

ncv:

nca:

对应的状态向量分别为和t为雷达扫描周期。

进一步地，将运动目标的目的地的x，y方向笛卡尔坐标增广到运动目标的状态向量中作为新的状态分量，增广后的状态向量为

其中(xn,yn)为目的地的笛卡尔坐标；

对应增广后的状态向量，所述增广状态方程为：

假设真实的目的地坐标是静止不变的，不受过程噪声影响，则增广后的状态转移矩阵和噪声分布矩阵分别为：

ncv:

nca:

其中t为扫描间隔；

对应的增广后的过程噪声协方差矩阵为：

其中分别为x，y方向的过程噪声方差。

进一步地，所述伪量测为：

所述增广量测方程为：

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

其中和分别是距离和方位角量测对应的量测噪声，和是对应的量测噪声方差，由于假设位置量测之间是不相关的，因此互协方差rk,rθ＝0；由于伪量测是一个常数，因此其方差rk,λλ以及与位置量测之间的互协方差rk,rλ，rk,θλ均为零；上标“a”代表增广的向量、矩阵或函数。

进一步地，在滤波过程中采用无迹卡尔曼滤波方法进行滤波，根据所述增广状态方程和所述增广量测方程进行滤波，并根据滤波结果更新运动目标的状态估计和状态估计协方差包括：

首先在雷达扫描次数k＝1,2时进行滤波初始化，采用两点差分法，即利用最初两个扫描周期k＝1、k＝2的笛卡尔坐标系下运动目标的位置量测值得到k＝2时关于运动目标的位置和速度的状态估计：

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

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

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

对应的协方差矩阵为

其中rk,xx为转换后x方向的量测噪声方差，rk,yy为y方向的量测噪声方差，rk,xy为x，y方向量测噪声的互协方差；上标“c”代表与转换量测相关的向量、矩阵和函数；

对表示目的地坐标的状态分量进行初始化，假设已知的带有偏差的目的地坐标服从高斯概率密度分布，即

其中是真实且未知的目的地坐标，这里假设方差和是已知的；

根据已知的带噪声目的地坐标和其方差对状态分量进行初始化：

从k＝3时开始滤波：

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

计算状态一步预测：

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

然后进行无迹变换：

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

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

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

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

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

计算滤波增益

更新状态估计及其协方差：

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

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

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

本发明实现了以下有益效果：

本发明在已知的目的地坐标可能存在偏差的情况下，提出了一种基于带噪声目的地信息的滤波方法，同时对目标状态和目的地坐标进行估计，避免了直接引入带偏差目的地信息导致的滤波性能恶化的问题；通过有效利用目的地先验信息，提高了跟踪精度。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为目标运动的真实目的地和已知目的地(有偏差)示意图；

图2为基于带噪声目的地信息的约束目标跟踪方法流程图；

图3为仿真实验中构造的满足线性等式约束目标运动的真实轨迹示意图；

图4为利用无约束的无迹卡尔曼滤波方法及不考虑目的地偏差的目的地约束滤波方法这两种对比方法和使用本发明提出的方法分别对仿真目标进行跟踪得到的位置均方根误差对比结果示意图；

图5为三种滤波方法的速度均方根误差对比结果示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明针对实际目的地约束下跟踪应用场景中可能存在的目的地坐标受噪声影响的问题，提出了一种基于带噪声目的地信息约束的跟踪方法。

实施例1

如图1、图2所示，本实施例的基于带噪声目的地信息约束的跟踪方法，包括以下步骤:

在笛卡尔坐标系下，对运动目标的状态进行建模，得到运动目标的状态方程；

将运动目标的目的地的笛卡尔坐标增广到运动目标的状态向量中作为新的状态分量，根据增广状态向量得到运动目标的增广状态方程；

根据所述增广状态向量各状态分量之间确定的约束关系构造伪量测；利用所述增广状态向量在滤波过程中同时估计运动目标的位置、速度和目的地坐标；

将所述伪量测增广到运动目标的量测向量中，得到运动目标的增广量测方程；

根据所述增广状态方程和所述增广量测方程进行滤波，并根据滤波结果更新运动目标的状态估计和状态估计协方差。

在笛卡尔坐标系下，所述运动目标的状态方程可以建模为：

xk+1＝φkxk+γkvk

其中xk是运动目标的运动状态向量，包含雷达扫描次数为k时沿x，y方向的位置分量xk、yk和速度分量φk是状态转移矩阵；vk是过程噪声向量，假设过程噪声是零均值方差已知的高斯白噪声，其协方差矩阵为cov(vk)＝qk≥0；γk是噪声分布矩阵。

对于沿直线运动的目标跟踪所采用的运动模型为近匀速模型ncv或近匀加速模型nca，两种模型所对应的状态转移矩阵和噪声分布矩阵分别为：

ncv:

nca:

对应的状态向量分别为和t为雷达扫描周期。

将运动目标的目的地的x，y方向笛卡尔坐标增广到运动目标的状态向量中作为新的状态分量，增广后的状态向量为

其中(xn,yn)为目的地的笛卡尔坐标；

对应增广后的状态向量，所述增广状态方程为：

假设真实的目的地坐标是静止不变的，不受过程噪声影响，则增广后的状态转移矩阵和噪声分布矩阵分别为：

ncv:

nca:

其中t为扫描间隔；

对应的增广后的过程噪声协方差矩阵为：

其中分别为x，y方向的过程噪声方差。

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

根据目的地坐标状态分量(位置，速度，目的地位置)间确定的约束关系来构造伪量测：

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

接着将其增广到量测向量中，得到增广量测方程为：

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

其中和分别是距离和方位角量测对应的量测噪声，和是对应的量测噪声方差，由于假设位置量测之间是不相关的，因此互协方差rk,rθ＝0；由于伪量测是一个常数，因此其方差rk,λλ以及与位置量测之间的互协方差rk,rλ，rk,θλ均为零；上标“a”代表增广的向量、矩阵或函数。

实施例2

在滤波过程中采用无迹卡尔曼滤波方法进行滤波，根据所述增广状态方程和所述增广量测方程进行滤波，并根据滤波结果更新运动目标的状态估计和状态估计协方差包括：

首先在雷达扫描次数k＝1,2时进行滤波初始化，采用两点差分法，即利用最初两个扫描周期k＝1、k＝2的笛卡尔坐标系下运动目标的位置量测值得到k＝2时关于运动目标的位置和速度的状态估计：

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

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

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

对应的协方差矩阵为

其中rk,xx为转换后x方向的量测噪声方差，rk,yy为y方向的量测噪声方差，rk,xy为x，y方向量测噪声的互协方差；上标“c”代表与转换量测相关的向量、矩阵和函数；

对表示目的地坐标的状态分量进行初始化，假设已知的带有偏差的目的地坐标服从高斯概率密度分布，即

其中是真实且未知的目的地坐标，这里假设方差和是已知的；

根据已知的带噪声目的地坐标和其方差对状态分量进行初始化：

从k＝3时开始滤波：

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

计算状态一步预测：

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

然后进行无迹变换：

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

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

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

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

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

计算滤波增益

更新状态估计及其协方差：

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

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

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

实施例3

为验证本发明的效果，利用仿真数据进行蒙特卡洛实验。仿真试验中的目标在一维约束空间中近匀速运动，在笛卡尔坐标系中其位置和速度满足线性等式约束，其运动轨迹如图2所示。此时假设已知的目的地坐标服从高斯分布，其均值为真实目的地坐标位置，方差已知。这里采用未引入任何约束的标准无迹卡尔曼滤波方法与不考虑目的地偏差的目的地约束滤波方法作为对比方法。仿真中雷达扫描间隔为1s，仿真目标200s的运动，重复进行500次蒙特卡洛实验。

图4示出了三种方法位置估计结果的均方根误差对比，图5示出了速度估计结果的均方根误差对比。从图4和图5中可以明显看到，相比无约束方法，基于带噪声目的地信息的约束滤波方法滤波误差明显减小，性能提升明显。这是由于本方法成功在跟踪系统中引入了目的地先验信息，这些先验信息中包含有关于目标状态的有用信息，增大了滤波器可利用的信息量，从而提高了滤波精度。而在图4中可以看到，利用现有的目的地约束跟踪方法跟踪仿真目标时出现了严重的性能恶化，这是由于该方法直接利用带偏差的目的地坐标构造伪量测，此时得到的伪量测无法准确的描述真实约束关系，引入到跟踪系统中会导致滤波结果向错误的直线进行投影，偏离目标真实状态。

与现有技术相比，本发明具有以下有益效果：

(1)将目的地的笛卡尔坐标增广到状态向量中作为新的状态分量，利用增广的状态向量在滤波过程中同时估计目标位置，速度和目的地坐标，进而根据状态分量之间的关系构造伪量测来描述目的地约束关系。克服了现有技术在目的地信息受噪声影响的情况下直接利用目的地坐标构造伪量测会引入较大约束误差，导致滤波性能恶化的问题。

(2)提出一种有效的增广状态滤波方法，在滤波过程中假定真实目的地坐标一直保持不变且不受过程噪声影响；在滤波初始化阶段，利用部分已知的目的地坐标概率分布信息对目的地坐标状态分量进行初始化，将目的地先验信息有效地引入跟踪系统中从而提高跟踪精度。

以上说明对本发明而言只是说明性的，而非限制性的，本领域普通技术人员理解，在不脱离以下所附权利要求所限定的精神和范围的情况下，可做出许多修改、变化或等效，但都将落入本发明的保护范围内。