CN103630137B - 一种用于导航系统的姿态及航向角的校正方法 - Google Patents

Abstract

本发明公布了一种用于导航系统的姿态及航向角的校正方法，包括提出了基于改进扩展卡尔曼滤波的椭球拟合方法进行磁强计误差补偿、姿态角的误差二级建模和校正以及实现整个系统误差校正的有效方法。本发明用于由惯性测量单元(IMU)和磁强计等组成的惯性组合导航与定位系统中的姿态及航向角的误差校正。磁场补偿方法从二维椭圆拟合拓展到三维椭球体拟合，利用新的椭球模型和改进扩展卡尔曼滤波方法进行椭球拟合，该方法可以有效地实现载体动态实时地自身三维磁场干扰的补偿，提高地磁场的测量精度，从而提高载体航向角的精度；对导航系统输出的姿态角信息进行误差二级建模，然后对其补偿以实时提高姿态角的精度。

Description

一种用于导航系统的姿态及航向角的校正方法

技术领域

本发明涉及一种导航校正方法，特别涉及一种用于导航系统的姿态及航向角的校正方法。

背景技术

在导航系统应用中，导航系统能否提供载体的准确姿态及航向角信息至关重要。对于仅仅使用加速度计和陀螺仪组成惯性测量单元(IMU)，由于加速度计和陀螺仪自身的误差及漂移使得IMU输出的姿态和航向参数精度不能满足一些导航系统的要求。由惯性测量单元(IMU)和磁强计组成的姿态及航向测量系统，可以提高姿态角及航向角的精度。

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

目前，有关对导航系统载体姿态角的误差建模方法已经很普遍，也很成熟，可以达到的一定的高精度要求。但是，对惯性测量器件直接建立模型处于一级建模阶段，如果对误差一级模型得到的姿态角数据再进行二级建模这样可以大大提高系统的精度。

载体自身干扰磁场的实时补偿是提高磁传感器测量精度的关键技术。

发明内容

发明目的：针对上述现有技术，本发明根据椭圆假设磁补偿方法的思路，一种用于导航系统的姿态及航向角的校正方法，克服由于载体自身干扰磁场的影响，提高导航精度。

技术方案：一种用于导航系统的姿态及航向角的校正方法，载体航向角的校正包括下列步骤：

步骤(1)，三轴磁强计误差建模：

选取导航坐标系为东北天(ENU)、载体坐标系的Y轴为载体的前进方向，X轴为与载体前进方向垂直向右，Z轴与X轴、Y轴垂直向上满足右手定则；将磁强计的三轴测量值投影到二维水平面的X、Y轴，分别为H_X、H_Y；三轴磁强计的误差模型为：

H_m＝C_isC_n(C_sH_e+b_n)+b₀(1)

式中：C_is为灵敏度误差矩阵；C_n为非正交误差矩阵；C_s为软磁误差矩阵；b_n为硬磁误差矢量；b₀为偏置误差矢量；H_e为地磁场强度；H_m为磁强计测量值；

将所述三轴磁强计的误差模型改写为：

H_m＝C_aH_e+b(2)

式中：C_a是总误差矩阵；b为总偏置矢量；

由式(2)可得地磁场矢量H_e为：

H_e＝C_a ^-1(H_m-b)(3)；

步骤(2)，改进的地球磁场的椭球模型建模：

地球磁场的椭球模型为：

AX²+BY²+CZ²+2EXY+2FXZ+2GYZ+2QX+2SY+2TZ+D＝0(4)

式中，A、B、C、E、F、G、Q、S、T、D是椭球模型中的各项参数；X、Y、Z分别是磁强计的三轴X、Y、Z的测量值；

对所述椭球模型进行改进，得到如下椭球模型：

a(x+y+p)²+b(x+z+q)²+c(y+z+r)²＝0(5)

式中，(x，y，z)是磁强计的三轴测量值；a、b、c、p、q、r为改进椭球模型的各项参数；

其中，式(5)与式(4)中参数的对应关系为：

A＝a+b

B＝a+c

C＝b+c

E＝a

F＝b

G＝c

Q＝ap+bq

S＝ap+cr

T＝bq+cr

