CN103954303A - 一种用于磁力计导航系统航向角动态计算及校正方法 - Google Patents

Abstract

本发明提供一种用于磁力计导航系统航向角动态计算及校正方法，该方法可以在外部磁场变化的工作环境中动态计算、校正及补偿导航系统的航向角。首先建立磁力计的动态误差模型，其次通过比较磁力计相邻两时刻三个轴输出的对应关系以及分析地磁场分布特征，获得一组包含有航向角信息的方程，最后通过求解方程组实现动态计算和校正航向角。本发明的优点在于应用性强、实时性强、可靠性强、成本低、精度高、计算过程处理简洁、稳定性强等，能够有效地提高导航系统的航向精度。本发明可应用于由惯性测量单元（IMU）和磁力计（或电子罗盘）等组成的惯性组合导航与定位系统中航向角的误差校正及补偿。

Description

一种用于磁力计导航系统航向角动态计算及校正方法

技术领域

本发明涉及一种导航系统航向角动态计算及校正的方法，特别是涉及一种用于由惯性测量单元(IMU)和磁力计组成的惯性组合导航与定位系统中航向角的误差校正及补偿方法。

背景技术

在导航系统的应用中，导航系统能否提供载体的航向角信息至关重要。对于使用加速度计和陀螺仪组成惯性测量单元(IMU)，并由惯性测量单元(IMU)和磁力计构成的导航系统，能够为载体提供较高精度的航向角和姿态角。目前，磁力计在导航系统中得到了广泛的应用，但其在磁场补偿方面仍然存在一些问题,尤其在外部磁场环境不断发生变化的情况下，磁力计在校正及误差补偿方面存在着很大的困难。

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

目前，对导航系统中的磁力计进行校正及补偿的方式已经很普遍，部分方法也很成熟，可以达到一定的精度要求。但是，大部分方法考虑的仅是导航系统在外部磁场环境不发生变化的情况下，对磁力计的校正及补偿。因此，这些方法存在局限性，不适用于外部磁场实时发生变化的场合。

基于磁力计校正及补偿技术的发展背景，寻求一种计算复杂度低、可实行性强、应用范围广的校正及补偿技术是一种趋势。本发明根据地磁场分布特征以及导航系统的航向角计算原理，提出了一种用于磁力计导航系统航向角动态计算及校正方法，该方法可以有效实现磁力计的校正及补偿，提高导航系统的精度。

发明内容

本发明目的在于提供一种用于磁力计导航系统航向角动态计算及校正方法，实现了针对外部磁场实时变化的环境中可以实时动态地计算、校正及补偿导航系统的航向角，从而扩大了导航系统的应用范围。

为达成上述目的，本发明所采用的技术方案如下：

一种用于磁力计导航系统航向角动态计算及校正方法，包括以下步骤：

步骤1：建立磁力计的动态误差模型，如下：

${\hat{B}}^h={\stackrel{\‾}{M}B}^b+b^b+N+{\mathrm{\ϵ}}_0---\left(1\right)$

其中，为磁力计的三个轴输出矢量，为总误差矩阵，B^b为当地磁场矢量，b^b为零偏置误差矢量，N为外部环境引起的误差矢量，ε₀是测量误差矢量，一般认为是高斯白色噪声，可以忽略。

不考虑高斯白色噪声ε₀的影响，通过一系列的化简变换得到磁力计的误差椭球模型如下：

${\hat{B}}^{\text{b}}={\stackrel{\‾}{M}}^{-1}(({\hat{B}}^h-b^b)+N)---\left(2\right)$

其中，为校正及补偿后的磁力计的三个轴输出矢量，为的逆矩阵；

记： $B^h={\hat{B}}^h-b^b,M={\stackrel{\‾}{M}}^{-1},$ 则上述公式(2)表示为：

${\hat{B}}^b=M(B^h+N)---\left(3\right)$

其中，总误差矩阵零偏置误差矢量b^b是不随外部环境变化而变化，能够通过现有的校正及补偿方法进行一次校正得到，而外部磁场环境变化所引起的误差矢量N，是未知变量。

