CN104406610A - 一种磁力计实时校正装置及方法 - Google Patents

Abstract

本发明公开了一种磁力计实时校正装置及方法，该方法首先分析磁力计误差干扰源特征，然后再分析地磁场分布特征及磁力计测量原理，其次设计一种实时校正装置同时给出相应的校正方法及流程，最终可以实现在系统级对导航系统航向角进行实时校正及补偿。本发明可以在导航系统连续工作的情况下实现实时自动校正，同时避免了现有一些校正及补偿方法(如椭球拟合方法等)的缺点，能够自主地，独立地，全面地实现磁力计误差校正。本发明的优点在于实时对导航系统航向角进行校正、考虑误差更全面，校正装置简单，抗干扰性强，成本低、精度高、计算过程处理简洁。

Description

一种磁力计实时校正装置及方法

技术领域

本发明属于导航定位领域，涉及一种用于磁力计实时校正的装置及方法。

背景技术

随着导航技术的发展，大多数的导航系统逐步向小型化、低成本、低功耗和高精度的方向发展。磁力计被广泛应用于捷联惯性导航系统通过测量地球磁场来提供可靠的、低成本和高精度的导航。同时，由于磁力计的估计误差不积累，因此它可以用来补偿陀螺仪的漂移从而提高姿态角的测量精度。但是，磁力计的输出会受到自身以及周围磁场环境干扰而失真产生误差，如偏置误差、比例因子误差、非正交误差、安装误差、硬磁和软磁误差。因此，在磁力计使用之前必须进行校正和补偿。在导航系统的发展过程中，导航系统一个最重要的功能就是提供载体的准确姿态及航向角信息。对于仅仅使用加速度计和陀螺仪组成惯性测量单元(IMU)，由于加速度计和陀螺仪自身的误差及漂移使得IMU输出的姿态和航向参数精度不能满足一些导航系统的要求。由惯性测量单元(IMU)和磁力计组成的导航系统，可以提高姿态角及航向角的精度。磁力计在国外应用比较广泛，但在磁场补偿方面仍然存在一定问题，致力于寻找一种高精度磁力计实时的校正和补偿方法是磁力计校正及补偿的难点所在。

利用地磁场进行导航定位，在技术上具有无源、无辐射、抗干扰、全天时、全天候、体积小、能耗低的优点，因此在飞机、舰船和潜艇等领域得到广泛应用。导航载体通过磁传感器测量空间的磁场信息，这些磁场信息不仅包括导航定位所用的地磁场信息，也包括载体自身的干扰磁场信息。高精度的地磁导航过程中需要对磁传感器的观测磁场信息进行处理，实时对载体磁场干扰进行补偿，提高地磁导航精度。

目前，有关对导航系统中的磁力计进行校正及已经很普遍，部分方法也很成熟，可以达到的一定的精度要求。但是，对于高精度的要求以及实连续工作时导航，现有一些方法的缺点主要集中在一部分是没有全面考虑误差源种类，另一部分是考虑了误差源种类，但是简化了误差干扰源特征(简化误差源特征矩阵)，即误差干扰源考虑不全面。同时现有的方法需要将磁力计的误差模型转化为椭球模型，估计椭球模型的参数再转化为误差模型，这样处理方法不够简洁。

基于磁力计校正及补偿技术的发展背景，寻求一种计算复杂度低、精度高、实时的校正及补偿技术是一种趋势。本发明设计的校正装置简单，可以在导航系统连续工作的情况下实现实时自动校正，该校正方法可以克服现有一些校正及补偿方法(如椭球拟合方法等)的缺点，能够自主地，独立地，全面地实现磁力计误差校正。

发明内容

技术问题：本发明提供一种可以在导航系统连续工作的情况下实现实时自动校正，同时避免了现有一些校正及补偿方法(如椭球拟合方法等)的缺点，能够自主地，独立地，全面地实现误差校正的用于磁力计实时校正的方法。

