CN104880201B - Mems陀螺自动标定方法 - Google Patents

Abstract

本发明公开了一种MEMS陀螺自动标定方法，包括步骤：步骤1，基于磁强计量测数据判断环境磁场是否为准静态，并标定准静态环境磁场；步骤2，融合（1）基于伪观测数据有限的约束条件；（2加速度计量测数据；（3）标定后的磁强计量测数据；（4）加速度计与磁强计的量测数据的叉乘向量；和（5）准静态姿态更新数据构造量测向量，采用参数估计法标定陀螺误差。本发明可在无人为干预及无专用设备工具的情况下，利用便携式带陀螺设备导航过程中运动，完成陀螺标定。

Description

MEMS陀螺自动标定方法

技术领域

本发明涉及微机电系统技术领域，尤其是一种MEMS陀螺自动标定方法。

背景技术

近年来，随着微机电系统MEMS(Micro-ElectroMechanicalSystems)技术的发展而产生的MEMS陀螺具有成本低(大批量生成时)、尺寸小、重量轻、功耗低、可靠性高等优点。正因为这些优点，MEMS陀螺被广泛应用于消费类电子产品，例如手机、游戏设备、可穿戴设备等。但是，MEMS陀螺误差(主要是零偏和比例因子误差)存在巨大的逐次上电不稳定性，并可能随着使用环境(尤其是温度)变化而变化。因此，即便是进行了实验室标定，还是可能存在明显的陀螺误差。由于现有陀螺算法多包含积分环节，残留的陀螺误差将不断积累，从而导致迅速增大的姿态和位置误差。

因此，若能对MEMS陀螺误差进行在线标定和补偿，将大幅提高其定姿、定位性能。陀螺标定算法需要能在后台自动进行，且不依赖于用户的干预。这是因为电子产品用户需要在不做任何干预的情况下，因为陀螺标定而享受到更高精度的定位和定姿结果。但是，要达到这样的效果非常困难。因为绝大多数的陀螺标定算法需要外部设备或工具来提供标定用的参考真值。而实际中，不可能要求电子产品用户使用专门的设备或工具来完成标定。

传统的导航算法中常将陀螺误差作为待估参数，在定位定姿的同时对其进行估计。但是，导航算法的首要目的是定位定姿，而不是标定陀螺。因此，不论是选用量测信息，还是设定参数，其原则均是保证极端情况下错误的陀螺误差估计不会对定位定姿结果造成破坏，而非保证陀螺误差估计的精度。所以，传统导航算法中对陀螺误差的估计效果难以得到保证。

水平陀螺误差可以由加速度计量测信息进行较好估计，但是垂向陀螺误差则难以估计。因此，目前最常用的标定消费产品中陀螺的方法为：建议用户将设备保持静止，或者根据陀螺或加速度计测量值判断设备为准静态；然后对静态或准静态下陀螺的输出值求平均，来计算陀螺零偏。这种方法能有效消除陀螺零偏，但是，无法标定陀螺比例因子误差，同时需要用户人工干预，影响用户体验。而且每次使用导航前要求用户将设备保持静态或准静态以标定陀螺也不现实。

发明内容

针对现有技术存在的问题，本发明提供了一种无须用户干预、也无须任何设备或工具的MEMS陀螺自动标定方法。

为解决上述技术问题，本发明采用如下的技术方案：

MEMS陀螺自动标定方法，包括步骤：

步骤1，基于磁强计量测数据判断环境磁场是否为准静态，并标定准静态环境磁场：

根据磁强计量测数据获得时段内各时刻的环境磁场强度，根据环境磁场强度的变化程度判断时段内环境磁场是否为准静态，若为准静态，利用该时段初始时刻的磁强计量测数据和带陀螺设备姿态标定环境磁场，获得导航坐标系下环境磁场参考向量；

其中，带陀螺设备姿态根据加速度计和磁强计的量测数据确定；环境磁场标定采用公式进行，为准静态时段的环境磁场参考向量，准静态时段为环境磁场为准静态的时段；为准静态时段初始时刻的磁强计量测数据向量；为设备坐标系向导航坐标系转换的方向余弦矩阵，由时段初始时刻的带陀螺设备姿态获得；