步骤2：分析相邻两时刻磁力计的三个轴输出对应关系得出包含有导航系统航向角H信息的方程；

相邻两时刻磁力计的三个轴输出对应关系可以表示为C₁X₁＝C₂X₂,其中X₁、X₂分别是磁力计相邻两时刻的三个轴输出矢量，C₁、C₂分别为磁力计相邻两时刻对应的姿态矩阵。

根据地磁场分布特征，将相邻两时刻磁力计在载体坐标系中的三个轴输出矢量转换到导航坐标系中是一致的，则所述的三个轴输出对应关系可表示为：

$C_b^n\left(k\right){\hat{B}}^b\left(k\right)=C_b^n(k+1){\hat{B}}^b(k+1)---\left(4\right)$

其中，分别为载体坐标系(b系)到导航坐标系(n系)方向余弦矩阵(也称为姿态矩阵)在k时刻和k+1时刻的值，分别为校正及补偿后的磁力计三个轴输出矢量在k时刻和k+1时刻的值。均为已知量，k+1时刻导航系统的航向角信息包含在中。

取导航坐标系(n系)为东北天(ENU)，载体坐标系(b系)为OX_bY_bZ_b；其中，OY_b轴沿载体前进方向，OX_b轴垂直载体前进方向向左，OZ_b轴与OX_b、OY_b轴构成右手定则。则载体坐标系(b系)到导航坐标系(n系)方向余弦矩阵为

$C_b^n=\left[\begin{array}{ccc}\sin R\cos H-\sin R\sin P\sin H& -\cos P\sin H& -\sin R\cos H+\sin P\cos R\sin H\\ \cos R\sin H+\sin R\sin P\cos H& \cos P\cos H& \sin R\sin H-\sin P\cos R\cos H\\ -\sin R\cos P& \sin P& \cos R\cos P\end{array}\right]---\left(5\right)$

其中，H、P、R分别为导航系统的航向角、俯仰角、横滚角，并且 式中，A_x、A_y、A_z分别为导航系统中加速度计X、Y、Z轴的输出值。

根据上述表达式(4)、(5)和关系式可以进一步得到：

其中，

T₁₁＝x₁sinR+y₁sinRsinP,T₁₂＝y₁cosR-x₁sinRsinP,T₁₃＝-(sinRcosP)z₁

T₂₁＝y₁cosP,T₂₂＝-(x₁cosP),T₂₃＝(sinP)z₁

T₃₁＝-(x₁sinR+y₁sinPcosR),T₃₂＝x₁sinPcosR+y₁sinR,T₃₃＝(cosRcosP)z₁

式中，(x₁,y₁,z₁)为k时刻磁力计的三个轴输出对应关系，即其中，为载体坐标系(b系)到导航坐标系(n系)方向余弦矩阵在k时刻的值，为校正及补偿后磁力计的三个轴输出矢量在k时刻的值；(x₂,y₂,z₂)根据固定参数和b^b以及磁力计的三轴输出矢量经过计算得到，即MB^h(k+1)＝[x₂y₂z₂]^T；(T₁₁,T₁₂,T₁₃,T₂₁,T₂₂,T₂₃,T₃₁,T₃₂,T₃₃)根据(P,R)和(x₁,y₁,z₁)通过计算得到；因此，对导航系统航向角的动态计算、校正及补偿实质就是求解未知参数(H,n₁,n₂,n₃)。

步骤3：根据相邻两时刻地磁场分布特征，得出动态求解导航系统航向角的附加条件；

考虑相邻两时刻地球地磁场的磁场强度相等，即||B₁||＝||B₂||，其中B₁、B₂均为磁力计三个轴前后两时刻输出的值经过校正及补偿之后得到的磁场矢量，对其取范数即可得到相应的地磁场强度值。则对磁力计的误差表示式(3)进行变换，得：

||M(B^h+N)||_k＝B₀＝||M(B^h+N)||_k+1     (7)

