一种基于因子图的多源导航信息融合方法与流程

本发明属于组合导航技术领域，特别涉及了一种基于因子图的多源导航信息融合方法。

背景技术：

随着科学技术的不断发展，人们对动态载体运动目标(飞机、车辆、舰船等)的状态估计精度要求越来越高，依赖单一传感器进行导航已不能满足需求，于是出现了多传感器的组合导航。多传感器信息融合应运而生，将多传感器提供的信息按某种融合准则进行最优融合，提高状态估计精度。融合导航的发展以导航传感器为基础。在组合导航系统中，常用的传感器系统包括：惯性导航系统、卫星导航系统、天文导航系统、多普勒导航系统、地形匹配导航系统、景象匹配导航、航位推算导航等。由于各传感器所采用的导航原理不同，各类传感器之间存在极强的互补性。在实际应用中，由于不同传感器的更新频率不同，存在时间不同步的问题，同时在组合过程中，各传感器在不同环境和条件下的使用局限，可用性会发生变化，会影响到滤波结构，比如，卫星导航信号难以穿透地面和建筑物等密度较大的物质，在都市、室内、地下环境中信号的衰减现象非常严重，导致GPS难以正常工作。采用固定的滤波结构和方法均不能很好地满足这种复杂多变的应用需求。

概率图模型是一种以图模型来表示变量概率依存关系的理论。概率图模型可以分为三类，分别是有向概率图模型、无向概率图模型和混合概率图模型；其中，有向概率图模型又可以分为单向图和双向图。因子图(Factor Graph,FG)是一种双向概率图模型，包含贝叶斯网络、马尔科夫随机场及Tanner图等多种图模型。图中包含两种类型的节点：一种是变量节点，代表全局多元函数中的变量；一种是因子节点，代表因式分解中的局部函数。每个局部函数只与全局多元函数中的部分变量相关，当且仅当变量是局部函数的自变元时，因子图中与之相应的变量节点与因子节点之间存在一条连接边。因子图作为一种分析问题的图形工具在很多领域都得到了广泛应用，例如：信号处理、人工智能、神经网络等。

因子图作为一种图模型工具，在编解码领域有着广泛的应用，同时，也被应用于统计学、信号处理和人工智能领域。有文献对基于因子图的信道估计与均衡及译码的迭代及进行了研究；也有文献利用因子图提出一种多目的地地图的生成方法。但总的来说，目前对因子图的应用主要集中在通信解码领域，对将因子图与多源信息融合算法结合起来的研究很少。

技术实现要素：

为了解决上述背景技术提出的技术问题，本发明旨在提供一种基于因子图的多源导航信息融合方法，将因子图与多源信息融合算法结合起来，实现对不同步量测信息的处理及对导航精度的需求。

为了实现上述技术目的，本发明的技术方案为：

一种基于因子图的多源导航信息融合方法，包括以下步骤：

(1)在机载动态环境下，根据载体的实际情况、任务需求以及所处环境设计载体的运动动作、运动轨迹，并确定与惯性测量单元进行组合导航的其他各类机载高精度传感器；

(2)通过惯性测量单元中的陀螺仪和加速度计获取载体的角速度信息和比力信息，同时，通过其他各类机载高精度传感器获取载体各类量测信息；

(3)定义导航系统状态矢量为因子图的变量节点，定义惯性测量单元以及其他各类机载高精度传感器获取的载体量测信息为因子图的因子节点，从而构建基于因子图的多源导航信息融合框架；

(4)在基于因子图的多源导航信息融合框架下，表征系统状态矢量和量测信息更新过程，建立滤波方程，经过实时滤波估计和修正，完成多源导航信息的有效融合。

进一步地，步骤(4)的具体过程如下：

(a)选择系统状态量X，构建导航系统的状态方程和量测方程；

(b)选取多源导航信息融合的约束规则：

$P\left(X\right)=\underset{n\∈N}{\Π}{f}_n\left(X_k\right)---\left(1\right)$

式(1)中，P(X)为定义在系统状态量X上的联合分布函数，f_n(X_k)为因子节点，N为因子节点数，X_k为k时刻的系统状态量；

(c)建立惯性测量单元的因子节点表达式，建立其他各类机载高精度传感器的因子节点表达式；

(d)选择因子节点的代价函数，并在其值取最小时对系统状态量X求偏导数，从而得到状态量X的估计。

进一步地，在步骤(c)中，惯性测量单元因子节点的表达式：

f_IMU＝L(X_k+1-F(X_k,Z_IMU)) (2)

其他各类机载高精度传感器的因子节点表达式：

f(X_k)＝L(Z_k-H) (3)

其中，L(·)是代价函数，H是量测函数，是对X_k的估计，Z_k为实际量测值，F是系统的传递函数矩阵，X_k+1为k+1时刻系统的状态量，Z_IMU为惯性测量单元的量测值Z_IMU＝{f_b,ω_b}，f_b、ω_b分别为惯性传感器测量得到的比力和角速度。

进一步地，步骤(d)的具体步骤如下：

选择因子节点的代价函数并令其取值最小：

