[发明专利]一种深海潜航器的SINS/DVL水下抗晃动对准方法

权利要求书

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)为求迹运算，即求矩阵对角线元素之和。