CN114659488B - 一种基于误差卡尔曼滤波的高动态车辆姿态估计方法 - Google Patents

Abstract

一种基于误差卡尔曼滤波的高动态车辆姿态估计方法，利用组合导航系统进行导航，本发明可以在无GPS状态下依然能保证良好的姿态估计精度，在GPS信号良好时，可以通过ESKF滤波模型同时估计IMU零偏进行补偿，进而提高系统鲁棒性和精度。在ESKF系统观测部分，增加车辆动力学模型解算车辆横滚角和俯仰角作为系统观测，通过读取GPS输出协议中的位置估计精度和航向估计精度，通过建立AR模型的方式判断GPS位置信息和航线信息是否有效，将GPS输出的WGS84坐标系转换为高斯平面直角坐标系，这样方便系统预测模型和矫正模型的建立。

Description

一种基于误差卡尔曼滤波的高动态车辆姿态估计方法

技术领域

本发明属于传感器信息融合领域，尤其涉及基于一种误差卡尔曼滤波高动态车辆姿态估计方法。

背景技术

随着现代汽车工业的发展。智能驾驶系统已成为当今汽车行业的发展趋势，人们对智能驾驶系统的需求越来越高，这些电子系统大多需要基于汽车姿势和位置信息的反馈。因此，智能驾驶车辆系统的发展基于准确、稳定、实时的姿态和导航信息。

由MEMS传感器组成的姿态测量系统由于功耗低、体积小、成本低，在当今汽车行业得到广泛的应用。然而，单个MEMS传感器有以下缺点：(1)陀螺仪在积分中存在严重的累计漂移误差。(2)线性加速度和振动效应在加速度计的姿态计算中引起较大误差。整个MEMS姿态测量系统的主要缺点是车辆在运动过程中产生复杂的噪音，这些噪音是随机的，无法提前预测和消除。因此，不能只使用传感器的原始数据来获得运动中汽车的高精度姿态。

目前车辆姿态解算利用车载速度计对加速度计数据进行补偿得到补偿后的加速度，通过运动加速度抑制处理得到加速度，进而得到观测姿态四元数；获取陀螺仪数据，利用陀螺仪输出的角速度值，通过四元数微分方程得到四元数的状态估计值；通过扩展卡尔曼进行多传感器的信息融合，输出最终的姿态角信息，单纯只用车载速度计对加速度计数据进行补偿，在车辆发生打滑时，车载速度计无法准确测量车体加速度，导致补偿后的加速度无法准确观测姿态四元数，导致车辆姿态误差变大，该算法没有对IMU的误差进行补偿，通常影响imu精度的主要指标是加速度计和陀螺的零偏，零偏会随着时间，温度不断变化，导致加速度计观测姿态不准确和陀螺预测姿态积分不准确。

如：基于误差状态卡尔曼滤波器的四元数运动学论文，提供了一种误差卡尔曼滤波方法，可以通过IMU和GPS做误差卡尔曼滤波，对车辆姿态和IMU零偏进行实时估计，该系统框图如图1所示，通过imu中的三轴陀螺积分进行姿态预测，通过imu中的三轴加速度计积分进行速度预测，位置预测，从而进行行位推算。但通过imu预测是有累计误差的，需要通过观测量进行误差消除，当系统接收GPS信号时，建立误差卡尔曼观测模型，实时估计出imu零偏误差，并进行补偿，从而提高系统精度，该系统方案在GPS信号良好状态下可以保证良好的位置，速度，姿态精度，但是当进入隧道等GPS信号不好的情况下，系统观测无法使用，位置，速度，姿态采用DR算法递推，此时由于系统姿态只能依靠陀螺积分得到，长时间后姿态角度会不断发散，导致车辆姿态无法估计。

发明内容

为解决现有技术的上述问题，本发明目的是要设计一种误差卡尔曼滤波高动态车辆姿态估计方法。

为实现上述目的，本发明技术方案如下：

一种基于误差卡尔曼滤波的高动态车辆姿态估计方法，其特征在于：利用组合导航系统进行导航，所述组合导航系统包括捷联惯导系统传感器即SINS、全球定位系统传感器即GPS、三维电子罗盘传感器即compass和嵌入式处理器；所述嵌入式处理器内采用误差卡尔曼滤波算法进行传感器信息融合，输出车辆的位置、速度和姿态信息；所述的误差卡尔曼滤波算法基于误差卡尔曼滤波器来实现；

所述误差卡尔曼滤波的高动态车辆姿态估计方法，包括以下步骤：

S1:建立系统状态更新方程：

其中，

x为状态向量，δx为误差状态向量；

S2：系统采用东-北-天导航坐标系，其状态预测方程展开为：