$L(Z-H)={(Z-H\hat{X})}^{T}W(Z-H\hat{X})=min---\left(4\right)$

对式(4)求状态量X的偏导数，并令其为0：

$\frac{\∂L}{\∂X}{|}_{X=\hat{X}}=-H^T(W+W^T)(Z-H\hat{X})=0---\left(5\right)$

根据式(5)得到对系统状态量X的估计：

$\hat{X}={\left(H^{T}WH\right)}^{-1}{H}^{T}WZ---\left(6\right)$

其中，W是正定加权矩阵。

进一步地，步骤(a)中选择系统状态量

其中，φ_E，φ_N，φ_U为平台误差角，δV_E，δV_N，δV_U东北天方向的速度误差，δL，δλ，δh纬度、经度、高度位置误差，ε_bx，ε_by，ε_bz为陀螺仪随机常数，ε_rx，ε_ry，ε_rz为陀螺仪一阶马尔柯夫过程，▽_x，▽_y，▽_z加速度计一阶马尔柯夫过程。

采用上述技术方案带来的有益效果：

(1)本发明采用因子图对不同的导航系统进行配置，针对传感器的不同应用特点设计软件架构，建立适用于异步异构导航系统的滤波架构。根据多源导航系统的实际性能表征及故障特性，研究滤波架构的自适应重构方案，提高导航系统整体滤波的精度及容错性能；

(2)本发明分析各传感器量测信息的时空差异，在多源信息智能融合结构的基础上，将不同更新率、不同误差状况的传感器抽象成量测因子，针对融合节点设计全局融合算法。

附图说明

图1为本发明的总体流程图；

图2为因子图的示例图；

图3为马尔科夫链的因子图的示例图；

图4为实施例中五类传感器构成的因子图。

具体实施方式

以下将结合附图，对本发明的技术方案进行详细说明。

如图1所示本发明的大体流程，首先通过轨迹设定得到符合实际环境及任务需求的轨迹，由各种传感器得到传感器量测信息，由惯性量测单元得到惯导IMU数据，通过因子图构多源导航信息融合框架，在此基础上，最终通过多源信息滤波融合得到导航信息。下文将对本发明的整个过程进行详细说明。

1、在机载动态环境下，根据载体的实际情况、任务需求以及所处环境设计载体的运动动作和运动轨迹。

此处以飞机为例，对航迹的设计进行具体的说明。在机载动态环境下，要设计符合飞机飞行需求、载体所处的复杂环境以及任务特点的飞行轨迹。对于不同类型的飞机，例如：机动性较强的飞机(如：歼击机)和机动性较弱的飞机(如：运输机)而言，其工作环境、飞行机动动作、任务需求也有很大的不同，剧烈程度也不一样。对于固定翼飞机而言，典型的飞行动作可以分为如下几种：①平飞和转弯；②螺旋；③盘旋转弯；④筋斗、S形和8字形；⑤缒头机动；⑥滚转；⑦直线的组合；⑧尾滑机动。

要测量不同系统的性能，飞行机动可以在上述动作中选择一种或多种进行组合。根据上述要求，设计一条符合实际情况的航迹，使其能满足任务要求和实际需要。

2、通过惯性测量单元中的陀螺仪和加速度计获取载体的角速度信息和比力信息，同时，通过其他各类机载高精度传感器获取载体各类量测信息。

因子图框架是一种具有快速集成和重新配置任意导航传感器和敏感元件的框架、抽象方法及滤波方法。其中，可用于因子图框架的导航要素类型众多，倾角罗盘，距离/伪距测距仪，气压计，温度传感器，方位角速率传感器，激光雷达，加速度计，陀螺仪，磁力计，计时器，计步器，星敏感器，红外传感器、偏振光传感器、X射线探测器、光流传感器等。根据任务需求及实际情况，对传感器进行选择。

3、多源信息融合框架的建立

定义导航系统状态矢量为因子图的变量节点，定义惯性测量单元以及其他各类机载高精度传感器获取的载体量测信息为因子图的因子节点，从而构建基于因子图的多源导航信息融合框架。

因子图是表示导航估计问题的二分图模型G＝(F,X,E)，其中，包含两种类型的节点：一种是因子节点f_i∈F，代表因式分解中的局部函数；一种是变量节点x_j∈X，代表全局多元函数中的变量。边缘e_ij∈E是指，当且仅当因子图中状态变量节点x_j和与之相应的因子节点f_i相关时，它们之间存在一条连接边。

假设g(x₁,...,x_n)是几个局部函数的乘积，每个局部函数的参数都包含在子集{x₁,...,x_n}中，例如：

$g(x_1,...,x_n)=\underset{j\∈J}{\Π}{f}_j\left(X_j\right)---\left(1\right)$

其中，J是离散指标集，X_j是{x₁,...,x_n}的子集，f_j(X_j)是一个函数，参数为X_j。

因子图即是一个二分图，用来描述(1)式因式分解的结构。令g(x₁,x₂,x₃,x₄,x₅)作为函数的5个变量，假如g可以被表示为5个因式乘积的形式：

