CN109443379B - 一种深海潜航器的sins/dvl水下抗晃动对准方法 - Google Patents

Abstract

本发明公开了一种深海潜航器的SINS(捷联惯导系统)/DVL(多普勒测速仪)水下抗晃动对准方法。本发明的方法为根据水下复杂环境和捷联惯导、四波束水下多普勒导航系统特点所建立的大失准角非线性误差模型以及精对准滤波方程，将深海潜航器的水下对准过程分为：SINS抗晃动双矢量定姿自对准和SINS/DVL的基于SVD(奇异值)分解的模糊自适应鲁棒CKF(容积卡尔曼滤波)滤波器对准；在保证鲁棒性的基础上，进一步使姿态失准角减小、使得姿态矩阵

更为精确，实现深海潜航器的准确定姿。本发明可以克服晃动基座，大失准角条件下的深海潜航器在初始对准时，传统惯导对准算法所导致的定姿速度、精度降低以至于潜航器无法继续正常工作的问题，最终实现深海潜航器的长航时工作。

Description

一种深海潜航器的SINS/DVL水下抗晃动对准方法

技术领域：

本发明涉及一种深海潜航器的SINS/DVL水下抗晃动对准方法，属于导航技术领域。

背景技术：

海洋有着丰富的资源和能源。在开发过程中，深海潜航器在诸多方面起着重要作用，如海洋环境的监测、水下目标的识别。而深海潜航器所具有的独特驱动和运动性能，使得其与其他无人水下航行器相比具有较大的性能优势。但大深度、远距离、长时间的水下航行对初始对准的精度要求高。然而水下复杂的洋流涌动环境以及鱼群冲击的可能性，会造成晃动基座，大失准角情况下，深海潜航器的初始对准速度、精度降低,从而使得潜航器惯导水下定位解算的累积误差进一步增大。

因此如何保证深海潜航器的快速精确定姿，成为制约深海潜航器长航时工作的重要因素。

同时由于晃动基座的特殊性，惯导线性误差模型无法保证滤波的精度满足要求，因此考虑采用更为精准的非线性误差模型和非线性滤波器完成水下DVL辅助惯导的精对准过程。

发明内容

本发明的目的是提供一种深海潜航器的SINS(捷联惯性导航系统)/DVL(多普勒测速仪)水下抗晃动对准方法，通过SINS抗晃动双矢量定姿自对准(粗对准)和SINS/DVL的基于SVD(奇异值)分解的模糊自适应鲁棒CKF(容积卡尔曼滤波)滤波器对准(精对准) 来保证深海潜航器准确定姿的鲁棒性和快速性，以克服晃动基座，大失准角条件下的深海潜航器在初始对准时，传统惯导对准算法所导致的定姿速度、精度降低以至于潜航器无法继续正常工作的问题，最终实现深海潜航器的长航时工作。

上述的目的通过以下技术方案实现：

一种深海潜航器的SINS/DVL水下抗晃动对准方法，该方法包括如下步骤：

(1)根据水下复杂环境和捷联惯导、四波束水下多普勒导航系统特点所建立的大失准角非线性误差模型以及精对准滤波方程，将深海潜航器的水下对准过程分为：SINS抗晃动双矢量定姿自对准和SINS/DVL的基于SVD分解的模糊自适应鲁棒CKF滤波器对准；

(2)SINS抗晃动双矢量定姿自对准在选定重力矢量为主参考矢量的前提下，预先对参与姿态解算的矢量作单位正交化处理；

(3)SINS/DVL的基于SVD分解的模糊自适应鲁棒CKF滤波器对准则在保证鲁棒性的基础上，进一步使姿态失准角减小、使得姿态矩阵

更为精确，实现深海潜航器的准确定姿。

所述的深海潜航器的水下抗晃动对准方法，步骤(1)中所述的根据水下复杂环境和捷联惯导、四波束水下多普勒导航系统特点所建立的大失准角非线性误差模型以及精对准滤波方程的具体方法是：

选取东北天地理坐标系作为导航坐标系，记为n系，选取SINS/DVL导航系统解算的导航坐标系作为导航计算坐标系为n′系，选取潜航器舱体的右前上坐标系作为载体坐标系，记为b系，

