基于因子图的无人机多源组合导航算法研究*

李 杰 陈 帅 王 琮 姚晓涵 朱晓晗

1.南京理工大学，南京210094 2.中国电子科技集团第三十二研究所，上海201808

0 引言

无人机飞行器具有低风险、体积小、长续航和环境适应能力强等优点，在军事和商业领域中得到了广泛的应用[1]。由于无人机工作环境的复杂性和不定性，如何持续为无人机飞行器提供精确的定位导航信息是当前的一大难题。以微惯性导航系统(Micro-inertial Navigation System,MINS)作为参考系统，与北斗卫星导航系统(BeiDou Navigation Satellite System,BDS)、磁罗盘、气压高度表共同构成的多源组合导航系统弥补了传统的MINS/BDS组合导航系统的不足，使得导航系统工作的可靠性大大提升，可以实时为无人机进行导航。

目前，对多源组合导航算法研究多以卡尔曼滤波为主，但由于各个传感器数据传输频率的不同，传统联邦卡尔曼滤波器需要全部量测信息到来之后才能进行滤波，使得在处理异步信息时能力有所不足，并且在使用外推内插法将量测信息同步时，将误差引入了滤波器，增加了计算量[2-3]。文献[4]利用因子图模型良好的扩展性和灵活性，在该模型中处理INS/GPS组合导航系统的信息滞后问题，快速地融合异步量测信息，从而保证了导航系统的精度和实时性，计算量大大减少。

本文以无人机飞行器MINS/BDS/磁罗盘/气压高度表多源组合导航系统为例，通过构建因子图模型，研究了基于因子图的数据融合方法，通过半物理仿真试验验证了该方法在无人机导航系统中的可行性。

1 因子图模型概述

因子图作为一种概率图模型，除解决数学概率问题外，还广泛应用于人工智能、信号处理、数字通信等领域[5-7]。

因子图表示的是将一个多变量的全局函数因子分解成局部函数的积而形成的二分图模型G=(F,X,E)，可用于表示导航的估计问题。该二分图模型由变量节点(xi∈X)、因子节点(fj∈F)和连线(eij∈E)构成，当变量节点和因子节点相关时，它们中间会产生一条连线。

因子图G将函数f(X)的因式分解为

(1)

式中：Xi为{x1,x2,…,xn}的子集，fi为局部函数。

设f(x1,x2,x3,x4,x5)是一个包含5个变量的函数，且f可表示为

f(x1,x2,x3,x4,x5)=
f1(x1)f2(x2,x3)f3(x1,x3,x4)f4(x4,x5)f5(x2,x5)

(2)

式中：X1={x1}，X2={x2,x3}，X3={x1,x3,x4}，X4={x4,x5}，X5={x2,x5}。则式(2)可表示的因子图结构如图1所示。

图1 函数f(x1,x2,x3,x4,x5)对应的因子图结构

图中，圆形表示变量节点，正方形表示因子节点，每个因子节点和不同的变量节点产生关系，从而构成不同的局部函数。

2 基于因子图的MINS/BDS/磁罗盘/气压高度表信息融合方法设计

2.1 基于因子图的MINS/BDS/磁罗盘/气压高度表信息融合结构

无人机飞行器MINS/BDS/磁罗盘/气压高度表多源组合导航系统，由BDS提供位置和速度信息、磁罗盘提供航向角信息、气压高度表提供高度信息， 将MINS捷联解算得到的姿态、位置、速度信息作为参考信息与上述传感器提供的数据基于因子图模型进行融合[8]。将MINS、BDS、磁罗盘和气压高度表抽象为4个因子节点，分别用fMINS、fBDS、fCompass和fBaro表示。基于因子图的MINS/GNSS/磁罗盘/气压高度表信息融合框架如图2所示。

图2 基于因子图的MINS/BDS/磁罗盘/气压高度表信息融合框架

图2中，xk代表系统的导航状态，fMINS表示来自MINS的量测信息，与时刻tk和时刻tk+1的导航状态相关；fBDS、fCompass和fBaro分别是来自于BDS、磁罗盘和气压高度表的量测信息。

1)MINS因子

无人机导航状态的时间更新可用如下方程进行抽象描述

(3)

式中：fb和ωb分别表示加速度计和陀螺仪测量的比力和角速度。

将MINS的量测表示为zk≐{fb,ωb}，其与连接的两个导航状态xk和xk+1相关，将式(3)离散化，可得

(4)

此时因子节点可表示为

(5)

式中，d[·]表示相应的代价函数。

2)BDS因子

将BDS量测方程表示为

(6)

式中：nBDS和hBDS分别为BDS量测函数和量测噪声。由上述方程可将新加入的因子节点fBDS表示为

(7)

磁罗盘和气压高度表的量测因子可参考BDS因子表示方法，本文不再赘述。

基于因子图架构的多源组合导航系统，能够有效实现导航传感器的即插即用，将不同频率、不同误差的传感器合并起来，增强了导航系统的冗余性，使得导航系统能够长时间为无人机进行导航。

2.2 组合导航系统数学模型

2.2.1 组合导航系统状态方程

为了导航系统能够快速进行数据融合，将MINS捷联解算得到的基本导航参数误差与惯性仪器参数误差合并，得到系统的状态向量

(8)

系统状态方程为

(9)

式中：F为状态转移矩阵；G为噪声转移矩阵；W为状态噪声矢量。

2.2.2 组合导航系统量测方程

1)MINS/BDS位置、速度量测误差方程

