CN110736484A - 基于陀螺仪及磁传感器融合的背景磁场标定方法 - Google Patents

Abstract

本发明公开了一种基于陀螺仪及磁传感器融合的背景磁场标定方法，包括：将磁传感器安装在弹体之内，磁传感器的安装点位于弹体的质心，使用陀螺仪测量载体的姿态角，根据陀螺仪初始状态的姿态角以及陀螺仪测量的姿态角计算载体坐标系，获得磁传感器测量的磁场强度，前期使用陀螺仪测算的磁场强度对背景磁场进行标定，后期使用磁传感器测算的磁场强度对背景磁场进行标定。本发明提供的陀螺姿态测量技术能够解决地磁姿态测量不能独立求解的问题，而且陀螺仪在姿态测量之中具有可靠性高、解算算法简单和测量精度高的优势。因此，本发明提供的基于陀螺仪及磁传感器融合的背景磁场标定方法，能够有效提高背景磁场的标定精度。

Description

基于陀螺仪及磁传感器融合的背景磁场标定方法

技术领域

本发明涉及磁场检测的技术领域，尤其涉及一种基于陀螺仪及磁传感器融合的背景磁场标定方法。

背景技术

地磁场是地球连续分布的稳定物理场，是一种非常理想的参考基准。随着微电子技术在地磁探测传感器方面的应用，使得地磁探测技术获得了巨大的进步，由于地磁场导航定位，具有无源、无辐射、抗干扰、全天时、全天候、体积小、能耗低的优点，因此在飞机、导弹制导、舰船和潜艇等领域得到广泛的关注和研究应用。根据Tolles-Lawson理论，任何在空间中运动的物体都会产生背景磁场。导航载体通过捷联式磁传感器测得的磁场信息包括导航定位所用的目标磁场信息和其他干扰磁场信息，这些干扰磁场统称为背景磁场，因此，为了获得准确的导航定位信息，对这些背景磁场误差的标定显得至关重要。本专利主要考虑的背景磁场为固定磁场(又称为剩余磁场)、感应磁场。

在没有背景磁场作用下，理想磁场轨迹为一个原点在圆心，半径为当地地磁矢量模值的的圆球面。在背景磁场作用下由地磁矢量构成的圆球面的原点位置和球体形状都发生变化，理想磁场轨迹变成了圆心不在原点的一个椭球。目前对背景磁场误差的标定就是寻找一组最优的椭球参数，使得磁传感器测量的磁场矢量与拟合的椭球之间距离最小。

目前对于背景磁场标定的方法大多数都是基于磁传感器的测量信息进行椭球拟合。但是磁传感器的测量误差精度较低，在实际工程应用中会对背景磁场标定精度造成一定的影响。

发明内容

为解决现有技术存在的局限和缺陷，本发明提供一种基于陀螺仪及磁传感器融合的背景磁场标定方法，包括：

将磁传感器安装在弹体之内，所述磁传感器的安装点位于所述弹体的质心，以使所述磁传感器的三个轴分别与陀螺仪坐标系的三个坐标轴、弹体坐标系的三个坐标轴平行而且方向一致；

使用陀螺仪测量载体的姿态角γ，θ，ψ；

根据所述陀螺仪初始状态的姿态角γ₀，θ₀，ψ₀，以及所述陀螺仪测量的姿态角γ，θ，ψ，计算载体坐标系H^b，当地磁场强度为H^e＝[H_N,H_E,H_Z]^T，陀螺仪测量的磁场强度H^M＝H^b；

获得磁传感器测量的磁场强度为：

其中，

为固定磁场在磁传感器测量坐标系下的磁场强度，

为磁化感应磁场在磁传感器测量坐标系下的磁场强度，为涡流磁场在磁传感器测量坐标系下的磁场强度，H^e′＝d(H^e)/d(t)，K为磁传感器测量坐标系下的磁化系数矩阵，E为磁传感器测量坐标系下的满流系数矩阵，

