CN111693984A - 一种改进的ekf-ukf动目标跟踪方法 - Google Patents

Abstract

本发明涉及一种改进的EKF‑UKF动目标跟踪方法，所述的方法包括如下步骤：建立多种运动状态模型相结合的模型集，既包括机动性低的运动模型(模型1)，又包含机动性高的变加速运动模型(模型2)；建立运动方程和观测方程；探测器对目标观测得到目标的观测量并计算运动目标的角度变化率

判断阈值和

的大小关系选择执行两种不同的跟踪滤波算法。当角度变化量较小表明运动目标的机动性较弱，为了降低工程实现的计算复杂度、节省系统资源、满足实时性要求，采用EKF算法进行目标跟踪。当角度变化量较大时表明运动目标的机动性较强，采用UKF算法满足目标跟踪的精度要求。本发明能兼顾EKF在机动性较弱时计算速度快和UKF在机动性较强时计算精度高的优点。

Description

一种改进的EKF-UKF动目标跟踪方法

技术领域

本发明涉及目标跟踪领域，具体涉及一种改进的EKF-UKF动目标跟踪方法。

背景技术

动目标跟踪在军事和民用领域都具有广泛的应用，目标跟踪技术就是利用声呐、雷达等各类传感器对目标进行观测，得到目标的位置、速度、角度等信息，根据目标不同的运动状态建立相应的运动模型，然后通过一系列的滤波器实现对目标的位置、速度、角度等信息的准确评估和精确预测。

在雷达对机动目标跟踪的技术中，广泛地采用了卡尔曼滤波及各种基于卡尔曼滤波改进的方法。算法方程一个是运动方程，另一个是观测方程。滤波围绕一系列递推公式展开，计算更新滤波增益，完整的滤波过程在重复递推卡尔曼滤波公式下完成。Kalman最初提出的滤波基本理论只适用于线性系统，要求观测方程也必须是线性的，对线性系统来说，这个算法是最优的算法，在保证精度的情况下，收敛速度和稳定性方面也表现很好。但是对于非线性系统，传统卡尔曼滤波方法适用性较差。

在此后的多年间，Bucy等人致力研究Kalman滤波理论在非线性系统和非线性观测下的扩展Kalman滤波(Extended Kalman Filter，EKF)，扩展卡尔曼滤波是一种应用广泛的非线性系统滤波方法。这种滤波器的思想是将非线性系统一阶线性化，对于非线性的状态方程和观测方程，使用传统的卡尔曼滤波器已经无法完成这个滤波任务。EKF将非线性关系进行线性近似，进而转化为线性问题，然后使用标准Kalman滤波进行跟踪。其中最常用的线性化方法是泰勒展开，忽略了高次项。但是这样做会在线性化过程中带来近似误差。为了解决线性化之后引入近似误差的问题，S.Julier提出了无迹Kalman滤波(Unscented KalmanFilter，UKF)，中文释义为无损Kalman滤波、无迹Kalman滤波或去芳香Kalman滤波。

无迹卡尔曼滤波以UT变换为基础，采用Kalman线性滤波的框架，摒弃了对非线性函数进行线性化的传统做法。对于一步预测方程，使用UT变换来处理均值和协方差的非线性传递，其基本思想是近似一个概率密度函数比近似一个非线性随机函数简单，无迹卡尔曼滤波在一组采样点中进行选择，保证这些采样点的协方差、均值和高阶项符合高斯随机变量。无迹卡尔曼滤波在本质上仍然是递推算法，它使用高斯随机变量来比表示状态分布，使用UT变换得到一组点，这组点作为状态变量的概率分布的近似，高斯随机变量的均值和协方差用点来逼近，然后采用卡尔曼滤波对系统的状态进行估计。扩展卡尔曼滤波只采用了泰勒展开的第一项，这样就只精确到一项。而无迹卡尔曼滤波不需要将非线性的状态方程和观测方程进行线性化处理，而是不去改变非线性系统的方程，不需要像EKF那样要计算Jacobian矩阵，不需要忽略高阶项，直接计算系统的概率密度函数。所以它的计算精度较高。

