CN108364014A - 一种基于因子图的多源信息融合方法 - Google Patents

Abstract

本发明涉及一种基于因子图的多源信息融合方法，为实现复杂环境下不依赖于卫星导航的全源定位与导航，以惯性导航系统为核心，利用一切可用的导航信息源，对异步异构传感器信息快速融合、优化配置和自适应切换。通过递推的贝叶斯估计构建因子图模型，当获取不同传感器量测信息后，通过系统的变量节点和因子节点对因子图进行拓展，基于设置的代价函数完成状态的递推和更新，采用增量平滑的方法，通过稀疏的QR分解，对因子图优化问题进行求解；本发明有效地解决了载体运动和量测可用性之间产生的时变状态空间问题，能根据承载平台的动态变化而计算出精确导航的解决方案，实现多传感器的即插即用，满足载体在复杂环境变化和不同任务的需求。

Description

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

技术领域

本发明涉及一种融合方法，具体涉及一种基于因子图的多源信息融合方法，适用于复杂环境、不同需求及任务下的全源导航。

背景技术

卫星导航存在信号弱、穿透能力差、易受遮挡、干扰和欺骗等脆弱性问题，将导致载体在复杂环境下导航、定位与授时服务受限。为解决卫星导航系统固有脆弱性的制约，以美国国防高级研究计划局(DARPA)为首的先进军事技术研究机构于2010年启动了全源定位与导航(ASPN)项目。全源导航系统以惯导和精确时钟集成的授时和惯性测量单元为核心，按环境、需求和任务要求的不同，通过即插即用的方式，接入或移除，灵活地配置不同类型的传感器和敏感器，引入不同类型的测量量和特征数据库，同时，降低系统集成成本，为用户提供卫星导航服务受限条件下的高精度定位、导航与授时服务，满足不断变化的任务需求与环境变化的要求。

为了实现复杂环境下多传感器快速融合和优化配置，需要对多源信息融合技术进行研究。多传感器信息融合通过对多个传感器获得的信息进行协调、组合、互补来提高系统的有效性，取得比单一传感器更好的性能。目前一般通过多源信息分布式并行融合方法、联邦卡尔曼滤波方法、动静滤波技术、交互自适应方法等滤波方法对多传感器信息进行融合。

随着多源信息融合时传感器数量和种类的增加，应用环境的复杂多变及其需求、任务的不同，不同的传感器具有不同的更新频率，所有的传感器又很难在同一时间下工作，在时间上存在异步甚至延迟等；同时，随着应用环境复杂多变和特殊任务的需求，传感器的应用受到限制，会出现一个或者多个传感器量测信息阶段性异常或者不可用的情况等，现有的多源信息融合多采用滤波方法。但是，每次有新的量测信息可用时，都需要对整个拓展的状态变量进行更新，增加了算法的复杂性。同时，多传感器信息融合时还存在多源量测信息更新频率不同，时间异步，信号延时等问题，例如，大多数导航传感器(例如视觉、GPS、激光雷达等)更新频率较IMU低很多而且时间相互不同步；一些导航传感器(GPS等)存在信号短时间内不可用的情况，还有一些导航传感器(室内导航、地基PNT、水声定位等)只能提供局部区域内导航等等。因此，最佳的解决方案就是利用一切可用的传感器进行信息融合，并随着应用环境进行自由切换。

发明内容

本发明正是针对现有技术中存在的技术问题，提供一种基于因子图的多源信息融合方法，该技术方案通过构建因子图模型，基于设置的代价函数，利用不同传感器的变量节点和因子节点完成状态的递推和更新，基于增量平滑的方法，通过稀疏的QR分解，对因子图优化的问题进行求解，从而对异步异构传感器的数据进行快速融合和优化配置。

为了实现上述目的，本发明的技术方案如下，一种基于因子图的多源信息融合方法，其特征在于，所述方法如下：

(1)采用因子图表示多元随机变量的联合概率分布，将贝叶斯估计的因式分解应用在因子图中，建立基于贝叶斯估计的因子图模型；

(2)当获取不同传感器量测信息后，通过系统变量节点和因子节点对因子图进行拓展，基于设置的代价函数，利用不同传感器的因子节点和变量节点完成状态的递推和更新，建立基于因子图的多源信息融合模型；

