CN116127406B - 一种基于混合h∞自适应卡尔曼滤波的数据融合方法 - Google Patents

Abstract

本发明公开一种基于混合H∞自适应卡尔曼滤波的数据融合方法，属于智能驾驶车辆多传感器数据融合领域。通过将自适应卡尔曼滤波和H∞滤波的预测状态和误差协方差进行融合，减小了系统噪声和环境噪声的影响，增加了数据观测的稳健性，提高了目标车辆运动状态的识别精度，最后经带有双摄像的线控底盘车辆实车验证表明，该方法相较于传统的自适应卡尔曼滤波数据融合方法，目标车辆运动状态识别精度有很明显的提高。

Description

一种基于混合H∞自适应卡尔曼滤波的数据融合方法

技术领域

本发明涉及智能驾驶车辆多传感器数据融合领域，具体地讲，涉及一种基于混合H∞自适应卡尔曼滤波的数据融合方法。

背景技术

在智能驾驶车辆中，目标障碍物运动状态的准确识别对车辆安全性起着极其重要的作用。然而单一传感器对目标障碍物运动状态的识别总是因为传感器硬件和环境干扰的影响而产生误报或漏报情况，这将极大地增加了智能驾驶车辆的风险性。

为此，基于多传感器数据融合的目标障碍物运动状态观测成为保证智能驾驶安全性必不可少的要素。然而，当前大部分研究均使用单一的数据融合方法将多传感器数据进行融合，这将不可避免的放大了单一数据融合方法本身存在的弊端，为了提高多传感器数据融合方法的精确性，本发明提出了一种基于混合H∞/自适应卡尔曼滤波的数据融合方法。

发明内容

本发明要解决的技术问题是提供一种基于混合H∞自适应卡尔曼滤波的数据融合方法，该方法将融合了自适应卡尔曼滤波和H∞的优势，可以更加精确地预测目标车辆的运动状态。

本发明采用如下技术方案实现发明目的：

一种基于混合H∞自适应卡尔曼滤波的数据融合方法，其特征在于，包括以下步骤：

P1：在智能车辆跟驰场景下，设置融合数据的状态变量

其中：x表示车辆的横向位移；

y表示车辆的纵向位移；

表示车辆的横向速度；

表示车辆的纵向速度；

表示车辆的横向加速度；

表示车辆的纵向加速度；

P2：为了更好地描述跟驰车辆的运动学性能，选择恒定车速运动学模型和恒定加速度运动学模型作为车辆数据融合的运动学模型；

针对恒定车速模型的状态方程和观测方程为：

其中：φ(k)表示状态转移矩阵；

h(k+1)表示测量矩阵；

w(k)表示系统噪声；

v(k+1)表示观测噪声；

φ(k)表示如下：

h(k+1)表示如下：

针对恒定加速度模型的状态方程和观测方程为：

其中：A(k)表示状态转移矩阵；

H(k+1)表示测量矩阵；

W(k)表示系统噪声；

V(k+1)表示观测噪声；

A(k)表示如下：

H(k+1)表示如下：

P3：利用卡尔曼滤波对数据的滤波和预测过程，卡尔曼滤波主要包括预测过程和校正过程，在预测过程，预测状态方程表示为：

预测协方差为：

在校正过程，测量误差为：

最优卡尔曼增益为：

状态估计为：

先验状态估计误差协方差为：

P_k+1＝(I-K_k+1h_k+1)P_k+1|k (式12)

P4：为了减少系统噪声和观测噪声的影响，将以上卡尔曼滤波器改进为自适应卡尔曼滤波器，并由于在状态观测后期，测量值将更加接近真值，所以引进了衰减因子，减小在滤波过程中噪声对状态估计的影响，增加状态观测后期中测量值的权重；

引用了指数加权方法去增加当前时刻的估计误差协方差P去衰减前期的测量的影响，该指数加权方法可以被表示为：

其中：n_i是一个合理的常数；

根据以上改进后的估计误差协方差，先验状态估计和估计误差协方差被改写为：

观测误差可以被表示为：

测量过程协方差更新矩阵为：

其中：

卡尔曼增益将被改写为：

后验状态估计和后验估计误差协方差可以被表示为：

状态过程协方差更新矩阵表示为：

其中：

其中：N代表了自适应卡尔曼滤波的记忆步长；

P5：使用H∞滤波对数据进行滤波和预测的过程；

首先，基于上述假设的状态方程和观测方程，现增加另外的组合方程：

Y_k＝L_kX_k (式24)

