基于因子图的无人机全源导航关键技术研究

王慧哲，曾庆化，刘建业，陈维娜

(1.南京航空航天大学导航研究中心，南京211106；2.卫星通信与导航协同创新中心，南京211106)

基于因子图的无人机全源导航关键技术研究

王慧哲1，2，曾庆化1，2，刘建业1，2，陈维娜1，2

(1.南京航空航天大学导航研究中心，南京211106；2.卫星通信与导航协同创新中心，南京211106)

针对DARPA提出的全源导航需要快速集成并重新配置任意导航传感器的要求，结合民用无人机的任务需求，通过概率图模型相关原理，对基于因子图的无人机全源导航关键技术开展研究工作。采用因子图对系统状态更新过程进行表示，实现系统状态的递推与更新，完成传感器信息的数据综合。仿真结果表明，该方法能在传感器可用性时变的情况下，将不同传感器的数据进行有效融合，确保系统导航定位精度，使载体满足不断变化的任务需求与环境变化的要求。

无人机；因子图；全源导航；信息融合

0 引言

无人机作为一种无人驾驶、能执行多种任务的航空器，曾在战场中显示出强大的战斗能力。随着科技的发展，民用无人机在航空摄影、地面灾害评估、航空测绘、交通监视、消防、人工增雨等民用领域同样发挥着重要作用［1］。同样，GPS的迅猛发展使其在人们的生活中扮演着重要角色。然而，由于GPS不可避免地具有卫星导航系统固有的脆弱性，其提供的服务与应用受到限制。例如，卫星导航信号功率低，易被敌方压制或欺骗，在高对抗作战环境下难以保障；卫星信号难以穿透地面和建筑物等密度较大的物质，在都市、室内、地下环境中信号衰减现象非常严重。为了解决卫星导航系统的制约，在各种情况下提供精确可靠的导航信息，亟需开展不依赖于GPS的新导航技术［2］。2014年，DARPA表示正在发展5项不依赖GPS的导航技术项目，包括定位、导航与授时微技术，自适应导航系统，量子辅助传感与读出，超速激光科学与工程，对抗环境下的空间、时间和方位信息项目。其中，自适应导航系统包括精确惯性导航系统和全源定位与导航两个子项目［3］。

本文以民用无人机为背景，结合全源导航需要快速集成并重新配置任意导航传感器的要求，通过研究因子图的相关理论与成果，对全源导航的关键技术展开研究。

1 全源导航

全源定位导航技术利用先进算法，根据不同任务和作战环境，将作战平台中的导航系统与雷达、红外、声学等多种传感器信息进行融合，提供低成本、高可靠的导航定位系统，解决GPS受阻环境下精确导航的需求，降低对卫星导航的依赖［4］。

2010年11月，DARPA启动了全源定位与导航(ASPN)项目，旨在开发一种在GPS受限情况下的廉价导航信息融合技术，可以与激光测距仪、相机和磁力计等多种传感器实现即插即用的组合［5］。不过，值得注意的是，全源导航并不是要全面取代或替代卫星导航系统，而是在无法使用卫星导航系统的情况下，提供一种解决方案，弥补因卫星导航系统固有脆弱性产生的服务或能力不足［6］。图1为全源定位与导航的方案示意图。

在复杂环境中，单一导航系统缺点明显，如惯导系统误差随时间累积、GPS易受到敌方压制等，不能满足军用领域对可靠性、稳定性及高精度的需求。而全源导航可以根据环境、需求及任务要求的不同，灵活地接入或移除不同类型的传感器，引入不同类型的量测量和特征数据库，从而为不同环境下的用户提供GPS受限情况下的精确定位。为了使多传感器能够快速融合和重新配置，本文结合因子图理论，通过构建系统框架，实现对任意传感器信息的融合。

2 概率图模型理论