(3)采用增量平滑的方法，以增量式对因子图进行精细的处理，通过稀疏的QR分解，加速对因子图优化的求解。本发明基于非线性优化的因子图的原理，提出了一种基于因子图的多源信息融合方法。因子图是一种典型的可以表示随机变量的联合概率分布的概率图模型，是一个通用的模型，将一个多元变量的全局函数因子分解成几个局部函数的乘积，以此为基础得到的一个双向图。每个变量对应于一个变量节点,每个局部函数对应于一个因子节点,这两类节点构成因子图的顶点集，因子图的边线由变量节点和函数的因子节点的连线组成。当且仅当该变量是因子的自变量时，对应的变量节点才与对应的因子节点相连，在因子图的基础上按一定规则进行消息传递和迭代就获得对图中参数的估计。利用可用的传感器(存在不同频率、异步、或者延时等的量测信息)，多源信息融合采用概率图模型(因子图)来表述，因式分解为所有量测量对应节点的概率分布函数，新的量测信息以因子节点形式包含进入因子图，实现复杂环境下多传感器快速融合和优化配置。

作为本发明的一种改进，所述步骤(1)具体如下，所述的因子图表示多元随机变量的联合概率分布，将一个多元变量的全局函数因子分解成几个局部函数的乘积，以此为基础得到的一个双向图；每个变量对应于一个变量节点,每个局部函数对应于一个因子节点，这两类节点构成因子图的顶点集，因子图的边线由变量节点和函数的因子节点的连线组成，当且仅当该变量是因子的自变量时，对应的变量节点才与对应的因子节点相连。将贝叶斯估计的因式分解应用在因子图中，基于设置的代价函数的约束，对因子图进行优化，通过调整变量节点的取值，使因子的乘积最小化，从而使得整个条件后验概率达到最大。在因子图的基础上按一定规则进行消息传递和迭代就获得对图中参数的估计。

作为本发明的一种改进，所述步骤(2)具体如下，基于因子图的多源信息融合模型以IMU为核心，其他的传感器辅助导航，建立步骤如下：

a.因子图模型的初始化；

在初始时刻，对因子图中定义变量节点ν和先验的因子节点f^Prior进行初始化设置；

b.因子图中变量节点与因子节点的引入；

当因子图引入新的变量节点X_k+1时，因子节点f^IMU用下面的代价函数来评估为：

其中，X_k表示k时刻载体的位置、速度和姿态的导航状态变量，C_k表示k时刻IMU的标定参数(包括常值漂移和随机游走项等)，用于修正IMU输出的数据，表示k时刻IMU输出的陀螺和加表的数据，h(.)为系统状态转移的函数矩阵，因子节点f^IMU连接不同时刻t_k和t_k+1时导航状态的变量节点X_k和X_k+1，C_k和偏置的因子节点f^bias(用来评估IMU误差)，为导航状态变量的估计值，当有新的IMU量测数据时，通过h(.)得到导航状态变量的预测值。类似地，偏置的因子节点f^bias连接不同时刻t_k和t_k+1时导航状态的变量节点C_k和C_k+1，用下面的代价函数来评估为：

f^bias(C_k+1,C_k)＝d(C_k+1-g(C_k))

其他传感器的量测方程可以表示为：

其中，h^others(.)是量测模型，n^others是观测噪声，当有新的量测信息可用时，会引入新的因子节点f^others，因子节点的引入不受多源传感器频率不同，时间异步或者延迟观测等影响。用下面的代价函数表示：

基于设置的代价函数，通过引入的因子节点和变量节点完成状态的递推和更新。

作为本发明的一种改进，所述步骤(3)具体如下，该算法充分利用了系统稀疏和图形拓扑来识别优化的变量。与每步迭代过程中都要对所有的节点和约束进行重新计算的批量式算法相比，増量式算法只需要更新要改变的地方，处理量少，更具时效性；同时，不断对雅克比矩阵进行稀疏化处理，减少了计算的复杂性。增量平滑算法的步骤如下：

a、当有新的量测信息时，在因子图中增加新的因子节点和变量节点；

b、用当前的估计对系统的代价函数线性化处理，求得系统代价函数的雅克比矩阵J和量测残差b；

c、通过稀疏的QR分解，对进行求解，得到状态变量的增量Δ(是状态变量的估计值)；