D＝ap²+bq²+cr²；

步骤(3)，利用改进扩展卡尔曼滤波器求解椭球模型的参数：

改进的扩展卡尔曼滤波器，直接把状态的更新值作为观测值进行下一步更新；利用改进扩展卡尔曼滤波器拟合椭球时，状态向量是所述式(5)中椭球参数估计值与真实值的偏差，利用改进扩展卡尔曼滤波对椭球参数的偏差进行估计，得出椭球模型的参数；

步骤(4)，由所述椭球模型参数求得所述式(3)中磁强计误差模型参数；

步骤(5)，根据所述式(2)的三轴磁强计误差模型，对三轴磁强计输出磁通量补偿；

步骤(6)，根据补偿后的三轴磁强计输出以及姿态角，计算得到校正后的载体航向角。

作为本发明的改进，在载体航向角校正之前，还包括姿态角校正步骤，包括如下具体步骤：

步骤(1)，对导航系统的经过一级误差补偿后的姿态角进行误差二级建模：

建立俯仰角误差模型：y_P＝m_Px_P+n_P，其中，y_P表示俯仰角误差值，所述俯仰角误差值为俯仰角与转台角度差值，x_P表示测量值，m_P、n_P是误差模型的系数；

建立横滚角误差模型：y_R＝m_Qx_R+n_Q，其中，y_R表示横滚角误差值，所述横滚角误差值为横滚角与转台角度差值，x_R表示测量值，m_Q、n_Q是误差模型的系数；

步骤(2)，导航系统三轴转台进行试验，上位机采集数据：

首先，三轴转台初始水平校正；然后，保持导航系统横滚角不变，调整转台角度，上位机采集经过一级误差补偿后的导航系统俯仰角测量值，并记录所述俯仰角测量值与转台角度差值；

将三轴转台恢复初始水平设置，保持导航系统俯仰角不变，调整转台角度，上位机采集经过一级误差补偿后的导航系统横滚角测量值，并记录所述横滚角与转台角度差值；

步骤(3)，根据俯仰角误差模型，利用最佳平方逼近对所述采集的俯仰角与转台角度差值进行计算后得到所述误差系数m_P、n_P的值，从而得到俯仰角误差模型；

根据横滚角误差模型，利用最小二乘法对所述采集的横滚角与转台角度差值进行计算后得到所述误差系数m_Q、n_Q的值，从而得到横滚角误差模型；

步骤(4)，利用所述俯仰角误差模型对经过一级误差补偿后的导航系统俯仰角进行补偿，得到补偿后的俯仰角P为：P＝y_P-x_P；利用所述横滚角误差模型对经过一级误差补偿后的导航系统横滚角进行补偿，得到补偿后的横滚角R为：R＝y_R-x_R。

有益效果：为了提高导航系统的精度，本发明通过建立姿态角的输出误差二级模型用以校正姿态角，并且在航向角的计算原理上寻找提高精度的方法，从而提出了新的椭球模型和基于改进扩展卡尔曼滤波的椭球拟合方法进行磁场补偿。该校正方法可以有效的提高导航系统的精度，进而实现高精度定位和导航，并取得良好效果。

利用三轴磁强计计算载体的航向角的主要原理是，由磁强计测量出的地磁场强度值，然后进行相应的计算，最后得到航向角。对于如图2所示的二维系统，定义载体前进方向与磁北的夹角为地磁航向角β，其与地理北极的夹角为地理航向角由图示可知：γ为磁偏角。已知磁偏角γ，求出地磁航向角β即可求得载体的地理航向角地磁航向角的计算公式其中H_X、H_Y分别为磁强计三轴输出值投影到二维水平面上X、Y轴的值。H_X、H_Y与三轴磁强计的三轴输出的地磁场强度值有关。

