一种基于卡尔曼滤波的MEMS陀螺仪Z轴零偏动态补偿方法与流程

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

背景技术：

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

2、本发明通过动态捕捉z轴零偏，并运用卡尔曼算法对零偏进行动态补偿，提高mems陀螺仪的精度，减小了陀螺仪z轴误差，同时降低了航向角yaw的测量误差。

技术实现思路

1、有鉴于此，本发明提供了一种基于卡尔曼滤波的mems陀螺仪z轴零偏动态补偿方法，以实现提高mems陀螺仪的精度，减小了陀螺仪z轴误差，同时降低了航向角yaw的测量误差的优点。

2、为了实现上述目的，本发明采用如下技术方案：

3、一种基于卡尔曼滤波的mems陀螺仪z轴零偏动态补偿方法，包括以下步骤：

4、s1：在产品出厂前，将mems传感器静置，获得1000个z轴角速度测量值并分别计算平均值e(0)和协方差cov(0)，用offset＝e(0)作为补偿值的出厂值；

5、s2：实测中，从imu中读取新的载体姿态数据；

6、s3：判断载体是否为静态，若新的载体记录数据跳动范围大于阈值，则判定时刻为动态，k值不变，k表示第k次成功获取静态数据，返回s2；

7、s4：通过动态捕捉z轴零偏并运用卡尔曼算法对零偏进行动态补偿，得到新的mems陀螺仪z轴角速度rz的补偿值，更新offset。

8、可选的，在s3中还包括若新的载体记录数据跳动范围小于阈值，则认定该时刻依然为静态，计算新的载体rz的平均值，记作e(k)，并计算其协方差，记作cov(k)，k表示第k次成功获取静态数据，若之后能继续成功获取静态载体姿态数据，则k＝k+1。

9、可选的，其中静态判断依据与阈值设定，具体包括：

10、从imu中读取载体姿态数据，记录1000组数据，数据中包括ax，ay，az，rz，其中ax，ay，az分别代表x轴、y轴、z轴的加速度；

11、其中阈值为：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为阈值比例系数，根据载体工作环境设定；

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

13、可选的，步骤s4中所述利用卡尔曼算法计算，具体步骤：

14、初值赋值如下：其中p(0|0)＝cov(0)，x(0|0)＝e(0),q(k)＝0.0001，q(k)根据具体情况设置，r(k)＝cov(k)，通过迭代计算，得到不同k时刻的补偿值x(k|k)；

15、x(k|k-1)＝x(k-1|k-1)；

16、其中k表示第k次测量满足静态条件的判定，此后称之为k时刻，此时，x(k-1|k-1)为k-1时刻rz的最优补偿值，即offset，x(k|k-1)为通过k-1时刻的rz最优补偿值得到的k时刻rz的估计补偿值；

17、p(k|k-1)＝p(k-1|k-1)+q(k)；

18、p(k-1|k-1)为k-1时刻的rz的最优补偿值的协方差；p(k|k-1)为k时刻的估计补偿值的协方差，q(k)为k时刻估计噪声的方差；

19、x(k|k)＝x(k|k-1)+kg(k)(e(k)–x(k|k-1))；

20、kg(k)＝p(k|k-1)/(p(k|k-1)+r(k))；

21、p(k|k)＝(1–kg(k))p(k|k-1)；

22、offset＝x(k|k)；

23、r(k)为k时刻测量噪声的方差，kg(k)为k时刻的卡尔曼增益，p(k|k)为k时刻最优补偿值的协方差，x(k|k)表示对k时刻rz的测量平均值e(k)与估计值x(k|k-1)通过卡尔曼增益值分配权重获得的k时刻rz的最优补偿值，最后将x(k|k)赋值给offset，完成rz补偿值的更新，此时的offset为获取的最终补偿值。

24、经由上述的技术方案可知，与现有技术相比，本发明提供了一种基于卡尔曼滤波的mems陀螺仪z轴零偏动态补偿方法，通过动态捕捉z轴零偏，并运用卡尔曼算法对零偏进行动态补偿，提高mems陀螺仪的精度，减小了陀螺仪z轴误差，同时降低了航向角yaw的测量误差。

1.一种基于卡尔曼滤波的mems陀螺仪z轴零偏动态补偿方法，其特征在于，包括以下步骤：

2.根据权利要求1所述的一种基于卡尔曼滤波的mems陀螺仪z轴零偏动态补偿方法，其特征在于，在s3中还包括若新的载体记录数据跳动范围小于阈值，则认定该时刻依然为静态，计算新的载体rz的平均值，记作e(k)，并计算其协方差，记作cov(k)，k表示第k次成功获取静态数据，若之后能继续成功获取静态载体姿态数据，则k＝k+1。

3.根据权利要求1或2所述的一种基于卡尔曼滤波的mems陀螺仪z轴零偏动态补偿方法，其特征在于，其中静态判断依据与阈值设定，具体包括：

4.根据权利要求1所述的一种基于卡尔曼滤波的mems陀螺仪z轴零偏动态补偿方法，其特征在于，步骤s4中所述利用卡尔曼算法计算，具体步骤：