CN103900613A - 一种基于磁力计n阶距检测的mems系统误差估计方法 - Google Patents

Abstract

本发明涉及一种基于磁力计N阶距检测的MEMS系统误差估计方法，首先利用磁力计输出的磁场强度，利用N阶矩检测法判断系统是否受到外界磁场干扰，若所测试的N阶距差值若小于等于阈值，则触发MEMS系统误差估计卡尔曼滤波器工作，结合磁力计输出估计出MEMS系统的位置、速度、姿态误差，以及MEMS陀螺的刻度系数误差、陀螺漂移；若大于阈值，则MEMS系统误差估计卡尔曼滤波器关闭。本发明方法可以在无卫星信号条件下，实现MEMS系统误差的估计，维持MEMS系统的自主性及精度可靠性。

Description

一种基于磁力计N阶距检测的MEMS系统误差估计方法

技术领域

本发明涉及导航领域中MEMS(Micro-electromechanical Systems)系统误差估计方法，特别是涉及一种基于磁力计N阶距检测的MEMS系统误差估计方法。

背景技术

现有的MEMS导航系统具有价格低、体积小、质量轻等优点，在低精度导航领域得到了广泛的应用。但由于其较大的陀螺漂移，通常需与卫星导航系统组合使用。但卫星信号信号为无线电波，在传播过程中易被建筑物、树木、峡谷等遮挡，特别是在室内受钢筋混凝土的阻隔而导致无法有效接收卫星信号，造成卫星信号失效，进而使得基于MEMS的组合导航系统的可靠性大大降低，定位精度急剧下降。为了实现系统在有无卫星辅助条件下的无缝切换，通常的解决方法是增加额外传感器，以提高系统的可信度。MEMS定位精度较差，其主要原因是由于陀螺漂移造成的。因此，如何估计出MEMS系统误差参数，提高MEMS系统的测姿及定位精度成为了关键问题。

现有的MEMS系统误差通常通过与卫星导航系统进行组合滤波的方式进行估计。但该方法在无卫星信号或卫星信号失效时，无法对MEMS系统误差进行有效估计。磁力计通过匹配当地磁场与地球磁场工作，并不依靠卫星信号等无线电信号工作。磁力计一般被用于对MEMS的航向误差进行补偿，并没有充分利用磁力计的输出信息对MEMS系统的其他误差进行估计。

发明内容

本发明的目的在于针对现有技术中存在的缺陷，提供一种基于磁力计N阶距检测的MEMS系统误差估计方法，

本发明的目的是这样实现的：

包括以下步骤：

步骤一、MEMS系统、磁力计开始工作，在计算机中实时存储MEMS系统输出的速度信息，磁力计测得的磁感应强度信息；

步骤二、利用N阶矩检测法判断是否存在外界磁干扰，所测试的N阶距差值若小于等于阈值，则触发MEMS系统误差估计卡尔曼滤波器工作，估计出MEMS系统的位置、速度、姿态误差，以及MEMS陀螺的刻度系数误差、陀螺漂移；若大于阈值，则MEMS系统误差估计卡尔曼滤波器关闭；

所涉及的N阶矩检测法判断条件为：

式中，b表示载体坐标系；

表示t₁时刻b系下的磁场向量；k表示磁场恒定阶段的采样点数；||·||₂表示向量的二范数；N为阶矩值；υ表示判断条件阈值；

所涉及的MEMS系统误差估计卡尔曼滤波器滤波器，其状态向量为：

${x_k}^{\mathrm{def}}={\left[\begin{array}{ccccc}{\left(x_k^1\right)}^T& {\left(x_k^2\right)}^T& {\left(x_k^3\right)}^T& {\left(x_k^4\right)}^T& {\left(x_k^5\right)}^T\end{array}\right]}^T$

式中，

表示定义为分别表示MEMS系统的位置、速度、姿态误差；

表示刻度系数误差；

表示MEMS陀螺漂移；

同时除去常规状态量外，关于刻度系数误差的微分方程为：

${\stackrel{\·}{x}}^4=\mathrm{\δ}{\stackrel{\·}{K}}_g=0$

式中，δK_g为MEMS陀螺标度系数误差。

所涉及的MEMS系统误差估计卡尔曼滤波器的6维量测量Z_6×1为：

$Z_{6\×1}={\left[\begin{array}{cc}{Z}_1^T& Z_2^T\end{array}\right]}^T={\left[\begin{array}{cccccc}\mathrm{\δ}{V}_x& \mathrm{\δ}{V}_y& \mathrm{\δ}{V}_z& \mathrm{\δ}{B}_x& \mathrm{\δ}{B}_y& \mathrm{\δ}{B}_z\end{array}\right]}^T$

