一种在线实时估计补偿磁强计误差的定位测姿方法

摘要

本发明公开了一种在线实时估计补偿磁强计误差的定位测姿方法，本发明所要解决的技术问题为：针对GNSS/MINS组合导航系统中，GNSS信号中断时，MINS航向信息快速发散的问题，通过利用GNSS观测状况良好时组合导航系统提供的姿态信息来对磁强计以及外部干扰磁强的实时标定，在GNSS信号中断时利用磁强计信息来辅助INS进行导航从而提高系统导航精度。

说明书

技术领域

本发明涉及GNSS/MINS(Global Navigation Satellite System/Micro InertialNavigation System)组合导航系统的定位测姿问题，尤其涉及到利用GNSS/MINS组合导航系统解算的姿态信息作为参考来标定磁强计的误差从而提高在GNSS信号中断时GNSS/MINS组合导航系统的定位测姿精度的方法。

背景技术

GNSS具有全球、快速、全天候定位的优势，是当前定位领域应用最广泛的定位方式，应用领域涵盖航海、航天、车辆导航等各个领域，然而在现代化城市中高楼林立，立交桥、隧道、绿化树木等对GNSS信号遮挡严重，并导致大量信号多径干扰。此外城市中各种通信设施的存在、电磁环境极为恶劣，各种有意无意的干扰对GNSS接收机信号接收能力带来了极大挑战。INS利用陀螺和加速度计提供的角速率和比力信息来解算载体的位置、速度、姿态等信息，其不受外界干扰，然而由于受误差积累的影响，导致导航定位解算精度随时间发散。GNSS和INS具有良好的互补性，两者的组合能够提供相较于单一系统更精确、可靠的导航定位结果。在GNSS/INS组合导航中，GNSS提供惯导需要的更新信息，从而抑制惯导信息的发散，当GNSS因为信号受到遮挡或者干扰而中断时，惯导仍能继续工作从而增加系统的可靠性和健壮性。

卫星导航系统(GNSS)与惯性导航系统(INS)的组合应用方式可以极大地提高现有导航系统的可用性，有效增强军事装备动态性能和抗干扰能力。目前，GNSS/INS组合导航系统已经获得了一些应用，尤其在军事领域。由于组成INS的惯性传感器普遍比较昂贵，也因此限制了GNSS/INS技术的应用范围。对于军事装备而言，高性能、低成本的导航技术在车辆、飞机、舰船、导弹、信息化弹药、微型卫星等多个应用领域具有十分迫切的需求，以实现系统的高可靠性、高抗干扰能力以及精确制导能力。

随着半导体集成电路微细加工技术以及超精密机械加工技术的发展，MEMS(Micro-Electrical-Mechanical System)传感器得到了蓬勃的发展。MEMS IMU具有体积小、重量轻、功耗低、低成本等优点。从而使惯性导航技术逐渐地进入民用如车载导航、无人机导航定位定姿等领域。MINS技术的出现使低成本GNSS/INS组合技术的广泛应用成为可能。

MINS具有体积小、重量轻、功耗低等优点，然而由于加工制造工艺的限制以及传感器本身噪声的影响会导致导航结果中包含有较大的误差。在GNSS/MINS组合导航系统中，当GNSS系统由于遮挡或者干扰而无法工作时，MINS误差会以较快的速度发散，例如陀螺的零偏误差会导致姿态角按时间的一次方发散，进而导致速度误差按时间的二次方发散，最终导致位置信息按时间的三次方发散。因此在GNSS信号中断过程中必须通过一定的方法来抑制惯导导航信息的发散，目前MINS系统中通常集成了磁强计，地磁场信息通常稳定地分布在地球各处，利用磁强计测量得到的地磁强信息并且进行适当的误差改正能够得到载体的航行信息，因此可以通过在GNSS/MINS组合导航系统中加入磁强计信息来提高GNSS信号中断时系统的定位测姿精度。