步骤2，陀螺误差标定：

融合(1)基于伪观测数据有限的约束条件；(2加速度计量测数据；(3)标定后的磁强计量测数据；(4)加速度计与磁强计的量测数据的叉乘向量；和(5)准静态姿态更新数据构造量测向量，采用参数估计法标定陀螺误差；准静态姿态更新数据量测模型 为准静态环境磁场下陀螺输出，b_g为陀螺零偏，n₅为量测噪声；

其中，在陀螺误差标定中，带陀螺设备的加速度大于预设加速度阈值Th_acc2时，不使用加速度计量测数据；磁强计量测数据和准静态姿态更新数据仅在准静态环境磁场下使用；若同时使用加速度计和磁强计的量测数据，则采用紧组合方式。

步骤1中，所述的环境磁场强度为(1)磁强计三轴输出中任意两轴输出的模、平方和或绝对值之和；或(2)磁强计三轴输出的模、平方和或绝对值之和。

步骤1中，所述的根据环境磁场强度的变化程度判断时段内环境磁场是否为准静态，具体为：

若时段内各时刻环境磁场强度的变化范围小于阈值Th_mag1、且标准差小于阈值Th_mag2，则该时段环境磁场为准静态；否则，不为准静态；阈值Th_mag1和Th_mag2为经验值，对准静态环境磁场下时段内各时刻环境磁场强度进行统计学处理获得。

所述的阈值Th_mag1和阈值Th_mag2采用如下方法获得：

在准静态环境磁场下获得时段内各时刻的环境磁场强度，以时段内各时刻环境磁场强度的变化范围和标准差分别作为阈值Th_mag1和阈值Th_mag2。

步骤2中，伪观测数据量测模型为 和分别为惯性导航推算位置和伪位置，伪位置即上一时刻惯性导航推算位置，δrⁿ为位置误差，n₁为量测噪声。

所述的量测噪声n₁采用如下方法获得：随机设定量测噪声n₁初始标准差；获得时段内带陀螺设备位置变化的时间序列；以时间序列标准差为量测噪声n₁的标准差。

步骤2中，加速度计的量测噪声基于当前时刻带陀螺设备的加速度设定：

带陀螺设备加速度A≤|Th_acc1|，以加速度计零偏稳定性为量测噪声标准差；带陀螺设备加速度|Th_acc1|≤A≤|Th_acc2|，以(A²/P)σ_a为量测噪声标准差，P为姿态误差协方差，σ_a为加速度计零偏稳定性；

阈值Th_acc1和阈值Th_acc2采用如下方法获得：

携带带陀螺设备分别步行和跑步，采集加速度计量测数据f^b，获得时段内各时刻对应的加速度A＝|norm(f^b)-g|；步行环境下时段内所有时刻的加速度A的均方根即阈值Th_acc1，跑步环境下时段内所有时刻的加速度A的均方根即阈值Th_acc2。

步骤2中，准静态姿态更新数据对应量测噪声n₅的标准差为：静止环境下时段内所有时刻陀螺量测数据的模的均方根。

本发明提出了一种陀螺的自动标定方法，可在不需任何用户干预，也不需要任何设备或工具的情况下，完成对消费电子产品中低成本MEMS陀螺的标定。

和现有技术相比，本发明具有如下优点和有益效果：

1、使用多种量测信息标定陀螺，包括伪观测信息、加速度计量测信息、磁强计量测信息、加速度计与磁强计的量测信息叉乘向量、准静态姿态更新等，多种量测信息的使用可保证陀螺标定的精度和可靠性。

2、采用紧组合方式使用加速度计和磁强计的量测信息，即直接使用加速度计和磁强计的输出构造量测向量，而非使用由加速度计量测信息求得的俯仰角、横滚角以及由加速度计和磁强计的量测信息求得的航向角；这样可保证本发明在各种行人动作下均适用且不会出现俯仰角接近±90°时的计算奇异问题；同时，可使磁强计量测完全不依赖加速度计信息；此外，还可以保证构造的加速度计与磁强计的量测信息叉乘向量的可用性。

3、可对陀螺周围环境磁场进行探测，仅当一段时间内环境磁场保持准静态时使用磁强计量测信息；同时，当判断某一时段内环境磁场强度为准静态后，利用磁强计量测信息标定该时段内的环境磁场。因此，本发明不需要任何环境磁场的信息即可利用磁强计量测信息提高陀螺标定精度。从而，本发明可以在周围环境磁场未知或受到严重磁场干扰的环境(如室内或高楼林立的地带)工作。

4、根据实际使用场景(如载体动态和周围环境)，由软件自适应调整和设置各量测向量的精度：例如利用加速度计输出的模来调整重力向量量测精度；利用环境磁场输出的模的稳定性来调整环境磁场向量量测精度等。因此，本发明可完全自动在后台完成，不需要任何人工干预。

5、本发明数据处理即可实时进行，也可事后处理。若采用卡尔曼滤波、递推最小二乘、粒子滤波等参数估计方法进行实时处理，则在数据处理过程中，已经不断对陀螺误差进行估计并反馈修正，导航结束后，即已经完成标定，不需要后处理；若对数据处理进行一次反向平滑处理，可进一步提高结果精度。若选用事后处理，则可在标定计算结束后的短时间内，解算出陀螺误差。

6，本发明方法可单独使用，也可和定位手段，如GNSS、WiFi、气压计、地图匹配、行人运动模型等配合使用。

附图说明

图1为本发明实施例的具体流程图；

图2为本发明实施例的算法流程图；

图3为本发明实施例磁强计数据使用结构图；

图4为实施例中手机在动态1下的室外标定结果，其中图(a)、(b)、(c)分别为手机#1、#2、#3在动态1下的室外标定结果；

图5为实施例中手机在动态2下的室外标定结果，其中图(a)、(b)、(c)分别为手机#1、#2、#3在动态2下的室外标定结果；

图6为实施例中手机在动态3下的室外标定结果，其中图(a)、(b)、(c)分别为手机#1、#2、#3在动态3下的室外标定结果；

图7为实施例中手机在动态4下的室外标定结果，其中图(a)、(b)、(c)分别为手机#1、#2、#3在动态4下的室外标定结果；

图8为实施例中手机在动态5下的室外标定结果，其中图(a)、(b)、(c)分别为手机#1、#2、#3在动态5下的室外标定结果；

图9为实施例中手机在动态6下的室外标定结果，其中图(a)、(b)、(c)分别为手机#1、#2、#3在动态6下的室外标定结果；

图10为实施例中手机在动态1下的室内标定结果，其中图(a)、(b)、(c)分别为手机#1、#2、#3在动态1下的室内标定结果；

图11为实施例中手机在动态2下的室内标定结果，其中图(a)、(b)、(c)分别为手机#1、#2、#3在动态2下的室内标定结果；

图12为实施例中手机在动态3下的室内标定结果，其中图(a)、(b)、(c)分别为手机#1、#2、#3在动态3下的室内标定结果；

图13为实施例中手机在动态4下的室内标定结果，其中图(a)、(b)、(c)分别为手机#1、#2、#3在动态4下的室内标定结果；

图14为实施例中手机在动态5下的室内标定结果，其中图(a)、(b)、(c)分别为手机#1、#2、#3在动态5下的室内标定结果；

图15为实施例中手机在动态6下的室内标定结果，其中图(a)、(b)、(c)分别为手机#1、#2、#3在动态6下的室内标定结果。

具体实施方式

本发明无需任何人工干预，也不需要保持陀螺为静态或准静态，一旦传感器开始工作即可进入陀螺自动标定，流程图参见图1，具体步骤如下：

步骤1，使用时段初始时刻的加速度计和磁强计的量测数据，确定带陀螺的设备(以下简称为“设备”)的初始姿态。

一旦设备内传感器开始工作，即开始执行步骤1。

设备初始姿态即设备在时段初始时刻下的横滚角、俯仰角和航向角。

横滚角和俯仰角可根据加速度计的量测数据计算得到，为本领域内的公知技术，计算公式如下：

$\begin{array}{c}\mathrm{\φ}=atan2(-f_y,{-f}_z)\\ \mathrm{\θ}=atan2(f_x,\sqrt{f_y^2+f_z^2})\end{array}---\left(1\right)$

式(1)中，φ和θ分别为俯仰角和横滚角，f_i(i＝x,y,z)表示i轴加速度计在时段初始时刻的量测数据，或i轴加速度计在时段内(如1秒内)的量测数据平均值。

本步骤的特点有：1)利用准静态时段内的磁强计量测数据计算设备航向的相对变化量，从而标定陀螺，而不需计算绝对的设备航向；2)对准静态环境磁场进行标定。因此，本发明不需要精确的绝对航向信息。所以，可将航向角设置为任意值(如0°)，或者直接使用准静态时段首时刻的磁强计量测数据计算得到的航向角作为初始航向。

