CN113984061A - 一种基于因子图优化的uuv多海域综合导航系统设计方法 - Google Patents

Abstract

本发明提供一种基于因子图优化的UUV多海域综合导航系统设计方法以实现以惯导为核心的UUV多海域信息融合定位，该方法将海域分为浅海域、深远海域以及极区海域，通过分析各个海域的传感器可用性，对传感器进行最优组合以满足长航时多海域的情况。本发明解决UUV传统组合导航下只针对某一特定环境下的导航问题，长航时的能耗问题以及传感器异步异构问题。

Description

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

技术领域

本发明涉及多源信息融合、极地导航、组合导航领域，具体为一种基于因子图优化的UUV多海域综合导航系统设计方法。

背景技术

无人水下潜航器(Unmanned Underwater Vehicle，UUV)作为一种自主能力强、可控制性好、续航时间长的海洋装备，在海洋开发和勘探和军事领域方面发挥着重要的作用。由于UUV体积小，属于智能高技术集成体，所以它本身所能携带的能源和质量都会受到一定程度的限制。并且水下环境不同于陆地和空中的另一特点是定位难度大，导航定位传感系统在复杂多变的海洋环境下易出现短时间的定位失效，此时UUV当前位置，速度及姿态等导航参数不能被正常反馈。传统的组合导航系统往往是针对特定环境设计的，不能适应不断变化的作战环境和要求。水下工作环境具有很强的复杂性和未知性，因此，要想达到以上的目标，UUV高精度的导航定位系统是必不可少的。它是UUV安全作业以及顺利返航的重要保障。

水下导航信息源没有陆地上面的多，UUV在水下的常用导航传感器有惯性导航系统(Inertial Navigation System,INS)、罗经、多普勒测速仪(Doppler Velocity Log,DVL)、全球定位系统(Global Position System,GPS)、深度计、模型辅助导航(当UUV在水下航行时，由于水下复杂的环境，DVL可能因为海底太深无法探测到对底速度，此时可以以模型辅助导航)、长基线(Long Base Line,LBL)、短基线(Short Base Line,SBL)和超短基线(Ultra Short Base Line,USBL)等等。系统的计算复杂度与使用的传感器数量成正比，且在部分情况下，载体的能源消耗限制也不允许系统使用过多的传感器。同时，由于各个传感器有不同的缺陷，适合的使用场景不同，有些时候，增加一个传感器不仅会增加计算复杂度，反而还会使定位精度下降。此外，部分传感器的定位精度还会受到载体运动状态的影响。此外，当UUV航行到高纬度地区甚至极区时，传统的惯导解算将会失效，这将严重影响到UUV的安全，因此及时判断当前纬度并且切换为游移方位惯导解算是必须的。

虽然信息的增多给了系统提升性能的条件，但由于当前系统引入了多种导航定位传感器，信息种类增多，信息量激增，此时将产生UUV长航行时的电源功耗和多传感器并行使用耗电问题面对不同种类的导航信息如何提取并有效利用将是多传感器信息融合所要开展研究的关键内容，并且还有各个传感器的异步异构问题。因此需要设计一种UUV在多种海域情况下的有用传感器的即插即用，传感器选择后能耗低，并且在高纬度地区将传统的惯导解算切换为游移方位的惯导解算来保障UUV的导航精度并安全航行。

发明内容

本发明的目的是为了解决UUV传统组合导航下只针对某一特定环境下的导航问题，长航时的能耗问题以及传感器异步异构问题而提供一种基于因子图优化的UUV多海域综合导航系统设计方法。

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

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

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

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

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

本发明还包括这样一些特征：

1.所述步骤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

设总的决策为Ψ，环境因素影响的可用性与电量因素影响分别为Ψ_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为磁力计可用。

2.所述构建多海域因子图框架的具体步骤如下：

A.在初始时刻，因子图需要对先验因子进行初始化，先验的信息可以用布放后的GPS接收机给出。

B.每两个时刻的状态节点用等效IMU因子进行连接。定义每两个时刻直接的等效IMU因子为：

式中Δx_T表示一个周期T内的预积分变化量。

表示周期T的两个相邻的时刻状态变量。

C.在t_j时刻INS解算出当前时刻的导航参数信息，则INS量测因子定义为：

式中

为惯导解算在t_j时刻的量测函数；θ_j为t_j时刻惯导解算量测噪声协方差矩阵。不管惯导的工作模式如何，均可这样定义。

D.在t_j时刻GPS接收机收到GPS信息，则GPS量测因子定义为：

式中

