CN115046539A - Mems电子罗盘动态校准方法 - Google Patents

Abstract

MEMS电子罗盘动态校准方法，其特征在于，在复杂动态干扰情况下，解算出载体精确姿态角信息并实现电子罗盘动态校准，包括以下步骤：步骤一、获取三轴MEMS电子罗盘的陀螺仪和磁力计数据，通过噪声模型进行预处理；步骤二、通过椭球拟合算法对磁力计进行初校准；步骤三、再采集电子罗盘加速度计、陀螺仪和磁力计的数据进行载体姿态解算；得到磁力计和陀螺仪的方位角数据；步骤四、通过插值算法对解算磁力计和陀螺仪的方位角数据进行去野值及数据补偿；得到陀螺仪数据最优曲线；步骤五、利用陀螺仪数据最优曲线拟合来补偿磁力计数据实现电子罗盘动态校准。本发明在MEMS电子罗盘工作中随时校准，大大提高了MEMS电子罗盘的测量精度，并且在实际工作中节省了大量时间。

Description

MEMS电子罗盘动态校准方法

技术领域

本发明是MEMS电子罗盘动态校准方法，涉及惯性导航领域，具体涉及在精确测量姿态信息的同时，进行mems电子罗盘动态校准。

背景技术

影响电子罗盘精度的误差源有很多，主要包括制造误差、安装误差和对磁场造成直接影响的磁场罗差。现在应用非常广泛的是磁阻传感器，全球磁阻电子罗盘制造有两大厂商分别是美国的霍尼韦尔和PNI公司，他们在该传感器技术领域处于领先位置。国内电子罗盘起步较晚，产品一直处于落后状态，因此，国内的研究学者主要将研究方向放在对传感器误差的校准算法上，通过对电子罗盘的误差分析，利用数学方法拟合误差模型，提高电子罗盘的精度。

在校准算法方面，静态校准算法比较成熟，主要有十二位置校准算法和基于最小二乘的椭球拟合算法，但静态校准有很大的局限性，一方面需要基准转台等辅助校准工具，并且辅助校准工具价格昂贵、操作复杂，在实际应用中需要定期返厂校准，极大影响生产效率；另一方面静态校准受运动状态限制，无法在动态时进行自校准，极为不便。在动态情况下抗干扰磁场的研究基本处于空白状态。

发明内容

本发明的目的是提出一种MEMS电子罗盘动态校准方法，在解算精确姿态的同时对电子罗盘动态自校准，以克服在动态情况下MEMS电子罗盘不能校准的缺陷。

MEMS电子罗盘动态校准方法，其特征在于，在复杂动态干扰情况下，解算出载体精确姿态角信息并实现电子罗盘动态校准，具体包括以下步骤：

步骤一、获取三轴MEMS电子罗盘的陀螺仪和磁力计数据，通过噪声模型进行预处理；

步骤二、通过椭球拟合算法对磁力计进行初校准；

步骤三、再采集电子罗盘加速度计、陀螺仪和磁力计的数据进行载体姿态解算；得到磁力计和陀螺仪的方位角数据；

步骤四、通过插值算法对解算磁力计和陀螺仪的方位角数据进行去野值及数据补偿；得到陀螺仪数据最优曲线；

步骤五、利用陀螺仪数据最优曲线拟合来补偿磁力计数据实现电子罗盘动态校准。

其获取陀螺仪数据并通过噪声模型进行预处理，对陀螺仪数据进行建立噪声模型并进行预处理，具体步骤如下，

对外界随机振动源设置一个判定的阈值，该阈值根据外界环境的振动变化而自适应变化，当陀螺仪检测到的角速度在该阈值内时，这是由于外界随机抖动产生的干扰角速度，为了避免该干扰角速度信息累积影响角度信息，设置此时陀螺仪的角速度输出为零，阈值公式为：

G_max＝max(G(n))

G_min＝min(G(n))

G_min≤G(k)≤G_max，G(k)＝0

G(k)≤G_minor G(k)≥G_max，G(k)＝G(k)

其中G_max和G_min为针对外界环境随机振动设置的自适应变化的阈值，G(n)代表需重新采样的角速度值，n代表陀螺仪每间隔一段时间后需要重新采样的样本数目，在本实验n设定为500，G(k)代表在k时刻陀螺仪输出的角速度信息；取陀螺仪静止时采样数据的最大和最小值为阈值，当检测角速度在该阈值内时视为外界干扰振动，陀螺仪输出角速度为零值；当测量值大于G_max或者小于G_min时，输出该测量值，这样就解决了干扰陀螺仪的外界随机振动；