其中，第k时刻的磁场强度值是已知量，记作B₀。

对上述表达式(7)做一系列处理得到

$n_1^2+n_2^2+n_3^2+{2x}_2{n}_1+{2y}_2{n}_2+{2z}_2{n}_3+x_2^2+y_2^2+z_2^2-B_0^2=0---\left(8\right)$

其中，MN(k+1)＝[n₁n₂n₃]^T，MB^h(k+1)＝[x₂y₂z₂]^T；由于(x₂,y₂,z₂,B₀)均为已知量，因此方程(8)中仅含有未知量(n₁,n₂,n₃)。

在外部环境无磁的条件下利用现有的磁力计校正及补偿算法——基于椭球约束的最小二乘的方法进行一次校正及补偿得到固定参数和

步骤4：利用表达式(6)和(8)，以及表达式sin²H+cos²H＝1动态计算、校正及补偿导航系统的航向角。

由以上本发明的技术方案可知，本发明的有益效果在于：1)本发明所提出的方法应用性强、实时性强、可靠强、成本低、精度高、稳定性强，适应于提高导航系统航向角的精度，从而提高导航及定位能力。2)本发明所提出的方法既适应于外部磁场环境不变的条件也适应于外部磁场环境变化的条件，尤其对外部磁场环境变化的条件优势更加明显，因此应用范围更加广泛。3)本发明所利用的数据信息少，只利用了相邻两时刻磁力计三个轴输出的磁场强度矢量以及初始校正后的两个误差矩阵，因而占用的数据存储空间小，进而不影响导航系统工作的实时性。4)本发明在计算导航系统航向角的过程中充分避免不确定因素的影响，例如，避免采用地磁场强度值是一个不变的常量的传统理论假设，而是采用相邻两时刻地磁场强度值相等，这样更加符合实际情况，并在一定程度上提高了导航系统的航向精度。

下面结合附图对本发明作进一步详细描述。

附图说明

图1为导航系统航向角计算、校正及补偿方法的流程图。

具体实施方式

下面结合附图对本发明做更进一步的解释。

如图1所示，磁力计导航系统航向角动态计算及校正方法包括以下步骤：

步骤1：建立磁力计的动态误差模型。

磁力计的误差来源可以分为两类，第一类是磁力计自身存在的仪表误差，第二类是由于受到外部环境的影响而产生的误差。其中，第一类误差可以认为是不随外部环境而发生变化的，可以通过一次校正得到；而当外部环境发生变化时，第二类误差会随之发生变化，因而可以将其设为可变参数。因此磁力计的一般误差模型可以由这两类误差组成，具体如下：

${\hat{B}}^h={\stackrel{\‾}{M}B}^b+b^b+N+{\mathrm{\ϵ}}_0---\left(1\right)$

其中，为磁力计的三个轴输出矢量，为总误差矩阵，B^b为当地磁场矢量，b^b为零偏置误差矢量，N为外部环境引起的误差矢量，ε₀是测量误差矢量，一般认为是高斯白色噪声，可以忽略。

不考虑高斯白色噪声ε₀的影响，通过一系列的化简变换得到磁力计的误差椭球模型，如下：

${\hat{B}}^{\text{b}}={\stackrel{\‾}{M}}^{-1}(({\hat{B}}^h-b^b)+N)---\left(2\right)$

其中，为校正及补偿后的磁力计的三个轴输出矢量，为的逆矩阵。

记： $B^h={\hat{B}}^h-b^b,M={\stackrel{\‾}{M}}^{-1},$ 则上述公式(2)表示为：

${\hat{B}}^b=M(B^h+N)---\left(3\right)$

其中，总误差矩阵零偏置误差矢量b^b是不随外部环境变化而变化，可以通过现有的校正及补偿方法进行一次校正得到。

步骤2：分析相邻两时刻磁力计的三个轴输出对应关系得出包含有导航系统航向角H信息的方程。

