基于自适应滤波的机动目标跟踪方法

摘要

本发明公开了一种基于自适应滤波的机动目标跟踪方法，包括以下步骤：S1、根据IMU传感器得到的观测残差和控制输入的特性，判断跟踪目标的机动状态；S2、修正上一时刻的误差协方差矩阵以及系统状态；S3、对系统状态向量进行预测，对系统状态误差协方差矩阵进行预测；S4、计算卡尔曼滤波增益矩阵；S5、对系统状态和系统状态误差协方差进行更新；S6、结束当前时刻对跟踪目标位置与速度信息的跟踪并开始预测下一时刻状态，即跳转到步骤S1并继续执行后续步骤。本发明可以针对工作环境自适应滤波，以实现比经典卡尔曼滤波系统更精确的预测跟踪。

说明书

技术领域

本发明属于目标追踪技术领域，具体涉及一种基于自适应滤波的机动目标跟踪方法。

背景技术

机动目标跟踪技术在二次世界大战的时候就已经成为了科学家们重点研究的热门课题之一。随着现代科学和技术的进一步改变和发展，在军事领域上也逐渐开始引用了机动目标跟踪技术。然而，随着目标状态变得越来越复杂，机动目标跟踪算法也在不断更新和发展，但并不是所有的目标跟踪算法都能应对机动目标复杂的运动情况。所以，如何改进机动目标跟踪方法，改善其在复杂应用场合的跟踪效果，它一直都是机动目标跟踪技术研究领域中最为备受重视和关注的科学技术问题之一，不仅在学术上具有十分重要的理论意义，还有广泛的科学技术应用和研究前景。

发明内容

本发明的主要目的在于克服现有技术的缺点与不足，提出一种基于自适应滤波的机动目标跟踪方法，可以针对机动目标复杂的运动情况，有效地完成机动目标的跟踪。

为了达到上述目的，本发明采用以下技术方案：

基于自适应滤波的机动目标跟踪方法，包括以下步骤：

S1、根据IMU传感器得到的观测残差和控制输入的特性，判断跟踪目标的机动状态；

S2、修正上一时刻的误差协方差矩阵以及系统状态；

S3、对系统状态向量进行预测，对系统状态误差协方差矩阵进行预测；

S4、计算卡尔曼滤波增益矩阵；

S5、对系统状态和系统状态误差协方差进行更新；

S6、结束当前时刻对跟踪目标位置与速度信息的跟踪并开始预测下一时刻状态，即跳转到步骤S1并继续执行后续步骤。

进一步的，所述步骤S1具体为：

将检测目标运动状态是否机动转化为假设检验问题：

设定H

如果跟踪目标没有发生机动，即处于非机动状态下，它的观测残差z～符合零均值的分布，并且机动状态的输入状态也是零均值随机过程，基于此特点来判断机动是否发生；

根据观测残差和控制输入特性，在机动目标跟踪中使用χ

假设在H

其中，α取为0.05或0.01，α表示置信度，

δ＝ε

根据残差平方定义

设计两种统计量来进行假设检验，一种是在长度为s的滑动窗口[t-s+1，t]内的和

其中

s＝1/(1-ρ)

其中，0＜ρ＜1，s为衰减记忆和的等效窗口，ρ表示加权衰减因子，s为衰减记忆和的等效窗口；

在H

进一步的，所述修正上一时刻的误差协方差矩阵具体为：

若运动状态从“机动”变化为“非机动”，则需要修正当前时刻K时刻的误差协方差矩阵：

其中，P

若运动状态为“非机动”变化为“机动”，则需要修正当前时刻K时刻的误差协方差矩阵：

其中，P

若运动状态从“机动”变化为“机动”或从“非机动”变化为“非机动”则不需要修正。

进一步的，所述修正上一时刻的系统状态具体为：

若运动状态从“机动”变化为“非机动”，则跟踪目标的状态变量用矩阵可表示为：

其中，X

若运动状态从“非机动”变化为“机动”，则跟踪目标的状态变量用矩阵可表示为：

其中，z

进一步的，所述对系统状态向量进行预测具体为：

h(K+1，K)表示在K+1时刻卡尔曼滤波系统的状态输出转移矩阵；

