CN104166169A - 一种捷联于水下潜器的三轴磁场传感器误差补偿方法 - Google Patents

Abstract

本发明属于水下地磁辅助导航领域，具体涉及到一种捷联于水下潜器的三轴磁场传感器误差补偿方法。本发明包括；通过捷联于水下潜器的三轴矢量磁传感器获得载体一定姿态下的地磁场模量梯度测量值；获取一组地磁场模量梯度测量值及每一测量值所对应的载体姿态角信息；将获得的不同姿态下的地磁场模量梯度测量值F^m代入包含载体感应磁场系数矩阵；用信赖域法估计方程中各个误差矩阵中的未知参数；得到各误差矩阵中的误差元素进而得到各误差矩阵；得到的各个误差矩阵代入包含载体感应磁场系数矩阵的方程对地磁场测量值进行补偿。本发明避免了初始点选取不合适可能导致迭代发散而无法得到正确的估计结果。

Description

一种捷联于水下潜器的三轴磁场传感器误差补偿方法

技术领域

本发明属于水下地磁辅助导航领域，具体涉及到一种捷联于水下潜器的三轴磁场传感器误差补偿方法。

背景技术

水下地磁导航定位具有无源、无辐射、全天时、全地域等特点，是实现水下潜航器实时、连续、精确的水下自主导航的理想途径之一。在地磁导航技术中，载体上地磁传感器测量地磁场数据的准确性决定了地磁导航的精度。一旦测量过程引入干扰项，地磁导航精度将受到严重影响，甚至无法进行导航。因此必须补偿载体矢量磁传感器的输出误差。载体环境磁场干扰及磁传感器自身误差是导致其输出误差的主要因素，必须予以补偿。实际中载体干扰场主要由固定磁场和载体感应磁场两部分组成，目前常用的载体磁场补偿方法主要有航磁补偿法、两步估计法、椭圆拟合法和无迹Kalman滤波(UKF)法。航磁补偿法是基于Tolles-Lawson方程的方法，该方法是将地磁场测量值的三分量都投影到地磁场方向上，由于忽略了地磁场真实方向与测量方向之间的误差，因此该方法只在干扰场较小的情况下适用。两步估计法算法简单，但由于其引入了中间变量，各变量之间的相关性可能造成系数矩阵奇异，无法得到正确的参数估计值。椭圆拟合法则是通过利用测量值拟合出一个椭圆，根据椭圆参数(中心点、长轴、短轴)与载体磁场系数之间的关系来获得载体磁场各参数。这种方法要求所采集的数据必须能够拟合出一个椭圆，算法复杂，在实际应用中受到限制。UKF法可以对载体磁场进行实时补偿，但这种方法对初始参数的选择很敏感，参数选取不当则有可能导致滤波发散。以上研究在进行载体磁场建模与补偿时通常将载体感应磁场系数矩阵视为常数，但载体感应磁场随着载体的工作状态、姿态等变化而变化。如果将载体感应磁场系数矩阵视为常数会造成载体磁场模型不精确，影响磁场补偿效果。

本发明提出一种捷联于水下潜器的三轴磁场传感器误差补偿方法。该方法在对导航载体磁场建模过程中载体感应磁场系数矩阵随载体工作状态、姿态等变化而变化，结合磁传感器自身误差，利用磁场模量梯度测量装置得到关于地磁场测量值的非线性方程，采用信赖域法辨识载体固定磁场参数及感应磁场磁测误差参数，最后再利用估计结果对测量值进行补偿。该方法参数估计精度高，实用性强，可以有效地对导航载体干扰磁场进行补偿。

发明内容

本发明的目的在于提供一种捷联于水下潜器的三轴磁场传感器误差补偿方法。

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

步骤1、通过捷联于水下潜器的三轴矢量磁传感器获得载体一定姿态下的地磁场模量梯度测量值F^m，同时利用载体上的惯导或陀螺罗经实时输出载体的姿态角，即航向角ψ和俯仰角γ；

步骤2、重复步骤1获取一组地磁场模量梯度测量值及每一测量值所对应的载体姿态角信息；