关于陀螺仪另一大影响因素自身的漂移问题，是通过计算陀螺仪采样数据的均值，得出其在该静止状态下的漂移均值，从检测的旋转角速度中除去该漂移均值即可消去漂移情况，去漂移公式为：

G_mean＝mean(G(n))

G(k)＝G(k)-G_mean

其中G_mean为采样点数据均值，将经过阈值判断为旋转的角速度值减去漂移均值，消去陀螺仪的自身漂移问题，就完成了对陀螺仪的预处理。

获取磁力计数据并通过噪声模型进行预处理，具体步骤为，

建立磁场误差模型：

H^m＝D×H^b+B

H^b是无外部磁干扰的三轴磁力计的输出，B是硬磁干扰误差，D是软磁干扰引起的误差矩阵，H^m是三轴磁力计的实际输出；在复杂强磁干扰情况下，测量地点处的磁场模型为一个球心不在原点的椭球。

采集电子罗盘加速度计、陀螺仪和磁力计的数据进行载体姿态解算，具体的步骤为，

对电子罗盘测量模型进行方位角姿态解算，假设电子罗盘的制造误差与安装误差十分微小，由此确定三轴加速度计和三轴磁力计，其三个轴向都是相互正交的；利用三轴加速度计解算出载体的姿态角，换言之就是俯仰角和横滚角，然后使用这两个姿态角计算大地磁场的三个轴向在水平面上的投影；其公式可表示为：

其中α是俯仰角，β是横滚角，A_x，A_y，A_z分别是在载体坐标系X，Y，Z轴向的加速度计所测量数值；透过加速度传感器解算出载体相应的倾斜姿态之后，便能够将磁力计所测量的三个轴向的磁场强度投影到大地坐标系X和Y轴向，换言之我们可以认为载体根据倾斜姿态先后做了两次转动轴向，即是先X轴向后Y轴向，其转换关系可表示为：

化简展开后，可得在水平面下所感应到的磁场强度：

H_nx＝H_bxcosα+H_bysinαsinβ-H_bzsinαcosβ

H_ny＝H_bycosβ+H_bzcosβ

然后再代入下式便能够解算出方位角：

其中H_nx是载体在水平面上X轴向的投影，H_ny是载体在水平面Y轴向的投影，ψ′是由三轴测量模型解算出来的地磁方位角。

通过椭球拟合算法对磁力计进行初校准，具体步骤为，当环境稳定的时候，电子罗盘在每个姿态下所感受到的磁场强度是相同的，在没有偏差以及电子罗盘内部各个轴相互垂直正交的情况下，磁力计测量的三轴磁场值在三维空间内可组成一个球面；

在实际工作环境应用中的磁力计受三轴非正交、零偏误差和刻度因子及外界环境中硬、软磁干扰，使得三轴输出的磁场数据组成一个球心偏离坐标系原点且形状发生畸变的椭球；磁扰状态下的电子罗盘中磁力计测得磁场值发生严重失真，导致其所得方位角可信度极低，这种情况下用椭球拟合算法对磁场进行初校准，椭球的一般方程为：

写成矩阵形式后化简：

上式中椭球系数矩阵为：

所求椭球球心坐标为：

由磁力计的整体误差模型构成椭球方程：

(H^b)^T(H^b)＝[E(H^m-B)]^T[E(H^m-B)]

＝(H^m)^TC(H^m)-2B^TACH^m+B^TC

其中：

由磁力计测量值数据得出线性方程组：

式中：e为磁场真实值和磁力计测量值残差，当磁力计不受干扰时e＝0，上

式中：

椭球方程系数：

δ＝[a₁，a₂，a₃，a₄，a₅，a₆，a₇，a₈，a₉，-1]^T

要使得残差e最小，我们以磁力计采集的磁场值数据与椭球球面的距离的平方和最小为判断准则，利用最小二乘法估计出椭球方程的各个系数：

这样即可求得畸变矩阵D和偏移矢量B；