若运动状态为“非机动状态”，卡尔曼滤波系统选用CV模型；

卡尔曼滤波系统的状态输出转移矩阵表示为

若运动状态为“机动状态”，卡尔曼滤波系统选用CA模型；

卡尔曼滤波系统的状态输出转移矩阵表示为

进一步的，所述对系统状态误差协方差矩阵进行预测具体为：

P[K+1/K]＝h(K+1，K)P(K/K)h

其中，Q

进一步的，所述步骤S4具体为：

卡尔曼滤波增益矩阵具体为：

K(K+1)＝P[K+1/K]C

其中，C(K)表示卡尔曼滤波系统的输出矩阵，R表示测量噪声的方差；

若运动状态为“非机动状态”，卡尔曼滤波系统选用CV模型；

卡尔曼滤波系统的输出转移矩阵表示为

系统观测噪声方差矩阵表示为

若运动状态为“机动状态”，卡尔曼滤波系统选用CA模型；

卡尔曼滤波系统的输出转移矩阵表示为

系统观测噪声方差矩阵表示为

进一步的，所述对系统状态进行更新具体为：

所述对系统状态误差协方差矩阵更新具体为：

P[(K+1)/(K+1)]＝[1-K(K+1)C(K+1)]P[K+1/K]。

本发明与现有技术相比，具有如下优点和有益效果：

1、本发明可以针对工作环境自适应滤波，以实现比经典卡尔曼滤波系统更精确的预测跟踪；由于加入外部变量以及控制矩阵，本方法可以在非线性运动状态下稳定工作。

2、当确定目标机动时，本发明方法自适应地调整目标运动模型，并且调整系统的协方差矩阵。

附图说明

图1是基于自适应滤波算法的机动目标跟踪方法流程图；

图2是机动目标的理想运动轨迹；

图3是应用经典卡尔曼滤波算法对机动目标跟踪的效果图；

图4a和图4b是应用经典卡尔曼滤波算法对机动目标跟踪误差的均值和标准差；

图4c和图4d是应用经典卡尔曼滤波算法对机动目标跟踪误差的标准差；

图5是应用自适应滤波算法对机动目标跟踪的效果图；

图6a和图6b是本发明方法对机动目标跟踪误差的均值；

图6c和图6d是本发明方法对机动目标跟踪误差的标准差。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

实施例

在本实施例中，以对机器人现实位置以及速度信息的跟踪为例；如图1所示，基于自适应滤波的机动目标跟踪方法，包括以下步骤：

S1、根据IMU传感器得到的观测残差和控制输入的特性，判断跟踪目标的机动状态，具体为：

将检测目标运动状态是否机动转化为假设检验问题：

设定H

如果跟踪目标没有发生机动，即处于非机动状态下，它的观测残差z～符合零均值的分布，并且机动状态的输入状态也是零均值随机过程，基于此特点来判断机动是否发生；

根据观测残差和控制输入特性，在机动目标跟踪中使用χ

假设在H

其中，α取为0.05或0.01，α表示置信度，

δ＝ε

根据残差平方定义

设计两种统计量来进行假设检验，一种是在长度为s的滑动窗口[t-s+1，t]内的和

其中

s＝1/(1-ρ)

其中，0＜ρ＜1，s为衰减记忆和的等效窗口，ρ表示加权衰减因子，s为衰减记忆和的等效窗口；

在H

S2、修正上一时刻的误差协方差矩阵以及系统状态，包括：

S21、修正上一时刻的误差协方差矩阵，具体为：

若运动状态为“机动”变化为“非机动”，则需要修正当前时刻K时刻的误差协方差矩阵：

其中，P

若运动状态为“非机动”变化为“机动”，则需要修正当前时刻K时刻的误差协方差矩阵：

其中，P

若运动状态为“机动”变化为“机动”或“非机动”变化为“非机动”则不需要修正。

S21、修正上一时刻的系统状态，具体为：

若运动状态为“机动”变化为“非机动”，则机器人的状态变量用矩阵可表示为：

其中，X

若运动状态为“非机动”变化为“机动”，则机器人的状态变量用矩阵可表示为：

其中，z