为3×1矩阵向量，K为3×1的矩阵，E为3×1的矩阵；

设置判定函数Ω(t)，所述判定函数Ω(t)与陀螺仪精度、磁传感器精度、陀螺仪的测量误差、磁传感器的测量误差相关；

当t＜Ω(t)时，使用所述陀螺仪测算的H^b对背景磁场进行标定；

当t＞Ω(t)时，使用所述磁传感器测算的H^M对背景磁场进行标定；

目标地磁场在弹体坐标系的三分量为：

可选的，所述计算载体坐标系H^b的步骤包括：

将地磁场在所述弹体坐标系的三分量表示为：

其中，D为当地的地磁偏角，B_H为弹体所在空间地磁场的水平分量，B_Z为弹体所在空间地磁场的垂向分量；

将地磁场从所述弹体坐标系转到所述弹体坐标系，计算公式如下：

根据公式(2)获得公式(3)，计算公式如下：

计算载体坐标系H^b。

可选的，所述获得磁传感器测量的磁场强度的步骤之前包括：

在没有背景磁场的干扰之下，所述磁传感器的测值量为当地地磁矢量H^e，满足如下公式：

||H^e||²＝(H^e)^TH^e (4)

将地磁场的三分量代入公式(4)获得公式(5)，计算公式如下：

(H_ex)²+(H_ey)²+(H_ez)²＝||H_e||² (5)。

可选的，将公式(6)扩展成矩阵形式，计算公式如下：

可选的，将公式(8)扩展成矩阵形式，计算公式如下：

可选的，还包括：

对弹体的固定磁场和磁化感应磁场进行标定，计算公式如下：

对弹体的涡流磁场进行标定，计算公式如下：

可选的，还包括：

涡流磁场

的大小随着磁场在磁传感器坐标系的三分量的变化率进行变化，计算公式如下：

对公式(12)的等号两边同时求导，获得地磁场在所述弹体坐标系的三分量的变化率，计算公式如下：

使用涡流磁场模型对涡流磁场进行标定，计算公式如下：

可选的，还包括：

将航向角ψ作为前一时刻(k-1)在前期标定时所述陀螺仪测算的航向角ψ，计算公式为：ψ(k)＝ψ(k-1)；

获得俯仰角θ和横滚角γ，计算公式如下：

获得当前时刻(k)的俯仰角θ(k)，计算公式如下：

获得当前时刻(k)的横滚角γ(k)，计算公式如下：

本发明具有下述有益效果：

本发明提供的基于陀螺仪及磁传感器融合的背景磁场标定方法，包括：将磁传感器安装在弹体之内，所述磁传感器的安装点位于所述弹体的质心，以使所述磁传感器的三个轴分别与陀螺仪坐标系的三个坐标轴、弹体坐标系的三个坐标轴平行而且方向一致，使用陀螺仪测量载体的姿态角，根据所述陀螺仪初始状态的姿态角以及所述陀螺仪测量的姿态角计算载体坐标系，获得磁传感器测量的磁场强度，前期使用所述陀螺仪测算的磁场强度对背景磁场进行标定，后期使用所述磁传感器测算的磁场强度对背景磁场进行标定。本发明提供的陀螺姿态测量技术能够解决地磁姿态测量不能独立求解的问题，而且陀螺仪在姿态测量之中具有可靠性高、解算算法简单和测量精度高的优势。因此，本发明提供的基于陀螺仪及磁传感器融合的背景磁场标定方法，能够有效提高背景磁场的标定精度。

附图说明

图1为本发明实施例一提供的算法流程图。

图2为本发明实施例一提供的传感器安装以及弹体姿态示意图。

图3为本发明实施例一提供的前期标定流程图。

图4为本发明实施例一提供的后期标定流程图。

具体实施方式

为使本领域的技术人员更好地理解本发明的技术方案，下面结合附图对本发明提供的基于陀螺仪及磁传感器融合的背景磁场标定方法进行详细描述。