然而UKF算法也不是完美的，事实上只有最为匹配的滤波算法，对于机动性较弱的运动目标来说，使用UKF算法的计算量大于EKF算法，消耗的资源较多。因此在满足跟踪精度的情况下优先采用EKF算法。而对机动性较强的运动目标进行跟踪滤波时，为了准确地跟踪目标，选择计算复杂但是精度高的UKF算法。因此研究自适应、跟踪准确的跟踪滤波算法具有重要意义。

发明内容

针对上述问题，为了兼顾目标跟踪的精度和计算量小，本发明提供了一种改进的EKF-UKF动目标跟踪方法，其特征在于，所述方法包括如下步骤：

(1)、建立多种运动状态模型相结合的模型集，既包括机动性低的匀速运动模型，又包含机动性高的变加速运动模型，建立运动方程和观测方程，根据观测方程得到目标的观测量；

(2)、设置阈值，系统完成初始化后，探测器连续收集6个测量数据，将相邻两时刻的角度测量值作差，得到角度变化率

记5个角度变化率中大于阈值的个数为N。

(3)、若N≥3，则将运动目标初始状态信息导入变加速运动模型；若N＜3，则将运动目标初始状态信息导入匀速运动模型；

(4)、对于N＜3，采用扩展卡尔曼滤波得到运动状态信息的滤波值；对于N≥3，采用无迹卡尔曼滤波得到运动状态信息的滤波值；

(5)、执行选定的跟踪滤波算法，得到目标的预测运动轨迹。

在一些实施例中，在步骤(1)中，所述观测方程中包括观测矩阵，观测矩阵中的观测量包括目标的位置信息、速度信息和角度信息。

在一些实施例中，在步骤(1)中，所述运动方程中包括状态转移矩阵。

在一些实施例中，步骤(2)的具体步骤包括：

(2-1)、利用雷达初期获取的几组数据计算角度变化信息；

(2-2)、比较角度变化量与设置阈值的大小关系。

上述的目标跟踪滤波方法中，在步骤(4)中，对于N＜3的情况，跟踪滤波采用扩展卡尔曼滤波算法，此时目标的运动方程为：

X(k)＝ΦX(k-1)+W(k-1) (1)

此时目标的观测方程为：

Z(k)＝HX(k-1)+V(k-1) (2)

其中，X(k)为k时刻的系统状态，Φ为所述系统状态转移参数，对于多模型系统，Φ是矩阵；Z_k为k时刻的观测值，H为所述系统测量参数，对于多测量系统，H是矩阵；W_k是均值为0且噪声协方差为Q的高斯白噪声，V_k是均值为0且噪声协方差为R的高斯白噪声；

采用下式预测下一时刻状态：

其中

为k-1时刻最优的预测结果，

为k-1时刻至k时刻的预测结果；

使用P表示协方差，并采用式(4)计算：

P(k|k-1)＝ΦP(k-1|k-1)Φ^T+Q (4)

其中，P(k|k-1)为k-1时刻至k时刻预测的协方差矩阵，P(k-1|k-1)为k-1时刻的协方差矩阵，Q为系统过程的协方差；

结合预测值和观测值，可以得到k时刻的最优估计：

式(5)中，K(k)表示k时刻的卡尔曼滤波增益，K(k)采用下式计算：

其中，S(k)为新息方差矩阵，R为测量噪声方差。

上述的目标跟踪滤波方法中，在步骤(4)中，对于N≥3的情况，跟踪滤波采用无迹卡尔曼滤波算法，此时目标的运动方程为：

X(k)＝f(x(k-1),W(k-1)) (7)

此时目标的观测方程为：

Z(k)＝h(x(k-1),V(k-1)) (8)

其中，f是非线性运动方程函数；h是非线性观测方程函数；设W(k)具有协方差Q，V(k)具有协方差R；

采用式(9)获得一组采样点，称为Sigma点集；

其中，

表示矩阵方根的第i列；

采用式(10)求得其对应权值：

