一种基于卡尔曼滤波的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陀螺仪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轴的加速度；其中阈值为：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实测跳变均小于阈值，则判定为静态，否则为动态；因实际应用中很难保证外部噪声信号能处于较小状态，故适当增加跳动范围。进一步地，步骤S4中所述利用卡尔曼算法计算，具体步骤：初值赋值如下：其中P(0|0)＝COV(0)，X(0|0)＝E(0),Q(k)＝0.0001(Q(k)可根据具体情况设置，R(k)＝COV(k)，通过迭代计算，可以得到不同k时刻的补偿值X(k|k)；X(k|k-1)＝X(k-1|k-1)其中k表示第k次测量满足静态条件的判定(此后称之为k时刻)，此时，X(k-1|k-1)为k-1时刻Rz的最优补偿值，即Offset，X(k|k-1)为通过k-1时刻的Rz最优补偿值得到的k时刻Rz的估计补偿值；P(k|k-1)＝P(k-1|k-1)+Q(k)P(k-1|k-1)为k-1时刻的Rz的最优补偿值的协方差；P(k|k-1)为k时刻的估计补偿值的协方差，Q(k)为k时刻估计噪声的方差；X(k|k)＝X(k|k-1)+Kg(k)(E(k)–X(k|k-1))Kg(k)＝P(k|k-1)/(P(k|k-1)+R(k))P(k|k)＝(1–Kg(k))P(k|k-1)Offset＝X(k|k)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为获取的最终补偿值。进一步地，步骤S1中所述载体姿态数据为1000个连续的MEMS陀螺仪Z轴角速度Rz测量值。本专利技术的有益技术效果：一种基于卡尔曼滤波的MEMS陀螺仪Z轴零偏动态补偿方法，主要包括：步骤S1：在IMU处于静态时，获取载体姿态数据，并通过获取的载体姿态数据计算得出MEMS陀螺仪Z轴角速度Rz的补偿值作为初始补偿值Offset；步骤S2：实测中，从IMU中读取新的载体姿态数据；步骤S3：判断载体是否为静态；步骤S4：利用卡尔曼算法计算，并得到新的MEMS陀螺仪Z轴角速度Rz的补偿值，之后跳转到所述步骤S2，形成循环。本专利通过动态捕捉Z轴零偏，并运用卡尔曼算法对零偏进行动态补偿，提高MEMS陀螺仪的精度，减小了陀螺仪Z轴误差，同时降低了航向角YAW的测量误差。本专利技术的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本专利技术而了解。下面通过附图和实施例，对本专利技术的技术方案做进一步的详细描述。附图说明附图用来提供对本专利技术的进一步理解，并且构成说明书的一部分，与本专利技术的实施例一起用于解释本专利技术，并不构成对本专利技术的限制。在附图中：图1为本专利技术实施例中MEMS陀螺仪未进行补偿时测得航向角值图；图2为本专利技术实施例中MEMS陀螺仪测出斜率后做简单补偿得到航向角值图；图3为本专利技术实施例中补偿过程逻辑图；图4为本专利技术实施例中以A轴加速度Ax为例Tax选取范围示意图；图5为本专利技术实施例中将使用该算法补偿后的YAW值与没做补偿的YAW值对比图；图6为本专利技术实施例中使用该算法补偿后的YAW值与MTi-300AHRS值同步比对图；图7为本专利技术实施例中使用该算法补偿后的YAW值与MTi-300AHRS差值曲线图；图8为本专利技术实施例中经过卡尔曼滤波算法更新的补偿值图。具体实施方式以下结合附图对本专利技术的优选实施例进行说明，应当理解，此处所描述的优选实施例仅用于说明和解释本专利技术，并不用于限定本专利技术。通过对MEMS陀螺仪的长期跟踪测量，发现MEMS陀螺仪的零偏具有短时稳定，长时间微小变动的特性，拿其中一次典型测量值为例，未进行补偿时测得航向角值如图1所示。测出斜率后做简单补偿得到航向角值如图2所示，由上图可以看出，简单的补偿并不能很好的解决Z轴的零偏问题。本专利采用动态自适应卡尔曼补偿算法，通过捕捉载体静态或近似静态时刻，运用卡尔曼算法对Z轴零偏进行计算并补偿，使补偿的零偏始终接近于真实值，大大改善MEMS陀螺仪的测量精度。补偿过程逻辑图如图3所本文档来自技高网...

【技术保护点】
1.一种基于卡尔曼滤波的MEMS陀螺仪Z轴零偏动态补偿方法，其特征在于，主要包括：步骤S1：在IMU处于静态，获取载体姿态数据，并通过获取的载体姿态数据计算得出MEMS陀螺仪Z轴角速度Rz的补偿值作为初始补偿值Offset；步骤S2：实测中，从IMU中读取新的载体姿态数据；步骤S3：判断载体是否为静态；步骤S4：利用卡尔曼算法计算，并得到新的MEMS陀螺仪Z轴角速度Rz的补偿值，更新Offset，之后跳转到所述步骤S2，形成循环。

【技术特征摘要】
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...

【专利技术属性】
技术研发人员：王雷，蒋佩宇，赵炜，刘飞，
申请(专利权)人：无锡凌思科技有限公司，
类型：发明
国别省市：江苏,32