实施例一

本实施例假设三轴磁传感器安装在弹体内，其安装点处于弹体质心，使磁阻传感器的三个轴与陀螺仪坐标系、弹体坐标系的三轴平行而且方向一致。图1为本发明实施例一提供的算法流程图。如图1所示，当地地磁分量H^e＝[H_N,H_E,H_Z]^T，磁传感器测量分量为

本实施例前期使用陀螺仪测出载体的姿态角，根据陀螺仪初始装定的姿态角γ₀，θ₀，ψ₀，和陀螺仪测出载体的姿态角γ，θ，ψ，使用公式(1)解算到载体坐标系H^b，而且H^b＝H^M，再结合当地磁场强度H^e，根据公式(8)进行标定。后期陀螺仪误差较大时，使用磁传感器测量值H^M和当地磁场强度H^e进行标定。

前期和后期的判定取决于是选取陀螺仪和当地地磁场解算出来的H^M还是选取磁传感器测出的H^M。设置判定函数为Ω(t)，其与陀螺仪精度、磁传感器精度以及它们的测量误差等因素相关，可以结合这些因素自行设定。当t＜Ω(t)时，采用陀螺仪测姿解算出的H^b对背景磁场进行标定，当t＞Ω(t)时，采用磁传感器测出的H^M对背景磁场进行标定。

图2为本发明实施例一提供的传感器安装以及弹体姿态示意图。如图2所示，将三轴磁传感器安装在弹体内，其安装点处于弹体质心，使磁阻传感器的三个轴与陀螺仪坐标系、弹体坐标系的三轴平行且方向一致。所述质心即为图2所示载体坐标系的原点，三个敏感轴分别沿弹体坐标系三坐标轴x^b，y^b，z^b，陀螺仪三轴为x^o，y^o，z^o。磁传感器三轴x^m，y^m，z^m上的测量值是地磁场在载体坐标系三轴上的分量，用

来表示。本实施例选取的导航坐标系为NED地理坐标系，其传感器安装及弹体姿态如图2所示。

根据坐标系旋转原理和地磁场，在弹体坐标系三轴上地磁分量可表示为：

其中，D为当地的地磁偏角，B_H、B_Z分别为弹体所在空间地磁场的水平分量和垂向分量，在利用地磁场进行导航和姿态测试的参考标准时，需要根据具体的情况选择合适的地磁模型计算得到地磁场要素，以此来得到参考标准。在计算中国区域地磁要素时，可以利用IGRF12、WMM2015模型或者中国地磁模型资料。

将导航坐标系转到弹体坐标系，计算公式如下：

整理公式(2)可以获得：

由图2可知，本实施例前期使用陀螺仪测出载体的姿态角，直接给出当地地磁分量H^e＝[H_N,H_E,H_Z]^T，根据陀螺仪初始装定的姿态角γ₀，θ₀，ψ₀，陀螺仪测出的姿态角为γ，θ，ψ，利用公式(1)解算到载体坐标系H^b，H^b＝H^M。结合当地磁场强度H^e，根据公式(8)进行标定。后期陀螺仪误差较大时，采用磁传感器测量值H^M和当地磁场强度H^e进行标定。

前期和后期的判定取决于是选取陀螺仪和当地地磁场解算出来的H^M还是选取磁传感器测出的H^M。本实施例设置判定函数为Ω(t)，其与陀螺仪精度，磁传感器精度以及它们的测量误差等因素相关，可结合这些因素自行设定。当t＜Ω(t)时，采用陀螺仪测姿解算出的H^b对背景磁场进行标定，当t＞Ω(t)时，采用磁传感器测出的H^M对背景磁场进行标定。

地球上某固定点处，在没有背景磁场干扰下，磁传感器的测值量为当地地磁矢量H^e，满足如下公式：

||H^e||²＝(H^e)^TH^e (4)