步骤2，持续采集陀螺、加速度计、磁强计的量测数据，采用时段内磁强计量测数据判断该时段内环境磁场是否为准静态，若为准静态，采用该时段初始时刻的磁强计量测数据和设备姿态标定环境磁场。所述的时段，其长度预先设定。

本步骤中，采用磁强计探测环境磁场，当时段内环境磁场保持准静态时，对该时段内环境磁场进行标定，并求取环境磁场参考向量。因此，本发明用于磁场干扰严重的室内环境时，仍可提供稳定可靠的陀螺标定结果。

本发明根据磁强计量测数据考察环境磁场在时段内的稳定性，来判断环境磁场是否为准静态。若时段内环境磁场强度的变化范围(即最大磁场强度和最小磁场强度之差)小于阈值Th_mag1，同时标准差小于阈值Th_mag2，则认为该时段环境磁场为准静态；否则不为准静态。

上述环境磁场强度根据磁强计量测数据获得，例如，环境磁场强度可以为(1)磁强计三轴输出中任意两轴输出的模、平方和或绝对值之和；或(2)磁强计三轴输出的模、平方和或绝对值之和。将磁强计三轴输出分别记为x、y、z，则环境磁场强度可以为三轴输出的模，即也可以为任意两轴输出的模，即

阈值Th_mag1和Th_mag2通过事先携带设备在室内按照预设轨迹多次走动所采集的多组磁强计量测数据统计得到，具体方法为：