其中：Y_k是X_k的线性组合，L_k是一个满秩的组合系数矩阵，为了后续表达方便，假设为Y_k值的估计值，/>为估计误差，/>是状态估计初始值，/>是初始估计误差；

为了得到一个合理估计以平衡估计误差/>和噪声，现引出以下代价函数：

其中：D_k是对应中参数的权重矩阵；

P₀对应与中不同参数的权重矩阵；

S_k代表测量噪声的权重矩阵；

U_k代表观测噪声的权重矩阵；

由于直接最小化J较为困难，引进一个灵活的性能边界系数1/θ，为此J将被重新改写为：

为此，H∞滤波问题将被转化为以下最小最大化问题：

根据(24)，估计误差可以被写成如下形式：

为此，H∞问题又可被转化如下最小最大化问题：

根据(4)，V_k可以被写成如下形式：

V_k＝Z_k-H_kX_k (式30)

为此H∞问题可最终被转化成如下最小最大化问题:

根据以上，将不等式重新改写成:

基于黎卡提方程，估计误差的协方差矩阵被表达为：

先验状态估计可以被表达如下：

H∞滤波增益可由以下公式计算：

P6：本发明的目的是融合自适应卡尔曼滤波和H∞的状态估计和误差协方差矩阵，兼顾这两者的优势以更精确的观测目标的状态；

恒定车速模型被应用到自适应卡尔曼滤波中，恒定加速度模型将被应用到H∞滤波中；

在目标观测的过程中，融合后的状态可以被表示为：

X_f＝γ₁X₁+γ₂X₂ (式36)

其中：X_f代表了融合后的目标状态；

X₁代表了自适应卡尔曼滤波观测状态；

X₂代表了H∞滤波观测状态；

γ₁及γ₂代表了两者的权重系数；

融合后的误差协方差矩阵可以被表示为：

其中：P_f代表融合后的误差协方差矩阵；

P₁代表了自适应卡尔曼滤波的误差协方差矩阵；

P₂代表了H∞滤波的误差协方差矩阵，

ζ₁决定了两者的权重系数；

P7：基于以上融合过程，最终将在安装有两个摄像头的线控底盘车辆中进行实车验证，并与原始的自适应卡尔曼滤波进行对比，已验证该方法的有效性。

作为本技术方案的进一步限定，所述P1中，将目标车辆的横纵向位置、速度和加速度设为系统的状态，对目标车辆的横纵向位置进行滤波，并基于此对目标车辆的横纵向速度和加速度进行预测。

作为本技术方案的进一步限定，所述P2中，引用了恒定车速模型和恒定加速度模型作为目标车辆的运动学方程，结合这两个车辆运动学模型可以更好地估计目标车辆的运动学特性，并能更加精确的估计机动性较强的目标车辆。

作为本技术方案的进一步限定，所述P4中，以卡尔曼滤波为基础，提出了自适应卡尔曼滤波，这将有效地减小噪声对观测值和系统不确定性的影响；同时，由于噪声因素的影响，在观测后期传感器的测量值将更好的反应目标车辆的状态，为此引入了指数衰减因子，增加当前时刻的误差协方差矩阵，以增加当前时刻测量值在自适应卡尔曼滤波中的权重。

作为本技术方案的进一步限定，所述P5中，应用了H∞滤波，并由于该滤波的使用，使得系统在不确定的内部参数、测量值和过程噪声的情况下更好的保持目标状态预测的稳健性。

作为本技术方案的进一步限定，所述P6中，将自适应卡尔曼滤波和H∞的估计状态和误差协方差进行融合，该融合过程将整合了自适应卡尔曼滤波和H∞的优势，可以更加精确地估计目标车辆的运动状态。

作为本技术方案的进一步限定，所述w(k)、所述v(k+1)、所述W(k)及所述V(k+1)属于高斯白噪声。

与现有技术相比，本发明的优点和积极效果是：

1、可以对目标车辆的横纵向位置进行滤波，并对目标车辆的横纵向速度和加速度进行预测

2、使用恒定车速模型和恒定加速度模型作为目标车辆的运动学模型，这将有效增加对机动性较强的目标车辆的识别精度。

3、引用了自适应卡尔曼滤波的预测和滤波过程，减小了噪声对预测过程和观测过程的影响，并引入了指数衰减因子，增加观测后期测量值的权重，提高摄像头的观测精度。

4、应用了H∞滤波的预测和滤波过程，增加了在不确定性内部参数、测量和噪声干扰情况下数据观测的稳健性。