技术方案：本发明的用于磁力计实时校正的方法，包括以下步骤：

1)搭建磁力计实时校正装置：将p个磁力计设置在第一同心圆的圆周上、并以所述第一同心圆的圆心中心对称，将q个磁力计设置在第二同心圆的圆周上、并以所述第二同心圆的圆心中心对称，p、q均为大于或等于3的整数，所述的第一同心圆与第二同心圆等直径、同圆心并且相交，所述的两个同心圆中，至少有一个同心圆中的磁力计按照以下条件设置：

存在至少一对磁力计满足：所述的一对磁力计相对应的X、Y、Z轴方向相反，同时其余任一磁力计的坐标轴均要满足：三个坐标轴都不与这两个磁力计的对应坐标轴同向；

2)利用控制器采集校正装置中磁力计三个坐标轴输出数据 $B_{\mathrm{mi}}^b={\left[\begin{array}{ccc}{X}_i& Y_i& Z_i\end{array}\right]}^T,$ 其中i为磁力计编号；

3)根据下式计算估计总偏置矢量b₀：

$b_0=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{B}_{\mathrm{mi}}^b+\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{B}_{\mathrm{mi}}^b,$

其中，b₀为一个向量矩阵，b₀＝[b₁ b₂ b₃]^T，b₁,b₂,b₃分别为b₀的三个坐标分量，n表示第一同心圆中相对应的X、Y、Z轴方向相反的个数，即存在对磁力计相对应的X、Y、Z轴方向相反，m表示第二同心圆中相对应的X、Y、Z轴方向相反的磁力计个数，即存在对磁力计相对应的X、Y、Z轴方向相反；

5)当矩阵A含有的高斯白噪声误差小于0.001高斯时(高斯是地球磁场强度单位)，根据下列方式a)计算估计总变换矩阵M，当矩阵A含有的高斯白噪声误差大于0.001高斯时，根据下列方式b)计算估计总变换矩阵M，其中，矩阵A是一个3×3的矩阵，其形式如下：

$A={\left[\begin{array}{ccc}{X}_1& Y_1& Z_1\\ & .& \\ & .& \\ & .& \\ X_i& Y_i& Z_i\\ & .& \\ & .& \\ & .& \\ X_{p+q}& Y_{p+q}& Z_{p+q}\end{array}\right]}_{(p+q)\×3}$

a)根据估计公式 $M={\left(A^{T}A\right)}^{-1}{A}^T\left[\begin{array}{c}R\\ S\\ T\end{array}\right]$ 计算，其中，上标T、-1分别表示矩阵的转置和矩阵的逆，R,S,T均为1×3的矩阵，矩阵R,S,T的表示形式如下：

$R={\left[\begin{array}{c}{X}_1-b_1\\ .\\ .\\ .\\ X_i-b_i\\ .\\ .\\ .\\ X_{p+q}-b_1\end{array}\right]}_{(p+q)\×1},S={\left[\begin{array}{c}{Y}_1-b_2\\ .\\ .\\ .\\ Y_i-b_2\\ .\\ .\\ .\\ Y_{p+q}-b_2\end{array}\right]}_{(p+q)\×1},T={\left[\begin{array}{c}{Z}_1-b_3\\ .\\ .\\ .\\ Z_i-b_3\\ .\\ .\\ .\\ Z_{p+q}-b_3\end{array}\right]}_{(p+q)\×1}$

b)根据估计公式 $M=\left[\begin{array}{c}{\mathrm{\σ}}_1\\ {\mathrm{\σ}}_2\\ {\mathrm{\σ}}_3\end{array}\right]$ 计算，其中， ${\mathrm{\σ}}_1=-\left[\frac{{\left(V^1\right)}_{4,j}}{{\left(V^1\right)}_{\mathrm{4,4}}}\right],j=\mathrm{1,2,3},$ ${\mathrm{\σ}}_2=-\left[\frac{{\left(V^2\right)}_{4,j}}{{\left(V^2\right)}_{\mathrm{4,4}}}\right],j=\mathrm{1,2,3},{\mathrm{\σ}}_3=-\left[\frac{{\left(V^3\right)}_{4,j}}{{\left(V^3\right)}_{\mathrm{4,4}}}\right],j=\mathrm{1,2,3}$

