一种消除载体磁干扰的磁罗盘校准方法与流程

本发明属于导航技术领域，具体涉及一种消除载体磁干扰的磁罗盘校准方法。

背景技术：

磁罗盘是利用测得的三轴方向地磁场数据对方位角进行解算，由于其具有体积小、功耗低、可靠性高、精度高、价格便宜等优势，在航空、航海、车载、各类观瞄设备领域等方面得到广泛应用。将磁罗盘安装在各类载体平台上应用时，由于大多数载体平台上均存在类似电池、电缆、电机、钢铁结构件等铁磁性物体，这些铁磁性物体将极大的影响磁罗盘方位角测量精度。因此，为提高磁罗盘在载体平台上方位角测量精度，需要设计适用于载体平台的磁罗盘校准方法，对载体平台存在的硬磁干扰及软磁干扰进行校准，消除这些磁干扰对磁罗盘的影响，从而准确解算出载体平台的方位角。

技术实现要素：

有鉴于此，本发明提供了一种消除载体磁干扰的磁罗盘校准方法，能够输出高精度的载体方位角。

本发明是通过下述技术方案实现的：

一种消除载体磁干扰的磁罗盘校准方法，所述校准方法步骤如下：

步骤一，采集载体平台不同姿态下的磁罗盘三轴磁场数据；

步骤二，建立关于三轴磁传感器校准前、后三轴磁场数据之间的误差模型，利用所述三轴磁场数据求解误差模型中的三轴磁传感器误差参数，得到误差模型；

步骤三，重新采集磁罗盘三轴磁场数据和三轴加速度数据，将重新采集的磁罗盘三轴磁场数据代入误差模型后得到校准后的三轴磁场数据，结合三轴加速度数据求解得到磁罗盘的俯仰角、横滚角及方位角，由此输出校准后的磁罗盘姿态角。

进一步地，所述方位角的求解方法为：将校准后的三轴磁场数据转化为水平坐标系下x、y轴方向的磁场数据；然后将所述水平坐标系下x、y轴方向的磁场数据代入磁方位角计算公式中求解出磁罗盘方位角。

进一步地，所述步骤二中的误差模型是利用泊松模型建立的。

进一步地，所述步骤二中三轴磁传感器误差参数的求解方法具体为：

将误差模型取二范数整理作为卡尔曼滤波器的量测方程，将误差模型中的三轴磁传感器误差参数作为卡尔曼滤波器的状态向量，将单位矩阵作为卡尔曼滤波器的状态转移矩阵，以此建立卡尔曼滤波器方程，利用十二种以上不同姿态下的三轴磁场数据求解卡尔曼滤波器方程，解算得到状态向量的估计值即三轴磁传感器误差参数。

进一步地，所述步骤三中俯仰角、横滚角的求解方法为：利用地理坐标系与载体坐标系之间的变换关系计算。

进一步地，所述载体平台不同姿态的采集方法如下：

若载体平台为手持式载体，则在空间内旋转载体平台，间隔固定时间动态采集十二组以上三轴磁场数据并输出；或改变载体平台的姿态，静态采集十二组以上三轴磁场数据并输出。

进一步地，在空间内旋转的方法为：手持载体，首先使载体平台绕垂直轴线z轴方向慢旋转一圈，然后绕俯仰轴y轴方向旋转一圈，最后绕横滚轴x轴方向转圈一圈。

进一步地，若所述载体平台在平面内校准，则在平面内旋转载体平台，间隔固定时间动态采集八组以上三轴磁场数据并输出；或改变载体平台的姿态，静态采集八组以上三轴磁场数据并输出。

有益效果：

本发明能够实现对载体平台上存在的硬磁干扰及软磁干扰的校准，输出高精度的载体方位角，且校准方法不借助复杂精密设备，利用磁罗盘内置的算法，结合采集的多组数据进行校准，操作简单，校准效率高。

附图说明

图1为磁罗盘姿态角及坐标系定义示意图；

图2为磁罗盘空间多点校准方法姿态摆放示意图；

图3为磁罗盘平面多点校准方法姿态摆放示意图；

图4为磁罗盘校准及方位角解算流程图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

磁罗盘由三轴磁传感器、三轴加速度传感器以及数据采集处理系统构成，三轴磁传感器用于测量三轴方向磁场数据，三轴加速度传感器用于测量三轴方向加速度数据。