相邻两时刻磁力计的三个轴输出对应关系可以表示为C₁X₁＝C₂X₂,其中X₁、X₂分别是磁力计相邻两时刻的三个轴输出矢量，C₁、C₂是分别为磁力计相邻两时刻对应的姿态矩阵。

根据地磁场分布特征，将相邻两时刻磁力计在载体坐标系中的三个轴输出矢量转换到当地地理坐标系中时是一致的。则所述的三个轴输出对应关系可表示为：

$C_b^n\left(k\right){\hat{B}}^b\left(k\right)=C_b^n(k+1){\hat{B}}^b(k+1)---\left(4\right)$

其中，分别为载体坐标系(b系)到导航坐标系(n系)方向余弦矩阵在k时刻和k+1时刻的值，分别为校正及补偿后磁力计的三个轴输出矢量在k时刻和k+1时刻的值，均为已知量，k+1时刻导航系统的航向角信息包含在中。

选取当地地理坐标系东北天(ENU)作为导航坐标系(n系)，取载体坐标系(b系)为OX_bY_bZ_b，其中，OY_b轴沿载体前进方向，OX_b轴垂直载体前进方向向左，OZ_b轴与OX_b、OY_b轴构成右手定则。则载体坐标系(b系)到导航坐标系(n系)的方向余弦矩阵为

$C_b^n=\left[\begin{array}{ccc}\sin R\cos H-\sin R\sin P\sin H& -\cos P\sin H& -\sin R\cos H+\sin P\cos R\sin H\\ \cos R\sin H+\sin R\sin P\cos H& \cos P\cos H& \sin R\sin H-\sin P\cos R\cos H\\ -\sin R\cos P& \sin P& \cos R\cos P\end{array}\right]---\left(5\right)$

其中，H、P、R分别为导航系统的航向角、俯仰角、横滚角，并且 式中，A_x、A_y、A_z分别为导航系统中加速度计X、Y、Z轴的输出值。

根据上述公式(4)可以进一步得到：

$\begin{array}{c}{C}_b^n(k+1){\hat{B}}^b(k+1)=C_b^n(k+1)M(B^h(k+1)+N(k+1))\\ =C_b^n(k+1)M({\hat{B}}^h(k+1)-b^b+N(k+1))\end{array}----\left(6\right)$

由于在b系至n系的等效旋转中各坐标系都保持为直角坐标系，因此为单位正交矩阵。根据单位正交矩阵的性质有则进一步化简上述公式(4)可得到：

$\begin{array}{c}{\hat{B}}^b(k+1)={\left(C_b^n(k+1)\right)}^{-1}\left(C_b^n\left(k\right){\hat{B}}^b\left(k\right)\right)\\ =C_n^b(k+1)\left(C_b^n\left(k\right){\hat{B}}^b\left(k\right)\right)\\ {=\left(C_b^n(k+1)\right)}^T\left(C_b^n\left(k\right){\hat{B}}^b\left(k\right)\right)\end{array}---\left(7\right)$

为了便于进一步化简和计算，在此处对上述表达式(4)、(6)和(7)中的变量做出如下假设，记：MN(k+1)＝[n₁n₂n₃]^T，MB^h(k+1)＝[x₂y₂z₂]^T。则由上述表达式(7)可得

其中，

T₁₁＝x₁sinR+y₁sinRsinP,T₁₂＝y₁cosR-x₁sinRsinP,T₁₃＝-(sinRcosP)z₁

T₂₁＝y₁cosP,T₂₂＝-(x₁cosP),T₂₃＝(sinP)z₁

T₃₁＝-(x₁sinR+y₁sinPcosR),T₃₂＝x₁sinPcosR+y₁sinR,T₃₃＝(cosRcosP)z₁