定义速度和姿态角误差为：

其中，

νⁿ为潜航器的真实速度在n系下的投影，

为捷联惯导系统的导航解算速度在n系下的投影，

为速度误差δνⁿ在东北天方向上的分量，

为潜航器的真实姿态角在n系下的投影，

为捷联惯导系统的导航解算姿态角在n系下的投影，

为姿态角误差

在东北天方向上的分量，

晃动基座条件下，n系与n′系间的姿态矩阵

不可忽略，组合系统大失准角非线性误差模型分为：SINS非线性误差模型和DVL线性误差模型：

1)建立SINS非线性误差模型：

SINS速度误差方程：

SINS姿态误差方程：

SINS位置误差方程：

其中，

为实际的加速度计输出；

为地球自转角速度，

为地球自转角速度的误差量，

为导航系相对地球系的旋转角速度，

为导航系相对地球系的旋转角速度的误差量；

为导航系相对惯性系的旋转角速度，

为导航系相对惯性系的旋转角速度的误差量；

ε^b为陀螺仪在载体系下的常值误差，

为加速度计在载体系下的常值误差；

R_E为卯酉圈半径，R_N为子午圈半径；

分别为导航系与计算系的姿态矩阵以及欧拉角微分系数矩阵的逆矩阵，具体矩阵形式如下：

2)建立DVL线性误差模型：

其中，刻度因数误差δC用随机常数描述，对地速度误差误差δν_d、偏流角误差δΔ用一阶马尔可夫过程描述，τ_d、τ_Δ分别为速度偏移误差和偏流角误差的相关时间，w_d、w_Δ分别为速度偏移误差和偏流角误差的零均值高斯白噪声；

3)建立精对准滤波方程：由于SINS/DVL组合系统的天向通道发散，因此忽略天向通道状态量，从而选取潜航器的纬度位置误差δL、经度位置误差δλ，潜航器的东向速度误差δν_e、北向速度误差δν_n，潜航器的欧拉角东向、北向、天向平台误差角分别为

陀螺仪传感器的东向、北向、天向常值误差分别为

加速度计传感器的东向、北向常值误差分别为

δν_d为水下DVL导航系统的对地速度误差，δΔ为水下DVL导航系统的偏流角误差，δC为水下DVL导航系统的刻度因数误差，成为15维状态变量：

SINS/DVL非线性滤波状态方程为：

其中，

为从载体系到计算系的姿态转换矩阵，

为从计算系到导航系的姿态转换矩阵；

SINS/DVL导航系统的非线性滤波状态方程简记为：

同时，选取SINS解算速度和DVL测量速度之差作为SINS/DVL导航系统的非线性滤波观测变量：

其中，ν_SINSe、ν_SINSn分别为捷联惯导系统的导航解算速度ν_SINS在导航系东向和北向的投影，δν_e、δν_n分别为捷联惯导系统的导航解算速度误差δν在导航系东向和北向的投影，ν_de、ν_dn分别为四波束水下多普勒导航系统的导航解算速度ν_d在导航系东向和北向的投影，δν_de、δν_dn分别为四波束水下多普勒导航系统的导航解算速度误差δν_d在导航系东向和北向的投影。

SINS/DVL导航系统的非线性滤波量测方程简记为：

Z＝h(X,t)+v(t)。

所述的深海潜航器的水下抗晃动对准方法，步骤(1)中所述的SINS抗晃动双矢量定姿自对准，包括以下步骤：

为抑制舱体横摇、纵摇产生的干扰线振动加速度

杆臂加速度

以及补偿零位误差

选取0-t₁、0-t₂时间段的重力矢量的积分v_t1、v_t2作为参考双矢量，

0-t时间段内重力在导航系(n系)积分为：

其中，

为0-τ时间段导航系的变化矩阵，

为τ时刻重力矢量在导航系下的分量。

为0-t₁、0-t₂时间段的重力矢量的积分在导航系下的分量，可由上式计算得到，

0-t时间段内重力在载体系(b系)下的积分为：

其中，

为载体系变化矩阵，

