基于INS/GPS/磁力计的全组合导航*

梅玲玉， 马 磊， 张 涛

(西南交通大学 电气学院，四川 成都610031)

0 引 言

随着传感器技术的迅猛发展，各种面向复杂应用背景的多传感器组合导航系统也随之大量涌现。目前应用最为广泛的是由惯性导航系统[1](inertial navigation system,INS)和全球定位系统[2](global positioning system,GPS)构成的组合导航系统[3～5]，如 INS/GPS,INS/北斗等。INS/GPS组合导航系统，具有良好的互补性，克服了纯惯性导航系统误差随时间累积的缺点，同时在GPS信号受到遮挡时惯性导航系统短期内能提供较高精度的导航信息。然而该系统存在着载体航向角误差可观测性较差的问题。地磁导航具有完全自主、抗干扰能力强、无累积误差的优点，可以作为惯导的辅助实现快速对准，同时提供实时的航向参考，从而弥补惯性测量单元(inertial measurement unit,IMU)与GPS组合导航系统的不足[6]。

本文首先对传感器数据进行预处理，其次推导基于误差四元数的定姿系统方程和量测方程，利用加速度、磁力计修正陀螺姿态信息；给出定位系统方程和量测方程，利用GPS修正陀螺位置、速度信息。最后在移动平台上搭建组合导航系统，进行实验验证。

1 数据预处理

磁力计和陀螺仪传感器作为组合导航中的关键器件[7]，不可避免地存在各种误差，为提高器件的可靠性、降低器件对导航精度的影响，需对传感器数据进行预处理。

1.1 磁力计数据处理

在某一地区，地磁强度可视为恒定的向量。因此,在只有地磁场的作用下，磁力计测量值的水平分量为圆形。但实际环境中存在各种磁干扰[8]，水平分量形成的圆被拉成圆心偏移的椭圆。环境干扰具体可表示为

(1)

式中K,Φp,Φs为与软磁有关的误差，bx，by分别为和硬磁材料有关的误差。

传统的拟合多采用最小二乘法求解椭圆参数估计值。该方法至少需要在椭圆上均匀分布的5个点，同时随机误差将严重影响到估计值的计算。一种解决方法是尽可能多地采集椭圆上的数据点，但数据点的增加将导致计算量的增大，并要求更多的存储空间，不利于整个导航系统的实现。因此需要一种在线校正方法，本文采用递推最小二乘法实现罗差补偿,最小二乘法的递推公式如下为[9]

(2)

(3)

式中B为与磁场强度有关的参数。求解出椭圆参数后，根据式(1)即可得到误差系数，从而对磁力计进行校正。

1.2 陀螺仪数据处理

陀螺仪的预处理一般采用时间序列分析法[10]，建立自回归滑动平均模型(auto regressive moving average model,ARMA)

yk-φ1yk-1-Lφpyk-p=ak-θ1ak-1-L-θqak-q

(4)

式中yi为时间序列;φi,ai分别为自回归系数和滑动平均系数。得到陀螺仪输出噪声的ARMA模型后，进行随机噪声的预判并在实时数据中抑制该部分噪声。随机噪声序列看作由一白噪声信号经过一系列线性变换得到的，符合卡尔曼滤波条件。将式(4)扩展成卡尔曼滤波方程，即

Y=Xφ+Aθ

(5)

式中 取X=[yk-1yk-p]T。

2 组合导航系统

本文研究了GPS/INS/磁力计全组合导航系统，分别就定姿系统和定位系统建立系统方程和量测方程，采用卡尔曼滤波器[11]进行信息融合，整体框架如图1所示。

图1 组合导航系统整体框图

2.1 定姿系统

传统的GPS/INS组合导航系统中，将平台误差角近似等同于姿态误差角进行姿态校正，从而导致较大的模型误差。本文直接以误差四元数为状态量，建立误差四元数模型进行信息融合。

2.1.1 系统方程

2.1.2 观测方程