对于每组磁强计量测数据，计算各采样时刻的环境磁场强度，分别计算各时段内环境磁场强度的变化范围和标准差；所有时段内环境磁场强度的变化范围和标准差的均方根，即阈值Th_mag1和Th_mag2；或者，根据室内磁干扰情况对均方根乘以修正系数，并将修正的变化范围和标准场的均方根作为阈值Th_mag1和Th_mag2。

阈值Th_mag1和Th_mag2Th_mag2是通过对准静态环境磁场下各时段内环境磁场强度的变化范围和标准差进行统计学处理获得，其设定方法并不限于上述，只要能反应准静态环境磁场下环境磁场强度的变化程度的阈值均可。

环境磁场的标定是利用时段初始时刻的磁强计量测数据，结合该初始时刻设备姿态计算导航坐标系下环境磁场参考向量。本具体实施中采用的导航坐标系为n系，是以加速度计测量中心为坐标原点，结合正北、正东以及垂直朝下这三个方向构成的直角坐标系。

本发明中环境磁场标定的公式如下：

$m_k^n=C_b^n{\tilde{m}}_{k,1}^b---\left(2\right)$

式(2)中，表示第k个准静态时段的环境磁场参考向量，这里的准静态时段表示环境磁场为准静态的时段；为第k个准静态时段初始时刻的磁强计量测数据向量，即磁强计输出；为设备坐标系向导航坐标系转换的方向余弦矩阵，设备坐标系为b系，是以加速度计测量中心为坐标原点，结合加速度计轴线构成的直角坐标系。

由时段初始时刻姿态角计算得到：

