CN110837854A - 一种基于因子图的auv多源信息融合方法和设备 - Google Patents

Abstract

本发明提供了一种基于因子图的AUV多源信息融合方法和设备，以实现惯性基自主式水下航行器(Autonomous Underwater Vehicle,AUV)组合导航系统的导航与定位。该方法利用因子图理论将AUV组合导航系统的信息融合问题转化为求解多元随机变量的联合概率分布的最大后验概率问题，并用因子图模型表示。根据因子图消息传递算法即和积算法在AUV多源信息组合导航系统的因子图模型中的传递与更新以实现导航状态的最优估计。本发明有效解决了AUV组合导航系统中不同导航信息源更新频率异步及短时失效的问题，适用于水下复杂多变的环境与任务需求。

Description

一种基于因子图的AUV多源信息融合方法和设备

技术领域

本发明涉及组合导航、信息融合领域，特别是一种基于因子图的AUV多源信息融合方法和设备。

背景技术

自主式水下航行器(Autonomous Underwater Vehicle,AUV)在海洋资源勘探、海洋环境保护和利用、海洋军事和救援等方面具有重要意义。而AUV导航系统能为其提供姿态、速度、位置等实时的导航信息，精确可靠的导航信息是AUV能够成功完成任务的重要保证。由于纯惯性导航系统存在误差随时间累积的不足，目前最常采用融合多种水下导航传感器提供的辅助信息来校正捷联惯性导航系统(Strapdown Inertial NavigationSystem,SINS)的累积误差，即AUV多源信息组合导航系统。但是由于传感器测量机理、海水介质、水下工作环境的复杂性等原因，在多源信息融合过程中往往存在区域性与阶段性、更新频率异步、时间延迟、易被干扰的不可完全依赖性等问题。例如，AUV在不同航行阶段，可用辅助信息源不同；同一阶段的不同区域，可用辅助信息源及其利用方式均可能发生变化；大多数水下导航传感器(例如多普勒测速仪DVL,磁罗盘MCP,地形匹配定位系统TAN,深度计DM等)更新频率较惯性测量单元IMU频率低很多且异步；一些导航传感器(DVL)易发生短时失效或间歇性失效的情况；一些导航传感器(MCP)精度不高且易受到环境干扰；还有一些导航传感器(TAN)只能提供局部区域内导航。

随着水下导航传感器的不断丰富，如何有效融合多种传感器的辅助信息来提高AUV组合导航系统精度成为关键。目前大多采用的是分布式结构的联邦卡尔曼滤波及其各类改进方法对多源信息进行融合。但由于上述问题的存在，该类方法在融合时间点上通常会由于信息源采样时间异步带来导航精度的损失，而且信息源故障和变化引起的系统重构等难题也会对系统的算法复杂度、实时性和灵活性造成影响，这在高动态环境下显得尤为不利。

发明内容

发明目的：针对现有技术的缺陷，本发明提供一种基于因子图的AUV多源信息融合方法和设备，以解决AUV组合导航系统中不同导航信息源更新频率异步及短时失效等问题，适用于其水下复杂多变的环境与任务需求。

技术方案：根据本发明的第一方面，提供一种基于因子图的AUV多源信息融合方法，包括：

S1、将AUV组合导航系统的导航状态误差抽象为变量节点，将来自不同异步异构导航传感器的量测信息抽象为因子节点，构建AUV多源信息组合导航系统因子图模型；

S2、建立AUV多源信息组合导航系统的线性离散状态空间模型，并以此为基础对因子图模型进行详细分解；

S3、利用和积算法在AUV多源信息组合导航系统因子图模型中的传递与更新，来实现对所有导航状态的最优估计。

其中，所述AUV组合导航系统的异步异构导航传感器包括：多普勒测速仪DVL、磁罗盘MCP、地形辅助定位系统TAN、深度计DM、惯性测量单元IMU。

作为优选，所述方法在步骤S1后还包括：将两个相邻量测信息之间的多个IMU因子节点等效为一个IMU因子节点，对因子图模型进行优化。

根据本发明的第二方面，提供一种计算机设备，所述设备包括：

一个或多个处理器；

存储器；以及

一个或多个程序，其中所述一个或多个程序被存储在所述存储器中，并且被配置为由所述一个或多个处理器执行，所述程序被处理器执行时实现如本发明第一方面所述的步骤。

有益效果：本发明首次提出用因子图的理论来解决多源信息融合的问题，将AUV组合导航系统的信息融合问题转化为求解多元随机变量的联合概率分布的最大后验概率问题，并用因子图(双向图)模型表示。因子图能够实现惯性导航系统与不同类型传感器之间异步信息的快速融合。通过简单地增减因子节点可以实现传感器的即插即用和重新配置等功能，具有很好的灵活性和可扩展性。此外，通过将两个相邻量测信息之间的多个IMU因子节点等效为一个IMU因子节点对因子图模型进行相应的优化，在不损失精度的前提下有效降低了计算量，进一步提高了导航性能。