其中，下标m为均值，c为协方差，上标为第几个采样点。参数λ是一个缩放比例参数，用来降低总的预测误差，数学表达式为λ＝a²(n+κ)-n，a的选取控制了采样点的分布状态，κ为待选参数，其具体取值虽然没有界限，但通常应确保矩阵(n+λ)P为半定矩阵；待选参数β≥0是一个非负的权系数，它可以合并方程中高阶项的动差，这样就可以把高阶项的影响包括在内。

在一些实施例中，所述的采样点表示为式(11)：

采用式(12)计算所述的2n+1个Sigma点集的一步预测及其协方差：

其中，i＝1,2,…,2n+1。

根据一步预测值，再次使用UT变换，得到新的Sigma点集为：

采用式(14)计算预测的观测量为：

Z⁽ⁱ⁾(k|k-1)＝h[X⁽ⁱ⁾(k|k-1)],i＝1,2,…,2n+1 (14)。

在一些实施例中，所述的系统预测的均值及协方差为：

在一些实施例中，所述的无迹卡尔曼滤波增益矩阵为：

在一些实施例中，所述的状态更新和协方差更新为：

根据以上的方案分析，本发明实施例具有以下优点：

在本发明的实施例中，通过收集的一组观测信息，总体上把握跟踪目标的运动状态，然后选择与跟踪目标匹配度更高的运动模型和跟踪滤波算法，有效地平衡了扩展卡尔曼滤波精度的不足和无迹卡尔曼滤波计算量大的矛盾。本发明提供的跟踪滤波方案，既能保证跟踪滤波的精度，又能满足系统的实时性要求，实现高效地跟踪滤波，是一种稳定、简易的跟踪方法。

附图说明

图1为本发明实施例的一种改进的EKF-UKF动目标跟踪方法的流程图；

图2为机动性较弱时EKF和UKF的跟踪轨迹；

图3为机动性较弱时EKF和UKF的跟踪误差对比；

图4为机动性较强时EKF和UKF的跟踪轨迹；

图5为图4的局部放大；

图6为机动性较强时两种跟踪滤波方法距离误差对比；

图7为机动性较强时两种跟踪滤波方法速度误差对比。

具体实施方式

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

如图1所示，本发明实施例中提供一种结合EKF和UKF算法的跟踪方法流程图，下面来具体介绍。

建立跟踪目标的运动模型，按机动性强弱来划分，两种模型分别采用不同的跟踪算法进行滤波，机动性较弱的采用EKF算法跟踪，机动性较强的采用UKF算法跟踪。

系统初始化完成后，探测器收集一组机动目标的观测信息，计算目标的角度变化量

根据前文所述的方法选择合适的运动模型和滤波算法进行跟踪。

首先以匀速直线运动系统为例，假设机动目标的速度为v，目标在k时刻的位置为x(k)，经过采样时间T后，目标的位置变为x(k)+vT。

将目标的状态信息扩展到3个，即位置x(k)、速度v(k)、加速度u(k)。仿真基于笛卡尔坐标系，目标具有x方向和y方向两个分量，根据运动状态模型写出系统的运动方程为：

将表达式写成向量表达式为：

目标k-1时刻受到的随机扰动用U(k-1)表示，因此系统的运动方程表示为：

X(k)＝ΦX(k-1)+ΓU(k-1)

其中，

探测器对运动目标进行探测，假设雷达的坐标为(x₀,y₀)，运动目标在k时刻的位置为(x(k),y(k))，雷达通过发射电磁波并接收有效回波测量雷达与运动目标的距离，这个距离就是观测量Z(k)，设传感器的随机量测噪声为V(k)，那么观测量的方程表示为：

以上推导了运动方程和观测方程，建立了匀速运动的系统模型，由于该运动目标的机动性较弱，系统采用扩展卡尔曼滤波算法进行跟踪，为了对比同等条件下EKF和UKF的跟踪效果，这里同时仿真了两种算法，对比两种方法的跟踪性能和运算时间。

可以看出，跟踪系统的运动方程是线性的，观测方程是非线性的。

初始化参数、均值和协方差，初始状态设置为X(0)＝[-100,2,20,200]^T，假设运动目标的随机过程噪声为W(k)，其协方差矩阵为Q，运动目标的观测噪声为V(k)，其协方差表示为R，W(k)和V(k)是不相关的高斯白噪声，分别表示如下：