为重力矢量在载体系下的分量；

粗对准过程中，加速度计输出比力

由重力矢量g^b、线振动干扰加速度

杆臂加速度

以及零位误差

构成：

经过简化

为：

根据双矢量定姿原理，由

不难得到晃动基座下t＝0时刻姿态矩阵

所述的深海潜航器的水下抗晃动对准方法，步骤(1)中所述的SINS/DVL的基于SVD分解的模糊自适应鲁棒CKF滤波器对准，包括以下步骤：

1)计算基本容积点和相应权值：

式中，m表示容积点总数(m＝2num)，num为CKF滤波器的状态维数，[1]表示对num维单位向量e＝[1,0,..,0]^T的全排列和改变元素符号产生的点集；

2)时间更新：

①基于SVD分解计算容积点X_j,k-1

其中，k为滤波时刻，U_j,k-1为k-1时刻SVD分解出的酉阵，s_j(j＝1,2,..,num)为k-1时刻滤波器输出的最优滤波估计协方差P_k-1|k-1的特征值的平方根，

为k-1时刻滤波器输出的最优状态估计；

②根据权利2所述的非线性状态方程计算传播容积点

③计算状态量

和状态预测P_k|k-1

其中Q_k-1为k-1时刻的导航系统过程噪声矩阵；

3)量测更新：

①基于SVD分解计算容积点X_j,k|k-1

②通过根据权利2所述的非线性量测方程计算传播容积点Z_j,k

Z_j,k＝h(X_j,k|k-1,t)

③计算量测量

量测协方差P_zz,k以及新息协方差估计P_xz,k

④计算增益阵K_k、更新最优状态估计

以及最优估计协方差：

K_k＝P_xz,k/P_zz,k

为在晃动基座下，使精对准过程具有一定鲁棒性，基于H∞滤波器的相关原理，对传统 CKF的最优估计协方差进行改写：

其中，γ为H∞次优解的阈值，与滤波器的鲁棒性能有关，H∞次优问题存在解的充分必要条件可由黎卡提不等式(Riccati inequality)给出:

阈值γ模糊自适应算法如下：

γ＝η·γ_a

其中，

且ρ()表示矩阵的谱半径，η为模糊自适应因子，

从新息序列的统计特性的变化入手，构造出了阈值γ的模糊自适应因子η更新式为：

其中，

为新息序列的平方和。Trace(P_xz,k)为求迹运算，即求矩阵对角线元素之和。

有益效果：

本发明可以克服晃动基座，大失准角条件下的深海潜航器在初始对准时，传统惯导对准算法所导致的定姿速度、精度降低以至于潜航器无法继续正常工作的问题，最终实现深海潜航器的长航时工作。

针对水下复杂的洋流涌动环境以及鱼群冲击潜航器的可能性，会造成潜航器晃动基座，大失准角情况下，深海潜航器的初始对准速度、精度降低,从而使得潜航器惯导水下定位解算的累积误差进一步增大。根据水下复杂环境和捷联惯导、四波束水下多普勒导航系统特点所建立了大失准角非线性误差模型以及精对准滤波方程，通过SINS抗晃动双矢量定姿自对准和 SINS/DVL的基于SVD分解的模糊自适应鲁棒CKF滤波器对准。在保证水下潜航器导航系统鲁棒性的基础上，进一步使姿态失准角减小、使得姿态矩阵

更为精确，实现深海潜航器的准确定姿。

附图说明

图1是本发明实施例的深海潜航器导航系统方案图

图2是本发明实施例的具体水下初始对准流程图。

具体实施方式

下面结合具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。

如图1、2所示，是本发明公开的深海潜航器导航系统方案图和具体水下初始对准流程图。

具体实施步骤如下：

1)在对准之前根据水下复杂环境和SINS、四波束DVL导航系统特点，分析SINS/DVL导航定位原理，再建立大失准角非线性误差模型以及精对准滤波方程。

选取东北天地理坐标系作为导航坐标系(n系)，选取SINS/DVL导航系统解算的导航坐标系作为导航计算坐标系为n′系，选取潜航器舱体的右前上坐标系作为载体坐标系(b系)。