d、导航状态变量更新为ν^*:

作为本发明的一种改进，所述步骤(1)中所述的因子图定义为二分图模型G＝f(F,ν_0:k,E)，包括两类节点，分别为因子节点f_i∈F和变量节点e_ij∈E表示两个节点之间的连线，因子节点表示为局部函数的形式为：f₁(χ₁)、f₂(χ₂)、...、f_i(χ_i)...，表示多源信息融合量测更新的过程，为非负函数。由于每个因子节点在因式分解中是相对独立的，基于贝叶斯估计的因式分解推导因子图模型，因子图G＝f(F,ν_0:k,E)表示为因式分解的形式为：

依据非线性最小二乘优化理论，因子节点f(χ_i)定义为：

f(χ_i)＝d[(z_i-h(χ_i))^TW(z_i-h(χ_i))]＝min；

其中，h_i(χ_i)表示与状态变量相关的量测模型，z_i表示实际的量测值，d(.)表示代价函数，W为正定的加权矩阵，一般取值为量测的协方差矩阵时，此时为马尔科夫估计，是加权最小二乘估计中的最优估计，设H为量测矩阵，则状态估计值为：

在因子图中，基于设置的代价函数的约束，对因子图进行优化，也就是通过调整变量节点ν的取值，使它们因子的乘积最小化，从而使整个条件后验概率达到最大，变量节点的最优估计值ν^*应满足：

相对于现有技术，本发明具有如下优点，1)该方案能够对多源异步异构传感器存在的信息频率不同、时间异步或者延时等特殊情况，进行快速融合、优化配置和自适应切换，利用一切可用的传感器，实现多传感器的即插即用；2)不同于信息融合时常用的滤波的方法，本发明提出了一种基于因子图的多源信息融合方法，能根据承载平台的动态变化而计算出精确导航的解决方案，提高了导航精度；3)针对最大后验估计时应用批优化的方法计算量大的问题，本发明基于增量平滑的方法，通过稀疏的QR分解，对因子图优化的问题进行求解，该算法充分利用了系统稀疏和图形拓扑来识别优化的变量，处理量少，更具时效性；同时，不断对雅克比矩阵进行稀疏化处理，减少了计算的复杂性。

附图说明

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

图2为基于因子图的多源信息融合的系统框图；

图3为因子图中t₄时刻引入新的量测量时的示意图。

具体实施方式：

为了加深对本发明的理解，下面结合附图对本实施例做详细的说明。

实施例1：如图1所示，一种基于因子图的多源信息融合方法，本方案采用因子图的方法来解决复杂环境下多源信息融合问题，以IMU为核心，其他的传感器辅助导航。涉及到IMU、GNSS、视觉、磁强计、激光雷达、地基PNT、气压计七类传感器，来模拟多传感器存在的频率不同、异步或者延时等情况，随时随地利用可用的传感器，使载体满足复杂环境变化和不同任务的需求，实现多传感器的即插即用；具体实现步骤如下：

(1)建立基于贝叶斯估计的因子图模型，

在因子图中，IMU及其标定参数的状态方程分别定义为：

C_k+1＝g(C_k)(2)；其中，X_k表示k时刻载体的位置、速度和姿态的导航状态变量，C_k表示k时刻IMU的标定参数(包括常值漂移和随机游走项等)，用于修正IMU输出的数据，表示k时刻IMU输出的陀螺和加表的数据，h(.)和g(.)为系统状态转移的函数矩阵，定义k时刻估计的状态变量ν_k＝[X_k,C_k]，用Z_k表示k时刻所有可用的量测量，根据贝叶斯估计理论，联合条件后验概率密度函数P(ν_0:k|Z_1:k)可以因式分解为先验概率函数，似然概率函数和状态转移先验概率函数的乘积形式。

贝叶斯估计目的就是通过调整导航状态变量ν的取值，使得整个联合条件的后验概率达到最大，即：

ν^*＝argmaxP(ν_0:k|Z_1:k)(4)；将基于贝叶斯估计的因式分解应用在因子图中，因子图定义为二分图模型G＝f(F,ν_0:k,E)，包括两类节点，分别为因子节点f_i∈F和变量节点e_ij∈E表示两个节点之间的连线。因子节点表示为局部函数的形式为：f₁(χ₁)、f₂(χ₂)、...、f_i(χ_i)...，表示多源信息融合量测更新的过程，为非负函数。由于每个因子节点在P(ν_0:k|Z_1:k)因式分解中是相对独立的，因子图G＝f(F,ν_0:k,E)表示为因式分解的形式为：