由此可知，对于三维系统，求航向角的关键在于求解H_X、H_Y。H_X、H_Y的精度决定着载体航向角的精度，又H_X、H_Y与三轴磁强计的三轴输出的地磁场强度值有关，因此，提高载体航向角精度的关键是提高三轴磁强计的输出磁场强度值的精度。然而载体在运动过程中，由于自身材料磁化和切割磁感线等因素，进而引起了诸多干扰磁场的产生，严重影响了磁强计的测量，因此，要提高输出磁场强度值的精度就是对其输出的磁场强度值进行磁场补偿。本发明包括采用新的椭球模型和基于改进扩展卡尔曼滤波椭球拟合的方法进行磁强计的误差校正方法。有效的将磁场补偿方法从二维椭圆拟合拓展到三维椭球体拟合。进行误差补偿需要求解式(4)中的各项系数，若直接利用改进的扩展卡尔曼滤波方法进行椭球拟合，即求解各项系数。由于未知系数有10个，如直接作为状态估计量则数据处理复杂，耗时长。从式(5)中，可知，需要求解的变量只有6个，这样大大减少了计算的复杂度，缩短了计算时间，从而提高了系统的实时性。

把改进扩展卡尔曼滤波用在椭球拟合的时候状态向量是椭球参数估计值与真实值的偏差，即利用改进扩展卡尔曼滤波对椭球参数的偏差进行估计。由此可以利用改进扩展卡尔曼滤波器估计出椭球模型的各个参数，进而通过相关算法将求得的椭球模型转化为式(3)，即H_e＝C^-1(H_m-b)，H_e即为补偿后的磁场分量，最后利用其进行计算求解。该扩展卡尔曼滤波器，是改进的扩展卡尔曼滤波器，没有状态转移矩阵，直接把状态的更新值作为观测值进行下一步更新。

为提高导航系统的姿态角的精度，需要对惯性测量器件(加速度计和陀螺仪)的输出误差进行校正，但是为了进一步提高精度，本发明公布了对导航系统的姿态角建立误差二级模型的方法以及模型。可以对由校正后的惯性器件输出的数据计算得出的姿态角进一步建立误差模型，即误差二级模型。通过实验与理论值直接对比，得到误差值与测量值的关系，从而建立误差二级校正模型。

在计算导航系统的姿态及航向角信息时，由于姿态角和航向角之间计算存在一定的耦合关系，因此需要适当的校正流程从而实现在保证航向角不变的条件下，改变姿态角而不影响航向角。

综上所述，本发明方法改进了地磁场椭球模型，大大缩减了基于改进扩展卡尔曼滤波器的椭球拟合进行磁场补偿方法的复杂度，提高了导航系统的航时性、可靠性和精度；将磁场补偿方法从二维椭圆拟合拓展到三维椭球体拟合，克服了载体二维平面运动姿态解算的限制；建立导航系统载体姿态角的误差二级模型，在不增加系统复杂度的条件下，大大提高载体姿态角的精度；因此，本发明的方法应用性强、实时性强、可靠强、成本低、精度高、计算过程处理简洁同时占用内存空间小，适应于同类导航系统的姿态及航向角的计算。

附图说明

图1为导航系统姿态及航向角校正示意图；

图2为二维平面导航系统载体航向角计算示意图；

图3为三维空间导航系统载体航向角计算示意图；

图4为导航系统姿态角校正示意图；

图5为导航系统姿态角及航向角校正流程图。

具体实施方式

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

一种用于导航系统的姿态及航向角的校正方法，图2为二维平面导航系统载体航向角计算示意图，根据图2所示航向角计算原理，将二维平面扩展到三维空间，如图3所示。图3为三维空间导航系统载体航向角计算示意图，将三维空间投影到二维平面进而计算出导航系统载体的航向角。选取导航坐标系为东北天(ENU)、载体坐标系的Y轴为载体的前进方向，X轴为与载体前进方向垂直向右，Z轴与X、Y轴垂直向上满足右手定则。

在载体航向角校正之前，首先对姿态角进行校正，校正流程图如图4所示，具体步骤如下：

步骤(1)，对导航系统的经过一级误差补偿后的姿态角进行误差二级建模：

基于上述坐标系可以确定载体姿态角和航向角参数计算公式：

俯仰角P： $P=-{\tan }^{-1}\frac{A_y}{g}$

横滚角R： $R={\tan }^{-1}\frac{A_x}{g}$

航向角H： $H={\tan }^{-1}\frac{H_X}{H_Y}$