利用磁强计来获取航向的前提是磁强计所在的地磁场不受外界的干扰，但是在实际的车载导航应用中，磁强计会受到车辆的金属结构产生的干扰磁场的影响，通常而言，车辆结构产生的干扰磁场的方向相对于车体的方向是固定不变的，在实际的车载导航应用中，由于MEMS INS与车体是固连的，因此干扰磁场在MEMS INS坐标系中的投影也是固定不变的，从而使得干扰磁场被消除成为可能。除了干扰磁场造成的误差，由于制造工艺的限制，磁强计本身存在误差，确定性的误差包括零偏、比例因子、交轴耦合。在GNSS/INS组合导航过程中，可以实时获取到载体的在地理坐标系下的姿态信息，同时也可以获得磁强计的输出，因此可以利用载体的绝对姿态信息对磁强计误差进行标定，当GNSS信号长时间中断的时候，可以利用经过标校之后的磁强计信息来辅助组合导航系统的航向信息，从而抑制惯导信息的快速发散从而提高定位测姿的精度。

发明内容

本发明所要解决的技术问题为：针对GNSS/MINS组合导航系统中，GNSS信号中断时，MINS航向信息快速发散的问题，通过利用GNSS观测状况良好时组合导航系统提供的姿态信息来对磁强计以及外部干扰磁强的实时标定，在GNSS信号中断时利用磁强计信息来辅助INS进行导航从而提高系统导航精度。

本发明的主要内容如下：

一种在线实时估计补偿磁强计误差的定位测姿方法，包括以下步骤：

步骤1：首先对GNSS/MINS组合导航系统进行初始化，初始化包括位置和姿态的初始化；

步骤2：实时导航过程中记录估计磁强计误差所需要的数据，包括磁强计的实时输出和磁强计的理论输出值；

步骤3：根据步骤2记录的磁强计的实时输出和磁强计的理论输出值通过最小二乘方法实时计算磁强计的误差,所述的磁强计的误差包括磁强计零偏、比例因子以及交轴耦合误差；

步骤4：当GNSS信号中断时，先用步骤3中计算得到的磁强计的误差补偿磁强计输出，再利用补偿后磁强计信息辅助组合导航系统的航向。

其中，步骤2的方法如下所示：

设定磁强计的数学模型为：

或者：

式中，M矩阵代表磁强计的误差矩阵，其中对角线元素对应磁强计的比例因子误差，非对角线元素代表磁强计的交轴耦合误差，b_mi代表磁强计i轴的零偏误差，i＝x,y,z；

设定某一时刻载体的姿态信息为[φ₁>1>1]，则在该时刻磁强计输出的理论值表示为：

其中，m₁'代表当地磁场在b系中的投影，即磁强计的理论输出值，m_bx1,m_by1,m_bz1依次代表当地磁场在磁强计x，y，z上的分量，为载体坐标系到导航坐标系的方向余弦矩阵，cθ、cφ、cψ依次代表横滚角、俯仰角以及航向角的余弦值，sθ、sφ、sψ依次代表横滚、俯仰以及航向角的正弦值，m_n为地磁场在导航坐标系的投影；

得到多组磁强计输出的理论值：

同时记录载体对应的[φ_i>i>i]角度时的磁强计的实际输出，组成如下所示的观测矩阵u_i:

其中m_xi,m_yi,m_zi依次代表磁强计x，y，z轴的输出，i组相互独立的磁强计实际输出记为：

其中，步骤3中通过最小二乘方法实时计算磁强计的误差具体为：

根据步骤2记录的磁强计的实时输出和磁强计的理论输出值，构建最小二乘的系数矩阵如下：

最小二乘的观测矩阵，如下：

U＝[u₁>2 ...>i]；

通过最小二乘方法计算得到误差矩阵：

M＝U·A^T·(AA^T)^-1；

根据误差矩阵实时计算磁强计的误差。

其中，步骤4具体为：

当GNSS信号中断时，首先利用步骤3中计算得到的磁强计误差补偿磁强计输出，再用补偿后的磁强计输出计算航向信息，航向信息具体计算方法如下：

其中，