其中(V^h)_k,j表示矩阵V^h的第k个列向量(V^h)_k的第j个分量，k为矩阵V^h的列向量序号，上标h取值为1,2,3，分别对应表示V¹,V²,V³的上标，所述矩阵V^h按照以下方法得到：

分别对矩阵D＝[A R]，E＝[A S]，F＝[A T]进行奇异值分解，得到：(U¹)^TDV¹＝Σ¹，(U²)^TEV²＝Σ²，(U³)^TFV³＝Σ³，其中，U¹、U²、U³、V¹、V²、V³均为单位正交矩阵，Σ¹、Σ²、Σ³为对角阵；

记矩阵V^h＝[(V^h)₁ (V^h)₂ (V^h)₃ (V^h)₄]，其中((V^h)₁,(V^h)₂,(V^h)₃,(V^h)₄)分别为矩阵V^h的列向量；

5)利用校正公式校正磁力计，其中是校正之前磁力计的输出，M是所述步骤4)中得到的总变换矩阵，b₀是所述步骤3)中得到的总偏置矢量，是校正后的磁力计的三个轴输出矢量。

本发明方法的优选方案中，磁力计总数量为6个，第一同心圆的圆周上和第二同心圆的圆周上分别设置3个磁力计，且每个同心圆上均存在两个磁力计相对应的X、Y、Z轴方向相反。

本发明方法的上述优选方案中，所述的6个磁力计数量分别标号为1,2,3,4,5,6，其中1,2,3号设置在第一同心圆的圆周上，且1,2号磁力计相对应的X、Y、Z轴方向相反，4,5,6号设置在第一同心圆的圆周上，且4,5号磁力计相对应的X、Y、Z轴方向相反，则步骤4)中估计参数b₀计算公式为：

本发明的磁力计实时校正装置，包括p个设置在第一同心圆圆周上、并以所述第一同心圆的圆心中心对称的磁力计，q个磁力计设置在第二同心圆的圆周上、并以所述第二同心圆的圆心中心对称，p、q均为大于或等于3的整数，所述的第一同心圆与第二同心圆等直径、同圆心并且相交，所述的两个同心圆中，至少有一个同心圆中的磁力计按照以下条件设置：

存在至少一对磁力计满足：所述的一对磁力计相对应的X、Y、Z轴方向相反，同时其余任一磁力计的坐标轴均要满足：三个坐标轴都不与这两个磁力计的对应坐标轴同向。

本发明装置的优选方案中，第一同心圆与第二同心圆垂直。

本发明装置的优选方案中，p、q相等。

本发明装置的优选方案中，p、q取值范围为6至12个。

本发明装置的优选方案中，该装置还包括控制器。

本发明装置的上述优选方案中，控制器为数字信号处理器、电子半导体微处理器或意法半导体微处理器单片机。

现有技术无法对磁力计进行校正及补偿达到实时，全面，高精度和强鲁棒性。本发明方法可以实时对导航系统航向角进行校正、考虑误差更全面，校正装置简单，抗干扰性强，成本低、精度高、计算过程处理简洁。

为了实现对导航系统中的磁力计进行实时校正，本发明首先分析磁力计误差干扰源特征，然后再分析地磁场分布特征及磁力计测量原理，其次设计一种实时校正装置同时给出相应的校正方法及流程，最终可以实现在系统级对导航系统航向角进行实时校正及补偿。本发明设计的校正装置简单，可以在导航系统连续工作的情况下实现实时自动校正，该校正方法可以克服现有一些校正及补偿方法(如椭球拟合方法等)的缺点，能够自主地，独立地，全面地实现磁力计误差校正。因此本发明可以有效的实现磁力计误差校正，从而实现导航系统航向角的校正及补偿，进而实现高精度导航和定位，并取得良好效果。