式中，A_x、A_y分别为加速度计X、Y轴的输出值；g为当地重力加速度值；H_X、H_Y分别为磁强计三轴输出值投影到二维水平面上X、Y轴的值；

$g=\sqrt{A_x^2+A_y^2+A_z^2}$

H_X＝XⁱcosR+Yⁱ*sinR*sinP+Zⁱ*cosP*sinR

H_Y＝Yⁱ*cosP+Zⁱ*sinP

式中，A_z为加速度计Z轴的输出值，Aⁱ、Yⁱ、Zⁱ分别是磁强计的三轴X、Y、Z的输出磁场强度值进行补偿校正之后得到的值；

建立俯仰角误差模型：y_P＝m_Px_P+n_P，其中，y_P表示俯仰角误差值，所述俯仰角误差值为俯仰角与转台角度差值，x_P表示测量值，m_P、n_P是误差模型的系数；

建立横滚角误差模型：y_R＝m_Qx_R+n_Q，其中，y_R表示横滚角误差值，所述横滚角误差值为横滚角与转台角度差值，x_R表示测量值，m_Q、n_Q是误差模型的系数；

步骤(2)，导航系统三轴转台进行试验，上位机采集数据：

首先，三轴转台初始水平校正；然后，保持导航系统横滚角不变，调整转台角度，上位机采集经过一级误差补偿后的导航系统俯仰角测量值，并记录所述俯仰角测量值与转台角度差值；

将三轴转台恢复初始水平设置，保持导航系统俯仰角不变，调整转台角度，上位机采集经过一级误差补偿后的导航系统横滚角测量值，并记录所述横滚角与转台角度差值；

在本实施例中，利用三轴转台分别对俯仰角和横滚角进行校正。调整转台的角度，在±90°范围内按一定顺序，每隔5°对采集经过一级误差补偿后的系统输出的俯仰角，记录每一组数据系统输出俯仰角以及误差值(系统输出俯仰角与转台角度差值，记为误差值)，得到n组数据；以类似俯仰角校正实验步骤，得到横滚角校正的m组数据；

步骤(3)，根据俯仰角误差模型，利用最小二乘法对所述采集的俯仰角与转台角度差值进行计算后得到所述误差系数m_P、n_P的值：

$m_P=\frac{\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{X}_i{Y}_i-\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{X}_i\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{Y}_i}{\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{X}_i^2-{\left(\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{X}_i\right)}^2},n_P=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{Y}_i-m_P\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{X}_i$

从而得到俯仰角误差模型；

根据横滚角误差模型，以同样的方法，利用最小二乘法对所述采集的横滚角与转台角度差值进行计算后得到所述误差系数m_Q、n_Q的值，从而得到横滚角误差模型；

步骤(4)，利用所述俯仰角误差模型对经过一级误差补偿后的导航系统俯仰角进行补偿，得到补偿后的俯仰角P为：P＝y_P-x_P；利用所述横滚角误差模型对经过一级误差补偿后的导航系统横滚角进行补偿，得到补偿后的横滚角R为：R＝y_R-x_R。由于俯仰角与横滚角之间不存耦合关系，所以校正不分先后。在校正完姿态角之后，再进行航向角计算和校正。这样可以避免姿态角和航向角耦合的干扰。

进而对载体航向角的校正，具体步骤如下：

步骤(5)，三轴磁强计误差建模：

三轴磁强计的误差模型为：

H_m＝C_isC_n(C_sH_e+b_n)+b₀(1)

式中：C_is为灵敏度误差矩阵；C_n为非正交误差矩阵；C_s为软磁误差矩阵；b_n为硬磁误差矢量；b₀为偏置误差矢量：H_e为地磁场强度；H_m为磁强计测量值；

将所述三轴磁强计的误差模型改写为：

H_m＝C_aH_e+b(2)

式中：C_a是总误差矩阵；b为总偏置矢量；

由式(2)可得地磁场矢量H_e为：

H_e＝C_a ^-1(H_m-b)(3)；

步骤(2)，改进的地球磁场的椭球模型建模：

地球磁场的椭球模型为：

AX²+BY²+CZ²+2EXY+2FXZ+2GYZ+2QX+2SY+2TZ+D＝0(4)