椭球拟合利用椭球拟合方法进行磁力计误差校正就是通过椭球拟合算法算出球心坐标和椭球方程系数，将球心拟合到坐标系原点，并且通过求得的系数得到校准矩阵和偏移矢量，以得出磁场强度的真实值从而达到校正误差的目的；通过磁力计采集数据得出线性方程组，应用最小二乘估计法得出椭球方程各个系数，最后将系数矩阵带入到磁场误差模型中即可校正受干扰磁场值。

其通过插值算法进行数据补偿，具体步骤为：利用MEMS陀螺在短时间内角度积分高可靠性特点，以陀螺短期旋转解算所得角度为基准，利用插值算法对电子罗盘进行动态校准；通过将含有MEMS三轴陀螺仪、加速度计和磁力计的电子罗盘以不同姿态旋转进行数据采样并进行去野值，通过姿态解算分别得到陀螺旋转角度和磁力计方位角；这里我们使用三次样条插值算法，三次样条函数要求函数f(x)在区间[a,b]内有意义，且将区间划分为长度相等的n个小区间，在每个区间内，f(x)都是一个三次多项式且在定义域内二阶连续且可导；三次样条函数的一般形式为：

在每一个小区间内的函数f(x)的形式应为：

f(x)＝a_ix³+b_ix²+c_ix+d_i，x∈[x_i，x_i+1]，t＝O，1，2，...，n-1

通过插值算法对磁力计和陀螺仪解算的方位角数据进行去野值及数据补偿然后进行曲线拟合，具体步骤为，利用陀螺仪积分角度拟合校准磁力计解算方位角；此时需要注意，陀螺仪解算出的为电子罗盘三轴旋转的角度，而不是电子罗盘整体旋转的角度，在水平情况下，其整体旋转角度与z轴旋转角度相同，但在非水平情况下，旋转角度在x和y轴上存在分量，则不能以单轴旋转角度作为整体旋转角度，在这里我们以陀螺仪三轴旋转角度均方和作为整体旋转角度，表达式为：

其中A_α为电子罗盘整体旋转角度，A_x、A_y、A_z分别为x、y、z轴旋转角度；接下来对由陀螺仪所得的整体旋转角度及由磁力计解算所得方位角进行去野值和去重值处理，由于陀螺仪的采样频率和磁力计的采样频率不同，各自通过采样点计算出的角度数据无法实现一一对应，进而无法进行曲线拟合，这里我们对数据点较少的磁力计方位角数据集进行插值处理，利用三次样条插值算法实现数据补偿，使之与陀螺仪角度数据样本数相同；最后以陀螺仪所得整体旋转角度值为基准量，磁力计解算的方位角值为代校准量，进行最小二乘曲线拟合。

本发明的有益效果：本发明实现MEMS电子罗盘动态校准，校准后精度在0.5度内，并且在运动载体解算姿态工作的同时实现MEMS电子罗盘的校准。本发明改变了现有MEMS电子罗盘校准必须返回校准单位校准的工作方法，克服了MEMS电子罗盘校准花费时间多、且不能及时校准的缺陷，本发明解决了现有MEMS电子罗使用时常常不精确，不能及时校准的问题，本发明能在MEMS电子罗盘工作中随时校准，其大大提高了MEMS电子罗盘的测量精度，并且在实际工作中节省了大量时间。

附图说明

图1、是本发明的总体流程图；

图2、是本发明的陀螺仪预处理效果图；

图3、是本发明的椭球拟合初校准前后的磁场值对比图；

图4、是本发明的磁力计数据插值拟合曲线；

图5、是本发明的电子罗盘校准前后的方位角值；

图6、是本发明的电子罗盘校准前后方位角误差值；

图7、是本发明的加入强磁干扰源的电子罗盘；

图8、是本发明的强磁干扰时插值拟合曲线；

图9、是本发明的强磁干扰时校准前后方位角值。

具体实施方式

MEMS电子罗盘动态校准方法，其特征在于，在复杂动态干扰情况下，解算出载体精确姿态角信息并实现电子罗盘动态校准，具体包括以下步骤：

步骤一、获取三轴MEMS电子罗盘的陀螺仪和磁力计数据，通过噪声模型进行预处理；

步骤二、通过椭球拟合算法对磁力计进行初校准；

步骤三、再采集电子罗盘加速度计、陀螺仪和磁力计的数据进行载体姿态解算；得到磁力计和陀螺仪的方位角数据；

步骤四、通过插值算法对解算磁力计和陀螺仪的方位角数据进行去野值及数据补偿；得到陀螺仪数据最优曲线；

