基于卡尔曼滤波的旋转式惯性导航系统外水平阻尼方法与流程

本发明涉及一种外水平阻尼算法，特别是一种基于卡尔曼滤波的旋转式惯性导航系统外水平阻尼方法。

背景技术

以舰船为例，对于长时间航行的舰船来说，惯性测量组件引起的导航误差随着时间不断积累，约占系统误差的90％左右，严重影响了导航定位的准确性。而当采用单轴旋转调制技术后，惯性器件常值误差得到有效抑制，此时惯性器件随机误差就成为影响导航精度的主要因素。

对于一定时间内有外部信息进行综合校正的船舶来说，抑制单轴旋转调制格网惯性导航系统的舒勒振荡可以有效地提高船舶航行的导航精度。借鉴常规惯性导航系统，基于水平阻尼网络的传统的外水平阻尼技术即可有效抑制84.4min的舒勒周期振荡，同时，调制舒勒周期振荡的傅科振荡也可得到有效抑制。

无阻尼和外水平阻尼工作方式是船舶航行最常用的两类工作方式，因而船舶将在无阻尼与外水平阻尼两种工作状态下频繁切换。由于外部参考速度存在误差同时由于系统达到稳态的过程中存在着时间延迟现象，基于阻尼网络的传统的外水平阻尼算法在系统由无阻尼切换到外水平阻尼工作状态时存在超调误差，这将极大地影响船舶的实时导航精度。因此本发明的目的就是在船用单轴旋转调制格网惯性导航系统的基础上，设计一种基于卡尔曼(kalman)滤波的外水平阻尼算法来抑制工作状态切换过程中的超调误差，从而提高船舶在极区航行时的导航精度。

技术实现要素：

针对上述现有技术，本发明要解决的技术问题是提供一种能够解决船用单轴旋转调制格网惯性导航系统状态切换过程中存在的超调问题的基于卡尔曼滤波的旋转式惯性导航系统外水平阻尼方法。

为解决上述技术问题，本发明一种基于卡尔曼滤波的旋转式惯性导航系统外水平阻尼方法，包括以下步骤：

步骤一：多普勒计程仪提供外部参考速度；

步骤二：通过多普勒计程仪判据判断多普勒计程仪提供的外部参考速度是否可用；当步骤一中提供的外部参考速度不可用时，执行步骤三；当步骤一中提供的外部参考速度可用时，执行步骤四；

步骤三：导航系统切换到无阻尼状态下进行解算并输出导航信息，包括速度信息、位置信息和姿态信息；

步骤四：导航系统切换到基于卡尔曼滤波的旋转调制格网惯性导航系统外水平阻尼状态下进行结算，具体过程为：根据步骤一提供的外部参考速度和前一次解算过程得到的速度、姿态、位置信息，通过卡尔曼滤波器准确估计出无阻尼系统的速度误差、姿态误差和位置误差并对无阻尼系统进行反馈校正，进而解算得到新的导航信息。

本发明一种基于卡尔曼滤波的旋转式惯性导航系统外水平阻尼方法，还包括：

1.步骤二中通过dvl判据判断多普勒计程仪提供的外部参考速度是否可用具体过程为：

式中vgsins和vdvl分别为惯性导航系统和多普勒计程仪输出速度信息，δvgsins和δvdvl分别为惯性导航系统和多普勒计程仪的速度测量误差，vtrue为船舶航行的真实速度，取t1时刻和邻近时刻t2的滤波器观测值如下：

式中和分别为t1和t2时刻滤波器观测值，则t1和t2时刻滤波器观测值差值满足：

式中为邻近时刻格网惯性导航系统解算的速度误差的差值，为多普勒计程仪测速误差在邻近时刻的差值，如果t1和t2时刻滤波器观测值差值发生大的改变，则步骤一中提供的外部参考速度不可用，系统由外水平阻尼状态向无阻尼状态切换；如果t1和t2时刻滤波器观测值差值由剧烈变化状态向小幅变化状态变化，则步骤一中提供的外部参考速度可用，系统由无阻尼状态向外水平阻尼状态切换。