式中，Z₁表示MEMS系统速度差量测量，Z₂表示磁力计测得的当地磁场与已知地球磁场的磁场强度差量测量，δV_x,δV_y,δV_z表示速度差量测量在在当地水平坐标系三个轴上的分量,表示由惯导解算出速度估计值与真实值之间的误差，具体可由速度估计值与速度基准值做差即可，其中速度基准可由卫星或者多普勒等等设备直接读取；δB_x,δB_y,δB_z表示磁力计测得的当地磁场与已知地球磁场的磁场强度差在当地水平坐标系三个轴上的分量；

所涉及的MEMS系统误差估计卡尔曼滤波器的量测方程为：

$Z_1=\left[I_{3\×3}\right]\left[\begin{array}{c}\mathrm{\δ}{V}_x\\ \mathrm{\δ}{V}_y\\ \mathrm{\δ}{V}_z\end{array}\right]+w_v$

式中，

w_v表示速度量测白噪声；w表示磁力计系统白噪声；

分别为测力计测得的磁场强度；δK_gx,δK_gy,δK_gz分别为MEMS陀螺标度系数误差δK_g在当地水平坐标系三轴上的分量；ε_bx,ε_by,ε_bz分别为MEMS陀螺常值漂移ε_b在当地水平坐标系三轴上的分量；ε_rx,ε_ry,ε_rz分别为MEMS陀螺随机漂移ε_r在当地水平坐标系三轴上的分量；

分别为载体坐标系对惯性坐标系转动角速率

在当地水平坐标系三轴上的分量。

本发明的有益效果为：

(1)充分利用了磁力计提供的信息，既可以用于更新姿态信息，又可以有效估计出包含MEMS陀螺误差参数在内的MEMS系统误差；

(2)具有较高的自主性、可靠性，无需提供外界卫星信号；

(3)对磁力计信息进行了阈值判断，仅在无外界磁场干扰时，进行MEMS系统误差估计，保证了磁场信息的精度，保障了系统误差估计的性能。

附图说明

图1为本发明方法的流程示意图。

图2为载体运动时三轴磁场的变化示意图。

图3为向量在动系与定系之间的关系示意图。

具体实施方式

以下对本发明的实施例进行详细说明。

一、建立微机电MEMS陀螺误差估计状态方程

关于无卫星信号辅助的微惯性导航，如何提高系统精度是其关键问题。为了全面补偿MEMS陀螺漂移产生的误差，本发明在建立导航系统状态方程时，除了传统的位置、速度、姿态、零偏，还要增加刻度系数误差，逐次启动漂移，以及慢变漂移。令x_k∈R作为系统在n时刻的状态向量，因此可定义多维导航系统状态向量为：

${x_k}^{\mathrm{def}}={\left[\begin{array}{ccccc}{\left(x_k^1\right)}^T& {\left(x_k^2\right)}^T& {\left(x_k^3\right)}^T& {\left(x_k^4\right)}^T& {\left(x_k^5\right)}^T\end{array}\right]}^T---\left(1\right)$

式中，

表示定义为；

分别表示系统位置、速度、姿态误差；

表示刻度系数误差；

表示陀螺漂移，其中包含零偏、逐次启动漂移以及慢变漂移。

二、无外界磁干扰判断—N阶距检测法

硬磁、软磁相关材料如磁石，建筑物中的钢筋或者移动电话均可被视为外磁场源。在磁力计工作时，会同时包含外磁场源对当地磁场的干扰值。因此如何判断当地磁场无外磁干扰处于恒定状态非常重要。磁场恒定指的是磁场强度不为时间的函数，随时间变化保持常量，可以用如下公式表示：

式中，b表示载体坐标系；

表示t₁时刻b系下的磁场向量；k表示磁场恒定阶段的采样点数；||·||₂表示向量的二范数。

通过恒定磁场中磁力计的输出信息反估MEMS陀螺误差参数，首先要准确判断规定的采样时间段内磁场恒为常值的采样时刻。式(2)所描述的等价并不仅限于模值严格相等，只要所测磁场属于同一量级，且相差的最大值在噪声允许范围之内即可。本发明通过采用N阶距检测的方法判断是否存在外磁干扰：

此时所测试的N阶距差值若小于等于阈值，则触发滤波器工作，若大于阈值则滤波器关闭。

此外，检测时所选取的时间段要达到一个平衡，使得对于噪声来说有足够的采样点平衡掉，并且在此时间段之内陀螺的角速率不发生变化，否则会引入不必要的速度误差。

三、无外磁干扰下，磁力计输出与MEMS陀螺误差参数关系

无卫星信号辅助的MIMU微惯性导航方案，陀螺精度是决定导航定位精度的关键因素。下面阐述无外磁干扰条件下磁力计估计陀螺误差的参数之间的关系。

惯性导航所遵循的基本定律是牛顿第二定律，设有一空间向量