5、融合了自适应卡尔曼滤波和H∞滤波的状态估计和误差协方差，整合了自适应卡尔曼滤波和H∞滤波的优势，提高目标状态观测的精度。

附图说明

图1为基于混合H∞/自适应卡尔曼滤波的数据融合方法流程图。

图2为基于双摄像头的智能线控底盘实车验证平台。

具体实施方式

下面结合附图，对本发明的一个具体实施方式进行详细描述，但应当理解本发明的保护范围并不受具体实施方式的限制。

如图1-图2所示，本发明包括以下步骤：

P1：在智能车辆跟驰场景下，设置融合数据的状态变量

其中：x表示车辆的横向位移；

y表示车辆的纵向位移；

表示车辆的横向速度；

表示车辆的纵向速度；

表示车辆的横向加速度；

表示车辆的纵向加速度；

P2：为了更好地描述跟驰车辆的运动学性能，选择恒定车速运动学模型和恒定加速度运动学模型作为车辆数据融合的运动学模型；

针对恒定车速模型的状态方程和观测方程为：

其中：φ(k)表示状态转移矩阵；

h(k+1)表示测量矩阵；

w(k)表示系统噪声；

v(k+1)表示观测噪声；

φ(k)表示如下：

h(k+1)表示如下：

针对恒定加速度模型的状态方程和观测方程为：

其中：A(k)表示状态转移矩阵；

H(k+1)表示测量矩阵；

W(k)表示系统噪声；

V(k+1)表示观测噪声；

A(k)表示如下：

H(k+1)表示如下：

P3：利用卡尔曼滤波对数据的滤波和预测过程，卡尔曼滤波主要包括预测过程和校正过程，在预测过程，预测状态方程表示为：

预测协方差为：

在校正过程，测量误差为：

最优卡尔曼增益为：

状态估计为：

先验状态估计误差协方差为：

P_k+1＝(I-K_k+1h_k+1)P_k+1|k (式12)

P4：为了减少系统噪声和观测噪声的影响，将以上卡尔曼滤波器改进为自适应卡尔曼滤波器，并由于在状态观测后期，测量值将更加接近真值，所以引进了衰减因子，减小在滤波过程中噪声对状态估计的影响，增加状态观测后期中测量值的权重；

引用了指数加权方法去增加当前时刻的估计误差协方差P去衰减前期的测量的影响，该指数加权方法可以被表示为：

其中：n_i是一个合理的常数；

根据以上改进后的估计误差协方差，先验状态估计和估计误差协方差被改写为：

观测误差可以被表示为：

测量过程协方差更新矩阵为：

其中：

卡尔曼增益将被改写为：

后验状态估计和后验估计误差协方差可以被表示为：

状态过程协方差更新矩阵表示为：

其中：

其中：N代表了自适应卡尔曼滤波的记忆步长；

P5：使用H∞滤波对数据进行滤波和预测的过程；

首先，基于上述假设的状态方程和观测方程，现增加另外的组合方程：

Y_k＝L_kX_k (式24)

其中：Y_k是X_k的线性组合，L_k是一个满秩的组合系数矩阵，为了后续表达方便，假设为Y_k值的估计值，/>为估计误差，/>是状态估计初始值，/>是初始估计误差；

为了得到一个合理估计以平衡估计误差/>和噪声，现引出以下代价函数：

其中：D_k是对应中参数的权重矩阵；

P₀对应与中不同参数的权重矩阵；

S_k代表测量噪声的权重矩阵；

U_k代表观测噪声的权重矩阵；

由于直接最小化J较为困难，引进一个灵活的性能边界系数1/θ，为此J将被重新改写为：

为此，H∞滤波问题将被转化为以下最小最大化问题：

根据(24)，估计误差可以被写成如下形式：

为此，H∞问题又可被转化如下最小最大化问题：

根据(4)，V_k可以被写成如下形式：

V_k＝Z_k-H_kX_k (式30)

为此H∞问题可最终被转化成如下最小最大化问题:

根据以上，将不等式重新改写成:

基于黎卡提方程，估计误差的协方差矩阵被表达为：

先验状态估计可以被表达如下：

H∞滤波增益可由以下公式计算：

P6：本发明的目的是融合自适应卡尔曼滤波和H∞的状态估计和误差协方差矩阵，兼顾这两者的优势以更精确的观测目标的状态；

恒定车速模型被应用到自适应卡尔曼滤波中，恒定加速度模型将被应用到H∞滤波中；

在目标观测的过程中，融合后的状态可以被表示为：

X_f＝γ₁X₁+γ₂X₂ (式36)

其中：X_f代表了融合后的目标状态；