步骤五、利用陀螺仪数据最优曲线拟合来补偿磁力计数据实现电子罗盘动态校准。

其中，对陀螺仪数据进行建立噪声模型并进行预处理，具体步骤如下：

MEMS陀螺仪的误差源主要来自于自身的漂移和外界随机振动源，为了解决这两个问题，我们对外界随机振动源设置一个判定的阈值，该阈值会根据外界环境的振动变化而自适应变化。因为外界振动对陀螺仪输出是具有周期性影响的，所以我们设置该阈值进行判断，当陀螺仪检测到的角速度在该阈值内时，我们认为这是由于外界随机抖动产生的干扰角速度，为了避免该干扰角速度信息累积影响角度信息，我们设置此时陀螺仪的角速度输出为零，阈值公式为：

G_max＝max(G(n))

G_min＝min(G(n))

G_min≤G(k)≤G_max，G(k)＝0

G(k)≤G_minorG(k)≥G_max，G(k)＝G(k)

其中G_max和G_min为针对外界环境随机振动设置的自适应变化的阈值，G(n)代表需重新采样的角速度值，n代表陀螺仪每间隔一段时间后需要重新采样的样本数目，在本实验n设定为500，G(k)代表在k时刻陀螺仪输出的角速度信息。由上式可知，我们取陀螺仪静止时采样数据的最大和最小值为阈值，当检测角速度在该阈值内时视为外界干扰振动，陀螺仪输出角速度为零值；当测量值大于G_max或者小于G_min时，输出该测量值。这样就解决了干扰陀螺仪的外界随机振动。

关于陀螺仪另一大影响因素自身的漂移问题，我们通过计算陀螺仪采样数据的均值，得出其在该静止状态下的漂移均值，从检测的旋转角速度中除去该漂移均值即可消去漂移情况，去漂移公式为：

G_mean＝mean(G(n))

G(k)＝G(k)-G_mean

其中G_mean为采样点数据均值，将经过阈值判断为旋转的角速度值减去漂移均值，消去陀螺仪的自身漂移问题，这样我们就完成了对陀螺仪的预处理。

对磁力计进行预处理，具体步骤如下，

建立磁场误差模型：

H^m＝D×H^b+B

H^b是无外部磁干扰的三轴磁力计的输出，B是硬磁干扰误差，D是软磁干扰引起的误差矩阵，H^m是三轴磁力计的实际输出。在复杂强磁干扰情况下，测量地点处的磁场模型为一个球心不在原点的椭球。

采集电子罗盘加速度计、陀螺仪和磁力计的数据进行载体姿态解算，具体步骤，设计电子罗盘测量模型进行方位角姿态解算。假设电子罗盘的制造误差与安装误差十分微小，由此我们能够确定三轴加速度计和三轴磁力计，其三个轴向都是相互正交的。利用三轴加速度计解算出载体的姿态角，换言之就是俯仰角和横滚角，然后使用这两个姿态角计算大地磁场的三个轴向在水平面上的投影。其公式可表示为：

其中α是俯仰角，β是横滚角，A_x，A_y，A_z分别是在载体坐标系X，Y，Z轴向的加速度计所测量数值。透过加速度传感器解算出载体相应的倾斜姿态之后，便能够将磁力计所测量的三个轴向的磁场强度投影到大地坐标系X和Y轴向，换言之我们可以认为载体根据倾斜姿态先后做了两次转动轴向，即是先X轴向后Y轴向，其转换关系可表示为：

化简展开后，可得在水平面下所感应到的磁场强度：

H_nx＝H_bxcosα+H_bysinαsinβ-H_bzsinαcosβ

H_ny＝H_bycosβ+H_bzcosβ

然后再代入下式便能够解算出方位角：

其中H_nx是载体在水平面上X轴向的投影，H_ny是载体在水平面Y轴向的投影，ψ′是由三轴测量模型解算出来的地磁方位角。