本发明的优点在于实时对导航系统航向角进行校正、考虑误差更全面，校正装置简单，抗干扰性强，成本低、精度高、计算过程处理简洁。因此本发明可以有效的实现磁力计误差校正，从而实现导航系统航向角的校正及补偿，进而实现高精度导航和定位，并取得良好效果。

有益效果：本发明与现有技术相比，具有以下优点：

(1)本发明所提出的方法使用磁力计数量较少，使得本发明成本低，同时装置设置结构合理，使得本发明鲁棒性强、可靠强、精度高，本发明可以提高磁力计输出精度，进而可以提高导航系统航向角精度，从而提高导航及定位能力；

(2)本发明所提出的方法可以在导航系统连续工作的情况下，不要任何外部信息实现实时进行磁力计校正及补偿，进而提高导航系统航向角的精度；

(3)本发明所提出的方法与现有技术相比，不需要把磁力计误差模型转化为椭球模型，再通过估计椭球模型参数，进而得到磁力计误差模型参数，本发明直接对磁力计误差模型处理，因而减少了因模型转化而产生的一些计算误差；

(4)现有方法在求解误差模型参数时，将总误差矩阵M看作为对称矩阵，但是在实际中，总误差矩阵不是对称矩阵。本发明考虑了总误差矩阵一般性，因而应用性广，同时所提出的方法算法处理简洁，使用数据量少，不需要外部任何信息。

附图说明

图1为磁力计校正及补偿流程示意图；

图2为地磁场示意图；

图3为磁力计校正装置示意图。

具体实施方式

下面结合实施例和说明书附图对发明的技术方案进行详细说明。

本发明首先分析磁力计误差干扰源特征，然后再分析地磁场分布特征及磁力计测量原理，其次设计一种实时校正装置同时给出相应的校正方法及流程，最终可以实现在系统级对导航系统航向角进行实时校正及补偿。因此本发明可以有效的实现磁力计误差校正，从而实现导航系统航向角的校正及补偿，进而实现高精度导航和定位，并取得良好效果。

为此，首先对磁力计误差干扰源特征进行分析：根据产生磁力计误差的原因，误差源可以被分成两个主要方面，一类是由于磁力计自身制造生产过程而产生的误差，包括偏置误差，比例因子误差和非正交误差；另一类是由于外部磁场的环境以及安装过程而产生的误差，包含硬铁，软铁误差和安装误差。磁力计的输出通常受到上述六个误差源的干扰。

磁力计的误差模型一般表达形式为：

$B_m^b=C_s{C}_n{C}_m(B^b+B_h^b+B_s^b)+b^b+{\mathrm{\ϵ}}_0---\left(1\right)$

$B_s^b=C_{\mathrm{si}}(B^b+B_h^b)---\left(2\right)$

其中，C_s是比例因子误差矩阵，C_m是安装误差矩阵，C_n是非正交误差矩阵，C_si是软磁误差矩阵，是硬磁误差矢量，是软磁误差矢量，b^b是偏置误差矢量，B^b是当地磁场矢量在载体坐标系中的分量，是磁力计的输出矢量，ε₀是系统噪声矢量。上标“b”表示该矢量是在载体坐标系中进行测量，本文中载体坐标系(b系)采取“右前上”，Y_b沿载体前进方向，X_b沿载体前进方向向右，Z_b垂直载体平面向上。对表达式(1)和(2)进一步化简可得到：

$B_m^b={\mathrm{MB}}^b+b_0+{\mathrm{\ϵ}}_0---\left(3\right)$

