本发明涉及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所示，一种基于卡尔曼滤波的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实测跳变均小于阈值，则判定为静态，否则为动态；因实际应用中很难保证外部噪声信号能处于较小状态，故适当增加跳动范围，以a轴加速度ax为例，示意图如图4所示，此处m＝1.5。
根据mems陀螺仪的零偏具有短时稳定，长时间微小变动的特性，应用单变量卡尔曼滤波算法进行零偏补偿自更新：
对于线性系统，正常卡尔曼滤波算法如下：
x(k)＝ax(k-1)+bu(k)+w(k)
z(k)＝hx(k)+v(k)
x(k)是k时刻的系统状态，u(k)是k时刻对系统的控制量。a和b是系统参数。z(k)是k时刻的测量值，h是测量系统的参数。w(k)和v(k)分别表示过程和测量的噪声，他们的协方差分别是q和r
x(k|k-1)＝ax(k-1|k-1)+bu(k)
(1)
式(1)中，x(k|k-1)是利用上一状态预测的结果，x(k-1|k-1)是上一状态最优的结果，u(k)为现在状态的控制量，
p(k|k-1)＝ap(k-1|k-1)a’+q(k)
(2)
式2中，p(k|k-1)是x(k|k-1)对应的协方差，p(k-1|k-1)是x(k-1|k-1)对应的协方差，
x(k|k)＝x(k|k-1)+kg(k)(z(k)–hx(k|k-1))
(3)
kg＝p(k|k-1)h’/(hp(k|k-1)h’+r(k))
(4)
其中kg为卡尔曼增益，x(k|k)为k时刻卡尔曼滤波结果，即最优估计值。
p(k|k)＝(1–kg(k)h)p(k|k-1)
(5)
p(k|k)为更新的k时刻协方差。
上面公式(1)-(5)就是卡尔曼滤波的5大公式，在本专利应用中,不存在系统控制量，观测值即为系统状态，u(k)＝0，h＝1，假定上一次的补偿值与下一次的预测值相同，则a＝1，协方差初值p(0|0)＝cov(0)，其中k表示第k次1000个点的陀螺仪转速值被判断为静态，则有z(k)＝e(k)，其中e(k)为该第k次所取1000个rz测量值的平均值，即被判定为静态下的z轴陀螺仪取值的1000点平均值e即为k时刻的系统状态值，在迭代计算中r(k)＝cov(k)，cov(k)表示该时刻1000个rz值的协方差。此时卡尔曼增益简化为kg＝p(k|k-1)/(p(k|k-1)+r(k))。根据以上描述，本专利中卡尔曼滤波计算过程则简化成如下公式：
进一步地，步骤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测量值。
将使用该算法补偿后的yaw值与没做补偿的yaw值做对比，对比图如图5所示。
将使用该算法补偿后的yaw值与mti-300ahrs做同步比对，可以看出该算法可以有效降低mems陀螺仪零偏导致的误差如图6-7所示，在该过程中，经过卡尔曼滤波算法更新的补偿值如图8所示。
实验证明，对于间歇性运动的载体，该自适应动态卡尔曼滤波滤波补偿算法可以有效的对mems陀螺仪z轴零偏进行间歇校正，有效提高了mems陀螺仪z轴航向角的测量精度。
根据mems陀螺仪的零偏具有短时稳定，长时间微小变动的特性，该专利将自适应动态零偏补偿算法与单变量卡尔曼滤波相结合，形成可自动调整补偿参数的自适应补偿系统，自适应动态补偿可以随时间变化，逐渐调整零偏的补偿值，使补偿值随imu状态变化而自动变化，卡尔曼滤波具有估计准确的特性，将卡尔曼滤波与自适应动态补偿相结合，使得系统的补偿值在自动更新的同时更能贴近真实的补偿值，使补偿结果更加精确，从而提高imu的测量精度。
至少可以达到以下有益效果：
一种基于卡尔曼滤波的mems陀螺仪z轴零偏动态补偿方法，主要包括：步骤s1：在imu处于静态状态时，获取载体姿态数据，并通过获取的载体姿态数据计算得出mems陀螺仪z轴角速度rz的补偿值offset；步骤s2：实测中，从imu中读取新的载体姿态数据；步骤s3：判断载体是否为静态；步骤s4：利用卡尔曼算法计算，并得到新的mems陀螺仪z轴角速度rz的补偿值，更新offset，之后跳转到所述步骤s2，形成循环。本专利通过动态捕捉z轴零偏，并运用卡尔曼算法对零偏进行动态补偿，提高mems陀螺仪的精度，减小了陀螺仪z轴误差，同时降低了航向角yaw的测量误差。
最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。