其量值与方向都随时间而变化，如图2所示动系b相对定系i的关系。相对角速度为因此动系相对定系存在运动，所以向量相对这两个坐标系的变化率不同，存在如下关系：

同理地球磁场为空间向量，将静坐标系选取为地球系，动系选取为载体坐标系，则存在如下关系：

当测量当地磁场处于恒定状态时，则相对于地球静止，磁场在惯性坐标系下相对时间的导数恒定为零，因此(5)可变形为：

当两个坐标系的相对角速率为

写作如下形式：

设在Δt时间内进行磁场强度N阶距检测测试成功。设初始时刻为t₁，经过k个采样点在t_k+1时刻截止，则在t_k+1时刻磁场对时间求导在载体系下的投影预测值为：

另外，通过导数的定义也可得t_k+1时刻磁场在载体系下对时间求导投影的测量值为：

真实的载体对惯性转动角速率与理想角速率之间的关系为：

式中，K_g为系统标度系数。

式(10)可化简为：

将(11)代入(8)式得：

本设计中状态变量包含刻度因子误差，因此令：

K_g＝1+δK_g(13)

带入(10)式中，可得沿载体轴误差角速率：

略去二阶小量得：

结合以上推导过程，可得磁场误差方程为：

以上就为静态磁场条件下，磁场误差参数方程的推导过程，由上述式子可以明显看出当当地磁场无外界干扰的情况下，即所测磁场相似于地球磁场时，磁力计沿载体轴的实际输出做微分与预测磁场导数的差值是与逐次启动、慢变漂移以及磁力计噪声相关的，因此通过观测磁场导数误差来估计MEMS陀螺的各项参数是可行的。

四、MEMS系统误差估计滤波器观测方程

选择载体速度为传统捷联惯导的观测量，再加上磁场微分，本设计的卡尔曼滤波器的观测量为6维，由Z_6×1表示。

可令：

$Z_{6\×1}={\left[\begin{array}{cc}{Z}_1^T& Z_2^T\end{array}\right]}^T={\left[\begin{array}{cccccc}\mathrm{\δ}{V}_x& \mathrm{\δ}{V}_y& \mathrm{\δ}{V}_z& \mathrm{\δ}{B}_x& \mathrm{\δ}{B}_y& \mathrm{\δ}{B}_z\end{array}\right]}^T---\left(17\right)$

式中，Z₁表示MEMS系统速度差量测量，Z₂表示磁力计测得的当地磁场与已知地球磁场的磁场强度差量测量，δV_x,δV_y,δV_z表示速度差量测量在在当地水平坐标系三个轴上的分量；δB_x,δB_y,δB_z表示磁力计测得的当地磁场与已知地球磁场的磁场强度差在当地水平坐标系三个轴上的分量。

由于Z₁为常规速度观测量，下面重点针对Z₂相关量测方程进行阐述。

根据式(16)可得：

带入(14)式：

观察(16)式，可以看出式子的第一项为误差量矩阵与磁场向量的乘积，并不方便列出观测方程，需进行化简，下面阐述一个简单的矩阵叉乘交换例子。设

均为三维向量，令

分别为二者的反对称阵，则有下式成立。

观察(19)式与(20)式的结果即可以得到以下结论,两个向量做叉乘等于交换位置之后叉乘结果的相反数。

结合这个简单的例子，对(20)式进一步化简可得：

$\mathrm{\δ}\stackrel{\·}{B}\left(t_{k+1}\right)=-\underset{\→}{B}\·b+c\·w---\left(26\right)$

式中，

为

反对称阵，

而可以得到观测方程为：

式中，w表示磁力计系统白噪声。

在磁场微分观测方程的基础上，可结合传统捷联惯性导航系统的状态方程模型，就能够得到完整的利用无外磁场干扰的条件下磁力计反估MEMS陀螺的卡尔曼滤波数学模型。

五、MEMS系统误差卡尔曼滤波估计

关于无GNSS辅助的MIMU微惯性导航方案，本发明采用的为卡尔曼滤波估计处理MEMS陀螺误差估计问题。通过前四步骤，统计所有关于MEMS误差的数学模型，即可得到卡尔曼滤波所需要的状态方程和量测方程。

X_k＝Φ_k,k-1X_k-1+Γ_k-1W_k-1          (29)

Z_k＝H_kX_k+V_k                 (30)

系统噪声方差阵Q_k非负定，量测噪声方差阵R_k正定，k时刻的量测为Z_k，则X_k的估计按下述方程求解：

状态一步预测

${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}---\left(31\right)$

状态估计

${\hat{X}}_k={\hat{X}}_{k,k-1}+X_k(Z_k-H_k{\hat{X}}_{k/k-1})---\left(32\right)$

滤波增益

K_k＝P_k/k-1H^T _k(H_kP_k/k-1H^T _k+R_k)^-1        (33)