所述的设计椭球拟合算法对磁力计进行初校准，具体步骤，设计椭球拟合算法进行磁场初校准。当环境稳定的时候，电子罗盘在每个姿态下所感受到的磁场强度是相同的，在没有偏差以及电子罗盘内部各个轴相互垂直正交的情况下，磁力计测量的三轴磁场值在三维空间内可组成一个球面。但是在实际工作环境应用中的磁力计受三轴非正交、零偏误差和刻度因子及外界环境中硬、软磁干扰，使得三轴输出的磁场数据组成一个球心偏离坐标系原点且形状发生畸变的椭球。磁扰状态下的电子罗盘中磁力计测得磁场值发生严重失真，导致其所得方位角可信度极低，为了提高磁力计解算的方位角精度，我们用椭球拟合算法对磁场进行初校准，椭球的一般方程为：

写成矩阵形式后化简：

上式中椭球系数矩阵为：

所求椭球球心坐标为：

由磁力计的整体误差模型构成椭球方程：

(H^b)^T(H^b)＝[E(H^m-B)]^T[E(H^m-B)]

＝(H^m)^TC(H^m)-2B^TACH^m+B^TC

其中：

由磁力计测量值数据得出线性方程组：

式中：e为磁场真实值和磁力计测量值残差，当磁力计不受干扰时e＝0，上

式中：

椭球方程系数：

δ＝[a₁，a₂，a₃，a₄，a₅，a₆，a₇，a_8，a₉，-1]^T

要使得残差e最小，我们以磁力计采集的磁场值数据与椭球球面的距离的平方和最小为判断准则，利用最小二乘法估计出椭球方程的各个系数：

这样即可求得畸变矩阵D和偏移矢量B。

椭球拟合利用椭球拟合方法进行磁力计误差校正就是通过椭球拟合算法算出球心坐标和椭球方程系数，将球心拟合到坐标系原点，并且通过求得的系数得到校准矩阵和偏移矢量，以得出磁场强度的真实值从而达到校正误差的目的。通过磁力计采集数据得出线性方程组，应用最小二乘估计法得出椭球方程各个系数，最后将系数矩阵带入到磁场误差模型中即可校正受干扰磁场值。

所述的通过插值算法进行数据补偿。利用MEMS陀螺在短时间内角度积分高可靠性特点，以陀螺短期旋转解算所得角度为基准，利用插值算法对电子罗盘进行动态校准。通过将含有MEMS三轴陀螺仪、加速度计和磁力计的电子罗盘以不同姿态旋转进行数据采样并进行去野值，通过姿态解算分别得到陀螺旋转角度和磁力计方位角。这里我们使用三次样条插值算法。三次样条函数要求函数f(x)在区间[a，b]内有意义，且将区间划分为长度相等的n个小区间，在每个区间内，f(x)都是一个三次多项式且在定义域内二阶连续且可导。三次样条函数的一般形式为：

在每一个小区间内的函数f(x)的形式应为：

f(x)＝a_ix³+b_ix²+c_ix+d_i，x∈[x_i，x_i+l]，i＝0，1，2，...，n-1

所述的通过插值算法对磁力计和陀螺仪解算的方位角数据进行去野值及数据补偿然后进行曲线拟合，具体做法是，利用陀螺仪积分角度拟合校准磁力计解算方位角。此时需要注意，陀螺仪解算出的为电子罗盘三轴旋转的角度，而不是电子罗盘整体旋转的角度，在水平情况下，其整体旋转角度与z轴旋转角度相同，但在非水平情况下，旋转角度在x和y轴上存在分量，则不能以单轴旋转角度作为整体旋转角度，在这里我们以陀螺仪三轴旋转角度均方和作为整体旋转角度，表达式为：

其中A_α为电子罗盘整体旋转角度，A_x、A_y、A_z分别为x、y、z轴旋转角度。接下来对由陀螺仪所得的整体旋转角度及由磁力计解算所得方位角进行去野值和去重值处理，由于陀螺仪的采样频率和磁力计的采样频率不同，各自通过采样点计算出的角度数据无法实现一一对应，进而无法进行曲线拟合，这里我们对数据点较少的磁力计方位角数据集进行插值处理，利用三次样条插值算法实现数据补偿，使之与陀螺仪角度数据样本数相同；最后以陀螺仪所得整体旋转角度值为基准量，磁力计解算的方位角值为代校准量，进行最小二乘曲线拟合。

为了解决现有技术的问题，本发明提供了MEMS电子罗盘动态校准算法。目前MEMS电子罗盘动态校准尚处空白状态。我的课题研究将另辟蹊径，涉足电子罗盘动态校准领域，探索创新出一种在任何复杂动态干扰情况下均可实现动态自校准新模式，实现运动中载体在检测实时姿态信息的同时可实现电子罗盘动态自校准

