CN112254718B - 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法 - Google Patents

Abstract

本发明提供了一种运动约束辅助的基于改进Sage‑Husa自适应滤波的水下组合导航方法，包括：建立多普勒计程仪误差模型；构建基于卡尔曼滤波的水下组合导航系统的状态方程；根据水下潜航器向心加速度和前向速度间的关系建立约束条件，构建完整运动约束模型；建立两个量测方程；建立滤波方程，在水下滑翔器正常行驶时采用标准卡尔曼滤波算法进行解算，当量测噪声发生变化时，采用改进的Sage‑Husa自适应滤波算法进行时间更新、量测更新和滤波更新。本发明能够提高水下组合导航系统的滤波精度，较好地抑制量测干扰时的滤波发散，具有较好的鲁棒性和可靠性。

Description

一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下 组合导航方法

技术领域

本发明属于导航制导和控制领域，涉及水下导航方法，具体涉及一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法。

背景技术

随着水下潜器技术愈渐成熟，水下潜航器作为可靠探测工具受到了国内外广泛关注，如何提高水下导航精度也成为各领域学者关注的焦点。目前应用最为成熟也最为广泛的是惯性/多普勒组合导航技术。惯性器件独立自主，不依靠外界信息的特性使其在水下环境依然具有较高可靠性，但惯性器件的误差会随着时间发散，从而影响定位的精度。而多普勒计程仪精度较高，误差不随着时间发散，因此可以很好的修正捷联惯导的速度信息，提高定位的精度。

但水下环境较为复杂，水下潜航器通常在海底以锯齿波滑翔，在滑翔过程中，若遇到海底沟渠、聚集性鱼群以及强机动性转弯时，多普勒计程仪提供的量测信息则会发生错误，并且预先设置的量测噪声矩阵将无法适应受到扰动的模型，从而影响定位的精度，而当水下潜航器以锯齿波滑翔或进行强机动性转弯时，所产生的向心加速度将会对水下潜航器的速度产生约束。

发明内容

为解决上述问题，本发明引入基于向心加速度约束以及改进的Sage-Husa自适应算法来辅助惯性/多普勒组合导航，提出一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法，在水下潜航器因为海底沟渠、鱼群以及强机动性转弯时导致多普勒量测信息出现错误时，引入向心加速度对水下潜航器的速度进行约束，并且对Sage-Husa自适应滤波算法进行改进，从而减小定位误差，提高定位精度。

为了达到上述目的，本发明提供如下技术方案：

一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法，包括如下步骤：

步骤1，根据多普勒计程仪的工作原理，建立多普勒计程仪误差模型；

步骤2，在惯性导航误差方程的基础上，引入多普勒误差模型中速度误差、偏流角误差以及比例系数误差作为水下组合导航的状态量构建基于卡尔曼滤波的水下组合导航系统的状态方程；

步骤3，建立载体坐标系，将水下潜航器的运动分解到垂直于Z轴平面和垂直于X轴平面，并根据水下潜航器向心加速度和前向速度间的关系建立约束条件，构建完整运动约束模型；

步骤4，根据惯性器件以及多普勒计程仪的导航信息建立第一量测方程，并根据步骤2中完整运动约束模型建立第二量测方程；

步骤5，对状态方程以及量测方程进行离散化处理，结合步骤2和步骤4建立滤波方程，在水下滑翔器正常行驶时采用标准卡尔曼滤波算法进行解算，当量测噪声发生变化时，采用改进的Sage-Husa自适应滤波算法进行时间更新、量测更新和滤波更新。

进一步的，所述步骤1中多普勒计程仪的工作原理为：

水下潜航器底端安装前后左右各两对换能器，向四个方向发射波束，通过测量发射波束的频率以及反射后波束频率得到多普勒频移，从而得到载体坐标系三个方向上水下潜航器的速度如下：

f_d13＝f_d1-f_d3,f_d24＝f_d2-f_d4

所述多普勒计程仪误差模型为：

其中，v_d为水下潜航器的前向速度，c为光速，v_x、v_y、v_z为潜航器载体坐标系三个方向的速度；f₀为发射波频率，f_d1、f_d2、f_d3、f_d4为安装在潜航器船底前后左右四个方向的发射器发射波束的多普勒频移；α为发射波束倾角；δv_dU、δv_dE、δv_dN为东北天坐标系下潜航器三个方向的速度误差；δv_d为多普勒测速误差；β为潜航器俯仰角；δC为比例因子误差；K_d、γ以及δΔ分别为潜航器考虑偏流角的航迹向、方位失准角以及偏流角误差。