θ、φ分别为载体实时的横滚角和俯仰角；ψ_mag为利用磁强计输出计算得到的载体的航向信息，γ_m为当地磁偏角，其值结合纬度信息查阅资料得到；M_x,M_y,M_z分别为经过补偿后的磁强计输出的X，Y，Z轴的输出。

利用磁强计和惯导计算的航向信息进行加权平均得到精度更高的航向信息，计算方法如下：

其中ψ_int为利用磁强计和惯导计算的航向值进行组合后计算的得到的航向值，ψ_ins为惯导计算的航向信息，而ψ_mag为磁强计计算的航向信息，σ_ins、σ_mag为惯导和磁强计计算得到的航向的方差。

本方法实现了磁强计的实时在线标定，通过利用GNSS信号良好时高精度的姿态信息作为参考值来标定磁强计的原始，能有有效提高实时导航的效率。

附图说明

图1为磁强计校准以及辅助初始对准步骤示意图。

具体实施方式

下面结合具体实施例和附图对本发明做进一步的描述：

步骤一：车载导航前首先进行初始对准。车载导航应用中，组合导航接收机与车体固连，导航之前首先对GNSS/MINS组合导航接收机进行初始对准，位置的初始对准可以通过GNSS的经度、纬度、大地高信息获得，水平姿态信息可以通过加速度计计算得到，航向信息可以通过磁强计或者双天线得到。如果航向信息是采用磁强计辅助MINS姿态初始对准，在对准前需要对磁强计进行校正，以消除干扰磁场的影响。

初始对准的具体流程如下：

1.导航开始前车辆处于静止状态，给GNSS/MINS组合导航接收机上电，给组合导航接收机上电后不断地记录组合导航接收机中INS天向陀螺的输出。使车辆在开阔场地旋转一周，即使车辆航向遍历0～360°。判断车辆开始旋转的方法为检测天向陀螺的输出是否有大于设定阈值(如10°/s),如果陀螺有大于阈值的输出则认为车辆已经开始了机动。车辆在机动的同时不断记录天向陀螺的输出，当天向陀螺的输出连续10秒小于设定的阈值(如2°/h)，认为车辆机动完成，并且处于静止状态。

如果没有外界磁场的干扰，GNSS/MINS组合导航接收机中磁强计水平方向各轴的最大值和最小值绝对值应该近似相等。但是由于外界干扰磁场的存在，磁强计水平各轴的输出最大值和最小值的绝对值通常不相等。

在此步骤中求取出航向遍历0～360°过程中磁强计X，Y，Z各个轴线输出的最大值和最小值Mx_max、Mx_min，My_max、My_min，Mz_max、Mz_min，根据车辆在机动过程中记录的磁强计输出的最大值和最小值来求取磁强计各个轴线的改正值，计算得到磁强计X，Y，Z轴的补偿值为：

2.车辆航向遍历0～360°后使车辆静止一定的时间段(如90s)，静止过程中不断地记录加速度计X，Y，Z轴的输出。计算得到加速度计X，Y，Z轴输出的平均值f_x,f_y,f_z，利用下列两式计算得到载体的横滚角和俯仰角：

φ＝atan2(f_y,f_z)

其中，f_x、f_y、f_z依次代表车辆静止时间段内加速度计X，Y，Z轴输出的平均值。

3.利用计算得到的磁强计各轴的补偿值biasx，biasy，biasz来补偿磁强计的输出，得到补偿后的磁强计输出，补偿方法如下：

上式中依次为磁强计x,y,z轴的原始输出，依次磁

强计x,y,z轴输出的校正值。

4.计算经过补偿后的磁强计输出的均值，在静止过程中不断记录加速度计的输出，并且求取磁强计输出的均值。计磁强计经过校正后的X，Y，Z轴的均值依次为：通过下式计算得到载体的航向信息：

其中，

γ_m为初始对准当地的磁偏角，其值可以结合纬度信息查阅资料得到。