$C_b^n=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& -\cos \mathrm{\φ}\sin \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\φ}\cos \mathrm{\ψ}+\cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}\\ -\sin \mathrm{\θ}& \sin \mathrm{\φ}\cos \mathrm{\θ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]---\left(3\right)$

式(3)中，φ为设备的俯仰角，θ为设备的横滚角，ψ为设备的航向角。

下面将以陀螺零偏和比例因子误差估计为例，对本步骤进行详细说明。

采用陀螺误差模型进行误差估计，陀螺误差模型如下：

${\mathrm{\δ\ω}}_{ib}^b=b_g+diag\left({\widetilde{\mathrm{\ω}}}_{ib}^b\right){\mathrm{\δs}}_g+w_g---\left(4\right)$

式(4)中，为陀螺输出误差向量，b_g和δs_g分别为陀螺的零偏和比例因子误差，w_g为白噪声；为陀螺量测数据向量，即陀螺输出；符号diag(v)表示由向量v中元素构成的对角阵。

当然，采用本发明还可以同时对其他陀螺参数，如交轴耦合误差、非线性误差等进行建模和估计，其他陀螺参数的建模和估计属于本技术领域内的公知技术，在此不做赘述。

本具体实施中采用卡尔曼滤波进行信息融合以完成陀螺标定。卡尔曼滤波状态模型选用惯性导航误差方程，如下：

$\left[\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{r}}^n\\ \mathrm{\δ}{\stackrel{\·}{v}}^n\\ \stackrel{\·}{\mathrm{\ψ}}\\ {\stackrel{\·}{b}}_g\\ \mathrm{\δ}{\stackrel{\·}{s}}_g\\ \·\\ \·\\ \·\end{array}\right]=\left[\begin{array}{c}-{\mathrm{\ω}}_{en}^n\×{\mathrm{\δr}}^n+{\mathrm{\δv}}^n\\ -2{\mathrm{\ω}}_{ie}^n\×{\mathrm{\δv}}^n+f^n\×\mathrm{\ψ}\\ -{\mathrm{\ω}}_{ie}^n\×\mathrm{\ψ}-C_b^n{\mathrm{\δ\ω}}_{ib}^b\\ -(1/{\mathrm{\τ}}_{bg})b_g+w_{bg}\\ -(1/{\mathrm{\τ}}_{sg}){\mathrm{\δs}}_g+w_{sg}\\ \·\\ \·\\ \·\end{array}\right]---\left(5\right)$

式(5)中，δrⁿ，δvⁿ，ψ，b_g和δs_g构成了卡尔曼滤波待估状态向量，δrⁿ，δvⁿ和ψ分别为位置误差、速度误差和姿态误差；分别为δrⁿ、δvⁿ、ψ、b_g和δs_g对时间的微分；为设备坐标系向导航坐标系转换的方向余弦矩阵；fⁿ＝[00-g]^T，为导航坐标系中比力向量，其中g为本地重力值；为地球自转角速度，为由于载体在地球椭球表面运动造成的角速度；为陀螺的输出误差；τ_bg和τ_sg为分别为陀螺零偏和比例因子的相关时间，w_bg和w_sg分别为陀螺零偏和比例因子的驱动白噪声。

步骤3，利用伪观测数据、加速度计量测数据、磁强计量测数据、加速度计与磁强计量测数据的叉乘向量、准静态姿态更新数据构造量测向量，采用参数估计法对陀螺误差进行估计和反馈修正。

本发明利用多种传感器输出或导航先验信息来构造量测方程，完成对陀螺误差的估计。下面将逐一介绍各种量测数据的使用。量测噪声的设定即可人工设定，也可根据设备动态，由软件自动完成，不需要人工干预。各种先验信息的将在下文分别介绍：

伪观测数据即导航环境下相邻两时刻间设备的位置变化范围，本发明中伪观测数据有限为约束条件。本发明提出了能在行人导航过程中使用的伪位置和伪速度量测。以伪位置为例，其量测模型如下：

${\hat{r}}^n-{\tilde{r}}^n={\mathrm{\δr}}^n+n_1---\left(6\right)$