为GPS接收机在t_j时刻的量测函数；Ω_j为t_j时刻GPS接收机量测噪声协方差矩阵。

E.在t_m时刻DVL接收到返回来的声波后得到的DVL量测信息，则DVL量测因子定义为：

式中

为DVL在t_m时刻的量测函数；Σ_m为t_m时刻DVL量测噪声协方差矩阵。不管DVL的工作模式如何，均可这样定义。

F.在t_n时刻模型辅助计算得到的量测信息，则模型辅助量测因子定义为：

式中

为模型辅助在t_n时刻的量测函数；Γ_n为t_n时刻模型辅助量测噪声协方差矩阵。

G.在t_p时刻USBL解算出当前时刻的量测信息，则USBL量测因子定义为：

式中

为USBL在t_p时刻的量测函数；ζ_p为t_p时刻USBL量测噪声协方差矩阵。

3.所述多源信息因子图采用iSAM2算法其包含以下步骤：

(1)首先初始化因子图，得到贝叶斯树F，非线性因子f，线性化点Θ，初始化时均为空集；

(2)添加新的因子：f＝f∪f′；

(3)初始化新的变量Θ′并且有Θ＝Θ∪Θ′；

(4)进行平滑再线性化，获取所有标记的贝叶斯树子团M；

(5)通过F，M，对所有受到影响的因子消元，重新构造贝叶斯树；

(6)求解影响大的集合Δ(向量)；

(7)求解当前值

通过对GTSAM里面的ISAM2Params类进行实例化，对实例化的对象进行参数设置，参阈值大小设置为0.01(差值大于0.01要重新线性化)，将重新线性化任意变量参数设置为true，将计算线性化误差参数设置为false，将保存线性化结果参数设置为false以优化性能，设置矩阵分解模式为QR分解，将只进行部分更新功能参数设置为false。然后添加待优化的因子图，调用update函数进行更新。

与现有技术相比，本发明的有益效果是：

(1)针对UUV在不同海域，传感器可信度发生变化问题，通过对UUV通常航行的浅海、深远海以及极区三个海域划分，在此基础上进行UUV导航因子图的构建与选择方案。采用UUV模型辅助导航的方法，对UUV在深远海没有水声定位系统和DVL失效的情况下对惯导误差进行修正，辅助定位。考虑到UUV在极区航行时DVL对底量测信息不可用的问题，采用DVL对冰面进行发射超声波得到量测信息的方法，进行惯导参数修正。

(2)针对UUV长航行时的电源功耗和多传感器并行使用耗电问题。在导航传感器可用性的基础上，提出并设计导航传感器的最优选择方案。并可以在高纬度地区可以随意切换惯性导航的力学编排。最后进行iSAM2的因子图融合以减少计算量。

附图说明

图1是本发明浅海情况下UUV导航传感器可用性划分图；

图2是本发明深海情况下UUV导航传感器可用性划分图；

图3是本发明极区情况下UUV导航传感器可用性划分图；

图4是本发明UUV电量大于20％时，导航传感器的在线决策流程图；

图5是本发明UUV电量大于20％时，导航传感器的在线决策流程图；

图6是本发明UUV基于因子图优化的多海域导航模型图；

图7是本发明UUV基于因子图优化的多海域系统设计流程图。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述。

目前，UUV在水下的可用传感器没有陆地无人机多，因此选择合适传感器的组合与长航时的探测任务非常重要。UUV在水下航行中，为了适应各种复杂环境下(在此分为浅、深远海和极区)的导航定位工作，需要用多种体制的传感器来满足高精度长航时的自主导航需求，各个传感器的量测数据是否有效在全源导航系统中和各海域是一个难以辨别的问题。对于单个传感器来说，影响其有效的因素有UUV当前航行的深度与状态，航行的海域是否已经布置了水声应答器的浅海海域，还是远海航行的陌生海域，这都影响着传感器的可用性。考虑到UUV在未知海域潜航时，当没有母船的指引，并且DVL短暂失效的情况下，UUV当前可用导航传感器仅有深度计、罗经和IMU，前两者对惯导的误差修正不大，因此，为了保障UUV的全源导航可用精度，引入UUV模型辅助导航，以此来代替DVL的速度量测信息，修正惯导误差。图1为浅海海域下，根据深度情况对UUV导航传感器的可用性进行的分析。

当UUV在深海进行工作潜航时，若没有应答器或者信标的辅助，则UUV传感器的可用性就会发生改变。图2为深海海域下，根据深度情况对UUV导航传感器的可用性进行的分析。