式中，A、B、C、E、F、G、Q、S、T、D是椭球模型中的各项参数；X、Y、Z分别是磁强计的三轴X、Y、Z的测量值；

载体磁场的干扰补偿就转化为利用三轴磁强计测量数据拟合轨迹椭球方程，通过椭球方程参数求取载体磁场补偿参数C、b，进而利用补偿参数对载体的干扰磁场进行补偿得到准确的地磁场矢量信息。

进行误差补偿就需要求解式(4)中的各项系数，若直接利用改进的扩展卡尔曼滤波方法进行椭球拟合，即求解各项系数。由于未知系数有10个，如直接作为状态估计量则数据处理复杂，耗时长。

对所述椭球模型进行改进，得到如下椭球模型：

a(x+y+p)²+b(x+z+q)²+c(y+z+r)²＝0(5)

式中，(x，y，z)是磁强计的三轴测量值；a、b、c、p、q、r为改进椭球模型的各项参数；

其中，式(5)与式(4)中参数的对应关系为：

A＝a+b

B＝a+c

C＝b+c

E＝a

F＝b

G＝c

Q＝ap+bq

S＝ap+cr

T＝bq+cr

D＝ap²+bq²+cr²；

步骤(7)，利用改进扩展卡尔曼滤波器求解椭球模型的参数：

改进的扩展卡尔曼滤波器，直接把状态的更新值作为观测值进行下一步更新；利用改进扩展卡尔曼滤波器拟合椭球时，状态向量是所述式(5)中椭球参数估计值与真实值的偏差，利用改进扩展卡尔曼滤波对椭球参数的偏差进行估计，得出椭球模型的参数；

从式(5)中可知，需要求解的变量只有6个，这样大大减少了计算的复杂度，缩短了计算时间，从而提高了系统的实时性。

将所求6个系数作为状态估计量，即X＝(a，b，c，p，q，r)^T；将磁强计的三轴测量值作为观测量，即Y＝(x，y，z)^T，也就是给出的待拟合数据点的坐标值。

记观测值与真实值也就是椭球上相应点的偏差为ε，则

ε为高斯白噪声，期望值其协方差矩Cov[ε]＝R＝∑²I。X的状态估计量和Y的观测量R表示协方差，I为单位矩阵。

设F(X，Y)＝a(x+y+p)²+b(x+z+q)²+c(y+z+r)²，取F(X，Y)＝0为观测方程。对于每一个点(磁强计的三轴测量值)来说，观测方程F(X，Y_i)=0，Y_i=(x_i，y_i，z_i)^T，其中，x_i，y_i，z_i分表示磁强计三轴每次输出磁场强度有值。

由于观测方程是非线性方程，则需将其线性化，在处对观测方程F(X，Y)=0左边作一阶泰勒公式展开，可得

$F(X,Y)=F(\hat{X},\hat{Y})+\left[{\∂}_{X}F(\hat{X},\hat{Y})\right](X-\hat{X})+\left[{\∂}_{Y}F(\hat{X},\hat{Y})\right](Y-\hat{Y})=0$

即， $-F(\hat{X},\hat{Y})=\left[{\∂}_{X}F(\hat{X},\hat{Y})\right](X-\hat{X})+\left[{\∂}_{Y}F(\hat{X},\hat{Y})\right](Y-\hat{Y})$

令X′表示X的偏差，以X′为状态估计量，则相应的观测方程可化为：

Z_i=H_iX′_i+V_i

式中，

$Z_i=-F({\hat{X}}_i,{\hat{Y}}_i)=-{(a{(x+y+p)}^2+b{(x+z+q)}^2+c{(y+z+r)}^2)}_i$

$\begin{array}{c}{H}_i={\∂}_{X}F({\hat{X}}_i,{\hat{Y}}_i)\\ ={\left[{(x+y+p)}^2{(x+z+q)}^2{(y+z+r)}^{2}2a(x+y+p)2b(x+z+q)2c(y+z+r)\right]}_i\end{array}$

$V_i=\left[{\∂}_{Y}F({\hat{X}}_i,{\hat{Y}}_i)\right](Y-{\hat{Y}}_i)$