进一步的，所述步骤2中基于卡尔曼滤波的水下组合导航系统的状态方程为：

X_k＝A_k,k-1X_k-1+Γ_k,k-1W_k-1

其中，A_k,k-1表示系统从k-1时刻到k时刻的状态转移矩阵；Γ_k,k-1表示系统的噪声驱动矩阵，W_k-1为系统噪声激励序列；

相应的状态向量为：

其中，δv_d、δΔ、δC由如下公式表示：

式中δL、δλ、δh表示载体经度、纬度以及高度误差；δv_E、δv_N、δv_U为载体在东、北、天三个方向上的速度误差；α、β、γ为载体的姿态角误差；ε_x、ε_y、ε_y为陀螺仪零偏； 表示加计零偏；β_d ^-1、β_Δ ^-1为速度偏移误差的相关时间和偏流角误差的相关时间；ω_d、ω_Δ均为激励白噪声。

进一步的，所述步骤3具体包括如下过程：

假设潜航器垂直于前向速度的两个方向速度只与海水流速有关且假设流速为0，可得约束条件：

潜航器的任意运动都分解到垂直于z轴平面与垂直于x轴平面，根据运动学公式，可得：

根据惯性器件可得：

由此可得完整的运动约束模型为：

从而得到完整运动约束的误差模型为：

其中，为潜航器在载体坐标系下的速度；a_rx、a_rz为潜航器载体坐标系中x和z方向的向心加速度值；/>为惯性器件x轴和z轴敏感角速度；f_x ^b、f_z ^b为加计x、z轴比力值；/>分别为地球自转角速度和由载体运动引起的角速度。

进一步的，所述步骤4中第一量测方程为：

第二量测方程为：

其中量测量Z₁为惯导解算出的速度与多普勒计程仪测速之差，量测量Z₂为潜航器两个方向的速度约束以及载体运动时的加速度约束。

进一步的，所述步骤5中状态空间模型包括：

X_k＝A_k,k-1X_k-1+Γ_k,k-1W_k-1

Z_k＝H_kX_k+V_k

其中，Z_k为k时刻传感器的观测向量；H为系统从状态空间到观测空间的转换矩阵,V_k为量测噪声序列；

系统的状态转移矩阵A为：

其中分别代表惯性导航系统的状态转移矩阵、陀螺漂移、加计误差的反相关时间矩阵以及多普勒计程仪误差反相关矩阵；

系统的量测矩阵为：

H₁＝[0_3×3 I_3×3 S₁ 0_3×6 S₂]

其中：

H₂＝[0_3×4 M₁ M₂ M₃ M₄ 0_3×4]

假设：

则：

其中：

进一步的，所述步骤5中改进的Sage-Husa自适应滤波算法过程如下：

X_k＝X_k,k-1+K_k(Z_k-H_kX_k,k-1)

P_k＝(I-K_kH_k)P_k,k-1

其中，X_k表示载体k时刻的状态变量，A表示系统从k时刻到k+1时刻的状态转移矩阵；Z_k为k时刻传感器的观测向量；H为系统从状态空间到观测空间的转换矩阵；K_k为k时刻的卡尔曼滤波增益；Q为系统噪声协方差矩阵；R为观测协方差矩阵；P为误差协方差矩阵。

进一步的，所述β_k通过如下过程推导：

在卡尔曼滤波中，量测预测误差公式为：

两边同时求方差可得：

利用指数减小记忆加权平均方法可得：

判断滤波是否发散，滤波正常，则按照一般滤波进行，若检测到滤波发散，则实时计算出最优β_k，阻止滤波的发散；根据下式判断滤波是否发散：

当上式成立时，则代表滤波发散，其中γ为储备系数，当γ＝1时是最严格的收敛判据，采用最严格的收敛判据可得：

进一步的，所述步骤5中，当量测噪声发生变化的情况包括以下情形中的至少一种：水下滑翔器遇到障碍物或强机动性转弯。

进一步的，所述障碍物包括水下沟渠、鱼群。

与现有技术相比，本发明具有如下优点和有益效果：