当UUV在极区进行潜航探测时，由于极区海洋的深度至少几千米深，远远超过DVL的探底距离。北冰洋上的海冰，对UUV起到了很好的隐蔽作用。因而，对于航行穿越北极的UUV而言，在20米至30米深度航行是较为安全的。此时采用UUV对冰探测模式，保证DVL的可用性。图3为极区环境下，根据深度情况对UUV导航传感器的可用性进行的分析。

从上面的图可以看出来，UUV不同于无人机和无人车，它的传感器非常少，尤其是DVL和水声导航系统对UUV的导航精度具有显著效果，在此不采用无人机上的对UUV的传感器进行最优组合。而是考虑到UUV未来长航时的必然趋势，以UUV的电池电量为侧重点进行传感器的决策选择。当UUV电量大于20％时，此时能用的传感器均用上，导航传感器的在线决策流程为图4所示。

当UUV电量低于20％时，为了保障UUV顺利返航，需要在导航精度可接受的范围内对传感器进行适当裁剪优化，以适应电量模式。图5为UUV电量低于20％时，导航传感器的在线决策流程图。

这里引用基于残差的χ²故障检测算法，利用新息的统计特性来判断传感器是否可用或者故障。各个传感器残差向量的协方差可表示为：

式中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在航行过程中的上述问题，传感器的选择受可用性(环境因素)、电量因素的影响。设总的决策为Ψ，环境因素影响的可用性与电量因素影响分别为Ψ_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为磁力计可用。

因子图是个“二分图”，由顶点和边组成。它表征了局部函数与全局函数之间的关系(将局部函数以乘积的形式来表示全局函数)。当处理多变量复杂全局函数时，利用因子图可以有效的将复杂系统简单化，有利于处理复杂的概率问题。

本发明在传统的传感器作为量测的基础上将惯导力学编排解算的导航参数信息也做量测信息抽象为因子图里面的因子，两个时刻状态节点之间用IMU预积分因子进行连接。

选取惯导的基本误差和惯性器件误差参数作为因子图中的系统状态节点：

X＝[φ_E φ _Nφ_D δV_E δV_N δV_D δL δλ δh ε_x ε_y ε_z ▽_x ▽_y ▽_z]^T

式中

为姿态角误差；V＝[δV_E δV_N δV_D]为三轴速度误差；P＝[δL δλδh]为三轴位置误差；ε＝[ε_x ε_y ε_z]和▽＝[▽_x ▽_y ▽_z]分别为陀螺漂移和加计零偏。下标x y z分别表示载体坐标系前向、右向、下向，N E D分别表示导航坐标系北向、东向和地向分别表示纬度、经度和高度。

a.等效IMU因子

由于IMU更新频率是其他传感器中最快的，因此，因子图模型总是以IMU更新频率来添加新的变量节点，而通过其他更新频率较慢的辅助传感器来添加因子节点。设更新周期为,在一个周期内就会有很多个IMU因子节点产生，为了简化因子图和计算量，我们对一个更新周期内的IMU量测数据在时刻载体系下进行预积分，也就是在一个周期不进行专题节点和因子节点的更新，将与时刻之间的多个IMU节点等效为一个IMU因子节点。等效的因子节点为：

式中Δx_T表示一个周期T内的预积分变化量。

表示周期T的两个相邻的时刻状态变量。

b.INS量测因子

在t_j时刻INS解算出当前时刻的导航参数信息，则INS量测因子定义为：

式中

为惯导解算在t_j时刻的量测函数；θ_j为t_j时刻惯导解算量测噪声协方差矩阵。

c.GPS量测因子

在t_j时刻GPS接收机收到GPS信息，则GPS量测因子定义为：

式中

为GPS接收机在t_j时刻的量测函数；Ω_j为t_j时刻GPS接收机量测噪声协方差矩阵。

d.DVL量测因子

在t_m时刻DVL接收到返回来的声波后得到的DVL量测信息，则DVL量测因子定义为：

式中

为DVL在t_m时刻的量测函数；Σ_m为t_m时刻DVL量测噪声协方差矩阵。

e.模型辅助(MOD)量测因子

在t_n时刻模型辅助计算得到的量测信息，则模型辅助量测因子定义为：

式中

为模型辅助在t_n时刻的量测函数；Γ_n为t_n时刻模型辅助量测噪声协方差矩阵。

f.USBL量测因子

在t_p时刻USBL解算出当前时刻的量测信息，则USBL量测因子定义为：

式中