g(x₁,x₂,x₃,x₄,x₅)＝f_A(x₁)f_B(x₂)f_C(x₁,x₂,x₃)f_D(x₃,x₄)f_E(x₃,x₅) (2)

那么，J＝{A,B,C,D,E}，X_A＝{x₁}，X_B＝{x₂}，X_C＝{x₁,x₂,x₃}，X_D＝{x₃,x₄}，X_E＝{x₃,x₅}。式(2)对应的因子图如图2所示。

概率模型是因子图的一个主要应用方向。例如，设X，Y，Z为可以组成马尔科夫链的随机变量，它们的联合概率密度

p_XYZ(x,y,z)＝p_X(x)p_Y|X(y|x)p_Z|Y(z|y) (3)

此表达式的因子图表示如图3所示。

本发明中，选择捷联惯导(Strapdown Inertial Navigation System,SINS)、天文导航(Celestial Navigation System,CNS)、地形轮廓匹配(Terrain Contour Matching,TERCOM)、合成孔径雷达(Synthetic Aperture Radar,SAR)景象匹配进行信息融合，并用气压高度计(Barometric altimeter)进行高度辅助，以此为例，对因子图融合框架进行说明。

将SINS、CNS、TERCOM、SAR、Barometer抽象为五个因子节点，并把多元传感器融合导航框架用因子图构造表示出来，如图4所示。图中，圆圈代表状态变量节点，黑色方块代表因子节点，X_k代表系统的导航状态，f代表各传感器量测信息。Prior表示先前的量测信息，f_IMU表示来自IMU的量测信息，与t_k时刻和t_k+1时刻的导航状态相关，f_CNS、f_TERCOM、f_SAR、f_Barometer也分别是来自CNS、TERCOM、SAR、Barometer的量测信息。

4、基于因子图的多源导航信息融合

在因子图的开放式结构框架下，表征系统的状态及量测更新过程，建立滤波方程，经过实时滤波估计和修正，从而完成多源传感器信息的有效融合，实现传感器的即插即用。

导航系统的18维系统的状态方程为：

$\stackrel{\·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(4\right)$

式中：X(t)为状态矢量；A(t)为状态系数矩阵；G(t)为误差系数矩阵；W(t)为白噪声随机误差矢量。

系统状态矢量X为：

包含了9维惯导系统的基本导航参数误差和9维惯性仪表的误差状态量。其中，φ_E，φ_N，φ_U为平台误差角，δV_E，δV_N，δV_U东北天方向的速度误差，δL，δλ，δh纬度、经度、高度位置误差，ε_bx，ε_by，ε_bz为陀螺仪随机常数，ε_rx，ε_ry，ε_rz为陀螺仪一阶马尔柯夫过程，▽_x，▽_y，▽_z加速度计一阶马尔柯夫过程。

基于因子图的多源信息融合算法旨在寻找一个能够满足约束规则的解，因此，需要有约束规则来约束求解过程。在此，我们将约束选取为

$P\left(X\right)=\underset{n\∈N}{\Π}{f}_n\left(X_k\right)---\left(6\right)$

式(6)中，P(X)为定义在X上的联合分布函数。一个量测因子节点可以写成f(X_k)＝L(Z_k-H)的形式，表示因子节点获取预测的量测信息和实际量测信息的差值，构建相应的指标函数从而获取状态变量的估计。L(·)是被估计量的代价函数；H为量测函数，是与状态变量相关的函数，在导航框架中H可以根据给定的状态估计来预测传感器的量测值；Z_k是由各类传感器得到的实际量测值。

因子节点中的IMU节点不同于其他的量测节点，其量测值Z_IMU与k时刻的估值X_k被用来预测k+1时刻的值X_k+1，量测值Z_IMU的表达式为

Z_IMU＝{f_b,ω_b} (7)

其中，f_b、ω_b分别为惯性传感器测量得到的比力和角速度，则可得到IMU因子节点的表达式

f_IMU＝L(X_k+1-F(X_k,Z_IMU)) (8)

其中，F是系统的传递函数矩阵，X_k+1为k+1时刻系统的状态向量。

选择因子节点的代价函数L，并使其值取最小，得到

$L(Z-H)={(Z-H\hat{X})}^{T}W(Z-H\hat{X})=min---\left(9\right)$

其中，W是适当取值的正定加权矩阵，是对状态量X的估计。要使上式成立，应满足

$\frac{\∂L}{\∂X}{|}_{X=\hat{X}}=-H^T(W+W^T)(Z-H\hat{X})=0---\left(10\right)$

由此得到当前对状态X的估计为

$\hat{X}={\left(H^{T}WH\right)}^{-1}{H}^{T}WZ---\left(11\right)$

基于因子图的多源导航信息融合方法，能方便地对来自异步异构传感器的数据进行处理，接收到传感器的输出数据后，扩充因子图节点，根据系统的状态方程和量测方程快速有效的进行系统状态的更新，实现多传感器的数据综合处理，有效提高导航系统的可靠性和快速配置的能力，大大提高导航性能。

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。