实施例：

下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定。

参照图1所示，本发明的MEMS电子罗盘动态校准研究方法是动态陀螺校准：利用MEMS陀螺在短时间内角度积分高可靠性特点，以陀螺短期旋转解算所得角度为基准，利用插值算法对电子罗盘进行动态校准。通过将含有MEMS三轴陀螺仪、加速度计和磁力计的电子罗盘以不同姿态旋转进行数据采样并进行去野值等预处理，通过姿态解算分别得到陀螺旋转角度和磁力计方位角。以陀螺所得角度为准确量，以磁力计所得方位角为待校准量，通过对比分析大量组数的实验数据，得到二者的最优拟合曲线。首先对陀螺仪的进行预处理，将电子罗盘置于载体上静止采样5s-10s，利用权利要求2所述的陀螺仪去零偏和漂移的方法进行预处理，其效果如图2所示；接下来利用椭球拟合法进行磁力计的预处理，采集磁扰环境下磁场值，将传感器分别绕x、y、z轴做无规则旋转采集受到干扰磁场的椭球样本点，按照权利要求5所述的方法进行椭球拟合，根据椭球拟合算法可以得到校准矩阵D和偏移矢量B：

校准前后磁场值如图3所示。为了便于观察和验证动态校准的效果，本课题设计的转台实验为在水平状态下方位角从0度开始每旋转60度停顿3秒，旋转一周，曲线拟合效果如图4所示的磁力计解算所得方位角经插值后与陀螺仪所得方位角的拟合曲线，可以看出两者的拟合曲线为沿着象限正对角线的不规则的曲线，说明磁力计解算的方位角有着明显的不规则误差。经动态校准前后的方位角数值如图5所示，红色虚线为通过磁力计结算的原始方位角值，蓝色线为通过陀螺仪校准后的方位角数值，通过观察对比可以发现，校准前的方位角曲线与旋转角度的标准曲线相比有着明显偏移和抖动，而校准后的方位角曲线不但与旋转角度相吻合，而且消去了抖动现象，使得方位角整体曲线平滑准确，且其精度值在0.5度以内，图6为电子罗盘经动态校准前后的误差值。上述实验为在标准无强磁干扰状态下的较为理想实验，接下来我们加入强磁干扰进行实验测试动态校准的可行性。如图7所示，给固定在转台上的电子罗盘加入强磁干扰源。重复上述实验并进行数据处理分析，可得到经插值算法后方位角的拟合曲线图8和校准前后方位角数值对比曲线图9。可以看出在强磁干扰下，校准前电子罗盘的方位角发生巨大偏差已无法正常工作，而经动态校准后将方位角曲线拉回准确值，使得在强磁干扰下的电子罗盘依然可以正常工作得到准确方位角数值。经实验验证，在动态情况下成功实现了电子罗盘的校准工作，并且其校准精度达到了0.5度以内，开创了MEMS电子罗盘动态校准新模式，弥补了电子罗盘动态校准领域的空白。

本发明为了解决在检测实时姿态信息的同时可实现MEMS电子罗盘动态自校准的问题。包括以下步骤：获取三轴加速度计、陀螺仪和磁力计数据通过噪声模型进行预处理；设计椭球拟合算法对磁力计实现初校准；采集校准数据并进行姿态解算；通过插值算法进行数据补偿；通过基于最小二乘法的最优曲线拟合利用陀螺仪实现MEMS电子罗盘动态校准，在复杂场景下检测实时姿态信息时可实现电子罗盘动态自校准。

以上所述实施例仅是为充分说明本发明而所举的较佳的实施例，本发明的保护范围不限于此。本技术领域的技术人员在本发明基础上所作的等同替代或变换，均在本发明的保护范围之内。本发明的保护范围以权利要求书为准。

Claims (7)

1.MEMS电子罗盘动态校准方法，其特征在于，在复杂动态干扰情况下，解算出载体精确姿态角信息并实现电子罗盘动态校准，具体包括以下步骤：

步骤一、获取三轴MEMS电子罗盘的陀螺仪和磁力计数据，通过噪声模型进行预处理；

步骤二、通过椭球拟合算法对磁力计进行初校准；

步骤三、再采集电子罗盘加速度计、陀螺仪和磁力计的数据进行载体姿态解算；得到磁力计和陀螺仪的方位角数据；

