陀螺罗经和计程仪辅助的GNSS/SINS松组合导航系统*

顾明星 刘 卫 胡 媛 谢宗轩 赵建森 王胜正

1.上海海事大学商船学院，上海 201306 2.上海海洋大学工程学院，上海 201306

0 引言

无人驾驶技术的发展对航行器导航系统的准确性和可靠性提出了更高要求[1-2]。由全球导航卫星系统(GNSS)和捷联式惯性导航系统(SINS)构成的GNSS/SINS组合导航系统[3]会因卫星信号的受干扰或丢失而失效。陀螺罗经和计程仪辅助的组合导航系统是一种新的导航系统，该系统是在GNSS/SINS松组合的基础上引入航行器的航向和航速信息，能抑制SINS单独工作时误差迅速扩大的影响。

GNSS/SINS组合导航系统能够优势互补，SINS能辅助GNSS捕获和跟踪卫星信号，增强GNSS的抗干扰性；GNSS能抑制SINS的误差积累，提高导航精度[3]。GNSS和SINS的组合方式分为松组合、紧组合和超紧组合[4]。其中，GNSS/SINS松组合是GNSS和SINS相互独立地进行定位、定速算法，然后两者的定位结果通过一个前反馈式卡尔曼滤波器(Kalman Filter，KF)整合到一起，最终SINS输出经滤波器修正的导航结果[5]。

目前主要有2种方法提高GNSS失效情况下SINS单独导航时的导航精度:1)预先对惯性器件的误差进行建模，然后在导航解算过程中对惯性误差进行补偿[6]；2)利用与载体相关的运动信息，辅助SINS修正其误差[4,7-9]。文献[4]直接利用飞行器的速度变化特性约束飞机的SINS/GNSS组合导航误差，对于其他航行器的导航应用具有局限性。文献[7]提出一种融入惯性器件随机噪声修正和载体运动特征约束的车载式MEMS-SINS/GNSS松组合导航系统，该系统利用加速度计提供的速度信息辅助修正SINS误差，使其在GNSS失效情况下的导航精度优于传统导航系统。文献[8-9]提出利用计程仪辅助GNSS/SINS组合导航系统，该系统在获取初始位置信息后能不依赖GNSS信号而完成一段时间的高精度导航。

在多传感器的数据融合方面[10-12]，根据系统特性，滤波方法包括加权最小二乘法(Weighted Least Square Principle，WLSP)、扩展卡尔曼滤波(Extended Kalman Filter，EKF)、无迹卡尔曼滤波(Unscented Kalman Filter，UKF)、中心差分卡尔曼滤波(Central Difference Kalman Filter，CDKF)等。文献[10]在GPS/INS/DVL的数据融合过程中，提出一种基于加权最小二乘法的自适应联邦强跟踪滤波算法以保证整个系统的可靠性。文献[11]提出一种基于模糊自适应UKF的舰船SINS/GPS/DVL组合系统联合滤波算法，SINS分别与GPS和多普勒计程仪构成2个子滤波器，再将子滤波得到的数据输入主滤波器，但系统的非线性误差仍然无法避免。文献[12]在CDKF的基础上，提出了基于高斯过程的CDKF参数预测能力，减小系统预测误差。

针对GNSS/SINS组合导航系统在GNSS失效情况下，SINS单独工作时导航误差会迅速扩大的特点，本文提出了利用陀螺罗经(Gyrocompass，GC)和计程仪(Vialog，VL)辅助GNSS/SINS的组合导航方法，可以实现通过增加载体的运动信息并构造线性系统来克服导航误差迅速扩大的缺点。该方法采用GNSS/SINS松组合方式，将GC和VL分别提供的航向和航速信息导入由SINS误差模型构造的KF中。仿真实验表明，在GNSS失效情况下，有GC/VL辅助的GNSS/SINS组合导航系统导航误差明显小于无辅助的导航系统。

1 GNSS/SINS组合导航系统设计方案

1.1 捷联式惯性导航系统更新算法

SINS的定位原理是利用惯性器件测得的角速度和加速度，推算出当前载体的位置、速度和姿态[13]，更新方程为

(1)

(2)

(3)

1.2 捷联式惯性导航系统误差分析

1.2.1 惯性传感器测量误差

惯性导航系统中的惯性传感器包括3个陀螺仪和3个加速度计[13]。

陀螺组件的测量误差模型为

(4)

加速度计组件的测量误差模型为

(5)

1.2.2 姿态误差方程

捷联惯导姿态误差微分方程为

(6)

(7)

(8)

(8.1)

(8.2)

(8.3)

则最终姿态误差方程(6)变为

(9)

1.2.3 速度误差方程

导航计算机解算出的速度和理想速度存在偏差，该偏差称为速度误差。捷联惯导速度误差微分方程为

(10)

M3为重力误差方程δgn=M3δp中的系数，则最终速度误差方程(10)变为

(11)

1.2.4 位置误差方程

位置误差方程为

(12)

则最终位置误差方程为

(13)

(13.1)

(13.2)

1.3 卡尔曼滤波算法

卡尔曼滤波算法包括状态方程和量测方程[14]。

根据SINS的线性误差模型，构造状态方程如下

(14)

(15)

GNSS正常运行时能为载体提供位置信息和速度信息，并实时校正SINS的误差。GNSS/SINS系统的量测方程为

(16)

