基于无人机状态信息的广义因子图融合导航方法

本发明属于组合导航技术领域，特别涉及了一种无人机的广义因子图融合导航方法。

背景技术：

近些年，无人机凭借其巨大的应用市场和潜在的扩展领域受到世界各国的高度重视，无人机技术在军事领域和民用领域均获得了广泛应用。无人机由于具有无人驾驶、远程操控的能力，在海上搜救、森林火灾救援、能源探测、商业运输等领域得到了广泛的应用。尽管目前已经有大量关于无人机飞行安全方面的研究，无人机也有着较好的飞行特性，但它还是会受到复杂大气环境的影响。特别是低空大气扰动会造成无人机姿态的变化。在复杂大气干扰下，无人机的高精度姿态及其他导航状态信息是一个亟待解决的问题。

无人机的定位主要由机载的导航系统完成，无人机导航系统实时输出无人机的位置、速度和姿态信息，为飞行器提供准确的位置坐标和状态信息。在无人机导航系统中，由于各传感器所采用的导航原理不同，各类传感器之间存在极强的互补性。在实际应用中，由于不同传感器的更新频率不同，存在时间不同步的问题，采用固定的滤波结构和方法均很难满足这种复杂多变的应用需求，因子图灵活易变的滤波结构更适合多源组合导航滤波融合方法。

概率图模型是一种以图模型表示变量概率依存关系的理论。因子图是一种双向概率图模型，图中包含两种类型的节点：一种是变量节点，代表全局多元函数中的变量；一种是因子节点，代表因式分解中的局部函数。每个局部函数只与全局多元函数中的部分变量相关，当且仅当变量是局部函数的自变元时，因子图中与之相应的变量节点与因子节点之间存在一条连接边。因子图作为一种分析问题的图形工具，或许有助于解决无人机传感器量测信息的非等间隔、不同步、动态变化等问题。

无人机姿态量测信息并不充足，无人机组合导航系统中仅有imu提供全部的姿态信息，磁力计仅提供航向角校正，而其他传感器多提供的是位置、速度信息。因子图架构将位置、速度、姿态分为三个因子图同步进行滤波，这样造成了姿态因子图量测量较少，需要姿态的精确量测来保证无人机的飞行安全，同时姿态精度的提升也修正位置、速度的精度。

技术实现要素：

为了解决上述背景技术提到的技术问题，本发明提出了基于无人机状态信息的广义因子图融合导航方法，提高了对无人机姿态量测的精确度，来保证无人机的飞行安全，同时姿态精度的提升也修正位置、速度的精度。

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

基于无人机状态信息的广义因子图融合导航方法，所述导航方法包括以下内容

步骤一，根据无人机的任务需求、所处环境以及导航传感器设计导航系统，通过在线神经网络学习的方式基于物理导航传感器的状态与量测信息构建虚拟传感器，确定无人机机载导航传感器的工作性能，在相关物理传感器故障时引入系统补充导航状态量测量；

步骤二，获得机载各导航系统得到传感器量测信息，分成位置、速度和姿态三个因子图进行信息融合，将无人机组合状态变量定义为因子图的变量节点，机载各导航传感器测量值定义为因子节点，构建基于位置因子图、速度因子图、姿态因子图的多源导航信息融合算法；

步骤三，基于位置因子图与速度因子图滤波估计的位置、速度信息，引入到广义姿态因子图滤波器中，对姿态因子图滤波的结果进行修正。

进一步的，所述步骤二包括以下具体流程：

步骤2a，定义无人机的导航系统状态变量为因子图的变量节点，定义惯性测量单元、虚拟传感器及其他各类机载传感器获取的载体量测信息为因子图的因子节点，构建基于因子图的多源导航信息融合系统；

步骤2b，在因子图多源导航信息融合框架下，选取多源导航信息融合的约束规则，建立惯性测量单元及其他各类机载导航传感器、虚拟传感器的因子节点表达式，通过分析因子图融合准则，通过各类传感器的因子节点表达式建立联合概率分布函数，并在联合概率分布取最大时取得状态变量的估计，经过实时滤波估计和修正，完成多源导航信息的有效融合；

所述导航系统状态变量x如下：

上式中，为平台误差角，δve,δvn,δvu为东北天方向的速度误差，δl,δλ,δh为纬度、经度、高度位置误差；

所述因子图的惯性导航变量因子节点如下：

上式中，fb、ωb分别为惯性测量单元得到的比力和角速度；

所述因子图中除惯导外的其他导航系统、虚拟导航系统因子节点统一定为公式如下：

上式中，代表其他辅助导航系统的量测值，h^sensor是其他辅助导航系统的量测方程，n^sensor是其他辅助导航系统的量测噪声；