方法一：采用扩展卡尔曼滤波器对目标进行跟踪，为了将观测方程线性化需要计算相应的Jacobian矩阵。

编写仿真程序，位置估计效果如图2所示，位置偏差的绝对值如图3所示。

方法二：采用无迹卡尔曼滤波器进行目标跟踪，以下是该方法的具体描述。

步骤1：设状态向量x为n维随机变量，已知其均值

和协方差P，通过下面的计算方法UT变换得到2n+1个Sigma点X和相应的权值ω来计算y的统计特性。

权值的计算方法为：

算法参数初始化为α＝1,β＝2,κ＝0,λ＝-1。

利用上面的式子获得Sigma点集。

步骤2：对步骤1得到的2n+1个Sigma点集进行一步预测。

X⁽ⁱ⁾(k|k-1)＝f[k-1,X⁽ⁱ⁾(k-1|k-1)]

步骤3：更新系统状态向量的一步预测及协方差矩阵，通过计算Sigma点集的加权和得到。

步骤4：使用更新的一步预测值，再次使用UT变换得到新的Sigma点集。

步骤5：计算观测量的预测值。

Z⁽ⁱ⁾(k|k-1)＝h[X⁽ⁱ⁾(k|k-1)],i＝1,2,…,2n+1

步骤6：计算观测量的先验估计值，观测量自身的协方差，状态量的先验估计值和观测量的先验估计值之间的协方差。

步骤7：根据步骤6中观测量自身的预测值的协方差

状态量的先验估计值和观测量的先验估计值之间的协方差

得到卡尔曼滤波增益的更新值。

步骤8：计算系统的状态最优估计和协方差最优估计：

按照以上步骤所描述的方法编写程序，位置估计效果如图2所示，位置偏差的绝对值如图3所示。

该模型仿真的两个算法独立运行50次，然后计算EKF算法单次运行的平均时间为0.73162秒，UKF算法单次运行的平均时间为1.53266秒。结合图2以及图3可以看出，在跟踪机动性较弱的的运动目标时两种算法的性能相差不大，因此对工程实现来说，如果不考虑系统的非线性强度、环境要求等因素，可以优先采用EKF算法。

接下来以机动性较强的运动目标作为研究对象，分析两种算法的跟踪性能和计算时间。

假设目标在三维平面内运动，k时刻位置、速度以及加速度表示为：

X(k)＝[r_x(k) r_y(k) r_z(k) v_x(k) v_y(k) v_z(k) a_x(k) a_y(k) a_z(k)]

假设运动具有系统噪声W(k)，那么目标的运动方程表示为：

X(k)＝f_k-1(X(k-1),W(k-1))

一般来说，上述方程是线性的，可以表示为：

X(k)＝ΦX(k-1)+ΓU(k-1)+W(k-1)

其中，

目标的观测量方程为：

Z(k)＝h[X(k)]+V(k)

其中，

按照上文所述的步骤，先计算Jacobian矩阵：

目标的初始状态：

x(0)＝[3500 1500 1000 -1100 -150 -50 10 10 10]^T

设定其他参数的初始值，按照上文所述的步骤编写EKF和UKF的仿真程序，两种算法的跟踪轨迹如图4所示，跟踪轨迹的局部放大如图5所示，两种算法的距离误差对比如图6所示，两种算法的速度误差对比如图7所示。可以看出，当系统的非线性强度增大时EKF的估计精度明显不如UKF，UKF能更好的适应中强性的非线性环境。

Claims (10)

1.一种改进的EKF-UKF动目标跟踪方法，其特征在于，所述方法包括如下步骤：

(1)、建立多种运动状态模型相结合的模型集，既包括机动性低的匀速运动模型，又包含机动性高的变加速运动模型，建立运动方程和观测方程，根据观测方程得到目标的观测量；

(2)、设置阈值，系统完成初始化后，探测器连续收集6个测量数据，将相邻两时刻的角度测量值作差，得到角度变化率

记5个角度变化率中大于阈值的个数为N。

(3)、若N≥3，则将运动目标初始状态信息导入变加速运动模型；若N＜3，则将运动目标初始状态信息导入匀速运动模型；