定义速度和姿态角误差为：

其中，νⁿ为潜航器的真实速度在n系下的投影、

为捷联惯导系统的导航解算速度在n 系下的投影，

为速度误差δνⁿ在东北天方向上的分量，

为潜航器的真实姿态角在n系下的投影、

为捷联惯导系统的导航解算姿态角在n系下的投影

为姿态角误差

在东北天方向上的分量。

组合系统大失准角非线性误差模型分为：SINS非线性误差模型和DVL线性误差模型。晃动基座条件下，n系与n′系间的姿态矩阵

不可忽略。

①建立SINS非线性误差模型：

速度误差方程：

姿态误差方程：

位置误差方程：

其中，

为实际的加速度计输出。

为地球自转角速度，

为导航系相对地球系的旋转角速度，

为导航系相对惯性系的旋转角速度，

为相应的误差量。ε^b为陀螺仪在载体系下的常值误差，

为加速度计在载体系下的常值误差，R_N为卯酉圈半径，R_E为子午圈半径。

分别为导航系与计算系的姿态矩阵以及欧拉角微分系数矩阵的逆矩阵，具体矩阵形式如下：

在四波束DVL詹纳斯(Janus)配置的船舶上。其中四波束詹纳斯配置指的是向船艏、船艉以及左右舷方向分别斜着发射一个声波束，由于四波束的对称性，当深海潜航器的舱体有上下、左右波动(横摇、纵摇)时，能够提高横向、纵向的速度测量精度。

四波束DVL速度测量表达式为：

其中，c为超声波在海水中传播的速度，视为常值。f₀为超声波频率，f_d13、f_d24为纵向 x和横向y的多普勒频移。α为超声波波束倾角。

根据多普勒频移f_d13、f_d24的可计算出舱体偏流角：

将实际误差项考虑其中后，DVL测速方程为:

其中，ν_d为水下DVL导航系统的实际对地速度，δν_d为水下DVL导航系统的对地速度误差，K_d为水下DVL导航系统未补偿偏流角Δ的航迹向，δΔ为水下DVL导航系统的偏流角误差，δC为水下DVL导航系统的刻度因数误差，

为水下DVL导航系统的未方位失准角。

②建立DVL线性误差模型：

其中，刻度因数δC用随机常数描述，误差δν_d、δΔ用一阶马尔可夫过程描述，τ_d、τ_Δ分别为速度偏移误差和偏流角误差的相关时间。w_d、w_Δ为速度偏移误差和偏流角误差的零均值高斯白噪声。误差。

③建立精对准滤波方程。由于SINS/DVL组合系统的天向通道发散，因此忽略天向通道状态量，从而选取潜航器的位置误差δL、δλ，潜航器的速度误差δν_e、δν_n，潜航器的欧拉角平台误差角

陀螺仪传感器的常值误差

加速度计传感器的常值误差

δν_d为水下DVL导航系统的对地速度误差，δΔ为水下DVL导航系统的偏流角误差，δC为水下DVL导航系统的刻度因数误差，成为15维状态变量：

SINS/DVL非线性滤波状态方程为：

其中，

为从载体系到计算系的姿态转换矩阵，

为从计算系到导航系的姿态转换矩阵；

SINS/DVL导航系统的非线性滤波状态方程可简记为：

同时，选取SINS解算速度和DVL测量速度之差作为SINS/DVL导航系统的非线性滤波观测变量：

其中，ν_SINSe、ν_SINSn为捷联惯导系统的导航解算速度ν_SINS在导航系东向和北向的投影，δν_e、δν_n为捷联惯导系统的导航解算速度误差δν在导航系东向和北向的投影，ν_de、ν_dn为四波束水下多普勒导航系统的导航解算速度ν_d在导航系东向和北向的投影，δν_de、δν_dn为四波束水下多普勒导航系统的导航解算速度误差δν_d在导航系东向和北向的投影。

SINS/DVL导航系统的非线性滤波量测方程可简记为：

Z＝h(X,t)+v(t)

2)DSP利用接收到的光纤惯导三轴陀螺仪、加速度计信号，进行SINS抗晃动双矢量定姿自对准(粗对准)。