步骤3、将步骤2获得的不同姿态下的地磁场模量梯度测量值F^m代入包含载体感应磁场系数矩阵K的方程，

$f={\left|\right|B_e\left|\right|}^2-B_m^2={\left|\right|B_e\left|\right|}^2-[{\left(F^m\right)}^T{\mathrm{\ΓF}}^m-{2O}^T{\mathrm{\ΓF}}^m+O^T\mathrm{\ΓO}]$

将测量的每一个地磁场模量梯度测量值代入包含载体感应磁场系数矩阵K的方程的同时，将所对应的载体姿态信息ψ和γ代入矩阵元素

$\begin{array}{c}{K}_{\mathrm{ij}}=g_{\mathrm{ij}}^0+g_{\mathrm{ij}}^1\cos \mathrm{\ψ}+g_{\mathrm{ij}}^2\sin \mathrm{\ψ}+g_{\mathrm{ij}}^3\cos \mathrm{\γ}+g_{\mathrm{ij}}^4\sin \mathrm{\γ}+g_{\mathrm{ij}}^5\cos 2\mathrm{\ψ}+g_{\mathrm{ij}}^6\sin 2\mathrm{\ψ}+g_{\mathrm{ij}}^7\cos 2\mathrm{\γ}\\ +g_{\mathrm{ij}}^8\sin 2\mathrm{\γ}+g_{\mathrm{ij}}^9\cos \mathrm{\ψ}\cos \mathrm{\γ}+g_{\mathrm{ij}}^{10}\cos \mathrm{\ψ}\sin \mathrm{\γ}+g_{\mathrm{ij}}^{11}\sin \mathrm{\ψ}\cos \mathrm{\γ}+g_{\mathrm{ij}}^{12}\sin \mathrm{\ψ}\sin \mathrm{\γ}\end{array}$

K_ij(i,j＝1,2,3)表示矩阵元素，为展开式系数；

其中||B_e||²代表地磁矢量模真实值的平方，代表地磁矢量模测量值的平方，Γ＝G^TG，G＝C^-1，C＝C_SC_NC_δC_β， $C_S=\left[\begin{array}{ccc}{c}_x& 0& 0\\ 0& c_y& 0\\ 0& 0& c_z\end{array}\right]$ 代表三轴磁传感器标度因子不一致引起的误差矩阵， $C_N=\left[\begin{array}{ccc}1& {\mathrm{\θ}}_3& {\mathrm{\θ}}_2\\ 0& 1& {\mathrm{\θ}}_1\\ 0& 0& 0\end{array}\right]$ 代表磁传感器三轴非正交引起的误差矩阵， $C_{\mathrm{\δ}}=\left[\begin{array}{ccc}1& {\mathrm{\γ}}_s& -{\mathrm{\β}}_s\\ -{\mathrm{\γ}}_s& 1& {\mathrm{\α}}_s\\ {\mathrm{\β}}_s& {-\mathrm{\α}}_s& 1\end{array}\right]$ 代表磁测坐标系与载体坐标系不一致引起的误差矩阵，C_β＝E+K表示软磁材料引起的误差矩阵，E为单位方阵； $K=\left[\begin{array}{ccc}{K}_{\mathrm{xx}}& K_{\mathrm{yx}}& K_{\mathrm{zx}}\\ K_{\mathrm{xy}}& K_{\mathrm{yy}}& K_{\mathrm{zy}}\\ K_{\mathrm{xz}}& K_{\mathrm{yz}}& K_{\mathrm{zz}}\end{array}\right]$ 代表载体感应磁场系数矩阵，其元素与载体的姿态有关；矩阵 $O=\left[\begin{array}{c}{O}_x\\ O_y\\ O_z\end{array}\right]$ 代表磁传感器零偏和硬磁偏差，各未知量就是载体感应磁场及磁测装置误差参数；

步骤4、用信赖域法估计方程中各个误差矩阵中的未知参数；

$\underset{X}{\min }g\left(X\right)=\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{\left|f_j\right|}^2$

其中，X表示待估计的参量向量，N为采样点数；

步骤5、根据步骤4估计得到各误差矩阵中的误差元素进而得到各误差矩阵；