X₁代表了自适应卡尔曼滤波观测状态；

X₂代表了H∞滤波观测状态；

γ₁及γ₂代表了两者的权重系数；

融合后的误差协方差矩阵可以被表示为：

P_f＝(ζP₁ ^-1+(1-ζ)P₂ ^-1)^-1 (式37)

其中：_Pf代表融合后的误差协方差矩阵；

P₁代表了自适应卡尔曼滤波的误差协方差矩阵；

P₂代表了H∞滤波的误差协方差矩阵，

ζ₁决定了两者的权重系数；

P7：基于以上融合过程，最终将在安装有两个摄像头的线控底盘车辆中进行实车验证，并与原始的自适应卡尔曼滤波进行对比，已验证该方法的有效性。

所述P1中，将目标车辆的横纵向位置、速度和加速度设为系统的状态，对目标车辆的横纵向位置进行滤波，并基于此对目标车辆的横纵向速度和加速度进行预测。

所述P2中，引用了恒定车速模型和恒定加速度模型作为目标车辆的运动学方程，结合这两个车辆运动学模型可以更好地估计目标车辆的运动学特性，并能更加精确的估计机动性较强的目标车辆。

所述P4中，以卡尔曼滤波为基础，提出了自适应卡尔曼滤波，这将有效地减小噪声对观测值和系统不确定性的影响；同时，由于噪声因素的影响，在观测后期传感器的测量值将更好的反应目标车辆的状态，为此引入了指数衰减因子，增加当前时刻的误差协方差矩阵，以增加当前时刻测量值在自适应卡尔曼滤波中的权重。

所述P5中，应用了H∞滤波，并由于该滤波的使用，使得系统在不确定的内部参数、测量值和过程噪声的情况下更好的保持目标状态预测的稳健性。

所述P6中，将自适应卡尔曼滤波和H∞的估计状态和误差协方差进行融合，该融合过程将整合了自适应卡尔曼滤波和H∞的优势，可以更加精确地估计目标车辆的运动状态。

所述w(k)、所述v(k+1)、所述W(k)及所述V(k+1)属于高斯白噪声。

以上公开的仅为本发明的具体实施例，但是，本发明并非局限于此，任何本领域的技术人员能思之的变化都应落入本发明的保护范围。

Claims (7)

1.一种基于混合H∞自适应卡尔曼滤波的数据融合方法，其特征在于，包括以下步骤：

P1：在智能车辆跟驰场景下，设置融合数据的状态变量

其中：x表示车辆的横向位移；

y表示车辆的纵向位移；

表示车辆的横向速度；

表示车辆的纵向速度；

表示车辆的横向加速度；

表示车辆的纵向加速度；

P2：为了更好地描述跟驰车辆的运动学性能，选择恒定车速运动学模型和恒定加速度运动学模型作为车辆数据融合的运动学模型；

针对恒定车速模型的状态方程和观测方程为：

其中：φ(k)表示状态转移矩阵；

h(k+1)表示测量矩阵；

w(k)表示系统噪声；

v(k+1)表示观测噪声；

φ(k)表示如下：

h(k+1)表示如下：

针对恒定加速度模型的状态方程和观测方程为：

其中：A(k)表示状态转移矩阵；

H(k+1)表示测量矩阵；

W(k)表示系统噪声；

V(k+1)表示观测噪声；

A(k)表示如下：

H(k+1)表示如下：

P3：利用卡尔曼滤波对数据的滤波和预测过程，卡尔曼滤波主要包括预测过程和校正过程，在预测过程，预测状态方程表示为：

预测协方差为：

在校正过程，测量误差为：

最优卡尔曼增益为：

状态估计为：

先验状态估计误差协方差为：

P_k+1＝(I-K_k+1h_k+1)P_k+1|k (式12)

P4：为了减少系统噪声和观测噪声的影响，将以上卡尔曼滤波器改进为自适应卡尔曼滤波器，并由于在状态观测后期，测量值将更加接近真值，所以引进了衰减因子，减小在滤波过程中噪声对状态估计的影响，增加状态观测后期中测量值的权重；

引用了指数加权方法去增加当前时刻的估计误差协方差P去衰减前期的测量的影响，该指数加权方法可以被表示为：

其中：n_i是一个合理的常数；

根据以上改进后的估计误差协方差，先验状态估计和估计误差协方差被改写为：

观测误差可以被表示为：

测量过程协方差更新矩阵为：

其中：

卡尔曼增益将被改写为：

后验状态估计和后验估计误差协方差可以被表示为：

状态过程协方差更新矩阵表示为：