$\begin{array}{c}{\∂}_{Y}F({\hat{X}}_i,{\hat{Y}}_i)=\\ {[2a(x+y+p)+2c(x+z+q)2a(x+y+p)+2b(x+z+r)2b(x+z+q)+2c(y+z+r)]}_i^T\\ E\left[V_i\right]=\left[{\∂}_{Y}F({\hat{X}}_i,{\hat{Y}}_i)\right](Y-{\hat{Y}}_i)=0\end{array}$

$\begin{array}{c}\mathrm{Var}\left[V_i\right]=\left[{\∂}_{Y}F({\hat{X}}_i,{\hat{Y}}_i)\right]R_i{\left[{\∂}_{Y}F({\hat{X}}_i,{\hat{Y}}_i)\right]}^T=\\ 4{({(a(x+y+p)+c(x+z+q))}^2+{(a(x+y+p)+b(x+z+r))}^2+{(b(x+z+q)+c(y+z+r))}^2)}_i{\mathrm{\Σ}}^{2}I\\ ={\mathrm{\σ}}^2\end{array}$

其中，表示X的第i次的估计值，为磁强计三轴输出的磁场强度值；

该扩展卡尔曼滤波器，是改进的扩展卡尔曼滤波器，没有状态转移矩阵，直接把状态的更新值作为观测值进行下一步更新，即X′_i=X′_i-1。

增益矩阵、状态向量更新方程、状态向量协方差矩阵以及状态向量协方差矩阵更新方程公式与标准卡尔曼滤波器相应公式类似，分别为：

状态一步预测：X′_i，i-1＝X′_i-1。

状态估计：X′_i＝X′_i，i-1+K_i(Z_i-H_iX′_i，i-1)

滤波器增益矩阵： $K_i=P_{i,i-1}{H}_i^T{(H_i{P}_{i,i-1}{H}_i^T+R_i)}^{-1}$ 或 $K_i=P_i{H}_i^T{R}_i^{-1}$

一步预测误差方差矩阵：P_i，i-1＝P_i-1

估计方差矩阵： $P_i=[I-K_i{H}_i]P_{i,i-1}{[I-K_i{H}_i]}^T+K_i{R}_i{K}_i^T$ 或P_i＝[I-K_iH_i]P_i，i-1

通过以上五步计算迭代，即可得到X′；

其中把改进扩展卡尔曼滤波用在椭球拟合的时候状态向量是椭球参数估计值与真实值的偏差，即利用改进扩展卡尔曼滤波对椭球参数的偏差进行估计。由此可以利用改进扩展卡尔曼滤波器估计出椭球模型的各个参数。

步骤(8)，由所述椭球模型参数求得所述式(3)，即H_e＝C^-1(H_m-b)中磁强计误差模型参数，H_e即为补偿后的磁场分量；

步骤(9)，根据所述式(2)的三轴磁强计误差模型，对三轴磁强计输出磁通量补偿；

步骤(10)，根据补偿后的三轴磁强计输出以及姿态角，计算得到校正后的载体航向角，整个系统的校正流程图，如图5所示。其中：

航向角H： $H={\tan }^{-1}\frac{H_X}{H_Y}$

H_X＝XⁱcosR+Yⁱ*sinR*sinP+Zⁱ*cosP*sinR

H_Y＝Yⁱ*cosP+Zⁱ*sinP

式中，Xⁱ、Yⁱ、Zⁱ分别是磁强计的三轴X、Y、Z的输出磁场强度值进行补偿校正之后得到的值。

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

Claims (2)

1.一种用于导航系统的姿态及航向角的校正方法，其特征在于，载体航向角的校正包括下列步骤：

步骤(1)，三轴磁强计误差建模：

选取导航坐标系为东北天(ENU)、载体坐标系的Y轴为载体的前进方向，X轴为与载体前进方向垂直向右，Z轴与X轴、Y轴垂直向上满足右手定则；将磁强计的三轴测量值投影到二维水平面的X、Y轴，分别为H_X、H_Y；三轴磁强计的误差模型为：

H_m＝C_isC_n(C_sH_e+b_n)+b₀(1)