将地磁场的三分量代入公式(4)获得公式(5)，计算公式如下：

(H_ex)²+(H_ey)²+(H_ez)²＝||H_e||² (5)

对于磁测量而言，背景磁场就是指除目标磁场外的其他干扰磁场。通常意义下，背景磁场主要包括弹体产生的固定磁场、磁化感应磁场、涡流磁场。弹体背景磁场在磁传感器测量坐标系下的总模型为：对于利用地磁来测量弹体姿态而言，背景磁场是除地磁场分量外的所有干扰磁场。本实施例获得磁传感器测量的磁场强度为：

其中，

为固定磁场在磁传感器测量坐标系下的磁场强度，

为磁化感应磁场在磁传感器测量坐标系下的磁场强度，

为涡流磁场在磁传感器测量坐标系下的磁场强度，H^e′＝d(H^e)/d(t)，K为磁传感器测量坐标系下的磁化系数矩阵，E为磁传感器测量坐标系下的满流系数矩阵，

为3×1矩阵向量，K为3×1的矩阵，E为3×1的矩阵。

将公式(6)扩展成矩阵形式，计算公式如下：

本实施例前期利用陀螺仪测出载体的姿态角，解算出三轴磁传感器在弹体坐标轴的三分量，后期利用三轴磁传感器实测的磁场强度在弹体坐标轴的三分量，经过下面公式所示的模型化补偿过程求得目标地磁场在弹体坐标轴的三分量。目标地磁场在弹体坐标系的三分量为：

将公式(8)扩展成矩阵形式，计算公式如下：

上式是一个待定常系数六元一次方程组，其中每个方程都是一个包含7个未知系数的六元一次线性方程，而且相互独立。背景磁场补偿就是求取上述方程组21个待定常系数。

本实施例空中自标定背景磁场分为两个部分，在炮弹发射出去的前期采用陀螺仪标定磁场，后期采用磁传感器标定。由于陀螺仪在姿态测量的初期具有较高的可靠性、算法简单且精度高的特性，后期随着时间的增加，测量误差越来越大，所以前期采用陀螺仪标定，可提高测量精度，从而降低标定误差，后期再采用磁传感器标定。

当数据点的数目为n时，

已知，求21个待定常系数

k_xx，k_xy，k_xz，k_yx，k_yy，k_yz，k_zx，k_zy，k_zz，e_xx，e_xy，e_xz，e_yx，e_yy，e_yz，e_zx，e_zy，e_zz。只要求解出公式(9)中各项待辨识的参数，就能够计算出公式(2)中待标定的各项误差。对于方程组(9)求解问题可直接利用递推算法、牛顿迭代算法、最小二乘法、神经网络算法等方法求解，由于方程组有21个待定系数，计算量大，因此本实施例采用融合算法求解：

①对弹体固定磁场和磁化感应磁场进行标定。

②对弹体的涡流磁场进行标定。

图3为本发明实施例一提供的前期标定流程图。如图3所示，前期陀螺仪的精度较高，利用陀螺仪解算出的和陀螺仪测出的姿态角分别对①、②进行标定。图4为本发明实施例一提供的后期标定流程图。如图4所示，后期利用磁传感器测出的H^M及其解算出的姿态角分别对①、②进行标定。

当弹体处于角运动状态下，具有较高的姿态角速度时，涡流磁场干扰EH^e′无法直接从传感器的输出中得到，本实施例首先利用椭球拟合法算出固定磁场和磁化感应磁场的补偿系数并结合公式(10)的输出模型算出传感器输出之中固定磁场和磁化感应磁场的干扰分量，再通过式(11)将涡流磁场干扰分量分离出来，分别对①、②进行标定，计算公式如下：