(17)

卡尔曼滤波算法具体流程如下：

状态一步预测误差

(18)

状态一步预测均方误差阵

Pk/k-1=(I+Fδt)Pk-1(I+Fδt)T+GQGTδt2

(19)

式(19)中：Q=D(Wb)。

滤波增益

Kk=Pk/k-1HT(HPk/k-1HT+R)-1

(20)

式(20)中：R=D(V)。

状态估计

(21)

状态估计均方误差阵

Pk=(I-KkH)Pk/k-1

(22)

2 GC/VL辅助系统

在GNSS失效情况下，SINS/GC/VL作为备用导航系统开始运行，并且GC/VL能够辅助修正SINS的惯性器件长时间累积的误差。

2.1 陀螺罗经辅助模型

陀螺罗经是根据陀螺仪的定轴性和进动性，并结合地球的自转与重力特性，为载体提供航向信息的仪器[15]。

(23)

(24)

式(23)中：Z1是量测向量，H1是量测矩阵，X是状态向量，V1是量测噪声向量。

2.2 计程仪辅助模型

计程仪是一种具有测速和计算航程功能的导航仪器[16]。计程仪可精确测量载体相对于地面或海底的绝对速度水平分量，其测量误差不随时间增加而累积，可用来实时补偿SINS的误差[17]。

(25)

(26)

式(25)中，Z2是量测向量，H2是量测矩阵，X是状态向量，V2是量测噪声向量。

2.3 GNSS/SINS/GC/VL组合导航系统设计

有陀螺罗经和计程仪辅助的GNSS/SINS松组合导航系统的结构框图如图1所示。

图1 GNSS/SINS/GC/VL松组合导航系统框图

在GNSS正常情况下，GNSS/SINS/GC组合导航系统正常运行。SINS输出航向、速度、位置信息，GNSS输出速度、位置信息，GC输出航向信息，这些数据分别作差并经KF1滤波得到惯导误差，反馈修正SINS，系统最终输出修正后的导航结果。

结合式(16)和(23)，GNSS/SINS/GC组合导航系统的量测方程为

(27)

(28)

式(27)中：Z是量测向量，H是量测矩阵，X是状态向量，V是量测噪声向量。

在SINS失效情况下，启用备用SINS/GC/VL组合导航系统。SINS输出航向、速度信息，GC输出航向信息，VL输出航速信息，这些数据分别作差并经KF2滤波得到航向误差和速度误差，并反馈修正SINS，系统最终输出修正后的导航结果。

结合式(23)和(25)，SINS/GC/VL组合导航系统的量测方程为

(29)

(30)

3 仿真实验

3.1 轨迹模拟与惯性器件数据的生成

利用轨迹发生器设计航行器航行轨迹，通过捷联惯导反演算，反推出航行器经该航迹产生的陀螺仪和加速度计数据，如图2～3所示。

图2 角速度值

图3 加速度值

3.2 仿真校验

考虑到SINS存在实际误差，设置SINS的标定误差参数，并进行仿真实验。SINS中，陀螺零偏为5(°)/h，加速度计零偏为200μg，角度随机游走系数为0.05(°)/h1/2，速度随机游走系数为5μg/Hz1/2。

为了探究有GC/VL辅助的GNSS/SINS在GNSS异常情况下的导航性能，分别进行三组实验并对比实验结果。

实验1：SINS单独导航，并记录位置、速度和姿态等导航结果；

实验2：SINS/GC/VL组合导航，并记录导航结果；

实验3：GNSS/SINS/GC/VL组合导航，并记录导航结果。

3种导航系统分别经过设计的轨迹，以实验3的导航结果为参考，实验1、2相对实验3的误差如图5～7所示。图4为3种导航系统的位置结果，图5为位置相对误差，图6为速度相对误差，图7为姿态相对误差。

图4 位置对比侧视图

图5 导航位置相对误差

图6 导航速度相对误差

图7 导航姿态相对误差

在图4中，以GNSS/SINS/GC/VL的位置结果作为参考(“○”标记)，SINS的位置结果(“+”标记)迅速偏移，SINS/GC/VL组合导航系统的位置结果(“△”标记)优于单独的SINS。

在图5和图6中，由于VL所提供的航速信息为水平方向上航行器的运动状态，SINS/GC/VL组合导航系统的速度和位置的水平误差明显小于SINS单独导航的，但在垂直方向上，两种导航系统的结果差异不大。

在图7中，由于GC所提供的航向信息被引入到导航系统，SINS/GC/VL组合导航系统的航向角误差明显小于SINS单独导航的，但俯仰角与横滚角的误差未得到显著改善。

4 结论

为解决GNSS/SINS松组合导航系统在GNSS失效时，导航结果失真的现象，本文提出一种有陀螺罗经和计程仪辅助的GNSS/SINS组合导航系统，根据引入的航向信息和航速信息构造两组滤波器，实现GNSS/SINS组合导航系统与陀螺罗经、计程仪的有效结合。根据SINS的误差模型，对GNSS/SINS/GC/VL组合导航系统进行仿真，结果表明，GNSS/SINS/GC/VL在GNSS暂时失效时，SINS/GC/VL的导航误差比单独SINS的导航误差小。此导航方法不仅减轻组合导航系统对GNSS的实时依赖性，还提高了航行器运动信息的利用率，是一种简单有效的导航信息融合方法。