步骤四、通过插值算法对解算磁力计和陀螺仪的方位角数据进行去野值及数据补偿；得到陀螺仪数据最优曲线；

步骤五、利用陀螺仪数据最优曲线拟合来补偿磁力计数据实现电子罗盘动态校准。

2.如权利要求1所述的MEMS电子罗盘动态校准方法，其特征在于，其获取陀螺仪数据并通过噪声模型进行预处理，对陀螺仪数据进行建立噪声模型并进行预处理，具体步骤如下，

对外界随机振动源设置一个判定的阈值，该阈值根据外界环境的振动变化而自适应变化，当陀螺仪检测到的角速度在该阈值内时，这是由于外界随机抖动产生的干扰角速度，为了避免该干扰角速度信息累积影响角度信息，设置此时陀螺仪的角速度输出为零，阈值公式为：

G_max＝max(G(n))

G_min＝min(G(n))

G_min≤G(k)≤G_max，G(k)＝0

G(k)≤G_mminorG(k)≥G_max，G(k)＝G(k)

其中G_max和G_min为针对外界环境随机振动设置的自适应变化的阈值，G(n)代表需重新采样的角速度值，n代表陀螺仪每间隔一段时间后需要重新采样的样本数目，在本实验n设定为500，G(k)代表在k时刻陀螺仪输出的角速度信息；取陀螺仪静止时采样数据的最大和最小值为阈值，当检测角速度在该阈值内时视为外界干扰振动，陀螺仪输出角速度为零值；当测量值大于G_max或者小于G_min时，输出该测量值，这样就解决了干扰陀螺仪的外界随机振动；

关于陀螺仪另一大影响因素自身的漂移问题，是通过计算陀螺仪采样数据的均值，得出其在该静止状态下的漂移均值，从检测的旋转角速度中除去该漂移均值即可消去漂移情况，去漂移公式为：

G_mean＝mean(G(n))

G(k)＝G(k)-G_mean

其中G_mean为采样点数据均值，将经过阈值判断为旋转的角速度值减去漂移均值，消去陀螺仪的自身漂移问题，就完成了对陀螺仪的预处理。

3.如权利要求1所述的MEMS电子罗盘动态校准方法，其特征在于，获取磁力计数据并通过噪声模型进行预处理，具体步骤为，

建立磁场误差模型：

H^m＝D×H^b+B

H^b是无外部磁干扰的三轴磁力计的输出，B是硬磁干扰误差，D是软磁干扰引起的误差矩阵，H^m是三轴磁力计的实际输出；在复杂强磁干扰情况下，测量地点处的磁场模型为一个球心不在原点的椭球。

4.如权利要求1所述的MEMS电子罗盘动态校准方法，其特征在于，采集电子罗盘加速度计、陀螺仪和磁力计的数据进行载体姿态解算，具体的步骤为，

对电子罗盘测量模型进行方位角姿态解算，假设电子罗盘的制造误差与安装误差十分微小，由此确定三轴加速度计和三轴磁力计，其三个轴向都是相互正交的；利用三轴加速度计解算出载体的姿态角，换言之就是俯仰角和横滚角，然后使用这两个姿态角计算大地磁场的三个轴向在水平面上的投影；其公式可表示为：

其中α是俯仰角，β是横滚角，A_x，A_y，A_z分别是在载体坐标系X，Y，Z轴向的加速度计所测量数值；透过加速度传感器解算出载体相应的倾斜姿态之后，便能够将磁力计所测量的三个轴向的磁场强度投影到大地坐标系X和Y轴向，换言之我们可以认为载体根据倾斜姿态先后做了两次转动轴向，即是先X轴向后Y轴向，其转换关系可表示为：

化简展开后，可得在水平面下所感应到的磁场强度：

H_nx＝H_bxcosα+H_bysinαsinβ-H_bzsinαcosβ

H_ny＝H_bycosβ与H_bzcosβ

然后再代入下式便能够解算出方位角：

其中H_nx是载体在水平面上X轴向的投影，H_ny是载体在水平面Y轴向的投影，ψ′是由三轴测量模型解算出来的地磁方位角。

5.如权利要求1所述的MEMS电子罗盘动态校准方法，其特征在于，通过椭球拟合算法对磁力计进行初校准，具体步骤为，当环境稳定的时候，电子罗盘在每个姿态下所感受到的磁场强度是相同的，在没有偏差以及电子罗盘内部各个轴相互垂直正交的情况下，磁力计测量的三轴磁场值在三维空间内可组成一个球面；

