CN107015944A - 一种用于目标跟踪的混合平方根容积卡尔曼滤波方法 - Google Patents

Abstract

本发明提出一种用于目标跟踪的混合平方根容积卡尔曼滤波方法。步骤包括：根据初始化后的状态向量、过程噪声协方差平方根因子、测量噪声协方差平方根因子、状态协方差平方根因子，预测目标先验状态向量和先验状态协方差平方根因子；计算状态向量容积采样点；计算测量向量容积采样点和测量向量容积采样点，获得测量向量；计算卡尔曼增益和后验状态协方差平方根因子；计算后验状态向量。本发明在保证滤波精度不变的前提下，大大提升了滤波效率，具有很强的实时性能。

Description

一种用于目标跟踪的混合平方根容积卡尔曼滤波方法

技术领域

本发明属于目标跟踪技术领域，具体涉及一种基于混合平方根容积卡尔曼滤波器的目标跟踪滤波方法，可广泛应用于汽车辅助驾驶、汽车自主驾驶和汽车主动安全等领域。

背景技术

卡尔曼滤波器(Kalman Filter，KF)是目标跟踪领域应用广泛的一种跟踪滤波算法。卡尔曼滤波器的作用在于，通过对运动目标运动过程和传感器测量过程建模，利用间接的、含噪声的测量值，尽量准确的估计目标的运动状态。运动状态包括目标的位置、速度和加速度等信息。传感器测量模型中包含噪声，称为测量噪声，这代表着对目标的测量含有一定的不准确性；目标运动过程模型中也包含噪声，称为过程噪声，这代表运动目标的运动过程并不完全符合运动模型的假设。真实世界的很多动态系统都并不确切的符合所假设的运动模型，但是由于卡尔曼滤波器被设计在有噪声的情况下工作，一个近似的符合已经可以使这个滤波器非常有用了。滤波的意义在于，通过假设运动模型和测量模型，减少测量噪声和过程噪声对目标跟踪效果的影响，更精确的估计目标车辆的运动状态。

传统的卡尔曼滤波器只适用于线性系统，即运动模型和测量模型必须是线性系统。而在例如汽车目标跟踪领域，这常常是难以达到的。多数情况下，在以传感器为坐标原点的笛卡尔坐标系下，目标运动状态可以用一个六维的状态向量X表示，其中x,表示目标在X方向上的距离、速度和加速度，y,表示目标在Y方向上的距离、速度和加速度。而传感器测量得到的目标通常表示为极坐标下的测量向量Y，Y＝[r,a,v]^T，其中r,a,v分别表示目标到传感器的径向距离、方位角和径向速度。测量模型h表示目标状态空间到测量空间的映射，即由于目标状态向量X和测量向量Y之间不满足线性关系，因此测量模型是非线性的。扩展卡尔曼滤波器(Extended Kalman Filter，EKF)是为了解决非线性问题而提出的一种滤波方法，它通过对非线性函数进行一阶泰勒展开达到线性化的目的，在非线性特性较强时往往会出现较大误差，甚至出现滤波发散的情况。不敏卡尔曼滤波器(Unscented Kalman Filter，UKF)是另一种适用于非线性系统的卡尔曼滤波器，它使用非线性函数概率逼近的方式能达到二阶精度，并且与EKF有同阶的计算复杂度。然而在实际使用UKF时，UT变换(Unscented Transform)中对非正定矩阵的乔利斯基分解将直接导致算法停滞。平方根不敏卡尔曼滤波器(Square Root Unscented Kalman Filter，SRUKF)成功的避免了矩阵非正定的问题成为UKF理想的替代方法。平方根容积卡尔曼滤波器(SquareRoot Cubature Kalman Filter，SRCKF)同样使用非线性函数概率密度逼近的方式，理论上比SRUKF具有更高的滤波精度。然而，SRCKF的高时间复杂度往往难以满足系统高实时性需求。

发明内容

本发明提出一种用于目标跟踪的混合平方根容积卡尔曼滤波方法，其结合了线性卡尔曼滤波器KF的预测技术和非线性卡尔曼滤波器SRCKF的更新技术，在取得与SRCKF相同的滤波精度的情况下，大大提升了滤波效率，具有很强的实时性能。

为了解决上述技术问题，本发明提供一种用于目标跟踪的混合平方根容积卡尔曼滤波方法，包括如下步骤：

