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

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

An AUV multi-source information fusion method and equipment based on factor graph

【技术实现步骤摘要】
一种基于因子图的AUV多源信息融合方法和设备
本专利技术涉及组合导航、信息融合领域，特别是一种基于因子图的AUV多源信息融合方法和设备。
技术介绍
自主式水下航行器(AutonomousUnderwaterVehicle,AUV)在海洋资源勘探、海洋环境保护和利用、海洋军事和救援等方面具有重要意义。而AUV导航系统能为其提供姿态、速度、位置等实时的导航信息，精确可靠的导航信息是AUV能够成功完成任务的重要保证。由于纯惯性导航系统存在误差随时间累积的不足，目前最常采用融合多种水下导航传感器提供的辅助信息来校正捷联惯性导航系统(StrapdownInertialNavigationSystem,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)。它包括两类节点：一类是因子节点fi∈F，是指因式分解中的局部函数；另一类是变量节点xj∈X，是指全局多元函数中的变量。边缘eij∈E是指，当且仅当因子节点fi与状态变量节点xj相关时存在一条连接边。将因子图的概念引入AUV多源信息组合导航系统的信息融合问题中，可以得到所有导航状态变量和量测信息的联合概率密度并用因子图模型表示。如图1所示，本专利技术提出一种基于因子图的AUV多源信息融合方法，以中高精度的捷联惯性导航系统(SINS)为核心，辅以多普勒测速仪(DVL)、磁罗盘(MCP)、地形匹配定位系统(TAN)以及压力深度计(DM)共同组成AUV用惯性基组合导航系统。选取因子图中的状态变量节点x为：x＝[φxφyφzδVNδVEδVDδLδλδhεxεyεz▽x▽y▽z]T其中，x表示导航状态误差量，包含了姿态误差(φx、φy、φz)、速度误差(δVN、δVE、δVD)、位置误差(δL、δλ、δh)、陀螺仪常值偏差(εx、εy、εz)和加速度计常值偏差(▽x、▽y、▽z)，下标xyz分别表示载体坐标系前向、右向、下向，NED分别表示导航坐标系北向、东向和地向、Lλh分别表示纬度、经度和高度。步骤S1，建立AUV多源信息组合导航系统因子图模型。步骤如下：a.在初始时刻，对因子图模型中定义的变量节点x0和先验因子节点fprior进行初始化设置。b.在tk时刻接收来自IMU的量测信息后，定义并添加新的IMU因子节点fIMU如下：其中，d(·)代表相应的代价函数，h(·)为系统状态转移矩阵,Λi是IMU量测过程噪声协方差矩阵，为平方马氏距离。一个IMU因子节点连接了两个相邻时刻tk和tk+1的两个变量节点xk和xk+1，我们可以根据IMU量测信息和当前的导航状态xk来预测下一时刻的导航状态以此来进行导航状态的更新和变量节点的扩增，即c.在tj时刻接收来自DVL的量测信息后，定义并添加新的DVL因子节点fDVL如下：其中，Σj为DVL量测噪声协方差矩本文档来自技高网...

【技术保护点】
1.一种基于因子图的AUV多源信息融合方法，其特征在于，包括以下步骤：/nS1、将AUV组合导航系统的导航状态误差抽象为变量节点，将来自不同异步异构导航传感器的量测信息抽象为因子节点，构建AUV多源信息组合导航系统因子图模型；/nS2、建立AUV多源信息组合导航系统的线性离散状态空间模型，并以此为基础对因子图模型进行详细分解；/nS3、利用和积算法在AUV多源信息组合导航系统因子图模型中的传递与更新，来实现对所有导航状态的最优估计。/n

【技术特征摘要】
1.一种基于因子图的AUV多源信息融合方法，其特征在于，包括以下步骤：
S1、将AUV组合导航系统的导航状态误差抽象为变量节点，将来自不同异步异构导航传感器的量测信息抽象为因子节点，构建AUV多源信息组合导航系统因子图模型；
S2、建立AUV多源信息组合导航系统的线性离散状态空间模型，并以此为基础对因子图模型进行详细分解；
S3、利用和积算法在AUV多源信息组合导航系统因子图模型中的传递与更新，来实现对所有导航状态的最优估计。


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


3.根据权利要求2所述的基于因子图的AUV多源信息融合方法，其特征在于，所述步骤S1包括：
S11、在初始时刻，对因子图模型中定义的变量节点x0和先验因子节点fprior进行初始化设置；
S12、在tk时刻接收来自IMU的量测信息后，定义并添加新的IMU因子节点fIMU如下：



其中，Λi是IMU量测过程噪声协方差矩阵，为平方马氏距离，h(·)为系统状态转移矩阵，d(·)代表相应的代价函数，xk和xk+1为IMU因子节点连接的两个相邻时刻tk和tk+1的两个变量节点，为下一时刻的导航状态估计值；
S13、在tj时刻接收来自DVL的量测信息后，定义并添加新的DVL因子节点fDVL如下：



其中，Σj为DVL量测噪声协方差矩阵，hDVL(·)为DVL量测方程；
S14、在tm时刻接收来自MCP的量测信息后，定义并添加新的MCP因子节点fMCP如下：



其中，Γm为MCP量测噪声协方差矩阵，hMCP(·)为MCP量测方程；
S15、在tn时刻接收来自TAN的量测信息后，定义并添加新的TAN因子节点fTAN如下：



其中，Ωn为TAN量测噪声协方差矩阵，hTAN(·)为TAN量...

【专利技术属性】
技术研发人员：刘锡祥，马晓爽，李晨龙，赵苗苗，张玉鹏，
申请(专利权)人：东南大学，
类型：发明
国别省市：江苏;32