式中：C_is为灵敏度误差矩阵；C_n为非正交误差矩阵；C_s为软磁误差矩阵；b_n为硬磁误差矢量；b₀为偏置误差矢量；H_e为地磁场强度；H_m为磁强计测量值；

将所述三轴磁强计的误差模型改写为：

H_m＝C_aH_e+b(2)

式中：C_a是总误差矩阵；b为总偏置矢量；

由式(2)可得地磁场矢量H_e为：

H_e＝C_a ^-1(H_m-b)(3)；

步骤(2)，改进的地球磁场的椭球模型建模：

地球磁场的椭球模型为：

AX²+BY²+CZ²+2EXY+2FXZ+2GYZ+2QX+2SY+2TZ+D＝0(4)

式中，A、B、C、E、F、G、Q、S、T、D是椭球模型中的各项参数；X、Y、Z分别是磁强计的三轴X、Y、Z的测量值；

对所述椭球模型进行改进，得到如下椭球模型：

a(x+y+p)²+b(x+z+q)²+c(y+z+r)²＝0(5)

式中，(x，y，z)是磁强计的三轴测量值；a、b、c、p、q、r为改进椭球模型的各项参数；

其中，式(5)与式(4)中参数的对应关系为：

Ａ＝a+b

B＝a+c

C＝b+c

E＝a

F＝b

G＝c

Q＝ap+bq

S＝ap+cr

T＝bq+cr

D＝ap²+bq²+cr²；

步骤(3)，利用改进扩展卡尔曼滤波器求解椭球模型的参数：

改进的扩展卡尔曼滤波器，直接把状态的更新值作为观测值进行下一步更新；利用改进扩展卡尔曼滤波器拟合椭球时，状态向量是所述式(5)中椭球参数估计值与真实值的偏差，利用改进扩展卡尔曼滤波对椭球参数的偏差进行估计，得出椭球模型的参数；

步骤(4)，由所述椭球模型参数求得所述式(3)中磁强计误差模型参数；

步骤(5)，根据所述式(2)的三轴磁强计误差模型，对三轴磁强计输出磁通量补偿；

步骤(6)，根据补偿后的三轴磁强计输出以及姿态角，计算得到校正后的载体航向角。

2.根据权利要求1所述的一种用于导航系统的姿态及航向角的校正方法，其特征在于，在载体航向角校正之前，还包括姿态角校正步骤，包括如下具体步骤：

步骤(1)，对导航系统的经过一级误差补偿后的姿态角进行误差二级建模：

建立俯仰角误差模型：y_P＝m_Px_P+n_P，其中，y_P表示俯仰角误差值，所述俯仰角误差值为俯仰角与转台角度差值，x_P表示测量值，m_P、n_P是误差模型的系数；

建立横滚角误差模型：y_R＝m_Qx_R+n_Q，其中，y_R表示横滚角误差值，所述横滚角误差值为横滚角与转台角度差值，x_R表示测量值，m_Q、n_Q是误差模型的系数；

步骤(2)，导航系统三轴转台进行试验，上位机采集数据：

首先，三轴转台初始水平校正；然后，保持导航系统横滚角不变，调整转台角度，上位机采集经过一级误差补偿后的导航系统俯仰角测量值，并记录所述俯仰角测量值与转台角度差值；

将三轴转台恢复初始水平设置，保持导航系统俯仰角不变，调整转台角度，上位机采集经过一级误差补偿后的导航系统横滚角测量值，并记录所述横滚角与转台角度差值；

步骤(3)，根据俯仰角误差模型，利用最佳平方逼近对所述采集的俯仰角与转台角度差值进行计算后得到所述误差系数m_P、n_P的值，从而得到俯仰角误差模型；

根据横滚角误差模型，利用最小二乘法对所述采集的横滚角与转台角度差值进行计算后得到所述误差系数m_Q、n_Q的值，从而得到横滚角误差模型；

步骤(4)，利用所述俯仰角误差模型对经过一级误差补偿后的导航系统俯仰角进行补偿，得到补偿后的俯仰角P为：P＝y_P-x_P；利用所述横滚角误差模型对经过一级误差补偿后的导航系统横滚角进行补偿，得到补偿后的横滚角R为：R＝y_R-x_R。