步骤二：初始对准完成后，进入导航状态，根据不同的应用场景卡尔曼滤波算法可以选择不同数量的状态向量。本实施例选取姿态误差(横滚、俯仰、航向)，位置误差(经度、纬度、高度)，速度误差(东向速度、北向速度、天向速度)，陀螺零偏，加速度计零偏，陀螺比例因子，加速度计比例因子，接收机钟差，钟漂共23个参数作为卡尔曼滤波的状态向量。状态向量如下所示：

在上式中ψ_N,ψ_E,ψ_D依次代表姿态的横滚，俯仰，航向误差；δv_N,δv_E,δv_U依次代表北向、东向、天向速度误差；δr_N,δr_E,δr_D依次代表纬度、经度、高程误差；依次代表陀螺X，Y，Z轴零偏，依次代表加速度计X，Y，Z轴零偏，依次代表陀螺X，Y，Z轴比例因子误差；依次代表加速度计X，Y，Z轴比例因子误差；δt_u,δt_ru依次为接收机的钟差和钟漂。

卡尔曼滤波组合过程包括预测和更新两个过程。预测过程包括状态向量的预测和协方差阵的预测，而更新过程包括状态向量的更新和协方差阵的更新，在更新的过程中卡尔曼滤波估计得到陀螺、加速度计各个轴线的零偏、比例因子误差后，对陀螺、加速度计的原始输出进行反馈补偿。在导航的过程中能够不断地获取到载体的姿态信息[φ θ ψ]，记录不同姿态下磁强计的输出u,不断地组建最小二乘方程实时估计磁强计的传感器误差。构建最小二乘观测量的时候，为了增加方程的可观测性，优先选取姿态差异较大场景下的磁强计观测量。例如最小二乘观测方程中的记录第一组观测量u₁时载体姿态为[φ₁>1>1]，如果车辆的航向变化大于5°则记录第二组观测量u₂，按照同样的方法记录多组数据用于平差计算。如果参与平差的数据超过20组，则将最先记录的数据剔除，将最新记录的数据添加进去，进行估计，这样更新的目的是为了得到最新的能够更加准确反映当地磁场强度的数据。

实时估计磁强计的误差，所估计的磁强计的误差包括磁强计零偏、比例因子以及交轴耦合误差。其中磁强计输出的数学模型可以表示为：

或者：

在上式中，M矩阵代表磁强计的误差矩阵，其中对角线元素对应磁强计的比例因子误差，非对角线元素代表磁强计的交轴耦合误差，b_mi(i＝x,y,z)代表磁强计i轴的零偏误差。在跑车测试过程中，当有GNSS信号时，GNSS/MINS组合导航系统可以准确地确定出载体的姿态信息即横滚φ、俯仰θ、航向ψ信息。假设在某一时刻载体的姿态信息为[φ₁>1>1]，那么在该时刻投影到磁强计x、y、z轴上的磁强强度可以表示为：

载体行驶过程中记录载体不同角度[φ_i>i>i]时地磁场在磁强计坐标系中的投影，i组相互独立的磁强计观测量如下：

同时记录载体对应的[φ_i>i>i]角度时的输出，组成如下所示的观测矩阵u_i

步骤三：实时估计磁强计的误差。构建最小二乘的系数矩阵如下：

观测矩阵如下：U＝[u₁>2 ...>i]，可以通过最小二乘方法计算得到误差矩阵：

M＝U·A^T·(AA^T)^-1

步骤四：在卫星信号中断后，卡尔曼滤波仍然在进行预测，由于短时间内磁场比较稳定因此磁强计受外界干扰的可能性比较小，利用磁强计计算的航向信息比较稳定，此时可以利用磁强计和惯导计算的航向信息进行加权平均得到精度更高的航向信息。计算方法如下：

上式中ψ_ins为惯导计算的航向信息，而ψ_mag为磁强计计算的航向信息，σ_ins、σ_mag为惯导和磁强计计算得到的航向的方差。

以上步骤便完成了GNSS/MINS系统中利用GNSS观测条件良好时的观测信息对磁强计进行标定，当GNSS信号较弱利用标定后的磁强计信息辅助航向的过程。