p←p+vVt+1/2(R(α_m-α_b)+g)Vt²

v←v+(R(α_m-α_b)+g)Vt

α_b←α_b

ω_b←ω_b；

S3:建立先验估计协方差方程为：

其中

其中/>

为加速度计白噪声

为陀螺白噪声

σ_αω为加速度计零偏稳定性

其中σ_ωω为陀螺零偏稳定性；

S4：建立汽车运动情况下的系统运动模型，忽略地球自转速度，假设车辆的俯仰加速度、横滚角速度和垂直速度都为0，可建立车辆动力学方程：

其中，ν_x，ν_y分别表示车辆的纵向速度和横向速度，α_x，α_y分别表示车辆的纵向加速度和横向加速度，ω_z表示车辆的横摆角速度，g表示重力加速度，φ，θ分别表示横滚角和俯仰角，考虑到车辆正常行驶过程中震动较大，通过车辆速度求导得到的车体加速度噪声过大，而车辆角度变化是一个低频运动，因此，需要对加速度观测值和车体加速度做一个移动平滑滤波，以得到更好的姿态观测效果，

将上式改写为：

从而得到横滚角和俯仰角的观测量；

S5:将S4得到的横滚角和俯仰角进行ESKF建模纠正，

Z_k＝[φ θ]

δZ_k＝H₁δx+η1＝[φ θ]^T-[φ_DR θ_DR]^T

其中:

滤波增益矩阵更新方程为：

误差状态更新方程：

δx＝K_kδZ_k

后验估计协方差更新方程为：

其中I_KH＝I-K_kH₁。

S6：预测GPS的航向信号和位置信号是否可用，为了检测GPS信号是否可用，预测的公式为：

利用实时求取各级模型参数a₁、a₂、a₃、b₁、b₂、c₁，

为下一时刻GPS的位置信息，通过上面的公式就可以实时在线估计出GPS输出定位信息，以达到处理的目的，判断性准则：利用95％的圆周半径作为GPS接收输出定位信息的判断标准，将3级AR模型的预测输出值与当前时刻GPS接收机定位输出值相比较，当两者的差距超过该半径，则认为5％的小概率事件发生了，当前的GPS输出定位信息不可用，判定该时刻的GPS输出值为离群值，95％的圆概率半径的计算式如下：

式中，HDOP为水平精度因子，取为1.5；σ_UERE为卫星伪距测量误差的标准偏差，对于C/A码来说，其估计值为33.3m；

如果当前GPS输出的信息为离群值，则为了不影响下一时刻模型的准确性，应该将输出信息剔除掉，并以当前时刻的AR模型的预测输出值代替，继续进行在线的参数估计和下一时刻的预测，并判断信息的可用性；

S7：当GPS航向信号判断可用后，对航向角观测量进行ESKF建模矫正，航向角的输出值为ψ，

Z_k＝[ψ]

δZ_k＝H₂δx+η2＝[ψ]^T-[ψ_DR]^T

其中：

H₂＝[0_1×3 0_1×3 0 0 I 0_1×3 0_1×3]

η₂＝[ξψ]

滤波增益矩阵更新方程为：

误差状态更新方程为：

δx＝K_kδZ_k

后验估计协方差更新方程为：

其中I_KH＝I-K_kH₂；

S8:当GPS位置信号判断可用时，对GPS位置观测量进行ESKF建模矫正，GPS点从WGS84坐标系转换成BJ54的坐标系，从而GPS位置信息的高斯吕克投影坐标为Z_k，

Z_k＝[P_x P_y P_z]

δZ_k＝H₃δx+η₃＝[P_x P_y P_z]^T-[P_xDR P_yDR P_yDR]^T

其中：

δZ_k＝H₃δx+η₃＝[P_x P_y P_z]^T-[P_xDR P_yDR P_yDR]^T

其中：

滤波增益矩阵更新方程为：

误差状态更新方程为：

δx＝K_kδZ_k

后验估计协方差更新方程为：

其中I_KH＝I-K_kH₃。

与现有技术相比，本发明具有以下优势：

本发明可以在无GPS状态下依然能保证良好的姿态估计精度，在GPS信号良好时，可以通过ESKF滤波模型同时估计IMU零偏进行补偿，进而提高系统鲁棒性和精度。在ESKF系统观测部分，增加车辆动力学模型解算车辆横滚角和俯仰角作为系统观测，通过读取GPS输出协议中的位置估计精度和航向估计精度，通过建立AR模型的方式判断GPS位置信息和航线信息是否有效，将GPS输出的WGS84坐标系转换为高斯平面直角坐标系，这样方便系统预测模型和矫正模型的建立。

附图说明