2.步骤四中卡尔曼滤波器满足：

系统误差状态方程和量测方程为：

式中，x＝[φgeφgnφguδvgeδvgnδxδyδzδvrgeδvrgn]^t，φgn、φge和φgu分别为格网姿态误差角；δvge和δvgn分别为格网东向速度误差和北向速度误差；δx、δy和δz分别为地心地固坐标系下三轴位置误差；δvrge和δvrgn分别为多普勒计程仪在格网东向和北向的速度测量误差；系统状态方程中a为状态方程系数矩阵，w为系统噪声序列；量测方程中选取z为量测值，h为量测矩阵，η为量测噪声序列，a、h分别为如下形式：

式中

上述子矩阵中(·×)表示向量的反对称矩阵，(·)m,n表示对矩阵取前m行n列，子矩阵cωgv、cωer、cωgr和cr的具体形式为：

其中r为地球半径，σ为格网方位角，l为纬度值，λ为经度值，ωie为地球相对惯性系的自转角速度，vgn为格网北向速度，vge为格网东向速度。

3.t1和t2时刻滤波器观测值差值发生大的改变指的幅值大于0.5m/s；t1和t2时刻滤波器观测值差值由剧烈变化状态向小幅变化状态变化指的幅值小于0.5m/s。

本发明的有益效果：本发明涉及的是一种基于卡尔曼滤波的旋转式惯性导航系统外水平阻尼方法，以船用单轴旋转调制格网惯性导航系统误差模型为基础，在传统的基于水平阻尼网络的外水平阻尼算法上提出了基于kalman滤波的外水平阻尼算法。本算法不仅可以有效抑制舒勒振荡误差和惯性器件误差，同时更可有效抑制系统在由无阻尼工作状态切换到阻尼工作状态时状态切换过程中所产生的超调误差，有效地保证了船舶在极区航行时的导航精度。

附图说明

图1为整体设计方案流程图；

图2为基于kalman滤波外水平阻尼算法原理图；

图3为基于kalman滤波外水平阻尼算法方块图；

图4为格网东向参考速度误；

图5为单通道北向外水平阻尼系统方块图；

图6为三种状态下格网姿态角误差；

图7为三种状态下格网水平速度误差；

图8为三种状态下地心地固坐标系(ecef坐标系)下位置误差；

具体实施方式

本发明目的在于解决船用单轴旋转调制格网惯性导航系统状态切换过程中存在的超调问题。在单轴旋转调制格网惯性导航解算的基础上，为抑制惯性器件随机误差对导航系统造成的不利影响，对于有外部信息进行定期校正的单轴旋转调制格网惯性导航系统，采用外水平阻尼算法对系统舒勒振荡误差进行抑制。综上，针对传统的基于阻尼网络的外水平阻尼算法在状态切换过程中存在的超调问题，本文提出了一种基于kalman滤波的外水平阻尼算法。

如图1所示，方法具体流程为:

(1)多普勒计程仪提供一个外部参考速度vdvl。

(2)通过dvl(多普勒计程仪)判据判断多普勒计程仪提供的外部参考速度是否可用。当外速度不可用时，导航系统切换到无阻尼状态下进行解算并输出相应的导航信息(包括船舶的速度、位置、姿态)。

(3)当外速度可用时，系统切换到基于kalman滤波的旋转调制格网惯性导航系统外水平阻尼状态下进行结算。在外水平阻尼状态下，导航系统不仅可以有效抑制舒勒振荡误差，还可有效抑制系统由无阻尼工作状态切换到阻尼工作状态时所产生的超调误差。具体过程如下：根据提供的外部参考速度信息vdvl与前一次解算过程得到的速度、姿态、位置信息，卡尔曼滤波器可以准确估计出无阻尼系统的水平速度误差水平姿态误差以及位置误差并对无阻尼系统进行反馈校正，进而解算得到更精确的导航信息(包括船舶的速度、位置、姿态)。