步骤6、将步骤5得到的各个误差矩阵代入包含载体感应磁场系数矩阵K的方程对地磁场测量值B_m进行补偿。

2.根据权利要求1所述的一种捷联于水下潜器的三轴磁场传感器误差补偿方法，其特征在于：

载体感应磁场系数矩阵K是这样建立的：在空间任意点，载体软铁磁场与地磁场之间的关系为

$\left[\begin{array}{c}{B}_{\mathrm{Sx}}\\ B_{\mathrm{Sy}}\\ B_{\mathrm{Sz}}\end{array}\right]=\left[\begin{array}{ccc}{K}_{\mathrm{xx}}& K_{\mathrm{yx}}& K_{\mathrm{zx}}\\ K_{\mathrm{xy}}& K_{\mathrm{yy}}& K_{\mathrm{zy}}\\ K_{\mathrm{xz}}& K_{\mathrm{yz}}& K_{\mathrm{zz}}\end{array}\right]\left[\begin{array}{c}{B}_x\\ B_y\\ B_z\end{array}\right]=K\left[\begin{array}{c}{B}_x\\ B_y\\ B_z\end{array}\right]$

其中，B_x、B_y、B_z为地磁场在载体坐标系下的三分量，B_Sx、B_Sy、B_Sz为载体软铁磁场在载体坐标系下的三分量，矩阵K为载体感应磁场系数矩阵，其元素与载体的姿态有关，对不同航向角ψ和俯仰角γ下的软铁磁场分布数据的一部分进行拟合，建立线性转换矩阵与载体航向角和俯仰角间的函数关系。

3.根据权利要求1所述的一种捷联于水下潜器的三轴磁场传感器误差补偿方法，其特征在于：

所述的信赖域法包括：

4.1设定初始点X₀，初始信赖域半径r₀以及参数0＜μ＜η＜1，ε＞0；

4.2计算若则X^*＝X_k，迭代终止，否则转4.3；

4.3求解 $\min h\left(d_k\right)=\frac{1}{2}{d}_k^T\frac{{\∂}^{2}g{\left(X_k\right)}^T}{{\∂X}^2}{d}_k+\frac{\∂g{\left(X_k\right)}^T}{\∂X}{d}_k,$ 约束条件为||d_k||≤r_k，其最优解为d^*，令若ρ≤μ，则X_k+1＝X_k，反之X_k+1＝X_k+d^*；

4.4确定r_k+1，若ρ≤μ则r_k+1＝0.5r_k，若μ＜ρ＜η则r_k+1＝r_k，否则r_k+1＝2r_k；

4.5令k＝k+1，返回4.4。

本发明的有益效果在于：提出的一种捷联于水下潜器的三轴磁场传感器误差补偿方法，步骤3中载体感应磁场随着载体的工作状态、姿态等变化而变化，更符合实际情况，而以往研究在进行载体磁场建模与补偿时通常将载体感应磁场系数矩阵视为常数，影响磁场补偿效果。步骤4采用的信赖域法估计误差参数法相比于其它算法对初始迭代点不敏感，避免初始点选取不合适可能导致迭代发散而无法得到正确的估计结果。

具体实施方式

下面对本发明的实施方式进行详细描述：

本发明的一种捷联于水下潜器的三轴磁场传感器误差补偿方法，捷联于水下潜器的三轴磁传感器构成地磁场模量梯度测量装置，令载体进行一系列变化姿态的校正机动航行，同时磁传感器测出一组载体不同姿态下的包含载体干扰磁场和传感器自身误差信息的地磁场模量梯度测量值，并利用惯导系统或陀螺罗经纪录每次测量时载体的姿态，利用磁传感器的测量值得到关于地磁场测量值的非线性方程，方程中误差矩阵中各未知矩阵元素就是载体干扰磁场及磁测装置误差参数，采用信赖域法估计出载体干扰磁场误差参数及磁测装置误差参数，最后再利用估计结果对地磁场测量值进行补偿。其具体步骤如下：

