[发明专利]一种基于误差卡尔曼滤波的高动态车辆姿态估计方法

说明书

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

技术领域

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

背景技术

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

由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：系统采用东-北-天导航坐标系，其状态预测方程展开为：