图1是现有技术IMU和GPS做误差卡尔曼滤波流程架构图；

图2是本发明系统流程图；

图3是本发明系统俯仰角对比图；

图4是本发明航向角对比图；

图5是本发明俯仰角对比图。

具体实施方式

下面结合附图对本发明做进一步描述。

实施例：如图2、3、4所示，一种基于误差卡尔曼滤波的高动态车辆姿态估计方法，利用组合导航系统进行导航，所述组合导航系统包括捷联惯导系统传感器即SINS、全球定位系统传感器即GPS、三维电子罗盘传感器即compass和嵌入式处理器；所述嵌入式处理器内采用误差卡尔曼滤波算法进行传感器信息融合，输出车辆的位置、速度和姿态信息；所述的误差卡尔曼滤波算法基于误差卡尔曼滤波器来实现；

所述误差卡尔曼滤波的高动态车辆姿态估计方法，包括以下步骤：

S1:建立系统状态更新方程：

其中，

x为状态向量，δx为误差状态向量；

S2：系统采用东-北-天导航坐标系，其状态预测方程展开为：

p←p+vVt+1/2(R(α_m-α_b)+g)Vt²

v←v+(R(α_m-α_b)+g)Vt

α_b←α_b

ω_b←ω_b；

S3:建立先验估计协方差方程为：

P←F_xPF_x ^T+F_iQ_iF_i ^T，

其中

其中/>

为加速度计白噪声

为陀螺白噪声

σ_αω为加速度计零偏稳定性

其中σ_ωω为陀螺零偏稳定性；

S4：建立汽车运动情况下的系统运动模型，忽略地球自转速度，假设车辆的俯仰加速度、横滚角速度和垂直速度都为0，可建立车辆动力学方程：

其中，ν_x，ν_y分别表示车辆的纵向速度和横向速度，α_x，α_y分别表示车辆的纵向加速度和横向加速度，ω_z表示车辆的横摆角速度，g表示重力加速度，φ，θ分别表示横滚角和俯仰角，考虑到车辆正常行驶过程中震动较大，通过车辆速度求导得到的车体加速度噪声过大，而车辆角度变化是一个低频运动，因此，需要对加速度观测值和车体加速度做一个移动平滑滤波，以得到更好的姿态观测效果，

将上式改写为：

从而得到横滚角和俯仰角的观测量；

S5:将S4得到的横滚角和俯仰角进行ESKF建模纠正，

Z_k＝[φ θ]

δZ_k＝H₁δx+η1＝[φ θ]^T-[φ_DR θ_DR]^T

其中:

滤波增益矩阵更新方程为：

误差状态更新方程：

δx＝K_kδZ_k

后验估计协方差更新方程为：

其中I_KH＝I-K_kH₁。

S6：预测GPS的航向信号和位置信号是否可用，为了检测GPS信号是否可用，建立3级级联的AR模型来逼近ARMA模型：

第一级：

x_t-a₁x_t-1-a₂x_t-2-a₃x_t-3＝u_t

第二级：

u_t-b₁u_t-1-b₂u_t-2＝v_t

第三级：

v_t-c₁v_t-1＝n_t

来实时地预测下一时刻的GPS的输出定位信息，根据以上建立的模型，得出一步预测的公式为：

利用实时求取各级模型参数a₁、a₂、a₃、b₁、b₂、c₁，

为下一时刻GPS的位置信息，通过上面的公式就可以实时在线估计出GPS输出定位信息，以达到处理的目的，判断性准则：利用95％的圆周半径作为GPS接收输出定位信息的判断标准，将3级AR模型的预测输出值与当前时刻GPS接收机定位输出值相比较，当两者的差距超过该半径，则认为5％的小概率事件发生了，当前的GPS输出定位信息不可用，判定该时刻的GPS输出值为离群值，95％的圆概率半径的计算式如下：

CEP₉₅≈2.0HDOP_σUERE

式中，HDOP为水平精度因子，取为1.5；σ_UERE为卫星伪距测量误差的标准偏差，对于C/A码来说，其估计值为33.3m；

如果当前GPS输出的信息为离群值，则为了不影响下一时刻模型的准确性，应该将输出信息剔除掉，并以当前时刻的AR模型的预测输出值代替，继续进行在线的参数估计和下一时刻的预测，并判断信息的可用性；

S7：当GPS航向信号判断可用后，对航向角观测量进行ESKF建模矫正，航向角的输出值为ψ，

Z_k＝[ψ]

δZ_k＝H₂δx+η2＝[ψ]^T-[ψ_DR]^T

其中：

H₂＝[0_1×3 0_1×3 0 0 I 0_1×3 0_1×3]

η₂＝[ξψ]