步骤1、通过捷联于水下潜器的三轴磁传感器获得载体一定姿态下的地磁场模量梯度测量值F^m，同时利用惯导或陀螺罗经输出每次测量所对应的载体姿态角，即航向角ψ和俯仰角γ。

安装于载体上的矢量磁传感器，导致其输出误差的因素有磁传感器自身的误差和环境磁场的干扰。环境磁场主要由硬铁磁场、软铁磁场和随机磁场等组成。对于水下慢速运载体而言，其切割地磁场产生的电磁感应场较小，水下导航载体磁场主要由硬铁磁场、软铁磁场和随机磁场等组成。硬铁磁场主要是由载体硬铁材料的剩磁所产生，短期内不随时间改变，为一常量。硬铁材料和磁传感器捷联于载体，不论载体姿态怎样变化，硬铁磁场在传感器坐标系下分量是不变的，相当于传感器的零偏。软铁磁场由载体软磁材料在地磁场中被磁化而产生，即载体感应磁场，其大小与外加磁场成正比，因此在传感器坐标系下载体感应磁场的大小与方向随着载体的姿态变化而变化，不能将其视为常量。

综合考虑磁传感器自身误差、磁传感器系统误差，载体感应磁场效应，不同磁传感器间配准误差，所以在一次测量中地磁场模量梯度测量值F^m可以表示为：

F^m＝C_SC_NC_δC_βB_e+O+n    (1)

其中，B_m为地磁场测量值， $C_S=\left[\begin{array}{ccc}{c}_x& 0& 0\\ 0& c_y& 0\\ 0& 0& c_z\end{array}\right]$ 代表三轴磁传感器标度因子不一致引起的误差矩阵， $C_N=\left[\begin{array}{ccc}1& {\mathrm{\θ}}_3& {\mathrm{\θ}}_2\\ 0& 1& {\mathrm{\θ}}_1\\ 0& 0& 0\end{array}\right]$ 代表磁传感器三轴非正交引起的误差矩阵； $C_{\mathrm{\δ}}=\left[\begin{array}{ccc}1& {\mathrm{\γ}}_s& -{\mathrm{\β}}_s\\ -{\mathrm{\γ}}_s& 1& {\mathrm{\α}}_s\\ {\mathrm{\β}}_s& {-\mathrm{\α}}_s& 1\end{array}\right]$ 代表磁测坐标系与载体坐标系不一致引起的误差矩阵，C_β＝E+K表示软磁材料引起的误差矩阵，E为单位方阵； $K=\left[\begin{array}{ccc}{K}_{\mathrm{xx}}& K_{\mathrm{yx}}& K_{\mathrm{zx}}\\ K_{\mathrm{xy}}& K_{\mathrm{yy}}& K_{\mathrm{zy}}\\ K_{\mathrm{xz}}& K_{\mathrm{yz}}& K_{\mathrm{zz}}\end{array}\right]$ 代表载体感应磁场系数矩阵，矩阵 $O=\left[\begin{array}{c}{O}_x\\ O_y\\ O_z\end{array}\right]$ 代表磁传感器零偏和硬磁偏差；向量n代表传感器噪声和外部残余随机干扰。噪声n的影响由数值仿真中加入噪声来体现。由(1)式可得到地磁场测量值B_m与地磁场模量梯度测量值F^m关系式：

B_m＝C^-1F^m-C^-1O＝G(F^m-O)    (2)

式中，C＝C_SC_NC_δC_β，G＝C^-1，且矩阵G一定存在。

所述的载体感应磁场系数矩阵K是这样建立的：在空间任意点，载体软铁磁场与地磁场之间的关系为

$\left[\begin{array}{c}{B}_{\mathrm{Sx}}\\ B_{\mathrm{Sy}}\\ B_{\mathrm{Sz}}\end{array}\right]=\left[\begin{array}{ccc}{K}_{\mathrm{xx}}& K_{\mathrm{yx}}& K_{\mathrm{zx}}\\ K_{\mathrm{xy}}& K_{\mathrm{yy}}& K_{\mathrm{zy}}\\ K_{\mathrm{xz}}& K_{\mathrm{yz}}& K_{\mathrm{zz}}\end{array}\right]\left[\begin{array}{c}{B}_x\\ B_y\\ B_z\end{array}\right]=K\left[\begin{array}{c}{B}_x\\ B_y\\ B_z\end{array}\right]---\left(3\right)$

