一种用于导航系统的姿态及航向角的校正方法
【专利摘要】本发明公布了一种用于导航系统的姿态及航向角的校正方法，包括提出了基于改进扩展卡尔曼滤波的椭球拟合方法进行磁强计误差补偿、姿态角的误差二级建模和校正以及实现整个系统误差校正的有效方法。本发明用于由惯性测量单元(IMU)和磁强计等组成的惯性组合导航与定位系统中的姿态及航向角的误差校正。磁场补偿方法从二维椭圆拟合拓展到三维椭球体拟合，利用新的椭球模型和改进扩展卡尔曼滤波方法进行椭球拟合，该方法可以有效地实现载体动态实时地自身三维磁场干扰的补偿，提高地磁场的测量精度，从而提高载体航向角的精度；对导航系统输出的姿态角信息进行误差二级建模，然后对其补偿以实时提高姿态角的精度。
【专利说明】一种用于导航系统的姿态及航向角的校正方法
【技术领域】
[0001]本发明涉及一种导航校正方法，特别涉及一种用于导航系统的姿态及航向角的校正方法。
【背景技术】
[0002]在导航系统应用中，导航系统能否提供载体的准确姿态及航向角信息至关重要。对于仅仅使用加速度计和陀螺仪组成惯性测量单元(mu)，由于加速度计和陀螺仪自身的误差及漂移使得MU输出的姿态和航向参数精度不能满足一些导航系统的要求。由惯性测量单元(IMU)和磁强计组成的姿态及航向测量系统，可以提高姿态角及航向角的精度。
[0003]用地磁场进行导航定位，具有无源、无辐射、抗干扰、全天时、全天候、体积小、能耗低的优点，因此在飞机、舰船和潜艇等领域得到广泛应用。导航载体通过磁传感器测量空间的磁场信息，这些磁场信息不仅包括导航定位所用的地磁场信息，也包括载体自身的干扰磁场信息。高精度的地磁导航过程中需要对磁传感器的观测磁场信息进行处理，实时对载体磁场干扰进行补偿，提高地磁导航精度。
[0004]目前，有关对导航系统载体姿态角的误差建模方法已经很普遍，也很成熟，可以达到的一定的高精度要求。但是，对惯性测量器件直接建立模型处于一级建模阶段，如果对误差一级模型得到的姿态角数据再进行二级建模这样可以大大提高系统的精度。
[0005]载体自身干扰磁场的实时补偿是提高磁传感器测量精度的关键技术。
【发明内容】
[0006]发明目的:针对上述现有技术，本发明根据椭圆假设磁补偿方法的思路，一种用于导航系统的姿态及航向角的校正方法，克服由于载体自身干扰磁场的影响，提高导航精度。
[0007]技术方案:一种用于导航系统的姿态及航向角的校正方法，载体航向角的校正包括下列步骤:
[0008]步骤(I)，三轴磁强计误差建模:
[0009]选取导航坐标系为东北天(ENU)、载体坐标系的Y轴为载体的前进方向，X轴为与载体前进方向垂直向右，Z轴与X轴、Y轴垂直向上满足右手定则；将磁强计的三轴测量值投影到二维水平面的X、Y轴，分别为Hx、Hy ;三轴磁强计的误差模型为:
[0010]Hffl = CisCn (CsHe+bn)+b0 (I)
[0011]式中:Cis为灵敏度误差矩阵；Cn为非正交误差矩阵；CS为软磁误差矩阵；bn为硬磁误差矢量为偏置误差矢量；H6为地磁场强度；Hm为磁强计测量值；
[0012]将所述三轴磁强计的误差模型改写为:
[0013]Hffl = CaHe+b (2)
[0014]式中:Ca是总误差矩阵；b为总偏置矢量；
[0015]由式⑵可得地磁场矢量He为:
[0016]He = C(HnTb) (3)；[0017]步骤(2)，改进的地球磁场的椭球模型建模:
[0018]地球磁场的椭球模型为:
[0019]AX2+BY2+CZ2+2EXY+2FXZ+2GYZ+2QX+2SY+2TZ+D = 0 (4)
[0020]式中，A、B、C、E、F、G、Q、S、T、D是椭球模型中的各项参数；X、Y、Z分别是磁强计的
三轴X、Y、Z的测量值；[0021]对所述椭球模型进行改进，得到如下椭球模型:
[0022]a (x+y+p) 2+b (x+z+q) 2+c (y+z+r)2 = 0 (5)
[0023]式中，(x, y, z)是磁强计的三轴测量值；a、b、c、p、q、r为改进椭球模型的各项参数；
[0024]其中，式(5)与式⑷中参数的对应关系为:
[0025]A = a+b
[0026]B = a+c
[0027]C = b+c
[0028]E = a
[0029]F = b
[0030]G = c
[0031]Q = ap+bq
[0032]S = ap+cr
[0033]T = bq+cr
[0034]D = ap2+bq2+cr2 ；
[0035]步骤(3)，利用改进扩展卡尔曼滤波器求解椭球模型的参数:
[0036]改进的扩展卡尔曼滤波器，直接把状态的更新值作为观测值进行下一步更新；利用改进扩展卡尔曼滤波器拟合椭球时，状态向量是所述式(5)中椭球参数估计值与真实值的偏差，利用改进扩展卡尔曼滤波对椭球参数的偏差进行估计，得出椭球模型的参数；
[0037]步骤(4)，由所述椭球模型参数求得所述式(3)中磁强计误差模型参数；
[0038]步骤(5)，根据所述式(2)的三轴磁强计误差模型，对三轴磁强计输出磁通量补偿；
[0039]步骤(6)，根据补偿后的三轴磁强计输出以及姿态角，计算得到校正后的载体航向角。
[0040]作为本发明的改进，在载体航向角校正之前，还包括姿态角校正步骤，包括如下具体步骤:
[0041]步骤(1)，对导航系统的经过一级误差补偿后的姿态角进行误差二级建模:
[0042]建立俯仰角误差模型:yP = mpxp+np，其中，yP表示俯仰角误差值，所述俯仰角误差值为俯仰角与转台角度差值，Xp表示测量值，mP、nP是误差模型的系数；
[0043]建立横滚角误差模型:yK = mQXK+nQ，其中，yE表示横滚角误差值,所述横滚角误差值为横滚角与转台角度差值，xK表示测量值，mQ、nQ是误差模型的系数；
[0044]步骤(2)，导航系统三轴转台进行试验，上位机采集数据:
[0045]首先，三轴转台初始水平校正；然后，保持导航系统横滚角不变，调整转台角度，上位机采集经过一级误差补偿后的导航系统俯仰角测量值，并记录所述俯仰角测量值与转台角度差值；
[0046]将三轴转台恢复初始水平设置，保持导航系统俯仰角不变，调整转台角度，上位机采集经过一级误差补偿后的导航系统横滚角测量值，并记录所述横滚角与转台角度差值；
[0047]步骤(3)，根据俯仰角误差模型，利用最佳平方逼近对所述采集的俯仰角与转台角度差值进行计算后得到所述误差系数mP、nP的值，从而得到俯仰角误差模型；
[0048]根据横滚角误差模型，利用最小二乘法对所述采集的横滚角与转台角度差值进行计算后得到所述误差系数mQ、nQ的值，从而得到横滚角误差模型；
[0049]步骤(4)，利用所述俯仰角误差模型对经过一级误差补偿后的导航系统俯仰角进行补偿，得到补偿后的俯仰角P为:P = Yp-Xp ;利用所述横滚角误差模型对经过一级误差补偿后的导航系统横滚角进行补偿，得到补偿后的横滚角R为:R = Ye-Xro
[0050]有益效果:为了提高导航系统的精度，本发明通过建立姿态角的输出误差二级模型用以校正姿态角，并且在航向角的计算原理上寻找提高精度的方法，从而提出了新的椭球模型和基于改进扩展卡尔曼滤波的椭球拟合方法进行磁场补偿。该校正方法可以有效的提高导航系统的精度，进而实现高精度定位和导航，并取得良好效果。
[0051]利用三轴磁强计计算载体的航向角的主要原理是，由磁强计测量出的地磁场强度值，然后进行相应的计算，最后得到航向角。对于如图2所示的二维系统，定义载体前进方向与磁北的夹角为地磁航向角P，其与地理北极的夹角为地理航向角P，由图示可知:
【权利要求】
1.一种用于导航系统的姿态及航向角的校正方法，其特征在于，载体航向角的校正包括下列步骤: 步骤(1)，三轴磁强计误差建模: 选取导航坐标系为东北天(ENU)、载体坐标系的Y轴为载体的前进方向，X轴为与载体前进方向垂直向右，Z轴与X轴、Y轴垂直向上满足右手定则；将磁强计的三轴测量值投影到二维水平面的X、Y轴，分别为Hx、Hy ;三轴磁强计的误差模型为:
Hm = CisCn (CsHe+bn)+b0 (I) 式中=Cis为灵敏度误差矩阵；Cn为非正交误差矩阵；CS为软磁误差矩阵；bn为硬磁误差矢量为偏置误差矢量；H6为地磁场强度；Hm为磁强计测量值； 将所述三轴磁强计的误差模型改写为:
Hm= CaHe+b (2) 式中:Ca是总误差矩阵；b为总偏置矢量； 由式(2)可得地磁场矢量H6为:
He = Ca 1 (Hm-b) (3)； 步骤(2)，改进的地球磁场的椭球模型建模: 地球磁场的椭球模型为:
AX2+BY2+CZ2+2EXY+2FXZ+2GYZ+2QX+2SY+2TZ+D = O (4) 式中，A、B、C、E、F、G、Q、S、T、D是椭球模型中的各项参数；X、Y、Z分别是磁强计的三轴X、Y、Z的测量值； 对所述椭球模型进行改进，得到如下椭球模型: a (x+y+p) 2+b (x+z+q) 2+c (y+z+r)2 = O (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 = ap2+bq2+cr2 ； 步骤(3)，利用改进扩展卡尔曼滤波器求解椭球模型的参数: 改进的扩展卡尔曼滤波器，直接把状态的更新值作为观测值进行下一步更新；利用改进扩展卡尔曼滤波器拟合椭球时，状态向量是所述式(5)中椭球参数估计值与真实值的偏差，利用改进扩展卡尔曼滤波对椭球参数的偏差进行估计，得出椭球模型的参数； 步骤(4)，由所述椭球模型参数求得所述式(3)中磁强计误差模型参数； 步骤(5)，根据所述式(2)的三轴磁强计误差模型，对三轴磁强计输出磁通量补偿；步骤(6)，根据补偿后的三轴磁强计输出以及姿态角，计算得到校正后的载体航向角。
2.根据权利要求1所述的一种用于导航系统的姿态及航向角的校正方法，其特征在于，在载体航向角校正之前，还包括姿态角校正步骤，包括如下具体步骤: 步骤(1)，对导航系统的经过一级误差补偿后的姿态角进行误差二级建模: 建立俯仰角误差模型:yP = mPxP+nP，其中，yP表示俯仰角误差值,所述俯仰角误差值为俯仰角与转台角度差值，Xp表示测量值，mP、nP是误差模型的系数； 建立横滚角误差模型= mQxK+nQ，其中，yK表示横滚角误差值，所述横滚角误差值为横滚角与转台角度差值，xK表示测量值，mQ、nQ是误差模型的系数； 步骤(2)，导航系统三轴转台进行试验，上位机采集数据: 首先，三轴转台初始水平校正；然后，保持导航系统横滚角不变，调整转台角度，上位机采集经过一级误差补偿后的导航系统俯仰角测量值，并记录所述俯仰角测量值与转台角度差值; 将三轴转台恢复初始水平设置，保持导航系统俯仰角不变，调整转台角度，上位机采集经过一级误差补偿后的导航系统横滚角测量值，并记录所述横滚角与转台角度差值； 步骤(3)，根据俯仰角误差模型，利用最佳平方逼近对所述采集的俯仰角与转台角度差值进行计算后得到所述误差系数mP、nP的值，从而得到俯仰角误差模型； 根据横滚角误差模型，利用最小二乘法对所述采集的横滚角与转台角度差值进行计算后得到所述误差系数HVne的值，从而得到横滚角误差模型； 步骤(4)，利用所述俯仰角误差模型`对经过一级误差补偿后的导航系统俯仰角进行补偿，得到补偿后的俯仰角P为:P = Yp-Xp ;利用所述横滚角误差模型对经过一级误差补偿后的导航系统横滚角进行补偿，得到补偿后的横滚角R为:R = yE-xRO
【文档编号】G01C21/20GK103630137SQ201310633933
【公开日】2014年3月12日 申请日期:2013年12月2日 优先权日:2013年12月2日
【发明者】陈熙源, 吕才平, 黄浩乾, 汤传业, 宋锐, 何昆鹏, 方琳 申请人:东南大学