为USBL在t_p时刻的量测函数；ζ_p为t_p时刻USBL量测噪声协方差矩阵。

其他传感器类似，不在赘述。上述传感器的因子插入没有先后顺序，即插即用。

本发明的因子图的模型图6所示。从图中可以看出传感器量测信息来了就插入图中，并且在浅海与深海区域惯导采用的是指北方位力学编排，而在极区海域则采用的是游移方位力学编排。并且在极区时DVL根据情况切换为对冰工作模式。

上述因子图问题采用iSAM2(Incremental Smoothing and Mapping Using theBayes Tree)算法进行因子图的优化估计，以减少计算量。采用GTSAM进行实现。GTSAM是一个在机器人领域和计算机视觉领域用于平滑(smoothing)和建图(mapping)的C++库，它采用因子图和贝叶斯网络的方式最大化后验概率。iSAM2将图优化问题转化为贝叶斯树的建立、更新和推理问题。每添加一个因子时，对于贝叶斯树上面的变量不需要全部更新，只需要更新那些改变的变量即可，也就是当变量的更新量超过一定的阈值时，才对变量进行重新线性化。

iSAM2首先需要平滑再线性化跟踪每个可能的线性化点的有效性，并且仅在需要时重新线性化。对于选择重新线性化的变量，必须从贝叶斯树中删除所有相关信息，并用重新线性化相应的原始非线性因子来代替。通过给定阈值β，若变量的更新量高于β，则对变量进行标记M，然后更新所有被标记的变量。其次iSAM2需要部分状态更新，求得精确的解，不需要对所有相关变量求解，虽然更新贝叶斯时只影响顶部的团，但是实际变量的估计值发生变化仍会传播到子树。但是顶部的影响往往是有限的，当没有大的回环存在时，新的测量值只影响局部值，更远的部分保持不变，从树的根节点开始，获取向量Δ(标记所有变化量，用来线性化点Θ)，递归处理所有子团，直到子团中不包含任何Δ与变化量超过阈值β中的值。

具体步骤即为：

步骤一：首先初始化因子图，得到贝叶斯树F,非线性因子f,线性化点Θ，初始化时均为空集；

步骤二：添加新的因子：f＝f∪f′；

步骤三：初始化新的变量Θ′并且有Θ＝Θ∪Θ′；

步骤四：进行平滑再线性化，获取所有标记的子团M；

步骤五：通过F,M,对所有受到影响的因子消元，重新构造贝叶斯树；

步骤六：求解影响大的集合Δ(向量)；

步骤七：求解当前值

通过对GTSAM里面的ISAM2Params类进行实例化，对实例化的对象进行参数设置，然后添加待优化的因子图，调用update函数进行更新。

本发明所提供的UUV多海域导航方法整体包括如下步骤(见图7所示流程图)：

步骤1：UUV上电后，所有传感器数据经过故障检测算法筛选出可用传感器，首先根据GPS给出的初始位置判断当前的经纬度，当纬度当前纬度低于67度时，惯性导航系统采用传统的指北方位解算，DVL采用对海底波束模式；当前纬度高于67度时，惯性导航系统采用游移方位进行惯导解算；

步骤2：UUV布放后，首先根据深度计的测量值进行实时监测，当深度小于5m时，GPS保持常开状态接收信息；当深度大于5m时，为了节省电量，关闭GPS。；

步骤3：实时监测当前电量变化情况，当电量大于20％时，UUV所配备的传感器只要在可信度范围内的都用上，以保证导航的精度；当电量低于20％时，首先判断GPS和USBL是否有效，当GPS有效时，将GPS因子插入图中；GPS无效时，关闭GPS。当USBL有效时，将USBL因子插入图中，此时为了保证后续电量安全返航，则此时不再将模型辅助与DVL信息插入图中增加计算量消耗电量。当USBL无效时，此时判断DVL是否有效，有效时将DVL因子插入图中，无效时再将模型辅助因子插入图中。；

步骤4：将插入到图中的因子进行因子图融合，运用iSAM2算法进行导航状态的最优估计。

步骤5：在融合得到当前时刻状态后，以此循环进行1至3中的信息判断，然后在执行步骤4进行优化估计。

本发明的第一方面针对UUV长航行时的电源功耗和多传感器并行使用耗电问题。在导航传感器可用性的基础上，提提供并设计导航传感器的最优选择方案，并且提供一种当前经纬度的判断以及进入极区时DVL对海底的波束模式改为对极区冰面的波束的模式方案，来满足极区导航的精度要求。