本实施例提供了一种消除载体磁干扰的磁罗盘校准方法，地理坐标系是指当地北东地坐标系，与一般地理坐标系定义所不同的是，地理坐标系x轴指向磁北而不是真北，y轴与x轴垂直指向东方，z轴沿着水平面垂直方向向下。载体坐标系是指与载体平台相对固定的坐标系，磁罗盘沿载体坐标系安装，载体坐标系x轴沿载体纵轴指向前方，y轴沿载体横轴指向右侧，z轴向下，与x、y轴形成右手坐标系。如图1所示，方位角测量范围为0°～360°，指向磁北时为0°，顺时针旋转为正；俯仰角测量范围为-90°～90°，水平时为0°，往上抬头为正，向下低头为负；横滚角测量范围为-180°～180°，水平时为0°，右倾为正，左倾为负；磁罗盘箭头方向为前向方向，对应三轴磁传感器x轴和三轴加速度传感器a轴，磁罗盘右向方向对应三轴磁传感器y轴和三轴加速度传感器b轴，磁罗盘垂直向下方向对应三轴磁传感器z轴和三轴加速度传感器c轴。

由于载体坐标系与地理坐标系在绝大部分条件下是不重合的，而求解载体方位角，需要利用地理坐标系下x轴和y轴方向的磁场数据进行计算，因此需要利用姿态旋转矩阵，将载体坐标系下三轴磁场数据变换到地理坐标系下，才能求解方位角α。

根据欧拉转动关系可得载体坐标系与地理坐标系间转换关系的矩阵表示：

其中，β为俯仰角、γ为横滚角、α为方位角。

俯仰角β、横滚角γ的计算方法为：地球重力加速度在地理坐标系中的水平方向上没有投影分量，三轴加速度传感器只在c轴方向上的值不为零，为g。则有：

式中，为磁罗盘载体坐标系下测得的三轴加速度值，为地理坐标系到载体坐标系的坐标旋转矩阵，将(2)式展开可得：

通过对式(3)求解，可计算出俯仰角β、横滚角γ：

载体坐标系下测得的三轴磁场值分别为hx、hy、hz，令式(1)中方位角α为0，利用坐标旋转矩阵将hx、hy、hz变换到地理坐标系下得到水平方向两轴磁场值分别为mx、my，对应的计算公式如公式(6)所示。

mx＝hxcosβ+hysinβsinγ+hzsinβcosγ

my＝hycosγ-hzsinγ(6)

得到地理坐标系下水平方向即水平坐标系下两轴磁场值mx、my后，利用磁方位角计算公式(7)可计算出磁罗盘方位角。

α＝arctan2(mx,my)(7)

从以上公式中可以看出，要计算出准确的磁罗盘方位角，必须采集得到载体坐标系下准确的三轴磁场值，由于载体平台上可能存在硬磁干扰及软磁干扰，必须将这些磁干扰消除，才能保证磁罗盘测得的载体方位角测量精度达到相应技术指标要求。

如果磁罗盘安装在小型载体平台上使用，如手持式载体，且在使用过程中，需要在载体大倾斜角下对方位角进行测量，可采用空间多点校准方法或空间旋转校准方法对载体平台上存在的硬磁干扰及软磁干扰进行校准。

假设磁罗盘直接采集得到的三轴磁场数据(校准前数据)为maga＝[bxbybz]^t，校准后得到的三轴磁场数据为magb＝[hxhyhz]^t，根据泊松模型构建误差模型如式(8)所示。

magb＝k(maga+z)(8)

式中，k为三轴磁传感器灵敏度误差、非正交度误差构成的参数矩阵，z为三轴磁传感器零偏误差构成的参数矩阵。

将式(8)取二范数整理可得式(9)，

在校准过程中，由于磁罗盘是在一定区域内摆放各种姿态采集数据，该区域地磁场||magb||为已知值，求解模型参数c1～c10通过卡尔曼滤波完成，c1～c10为三轴磁传感器误差参数，参数矩阵k、z由三轴磁传感器误差参数c1～c10构成。

卡尔曼滤波器的状态向量为：

x＝[c1c2c3c4c5c6c7c8c9c10]^t

将式(9)作为卡尔曼滤波器的量测方程，卡尔曼滤波器的状态转移矩阵为单位矩阵i。

建立卡尔曼滤波器系统状态方程xi与量测方程后，卡尔曼滤波器求解过程基本方程如式(10)所示。

式中，k为计算时输入数据的次数，qk-1为卡尔曼滤波器系统噪声序列的方差矩阵；rk为量测噪声序列的方差矩阵；和pk-1为滤波器的状态估计初值，φk,k-1为滤波器状态转移矩阵，为滤波的状态一步预测，pk/k-1为一步预测均方误差，kk为卡尔曼滤波增益，为滤波状态估计，pk为状态估计误差，zk为状态量即||magb||，hk为量测矩阵，为量测方程。量测矩阵hk由式(9)得到，量测方程中包含c1～c10。

如图4所示，给定滤波器初值和p0，采集十二种以上不同姿态下的磁罗盘三轴磁场数据bx、by、bz并将该数据输入卡尔曼滤波器方程式(10)中，递推计算出k时刻的状态估计得到的卡尔曼滤波器状态向量的估计值即为需要求解的参数c1～c10。