其中M＝C_mC_nC_s(I_3×3+C_si)是总变换矩阵，I_3×3是3×3维单位矩阵，是总偏置矢量，忽略高斯白噪声ε₀，得到如下表示式：

$B_m^b={\mathrm{MB}}^b+b_0---\left(3\right)$

利用该表达式进行磁力计校正及补偿，为了考虑干扰误差源的一般性，假设矩阵M为一般矩阵，表达式的参数如下所示：

$\begin{array}{c}{B}_m^b={\left[\begin{array}{ccc}{X}_m& Y_m& Z_m\end{array}\right]}^T\\ B^b={\left[\begin{array}{ccc}X& Y& Z\end{array}\right]}^T\\ b_0={\left[\begin{array}{ccc}{b}_1& b_2& b_3\end{array}\right]}^T\end{array},M=\left[\begin{array}{ccc}{a}_{11}& a_{12}& a_{13}\\ a_{21}& a_{22}& a_{23}\\ a_{31}& a_{32}& a_{33}\end{array}\right]$

上述所有参数未知参数均具有一般性，因此本发明的方法适用性更强。通过以上分析可知，通过估计出参数b₀，则可以利用对磁力计测量的值进行校正得到实际地磁场值B^b。

根据参数估计需求，同时考虑磁力计误差校正的实时性，本发明设计了一种实时校正装置。对地磁场特征及测量原理进行分析：由地磁场分布特征可知地磁场水平分量在磁极点出为0，在除了极地附近外的其他任何地方均指向北；垂直分量在极点附近处最大，在北半球指向下，在南半球向上。地球上任意一点的地磁场可由地磁场要素描述。如图2所示，设观测点在O处，并以O点为原点建立坐标系(选地理坐标系为东北天坐标系，ENU)。图中T表示地磁场强度，地磁场在水平面内的投影为B_H，在水平面内东向分量为B_E，北向分量为B_N，在垂直平面内的投影为B_Z。D、I分别为当地磁偏角和磁倾角，根据地磁场特征可知当地磁偏角和磁倾角D、I可以看作是常量，随着地域的变化而变化，因此在地域一定的时候可以看作是一个恒定的量。同时根据地磁场分布特征可知，相邻两点的地磁场强度和方向可以看作是一致的，那么理想情况下，在这两点放置X、Y、Z轴安装方向一致的磁力计，则两个磁力计的三个轴输出一致，如果两个磁力计X、Y、Z轴安装方向相反，那么三个轴测得地磁场值也相反。

估计参数b₀，通过权利要求1的分析可知，可以利用1，2号两个磁力计进行参数估计(也可以利用4，5号两个磁力计)。由于1，2号两个磁力计的三个轴的方向相反的，那么感应出的地磁场方向相反，即可以用如下表达式形式所示：

1号磁力计： $B_{m1}^b={\mathrm{MB}}^b+b_0---\left(4\right)$

2号磁力计： $B_{m2}^b={-\mathrm{MB}}^b+b_0---\left(5\right)$

其中，分别为去，1，2号磁力计的输出值，由表达式(4)、(5)进行代数计算可以得到：

$b_0=\frac{1}{2}(B_{m1}^b+B_{m2}^b)---\left(6\right)$

同理利用4，5号两个磁力计输出值，可以测量得到：

$b_0=\frac{1}{2}(B_{m4}^b+B_{m5}^b)---\left(7\right)$

其中，分别为去，4，5号磁力计的输出值。可以通过对表达式(6)和(7)求得的值，再做平均值来提高精度。

估计参数M：通过(2)已经求得参数b₀，则参数b₀＝[b₁ b₂ b₃]^T可以看作是已知值。将所有参数代入表达式(3)中，可以得到如下形式：