一步预测均方误差

P_k/k-1＝Φ_k,k-1P_k-1Φ^T _k,k-1+Γ_k-1Q_k-1Γ^T _k-1       (34)

估计均方误差

P_k＝(I-K_kH_k)P_k/k-1(I-K_kH_k)^T+K_kR_kK^T _k         (35)

综上所述，，在当地磁场恒为地球磁场时，通过判定是否受无外界磁场干扰。若无受外界磁场干扰，则磁力计输出在载体轴向上的变化，来观测MEMS陀螺器件相关误差，利用卡尔曼滤波器进行MEMS系统误差估计。

采用本发明中的方法，可以一定程度上补偿由于MEMS陀螺漂移较大从而产生的定位误差。并且充分利用磁力计信息，并不局限于仅仅提高姿态信息中的航向值。因此本设计可以有效地提高无卫星信号下导航系统的精度，同时相比其他导航系统具有结构简单、操作简便等优点。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种基于磁力计N阶距检测的MEMS系统误差估计方法，其特征在于，包含如下步骤：

步骤一、MEMS系统、磁力计开始工作，在计算机中实时存储MEMS系统输出的速度信息，磁力计测得的磁感应强度信息；

步骤二、利用N阶矩检测法判断是否存在外界磁干扰，所测试的N阶距差值若小于等于阈值，则触发MEMS系统误差估计卡尔曼滤波器工作，结合磁力计输出估计出MEMS系统的位置、速度、姿态误差，以及MEMS陀螺的刻度系数误差、陀螺漂移；若大于阈值，则MEMS系统误差估计卡尔曼滤波器关闭；

所涉及的N阶矩检测法判断条件为：

式中，b表示载体坐标系；

表示t₁时刻b系下的磁场向量；k表示磁场恒定阶段的采样点数；||·||₂表示向量的二范数；N为阶矩值；v表示判断条件阈值；

所涉及的MEMS系统误差估计卡尔曼滤波器滤波器，其状态向量为：

${x_k}^{\mathrm{def}}={\left[\begin{array}{ccccc}{\left(x_k^1\right)}^T& {\left(x_k^2\right)}^T& {\left(x_k^3\right)}^T& {\left(x_k^4\right)}^T& {\left(x_k^5\right)}^T\end{array}\right]}^T$

，表示定义为；分别表示MEMS系统的位置、速度、姿态误差；

表示刻度系数误差；

表示MEMS陀螺漂移；

同时除去常规状态量外，关于刻度系数误差的微分方程为：

${\stackrel{\·}{x}}^4=\mathrm{\δ}{\stackrel{\·}{K}}_g=0$

式中，δK_g为MEMS陀螺标度系数误差。

所涉及的MEMS系统误差估计卡尔曼滤波器的6维量测量Z_6×1为：

$Z_{6\×1}={\left[\begin{array}{cc}{Z}_1^T& Z_2^T\end{array}\right]}^T={\left[\begin{array}{cccccc}\mathrm{\δ}{V}_x& \mathrm{\δ}{V}_y& \mathrm{\δ}{V}_z& \mathrm{\δ}{B}_x& \mathrm{\δ}{B}_y& \mathrm{\δ}{B}_z\end{array}\right]}^T$

式中，Z₁表示MEMS系统速度差量测量，Z₂表示磁力计测得的当地磁场与已知地球磁场的磁场强度差量测量，δV_x,δV_y,δV_z表示速度差量测量在在当地水平坐标系三个轴上的分量,表示由惯导解算出速度估计值与真实值之间的误差，具体可由速度估计值与速度基准值做差即可，其中速度基准可由卫星或者多普勒等等设备直接读取；δB_x,δB_y,δB_z表示磁力计测得的当地磁场与已知地球磁场的磁场强度差在当地水平坐标系三个轴上的分量；

所涉及的MEMS系统误差估计卡尔曼滤波器的量测方程为：

$Z_1=\left[I_{3\×3}\right]\left[\begin{array}{c}\mathrm{\δ}{V}_x\\ \mathrm{\δ}{V}_y\\ \mathrm{\δ}{V}_z\end{array}\right]+w_v$

式中，

w_v表示速度量测白噪声；w表示磁力计系统白噪声；分别为测力计测得的磁场强度；δK_gx,δK_gy,δK_gz分别为MEMS陀螺标度系数误差δK_g在当地水平坐标系三轴上的分量；ε_bx,ε_by,ε_bz分别为MEMS陀螺常值漂移ε_b在当地水平坐标系三轴上的分量；ε_rx,ε_ry,ε_rz分别为MEMS陀螺随机漂移ε_r在当地水平坐标系三轴上的分量；

分别为载体坐标系对惯性坐标系转动角速率

在当地水平坐标系三轴上的分量。

Images