晃动基座下大幅度晃动产生的干扰角速度较大，且光纤陀螺输出的信噪比较小，无法分离陀螺输出中的干扰加速度和地球自转角速度ω_ie，从而无法通过传统解析粗对准完成姿态矩阵

的粗略估计。但是地球自转角速度ω_ie是一固定值，当舱体姿态在晃动基座下随时间变化时，使得惯性空间内重力矢量g方向的改变能够包含地球北向信息。因此选用不同时刻t₁、 t₂(0＜t₁＜t₂)的两个重力相关矢量(不相互平行)作为参考双矢量，即可以实现晃动基座下t＝0时刻姿态矩阵

的粗略估计。

为抑制舱体横摇、纵摇产生的干扰线振动加速度

杆臂加速度

以及补偿零位误差

选取0-t₁、0-t₂时间段的重力矢量的积分v_t1、v_t2作为参考双矢量。

0-t时间段内重力在导航系(n系)积分为：

其中，

为导航系变化矩阵，

为重力矢量在导航系下的分量。

为0-t₁、 0-t₂时间段的重力矢量的积分在导航系下的分量，可由上式计算得到。

0-t时间段内重力在载体系(b系)下的积分为：

其中，

为载体系变化矩阵，

为重力矢量在载体系下的分量。

粗对准过程中，加速度计输出比力

由重力矢量g^b、线振动干扰加速度

杆臂加速度

以及零位误差

构成：

将上式代入重力积分式，得:

其中，

分别为线振动干扰速度和零位误差速度，在粗对准过程中可以近似忽略。

为杆臂速度，若已知杆臂长度r^b，则：

因此

可以简化为：

根据双矢量定姿原理，由

不难得到晃动基座下t＝0时刻姿态矩阵

3)DSP利用接收到的光纤惯导三轴陀螺仪、加速度计信号，四波束水下多普勒测速仪速度信号，进行SINS/DVL的基于SVD分解的模糊自适应鲁棒CKF滤波器对准(精对准)。

大深度、远距离、长时间的水下航行对初始对准的精度要求非常高，需要利用DVL速度信号辅助惯导，再根据步骤1)建立的非线性误差模型和精对准滤波方程进行基于SVD分解的CKF滤波，完成精对准过程，进一步提高姿态矩阵

的精度。基于SVD分解的模糊自适应鲁棒CKF滤波器过程如下：

①计算基本容积点和相应权值：

式中，m表示容积点总数(m＝2num)，num为CKF滤波器的状态维数。[1]表示对num维单位向量e＝[1,0,..,0]^T的全排列和改变元素符号产生的点集。

②时间更新：

a)基于SVD分解计算容积点X_j,k-1

其中，S＝diag(s₁,s₂,...,s_num)为对角阵。由于最优滤波估计协方差P_k-1|k-1通常为对称阵，因此其特征值为

且U＝V。最优滤波估计协方差P_k-1|k-1可改写为：

b)通过步骤1)建立的非线性状态方程计算传播容积点

c)计算状态量

和状态预测P_k|k-1

③量测更新

a)基于SVD分解计算容积点X_j,k|k-1

b)通过步骤1)建立的非线性量测方程计算传播容积点Z_j,k

Z_j,k＝h(X_j,k|k-1,t)

c)计算量测量

量测协方差P_zz,k以及新息协方差估计P_xz,k

d)计算增益阵K_k、更新最优状态估计

以及最优估计协方差

K_k＝P_xz,k/P_zz,k

为在晃动基座下，使精对准过程具有一定鲁棒性。基于H∞滤波器的相关原理，对传统CKF的最优估计协方差进行改写：

其中，γ为H∞次优解的阈值，与滤波器的鲁棒性能有关。如果能够使得阈值γ自适应于不同水域环境，则能够在保证鲁棒性的基础上，使得姿态矩阵

更为精确，实现深海潜航器的准确定姿。

H∞次优问题存在解的充分必要条件可由黎卡提不等式(Riccati inequality)给出:

阈值γ模糊自适应算法如下：

γ＝η·γ_a

其中，