(4)、对于N＜3，采用扩展卡尔曼滤波得到运动状态信息的滤波值；对于N≥3，采用无迹卡尔曼滤波得到运动状态信息的滤波值；

(5)、执行选定的跟踪滤波算法，得到目标的预测运动轨迹。

2.如权利要求1所述的方法，其特征在于，在步骤(1)中，所述观测方程中包括观测矩阵，观测矩阵中的观测量包括目标的位置信息、速度信息和角度信息。

3.如权利要求2所述的方法，其特征在于，在步骤(1)中，所述运动方程中包括状态转移矩阵。

4.如权利要求1所述的方法，其特征在于，步骤(2)包括：

(2-1)、利用雷达初期获取的几组数据计算角度变化信息；

(2-2)、比较角度变化量与设置阈值的大小关系。

5.如权利要求1所述的方法，其特征在于，在步骤(4)中，对于N＜3的情况，跟踪滤波采用扩展卡尔曼滤波算法，此时目标的运动方程为：

X(k)＝ΦX(k-1)+W(k-1) (1)

此时目标的观测方程为：

Z(k)＝HX(k-1)+V(k-1) (2)

其中，X(k)为k时刻的系统状态，Φ为所述系统状态转移参数，对于多模型系统，Φ是矩阵；Z_k为k时刻的观测值，H为所述系统测量参数，对于多测量系统，H是矩阵；W_k是均值为0且噪声协方差为Q的高斯白噪声，V_k是均值为0且噪声协方差为R的高斯白噪声；

采用下式预测下一时刻状态：

其中

为k-1时刻最优的预测结果，

为k-1时刻至k时刻的预测结果；

使用P表示协方差，并采用式(4)计算：

P(k|k-1)＝ΦP(k-1|k-1)Φ^T+Q (4)

其中，P(k|k-1)为k-1时刻至k时刻预测的协方差矩阵，P(k-1|k-1)为k-1时刻的协方差矩阵，Q为系统过程的协方差；

结合预测值和观测值，可以得到k时刻的最优估计：

式(5)中，K(k)表示k时刻的卡尔曼滤波增益，K(k)采用下式计算：

其中，S(k)为新息方差矩阵，R为测量噪声方差。

6.如权利要求4所述的方法，其特征在于，在步骤(4)中，对于N≥3的情况，跟踪滤波采用无迹卡尔曼滤波算法，此时目标的运动方程为：

X(k)＝f(x(k-1),W(k-1)) (7)

此时目标的观测方程为：

Z(k)＝h(x(k-1),V(k-1)) (8)

其中，f是非线性运动方程函数；h是非线性观测方程函数；设W(k)具有协方差Q，V(k)具有协方差R；

采用式(9)获得一组采样点，称为Sigma点集；

其中，

表示矩阵方根的第i列；

采用式(10)求得其对应权值：

其中，下标m为均值，c为协方差，上标为第几个采样点。参数λ是一个缩放比例参数，用来降低总的预测误差，数学表达式为λ＝a²(n+κ)-n，a的选取控制了采样点的分布状态，κ为待选参数，其具体取值虽然没有界限，但通常应确保矩阵(n+λ)P为半定矩阵；待选参数β≥0是一个非负的权系数，它可以合并方程中高阶项的动差，这样就可以把高阶项的影响包括在内。

7.如权利要求6所述的方法，其特征在于，所述的采样点表示为式(11)：

采用式(12)计算所述的2n+1个Sigma点集的一步预测及其协方差：

其中，i＝1,2,…,2n+1。

根据一步预测值，再次使用UT变换，得到新的Sigma点集为：

采用式(14)计算预测的观测量为：

Z⁽ⁱ⁾(k|k-1)＝h[X⁽ⁱ⁾(k|k-1)],i＝1,2,…,2n+1 (14)。

8.如权利要求7所述的方法，其特征在于，所述的系统预测的均值及协方差为：

9.如权利要求1所述的方法，其特征在于，所述的无迹卡尔曼滤波增益矩阵为：

10.如权利要求7所述的方法，其特征在于，所述的状态更新和协方差更新为：

Images