依据非线性最小二乘优化理论，因子节点f(χ_i)定义为：

f(χ_i)＝d[(z_i-h(χ_i))^TW(z_i-h(χ_i))]＝min(6)；其中，h_i(χ_i)表示与状态变量相关的量测模型，z_i表示实际的量测值，d(.)表示代价函数，W为正定的加权矩阵，一般取值为量测的协方差矩阵时，此时为马尔科夫估计，是加权最小二乘估计中的最优估计，H表示量测矩阵，则状态估计值为：

在因子图中，基于设置的代价函数的约束，对因子图进行的优化，就是通过调整变量节点ν的取值，使它们因子的乘积最小化，从而使得整个条件后验概率达到最大，变量节点的最优估计值ν^*应满足：

(2)、建立基于因子图的多源信息融合模型：

a.因子图模型的初始化；

在初始时刻，对因子图中定义变量节点ν和先验的因子节点f^Prior进行初始化设置；

b.因子图中变量节点与因子节点的引入；

当因子图引入新的变量节点X_k+1时，因子节点f^IMU用式(6)的代价函数来评估为：

因子节点f^IMU连接不同时刻t_k和t_k+1时导航状态的变量节点X_k和X_k+1，C_k和偏置的因子节点f^bias(用来评估IMU误差)，为导航状态变量的估计值。当有新的IMU量测数据时，通过h(.)得到导航状态变量的预测值，而代价函数通过其于状态变量的估计值的差值得到。类似地，偏置的因子节点f^bias连接不同时刻t_k和t_k+1时导航状态的变量节点C_k和C_k+1，用式(6)的代价函数来评估为：

f^bias(C_k+1,C_k)＝d(C_k+1-g(C_k))(10)；

其他传感器的量测方程可以表示为：

其中，h^others(.)是量测模型，n^others是观测噪声，当有新的量测信息可用时，会引入新的因子节点f^others，因子节点的引入不受多源传感器频率不同，时间异步或者延迟观测等影响。同样通过式(6)的代价函数表示：

基于设置的代价函数，通过引入的因子节点和变量节点完成状态的递推和更新。

(3)、基于增量平滑的因子图的优化求解；

对于因子图中非线性优化问题的求解，可以采用一个标准的高斯-牛顿非线性优化器进行重复线性化处理。因子图中，决定系统性能的一个重要因素是矩阵中元素的顺序，变量的不同排序能产生或多或少的填充，变量消除顺序将影响贝叶斯网络的结构和相应的计算量。本发明采用增量平滑的方法，对因子图进行精细的处理，以增量式地处理因子图优化，通过稀疏的QR分解，加速对图因子优化的求解。增量式平滑算法的步骤如下：

a、当有新的量测信息时，在因子图中增加新的因子节点和变量节点。

b、用当前的估计对系统的代价函数线性化处理，求得系统代价函数的雅克比矩阵J和量测残差b。

c、通过稀疏的QR分解，对进行求解，求得状态变量的增量Δ(是状态变量的估计值)。

d、导航状态变量值更新为：

当有新的量测信息可用时，新的因子节点和变量节点会引入到因子图中，线性化处理得到新的雅克比矩阵J'，通过J'＝QR'因式分解，求得上三角平方根的信息矩阵R'。

在雅克比矩阵J'中，非零项用×表示，每一行代表一个线性化的因子节点，因子图中每一个新的因子节点加入，对应的在原来量测的雅克比矩阵中增加一行。应用增量平滑的方法对贝叶斯网络进行估计时，只需关注与引入的因子节点有关部分，而不需要对整个贝叶斯网络进行重新估计。例如，在t₄时刻，当接收到新的视觉导航信息可用时，新的变量节点X₄，C₄，因子节点f^visual，f^IMU，f^bias引入到因子图中，在公式(13)中，×表示在原来矩阵基础上的更改项，J'增加了3行，R'新增了4行。当有新的量测时，增量平滑算法直接更新平方根因子矩阵R，而不是重新求矩阵R，同时也可以不必重新计算雅克比矩阵，对其每一步都重新分解，降低了算法的复杂性。

