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

摘要

本发明公布了一种用于导航系统的姿态及航向角的校正方法，包括提出了基于改进扩展卡尔曼滤波的椭球拟合方法进行磁强计误差补偿、姿态角的误差二级建模和校正以及实现整个系统误差校正的有效方法。本发明用于由惯性测量单元(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+bB＝a+cC＝b+cE＝aF＝bG＝cQ＝ap+bqS＝ap+crT＝bq+crD＝ap²+bq²+cr²；步骤(3)，利用改进扩展卡尔曼滤波器求解椭球模型的参数：改进的扩展卡尔曼滤波器，直接把状态的更新值作为观测值进行下一步更新；利用改进扩展卡尔曼滤波器拟合椭球时，状态向量是所述式(5)中椭球参数估计值与真实值的偏差，利用改进扩展卡尔曼滤波对椭球参数的偏差进行估计，得出椭球模型的参数；步骤(4)，由所述椭球模型参数求得所述式(3)中磁强计误差模型参数；步骤(5)，根据所述式(2)的三轴磁强计误差模型，对三轴磁强计输出磁通量补偿；步骤(6)，根据补偿后的三轴磁强计输出以及姿态角，计算得到校正后的载体航向角。