得到参数c1～c10后，带入式(8)中得到校准三轴磁场数据所用的误差模型。

重新采集磁罗盘的三轴磁场数据，将校准前的三轴磁场数据代入式(8)中得得到校准后的三轴磁场数据hx、hy、hz，然后利用式(6)～(7)计算得到磁罗盘的方位角α。

上述磁罗盘校准及姿态角解算方法为磁罗盘的内置算法，无需借助其他精密仪器。

计算得到的参数c1～c10的准确性取决于卡尔曼滤波器量测输入数据是否能够均匀稳定的分布在空间内。理论上讲，为了得到较为准确的参数，最少需要10组量测输入数据，并且量测输入数据越多，解算的参数c1～c10越精确，但在实际校准过程中，量测输入数据量越大，解算效率越低，并且会增加采集数据的工作量。一般采集12～32组数据即可精确的解算出模型参数。

为了便于用户开展空间校准工作，设计了空间多点校准及空间旋转校准两种校准方法。

对于空间多点校准方法，可将载体平台按图2所示进行姿态摆放，在每个姿态下静态采集相关数据，侧视图仅展示俯仰角状态。载体平台每个姿态摆放的姿态角说明如下：

姿态1表示：使载体方位角指向0°，俯仰角0°，横滚角45°；

姿态2表示：使载体方位角指向90°，俯仰角0°，横滚角-45°；

姿态3表示：使载体方位角指向180°，俯仰角0°，横滚角45°；

姿态4表示：使载体方位角指向270°，俯仰角0°，横滚角-45°；

姿态5表示：使载体方位角指向30°，俯仰角45°，横滚角45°；

姿态6表示：使载体方位角指向120°，俯仰角45°，横滚角-45°；

姿态7表示：使载体方位角指向210°，俯仰角45°，横滚角45°；

姿态8表示：使载体方位角指向300°，俯仰角45°，横滚角-45°；

姿态9表示：使载体方位角指向60°，俯仰角-45°，横滚角45°；

姿态10表示：使载体方位角指向150°，俯仰角-45°，横滚角-45°；

姿态11表示：使载体方位角指向240°，俯仰角-45°，横滚角45°；

姿态12表示：使载体方位角指向330°，俯仰角-45°，横滚角-45°。

需要特别说明的是：

a)本实施例的空间多点校准方法包含但不限于空间12点校准，可根据实际情况来增加更多姿态采集相应数据；

b)以上提及的姿态摆放要点中所表述的方位角不是指载体平台绝对的方位角，而是相对的角度，例如摆放姿态1时，磁罗盘方位角可指向任意角度，比如42°，但在摆放姿态2时，磁罗盘方位角就需要摆放为132°左右，以此类推；

c)以上提及的姿态摆放要点中所表述的俯仰角、横滚角是指载体平台相对水平面的绝对角度，但摆放以上各个姿态时，方位角、俯仰角、横滚角的角度值不需要特别严格，各个角度误差在±15°以内均可接受；

d)在一些应用场景中，在摆放载体姿态时，可不对载体施加横滚角进行摆放，只需对载体按上述说明摆放方位角及俯仰角即可，且载体俯仰角摆放的具体角度可根据实际情况进行调整；

e)以上所表述的姿态1到姿态12只是为了方便进行表述，实际操作过程中，并不限定数据采集顺序。

对于空间旋转校准方法，按以下步骤实施校准：手持载体，首先使其绕垂直轴线z轴方向匀速缓慢旋转一圈(转动方位角)，然后绕俯仰轴y轴方向匀速缓慢旋转一圈(转动俯仰角)，最后绕横滚轴x轴方向匀速缓慢转圈一圈(转动横滚角)。在以上匀速缓慢转动过程中，间隔固定时间磁罗盘自动发送相关命令，动态获取转动过程中的三轴磁场数据。

数据采集完成后，磁罗盘内置算法即可自动完成空间多点校准或空间旋转校准工作，对误差模型进行求解，保存得到的校准参数，将得到的参数带入式(8)中得到校准后的三轴磁场数据，然后利用式(7)计算得到载体实时的方位角α。

如果磁罗盘所安装的载体平台只需要在接近水平状态下来对载体方位角进行测量，可采用平面多点校准方法或平面圆周校准方法对载体平台上存在的硬磁干扰及软磁干扰进行校准。

考虑到平面校准过程中，采集的数据均为接近水平状态下的数据，缺少与z轴相关的磁场数据，因此将式(8)取二范数整理可得式(11)：

d1w1²+d2w2²+d3w1w2+d4w1w3+d5w2w3+d6w1+d7w2＝||magb||(11)