附图说明

图1为基于因子图的AUV多源信息融合方法流程图；

图2为AUV多源信息组合导航系统因子图模型；

图3为简化后的AUV多源信息组合导航系统因子图模型；

图4为详细分解后的AUV多源信息组合导航系统因子图模型；

图5为AUV模拟海底作业轨迹；

图6为基于因子图与联邦卡尔曼滤波方法位置误差对比曲线。

具体实施方式

以下结合附图，详细描述本发明的实施例，所述实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

近年来在全源导航需求的牵引下，因子图方法为导航系统的信息融合提供了一个新思路。因子图方法作为一种概率图模型，它可以将抽象数学问题中变量之间的关系进行可视化表达，进而将复杂的计算过程转化为因子图上的消息传递和更新来完成问题求解。因子图是用来表达随机变量的联合概率分布的双向图模型G＝(F,X,E)。它包括两类节点：一类是因子节点f_i∈F，是指因式分解中的局部函数；另一类是变量节点x_j∈X，是指全局多元函数中的变量。边缘e_ij∈E是指，当且仅当因子节点f_i与状态变量节点x_j相关时存在一条连接边。将因子图的概念引入AUV多源信息组合导航系统的信息融合问题中，可以得到所有导航状态变量和量测信息的联合概率密度并用因子图模型表示。

如图1所示，本发明提出一种基于因子图的AUV多源信息融合方法，以中高精度的捷联惯性导航系统(SINS)为核心，辅以多普勒测速仪(DVL)、磁罗盘(MCP)、地形匹配定位系统(TAN)以及压力深度计(DM)共同组成AUV用惯性基组合导航系统。选取因子图中的状态变量节点x为：

x＝[φ_x φ_y φ_z δV_N δV_E δV_D δL δλ δh ε_x ε_y ε_z ▽_x ▽_y ▽_z]^T

其中，x表示导航状态误差量，包含了姿态误差(φ_x、φ_y、φ_z)、速度误差(δV_N、δV_E、δV_D)、位置误差(δL、δλ、δh)、陀螺仪常值偏差(ε_x、ε_y、ε_z)和加速度计常值偏差(▽_x、▽_y、▽_z)，下标xyz分别表示载体坐标系前向、右向、下向，NED分别表示导航坐标系北向、东向和地向、Lλh分别表示纬度、经度和高度。

步骤S1，建立AUV多源信息组合导航系统因子图模型。

步骤如下：

a.在初始时刻，对因子图模型中定义的变量节点x₀和先验因子节点f^prior进行初始化设置。

b.在t_k时刻接收来自IMU的量测信息

后，定义并添加新的IMU因子节点f^IMU如下：

其中，d(·)代表相应的代价函数，h(·)为系统状态转移矩阵,Λ_i是IMU量测过程噪声协方差矩阵，

为平方马氏距离。

一个IMU因子节点连接了两个相邻时刻t_k和t_k+1的两个变量节点x_k和x_k+1，我们可以根据IMU量测信息

和当前的导航状态x_k来预测下一时刻的导航状态以此来进行导航状态的更新和变量节点的扩增，即

c.在t_j时刻接收来自DVL的量测信息后，定义并添加新的DVL因子节点f^DVL如下：

其中，Σ_j为DVL量测噪声协方差矩阵，h^DVL(·)为DVL量测方程。

d.在t_m时刻接收来自MCP的量测信息后，定义并添加新的MCP因子节点f^MCP如下：

其中，Γ_m为MCP量测噪声协方差矩阵，h^MCP(·)为MCP量测方程。

e.在t_n时刻接收来自TAN的量测信息

后，定义并添加新的TAN因子节点f^TAN如下：

其中，Ω_n为TAN量测噪声协方差矩阵，h^TAN(·)为TAN量测方程。

f.在t_l时刻接收来自DM的量测信息

后，定义并添加新的DM因子节点f^DM如下：

其中，Υ_l为DM量测噪声协方差矩阵，h^DM(·)为DM量测方程。

上述b～f没有先后顺序要求，体现了因子图即插即用的灵活性。

步骤S2，基于惯性基AUV组合导航系统中IMU更新频率总是高于其他辅助传感器更新频率的客观事实和AUV水下运动平稳等特性，对因子图模型进行优化。