本发明方法引入水下潜航器拐弯或进行强机动动作时产生的向心加速度误差为量测量，相比于传统的运动约束更为完整，可以有效约束载体的前向速度，避免载体因为强机动运动导致的量测信息出现较大误差，有效提高了系统的导航精度。本发明提出的改进的Sage-Husa算法，在传统的Sage-Husa自适应滤波基础上，增加了对量测信息的故障判断以及比例因子的最优估计，不仅可以减小量测信息正确时的滤波计算量，还可以在量测信息出现误差时减少系统滤波的发散，具有较好的鲁棒性和可靠性，从而提高系统的导航精度。本发明方法可以在多普勒量测信息发生错误时有效减小误差，可用于提高水下组合导航精度、定位精度以及导航系统容错性，实现水下潜航器更精确的作业。

附图说明

图1为多普勒计程仪的工作原理图。

图2为多普勒计程仪误差模型示意图。

图3为水下潜航器运动分解示意图。

图4为运动约束辅助的基于改进Sage-Husa自适应滤波算法流程图。

图5为标准卡尔曼滤波算法下导航系统的位置及速度误差图。

图6为运动约束辅助的基于改进Sage-Husa自适应滤波算法下导航系统的位置及速度误差图。

图7为量测噪声受到干扰时，运动约束辅助的Sage-Husa自适应算法下导航系统的位置及速度误差图。

图8为量测噪声受到干扰时，运动约束辅助的改进Sage-Husa自适应算法下导航系统的位置及速度误差图。

具体实施方式

以下将结合具体实施例对本发明提供的技术方案进行详细说明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。

本发明提出一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法，实现原理及方法流程如图1-图4所示，具体步骤如下：

步骤1，根据多普勒计程仪的工作原理，建立多普勒计程仪误差模型：

多普勒计程仪的原理较为简单，如图1所示，水下潜航器底端安装前后左右各两对换能器，向四个方向发射波束，通过测量发射波束的频率以及反射后波束频率得到多普勒频移，从而得到载体坐标系三个方向上水下潜航器的速度如下：

f_d13＝f_d1-f_d3,f_d24＝f_d2-f_d4

相应的，得到多普勒计程仪的误差模型为：

其中，v_d为水下潜航器的前向速度，c为光速，为潜航器在载体坐标系下的速度；f₀为发射波频率，f_d1、f_d2、f_d3、f_d4为安装在潜航器船底前后左右四个方向的发射器发射波束的多普勒频移；α为发射波束倾角；δv_dU、δv_dE、δv_dN为东北天坐标系下潜航器三个方向的速度误差；δv_d为多普勒测速误差；β为潜航器俯仰角；δC为比例因子误差；K_d、γ以及δΔ分别为潜航器考虑偏流角的航迹向、方位失准角以及偏流角误差。

步骤2，在惯性导航误差方程的基础上，引入多普勒误差模型中速度误差δv_d、偏流角误差δΔ以及比例系数误差δC作为水下组合导航的状态量，如图2所示，构建基于卡尔曼滤波的水下组合导航系统的状态方程：

其中，运动约束辅助的基于改进Sage-Husa自适应滤波算法的水下组合导航系统的状态方程为：

X_k＝A_k,k-1X_k-1+Γ_k,k-1W_k-1

其中，A_k,k-1表示系统从k-1时刻到k时刻的状态转移矩阵；Γ_k,k-1表示系统的噪声驱动矩阵，W_k-1为系统噪声激励序列。相应的状态向量为：

其中，δv_d、δΔ、δC可由如下公式表示：

式中δL、δλ、δh表示载体经度、纬度以及高度误差；δv_E、δv_N、δv_U为载体在东、北、天三个方向上的速度误差；α、β、γ为载体的姿态角误差；ε_x、ε_y、ε_y为陀螺仪零偏； 表示加计零偏；β_d ^-1、β_Δ ^-1为速度偏移误差的相关时间和偏流角误差的相关时间；ω_d、ω_Δ均为激励白噪声。

步骤3，水下潜航器的任意运动皆可以分解到相互垂直的两个平面上，建立载体坐标系，将水下潜航器的运动分解到垂直于Z轴平面和垂直于X轴平面，如图3所示，并根据水下潜航器向心加速度和前向速度间的关系建立约束条件，构建完整运动约束模型：

