[发明专利]一种基于卡尔曼滤波的MEMS陀螺仪Z轴零偏动态补偿方法

说明书

本发明公开了一种基于卡尔曼滤波的MEMS陀螺仪Z轴零偏动态补偿方法，主要包括：步骤S1：在IMU处于静态时，获取载体姿态数据，并通过获取的载体姿态数据计算得出MEMS陀螺仪Z轴角速度Rz的补偿值作为初始补偿值Offset；步骤S2：实测中，从IMU中读取新的载体姿态数据；步骤S3：判断载体是否为静态；步骤S4：利用卡尔曼算法计算，并得到新的MEMS陀螺仪Z轴角速度Rz的补偿值，更新Offset，之后跳转到所述步骤S2，形成循环。本发明的一种基于卡尔曼滤波的MEMS陀螺仪Z轴零偏动态补偿方法，通过动态捕捉Z轴零偏，并运用卡尔曼算法对零偏进行动态补偿，提高MEMS陀螺仪的精度，减小陀螺仪Z轴误差，同时降低了航向角YAW的测量误差。

技术领域

本发明涉及MEMS陀螺仪Z轴零偏动态补偿领域，具体地，涉及一种基于卡尔曼滤波的MEMS陀螺仪Z轴零偏动态补偿方法。

背景技术

基于MEMS陀螺仪的IMU模块都存在零偏问题，对于X轴和Y轴的零偏，可以通过X轴和Y轴的加速度计的测量值运用卡尔曼滤波算法进行校准，而Z轴无法通过该方法校准，所以目前市场上的基于MEMS的IMU模块的Z轴测量值普遍不准，带来的结果是航向角的测量会具有很大的偏差，该偏差很大程度上是由于Z轴的零偏引起。

本专利通过动态捕捉Z轴零偏，并运用卡尔曼算法对零偏进行动态补偿，提高MEMS陀螺仪的精度，减小了陀螺仪Z轴误差，同时降低了航向角YAW的测量误差。

发明内容

本发明的目的在于，针对上述问题，提出一种基于卡尔曼滤波的MEMS陀螺仪Z轴零偏动态补偿方法，以实现提高MEMS陀螺仪的精度，减小了陀螺仪Z轴误差，同时降低了航向角YAW的测量误差的优点。

为实现上述目的，本发明采用的技术方案是：一种基于卡尔曼滤波的MEMS陀螺仪Z轴零偏动态补偿方法，主要包括：

步骤S1：在IMU处于静态，获取载体姿态数据，并通过获取的载体姿态数据计算得出MEMS陀螺仪Z轴角速度Rz的补偿值作为初始补偿值Offset；

步骤S2：实测中，从IMU中读取新的载体姿态数据；

步骤S3：判断载体是否为静态；

步骤S4：利用卡尔曼算法计算，并得到新的MEMS陀螺仪Z轴角速度Rz的补偿值，更新Offset，之后跳转到所述步骤S2，形成循环。

进一步地，所述步骤S1，具体包括：

在IMU处于静态状态或近似静态时，

初始化，在产品出厂前，将MEMS传感器静置5秒，获得载体姿态z轴转速Rz的数据，计算此过程载体的Rz的平均值，记作E(0)，并用Offset＝E(0)作为当前MEMS陀螺仪Z轴转速Rz出厂时补偿值，此时该1000个Rz测量值的方差记作COV(0)。

进一步地，所述步骤S3，具体包括：

步骤S31:若新的载体姿态数据跳动范围小于阈值，则认定该时刻依然为静态，计算新的载体Rz的平均值，记作E(k)，并计算其协方差，记作COV(k)，k表示第k次成功获取静态数据，若之后能继续成功获取静态载体姿态数据，则k＝k+1。

步骤S32：若新的载体姿态数据跳动范围大于阈值，则判定该时刻为动态，k值不变，跳回所述步骤S2。

进一步地，其中静态判断依据与阈值设定，具体包括：

从IMU中读取载体姿态数据，记录1000组数据，数据中包括Ax，Ay，Az，Rz，其中Ax，Ay，Az分别代表x轴、y轴、z轴的加速度；