本实施例对固定磁场和磁化感应磁场进行标定。前期先用陀螺仪测出载体的姿态角，直接给出当地地磁分量H^e＝[H_N,H_E,H_Z]^T，根据陀螺仪初始装定的姿态角γ₀，θ₀，ψ₀，陀螺仪测出的姿态角为γ，θ，ψ，利用公式(1)解算到载体坐标系H^b，H^b＝H^M，再结合当地磁场强度H^e，根据公式(10)利用椭球拟合法对弹体固定磁场

和磁化感应磁场KH^e进行标定。

本实施例对涡流磁场进行标定。涡流磁场

的大小随地磁场在磁传感器坐标系内三分量的变化率改变而变化，即与弹体的姿态角信息和相关地磁场要素有关，可以使用如下公式进行表示：

其中，γ，θ，ψ为由陀螺仪测出载体的姿态角，H^e为当地地磁场强度。对公式(12)的等号两边同时求导，获得地磁场在所述弹体坐标系的三分量的变化率，计算公式如下：

本实施例使用公式(1)解算到载体坐标系H^b，H^b＝H^M，结合当地磁场强度H^e，根据公式(11)使用公式(14)的涡流磁场模型对涡流磁场进行标定，计算公式如下：

后期陀螺仪误差较大时，本实施例使用磁传感器测量值H^M和其解算出的姿态角分别对①、②进行标定。对于大多数的常规弹药的距离变化，在炮兵标准气象条件下，航向角变化很小。姿态解算时可将航向角ψ近似看为前一时刻(k-1)前期标定时陀螺仪解算出的航向角ψ，即ψ(k)＝ψ(k-1)。俯仰角θ和横滚角γ，可以通过公式(15)求解。根据公式(3)使用公式(15)解算出姿态角γ，θ，ψ。根据公式(10)利用椭球拟合法对弹体固定磁场

和磁化感应磁场KH^e进行标定，利用公式(14)的涡流磁场模型对涡流磁场进行标定。

已知ψ(k)＝ψ(k-1)，公式(3)第二式可以写成公式(15)的形式，求得当前时刻(k)的俯仰角θ(k)。公式(3)第一式可以写成公式(16)的形式，求得当前时刻(k)的俯仰角θ(k)。公式(3)第一式可以写成公式(17)的形式，求得当前时刻(k)的横滚角γ(k)，计算公式如下：

本实施例提供的基于陀螺仪及磁传感器融合的背景磁场标定方法，包括：将磁传感器安装在弹体之内，所述磁传感器的安装点位于所述弹体的质心，以使所述磁传感器的三个轴分别与陀螺仪坐标系的三个坐标轴、弹体坐标系的三个坐标轴平行而且方向一致，使用陀螺仪测量载体的姿态角，根据所述陀螺仪初始状态的姿态角以及所述陀螺仪测量的姿态角计算载体坐标系，获得磁传感器测量的磁场强度，前期使用所述陀螺仪测算的磁场强度对背景磁场进行标定，后期使用所述磁传感器测算的磁场强度对背景磁场进行标定。本实施例提供的陀螺姿态测量技术能够解决地磁姿态测量不能独立求解的问题，而且陀螺仪在姿态测量之中具有可靠性高、解算算法简单和测量精度高的优势。因此，本实施例提供的基于陀螺仪及磁传感器融合的背景磁场标定方法，能够有效提高背景磁场的标定精度。

可以理解的是，以上实施方式仅仅是为了说明本发明的原理而采用的示例性实施方式，然而本发明并不局限于此。对于本领域内的普通技术人员而言，在不脱离本发明的精神和实质的情况下，可以做出各种变型和改进，这些变型和改进也视为本发明的保护范围。

Claims (8)

1.一种基于陀螺仪及磁传感器融合的背景磁场标定方法，其特征在于，包括：

将磁传感器安装在弹体之内，所述磁传感器的安装点位于所述弹体的质心，以使所述磁传感器的三个轴分别与陀螺仪坐标系的三个坐标轴、弹体坐标系的三个坐标轴平行而且方向一致；

使用陀螺仪测量载体的姿态角γ，θ，ψ；