其中，B_x、B_y、B_z为地磁场在载体坐标系下的三分量，B_Sx、B_Sy、B_Sz为载体软铁磁场在载体坐标系下的三分量，矩阵K为载体感应磁场系数矩阵，其元素与载体的姿态有关。由于三个姿态角的函数组合很多，描述软铁磁场与三个姿态角的完整函数关系过于复杂，而一般情况下载体横滚角不会改变，因此建立软铁磁场与航向角和俯仰角间数学模型，模型相对简单。

对不同航向角ψ和俯仰角γ下的软铁磁场分布数据的一部分进行拟合，按式(4)所示规律，建立线性转换矩阵与载体航向角和俯仰角间的函数关系。

$\begin{array}{c}{K}_{\mathrm{ij}}=g_{\mathrm{ij}}^0+g_{\mathrm{ij}}^1\cos \mathrm{\ψ}+g_{\mathrm{ij}}^2\sin \mathrm{\ψ}+g_{\mathrm{ij}}^3\cos \mathrm{\γ}+g_{\mathrm{ij}}^4\sin \mathrm{\γ}+g_{\mathrm{ij}}^5\cos 2\mathrm{\ψ}+g_{\mathrm{ij}}^6\sin 2\mathrm{\ψ}+g_{\mathrm{ij}}^7\cos 2\mathrm{\γ}\\ +g_{\mathrm{ij}}^8\sin 2\mathrm{\γ}+g_{\mathrm{ij}}^9\cos \mathrm{\ψ}\cos \mathrm{\γ}+g_{\mathrm{ij}}^{10}\cos \mathrm{\ψ}\sin \mathrm{\γ}+g_{\mathrm{ij}}^{11}\sin \mathrm{\ψ}\cos \mathrm{\γ}+g_{\mathrm{ij}}^{12}\sin \mathrm{\ψ}\sin \mathrm{\γ}\end{array}---\left(4\right)$

式中，K_ij(i,j＝x,y)表示矩阵元素，是待估计的展开式系数。

步骤2、重复步骤1获取一组地磁场模量梯度测量值及其所对应的载体姿态角信息。

步骤3、将步骤2中获得的不同姿态下的地磁场模量梯度测量值F^m代入地磁矢量模真实值的平方||B_e||²与测量值的平方之差关系

$f={\left|\right|B_e\left|\right|}^2-B_m^2={\left|\right|B_e\left|\right|}^2-[{\left(F^m\right)}^T{\mathrm{\ΓF}}^m-{2O}^T{\mathrm{\ΓF}}^m+O^T\mathrm{\ΓO}]---\left(5\right)$

步骤4、用信赖域法估计出方程(5)中各个误差矩阵中的未知参数(c_x,c_y,c_z)、(θ₁,θ₂,θ₃)、(α_s,β_s,γ_s)、(g^l _ij)、和(O_x,O_y,O_z)。所以估计载体干扰磁场及磁测装置误差参数的问题可转化为下面的非线性优化问题：

$\underset{X}{\min }g\left(X\right)=\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{\left|f_j\right|}^2---\left(6\right)$

其中，X表示待估计的参量向量，N为采样点数。

非线性模型参数估计问题常采用迭代法求解，高斯-牛顿法等迭代算法对初始参数依赖性比较强，初始点选取不合适可能导致迭代发散而无法得到正确的估计值。信赖域法是一个非常有效的优化方法，基本思想是通过将试探步长d_k限制在一个可“信赖”的区域内保证算法的全局收敛性。算法步骤：

(a)设定初始点X₀，初始信赖域半径r₀以及参数0＜μ＜η＜1，ε＞0；

(b)计算若则X^*＝X_k，迭代终止，否则转(c)；