$\left[\begin{array}{c}{a}_{11}X+a_{12}Y+a_{13}Z\\ a_{21}X+a_{22}Y+a_{23}Z\\ a_{31}X+a_{32}Y+a_{33}Z\end{array}\right]=\left[\begin{array}{c}{X}_m-b_1\\ Y_m-b_2\\ Z_m-b_3\end{array}\right]---\left(8\right)$

将表达式(8)进一步化简写成如下形式：

a₁₁X+a₁₂Y+a₁₃Z＝X_m-b₁           (9)

a₂₁X+a₂₂Y+a₂₃Z＝Y_m-b₂           (10)

a₃₁X+a₃₂Y+a₃₃Z＝Z_m-b₃          (11)

作如下参数假设：

X_m-b₁＝B₁,Y_m-b₂＝B₂,Z_m-b₃＝B₃

则上述表示式中，已知的参数为(X,Y,Z)和(B₁，B₂，B₃)，未知参数为(a₁₁,a₁₂,a₁₃)、(a₂₁,a₂₂,a₂₃)和(a₃₁,a₃₂,a₃₃)，需要估计的参数就是未知参数

根据表达式(9)、(10)和(11)将三组未知参数分开进行估计，具体做法如下所示(以表示式(9)为例)：

a₁₁X+a₁₂Y+a₁₃Z＝B₁               (12)

将6个磁力计的三个轴的输出值分别代入到表达式(12)中，写成举证形式如下：

Aσ₁＝R               (13)

其中 $A={\left[\begin{array}{c}{\left(\begin{array}{ccc}{X}_m& Y_m& Z_m\end{array}\right)}_1\\ .\\ .\\ .\\ {\left(\begin{array}{ccc}{X}_m& Y_m& Z_m\end{array}\right)}_i\\ .\\ .\\ .\\ {\left(\begin{array}{ccc}{X}_m& Y_m& Z_m\end{array}\right)}_6\end{array}\right]}_{6\×3},R={\left[\begin{array}{ccc}{B}_1& ...& B_2\end{array}\right]}_{1\×6}^T,$ σ₁＝[a₁₁ a₁₂ a₁₃]^T

利用直接最小二乘法或总体最小二乘法均可以求解，但是由于矩阵A不是精确地因此选择总体最小二乘法精度更高。

通过求解表达式(13)即可以得到参数(a₁₁,a₁₂,a₁₃)，同理可以求得参数(a₂₁,a₂₂,a₂₃)和(a₃₁,a₃₂,a₃₃)。

(4)两种参数求解方法：

利用直接最小二乘方法求解方法如下：

σ₁＝(A^TA)^-1A^TR          (14)

利用总体最小二乘方法求解方法如下：

假设A的最佳逼近A′为：A′＝A+E，其中，E为A中含有的系统误差干扰，则表达式(13)可以转化为如下形式：

A′σ₁＝(A+E)σ₁＝R                  (15)

记，矩阵D＝[A R]，对矩阵D进行奇异值分解，可得：(U¹)^TDV¹＝Σ¹，其中，U¹、V¹均为单位正交矩阵，Σ¹为对角阵。记矩阵V¹＝[(V¹)₁ (V¹)₂ (V¹)₃ (V¹)₄]，其中((V¹)₁,(V¹)₂,(V¹)₃,(V¹)₄)分别为矩阵V的列向量。则，可以得到表达式(13)的解为：

${\mathrm{\σ}}_1=-\left[\frac{{\left(V^1\right)}_{4,j}}{{\left(V^1\right)}_{\mathrm{4,4}}}\right],j=\mathrm{1,2,3}---\left(16\right)$

通过求解表达式(15)或(16)即可以得到参数(a₁₁,a₁₂,a₁₃)，同理可以求得参数(a₂₁,a₂₂,a₂₃)和(a₃₁,a₃₂,a₃₃)。

上述实施例仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和等同替换，这些对本发明权利要求进行改进和替换后的技术方案，均落入本发明的保护范围。

Claims (9)

1.一种磁力计实时校正方法，其特征在于，该方法包括以下步骤：