需要说明的是上述实施例，并非用来限定本发明的保护范围，在上述技术方案的基础上所做出的等同变换或替代均落入本发明权利要求所保护的范围。

Claims (6)

1.一种基于因子图的多源信息融合方法，其特征在于，所述方法如下：

(1)采用因子图表示多元随机变量的联合概率分布，将贝叶斯估计的因式分解应用在因子图中，建立基于贝叶斯估计的因子图模型；

(2)当获取不同传感器量测信息后，通过系统变量节点和因子节点对因子图进行拓展，基于设置的代价函数，利用不同传感器的因子节点和变量节点完成状态的递推和更新，建立基于因子图的多源信息融合模型；

(3)采用增量平滑的方法，以增量式对因子图进行精细的处理，通过稀疏的QR分解，加速对因子图优化的求解。

2.根据权利要求1所述的基于因子图的多源信息融合方法，其特征在于，所述步骤(1)具体如下，所述的因子图表示多元随机变量的联合概率分布，将一个多元变量的全局函数因子分解成几个局部函数的乘积，以此为基础得到的一个双向图；每个变量对应于一个变量节点,每个局部函数对应于一个因子节点，这两类节点构成因子图的顶点集，因子图的边线由变量节点和函数的因子节点的连线组成，当且仅当该变量是因子的自变量时，对应的变量节点才与对应的因子节点相连。

3.根据权利要求2所述的基于因子图的多源信息融合方法，其特征在于，所述步骤(2)具体如下，基于因子图的多源信息融合模型以IMU为核心，其他的传感器辅助导航，建立步骤如下：

a.因子图模型的初始化；

在初始时刻，对因子图中定义变量节点ν和先验的因子节点f^Prior进行初始化设置；

b.因子图中变量节点与因子节点的引入；

当因子图引入新的变量节点X_k+1时，因子节点f^IMU用下面的代价函数来评估为：

其中，X_k表示k时刻载体的位置、速度和姿态的导航状态变量，C_k表示k时刻IMU的标定参数(包括常值漂移和随机游走项等)，用于修正IMU输出的数据，表示k时刻IMU输出的陀螺和加表的数据，h(.)为系统状态转移的函数矩阵，因子节点f^IMU连接不同时刻t_k和t_k+1时导航状态的变量节点X_k和X_k+1，C_k和偏置的因子节点f^bias(用来评估IMU误差)，为导航状态变量的估计值，当有新的IMU量测数据时，通过h(.)得到导航状态变量的预测值。

4.根据权利要求3所述的基于因子图的多源信息融合方法，其特征在于，所述步骤(3)具体中增量平滑算法的步骤如下：

a、当有新的量测信息时，在因子图中增加新的因子节点和变量节点；

b、用当前的估计对系统的代价函数线性化处理，求得系统代价函数的雅克比矩阵J和量测残差b；

c、通过稀疏的QR分解，对进行求解，得到状态变量的增量Δ(是状态变量的估计值)；

d、导航状态变量更新为ν^*:

5.根据权利要求2或3或4所述的基于因子图的多源信息融合方法，其特征在于，所述步骤(1)中所述的因子图定义为二分图模型G＝f(F,ν_0:k,E)，包括两类节点，分别为因子节点f_i∈F和变量节点e_ij∈E表示两个节点之间的连线，因子节点表示为局部函数的形式为：f₁(χ₁)、f₂(χ₂)、...、f_i(χ_i)...，表示多源信息融合量测更新的过程，为非负函数。

6.根据权利要求5所述的基于因子图的多源信息融合方法，其特征在于，所述因子图G＝f(F,ν_0:k,E)表示为因式分解的形式为：

依据非线性最小二乘优化理论，因子节点f(χ_i)定义为：

其中，h_i(χ_i)表示与状态变量相关的量测模型，z_i表示实际的量测值，d(.)表示代价函数，W为正定的加权矩阵，设H为量测矩阵，则状态估计值为：

在因子图中，基于设置的代价函数的约束，对因子图进行优化，通过调整变量节点ν的取值，使它们因子的乘积最小化，从而使整个条件后验概率达到最大，变量节点的最优估计值ν^*应满足：