概率图模型是一种表达随机变量联合概率分布的图模式，由图和与之相关的参数两部分组成。目前，常用的概率图模型包括贝叶斯网络(Bayesian Networks)、马尔科夫网络(Markov Networks)、因子图(Factor Graphs)等［7］。因子图是一个通用模型，具有很强的表达能力。

因子图可以用作表达随机变量联合概率分布的概率图模型，由变量节点、因子节点和连线组成。假设一个因子图中的所有变量为x1、x2、…、xn，所有的局部函数为 f1(X1)、 f2(X2)、 …、fK(XK)。其中，Xk⊆｛x1，x2，…，xn｝，1≤k≤K，表示第k个局部函数fk(·)的自变量点集，K表示局部函数的总数，则因子图所指定的联合概率分布为：

其中，fk(Xk)(1≤k≤K)为非负有限函数，Z为归一化因子，若联合概率分布函数P(x1，x2，x3，x4)可表示为如下因子分解形式：

与其对应的因子图如图2所示，其中空心圆形节点代表变量，实心圆形节点代表局部函数。

3 基于因子图的多信息融合方法设计

随着传感器数量和种类的增加，不同的传感器具有不同的更新频率且在时间上存在不同步性，传统的信息融合方法很难满足系统日益复杂多变化的需求。本文结合因子图的原理，提出了一种基于因子图的多信息融合方法，构建因子图模型，获取不同传感器测量信息后，通过定义的系统变量节点以及因子节点，进行因子图的拓展，对应不同传感器的因子节点实现系统状态的递推和更新，从而完成多传感器的信息融合。

因子图是一种二分图G＝(F，X，E)，包括两类节点——因子节点fi∈F和变量节点xj∈X，其中，eij∈E表示因子节点与变量节点之间的边。因子图G定义函数f(X)的因式分解为f(X)＝∏ifi(Xi)，其中，Xi是所有变量xj的集合，与因子fi由一条边相连［8］。将因子图的概念引入导航信息融合中，变量节点表示组合导航状态变量，因子节点表示组合导航的量测更新过程，接收到来自传感器的量测信息后，利用因子节点进行状态的递推估计。

图3为信息融合的因子图框架，其中涉及的传感器包括IMU、GPS、气压高度计、光流传感器、磁航向传感器、星敏感器。

图3中，x为变量节点。设定系统的状态变量为变量节点：

变量节点包括［9］平台误差角ϕE、ϕN、ϕU，速度误差δνE、δνN、δνU，位置误差δL、δλ、δh，陀螺常值漂移误差εbx、εby、εbz，陀螺1阶马尔可夫漂移误差εrx、εry、εrz和加速度计零偏▽x、▽y、▽z。

在导航框架中，定义量测模型h(·)可以根据给定的状态估计来预测传感器的量测值。因子节点获取预测的量测信息和实际量测信息的差值，构建相应的指标函数从而获取状态变量的估计，基于高斯白噪声模型假设，一个量测因子节点可以用如下公式表示：

式中，hi(Xi)表示量测模型，是与状态变量相关的函数；zi表示实际的观测信息；d(·)表示代价函数。

接收来自IMU的观测信息后，定义因子节点fIMU。fIMU连接不同时刻tk和tk＋1的两个变量节点，即导航状态xk和xk＋1。根据系统的状态方程进行状态更新和变量节点扩增：

其中，a、w分别代表加速度计和陀螺的输出，F(·)表示系统的状态转移函数矩阵。那么因子节点IMU可以表示为：式(6)中所涉及到的未知向量都代表了因子图中的变量节点，其中，IMU因子是连接变量的因子节点［10］。

在tl时刻接收到GPS的观测信息后，定义因子节点fGPS。fGPS连接至变量节点xl，GPS的量测方程如下：

式中，zGPS是GPS的观测信息，hGPS是观测函数，vGPS是观测噪声。

因此，因子节点fGPS可以表示为：

选定因子节点的代价函数如下：