且ρ()表示矩阵的谱半径。η为模糊自适应因子。

在实际系统中，系统不确定性影响会导致观测量的异常，从而引起滤波器的失常。这样的失常会引起新息序列的统计特性的变化。理想情况下CKF的新息协方差阵为 V_k＝N(0,P_xz,k)，因此模糊自适应因子η更新式为：

其中，

为新息序列的平方和。Trace(P_xz,k)为求迹运算，即求矩阵对角线元素之和。

Claims (2)

1.一种深海潜航器的SINS/DVL水下抗晃动对准方法，其特征在于：该方法包括如下步骤：

(1)根据水下复杂环境和捷联惯导、四波束水下多普勒导航系统特点所建立的大失准角非线性误差模型以及精对准滤波方程，将深海潜航器的水下对准过程分为：SINS抗晃动双矢量定姿自对准和SINS/DVL的基于SVD分解的模糊自适应鲁棒CKF滤波器对准；

(2)SINS抗晃动双矢量定姿自对准在选定重力矢量为主参考矢量的前提下，预先对参与姿态解算的矢量作单位正交化处理；

(3)SINS/DVL的基于SVD分解的模糊自适应鲁棒CKF滤波器对准则在保证鲁棒性的基础上，进一步使姿态失准角减小、使得姿态矩阵

更为精确，实现深海潜航器的准确定姿；

步骤(1)中所述的根据水下复杂环境和捷联惯导、四波束水下多普勒导航系统特点所建立的大失准角非线性误差模型以及精对准滤波方程的具体方法是：

选取东北天地理坐标系作为导航坐标系，记为n系，选取SINS/DVL导航系统解算的导航坐标系作为计算坐标系为n′系，选取潜航器舱体的右前上坐标系作为载体坐标系，记为b系，

定义速度和姿态角误差为：

其中，

νⁿ为潜航器的真实速度在n系下的投影，

为捷联惯导系统的导航解算速度在n系下的投影，

为速度误差δνⁿ在东北天方向上的分量，

为潜航器的真实姿态角在n系下的投影，

为捷联惯导系统的导航解算姿态角在n系下的投影，

为姿态角误差

在东北天方向上的分量，

晃动基座条件下，n系与n′系间的姿态矩阵

不可忽略，组合系统大失准角非线性误差模型分为：SINS非线性误差模型和DVL线性误差模型：

1)建立SINS非线性误差模型：

SINS速度误差方程：

SINS姿态误差方程：

SINS位置误差方程：

其中，

为实际的加速度计输出；

为地球自转角速度，

为地球自转角速度的误差量，

为导航系相对地球系的旋转角速度，

为导航系相对地球系的旋转角速度的误差量；

为导航系相对惯性系的旋转角速度，

为导航系相对惯性系的旋转角速度的误差量；

ε^b为陀螺仪在载体系下的常值误差，

为加速度计在载体系下的常值误差；

R_E为卯酉圈半径，R_N为子午圈半径；

分别为导航系与计算系的姿态矩阵以及欧拉角微分系数矩阵的逆矩阵，具体矩阵形式如下：

2)建立DVL线性误差模型：

其中，刻度因数误差δC用随机常数描述，对地速度误差误差δν_d、偏流角误差δΔ用一阶马尔可夫过程描述，τ_d、τ_Δ分别为速度偏移误差和偏流角误差的相关时间，w_d、w_Δ分别为速度偏移误差和偏流角误差的零均值高斯白噪声；

3)建立精对准滤波方程：由于SINS/DVL组合系统的天向通道发散，因此忽略天向通道状态量，从而选取潜航器的纬度位置误差δL、经度位置误差δλ，潜航器的东向速度误差δν_e、北向速度误差δν_n，潜航器的欧拉角东向、北向、天向平台误差角分别为

陀螺仪传感器的东向、北向、天向常值误差分别为

加速度计传感器的东向、北向常值误差分别为

δν_d为水下DVL导航系统的对地速度误差，δΔ为水下DVL导航系统的偏流角误差，δC为水下DVL导航系统的刻度因数误差，成为15维状态变量：

SINS/DVL非线性滤波状态方程为：