在实际工作环境应用中的磁力计受三轴非正交、零偏误差和刻度因子及外界环境中硬、软磁干扰，使得三轴输出的磁场数据组成一个球心偏离坐标系原点且形状发生畸变的椭球；磁扰状态下的电子罗盘中磁力计测得磁场值发生严重失真，导致其所得方位角可信度极低，这种情况下用椭球拟合算法对磁场进行初校准，椭球的一般方程为：

写成矩阵形式后化简：

上式中椭球系数矩阵为：

所求椭球球心坐标为：

由磁力计的整体误差模型构成椭球方程：

(H^b)^T(H^b)＝[E(H^m-B)]^T[E(H^m-B)]

＝(H^m)^TC(H^m)-2B^TACH^m+B^TC

其中：

由磁力计测量值数据得出线性方程组：

式中：e为磁场真实值和磁力计测量值残差，当磁力计不受干扰时e＝0，上式中：

椭球方程系数：

δ＝[a₁，a₂，a₃，a₄，a₅，a₆，a₇，a_a，a₉，-1]^T

要使得残差e最小，我们以磁力计采集的磁场值数据与椭球球面的距离的平方和最小为判断准则，利用最小二乘法估计出椭球方程的各个系数：

这样即可求得畸变矩阵D和偏移矢量B；

椭球拟合利用椭球拟合方法进行磁力计误差校正就是通过椭球拟合算法算出球心坐标和椭球方程系数，将球心拟合到坐标系原点，并且通过求得的系数得到校准矩阵和偏移矢量，以得出磁场强度的真实值从而达到校正误差的目的；通过磁力计采集数据得出线性方程组，应用最小二乘估计法得出椭球方程各个系数，最后将系数矩阵带入到磁场误差模型中即可校正受干扰磁场值。

6.如权利要求1所述的MEMS电子罗盘动态校准方法，其特征在于，其通过插值算法进行数据补偿，具体步骤为：利用MEMS陀螺在短时间内角度积分高可靠性特点，以陀螺短期旋转解算所得角度为基准，利用插值算法对电子罗盘进行动态校准；通过将含有MEMS三轴陀螺仪、加速度计和磁力计的电子罗盘以不同姿态旋转进行数据采样并进行去野值，通过姿态解算分别得到陀螺旋转角度和磁力计方位角；这里我们使用三次样条插值算法，三次样条函数要求函数f(x)在区间[a，b]内有意义，且将区间划分为长度相等的n个小区间，在每个区间内，f(x)都是一个三次多项式且在定义域内二阶连续且可导；三次样条函数的一般形式为：

在每一个小区间内的函数f(x)的形式应为：

f(x)＝a_ix³+b_ix²+c_ix+d_i，x∈[x_i，x_i+1]，i＝0，1，2，...，n-1

7.如权利要求1所述的MEMS电子罗盘动态校准方法，其特征在于，通过插值算法对磁力计和陀螺仪解算的方位角数据进行去野值及数据补偿然后进行曲线拟合，具体步骤为，利用陀螺仪积分角度拟合校准磁力计解算方位角；此时需要注意，陀螺仪解算出的为电子罗盘三轴旋转的角度，而不是电子罗盘整体旋转的角度，在水平情况下，其整体旋转角度与z轴旋转角度相同，但在非水平情况下，旋转角度在x和y轴上存在分量，则不能以单轴旋转角度作为整体旋转角度，在这里我们以陀螺仪三轴旋转角度均方和作为整体旋转角度，表达式为：

其中A_α为电子罗盘整体旋转角度，A_x、A_y、A_z分别为x、y、z轴旋转角度；接下来对由陀螺仪所得的整体旋转角度及由磁力计解算所得方位角进行去野值和去重值处理，由于陀螺仪的采样频率和磁力计的采样频率不同，各自通过采样点计算出的角度数据无法实现一一对应，进而无法进行曲线拟合，这里我们对数据点较少的磁力计方位角数据集进行插值处理，利用三次样条插值算法实现数据补偿，使之与陀螺仪角度数据样本数相同；最后以陀螺仪所得整体旋转角度值为基准量，磁力计解算的方位角值为代校准量，进行最小二乘曲线拟合。

Images