步骤1，初始化参数，包括：k＝0时刻的目标状态向量X₀，过程噪声协方差平方根因子Q_sqrt，测量噪声协方差平方根因子R_sqrt，状态协方差平方根因子S₀；

步骤2，根据k-1时刻目标后验状态向量X_k-1、k-1时刻后验状态协方差平方根因子S_k-1、目标运动模型A_k和过程噪声协方差平方根因子Q_sqrt，预测k时刻目标先验状态向量和k时刻先验状态协方差平方根因子

步骤3，根据k时刻目标先验状态向量和k时刻先验状态协方差平方根因子计算k时刻状态向量容积采样点σ_x,k；

步骤4，根据k时刻状态向量容积采样点σ_x,k和非线性测量模型h(·)计算k时刻的测量向量容积采样点σ_y,k以及k时刻先验测量向量

步骤5，根据k时刻先验状态向量状态向量容积采样点σ_xk、测量向量容积采样点σ_y,k和先验测量向量计算k时刻卡尔曼增益K_k和后验状态协方差平方根因子S_k；

步骤6，根据k时刻先验状态向量k时刻卡尔曼增益K_k、k时刻传感器获得的测量向量Y_k以及k时刻先验测量向量计算k时刻后验状态向量X_k。

进一步，定义k时刻的目标状态向量其中，x_k、分别表示k时刻目标在X方向上的距离、速度和加速度，y_k、分别表示k时刻目标在Y方向上的距离、速度和加速度，T表示矩阵转置；

定义k时刻的目标测量向量Y_k＝[r_k,a_k,v_k]^T，其中r_k、a_k、v_k分别表示k时刻目标到传感器的径向距离、方位角和径向速度。

进一步，步骤1中，

根据k＝0时刻的目标测量向量Y₀初始化k＝0时刻的目标状态向量X₀，初始化方法如公式(1)所示：

X₀＝[r₀cos(a₀),v₀cos(a₀),0,r₀sin(a₀),v₀sin(a₀),0]^T 公式(1)

公式(1)中，r₀,a₀,v₀分别表示k＝0时刻传感器测量的目标径向距离、方位角和径向速度；

根据运动模型初始化过程噪声协方差平方根因子Q_sqrt，初始化方法如公式(2)所示：

公式(2)中，过程噪声协方差平方根因子Q_sqrt为6维方阵，chol(·)表示矩阵的乔利斯基分解，ζ_x、分别表示目标运动模型在X方向距离x、速度和加速度的建模误差方差；ζ_y、分别表示目标运动模型在Y方向距离y、速度和加速度的建模误差方差；

根据传感器的测量精度初始化测量噪声协方差平方根因子R_sqrt，初始化方法如公式(3)所示：

公式(3)中测量噪声协方差平方根因子R_sqrt为3维方阵，ξ_r、ξ_a、ξ_v分别测量模型在径向距离r、方位角a和径向速度v的测量误差方差；

根据经验初始化状态协方差平方根因子S₀，初始化如公式(4)所示：

进一步，步骤2中，预测k时刻目标先验状态向量和k时刻先验状态协方差平方根因子的方法如公式(5)和公式(6)所示：

公式(6)中，qr(·)表示矩阵的QR分解；公式(5)中，A_k为目标运动模型，表示目标状态向量从k-1时刻到k时刻的变换规律。

进一步，步骤3中，计算k时刻状态向量容积采样点σ_x,k的方法如公式(7)所示：

公式(7)中，表示k时刻状态向量容积采样点σ_x,k第i行，表示k时刻先验状态协方差平方根因子第i列，L表示目标状态向量的维数。

进一步，步骤4中，计算k时刻的测量向量容积采样点σ_y,k以及k时刻先验测量向量的方法如公式(8)和公式(9)所示：

σ_y,k＝h(σ_x,k) 公式(8)

公式(8)中，h(·)为非线性测量模型，其定义如公式(10)所示：

进一步，步骤5中，计算k时刻卡尔曼增益K_k和后验状态协方差平方根因子S_k的过程如公式(11)-(14)所示：

其中，S_yy,k为测量协方差平方根因子，P_xy,k为互协方差，表示状态向量容积采样点σ_x,k第1列到第2L列，表示测量向量容积采样点σ_y,k第1列到第2L列。