式中，d1～d7为模型平面三轴磁传感器误差参数，w1、w2、w3分别为磁罗盘三轴磁传感器x轴、y轴和z轴测得的三轴磁场数据，在校准过程中，由于载体是在一定区域内转动，采集各个方向的数据，该区域地磁场||magb||为已知值，求解参数d1～d7可同样通过卡尔曼滤波完成，卡尔曼滤波器的状态向量为：

x＝[d1d2d3d4d5d6d7]^t

将式(11)作为卡尔曼滤波器的量测方程，卡尔曼滤波器的状态转移矩阵为单位矩阵i。

建立卡尔曼滤波器系统状态方程与量测方程后，卡尔曼滤波器求解过程基本方程如式(12)所示。

式中，k为计算时输入数据的次数，qk-1为卡尔曼滤波器系统噪声序列的方差矩阵；rk为量测噪声序列的方差矩阵；和pk-1为滤波器的状态估计初值，φk,k-1为滤波器状态转移矩阵，为滤波的状态一步预测，pk/k-1为一步预测均方误差，kk为卡尔曼滤波增益，为滤波状态估计，pk状态估计误差，hk为量测矩阵，为量测方程。量测矩阵hk由式(11)得到，量测方程中包含d1～d7。

给定滤波器初值和p0，采集八种以上不同姿态下的三轴磁场数据wx、wy、wz并将该数据输入卡尔曼滤波器方程式(12)中，递推计算出k时刻的状态估计将所有测得数据输入后得到的卡尔曼滤波器状态向量的估计值即为需要求解的参数d1～d7。

计算得到的参数d1～d7的准确性取决于卡尔曼滤波器量测输入数据是否能够均匀稳定的分布在平面内。理论上讲，为了得到较为准确的参数，最少需要7组量测输入数据，并且量测输入数据越多，解算的参数d1～d7越精确，但在实际校准过程中，量测输入数据量越大，解算效率越低，并且会增加采集数据的工作量。一般采集平面内8～24个姿态的数据即可精确的解算出模型参数。

为了便于用户开展平面校准工作，设计了平面多点校准及平面圆周校准两种校准方法。

对于平面多点校准方法，可将载体平台按图3所示进行姿态摆放，在每个姿态下静态采集相关数据，侧视图仅展示俯仰角状态。载体平台每个姿态摆放姿态角具体说明如下：

姿态1表示：使载体方位角指向0°，俯仰角0°，横滚角0°；

姿态2表示：使载体方位角指向45°，俯仰角0°，横滚角0°；

姿态3表示：使载体方位角指向90°，俯仰角0°，横滚角0°；

姿态4表示：使载体方位角指向135°，俯仰角0°，横滚角0°；

姿态5表示：使载体方位角指向180°，俯仰角0°，横滚角0°；

姿态6表示：使载体方位角指向225°，俯仰角0°，横滚角0°；

姿态7表示：使载体方位角指向270°，俯仰角0°，横滚角0°；

姿态8表示：使载体方位角指向315°，俯仰角0°，横滚角0°。

需要特别说明的是：

a)平面多点校准方法包含但不限于平面8点校准，可根据实际情况来增加或减小姿态采集相应数据；

b)以上提及的姿态摆放要点中所表述的方位角不是指载体绝对的方位角，而是相对的角度，例如摆放姿态1时，磁罗盘方位角可指向任意角度，比如42°，但在摆放姿态2时，磁罗盘方位角就需要摆放为87°左右，以此类推；

c)以上提及的姿态摆放要点中所表述的俯仰角、横滚角是指载体相对水平面的绝对角度，但摆放以上各个姿态时，方位角、俯仰角、横滚角角度不需要特别严格，各个角度误差在±15°以内均可接受；

d)以上所表述的姿态1到姿态8只是为了方便进行表述，实际操作过程中，并不限定数据采集顺序。

对于平面圆周校准方法，按以下步骤实施校准：保持载体平台水平，使其绕垂直轴线z轴方向匀速缓慢旋转一圈(转动方位角)，可在任意方位角下开始转动，且按顺时针或逆时针方向转动均可。在匀速缓慢转动过程中，间隔固定时间磁罗盘自动发送相关命令，动态获取转动过程中的三轴磁场数据。

无论是平面多点校准或平面圆周校准中提及的旋转一圈，均是指在载体平台在限定区域内旋转一圈，限定区域是指：该设备旋转一圈所需的最小区域。

数据采集完成后，磁罗盘内置算法即可自动完成平面多点校准或平面圆周校准工作，对误差模型进行求解，保存得到的三轴磁传感器误差参数，将得到的三轴磁传感器误差参数带入式(8)中得到校准后的三轴磁场数据，然后利用式(7)计算得到载体实时的方位角α。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。