S3、对系统状态向量进行预测，对系统状态误差协方差矩阵进行预测，包括：

S31、对系统状态向量进行预测，具体为：

h(K+1，K)表示在K+1时刻卡尔曼滤波系统的状态输出转移矩阵。

若运动状态为“非机动状态”，卡尔曼滤波系统选用CV模型(匀速运动)

卡尔曼滤波系统的状态输出转移矩阵表示为

若运动状态为“机动状态”，卡尔曼滤波系统选用CA模型(匀加速运动)

卡尔曼滤波系统的状态输出转移矩阵表示为

S32、对系统状态误差协方差矩阵进行预测，具体为：

P[K+1/K]＝h(K+1，K)P(K/K)h

其中，Q

S4、计算卡尔曼滤波增益矩阵，具体为：

卡尔曼滤波增益矩阵具体为：

K(K+1)＝P[K+1/K]C

其中，C(K)表示系统的输出矩阵，R表示测量噪声的方差；

若运动状态为“非机动状态”，卡尔曼滤波系统选用CV模型(匀速运动)

卡尔曼滤波系统的输出转移矩阵表示为

系统观测噪声方差矩阵表示为

若运动状态为“机动状态”，卡尔曼滤波系统选用CA模型(匀加速运动)

卡尔曼滤波系统的输出转移矩阵表示为

系统观测噪声方差矩阵表示为

S5、对系统状态和系统状态误差协方差进行更新，具体为：

S51、对系统状态进行更新：

S52、对系统状态误差协方差矩阵更新：

P[(K+1)/(K+1)]＝[1-K(K+1)C(K+1)]P[K+1/K]。

S6、结束当前时刻对跟踪目标位置与速度信息的跟踪并开始预测下一时刻状态，即跳转到步骤S1并继续执行后续步骤。

在本实施例中，对MATLAB搭建的机动检测滤波系统进行迭代计算，假定在一个平面坐标上有一个雷达对机动目标进行观测，跟踪目标从坐标为(2000米，10000米)的点开始出发，在0-400秒，跟踪目标沿着X轴方向，做运动速度为15米/S的匀速直线运动，在t＝400-600秒，X和Y轴方向有一恒定的加速度加入，沿着Y轴方向开始做转弯运动，目标在X和Y轴方向的加速度大小为-0.075米/S，在t＝610秒，X和Y轴方向有一恒定的加速度加入，目标沿着X轴方向开始快转弯运动，目标在X和Y轴方向的加速度大小为0.3米/S，在t＝660秒，目标结束转弯。雷达每隔2秒进行一次扫描，分别对X和Y进行观测，X和Y的观测噪声的标准差为100米；

图2是机动目标的理想运动轨迹；图3是应用经典卡尔曼滤波算法对机动目标跟踪的效果图；图4a、图4b、图4c以及图4d是该算法对机动目标跟踪误差的均值和标准差。从图3、图4a、图4b、图4c以及图4d可以看出，当机动目标处于线性运动时，跟踪效果良好，即在0-200的采样次数中，跟踪误差较小；但机动目标发生机动时，跟踪效果较差，即在200-400的采样次数中，跟踪误差较大。图5是应用自适应滤波算法对机动目标跟踪的效果图；图6a、图6b、图6c以及图6d是该算法对机动目标跟踪误差的均值和标准差。从图5、图6a、图6b、图6c以及图6d可以看出，当机动目标处于线性运动时，跟踪效果良好，即在0-200的采样次数中，跟踪误差较小；机动目标发生机动时，跟踪效果良好，即在200-400的采样次数中，跟踪误差减小。

通过图4a、图4b、图4c以及图4d和图6a、图6b、图6c以及图6d可以看出，自适应滤波算法对机动目标的跟踪效果，比经典卡尔曼滤波算法的跟踪效果是要好，在采样次数0-200之间，此时机动目标处于恒速运动状态，两个算法对机动目标的跟踪误差都是较小；在采样次数200-400之间，此时机动目标处于加速状态，自适应滤波算法对机动目标的位置跟踪的误差范围，即XY坐标的均值和标准差的范围，要优于经典卡尔曼滤波算法。

还需要说明的是，在本说明书中，诸如术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其他实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。