其中，

为从载体系到计算系的姿态转换矩阵，

为从计算系到导航系的姿态转换矩阵；

SINS/DVL导航系统的非线性滤波状态方程简记为：

同时，选取SINS解算速度和DVL测量速度之差作为SINS/DVL导航系统的非线性滤波观测变量：

其中，ν_SINSe、ν_SINSn分别为捷联惯导系统的导航解算速度ν_SINS在导航系东向和北向的投影，δν_e、δν_n分别为捷联惯导系统的导航解算速度误差δν在导航系东向和北向的投影，ν_de、ν_dn分别为四波束水下多普勒导航系统的导航解算速度ν_d在导航系东向和北向的投影，δν_de、δν_dn分别为四波束水下多普勒导航系统的导航解算速度误差δν_d在导航系东向和北向的投影；

SINS/DVL导航系统的非线性滤波量测方程简记为：

Z＝h(X,t)+v(t)；

步骤(1)中所述的SINS抗晃动双矢量定姿自对准，包括以下步骤：

为抑制舱体横摇、纵摇产生的干扰线振动加速度

杆臂加速度

以及补偿零位误差

选取0-t₁、0-t₂时间段的重力矢量的积分v_t1、v_t2作为参考双矢量，

0-t时间段内重力在导航系(n系)积分为：

其中，

为0-τ时间段导航系的变化矩阵，

为τ时刻重力矢量在导航系下的分量，

为0-t₁、0-t₂时间段的重力矢量的积分在导航系下的分量，可由上式计算得到，

0-t时间段内重力在载体系(b系)下的积分为：

其中，

为载体系变化矩阵，

为重力矢量在载体系下的分量；

粗对准过程中，加速度计输出比力

由重力矢量g^b、线振动干扰加速度

杆臂加速度

以及零位误差

构成：

经过简化

为：

根据双矢量定姿原理，由

不难得到晃动基座下t＝0时刻姿态矩阵

2.根据权利要求1所述的深海潜航器的SINS/DVL水下抗晃动对准方法，其特征在于：所述的深海潜航器的水下抗晃动对准方法，步骤(1)中所述的SINS/DVL的基于SVD分解的模糊自适应鲁棒CKF滤波器对准，包括以下步骤：

1)计算基本容积点和相应权值：

式中，m表示容积点总数，m＝2num，num为CKF滤波器的状态维数，[1]表示对num维单位向量e＝[1,0,..,0]^T的全排列和改变元素符号产生的点集；

2)时间更新：

①基于SVD分解计算容积点X_j,k-1

其中，k为滤波时刻，U_j,k-1为k-1时刻SVD分解出的酉阵，s_j为k-1时刻滤波器输出的最优滤波估计协方差P_k-1|k-1的特征值的平方根，j＝1,2,…，num，

为k-1时刻滤波器输出的最优状态估计；

②根据所述的非线性状态方程计算传播容积点

③计算状态量

和状态预测P_k|k-1

其中Q_k-1为k-1时刻的导航系统过程噪声矩阵；

3)量测更新：

①基于SVD分解计算容积点X_j,k|k-1

②通过所述的非线性量测方程计算传播容积点Z_j,k

Z_j,k＝h(X_j,k|k-1,t)

③计算量测量

量测协方差P_zz,k以及新息协方差估计P_xz,k

④计算增益阵K_k、更新最优状态估计

以及最优估计协方差：

K_k＝P_xz,k/P_zz,k

为在晃动基座下，使精对准过程具有一定鲁棒性，基于H∞滤波器的相关原理，对传统CKF的最优估计协方差进行改写：

其中，γ为H∞次优解的阈值，与滤波器的鲁棒性能有关，H∞次优问题存在解的充分必要条件可由黎卡提不等式给出:

阈值γ模糊自适应算法如下：

γ＝η·γ_a

其中，

且ρ()表示矩阵的谱半径，η为模糊自适应因子，从新息序列的统计特性的变化入手，构造出了阈值γ的模糊自适应因子η更新式为：

其中，

为新息序列的平方和，Trace(P_xz,k)为求迹运算，即求矩阵对角线元素之和。

Images