(c)求解 $\min h\left(d_k\right)=\frac{1}{2}{d}_k^T\frac{{\∂}^{2}g{\left(X_k\right)}^T}{{\∂X}^2}{d}_k+\frac{\∂g{\left(X_k\right)}^T}{\∂X}{d}_k,$ 约束条件为||d_k||≤r_k。假定其最优解为d^*，令若ρ≤μ，则X_k+1＝X_k，反之X_k+1＝X_k+d^*；

(d)确定r_k+1，若ρ≤μ则r_k+1＝0.5r_k，若μ＜ρ＜η则r_k+1＝r_k，否则r_k+1＝2r_k；

(e)令k＝k+1，返回(b)。

步骤5、根据步骤4估计得到各误差矩阵中的误差元素进而得到各误差矩阵。

步骤6、将步骤5得到的各个误差矩阵代入(2)式对地磁场测量值B_m进行补偿

本发明提供了一种捷联于水下潜器的三轴磁场传感器误差补偿方法，该方法具有参数估计精度高、方法简单、补偿效果好的特点，能够用来对地磁场测量值进行高精度补偿。

Claims (3)

1.一种捷联于水下潜器的三轴磁场传感器误差补偿方法，其特征在于：

步骤1、通过捷联于水下潜器的三轴矢量磁传感器获得载体一定姿态下的地磁场模量梯度测量值F^m，同时利用载体上的惯导或陀螺罗经实时输出载体的姿态角，即航向角ψ和俯仰角γ；

步骤2、重复步骤1获取一组地磁场模量梯度测量值及每一测量值所对应的载体姿态角信息；

步骤3、将步骤2获得的不同姿态下的地磁场模量梯度测量值F^m代入包含载体感应磁场系数矩阵K的方程，

$f={\left|\right|B_e\left|\right|}^2-B_m^2={\left|\right|B_e\left|\right|}^2-[{\left(F^m\right)}^T{\mathrm{\ΓF}}^m-{2O}^T{\mathrm{\ΓF}}^m+O^T\mathrm{\ΓO}]$

将测量的每一个地磁场模量梯度测量值代入包含载体感应磁场系数矩阵K的方程的同时，将所对应的载体姿态信息ψ和γ代入矩阵元素

$\begin{array}{c}{K}_{\mathrm{ij}}=g_{\mathrm{ij}}^0+g_{\mathrm{ij}}^1\cos \mathrm{\ψ}+g_{\mathrm{ij}}^2\sin \mathrm{\ψ}+g_{\mathrm{ij}}^3\cos \mathrm{\γ}+g_{\mathrm{ij}}^4\sin \mathrm{\γ}+g_{\mathrm{ij}}^5\cos 2\mathrm{\ψ}+g_{\mathrm{ij}}^6\sin 2\mathrm{\ψ}+g_{\mathrm{ij}}^7\cos 2\mathrm{\γ}\\ +g_{\mathrm{ij}}^8\sin 2\mathrm{\γ}+g_{\mathrm{ij}}^9\cos \mathrm{\ψ}\cos \mathrm{\γ}+g_{\mathrm{ij}}^{10}\cos \mathrm{\ψ}\sin \mathrm{\γ}+g_{\mathrm{ij}}^{11}\sin \mathrm{\ψ}\cos \mathrm{\γ}+g_{\mathrm{ij}}^{12}\sin \mathrm{\ψ}\sin \mathrm{\γ}\end{array}$

K_ij(i,j＝1,2,3)表示矩阵元素，为展开式系数；