由于海底一定深度的水流速较为稳定，因此假设潜航器垂直于前向速度的两个方向速度只与海水流速有关且假设流速为0，可得约束条件：

该约束条件只能约束潜航器两个方向的速度，而潜航器在拐弯或进行强机动运动时会产生向心加速度，根据运动学公式，可得：

根据惯性器件可得：

由此可得完整的运动约束模型为：

从而得到完整运动约束的误差模型为：

其中，为从导航坐标系到载体坐标系的转移矩阵，/>表示导航坐标系下的姿态角。vⁿ为导航坐标系下潜航器的速度，gⁿ为导航坐标系下的重力加速度，/>为载体坐标系下陀螺仪x轴和z轴的零偏，a_rx、a_rz为潜航器载体坐标系中x和z方向的向心加速度值；/> 为惯性器件x轴和z轴敏感角速度；f_x ^b、f_z ^b为加速度计x、z轴比力值；/>分别为地球自转角速度和由载体运动引起的角速度。

步骤4，根据惯性器件以及多普勒计程仪的导航信息建立量测方程(1)，并根据完整运动约束模型建立量测方程(2)：

式中，v_E，v_N、v_U为惯导解算出的潜航器的东北天方向(本发明中选用东北天坐标系为导航坐标系)的速度。v_dE、v_dN、v_dU为多普勒计程仪所测得经过坐标转换后东北天三个方向的速度。量测量Z₁为惯导解算出的速度与多普勒计程仪测速之差，量测量Z₂为潜航器两个方向的速度约束以及载体运动时的加速度约束，在潜航器正常工作时，量测量Z₂的值应为0或为白噪声。

步骤5，对状态方程以及量测方程进行离散化处理，结合步骤2和步骤4建立滤波方程，在水下滑翔器正常行驶时采用标准卡尔曼滤波算法进行解算，当水下滑翔器遇到水下沟渠、鱼群或强机动性转弯等情况时，量测噪声发生变化时，采用改进的Sage-Husa自适应滤波算法进行时间更新、量测更新和滤波更新，具体流程如图4所示：

结合步骤1-步骤4，系统的状态空间模型包括状态方程和量测方程，具体如下：

X_k＝A_k,k-1X_k-1+Γ_k,k-1W_k-1

Z_k＝H_kX_k+V_k

其中，Z_k为k时刻传感器的观测向量；H为系统从状态空间到观测空间的转换矩阵,V_k为量测噪声序列；

系统的状态转移矩阵A为：

其中分别代表惯性导航系统的状态转移矩阵、陀螺漂移、加计误差的反相关时间矩阵以及多普勒计程仪误差反相关矩阵。

系统的量测矩阵为：

H₁＝[0_3×3 I_3×3 S₁ 0_3×6 S₂]

其中：

H₂＝[0_3×4 M₁ M₂ M₃ M₄ 0_3×4]

假设：

则：

其中：

在水下组合导航系统中，通常系统状态噪声是较为稳定的，因此本发明只对量测噪声进行自适应。在卡尔曼滤波中，量测预测误差公式为：

两边同时求方差可得：

利用指数减小记忆加权平均方法可得：

传统Sage-Husa滤波中认为：

其中b为渐消因子，但由于随着滤波次数k的增大，b_k会趋近0，自适应滤波的权重会趋近于1-b，并将保持不变；同时，初始值对/>的分配权值逐渐衰减，并逐渐接近于常值0。上述原因使得噪声估计器的自适应程度降低，滤波的精度便会随之下降。

根据预报残差法，可以人为判断滤波是否发散，滤波正常，则按照一般滤波进行，若检测到滤波发散，则实时计算出最优β_k，阻止滤波的发散。

滤波发散判据：

当上式成立时，则代表滤波发散，其中γ为储备系数，当γ＝1时是最严格的收敛判据，采用最严格的收敛判据可得：

将取代式中R_k可得：

解得：

综上：

由此可得步骤5中基于改进Sage-Husa自适应滤波算法过程如下：

X_k＝X_k,k-1+K_k(Z_k-H_kX_k,k-1)

P_k＝(I-K_kH_k)P_k,k-1

