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

权利要求书

1.一种基于卡尔曼滤波的MEMS陀螺仪Z轴零偏动态补偿方法，其特征在于，主要包括：

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

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

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

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

2.根据权利要求1所述的一种基于卡尔曼滤波的MEMS陀螺仪Z轴零偏动态补偿方法，其特征在于，

所述步骤S1，具体包括：

在IMU处于静态时，

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

3.根据权利要求1所述的一种基于卡尔曼滤波的MEMS陀螺仪Z轴零偏动态补偿方法，其特征在于，

所述步骤S3，具体包括：

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

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

4.根据权利要求3所述的一种基于卡尔曼滤波的MEMS陀螺仪Z轴零偏动态补偿方法，其特征在于，

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

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

其中阈值为：Tax＝M*[max(Ax)–min(Ax)]，Tay＝M*[max(Ay)–min(Ay)]，Taz＝M*[max(Az)–min(Az)]，Trz＝M*[max(Rz)–min(Rz)]，max与min分别对应向量的最大值与最小值，Tax、Tay、Taz、Trz分别代表x轴加速度阈值、y轴加速度阈值、z轴加速度阈值以及z轴转速阈值，M为阈值比例系数，可根据载体工作环境设定；

若三轴加速度Ax，Ay，Az与MEMS陀螺仪Z轴转速Rz实测跳变均小于阈值，则判定为静态，否则为动态；因实际应用中很难保证外部噪声信号能处于较小状态，故适当增加跳动范围。