进一步，步骤6中，计算k时刻后验状态向量X_k的方法如公式(15)所示：

本发明与现有技术相比，其显著优点在于：

(1)本发明提出的混合平方根容积卡尔曼滤波方法是基于KF的线性预测和SRCKF的非线性更新，在保证与SRCKF具有相同的滤波精度的情况下，混合平方根容积卡尔曼滤波方法大大提高了SRCKF的运行效率；

(2)本发明混合SRCKF是非线性卡尔曼滤波器的一种，它避免了CKF在对非正定矩阵进行乔利斯基分解时导致的算法停滞问题；相对SRUKF而言，混合SRCKF具有更少的输入参数，实用性更强；

(3)本发明混合SRCKF拥有与SRCKF相同的滤波精度，使用线性预测替换SRCKF的预测环节，大量统计实验表明，混合SRCKF相比SRCKF在效率提升约26.34％。

附图说明

图1是本发明方法流程图。

图2是本发明实验中理论轨迹、测量轨迹及滤波轨迹对比图。

图3是本发明实验中X、Y方向距离测量误差和滤波误差对比图。

图4是本发明实验中测量轨迹与滤波轨迹对比图。

图5是本发明实验中径向距离测量值与滤波值、方位角测量值与滤波值对比图。

具体实施方式

容易理解，依据本发明的技术方案，在不变更本发明的实质精神的情况下，本领域的一般技术人员可以想象出本发明用于目标跟踪的混合平方根容积卡尔曼滤波方法的多种实施方式。因此，以下具体实施方式和附图仅是对本发明的技术方案的示例性说明，而不应当视为本发明的全部或者视为对本发明技术方案的限制或限定。

结合附图，本发明用于目标跟踪的混合平方根容积卡尔曼滤波方实施方式如下：

步骤1：定义k时刻的目标状态向量其中，x_k、分别表示k时刻目标在X方向上的距离、速度和加速度，y_k、分别表示k时刻目标在Y方向上的距离、速度和加速度，符号T表示矩阵转置；定义k时刻的目标测量向量Y_k＝[r_k,a_k,v_k]^T，其中r_k、a_k、v_k分别表示k时刻目标到传感器的径向距离、方位角和径向速度。k＝1,2…n,n∈Z⁺。

根据k＝0时刻的目标测量向量Y₀初始化k＝0时刻的目标状态向量X₀，初始化方法如公式(1)所示：

X₀＝[r₀cos(a₀),v₀cos(a₀),0,r₀sin(a₀),v₀sin(a₀),0]^T 公式(1)

公式(1)中，r₀,a₀,v₀分别表示k＝0时刻传感器测量的目标径向距离、方位角和径向速度。

根据运动模型初始化过程噪声协方差平方根因子Q_sqrt，初始化方法如公式(2)所示：

公式(2)中，过程噪声协方差平方根因子Q_sqrt为6维方阵，chol(·)表示矩阵的乔利斯基分解，ζ_x、分别表示目标运动模型在X方向距离x、速度和加速度的建模误差方差；ζ_y、分别表示目标运动模型在Y方向距离y、速度和加速度的建模误差方差。

根据传感器的测量精度初始化测量噪声协方差平方根因子R_sqrt，初始化方法如公式(3)所示：

公式(3)中测量噪声协方差平方根因子R_sqrt为3维方阵，ξ_r、ξ_a、ξ_v分别测量模型在径向距离r、方位角a和径向速度v的测量误差方差。

根据经验初始化状态协方差平方根因子S₀，初始化如公式(4)所示：

步骤2：根据k-1时刻目标后验状态向量X_k-1、k-1时刻后验状态协方差平方根因子S_k-1、目标运动模型A_k和过程噪声协方差平方根因子Q_sqrt，预测k时刻目标先验状态向量和k时刻先验状态协方差平方根因子预测方法如公式(5)、公式(6)所示：

公式(6)中，qr(·)表示矩阵的QR分解；公式(5)中，A_k为目标运动模型，表示目标状态向量从k-1时刻到k时刻的变换规律。例如，常加速度(Constant Acceleration，CA)模型表述如公式(7)所示：

其中，dt表示k-1时刻到k时刻的时间差。