无人机导航系统估计的状态量就是量测z(t)发生条件下x(t)发生的最大概率，因此我们求取联合分布概率函数的最大后验概率估计值，即为最有可能出现的状态量：

其中hi(xi)为观测方程，zi为真实量测量，∑i为协方差矩阵；

对非线性观测方程hi(xi)进行一阶泰勒展开来实现线性化，得到状态更新向量：

其中hi是观测雅克比矩阵，δ^*为估计的状态更新向量，δi为i时刻的状态更新向量；求取状态更新向量后计算得出基于因子图的无人机的速度、位置、姿态融合信息。

进一步的，所述步骤三包括以下具体流程：

不考虑姿态误差二阶小量的基础上，把地理坐标系作为导航坐标系，惯性导航的平台误差角方程如下所示：

其中lfg和hfg分别是位置因子图融合后的经度和高度信息，和分别是速度因子图融合后的东向和北向速度信息，和是姿态因子图融合的惯性导航平台角信息，ωie为地球自转角速度，rm为子午面内曲率半径，rn为卯酉圈曲率半径；

将所述无人机平台误差角转换成等效的旋转矩阵，表示如下：

由于姿态角误差的存在，姿态因子图融合的姿态角看作计算系，从计算系到机体系的转换矩阵如下式所示：

其中θfg、γfg和ψfg分别代表姿态因子图姿态角；

将式(9)和式(10)相乘，得到姿态角误差与其他因子图融合后信息之间的函数映射关系，姿态校正方程如下：

将式(9)和式(10)带入式(11)得：

进一步的，通过反馈矫正优化广义因子图滤波器得到更高精度的导航信息。

附图说明

图1为本发明各飞行阶段因子图融合结构图；

图2为本发明的整体流程图。

具体实施方式

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

本实施例公开的基于无人机状态信息的广义因子图融合导航方法包括以下内容

步骤一，根据无人机的任务需求、所处环境以及导航传感器设计导航系统，通过在线神经网络学习的方式基于物理导航传感器的状态与量测信息构建虚拟传感器，确定无人机机载导航传感器的工作性能，在相关物理传感器故障时引入系统补充导航状态量测量；

步骤二，获得机载各导航系统得到传感器量测信息，分成位置、速度和姿态三个因子图进行信息融合，将无人机组合状态变量定义为因子图的变量节点，机载各导航传感器测量值定义为因子节点，构建基于位置因子图、速度因子图、姿态因子图的多源导航信息融合算法；

步骤三，基于位置因子图与速度因子图滤波估计的位置、速度信息，引入到广义姿态因子图滤波器中，对姿态因子图滤波的结果进行修正。

进一步的，所述步骤二包括以下具体流程：

步骤2a，定义无人机的导航系统状态变量为因子图的变量节点，定义惯性测量单元、虚拟传感器及其他各类机载传感器获取的载体量测信息为因子图的因子节点，构建基于因子图的多源导航信息融合系统；

步骤2b，在因子图多源导航信息融合框架下，选取多源导航信息融合的约束规则，建立惯性测量单元及其他各类机载导航传感器、虚拟传感器的因子节点表达式，通过分析因子图融合准则，通过各类传感器的因子节点表达式建立联合概率分布函数，并在联合概率分布取最大时取得状态变量的估计，经过实时滤波估计和修正，完成多源导航信息的有效融合；

所述导航系统状态变量x如下：

上式中，为平台误差角，δve,δvn,δvu为东北天方向的速度误差，δl,δλ，,δh为纬度、经度、高度位置误差；

所述因子图的惯性导航变量因子节点如下：

上式中，fb、ωb分别为惯性测量单元得到的比力和角速度；

所述因子图中除惯导外的其他导航系统、虚拟导航系统因子节点统一定为公式如下：

上式中，代表其他辅助导航系统的量测值，h^sensor是其他辅助导航系统的量测方程，n^sensor是其他辅助导航系统的量测噪声；

无人机导航系统估计的状态量就是量测z(t)发生条件下x(t)发生的最大概率，因此我们求取联合分布概率函数的最大后验概率估计值，即为最有可能出现的状态量：

其中hi(xi)为观测方程，zi为真实量测量，∑i为协方差矩阵；

对非线性观测方程hi(xi)进行一阶泰勒展开来实现线性化，得到状态更新向量：

其中hi是观测雅克比矩阵，δ^*为估计的状态更新向量，δi为i时刻的状态更新向量；求取状态更新向量后计算得出基于因子图的无人机的速度、位置、姿态融合信息。

进一步的，所述步骤三包括以下具体流程：