同理，在其他时刻接收到气压高度计、光流传感器、磁航向传感器、以及星敏感器的观测信息后，定义因子节点fbaro、fflow、fmag、fstar拓展因子图，根据传感器的观测方程以及相应的代价函数进行变量节点的状态更新。基于因子图的多传感器信息融合方法，能方便地对来自异步传感器的数据进行处理，接收到传感器的输出数据后，扩充因子图节点，根据系统的状态方程和量测方程快速有效地进行系统状态的更新，实现多传感器的数据综合处理。

4 仿真验证

采用仿真手段对文中提出的方法进行分析验证，本文以Matlab为试验平台，对仿真方法进行了设计，其中包括对飞行器运行轨迹的设计、模拟不同传感器的数据输出以及对文中提出的算法仿真验证。

4.1 仿真条件和方法

仿真中模拟装备多种传感器的无人飞行器的飞行过程，设定飞行器的运行轨迹。模拟飞行器搭载的传感器，包括以下几种：100Hz的MEMS、1Hz的GPS、5Hz的气压高度计、10Hz的光流传感器、10Hz的磁航向传感器、8Hz的星敏感器。根据上节提出的方法对这6种不同更新率的传感器进行因子图构造，对输出数据进行综合处理，从而实现信息融合。

飞行器的初始位置坐标设置为东经106.491°、北纬29.528°、高度300m。给定的飞行器初始姿态角为：横滚角＝0°、俯仰角＝0°、航向角＝90°。设置陀螺和加速度计的误差分别为：陀螺常值漂移误差为0.03(°)／h带有随机白噪声、加速度计的常值偏移量为0.1mg带有随机白噪声，惯导系统的解算周期为0.02s，仿真总时长为1800s。

考虑要验证所提方法在传感器可用性时变情况下的可靠性，对载体飞行过程中的传感器可用性，即故障情况进行设置。由于全源导航的初衷是开发一种在GPS受限情况下的定位导航系统，因此，假设GPS受阻，设置其在500s～1000s时间段内不可用；星敏感器使用条件也较为苛刻，所以设置其在800s～1200s不可用；光流传感器设置为1300s～1400s不可用。通过上述故障情况，测试因子图方法的稳定性、可靠性。

4.2 仿真结果与分析

本次仿真中设计的飞行器运行轨迹如图4所示，运行轨迹仿真通过抬头、爬升、平飞、倾斜、转弯、俯冲等机动动作，以及在经度、纬度、高度的大幅度变化，模拟飞行器真实的飞行状况。

以上述运行轨迹及3.1节中的初始条件为基准进行仿真，分别得到组合导航系统的位置误差曲线、速度误差曲线和姿态误差曲线，如图5所示。

由图5可知，飞行器经过1800s飞行，对于位置误差而言，经度误差在5m以内，纬度误差在10m以内，高度误差在5m以内；东向速度误差在0.2m／s以内，北向速度误差在0.2m／s以内，天向速度误差在0.5m／s以内；对于姿态误差而言，横滚角误差在0.005°以内，俯仰角误差在0.002°以内，航向角在0.005°以内。总体来说，在飞行器机动过程中，可用传感器变更的情况下，导航性能可以得到有效保障。因此，本文提出的方法能很好地适应传感器的可用性变化，根据实时获取的测量数据，快速有效地进行信息融合，从而确保载体的定位导航精度。

5 结论

针对载体多传感器信息融合的应用需求，为了实现多传感器数据的快速集成和组合架构配置，本文结合概率图模型理论，提出了一种基于因子图的多信息融合方法，可用于全源导航的相关领域。采用因子图节点描述系统的状态递推和更新过程，基于因子图模型实现传感器信息的即插即用，综合来自不同传感器的数据，最后进行仿真分析。仿真结果表明，本文所提方法能对异步异构传感器的数据进行有效融合，并适应传感器在运行过程中可用性发生变化的情况，使载体满足不断变化的任务需求与环境变化的要求。