步骤3：根据k时刻目标先验状态向量和k时刻的先验状态协方差平方根因子计算k时刻状态向量容积采样点σ_x,k，计算方法如公式(8)所示：

公式(8)中，表示k时刻状态向量容积采样点σ_x,k第i行，表示k时刻先验状态协方差平方根因子第i列，L＝6表示目标状态向量的维数。

步骤4：根据k时刻状态向量容积采样点σ_x,k和非线性测量模型h(·)计算k时刻的测量向量容积采样点σ_y，k以及k时刻先验测量向量预测方法如公式(9)、公式(10)所示：

σ_y,_k＝h(σ_x,k) 公式(9)

公式(9)中，h(·)为非线性测量模型，是状态向量到测量向量的转换，定义如下：

步骤5：根据k时刻先验状态向量状态向量容积采样点σ_x,k、测量向量容积采样点σ_y，k和先验测量向量计算k时刻卡尔曼增益K_k和后验状态协方差平方根因子S_k，计算过程如公式(12)-(15)所示：

其中，S_yy,k为测量协方差平方根因子，P_xy,k为互协方差，表示状态向量容积采样点σ_x,k第1列到第2L列，表示测量向量容积采样点σ_y,k第1列到第2L列。

S_k和X_k是k时刻所有计算流程最终需要计算的变量，下一次迭代需要使用。

步骤6：根据k时刻先验状态向量k时刻卡尔曼增益K_k、k时刻传感器获得的测量向量Y_k以及k时刻先验测量向量计算k时刻后验状态向量X_k，计算方法如公式(16)所示：

步骤7：本次迭代结束，转步骤2循环迭代，直至传感器停止工作完成目标跟踪为止。

本发明效果可以通过以下实验进一步说明。

实验条件：设目标初始状态向量X₀＝[10,0.5,0.0,10,10,-0.6]^T，模拟目标真实运动轨迹如图3所示，在目标理论轨迹基础上添加高斯白噪声后模拟传感器测量向量。

实验方法：分别使用本发明提出的混合SRCKF和传统的SRCKF分别对图3中的测量轨迹进行迭代滤波，进行10,000次蒙特卡洛仿真实验并统计滤波后的误差均方根值。

实验结果：目标理论轨迹、目标测量轨迹和目标滤波轨迹如图2所示；测量误差均方根、滤波误差均方根对比图如图3所示。

由图3统计X、Y方向距离误差均方根如表1所示，统计两种滤波器平均耗时如表2所示。

表1 10,000次蒙特卡罗实验误差统计表

	X方向距离	Y方向距离
			测量值误差均方根/(m)	1.8252	1.9045
SRCKF滤波误差均方根/(m)	0.1237	0.2084
			混合SRCKF滤波误差均方根/(m)	0.1237	0.2084

表2 10,000次蒙特卡罗实验平均时间消耗统计表

	SRCKF平均时间消耗	混合SRCKF平均时间消耗
			时间消耗/(sec)	0.2346	0.1728

实验结论：由表1可以看出，本发明混合SRCKF与SRCKF具有相同的滤波精度，滤波后误差相比测量误差降低了约90％；由表2可以看出，在滤波效率上，本发明混合SRCKF比传统的SRCKF提高了26.34％。

将本发明应用于汽车毫米波雷达目标跟踪滤波也取得了良好的实验效果。实验采用IVQ-905雷达传感器采集数据，使用本发明提出的混合SRCKF对采集的数据进行滤波后的效果如图4、图5所示。

Claims (8)

1.一种用于目标跟踪的混合平方根容积卡尔曼滤波方法，其特征在于，包括如下步骤：

步骤1，初始化参数，包括：k＝0时刻的目标状态向量X₀，过程噪声协方差平方根因子Q_sqrt，测量噪声协方差平方根因子R_sqrt，状态协方差平方根因子S₀；

步骤2，根据k-1时刻目标后验状态向量X_k-1、k-1时刻后验状态协方差平方根因子S_k-1、目标运动模型A_k和过程噪声协方差平方根因子Q_sqrt，预测k时刻目标先验状态向量和k时刻先验状态协方差平方根因子

步骤3，根据k时刻目标先验状态向量和k时刻先验状态协方差平方根因子计算k时刻状态向量容积采样点σ_x,k；

步骤4，根据k时刻状态向量容积采样点σ_x,k和非线性测量模型h(·)计算k时刻的测量向量容积采样点σ_y,k以及k时刻先验测量向量