不考虑姿态误差二阶小量的基础上，把地理坐标系作为导航坐标系，惯性导航的平台误差角方程如下所示：

其中lfg和hfg分别是位置因子图融合后的经度和高度信息，和分别是速度因子图融合后的东向和北向速度信息，和是姿态因子图融合的惯性导航平台角信息，ωie为地球自转角速度，rm为子午面内曲率半径，rn为卯酉圈曲率半径；

将所述无人机平台误差角转换成等效的旋转矩阵，表示如下：

由于姿态角误差的存在，姿态因子图融合的姿态角看作计算系，从计算系到机体系的转换矩阵如下式所示：

其中θfg、γfg和ψfg分别代表姿态因子图姿态角；

将式(9)和式(10)相乘，得到姿态角误差与其他因子图融合后信息之间的函数映射关系，姿态校正方程如下：

将式(9)和式(10)带入式(11)得：

进一步的，通过反馈矫正优化广义因子图滤波器得到更高精度的导航信息。

以无人机机载导航系统为例，首先基于任务需求，获得机载各导航系统得到传感器量测信息，分成位置、速度和姿态三个因子图进行信息融合，接着基于位置因子图与速度因子图滤波估计的位置、速度信息，引入到广义姿态因子图滤波器中，对姿态因子图滤波的结果进行修正，并通过反馈矫正优化广义因子图滤波器得到更高精度的导航信息。下文将对本发明的整个过程进行详细说明。

1、在无人机飞行环境下，根据实际情况、任务需求以及所处环境设计无人机飞行航迹、传感器信号、运动状态等。

此处以无人机为例，对无人机航迹的设计进行具体的说明。根据任务目标，要设计符合无人机飞行需求、载体所处的复杂环境以及任务特点的飞行轨迹。对于不同任务的需求，例如：军事攻击、抢险救援、线路巡检、快递运送、飞行表演等，其工作环境、飞行动作、任务需求也有很大的不同，飞行构型和飞行航迹也不一样。对于无人机而言，典型的飞行构型可以分为如下几种：

①三角形；②菱形；③s形；④8字形；⑤梯形；⑥圆形；⑦直线形等的组合。

要完成不同的飞行任务，无人机可以在上述构型中选择一种或多种进行组合。根据上述要求，设计一个符合实际情况的构型和各自的航迹，使其能满足任务要求和实际需要。

2、由于无人机的任务需求不同，同时考虑成本与无人机的机载载重的因素，需要以此确定可用的传感器类型。

可用于因子图算法的导航传感器类型众多，根据任务需求及实际情况，对无人机的导航系统方案进行设计，选用惯性导航传感器、卫星导航传感器、视觉相机、大气数据系统、磁力计作为具体实施传感器选型方案，并通过bp神经网络、长短期记忆神经网络、循环神经网络等在线学习的方式构建虚拟导航系统。

3、广义态因子图信息融合修正架构的建立

因子图是一种概率图模型g＝(f,x,e)，包含两种类型的节点：一种是因子节点fi∈f，代表因式分解中的局部函数；一种是变量节点xj∈x，代表全局多元函数中的变量。当因子图架构中的状态变量节点xj和与之对应的因子节点fi相关时，它们之间存在一条连接边eij∈e。

如图1所示，本发明中，无人机采用广义姿态因子图架构作为姿态信息融合修正方案。图中，空心圆圈代表状态变量节点，实心圆圈代表因子节点，x代表系统的导航状态，在位置因子图、速度因子图和姿态因子图中分别为位置状态量、速度状态量和姿态状态量，f代表各传感器量测信息，fprior表示先前的量测信息，fimu表示来自imu的量测信息，与k时刻和k+1时刻的导航状态相关，fgnss、fads、fsar、fba也分别是其他导航系统的量测信息。在因子图架构下，建立滤波估计方程，经过实时滤波估计和修正，从而完成多源传感器信息的有效融合。

在图2中，姿态因子图架构将无人机的姿态状态量定义为变量节点，选择无人机的惯性导航系统、磁航向系统等姿态量测传感器量测值定义为因子节点，由于在多源组合导航系统中，提供姿态量测信息的导航传感器较少，因此采用姿态因子图滤波估计的姿态量作为主要参考姿态信息，将位置因子图与速度因子图融合得到的结果引入到姿态因子图中进行姿态修正。

广义姿态因子图滤波器将位置因子图、速度因子图中融合后的位置与速度信息引入姿态因子图中，基于位置、速度与姿态误差之间的函数映射关系，对无人机姿态因子图融合的效果进行修正，提高无人机的姿态量测精度，高精度的姿态信息通过反馈矫正达到修正位置、速度因子图的效果。

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