式中，(P,R)根据导航系统的加速度计信息通过计算得到，(x₁,y₁,z₁)为k时刻磁力计的三个轴输出对应关系，(x₂,y₂,z₂)通过确定固定参数和b^b以及磁力计的三轴输出矢量再经过计算得到，(T₁₁,T₁₂,T₁₃,T₂₁,T₂₂,T₂₃,T₃₁,T₃₂,T₃₃)根据(P,R)和(x₁,y₁,z₁)通过计算得到。因此，实现导航系统航向角的计算、校正及补偿实质就是求解未知参数(H,n₁,n₂,n₃)。

在本发明中，利用无磁转台进行实验，通过转台和数据采集系统来得到磁力计的动态误差模型中的固定参数和b^b。

首先通过夹具将磁力计固定在三轴转台上，然后通过转台控制柜控制转台在三维的各个方向上转动并进行数据采集，从而保证数据的全面性。为了保证数据处理的精度和效率，先对采集得来的数据进行归一化处理，再利用基于椭球约束的直接最小二乘法对归一化处理后的数据进行处理，从而得到动态误差模型中的总误差矩阵和零偏置误差矢量b^b。

根据上述内容，可知求解上述未知参数(H,n₁,n₂,n₃)的条件不足，因此需要添加计算导航系统航向角的附加条件。

步骤3：根据相邻两时刻地磁场分布特征，得出动态求解导航系统航向角的附件条件。

考虑相邻两时刻地球地磁场的磁场强度相等，即||B₁||＝||B₂||，其中B₁、B₂均为磁力计三个轴前后两时刻输出的值经过校正及补偿之后得到的磁场矢量，对其取范数即可到相应的地磁场强度值。则对磁力计的误差表示式(3)进行如下变换

||M(B^h+N)||_k＝||M(B^h+N)||_k+1     (9)

其中，第k时刻的磁场强度值是已知量，记作B₀，则

||M(B^h+N)||_k＝B₀＝||M(B^h+N)||_k+1       (10)

进行计算化简如下：

$B_0={\left|\right|M(B^h+N)\left|\right|}_{k+1}={\left|\right|{\mathrm{MB}}^h+\mathrm{MN}\left|\right|}_{k+1}={||\begin{array}{c}{x}_2+n_1\\ y_2+n_2\\ z_2+n_3\end{array}||}_{k+1}---\left(11\right)$

根据范数定义对上式(11)进行进一步化简，表示成矩阵形式可得到：

${(x_2+n_1)}^2+{(y_2+n_2)}^2+{(z_2+n_3)}^2=B_0^2---\left(12\right)$

对上式(12)进行进一步化简，将平方项展开，表示成一般形式可得到：

$n_1^2+n_2^2+n_3^2+{2x}_2{n}_1+{2y}_2{n}_2+{2z}_2{n}_3+x_2^2+y_2^2+z_2^2-B_0^2=0---\left(13\right)$

其中，MN(k+1)＝[n₁n₂n₃]^T，MB^h(k+1)＝[x₂y₂z₂]^T；由于(x₂,y₂,z₂,B₀)均为已知量，因此方程(13)中仅含有未知量(n₁,n₂,n₃)。

步骤4：通过步骤1，2，3可以得到求解未知参数(H,n₁,n₂,n₃)的条件，即利用方程(8)和(13)，以及表达式sin²H+cos²H＝1得到一组方程组，通过求解方程组实现动态计算、校正以及补偿导航系统的航向角，即求解输出参数(H,n₁,n₂,n₃)。

步骤5：导航系统工作：导航系统在进入外部磁场变化的环境之前便开始工作，以保证起始时刻磁力计校正及补偿后的三个轴输出矢量的准确性。当进入外部磁场不断变化的环境时，开始利用相邻两时刻磁力计的三个轴输出信息以及导航信息，并利用表达式(8)和(13)，以及表达式sin²H+cos²H＝1计算未知参数，从而实现实时动态计算、校正以及补偿导航系统的航向角。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (2)

1.一种用于磁力计导航系统航向角动态计算及校正方法，其特征在于，该方法包括以下步骤：

步骤1：建立磁力计的动态误差模型，如下：

${\hat{B}}^{\text{b}}={\stackrel{\‾}{M}}^{-1}(({\hat{B}}^h-b^b)+N)---\left(1\right)$