由于BDS能提供位置和速度的量测信息，则构造量测误差方程为

(10)

式中：LINS、λINS、hINS分别表示MINS解算得到的纬度、经度和高度；LBDS、λBDS、hBDS分别表示BDS量测的纬度、经度和高度；VINS_E、VINS_N、VINS_U分别表示MINS解算得到的东向、北向和天向速度；VBDS_E、VBDS_N、VBDS_U分别表示BDS量测的东向、北向和天向速度；δLBDS、δλBDS、δhBDS和δVBDS_E、δVBDS_N、δVBDS_U分别表示位置和速度量测误差；HBDS和VBDS分别表示BDS量测矩阵和噪声矩阵。

2)MINS/磁罗盘航向角量测误差方程

由于磁罗盘能提供航向角的量测信息，则构造量测误差方程为

ZCompass=[ψINS-ψCompass]=[φU+δψCompass]=
HCompassX+VCompass

(11)

式中：ψINS表示MINS解算得到的航向角；ψCompass表示磁罗盘量测的航向角；δψCompass为航向角量测误差；HCompass和VCompass分别表示磁罗盘量测矩阵和噪声矩阵。

3)MINS/气压高度表高度量测误差方程

由于气压高度表能提供高度的量测信息，则构造量测误差方程为

ZBaro=[hINS-hBaro]=[δh+δhBaro]=
HBaroX+VBaro

(12)

式中：hBaro表示气压高度表量测的高度值；δhBaro为高度量测噪声；HBaro和VBaro分别表示气压高度表量测矩阵和噪声矩阵。

2.3 基于因子图的MINS/BDS/磁罗盘/气压高度表信息融合算法

定义导航状态集为Xk，当前时刻为tk，则由此定义的联合概率密度函数为P(Xk|Zk)，其中Zk为时刻tk得到的量测信息，则最大后验概率估计为

(13)

根据因子的定义可知，每个因子表示为一个独立的项，因此

(14)

(15)

对于Gauss噪声分布，式(15)可写成[9]

(16)

式中，h(·)为与状态变量相关的量测函数，zi为各类传感器得到的实际量测值，且zi=hi(xk)+vi，vi为量测误差。

根据上述推导得到矢量形式的表达式：

(17)

(18)

式中，J为误差函数在初始值附近的雅克比矩阵。由式(17)和式(18)可知

(19)

(20)

(21)

M=JTR-1J

(22)

为使式(19)的值最小，即使其一阶导数为0，可得：

MΔX*=-b

(23)

则状态变量的解为

(24)

利用高斯——牛顿迭代法求最优解，通过不断迭代使ΔX*=0，获得增量ΔX[10]。

3 仿真校验

为了验证本文所述的基于因子图的飞行器多源组合导航算法在实际工程应用中的可靠性和有效性，设计了半物理车载实验。由NovAtel公司的SPAN-KVH1750高精度组合导航系统提供基准数据，采集BDS卫星接收机、MEMS惯导、磁罗盘和气压高度表的原始数据进行离线仿真，设备装车效果如图3所示。上述设备的主要性能指标如表1所示。

图3 设备装车效果图

表1 试验设备性能指标

跑车时长400s，数据通过上位机监控存入计算机，然后分别进行因子图算法和传统分散式联邦滤波算法数据融合离线仿真，通过与高精度基准对比得出误差，两者的误差对比曲线如图4～6所示。

图4 位置误差对比曲线

图5 速度误差对比曲线

图6 姿态误差对比曲线

通过位置、速度和姿态误差曲线对比可知，使用因子图融合算法和分散式联邦滤波融合算法的位置和速度精度相当，并且部分导航数据优于联邦滤波。姿态中，使用分散式联邦滤波算法时，MINS/BDS子滤波器能够根据姿态失准角误差方程估计出东向和北向失准角，以此修正俯仰角和滚转角；而因子图算法根据导航信息源进行数据估计，由于缺少输出俯仰角和滚转角信息的传感器，其俯仰角和滚转角均处于发散状态。均方根误差统计结果对比如表2所示。

上述结果表明，基于因子图的多源组合导航算法能够解决以MEMS惯导、BDS接收机、磁罗盘、气压高度表作为导航信息源的无人机飞行器的定位问题，由于因子图算法能够很好地利用不同输出频率的导航信息，因此其信息融合的效果更好，高度定位和航向角测姿精度较传统分散式联邦滤波高。另外因子图融合算法能够有效地实现导航传感器的即插即用，对于外扩导航信息源具有良好的灵活性，因此可以通过外扩其它导航信息源如视觉导航传感器等进行因子图数据融合解决无人机飞行器俯仰角和滚转角的测姿问题。

表2 均方根误差结果对比

4 结论

本文详细介绍了因子图模型和信息融合设计方法，以无人机飞行器MINS/BDS/磁罗盘/气压高度表多源组合导航系统作为研究对象，通过车载采集导航信息进行离线仿真，将因子图融合效果与传统分散式联邦滤波算法进行对比。实验结果表明，经因子图算法进行数据融合后，定位精度与联邦滤波算法相当，并且部分导航信息精度优于联邦滤波算法，验证了因子图融合算法应用于无人机多源导航系统的可行性。由于本文研究的对象缺少输出俯仰角和滚转角的导航传感器，因此在无人机测姿问题中还有所欠缺，下一阶段可利用因子图算法的即插即用功能对导航传感器进行扩展，增减传感器只需相应增减方程中的量测值，借此提高无人机定位、测姿的完整性，可为未来实现无人机全源导航打下基础。