根据所述陀螺仪初始状态的姿态角γ₀，θ₀，ψ₀，以及所述陀螺仪测量的姿态角γ，θ，ψ，计算载体坐标系H^b，当地磁场强度为H^e＝[H_N,H_E,H_Z]^T，陀螺仪测量的磁场强度H^M＝H^b；

获得磁传感器测量的磁场强度为：

其中，

为固定磁场在磁传感器测量坐标系下的磁场强度，

为磁化感应磁场在磁传感器测量坐标系下的磁场强度，为涡流磁场在磁传感器测量坐标系下的磁场强度，H^e′＝d(H^e)/d(t)，K为磁传感器测量坐标系下的磁化系数矩阵，E为磁传感器测量坐标系下的满流系数矩阵，

为3×1矩阵向量，K为3×1的矩阵，E为3×1的矩阵；

设置判定函数Ω(t)，所述判定函数Ω(t)与陀螺仪精度、磁传感器精度、陀螺仪的测量误差、磁传感器的测量误差相关；

当t＜Ω(t)时，使用所述陀螺仪测算的H^b对背景磁场进行标定；

当t＞Ω(t)时，使用所述磁传感器测算的H^M对背景磁场进行标定；

目标地磁场在弹体坐标系的三分量为：

2.根据权利要求1所述的基于陀螺仪及磁传感器融合的背景磁场标定方法，其特征在于，所述计算载体坐标系H^b的步骤包括：

将地磁场在所述弹体坐标系的三分量表示为：

其中，D为当地的地磁偏角，B_H为弹体所在空间地磁场的水平分量，B_Z为弹体所在空间地磁场的垂向分量；

将地磁场从所述弹体坐标系转到所述弹体坐标系，计算公式如下：

根据公式(2)获得公式(3)，计算公式如下：

计算载体坐标系H^b。

3.根据权利要求1所述的基于陀螺仪及磁传感器融合的背景磁场标定方法，其特征在于，所述获得磁传感器测量的磁场强度的步骤之前包括：

在没有背景磁场的干扰之下，所述磁传感器的测值量为当地地磁矢量H^e，满足如下公式：

||H^e||²＝(H^e)^TH^e (4)

将地磁场的三分量代入公式(4)获得公式(5)，计算公式如下：

(H_ex)²+(H_ey)²+(H_ez)²＝||H_e||² (5)。

4.根据权利要求1所述的基于陀螺仪及磁传感器融合的背景磁场标定方法，其特征在于，将公式(6)扩展成矩阵形式，计算公式如下：

5.根据权利要求1所述的基于陀螺仪及磁传感器融合的背景磁场标定方法，其特征在于，将公式(8)扩展成矩阵形式，计算公式如下：

6.根据权利要求1所述的基于陀螺仪及磁传感器融合的背景磁场标定方法，其特征在于，还包括：

对弹体的固定磁场和磁化感应磁场进行标定，计算公式如下：

对弹体的涡流磁场进行标定，计算公式如下：

7.根据权利要求6所述的基于陀螺仪及磁传感器融合的背景磁场标定方法，其特征在于，还包括：

涡流磁场

的大小随着磁场在磁传感器坐标系的三分量的变化率进行变化，计算公式如下：

对公式(12)的等号两边同时求导，获得地磁场在所述弹体坐标系的三分量的变化率，计算公式如下：

使用涡流磁场模型对涡流磁场进行标定，计算公式如下：

8.根据权利要求6所述的基于陀螺仪及磁传感器融合的背景磁场标定方法，其特征在于，还包括：

将航向角ψ作为前一时刻(k-1)在前期标定时所述陀螺仪测算的航向角ψ，计算公式为：ψ(k)＝ψ(k-1)；

获得俯仰角θ和横滚角γ，计算公式如下：

获得当前时刻(k)的俯仰角θ(k)，计算公式如下：

获得当前时刻(k)的横滚角γ(k)，计算公式如下：

Images