1)搭建磁力计实时校正装置：将p个磁力计设置在第一同心圆(C1)的圆周上、并以所述第一同心圆(C1)的圆心中心对称，将q个磁力计设置在第二同心圆(C2)的圆周上、并以所述第二同心圆(C2)的圆心中心对称，p、q均为大于或等于3的整数，所述的第一同心圆(C1)与第二同心圆(C2)等直径、同圆心并且相交，所述的两个同心圆中，至少有一个同心圆中的磁力计按照以下条件设置：

存在至少一对磁力计满足：所述的一对磁力计相对应的X、Y、Z轴方向相反，同时其余任一磁力计的坐标轴均要满足：三个坐标轴都不与这两个磁力计的对应坐标轴同向；

2)利用控制器采集校正装置中磁力计三个坐标轴输出数据 $B_{\mathrm{mi}}^b={\left[\begin{array}{ccc}{X}_i& Y_i& Z_i\end{array}\right]}^T,$ 其中i为磁力计编号；

3)根据下式计算估计总偏置矢量b₀：

$b_0=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{B}_{\mathrm{mi}}^b+\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{B}_{\mathrm{mi}}^b,$

其中，b₀为一个向量矩阵，b₀＝[b₁ b₂ b₃]^T，b₁,b₂,b₃分别为b₀的三个坐标分量，n表示第一同心圆(C1)中相对应的X、Y、Z轴方向相反的个数，即存在对磁力计相对应的X、Y、Z轴方向相反，m表示第二同心圆(C2)中相对应的X、Y、Z轴方向相反的磁力计个数，即存在对磁力计相对应的X、Y、Z轴方向相反；

4)当矩阵A含有的高斯白噪声误差小于0.001高斯时，根据下列方式a)计算估计总变换矩阵M，当矩阵A含有的高斯白噪声误差大于0.001高斯时，根据下列方式b)计算估计总变换矩阵M，其中，矩阵A是一个3×3的矩阵，其形式如下：

$A={\left[\begin{array}{ccc}{X}_1& Y_1& Z_1\\ & \·& \\ & \·& \\ & \·& \\ X_i& Y_i& Z_i\\ & \·& \\ & \·& \\ & \·& \\ X_{p+q}& Y_{p+q}& Z_{p+q}\end{array}\right]}_{(p+q)\×3}$

a)根据估计公式 $M={\left(A^{T}A\right)}^{-1}{A}^T\left[\begin{array}{c}R\\ S\\ T\end{array}\right]$ 计算，其中，上标T、-1分别表示矩阵的转置和矩阵的逆，R,S,T均为1×3的矩阵，矩阵R,S,T的表示形式如下：

$R={\left[\begin{array}{c}{X}_1-b_1\\ \·\\ \·\\ \·\\ X_i-b_1\\ \·\\ \·\\ \·\\ X_{p+q}-b_1\end{array}\right]}_{(p+q)\×1},S={\left[\begin{array}{c}{Y}_1-b_2\\ \·\\ \·\\ \·\\ Y_i-b_2\\ \·\\ \·\\ \·\\ Y_{p+q}-b_2\end{array}\right]}_{(p+q)\×1},T={\left[\begin{array}{c}{Z}_1-b_3\\ \·\\ \·\\ \·\\ Z_i-b_3\\ \·\\ \·\\ \·\\ Z_{p+q}-b_3\end{array}\right]}_{(p+q)\×1}$

b)根据估计公式 $M=\left[\begin{array}{c}{\mathrm{\σ}}_1\\ {\mathrm{\σ}}_2\\ {\mathrm{\σ}}_3\end{array}\right]$ 计算，其中， ${\mathrm{\σ}}_1=-\left[\frac{{\left(V^1\right)}_{4,j}}{{\left(V^1\right)}_{\mathrm{4,4}}}\right],j=\mathrm{1,2,3},$ ${\mathrm{\σ}}_2=-\left[\frac{{\left(V^2\right)}_{4,j}}{{\left(V^2\right)}_{\mathrm{4,4}}}\right],j=\mathrm{1,2,3},{\mathrm{\σ}}_3=-\left[\frac{{\left(V^3\right)}_{4,j}}{{\left(V^3\right)}_{\mathrm{4,4}}}\right]j=\mathrm{1,2,3}$