在某一地区，重力加速度和地磁强度可视为恒定的向量。由加速度计和磁力计数据可得到三轴姿态

(6)

(7)

2.2 定位系统

2.2.1 系统方程

根据捷联惯导解算方程，可推导出位置误差、速度误差、平台失准角误差方程，本文直接给出各方程为

(8)

式中F11～F34参见文献[1]。本文仅考虑传感器的常值零偏和随机游走误差，则陀螺仪和加速度的误差可写为

(9)

式中bg,εg,ba,εa分别为陀螺仪和加速度计的零偏误差和随机游走误差。常值零偏的特性可知

(10)

将式(9)、式(10)代入式(8)，可得系统方程

(11)

2.2.2 量测方程

将惯性导航系统解算的位置、速度信息与GPS测量得到的位置、速度信息的差值作为系统的量测信息。考虑组合导航应用于移动平台上，暂不考虑载体的高度信息。

位置量测方程如下

(12)

式中xI,yI为惯性导航解算得到的东、北向位置信息;xG,yG为GPS测量得到的东向北向位置信息;Hp=I2×2，Vp=[npxnpy]T。同理可以得到，速度量测方程

(13)

式中vxI,vyI为惯性导航解算得到的东、北向速度;vxG,vyG为GPS测量得到的东、北向速度;Hp=I2×2，Vv=[nvxnvy]T。合并位置、速度量测方程得到定位系统量测方程

(14)

3 实验与结果分析

核心处理器采用STM32，IMU采用ADIS16405，输出频率为81.9 Hz，GPS采用诺瓦泰公司的OEM615板卡，输出频率为1 Hz。实验过程中，将IMU和GPS搭载在差分移动平台上，测试区域为165 m×70 m的方形区域，移动平台以一定的速度沿实验路线运动。实验路线标记轨迹如图2。

图2 实验路线

跑车实验前，差分移动平台匀速自转1周，得到磁力计校正参数。图3中，实线为校正前的数据，虚线为校正后的数据。校正后的磁力计x轴,y轴轨迹为圆心在原点的圆，航向角趋于直线，对应差分移动平台匀速自转。校正前后航向误差最大可达23.9°。可以看出，递推最小二乘法较好地修正了环境误差。

图3 最小二乘法处理对比

实验姿态结果如图4，实线为滤波前的观测姿态角，即加速度计、磁力计计算出的姿态，虚线为卡尔曼滤波后的姿态角。由于采用了反馈校正，本文不再给出惯性导航的姿态信息。可以看出，滤波后横滚角和俯仰角控制在4°以内。卡尔曼滤波法解决了单惯导系统漂移的问题，同时精度与单独的加计、磁力计解算相比也得到了较大改善。

图4 卡尔曼滤波姿态对比

实验定位结果如图5所示,实线为GPS测量的位置信息，虚线为卡尔曼滤波后的位置信息。GPS轨迹突出部分的数据明显异常，可看作GPS失效点。可以看出，组合导航系统抑制了陀螺单独解算的发散，同时在GPS失效时，仍能很好地工作。

图5 卡尔曼滤波位置对比

速度测量结果如图6所示，实线为GPS测量的速度信息，虚线为滤波后的速度信息。由于移动平台的速度较慢，导致速度对比不够明显，滤波前后速度的标准差如下：GPS测量值东向速度标准差为0.146 068 58 m/s，北向速度标准差为0.084 437 32 m/s；卡尔曼滤波东向速度标准差为0.145 171 12 m/s，北向速度方差为0.084 024 41 m/s。该结果表明组合导航系统的性能有所改善。

图6 卡尔曼滤波速度对比

4 结束语

本文建立了多传感器信息融合的组合导航系统，针对定姿、定位系统分别建模并进行了实验验证。最终结果表明组合导航系统相比惯性导航和GPS定位得到了有效改善。但该模型中将GPS的误差看作白噪声处理，可进一步对GPS进行误差建模，进行GPS/INS紧组合研究。