滤波增益矩阵更新方程为：

误差状态更新方程为：

δx＝K_kδZ_k

后验估计协方差更新方程为：

其中I_KH＝I-K_kH₂；

S8:当GPS位置信号判断可用时，对GPS位置观测量进行ESKF建模矫正，GPS点从WGS84坐标系转换成BJ54的坐标系，从而GPS位置信息的高斯吕克投影坐标为Z_k，

Z_k＝[P_x P_y P_z]

δZ_k＝H₃δx+η₃＝[P_x P_y P_z]^T-[P_xDR P_yDR P_yDR]^T

其中：

δZ_k＝H₃δx+η₃＝[P_x P_y P_z]^T-[P_xDR P_yDR P_yDR]^T

其中：

滤波增益矩阵更新方程为：

误差状态更新方程为：

δx＝K_kδZ_k

后验估计协方差更新方程为：

其中I_KH＝I-K_kH₃。

如图3、4、5所示，在本发明的作用下，车辆高速行驶过程中，系统的精度高，鲁棒性强；车辆各种曲线均显示正常，说明系统的实用性比现有技术更加的宽泛。

Claims (1)

1.一种基于误差卡尔曼滤波的高动态车辆姿态估计方法，其特征在于：

包括以下步骤：

S1:建立系统状态更新方程：

其中，

x为状态向量，δx为误差状态向量；

S2：系统采用东-北-天导航坐标系，其状态预测方程展开为：

p←p+vVt+1/2(R(α_m-α_b)+g)Vt²

v←v+(R(α_m-α_b)+g)Vt

α_b←α_b

ω_b←ω_b；

S3:建立先验估计协方差方程为：

其中/>

其中

为加速度计白噪声

为陀螺白噪声

σ_αω为加速度计零偏稳定性

其中σ_ωω为陀螺零偏稳定性；

S4：建立汽车运动情况下的系统运动模型，忽略地球自转速度，假设车辆的俯仰加速度、横滚角速度和垂直速度都为0，可建立车辆动力学方程：

从而得到横滚角和俯仰角的观测量；

S5:将S4得到的横滚角和俯仰角进行ESKF建模纠正，

Z_k＝[φ θ]

δZ_k＝H₁δx+η1＝[φ θ]^T-[φ_DR θ_DR]^T

其中:

滤波增益矩阵更新方程为：

误差状态更新方程：

δx＝K_kδZ_k

后验估计协方差更新方程为：

其中I_KH＝I-K_kH₁；

S6：预测GPS的航向信号和位置信号是否可用，为了检测GPS信号是否可用，预测的公式为：

利用实时求取各级模型参数，

为下一时刻GPS的位置信息，通过上面的公式就可以实时在线估计出GPS输出定位信息，以达到处理的目的，判断性准则：利用95％的圆周半径作为GPS接收输出定位信息的判断标准，将3级AR模型的预测输出值与当前时刻GPS接收机定位输出值相比较，当两者的差距超过该半径，则认为5％的小概率事件发生了，当前的GPS输出定位信息不可用，判定该时刻的GPS输出值为离群值，95％的圆概率半径的计算式如下：

式中，HDOP为水平精度因子，取为1.5；σ_UERE为卫星伪距测量误差的标准偏差，对于C/A码来说，其估计值为33.3m；

如果当前GPS输出的信息为离群值，则为了不影响下一时刻模型的准确性，应该将输出信息剔除掉，并以当前时刻的AR模型的预测输出值代替，继续进行在线的参数估计和下一时刻的预测，并判断信息的可用性；

S7：当GPS航向信号判断可用后，对航向角观测量进行ESKF建模矫正，航向角的输出值为ψ，

Z_k＝[ψ]

δZ_k＝H₂δx+η2＝[ψ]^T-[ψ_DR]^T

其中：

H₂＝[0_1×3 0_1×3 0 0 I 0_1×3 0_1×3]

η₂＝[ξψ]

滤波增益矩阵更新方程为：

误差状态更新方程为：

δx＝K_kδZ_k

后验估计协方差更新方程为：

其中I_KH＝I-K_kH₂；

S8:当GPS位置信号判断可用时，对GPS位置观测量进行ESKF建模矫正，GPS点从WGS84坐标系转换成BJ54的坐标系，从而GPS位置信息的高斯吕克投影坐标为Z_k，

Z_k＝[P_x P_y P_z]

δZ_k＝H₃δx+η₃＝[P_x P_y P_z]^T-[P_xDR P_yDR P_yDR]^T

其中：

滤波增益矩阵更新方程为：

误差状态更新方程为：

δx＝K_kδZ_k

后验估计协方差更新方程为：

其中I_KH＝I-K_kH₃。

Images