[发明专利]一种基于因子图优化的UUV多海域综合导航系统设计方法

权利要求书

1.一种基于因子图优化的UUV多海域综合导航系统设计方法，其特征是：包括以下步骤：

步骤1：实时监测UUV当前导航参数状态；

步骤2：对当前状态的传感器进行可用度分析；

步骤3：根据当前导航参数与电量选择惯导工作模式与最优传感器组合；

设总的决策为Ψ，环境因素影响的可用性与电量因素影响分别为Ψ_Env和Ψ_Elec，则有：

Ψ＝Ψ_Env∩Ψ_Elec

式中的Ψ_Env和Ψ_Elec由权重矩阵W和信息向量X组成；其中权重矩阵表示为：

则Ψ_Env和Ψ_Elec表示为：

Ψ_Env(Ψ_Elec)＝WX

根据不同的情况，权重因子定义为：

a.w^imu：0为不能获取imu信息；1为可以获取imu信息；

b.w^GPS：0为GPS被检测故障不可用，或者电量决策不可用；1为GPS被检测故障可用，或者电量决策可用；

c.w^DVL：0为DVL被检测故障不可用，或者电量决策不可用；1为DVL被检测故障可用，或者电量决策可用；

d.w^MOD：0为MOD被检测故障不可用，或者电量决策不可用；1为MOD被检测故障可用，或者电量决策可用；

e.w^USBL：0为USBL被检测故障不可用，或者电量决策不可用；1为USBL被故障可用，或者电量决策可用；

f.w^深度计：0为深度计不可用；1为深度计可用；

g.w^磁力计：0为磁力计不可用；1为磁力计可用；

步骤4：将传感器量测抽象为因子节点，导航参数误差抽象为状态节点采用iSAM2进行因子图融合。

2.根据权利要求1所述的一种基于因子图优化的UUV多海域综合导航系统设计方法，其特征是：所述步骤2具体为基于卡方检测的故障检测算法来对传感器进行可用度分析；这里引用基于残差的χ²故障检测算法，利用新息的统计特性来判断传感器是否可用或者故障；各个传感器残差向量的协方差可表示为：

式中H_k为量测矩阵，P_k,k-1为一步预测协方差矩阵，R_k为噪声序列量测方差阵；

新息为：γ_k＝Z_k-H_kX_k,k-1

式中Z_k为量测向量，X_k,k-1为状态的一步估计；

则故障检测函数应为：

由统计特性知，λ_k服从卡方分布，取λ_k大于某一阈值Θ的概率为α，则此检测过程为：当λ_k＞Θ时，传感器存在一定问题，不可用；当λ_k＜Θ时，此时传感器工作正常，正常使用；

UUV在水下复杂环境下，没有类似陆地无人机一样的多种传感器，因此本系统传感器包括惯性测量单元IMU、全球卫星定位系统GPS、多普勒测速仪DVL、模型辅助导航MOD、深度计、罗经以及超短基线USBL；定义传感器信息向量X为：

X＝[x^imu,x^GPS,x^DVL,x^MOD,x^USBL,x^深度计，x^磁力计]^T。