其中，X_k表示载体k时刻的状态变量，A表示系统从k时刻到k+1时刻的状态转移矩阵；Z_k为k时刻传感器的观测向量；H为系统从状态空间到观测空间的转换矩阵；K_k为k时刻的卡尔曼滤波增益；Q为系统噪声协方差矩阵；R为观测协方差矩阵；P为误差协方差矩阵。

具体实施例

为了验证提出算法的正确性，这里提供了一个基于Matlab平台的仿真测试，仿真参数设置如下：

1.惯性元件指标及导航初始参数设置：

陀螺零偏稳定性：eb＝0.2°/h

加计零偏稳定性：db＝100ug

角度随机游走：

DVL速度偏移误差：

DVL偏流角误差：

DVL刻度系数误差：

2.误差分析

结合上述参数，分别采用标准卡尔曼滤波算法和本发明提供的运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法进行对比。图5给出了标准卡尔曼滤波算法下组合系统的位置误差及速度误差，可以看出，标准卡尔曼算法在实验解算初期缺乏对系统统计噪声特性的了解，滤波误差较大。附加运动约束的改进Sage-Husa自适应滤波算法对载体的三个方向速度进行约束，对量测噪声进行自适应，可以很好的抑制组合系统误差的发散，也大大提高了系统的导航精度，如图6所示。

为了验证滤波发散情况下改进自适应滤波的性能，在量测量600秒到610秒处加了十倍的量测噪声。图7和图8给出了Sage-Husa自适应滤波算法改进前后系统的导航误差情况，经过改进的Sage-Husa自适应滤波算法可以很好的抑制滤波发散，提高导航系统的精度及鲁棒性。

本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段，还包括由以上技术特征任意组合所组成的技术方案。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。

Claims (9)

1.一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法，其特征在于，包括如下步骤：

步骤1，根据多普勒计程仪的工作原理，建立多普勒计程仪误差模型；

步骤2，在惯性导航误差方程的基础上，引入多普勒误差模型中速度误差、偏流角误差以及比例系数误差作为水下组合导航的状态量构建基于卡尔曼滤波的水下组合导航系统的状态方程；

步骤3，建立载体坐标系，将水下潜航器的运动分解到垂直于载体坐标系Z轴平面和垂直于载体坐标系X轴平面，并根据水下潜航器向心加速度和前向速度间的关系建立约束条件，构建完整运动约束模型；具体包括如下过程：

假设潜航器垂直于前向速度的两个方向速度只与海水流速有关且假设流速为0，可得约束条件：

潜航器的任意运动都分解到垂直于z轴平面与垂直于x轴平面，根据运动学公式，可得：

根据惯性器件可得：

由此可得完整的运动约束模型为：

从而得到完整运动约束的误差模型为：

其中，为潜航器在载体坐标系下的速度；/>为从导航坐标系到载体坐标系的转移矩阵，/>表示导航坐标系下的姿态角，vⁿ为导航坐标系下潜航器的速度，gⁿ为导航坐标系下的重力加速度，/>为载体坐标系下陀螺仪x轴和z轴的零偏，a_rx、a_rz为潜航器载体坐标系中x和z方向的向心加速度值；/>为惯性器件x轴和z轴敏感角速度；f_x ^b、f_z ^b为加计x、z轴比力值；/>分别为地球自转角速度和由载体运动引起的角速度；

步骤4，根据惯性器件以及多普勒计程仪的导航信息建立第一量测方程，并根据步骤2中完整运动约束模型建立第二量测方程；

步骤5，对状态方程以及量测方程进行离散化处理，结合步骤2和步骤4建立滤波方程，在水下滑翔器正常行驶时采用标准卡尔曼滤波算法进行解算，当量测噪声发生变化时，采用改进的Sage-Husa自适应滤波算法进行时间更新、量测更新和滤波更新。

2.根据权利要求1所述的运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法，其特征在于：所述步骤1中多普勒计程仪的工作原理为：

水下潜航器底端安装前后左右各两对换能器，向四个方向发射波束，通过测量发射波束的频率以及反射后波束频率得到多普勒频移，从而得到载体坐标系三个方向上水下潜航器的速度如下：

f_d13＝f_d1-f_d3,f_d24＝f_d2-f_d4

所述多普勒计程仪误差模型为：