式(6)中，和分别为惯性导航推算的位置和伪位置，使用上一时刻惯性导航推算的位置为当前时刻伪位置，δrⁿ为位置误差，n₁为伪位置量测噪声，即实际导航过程中设备运动的范围。

本实施例由软件自动设定伪观测量测噪声的一种具体方法为：

首先，随机设定量测噪声初始标准差；然后，采用本发明方法进行陀螺标定得到当前时段设备位置变化的时间序列；随后，计算该时间序列的标准差，以该标准差为伪观测量测噪声的标准差。

本发明使用紧组合的方式使用加速度计和磁强计的量测数据，即直接使用加速度计、磁强计的量测数据构造量测向量，而非使用由加速度计量测数据求得的俯仰角、横滚角以及由加速度计和磁强计的量测数据求得的航向角。此举可保证本发明在各种行人动作下均可用且不会出现俯仰角接近±90°时的计算奇异问题；同时，可使磁强计量测完全不依赖加速度计数据；此外，还可以保证构造的加速度计与磁强计叉乘向量数据的有用性。

本具体实施中，加速度计相应的量测模型如下：

${\mathrm{\δf}}^n=\[g^n\×\]\mathrm{\ψ}+C_b^n{n}_2---\left(7\right)$

式(7)中，gⁿ＝[00g]^T，g为本地重力值；ψ为姿态误差，n₂为量测噪声。为加速计量测误差向量，fⁿ＝-gⁿ， 为加速度计输出向量。

任一时刻的量测噪声n₂可基于该时刻的加速度A＝|norm(f^b)-g|设定：

无加速度情形下，即A≤|Th_acc1|，量测噪声n₂标准差设为为σ_a，σ_a为加速度计零偏稳定性；较小加速度情形下，即|Th_acc1|≤A≤|Th_acc2|，量测噪声n₂标准差设为(A²/P)σ_a，P为卡尔曼滤波法估计的姿态误差ψ的协方差；较大加速度情形下，即A≥|Th_acc2|，不使用加速度计量测数据进行陀螺标定。

阈值Th_acc1和Th_acc2根据均匀移动环境和加速度移动环境下各时刻的加速度A＝|norm(f^b)-g|值确定，具体为：

携带设备分别均匀走路和跑步，采集加速度计量测数据f^b，根据量测数据f^b计算均匀走路和跑步环境下各时刻对应的加速度A＝|norm(f^b)-g|；将均匀走路环境下所有时刻的加速度值求均方根，即阈值Th_acc1；将跑步环境下所有时刻的加速度值求均方根，即阈值Th_acc2。

阈值Th_acc1和Th_acc2是通过对均匀移动和加速度移动下采集的设备各时刻加速度A值经统计学处理获得，其设定方法并不限于上述。

基于标定后的环境磁场参考向量构建磁强计量测模型，如下：

${\mathrm{\δm}}^n=\[m^n\×\]\mathrm{\ψ}+C_b^n{n}_3---\left(8\right)$

式(8)中，为磁强计量测误差向量，为磁强计输出，mⁿ为标定后的环境磁场参考向量，n₃为量测噪声。

磁强计量测数据向量的使用是基于环境磁场的稳定性，只有当环境磁场为准静态时才使用。其量测噪声n₃设定方式类似于量测噪声n₂。当环境磁场为准静态时，量测噪声n₃设为磁强计生产厂家提供的磁强计白噪声σ_m；当环境磁场不为准静态、且时段内环境磁场强度标准差不大于阈值Th_mag2时，量测噪声n₃标准差设定为(M²/P)σ_m，M为磁场强度标准差，P为卡尔曼滤波法估计的姿态误差ψ的协方差；当环境磁场不为准静态、且时段内环境磁场强度标准差大于阈值Th_{mag_max}时，不使用磁强计量测数据。

阈值Th_{mag_max}根据多种室内环境环境磁场强度的变化确定，具体为：

携带设备分别在多种室内环境步行，分别采集磁强计量测数据m^b。对于各种室内环境对应的各组磁强计量测数据，分别计算时段内(该时段长度同环境磁场准静态判断是标定的时间长度)环境磁场强度标准差；所有环境磁场强度标准差的最大值，即阈值Th_{mag_max}。