[1] Xiang H，Tian L.Development of a low⁃cost agricultural remote sensing system based on an autonomous unmanned aerial vehicle(UAV)[J].Biosystems Engineering，2011，108(2):174⁃190.

[2] Williams P，Crump M.All⁃source navigation for enhancing UAV operations in GPS⁃denied environments[EB／OL].ht⁃tp:／／www.icas.org／ICAS＿ARCHIVE／ICAS2012／PA⁃PERS／130.PDF，2012.

[3] Wang Y，Meng Y S，Su Z，et al.Study on space⁃based all source navigation technology[C].China Satellite Navigation Conference(CSNC)2015 Proceedings:Volume II.Springer Berlin Heidelberg，2015:113⁃123.

[4] 刘钝，甄卫民，张风国，等.PNT系统体系结构与PNT新技术发展研究[J].全球定位系统，2015，40(2):48⁃52. LIU Dun，ZHEN Wei⁃min，ZHANG Feng⁃guo，et al.PNT system architecture and PNT technology development re⁃search[J].GNSS World of China，2015，40(2):48⁃52.

[5] Fisher K A，Raquet J F.Precision position，navigation，and timing without the global positioning system.[J].Air ＆Space Power Journal，2011，25(2):24⁃30.

[6] 文苏丽，张国庆.美国GPS受限条件下导航定位技术的新发展[J].战术导弹技术，2014(6):81⁃87. WEN Su⁃li，ZHANG Guo⁃qing.The technology progress of PNT in GPS limited conditions[J].Tactical Missile Technology，2014(6):81⁃87.

[7] Loeliger H A，Dauwels J，Hu J，et al.The factor graph ap⁃proach to model⁃based signal processing[C].Proceedings of the IEEE，2007，95(6):1295⁃1322.

[8] Ta D N，Kobilarov M，DellaertF. A factorgraph approach to estimation and model predictive control on un⁃manned aerial vehicles[C].International Conference on Unmanned Aircraft Systems，2014:181⁃188.

[9] 柳敏，赖际舟，黄凯，等.基于加权奇偶矢量的机载自主完好性监测算法[J].中国惯性技术学报，2015，23 (1):43⁃48. LIU Min，LAI Ji⁃zhou，HUANG Kai，et al.Aircraft au⁃tonomous integrity monitoring algorithm based on weighted parity vector[J].Journal of Chinese Inertial Technology，2015，23(1):43⁃48.

[10] Indelman V，Williams S，Kaess M，et al.Information fu⁃sion in navigation systems via factor graph based incre⁃mental smoothing[J].Robotics＆Autonomous Systems，2013，61(8):721⁃738.

Research on the Key Technology of UAV of All Source Position Navigation Based on Factor Graph

WANG Hui⁃zhe1，2，ZENG Qing⁃hua1，2，LIU Jian⁃ye1，2，CHEN Wei⁃na1，2
(1.Navigation Research Center，Nanjing University of Aeronautics＆Astronautics，Nanjing 211106; 2.Satellite Communication and Navigation Collaborative Innovation Center，Nanjing 211106)

The all source position navigation proposed by DARPA needs to integrate and reconfigure any navigation sensors quickly.According to the theory of probability graph model and the mission requirements of the civil unmanned aer⁃ial vehicle，the research on the key technology of all source position navigation based on factor graph was proposed.Factor nodes are used to express system state and measurement update procedure，and system state recursion and updates can be realized，and plug and play sensor information data synthesis can be completed.The simulation results show that the meth⁃od can fuse the data of different sensors effectively，and make sure that the system navigation and positioning accuracy is guaranteed，so that the vehicle can satisfy changing task needs and environmental changes.

unmanned aerial vehicle(UAV);factor graph;all source position navigation;information fusion

V249.32

A

1674⁃5558(2017)01⁃01284

10.3969／j.issn.1674⁃5558.2017.02.001

王慧哲，女，硕士，研究方向为惯性技术与多信息融合组合导航技术。

2016⁃06⁃17

国家自然科学基金(编号：61533008，61328301)