其中(V^h)_k,j表示矩阵V^h的第k个列向量(V^h)_k的第j个分量，k为矩阵V^h的列向量序号，上标h取值为1,2,3，分别对应表示V¹,V²,V³的上标，所述矩阵V^h按照以下方法得到：

分别对矩阵D＝[A R]，E＝[A S]，F＝[A T]进行奇异值分解，得到：(U¹)^TDV¹＝Σ¹，(U²)^TEV²＝Σ²，(U³)^TFV³＝Σ³，其中，U¹、U²、U³、V¹、V²、V³均为单位正交矩阵，Σ¹、Σ²、Σ³为对角阵；

记矩阵V^h＝[(V^h)₁ (V^h)₂ (V^h)₃ (V^h)₄]，其中((V^h)₁,(V^h)₂,(V^h)₃,(V^h)₄)分别为矩阵V^h的列向量；

5)利用校正公式校正磁力计，其中，是校正之前磁力计的输出，M是所述步骤4)中得到的总变换矩阵，b₀是所述步骤3)中得到的总偏置矢量，是校正后的磁力计的三个轴输出矢量。

2.根据权利要求1所述的一种磁力计实时校正方法，其特征在于，所述磁力计总数量为6个，第一同心圆(C1)的圆周上和第二同心圆(C2)的圆周上分别设置3个磁力计，且每个同心圆上均存在两个磁力计相对应的X、Y、Z轴方向相反。

3.根据权利要求2所述的一种磁力计实时校正方法，其特征在于，所述的6个磁力计数量分别标号为1,2,3,4,5,6，其中1,2,3号设置在第一同心圆(C1)的圆周上，且1,2号磁力计相对应的X、Y、Z轴方向相反，4,5,6号设置在第一同心圆(C2)的圆周上，且4,5号磁力计相对应的X、Y、Z轴方向相反，则步骤4)中估计参数b₀计算公式为： $b_0=\frac{1}{4}(B_{m4}^b+B_{m5}^b+B_{m1}^b+B_{m2}^b).$

4.一种磁力计实时校正装置，其特征在于，该装置包括p个设置在第一同心圆(C1)圆周上、并以所述第一同心圆(C1)的圆心中心对称的磁力计，q个磁力计设置在第二同心圆(C2)的圆周上、并以所述第二同心圆(C2)的圆心中心对称，p、q均为大于或等于3的整数，所述的第一同心圆(C1)与第二同心圆(C2)等直径、同圆心并且相交，所述的两个同心圆中，至少有一个同心圆中的磁力计按照以下条件设置：

存在至少一对磁力计满足：所述的一对磁力计相对应的X、Y、Z轴方向相反，同时其余任一磁力计的坐标轴均要满足：三个坐标轴都不与这两个磁力计的对应坐标轴同向。

5.根据权利要求4所述的一种磁力计实时校正装置，其特征在于，所述的第一同心圆(C1)与第二同心圆(C2)垂直。

6.根据权利要求4所述的一种磁力计实时校正装置，其特征在于，所述p、q相等。

7.根据权利要求4所述的一种磁力计实时校正装置，其特征在于，所述p、q取值范围为6至12个。

8.根据权利要求4、5、6或7所述的一种磁力计实时校正装置，其特征在于，该装置还包括控制器。

9.根据权利要求8所述的一种磁力计实时校正装置，其特征在于，所述控制器为数字信号处理器、电子半导体微处理器或意法半导体微处理器单片机。