加速度计与磁强计的量测数据叉乘向量相应的量测模型如下：

$\mathrm{\δ}{1}^n=\[1^n\×\]\mathrm{\ψ}+{\hat{C}}_b^n{n}_4---\left(9\right)$

式(9)中，lⁿ＝fⁿ×mⁿ；和分别为加速度计和磁强计的量测数据向量，n₄为量测噪声，其标准差σ_n2、σ_n3和σ_n4分别为加速度计量测、磁强计量测和二者叉乘向量的量测噪声标准差。

准静态姿态更新相应的量测模型如下：

当判断为准静态时，可施加准静态姿态更新，即认为导航算法求得的姿态变化均由陀螺零偏造成。相应的量测模型如下：

${\widetilde{\mathrm{\ω}}}_{ib}^b=b_g+n_5---\left(10\right)$

式(10)中，为准静态下陀螺输出，b_g为陀螺零偏，n₅为量测噪声。

量测噪声n₅的标准差根据事先手持设备保持静止采集的多组陀螺数据统计计算得到，具体为：分别计算各时刻陀螺输出的模，所有时刻陀螺输出的模的均方根值即量测噪声的标准差。

若使用其他定位手段，如测GNSS、WiFi、气压计、地图匹配、行人运动模型等，也可使用其定位结果构造量测方程，提高标定结果。

以WiFi定位结果为例，其相应的量测方程为：

${\hat{r}}^n-{\tilde{r}}_{WiFi}^n={\mathrm{\δr}}^n+n_6---\left(11\right)$

式(12)中，和分别为惯性导航推算的位置和WiFi提供的位置，δrⁿ为位置误差，n₆为量测噪声。

步骤4，重复步骤2～3，直至导航结束。

下面将结合具体应用实例进一步说明本发明技术效果。

测试所用带陀螺设备为三星GalaxyS3(两支)和GalaxyS4(一支)手机，分别进行室外和室内标定实验，两支三星GalaxyS3手机分别记为手机#1和手机#2，三星GalaxyS4手机记为手机#3。每次实验携带一只手机，以一种手机动态行走。所述手机动态包括手持端平、打电话、随手摆动、放在口袋、放在腰包以及放在背包，手持端平、打电话、随手摆动、放在口袋、放在腰包以及放在背包分别记为手机动态1、手机动态2、手机动态3、手机动态4、手机动态5、手机动态6。每次实验后，将手机保持静止一分钟，以求取陀螺零偏参考真值。因为无法求取陀螺比例因子真值，所以，在此仅展示陀螺零偏的估计效果。室外和室内标定实验结果分别见图4～15。图4～15横坐标轴上(即纵坐标为0)的线段表示准静态环境磁场时段，最后一段时间出现的短线为陀螺零偏参考真值。

室外测试和室内测试陀螺零偏估计误差统计结果见表1-4，其中表1和表3按照所用手机分类，表2和表4按照手机动态分类。

表1室外测试实验估计误差

表2室外测试实验估计误差

表3室内测试实验估计误差

表4室内测试实验估计误差

可见，本发明方法将测试手机的陀螺零偏降至室外低于0.1deg/s、室内低于0.15deg/s。在行人导航最具挑战性的随手摆动和放在口袋的手机动态下，本发明室内标定误差分别为0.17deg/s和0.14deg/s，室外标定误差分别为0.13deg/s和0.09deg/s。而在其他手机动态下，室内和室外标定误差分别小于0.12deg/s和0.07deg/s。因此，本发明在各种手机动态下均可有效标定陀螺误差，且在存在磁场干扰的室内环境下仍能很好工作。

Claims (8)

1.MEMS陀螺自动标定方法，其特征在于，包括步骤：

步骤1，基于磁强计量测数据判断环境磁场是否为准静态，并标定准静态环境磁场：

根据磁强计量测数据获得时段内各时刻的环境磁场强度，根据环境磁场强度的变化程度判断时段内环境磁场是否为准静态，若为准静态，利用该时段初始时刻的磁强计量测数据和带陀螺设备姿态标定环境磁场，获得导航坐标系下环境磁场参考向量；