其中||B_e||²代表地磁矢量模真实值的平方，代表地磁矢量模测量值的平方，Γ＝G^TG，G＝C^-1，C＝C_SC_NC_δC_β， $C_S=\left[\begin{array}{ccc}{c}_x& 0& 0\\ 0& c_y& 0\\ 0& 0& c_z\end{array}\right]$ 代表三轴磁传感器标度因子不一致引起的误差矩阵， $C_N=\left[\begin{array}{ccc}1& {\mathrm{\θ}}_3& {\mathrm{\θ}}_2\\ 0& 1& {\mathrm{\θ}}_1\\ 0& 0& 0\end{array}\right]$ 代表磁传感器三轴非正交引起的误差矩阵， $C_{\mathrm{\δ}}=\left[\begin{array}{ccc}1& {\mathrm{\γ}}_s& -{\mathrm{\β}}_s\\ -{\mathrm{\γ}}_s& 1& {\mathrm{\α}}_s\\ {\mathrm{\β}}_s& {-\mathrm{\α}}_s& 1\end{array}\right]$ 代表磁测坐标系与载体坐标系不一致引起的误差矩阵，C_β＝E+K表示软磁材料引起的误差矩阵，E为单位方阵； $K=\left[\begin{array}{ccc}{K}_{\mathrm{xx}}& K_{\mathrm{yx}}& K_{\mathrm{zx}}\\ K_{\mathrm{xy}}& K_{\mathrm{yy}}& K_{\mathrm{zy}}\\ K_{\mathrm{xz}}& K_{\mathrm{yz}}& K_{\mathrm{zz}}\end{array}\right]$ 代表载体感应磁场系数矩阵，其元素与载体的姿态有关；矩阵 $O=\left[\begin{array}{c}{O}_x\\ O_y\\ O_z\end{array}\right]$ 代表磁传感器零偏和硬磁偏差，各未知量就是载体感应磁场及磁测装置误差参数；

步骤4、用信赖域法估计方程中各个误差矩阵中的未知参数；

$\underset{X}{\min }g\left(X\right)=\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{\left|f_j\right|}^2$

其中，X表示待估计的参量向量，N为采样点数；

步骤5、根据步骤4估计得到各误差矩阵中的误差元素进而得到各误差矩阵；

步骤6、将步骤5得到的各个误差矩阵代入包含载体感应磁场系数矩阵K的方程对地磁场测量值B_m进行补偿。

2.根据权利要求1所述的一种捷联于水下潜器的三轴磁场传感器误差补偿方法，其特征在于：

载体感应磁场系数矩阵K是这样建立的：在空间任意点，载体软铁磁场与地磁场之间的关系为

$\left[\begin{array}{c}{B}_{\mathrm{Sx}}\\ B_{\mathrm{Sy}}\\ B_{\mathrm{Sz}}\end{array}\right]=\left[\begin{array}{ccc}{K}_{\mathrm{xx}}& K_{\mathrm{yx}}& K_{\mathrm{zx}}\\ K_{\mathrm{xy}}& K_{\mathrm{yy}}& K_{\mathrm{zy}}\\ K_{\mathrm{xz}}& K_{\mathrm{yz}}& K_{\mathrm{zz}}\end{array}\right]\left[\begin{array}{c}{B}_x\\ B_y\\ B_z\end{array}\right]=K\left[\begin{array}{c}{B}_x\\ B_y\\ B_z\end{array}\right]$

其中，B_x、B_y、B_z为地磁场在载体坐标系下的三分量，B_Sx、B_Sy、B_Sz为载体软铁磁场在载体坐标系下的三分量，矩阵K为载体感应磁场系数矩阵，其元素与载体的姿态有关，对不同航向角ψ和俯仰角γ下的软铁磁场分布数据的一部分进行拟合，建立线性转换矩阵与载体航向角和俯仰角间的函数关系。

3.根据权利要求1所述的一种捷联于水下潜器的三轴磁场传感器误差补偿方法，其特征在于：

所述的信赖域法包括：

4.1设定初始点X₀，初始信赖域半径r₀以及参数0＜μ＜η＜1，ε＞0；

4.2计算若则X^*＝X_k，迭代终止，否则转4.3；

4.3求解 $\min h\left(d_k\right)=\frac{1}{2}{d}_k^T\frac{{\∂}^{2}g{\left(X_k\right)}^T}{{\∂X}^2}{d}_k+\frac{\∂g{\left(X_k\right)}^T}{\∂X}{d}_k,$ 约束条件为||d_k||≤r_k，其最优解为d^*，令若ρ≤μ，则X_k+1＝X_k，反之X_k+1＝X_k+d^*；

4.4确定r_k+1，若ρ≤μ则r_k+1＝0.5r_k，若μ＜ρ＜η则r_k+1＝r_k，否则r_k+1＝2r_k；

4.5令k＝k+1，返回4.4。