其中，v_d为水下潜航器的前向速度，c为光速，为潜航器在载体坐标系下的速度；f₀为发射波频率，f_d1、f_d2、f_d3、f_d4为安装在潜航器船底前后左右四个方向的发射器发射波束的多普勒频移；α为发射波束倾角；δv_dU、δv_dE、δv_dN为东北天坐标系下潜航器三个方向的速度误差；δv_d为多普勒测速误差；β为潜航器俯仰角；δC为比例因子误差；K_d、γ以及δΔ分别为潜航器考虑偏流角的航迹向、方位失准角以及偏流角误差。

3.根据权利要求1所述的运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法，其特征在于，所述步骤2中基于卡尔曼滤波的水下组合导航系统的状态方程为：

X_k＝A_k,k-1X_k-1+Γ_k,k-1W_k-1

其中，A_k,k-1表示系统从k-1时刻到k时刻的状态转移矩阵；Γ_k,k-1表示系统的噪声驱动矩阵，W_k-1为系统噪声激励序列；

相应的状态向量为：

其中，δv_d、δΔ、δC由如下公式表示：

式中δL、δλ、δh表示载体经度、纬度以及高度误差；δv_E、δv_N、δv_U为载体在东、北、天三个方向上的速度误差；α、β、γ为载体的姿态角误差；ε_x、ε_y、ε_y为陀螺仪零偏； 表示加计零偏；β_d、β_Δ为速度偏移误差的相关时间和偏流角误差的相关时间；ω_d、ω_Δ均为激励白噪声。

4.根据权利要求1所述的运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法，其特征在于，所述步骤4中第一量测方程为：

第二量测方程为：

其中，v_E，v_N、v_U为惯导解算出的潜航器的东北天方向的速度，v_dE、v_dN、v_dU为多普勒计程仪所测得经过坐标转换后东北天三个方向的速度，量测量Z₁为惯导解算出的速度与多普勒计程仪测速之差，量测量Z₂为潜航器两个方向的速度约束以及载体运动时的加速度约束。

5.根据权利要求1所述的运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法，其特征在于，所述步骤5中状态空间模型包括：

X_k＝A_k,k-1X_k-1+Γ_k,k-1W_k-1

Z_k＝H_kX_k+V_k

其中，Z_k为k时刻传感器的观测向量；H为系统从状态空间到观测空间的转换矩阵,V_k为量测噪声序列；

系统的状态转移矩阵A为：

其中分别代表惯性导航系统的状态转移矩阵、陀螺漂移、加计误差的反相关时间矩阵以及多普勒计程仪误差反相关矩阵；

系统的量测矩阵为：

H₁＝[0_3×3 I_3×3 S₁ 0_3×6 S₂]

其中：

H₂＝[0_3×4 M₁ M₂ M₃ M₄ 0_3×4]

假设：

则：

其中：

其中，β为潜航器俯仰角，K_d为潜航器考虑偏流角的航迹向，v_E，v_N、v_U为惯导解算出的潜航器的东北天方向的速度。

6.根据权利要求1所述的运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法，其特征在于，所述步骤5中改进的Sage-Husa自适应滤波算法过程如下：

X_k＝X_k,k-1+K_k(Z_k-H_kX_k,k-1)

P_k＝(I-K_kH_k)P_k,k-1

其中，X_k表示载体k时刻的状态变量，A表示系统从k时刻到k+1时刻的状态转移矩阵；Z_k为k时刻传感器的观测向量；H为系统从状态空间到观测空间的转换矩阵；K_k为k时刻的卡尔曼滤波增益；Q为系统噪声协方差矩阵；R为观测协方差矩阵；P为误差协方差矩阵。

7.根据权利要求6所述的运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法，其特征在于，所述β_k通过如下过程推导：

在卡尔曼滤波中，量测预测误差公式为：

两边同时求方差可得：

利用指数减小记忆加权平均方法可得：

判断滤波是否发散，滤波正常，则按照一般滤波进行，若检测到滤波发散，则实时计算出最优β_k，阻止滤波的发散；根据下式判断滤波是否发散：

当上式成立时，则代表滤波发散，其中γ为储备系数，当γ＝1时是最严格的收敛判据，采用最严格的收敛判据可得：

8.根据权利要求1所述的运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法，其特征在于，所述步骤5中，当量测噪声发生变化的情况包括以下情形中的至少一种：水下滑翔器遇到障碍物或强机动性转弯。

9.根据权利要求8所述的运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法，其特征在于，所述障碍物包括水下沟渠、鱼群。