图2所示因子图模型总是以IMU更新频率来添加新的变量节点。由于传感器测量机理等原因，在AUV多源信息组合导航系统的因子图模型中，通常以较快的频率添加变量节点和IMU因子节点(IMU以较快的速率采集加速度与角速度信息)，而以较慢的频率添加其他因子节点(其他辅助导航传感器以较慢的频率采集辅助信息)。基于此，将两个相邻量测信息之间的多个IMU因子节点等效为一个IMU因子节点，即在一个量测更新周期内不进行状态变量节点x_k和IMU因子节点f^IMU的更新，而仅根据SINS参数的变化求解系统矩阵F，并进行累加。在这里不妨假设IMU更新频率为200HZ，在量测信息到达时刻，计算状态转移矩阵Φ如下：

式中，I为单位矩阵，T为量测更新周期，F_k是第k时刻的系统矩阵，

为等效的系统矩阵。这样，可以大大减少必须添加到因子图模型中的状态节点的数量。简化后的因子图模型如图3所示。

步骤S3，建立AUV多源信息组合导航系统的线性离散状态空间模型并以此为基础对因子图模型进行详细分解。

AUV组合导航系统的线性离散状态空间模型建立如下：

X_k＝Φ_k/k-1X_k-1+Γ_k/k-1W_k-1

Z_k＝H_kX_k+V_k

式中，X_k是n×1维的状态向量，Z_k是m×1维的量测向量；Φ_k/k-1是n×n维的状态一步转移矩阵，Γ_k/k-1是n×l维的系统噪声分配矩阵，H_k是m×n维的量测矩阵；W_k-1是l×1维的系统噪声向量，V_k是m×1维的量测噪声向量，两者都是零均值的高斯白噪声向量序列(服从正态分布)且互不相关。

对上述线性离散状态空间方程进行详细分解，绘制AUV多源信息组合导航系统因子图如图4所示。

步骤S4，利用因子图的消息传递算法——和积算法在AUV多源信息组合导航系统因子图模型中的传递与更新，来实现对所有导航状态的最优估计。

和积算法在AUV多源信息组合导航系统因子图模型中的传递与更新，可表示为高斯消息在线性状态空间模型中的传递与更新。其中，所有消息都服从多维高斯概率密度分布。用均值向量m和方差矩阵V，或者权重矩阵

和精度-权重均值向量Wm来描述一个高斯分布。如果某个变量节点的前向消息中均值向量表示为

方差矩阵表示为

权重矩阵表示为

后向消息中均值向量表示为

方差矩阵表示为

权重矩阵表示为

那么可以利用下表在因子图中每条边的两个方向各传递一次消息，以此来计算因子图模型中的所有消息。其中，(.)^H表示Hermitian转置矩阵，(.)^#表示Moore-Penrose广义逆矩阵。

根据上表在因子图中的传递与更新就可以得到所有导航状态的最优估计，因子图中的所有变量节点即为所有导航状态的最优估计。本发明提供的基于因子图的AUV多源信息融合方法利用所有可用的信息源实现传感器的即插即用，具有良好的灵活性与扩展性。

通过如下仿真试验验证本发明所提方法的性能效果：

在仿真中设置IMU采样时间间隔为0.05s，陀螺常值漂移误差为0.02°/h，角度随机游走系数为0.01(°/h)Hz^0.5；加速度计常值飘逸误差为100μg，速度随机游走系数为10(μg/Hz)^0.5。DVL采样时间间隔为1s，MCP采样时间间隔为0.5s，TAN采样时间间隔为3s，DM采样时间间隔为2s。设置其失效区间如下：DVL失效区间150s～350s；TAN失效区间450s～550s；MCP失效区间800s～850s；DM失效区间650s～700s；SINS作为一种自主工作的导航系统，抗干扰能力强，因此设置IMU信息在整个仿真时间段内始终有效。

为了充分验证本发明所提因子图方法的有效性与可靠性，分别采用联邦卡尔曼滤波、未简化的因子图作为对比方法。AUV在海底作业的三维模拟运动真实轨迹、基于因子图方法以及联邦卡尔曼滤波方法的仿真曲线如图5所示。

从图6位置误差曲线中可以看出，在整个仿真时间段内，本发明提供的基于因子图的AUV多源信息融合方法与其他两种方法始终都能保持相对较高的定位精度，验证了本发明所提方法的有效性与可靠性。简化后的因子图方法与未简化的因子图方法相比，每次导航解算结果差异几乎相同，导航精度相当。在传感器失效区间，由于可利用的量测信息源减少，与其他时间段相比，定位精度会有所降低。

综上所述，本发明能够实现惯性导航系统与不同类型传感器之间的异步信息融合，并且可以快速灵活的实现传感器的即插即用和重新配置等功能，有效地解决了AUV组合导航系统中不同导航信息源更新频率异步及短时失效等问题，适用于水下复杂多变的环境与任务需求，应用前景广阔。