基于最优估计和反馈校正理论，运用kalman滤波进行单轴旋转调制格网惯性导航系统外水平阻尼算法的原理图如附图2所示。当外部设备如多普勒计程仪提供的外部参考速度可用时，在外速度辅助下，单轴旋转调制格网惯导系统的格网水平姿态误差和速度误差可以被准确估计。显然，通过反馈校正估计出的格网水平姿态误差以及水平速度误差可有效地提高单轴旋转调制格网惯导系统的定位、定速和定姿精度。

本发明提出的设计方法为：

(1)dvl测量速度可用性判据判断外速度是否可用，当外速度不可用时系统工作在无阻尼状态。当外速度可用时系统工作在外水平阻尼状态，即本发明设计的基于kalman滤波的外水平阻尼系统状态。

(2)kalman滤波器的设计：单轴旋转调制技术仅抑制了惯性器件常值误差对导航系统的不利影响，而其他误差的传播规律仍与格网惯性导航系统一致，因而，单轴旋转调制格网惯性导航系统误差模型与格网惯导系统误差模型一致。本文在机载格网惯性导航系统误差模型基础上，忽略高速通道的影响，使之满足船。舶导航需求。系统误差状态方程和量测方程为：

式中，x＝[φgeφgnφguδvgeδvgnδxδyδzδvrgeδvrgn]^t，φgn、φge和φgu分别为格网姿态误差角；δvge和δvgn分别为格网东向速度误差和北向速度误差；δx、δy和δz分别为地心地固坐标系(ecef坐标系)下三轴位置误差；δvrge和δvrgn分别为多普勒计程仪在格网东向和北向的速度测量误差。系统状态方程中a为状态方程系数矩阵，w为系统噪声序列；量测方程中选取z为量测值，h为量测矩阵，η为量测噪声序列。a、h分别为如下形式：

式中

上述子矩阵中(·×)表示向量的反对称矩阵，(·)m,n表示对矩阵取前m行n列。子矩阵cωgv、cωer、cωgr和cr的具体形式参考文献《极区飞行间接格网惯性导航算法》，子矩阵cωgv、cωer、cωgr和cr的具体形式如下：

其中r为地球半径，σ为格网方位角，l为纬度值，λ为经度值，ωie为地球相对惯性系的自转角速度，vgn为格网北向速度，vge为格网东向速度。

附图3给出了基于kalman滤波的外水平阻尼算法方块图，图中分别为滤波器估计出的格网东向姿态误差角和格网北向速度误差。当多普勒计程仪提供的外部速度包含常值误差时，仅格网水平姿态误差角和格网水平速度误差可以被准确估计出来，而ecef坐标系下的位置误差无法被准确估计。采取反馈校正方式不仅可以有效抑制格网姿态误差和格网水平速度误差，同时由于惯性导航系统若闭环系统的本质，上述两者的抑制作用也将对位置误差产生一定的抑制作用。

(3)dvl测量速度可用性判据：

多普勒计程仪的测速误差通常可以被视为阶段性常值。然而当船舶做复杂的机动运动或当船舶航行到海湾或海沟等复杂的海洋环境当中时，多普勒计程仪的测速误差通常很大，此时认为计程仪测得的速度不可用。这样建立准确的判据来判断外部速度的可用性就很有必要。

式中vgsins和vdvl分别为惯性导航系统和多普勒计程仪输出速度信息，δvgsins和δvdvl分别为惯性导航系统和多普勒计程仪的速度测量误差，vtrue为船舶航行的真实速度。取t1时刻和邻近时刻t2的滤波器观测值如下：

式中和分别为t1和t2时刻滤波器观测值。对t1和t2时刻的观测值做差得到：

式中等号右侧第一项为邻近时刻格网惯性导航系统解算的速度误差的差值，第二项为多普勒计程仪测速误差在邻近时刻的差值。惯性导航系统一个重要的特点即是信息的连续性，显然第一项在短时间内不会发生大的变化，因而若邻近两个时刻的观测量发生大的改变(的幅值大于0.5m/s时)，如为图4的前5个小时表现的形式时，则发生大的改变的根本原因在于多普勒计程仪的测速误差发生了大的改变。因而若邻近时刻滤波器观测量差值发生大的改变，则系统由外水平阻尼状态向无阻尼状态切换；若邻近时刻滤波器观测量差值由剧烈变化状态向小幅变化状态变化(的幅值小于0.5m/s时)，如为图4的后5个小时表现的形式时，则系统由无阻尼状态向外水平阻尼状态切换。