其中，带陀螺设备姿态根据加速度计和磁强计的量测数据确定；环境磁场标定采用公式进行，为准静态时段的环境磁场参考向量，准静态时段为环境磁场为准静态的时段；为准静态时段初始时刻的磁强计量测数据向量；为设备坐标系向导航坐标系转换的方向余弦矩阵，由时段初始时刻的带陀螺设备姿态获得；

步骤2，陀螺误差标定：

融合(1)基于伪观测数据有限的约束条件；(2加速度计量测数据；(3)标定后的磁强计量测数据；(4)加速度计与磁强计的量测数据的叉乘向量；和(5)准静态姿态更新数据构造量测向量，采用参数估计法标定陀螺误差；准静态姿态更新数据量测模型 为准静态环境磁场下陀螺输出，b_g为陀螺零偏，n₅为量测噪声；

其中，在陀螺误差标定中，带陀螺设备的加速度大于预设加速度阈值Th_acc2时，不使用加速度计量测数据；磁强计量测数据和准静态姿态更新数据仅在准静态环境磁场下使用；若同时使用加速度计和磁强计的量测数据，则采用紧组合方式。

2.如权利要求1所述的MEMS陀螺自动标定方法，其特征在于：

步骤1中，所述的环境磁场强度为(1)磁强计三轴输出中任意两轴输出的模、平方和或绝对值之和；或(2)磁强计三轴输出的模、平方和或绝对值之和。

3.如权利要求1所述的MEMS陀螺自动标定方法，其特征在于：

步骤1中，所述的根据环境磁场强度的变化程度判断时段内环境磁场是否为准静态，具体为：

若时段内各时刻环境磁场强度的变化范围小于阈值Th_mag1、且标准差小于阈值Th_mag2，则该时段环境磁场为准静态；否则，不为准静态；阈值Th_mag1和Th_mag2为经验值，对准静态环境磁场下时段内各时刻环境磁场强度进行统计学处理获得。

4.如权利要求3所述的MEMS陀螺自动标定方法，其特征在于：

所述的阈值Th_mag1和阈值Th_mag2采用如下方法获得：

在准静态环境磁场下获得时段内各时刻的环境磁场强度，以时段内各时刻环境磁场强度的变化范围和标准差分别作为阈值Th_mag1和阈值Th_mag2。

5.如权利要求1所述的MEMS陀螺自动标定方法，其特征在于：

步骤2中，伪观测数据量测模型为 和分别为惯性导航推算位置和伪位置，伪位置即上一时刻惯性导航推算位置，δrⁿ为位置误差，n₁为量测噪声。

6.如权利要求5所述的MEMS陀螺自动标定方法，其特征在于：

所述的量测噪声n₁采用如下方法获得：随机设定量测噪声n₁初始标准差；获得时段内带陀螺设备位置变化的时间序列；以时间序列标准差为量测噪声n₁的标准差。

7.如权利要求1所述的MEMS陀螺自动标定方法，其特征在于：

步骤2中，加速度计的量测噪声基于当前时刻带陀螺设备的加速度设定：

带陀螺设备加速度A≤|Th_acc1|，以加速度计零偏稳定性为量测噪声标准差；带陀螺设备加速度|Th_acc1|≤A≤|Th_acc2|，以(A²/P)σ_a为量测噪声标准差，P为姿态误差协方差，σ_a为加速度计零偏稳定性；

阈值Th_acc1和阈值Th_acc2采用如下方法获得：

携带带陀螺设备分别步行和跑步，采集加速度计量测数据f^b，获得时段内各时刻对应的加速度A＝|norm(f^b)-g|，g为本地重力值；步行环境下时段内所有时刻的加速度A的均方根即阈值Th_acc1，跑步环境下时段内所有时刻的加速度A的均方根即阈值Th_acc2。

8.如权利要求1所述的MEMS陀螺自动标定方法，其特征在于：

步骤2中，准静态姿态更新数据对应量测噪声n₅的标准差为：静止环境下时段内所有时刻陀螺量测数据的模的均方根。