基于与方法实施例相同的技术构思，根据本发明的另一实施例，提供一种计算机设备，所述设备包括：一个或多个处理器；存储器；以及一个或多个程序，其中所述一个或多个程序被存储在所述存储器中，并且被配置为由所述一个或多个处理器执行，所述程序被处理器执行时实现方法实施例中的各步骤。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

Claims (8)

1.一种基于因子图的AUV多源信息融合方法，其特征在于，包括以下步骤：

S1、将AUV组合导航系统的导航状态误差抽象为变量节点，将来自不同异步异构导航传感器的量测信息抽象为因子节点，构建AUV多源信息组合导航系统因子图模型；

S2、建立AUV多源信息组合导航系统的线性离散状态空间模型，并以此为基础对因子图模型进行详细分解；

S3、利用和积算法在AUV多源信息组合导航系统因子图模型中的传递与更新，来实现对所有导航状态的最优估计。

2.根据权利要求1所述的基于因子图的AUV多源信息融合方法，其特征在于，所述AUV组合导航系统的异步异构导航传感器包括：多普勒测速仪DVL、磁罗盘MCP、地形辅助定位系统TAN、深度计DM、惯性测量单元IMU。

3.根据权利要求2所述的基于因子图的AUV多源信息融合方法，其特征在于，所述步骤S1包括：

S11、在初始时刻，对因子图模型中定义的变量节点x₀和先验因子节点f^prior进行初始化设置；

S12、在t_k时刻接收来自IMU的量测信息

后，定义并添加新的IMU因子节点f^IMU如下：

其中，Λ_i是IMU量测过程噪声协方差矩阵，

为平方马氏距离，h(·)为系统状态转移矩阵，d(·)代表相应的代价函数，x_k和x_k+1为IMU因子节点连接的两个相邻时刻t_k和t_k+1的两个变量节点，

为下一时刻的导航状态估计值；

S13、在t_j时刻接收来自DVL的量测信息

后，定义并添加新的DVL因子节点f^DVL如下：

其中，Σ_j为DVL量测噪声协方差矩阵，h^DVL(·)为DVL量测方程；

S14、在t_m时刻接收来自MCP的量测信息

后，定义并添加新的MCP因子节点f^MCP如下：

其中，Γ_m为MCP量测噪声协方差矩阵，h^MCP(·)为MCP量测方程；

S15、在t_n时刻接收来自TAN的量测信息

后，定义并添加新的TAN因子节点f^TAN如下：

其中，Ω_n为TAN量测噪声协方差矩阵，h^TAN(·)为TAN量测方程；

S16、在t_l时刻接收来自DM的量测信息

后，定义并添加新的DM因子节点f^DM如下：

其中，Υ_l为DM量测噪声协方差矩阵，h^DM(·)为DM量测方程。

4.根据权利要求1所述的基于因子图的AUV多源信息融合方法，其特征在于，所述步骤S2中AUV组合导航系统的线性离散状态空间模型建立如下：

X_k＝Φ_k/k-1X_k-1+Γ_k/k-1W_k

Z_k＝H_kX_k+V_k

其中，X_k是n×1维的状态向量，Z_k是m×1维的量测向量；Φ_k/k-1是n×n维的状态一步转移矩阵，Γ_k/k-1是n×l维的系统噪声分配矩阵，H_k是m×n维的量测矩阵；W_k-1是l×1维的系统噪声向量，V_k是m×1维的量测噪声向量，两者都是零均值的高斯白噪声向量序列且互不相关。

5.根据权利要求1所述的基于因子图的AUV多源信息融合方法，其特征在于，所述步骤S3包括：

S31、设置初始消息服从多维高斯概率密度分布；

S32、利用和积算法的传递与更新规则在因子图中每条边的两个方向各传递一次消息，得到对所有导航状态变量的最优估计。

6.根据权利要求3所述的基于因子图的AUV多源信息融合方法，其特征在于，在步骤S1得到AUV多源信息组合导航系统因子图模型后还包括：将两个相邻量测信息之间的多个IMU因子节点等效为一个IMU因子节点，加速两个相邻量测信息之间的SINS误差估计过程，对因子图模型进行优化。

7.根据权利要求6所述的基于因子图的AUV多源信息融合方法，其特征在于，所述优化方法如下：在一个量测更新周期内不进行状态变量节点x_k和IMU因子节点f^IMU的更新，而仅根据SINS参数的变化求解系统矩阵F，并进行累加。

8.一种计算机设备，其特征在于，所述设备包括：

一个或多个处理器；

存储器；以及

一个或多个程序，其中所述一个或多个程序被存储在所述存储器中，并且被配置为由所述一个或多个处理器执行，所述程序被处理器执行时实现如权利要求1-7中的任一项所述的步骤。

Images