为对比两类阻尼算法在阻尼状态切换过程中的性能，对多普勒计程仪提供的外部参考速度的速度误差设置如下：t≤5h时，外部参考速度误差设置为大幅值白噪声，此时认为外部参考速度不可用，系统工作在无阻尼状态；t＞5h时，外部参考速度包含常值误差(1nmile)，此时认为外部参考速度可用，系统工作在阻尼工作状态下。以格网东向为例，外部参考速度误差如图4所示。

以下描述本发明的实施例：

为验证本文提出的基于kalman滤波的外水平阻尼算法对单轴旋转调制格网惯性导航系统随机误差抑制性能的优势，在相同的仿真条件下同时对单轴旋转调制格网惯性导航系统解算及针对其设计的传统外水平阻尼算法(如附图5)和基于kalman外水平阻尼算法进行仿真测试并进行对比。仿真条件设置如下：陀螺仪和和加速度计输出由轨迹发生器产生，仿真时长10h；陀螺常值漂移和加速度计常值零偏分别设置为：0.01°/h和10^-4g，陀螺随机漂移和加速度计随机零偏设置为白噪声；转位机构每秒旋转10°；船舶航行初始位置信息设置为：纬度85°n，经度18°e；船舶沿85°n纬度圈以10m/s的速度做匀速运动；初始姿态误差角设置为：格网纵摇角误差6”，格网横摇角误差6”，格网航向角误差6'；船舶摇摆设置为下式所示的正弦函数：

式中，pitch、roll和yaw分别表示船舶摇摆的纵摇角、横摇角和航向角；姿态角摇摆幅度pitchm、rollm和yawm分别设置为3°、5°和0°；姿态角摇摆周期tp、tr和ty分别设置为3s、4s和5s；姿态角摇摆初始相位和分别设置为随机值。

附图6、7、8表明，t≤5h时，三种算法均工作于无阻尼工作状态，三类算法仿真结果一致。t＞5h系统根据判据规则切换到外水平阻尼工作状态，由仿真结果可知：一方面，所提出的基于kalman滤波的外水平阻尼算法与传统的外水平阻尼算法类似，均可以达到有效抑制系统舒勒振荡的目的，亦即单轴旋转调制格网惯性导航系统由于惯性器件随机误差造成的导航误差得到了有效抑制；另一方面，与传统的外水平阻尼算法相比，当系统工作状态由无阻尼工作状态切换到外水平阻尼工作状态时，所提出的基于kalman滤波的外水平阻尼算法可有效抑制状态切换过程中产生的超调，有效地保证了在系统状态切换过程中导航系统输出参数的可靠性，极大的提高了船舶在极区航行时的导航精度。

本发明涉及的是一种基于kalman滤波的旋转调制格网惯性导航系统外水平阻尼算法，以船用单轴旋转调制格网惯性导航系统误差模型为基础，在传统的基于水平阻尼网络的外水平阻尼算法上提出了基于kalman滤波的外水平阻尼算法，本算法不仅可以有效抑制舒勒振荡误差，还可有效抑制系统由无阻尼工作状态切换到阻尼工作状态时所产生的超调误差，提高单轴旋转格网惯导系统的定位、定速和定姿精度，保证了船舶在极区航行时的导航精度。

本发明的主要设计内容：(1)kalman滤波器的设计，与单轴旋转调制格网惯性导航系统构成基于kalman滤波的旋转调制格网惯性导航系统外水平阻尼系统(2)通过dvl判据判断外速度是否可用，使得系统工作状态可以在无阻尼和外水平阻尼之间进行切换。(3)当系统工作在外水平阻尼状态时，根据单轴旋转调制格网惯性导航系统提供的计算输出值和多普勒计程仪提供的外参速度，kalman滤波器进行误差估计，并对单轴旋转调制格网惯性导航系统进行反馈校正，使得系统输出更加精准的速度、位置、姿态信息。