步骤5，根据k时刻先验状态向量状态向量容积采样点σ_x，k、测量向量容积采样点σ_y,k和先验测量向量计算k时刻卡尔曼增益K_k和后验状态协方差平方根因子S_k；

步骤6，根据k时刻先验状态向量k时刻卡尔曼增益K_k、k时刻传感器获得的测量向量Y_k以及k时刻先验测量向量计算k时刻后验状态向量X_k。

2.如权利要求1所述用于目标跟踪的混合平方根容积卡尔曼滤波方法，其特征在于，定义k时刻的目标状态向量其中，x_k、分别表示k时刻目标在X方向上的距离、速度和加速度，y_k、分别表示k时刻目标在Y方向上的距离、速度和加速度，T表示矩阵转置；

定义k时刻的目标测量向量Y_k＝[r_k,a_k,v_k]^T，其中r_k、a_k、v_k分别表示k时刻目标到传感器的径向距离、方位角和径向速度。

3.如权利要求2所述用于目标跟踪的混合平方根容积卡尔曼滤波方法，其特征在于，步骤1中，

根据k＝0时刻的目标测量向量Y₀初始化k＝0时刻的目标状态向量X₀，初始化方法如公式(1)所示：

X₀＝[r₀cos(a₀),v₀cos(a₀),0,r₀sin(a₀),v₀sin(a₀),0]^T 公式(1)

公式(1)中，r₀,a₀,v₀分别表示k＝0时刻传感器测量的目标径向距离、方位角和径向速度；

根据运动模型初始化过程噪声协方差平方根因子Q_sqrt，初始化方法如公式(2)所示：

公式(2)中，过程噪声协方差平方根因子Q_sqrt为6维方阵，chol(·)表示矩阵的乔利斯基分解，ζ_x、分别表示目标运动模型在X方向距离x、速度和加速度的建模误差方差；ζ_y、分别表示目标运动模型在Y方向距离y、速度和加速度的建模误差方差；

根据传感器的测量精度初始化测量噪声协方差平方根因子R_sqrt，初始化方法如公式(3)所示：

公式(3)中测量噪声协方差平方根因子R_sqrt为3维方阵，ξ_r、ξ_a、ξ_v分别测量模型在径向距离r、方位角a和径向速度v的测量误差方差；

根据经验初始化状态协方差平方根因子S₀，初始化如公式(4)所示：

4.如权利要求3所述用于目标跟踪的混合平方根容积卡尔曼滤波方法，其特征在于，步骤2中，预测k时刻目标先验状态向量和k时刻先验状态协方差平方根因子的方法如公式(5)和公式(6)所示：

公式(6)中，qr(·)表示矩阵的QR分解；公式(5)中，A_k为目标运动模型，表示目标状态向量从k-1时刻到k时刻的变换规律。

5.如权利要求4所述用于目标跟踪的混合平方根容积卡尔曼滤波方法，其特征在于，步骤3中，计算k时刻状态向量容积采样点σ_x,k的方法如公式(7)所示：

公式(7)中，表示k时刻状态向量容积采样点σ_x,k第i行，表示k时刻先验状态协方差平方根因子第i列，L表示目标状态向量的维数。

6.如权利要求5所述用于目标跟踪的混合平方根容积卡尔曼滤波方法，其特征在于，步骤4中，计算k时刻的测量向量容积采样点σ_y，k以及k时刻先验测量向量的方法如公式(8)和公式(9)所示：

σ_y,k＝h(σ_x,k) 公式(8)

公式(8)中，h(·)为非线性测量模型，其定义如公式(10)所示：

7.如权利要求6所述用于目标跟踪的混合平方根容积卡尔曼滤波方法，其特征在于，步骤5中，计算k时刻卡尔曼增益K_k和后验状态协方差平方根因子S_k的过程如公式(11)-(14)所示：

其中，S_yy,k为测量协方差平方根因子，P_xy,k为互协方差，表示状态向量容积采样点σ_x,k第1列到第2L列，表示测量向量容积采样点σ_y,k第1列到第2L列。

8.如权利要求7所述用于目标跟踪的混合平方根容积卡尔曼滤波方法，其特征在于，步骤6中，计算k时刻后验状态向量X_k的方法如公式(15)所示：