其中，为校正及补偿后的磁力计的三个轴输出矢量，为总误差矩阵的逆矩阵，为磁力计的三个轴输出矢量，b^b为零偏置误差矢量，N为外部磁场环境发生变化而引起的误差矢量；

记：则上述公式(1)表示为下述公式(2)：

${\hat{B}}^b=M(B^h+N)---\left(2\right)$

步骤2：分析相邻两时刻磁力计的三个轴输出对应关系得出包含有导航系统航向角H信息的方程，如下：

$\left[\begin{array}{c}(x_1\sin R+y_1\sin R\sin P)\cos H+(y_1\cos R-x_1\sin R\sin P)\sin H-\left(\sin P\cos P\right)z_1\\ \left(y_1\cos P\right)\cos H-\left(x_1\cos P\right)\sin H+\left(\sin P\right)z_1\\ -(x_1\sin R+y_1\sin P\cos R)\cos H+(x_1\sin P\cos R+y_1\sin R)\mathrm{isnH}+\left(\cos R\cos P\right)z_1\end{array}\right]\text{-}\left[\begin{array}{c}{x}_2+n_1\\ y_2+n_2\\ z_2+n_3\end{array}\right]=0---\left(3\right)$

其中，P、R分别为导航系统的俯仰角、横滚角，(P,R)根据导航系统的加速度计信息通过计算得到，即 $P=-{\tan }^{-1}\frac{A_y}{g},R={\tan }^{-1}\frac{A_x}{g},$ 其中 $g=\sqrt{A_x^2{+A}_y^2+A_z^2},$ A_x、A_y、A_z分别为导航系统中加速度计X、Y、Z轴的输出值；(x₁,y₁,z₁)为k时刻磁力计的三个轴输出对应关系，即其中，为载体坐标系到导航坐标系方向余弦矩阵在k时刻的值，为校正及补偿后磁力计的三个轴输出矢量在k时刻的值；(x₂,y₂,z₂)根据固定参数和b^b以及磁力计的三轴输出矢量经过计算得到，即MB^h(k+1)＝[x₂y₂z₂]^T；因此，对导航系统航向角的动态计算、校正及补偿实质就是求解未知参数(H,n₁,n₂,n₃)；

步骤3：根据相邻两时刻地磁场磁场强度相等的分布特征，对磁力计的误差表示式(2)进行变换，得出动态求解导航系统航向角的附加条件：

$n_1^2+n_2^2+n_3^2+{2x}_2{n}_1+{2y}_2{n}_2+{2z}_2{n}_3+x_2^2+y_2^2+z_2^2-B_0^2=0---\left(4\right)$

其中，第k时刻的磁场强度值为已知量，记作B₀，由于(x₂,y₂,z₂,B₀)均为已知量，因此方程(4)中仅含有未知量(n₁,n₂,n₃)；

步骤4：利用表达式(3)和(4)，以及表达式sin²H+cos²H＝1动态计算、校正及补偿导航系统的航向角。

2.根据权利要求1所述的一种用于磁力计导航系统航向角动态计算及校正方法，其特征在于，在所述步骤2中，取导航坐标系为东北天ENU，载体坐标系为OX_bY_bZ_b，其中，OY_b轴沿载体前进方向，OX_b轴垂直载体前进方向向左，OZ_b轴与OX_b、OY_b轴构成右手定则；则载体坐标系到导航坐标系方向余弦矩阵为

$C_b^n=\left[\begin{array}{ccc}\sin R\cos H-\sin R\sin P\sin H& -\cos P\sin H& -\sin R\cos H+\sin P\cos R\sin H\\ \cos R\sin H+\sin R\sin P\cos H& \cos P\cos H& \sin R\sin H-\sin P\cos R\cos H\\ -\sin R\cos P& \sin P& \cos R\cos P\end{array}\right]---\left(5\right)$

其中，H、P、R分别为导航系统的航向角、俯仰角、横滚角。