其中：

其中：N代表了自适应卡尔曼滤波的记忆步长；

P5：使用H∞滤波对数据进行滤波和预测的过程；

首先，基于上述假设的状态方程和观测方程，现增加另外的组合方程：

Y_k＝L_kX_k (式24)

其中：Y_k是X_k的线性组合，L_k是一个满秩的组合系数矩阵，为了后续表达方便，假设为Y_k值的估计值，/>为估计误差，/>是状态估计初始值，/>是初始估计误差；

为了得到一个合理估计以平衡估计误差/>和噪声，现引出以下代价函数：

其中：D_k是对应中参数的权重矩阵；

P₀对应与中不同参数的权重矩阵；

S_k代表测量噪声的权重矩阵；

U_k代表观测噪声的权重矩阵；

由于直接最小化J较为困难，引进一个灵活的性能边界系数1/θ，为此J将被重新改写为：

为此，H∞滤波问题将被转化为以下最小最大化问题：

根据(24)，估计误差可以被写成如下形式：

为此，H∞问题又可被转化如下最小最大化问题：

根据(4)，V_k可以被写成如下形式：

V_k＝Z_k-H_kX_k (式30)

为此H∞问题可最终被转化成如下最小最大化问题:

根据以上，将不等式重新改写成:

基于黎卡提方程，估计误差的协方差矩阵被表达为：

先验状态估计可以被表达如下：

H∞滤波增益可由以下公式计算：

P6：融合自适应卡尔曼滤波和H∞的状态估计和误差协方差矩阵，兼顾这两者的优势以更精确的观测目标的状态；

恒定车速模型被应用到自适应卡尔曼滤波中，恒定加速度模型将被应用到H∞滤波中；

在目标观测的过程中，融合后的状态可以被表示为：

X_f＝γ₁X₁+γ₂X₂ (式36)

其中：X_f代表了融合后的目标状态；

X₁代表了自适应卡尔曼滤波观测状态；

X₂代表了H∞滤波观测状态；

γ₁及γ₂代表了两者的权重系数；

融合后的误差协方差矩阵可以被表示为：

其中：P_f代表融合后的误差协方差矩阵；

P₁代表了自适应卡尔曼滤波的误差协方差矩阵；

P₂代表了H∞滤波的误差协方差矩阵，

ζ决定了两者的权重系数；

P7：基于以上融合过程，最终将在安装有两个摄像头的线控底盘车辆中进行实车验证，并与原始的自适应卡尔曼滤波进行对比，已验证该方法的有效性。

2.根据权利要求1所述的基于混合H∞自适应卡尔曼滤波的数据融合方法，其特征在于：所述P1中，将目标车辆的横纵向位置、速度和加速度设为系统的状态，对目标车辆的横纵向位置进行滤波，并基于此对目标车辆的横纵向速度和加速度进行预测。

3.根据权利要求1所述的基于混合H∞自适应卡尔曼滤波的数据融合方法，其特征在于：所述P2中，引用了恒定车速模型和恒定加速度模型作为目标车辆的运动学方程，结合这两个车辆运动学模型可以更好地估计目标车辆的运动学特性，并能更加精确的估计机动性较强的目标车辆。

4.根据权利要求1所述的基于混合H∞自适应卡尔曼滤波的数据融合方法，其特征在于：所述P4中，以卡尔曼滤波为基础，提出了自适应卡尔曼滤波，这将有效地减小噪声对观测值和系统不确定性的影响；同时，由于噪声因素的影响，在观测后期传感器的测量值将更好的反应目标车辆的状态，为此引入了指数衰减因子，增加当前时刻的误差协方差矩阵，以增加当前时刻测量值在自适应卡尔曼滤波中的权重。

5.根据权利要求1所述的基于混合H∞自适应卡尔曼滤波的数据融合方法，其特征在于：所述P5中，应用了H∞滤波，并由于该滤波的使用，使得系统在不确定的内部参数、测量值和过程噪声的情况下更好的保持目标状态预测的稳健性。

6.根据权利要求4所述的基于混合H∞自适应卡尔曼滤波的数据融合方法，其特征在于：所述P6中，将自适应卡尔曼滤波和H∞的估计状态和误差协方差进行融合，该融合过程将整合了自适应卡尔曼滤波和H∞的优势，可以更加精确地估计目标车辆的运动状态。

7.根据权利要求1所述的基于混合H∞自适应卡尔曼滤波的数据融合方法，其特征在于：所述w(k)、所述v(k+1)、所述W(k)及所述V(k+1)属于高斯白噪声。