UUV不同于无人机和无人车，它的传感器非常少，尤其是DVL和水声导航系统对UUV的导航精度具有显著效果，在此不采用无人机上的对UUV的传感器进行最优组合。而是考虑到UUV未来长航时的必然趋势，以UUV的电池电量为侧重点进行传感器的决策选择。当UUV电量大于20％时，此时能用的传感器均用上。

当UUV电量低于20％时，为了保障UUV顺利返航，需要在导航精度可接受的范围内对传感器进行适当裁剪优化，以适应电量模式。

两种电量模式的前提都是要先判断当前经纬度信息，当前纬度低于67度时，采用传统的惯导解算，DVL采用对海底波束模式；当前纬度高于67度时，采用游移方位进行惯导解算，并且此时将DVL切换为对冰面波束模式。

本发明的第二方面提供一种解决传感器即插即用的基于因子图优化的多源信息融合方法。

将UUV的传统组合导航的导航参数误差状态抽象为状态节点，将来自于不同传感器的异步异构信息量测抽象为因子节点，构建UUV多源信息融合因子图框架。

其中因子图中各个状态节点用IMU预积分因子节点进行连接，其次，将传统的惯导解算与游移方位解算也抽象为因子节点插入到因子图中，提高导航精度。

因子图框架采用iSAM2方法对因子图进行迭代更新，对状态进行最优估计。

其中所述的UUV传感器包括惯性测量单元IMU、罗经、多普勒测速仪DVL，全球定位系统GPS、深度计、模型辅助导航以及超短基线USBL。

Claims (4)

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

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

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

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

步骤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

设总的决策为Ψ，环境因素影响的可用性与电量因素影响分别为Ψ_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为磁力计可用。

3.根据权利要求2所述的一种基于因子图优化的UUV多海域综合导航系统设计方法，其特征是：所述构建多海域因子图框架的具体步骤如下：

A.在初始时刻，因子图需要对先验因子进行初始化，先验的信息可以用布放后的GPS接收机给出。

B.每两个时刻的状态节点用等效IMU因子进行连接。定义每两个时刻直接的等效IMU因子为：

式中Δx_T表示一个周期T内的预积分变化量。

表示周期T的两个相邻的时刻状态变量。

C.在t_j时刻INS解算出当前时刻的导航参数信息，则INS量测因子定义为：

式中

为惯导解算在t_j时刻的量测函数；θ_j为t_j时刻惯导解算量测噪声协方差矩阵。不管惯导的工作模式如何，均可这样定义。

D.在t_j时刻GPS接收机收到GPS信息，则GPS量测因子定义为：

式中

为GPS接收机在t_j时刻的量测函数；Ω_j为t_j时刻GPS接收机量测噪声协方差矩阵。

E.在t_m时刻DVL接收到返回来的声波后得到的DVL量测信息，则DVL量测因子定义为：

式中

为DVL在t_m时刻的量测函数；Σ_m为t_m时刻DVL量测噪声协方差矩阵。不管DVL的工作模式如何，均可这样定义。

F.在t_n时刻模型辅助计算得到的量测信息，则模型辅助量测因子定义为：

式中

为模型辅助在t_n时刻的量测函数；Γ_n为t_n时刻模型辅助量测噪声协方差矩阵。

G.在t_p时刻USBL解算出当前时刻的量测信息，则USBL量测因子定义为：

式中

为USBL在t_p时刻的量测函数；ζ_p为t_p时刻USBL量测噪声协方差矩阵。

4.根据权利要求1所述的一种基于因子图优化的UUV多海域综合导航系统设计方法，其特征是：所述多源信息因子图采用iSAM2算法其包含以下步骤：

(1)首先初始化因子图，得到贝叶斯树F，非线性因子f，线性化点Θ，初始化时均为空集；

(2)添加新的因子：f＝f∪f′；

(3)初始化新的变量Θ′并且有Θ＝Θ∪Θ′；

(4)进行平滑再线性化，获取所有标记的贝叶斯树子团M；

(5)通过F，M，对所有受到影响的因子消元，重新构造贝叶斯树；

(6)求解影响大的集合Δ(向量)；

(7)求解当前值

通过对GTSAM里面的ISAM2Params类进行实例化，对实例化的对象进行参数设置，参阈值大小设置为0.01(差值大于0.01要重新线性化)，将重新线性化任意变量参数设置为true，将计算线性化误差参数设置为false，将保存线性化结果参数设置为false以优化性能，设置矩阵分解模式为QR分解，将只进行部分更新功能参数设置为false。然后添加待优化的因子图，调用update函数进行更新。

Images