一种基于INS/GPS/CNS 的全信息导航滤波算法

尹洪亮，张 嵘，郝 强，于东康

(1.清华大学精密仪器系，北京 100084；2.中国舰船研究院，北京 100192；3.哈尔滨工业大学仪器科学与工程学院，哈尔滨 150001)

0 引 言

由于惯性导航系统（Inertal Navigation System,INS）与全球定位系统（Global Positioning System，GPS）组合可以提供较高精度的速度、位置信息，所以INS/GPS 组合系统被广泛应用在船舶、飞机、导弹等领域。但因INS/GPS 组合系统量测信息仅包括位置和速度信息，缺少姿态信息观测量，导致系统的可观测性较差，容易造成系统在长时间导航应用中精度不高，甚至姿态信息发散的问题[1-3]。

天文导航系统（Celestial Navigation System，CNS）采用星体敏感器测量天体方位信息，在经过解算后输出载体姿态信息，其特点是姿态测量精度高、测量误差误差不随时间积累、系统可靠性高，是一种新型的高精度姿态测量系统[4-6]。

为增加INS/GPS 组合系统的量测信息，提高系统导航精度，改善系统航向误差发散的问题，将CNS 与传统INS/GPS 系统组合是目前的研究热点。张科设计了基于联邦滤波的INS/GPS/CNS 组合导航算法，但由于其采用的是线性卡尔曼滤波框架，不能很好适应INS/GPS/CNS 这类典型非线性系统，造成滤波结果精度较低；吴坤峰考虑到CNS 可能存在输出不连续的问题，利用GPS 和CNS 输出的高精度导航信息，分别设计了GPS/INS 和CNS/INS 滤波方案，在获得滤波后的导航信息后，将其作为全信息观测量进行二次滤波，最终获得高精度的导航信息。该方法计算较为繁琐，且二次滤波会造成系统实时性变差的问题，不宜开展算法的工程化应用[7-8]。

针对上述问题，本文提出一种基于扩展卡尔曼滤波（ Extended Kalman filter， EKF） 的INS/GPS/CNS 全信息导航滤波算法，利用GPS 和CNS 输出的高精度导航信息，结合非线性扩展卡尔曼滤波框架，设计全观测量的滤波算法，通过研制的INS/GPS/CNS 三组合导航系统进行试验验证，验证了所提算法的高精度和长时间工作条件下的稳定性。

1 INS/GPS/CNS 滤波模型

根据INS，GPS 和CNS 的系统特点，建立INS/GPS/CNS 系统的数学模型。

1.1 状态方程模型

根据文献[9]，建立INS 数学模型如下：

1）速度误差模型

式中： i 为惯性坐标系； e 为地球坐标系； fn为加速度计测得的比力在导航系中的投影；为东、北、天向的速度误差；，为东、北、天向的平台失准角； ∇n为加速度计零偏在导航坐标系中的投影； ωen为牵连角速度； ωie为地球自转角速度，根据INS 系统特点，有

其中： L 为当地地理纬度， h为系统重心距地面高度，R为地球半径。

2）平台失准角误差方程

式中： εn为陀螺常值漂移在导航坐标系中的投影；

3）位置误差方程

式中：

基于上述分析，选用INS 模型作为INS/CNS/GPS系统滤波的基础框架，可获得状态模型如下：

其中： A为 状态矩阵； B为系统噪声矩阵； W 为系统噪声，具体表达式可参见文献[9]； X为系统状态参量，表达式如下：

1.2 量测方程模型

1）INS/GPS 量测方程

由于GPS 可提供高精度的位置和速度信息，所以取I N S 和G P S 输出的位置和速度信息之差作为INS/GPS 系统的观测量，定义量测方程为：

2）INS/CNS 量测方程

由于INS 输出为机体系下姿态（ ΦIb），CNS 为惯性系下姿态（ ΦCb），所以需将两坐标系转化后构建量测方程。将姿态信息转化到导航系下：

获得量测方程如下：

3）INS/GPS/CNS 量测方程

基于上述量测方程，构建INS/GPS/CNS 全信息观测量方程，如下：

2 基于EKF 的INS/GPS/CNS 滤波算法

将式（ 8） 和式（ 13） 离散化获得系统离散化模型：

根据上述离散化模型建立扩展卡尔曼滤波方程如下：

其中： Fk为状态矩阵 Ak的雅克比行列式， Pk， Qk分别为系统噪声和量测噪声协方差矩阵。

在初始状态向量 X0和 初始系统噪声 P0已知时，再根据k 时刻的量测信息 Zk，即可计算得到k 时刻状态向量的最优估计，获得高精度稳定的导航信息输出。

3 试验结果与分析

为验证本文提出的基于EKF 全信息导航算法的有效性，搭建INS/GPS/CNS 三组合导航系统进行精度验证，系统由激光惯导（陀螺漂移 0.001°/h）、GPS 接收机（定位误差100 m）和星敏感器组成，为便于精度验证，采用高精度转台作为姿态基准进行摇摆试验测试，速度、位置基准分别为0 m/s 和0 m。为验证系统在长航时下的精度稳定性，试验时间设为24 h。采用传统两组合滤波算法作为对比，为更直观体现算法稳定性，在进行统计时，以初始时刻的导航误差 e0为基准，计算后续误差 ek相 对于 e0的稳定性A，如下式：

试验结果如图1、图2 及图3 所示。

由图1 可知，2 种滤波算法的处理结果均收敛，但从数据波动程度来看，两组合算法波动较大，且根据稳定性比值峰峰值，三组合算法峰峰值较小，证明算法稳定性较好。由图2 东向速度试验结果，三组合算法误差收敛很快，误差远小于两组合结果，证明三组合算法精度较高。由图3 姿态误差试验结果可知，在长航时条件下，姿态信息有不同程度的发散，这是由于INS 等效东向陀螺漂移的长期稳定性很难保证所致，在增加了CNS 的姿态信息作为观测量后，滤波效果得到明显改善，姿态信息收敛且保持稳定。所以，相较于传统的INS/GPS 两组合扩展卡尔曼滤波算法，INS/GPS/CNS 三组合扩展卡尔曼滤波算法结果精度更高，长航时工作条件下稳定性更好。

图 1 位置误差试验结果对比图Fig.1 Comparison of position error test results

图 2 速度误差试验结果对比图Fig.2 Comparison of speed error test results

图 3 姿态误差试验结果对比图Fig.3 Comparison of attitude error test results

4 结 语

本文针对INS/GPS 两组合系统长航时条件下姿态精度发散、导航精度较低、系统稳定性较差的问题，引入CNS 系统的姿态信息，构建一种基于INS/GPS/CNS 的全信息导航滤波模型，提出一种基于EKF 的INS/GPS/CNS 滤波算法。搭建INS/GPS/CNS 三组合系统对算法有效性进行验证，试验结果表明，相较于传统INS/GPS 两组合算法，所提算法在长航时条件下稳定性强、收敛性好且精度高，工程应用意义较大。