基于因子图的GNSS鲁棒优化算法分析

吴坤 刘海颖 谢阳光 罗曼

摘要：精确的导航定位系统是飞机导航、汽车自主驾驶等领域的重要组成部分，但由于全球导航卫星系统（GNSS）在复杂环境中会受到多种因素的影响（如信号遮挡、多路径效应等）而导致可用性降低，传统的导航定位算法（如卡尔曼滤波）有可能使性能难以达到预期效果。本文将同步定位与地图构建（SLAM）中的因子图优化算法应用于GNSS中，可提高导航定位精度，并采用最大混合（max-mixture，MM）算法提高导航的鲁棒性。通过对实测试验数据的评估验证并与传统方法进行对比分析，结果表明本文所采用的最大混合图优化算法相比传统算法，在GNSS导航中的精度更高、鲁棒性更强。

关键词：全球导航卫星系统；导航定位算法；因子图优化；最大混合；鲁棒导航

中图分类号：TN967.1文献标识码：ADOI：10.19452/j.issn1007-5453.2021.04.011

全球导航卫星系统（GNSS）在航空航天、自动驾驶以及机器人等领域中发挥着重要作用。但是在森林峡谷、高楼林立等复杂环境中会受到各种因素的影响（如信号遮挡、多路径效应等），导致GNSS观测数据的质量下降、导航系统的观测能力受到限制，进而影响GNSS精确导航定位的能力。而且在飞行安全中，导航系统的性能优劣不仅影响着飞机的定位精度、控制性能，而且直接关系到飞机的安全性[1]。因此，将GNSS的观测值安全准确地纳入现有的推理算法中非常必要，特别是在面临有可能降低GNSS可观测性的环境之中[2]。

为了解决这个问题，参考文献[2]利用了机器人研究领域中同步定位与地图构建（SLAM）所取得的进展来高效可靠地处理GNSS数据，其中一个较新的研究方向即因子图的概率推理。目前的方法如通用图优化（G2O）最终都是将SLAM问题转化为最小二乘问题，并通过高斯-牛顿（Gauss-Newton，GN）迭代法等来进行求解。但是最小二乘法容易受到离群值的影响，会导致计算的推断结果出现严重错误，所以必须有更好的鲁棒优化方法来降低离群值的影响。目前的鲁棒方法包括传统的M估计[3]，适用于单峰高斯噪声分布的开关约束（SC）[4]、动态协方差标度（DCS）[5]法，以及适用于混合高斯分布最大混合法（MM）[6-7]。目前这些鲁棒方法主要还是应用在SLAM中，用于GNSS数据处理中的性能未进行过直接比较，特别是基于因子图的方法。

本文将SLAM领域中新兴的因子图优化算法应用到GNSS导航定位中。首先描述因子图的构建和优化，给出因子图优化算法来提高导航精度，其次采用两种不同的鲁棒优化方法提高导航鲁棒性，最后通过试验数据对所提方法进行评估分析。

1因子图优化

因子图是Wiberg等在参考文献[8]和参考文献[9]中提出的Tanner图的一个简单的推广。它作为一种数学工具，是用因子描述多变量复杂函数的二部图，通常被用于SLAM视觉中。图优化就是将优化问题转换成图的形式，图由边和顶点组成，边连接着顶点，表示顶点之间的一种关系。变量表示估计问题中的未知随机变量，而因子表示有关这些变量的概率信息，这些信息是从测量或先验信息中得出的。

图2中x为状态变量，e为对变量x的概率约束，ψ为因子节点，若在模型中加入其他传感器，只需将其添加到框架中相关的因子节点处即可，如添加惯性测量单元（IMU）fIMU。

在本文所介绍的试验中选择的是高斯-牛顿法来进行迭代优化，高斯-牛顿法是解决非线性最小二乘问题的最基本方法，并且仅用于处理这类问题，达到数据拟合、参数估计和状态估计的目的。最小二乘问题可分为线性和非线性两类，对于线性的最小二乘问题，通过一般的理论推导即可得到其解析解，但是对于非线性最小二乘问题，则需要依靠迭代优化的方法来解决。

2鲁棒优化算法

在复杂多变的实际环境中GNSS导航的精度会受到多种因素的影响，为了能够使GNSS导航具有更好的鲁棒性能，必须将优化性能更好的算法加入到系统的数据处理中。在本节下文中会对传统优化算法与新兴的鲁棒优化技术最大混合进行介绍。

2.1 L2-norm

L2范数能够有效地避免过拟合，在参数较小的情况下，即使样本的数据发生比较大的变化，模型的预测值受到的影响也会很小。这主要是因为让L2范数的规则项||W||2尽可能的小（W为一个参数矩阵），可以把W中的每個元素都变得很小而又不会等于0，这是和L1不同的地方所在，得到的模型抗干扰能力比原来的模型强。传统的状态估计技术（如卡尔曼滤波）的基础代价函数是估计误差的L2范数。当假设的模型能够精确地表征所提供的观测值时，这种代价函数可实现准确且有效的估计。但是L2范数代价函数的一个主要的缺点是有一个渐进崩溃性质，具体来说就是任何一个仅使用L2范数代价函数的估计量都有一个渐进崩溃点零，可以通过让任意观测值偏离模型来更直观的理解这一性质，即当所利用的模型不能准确地表征所提供的测量时，估计框架可能由于单个观测值的偏离使状态估计的解产生偏差。

2.2最大混合

目前的鲁棒优化算法有开关约束、动态协方差约束，还有参考文献[12]中提出的动态协方差估计（DCE）等。前两者虽然也都对异常值具有鲁棒性，但是它们都有一个缺点在于它们都先假设所有不确定度都是遵循高斯单峰分布的，在传统的用因子图表示位姿图SLAM时也是如此。而DCE由于优化曲面是非凸的，该方法仍然局限于初始值良好、离群值适中的问题。为了能够放宽前面说到的限制，可以使用高斯混合模型（Gaussian mixture model，GMM）来表示这个不确定度模型，但是采用高斯混合模型来表示这个不确定度时，虽然可以很大程度上提高描述误差的精度，但是其不能将高斯分量的加权和转化为最小二乘问题来进行计算，所以会大大增加其计算量，优化问题的复杂性也随之增加。

为了能够使用高斯混合模型，又要使优化过程的计算量降到最低，在参考文献[6]中Olson和Agarwal提出了最大混合模型。与前面提到的方法不同的是，这允许处理非对称或多模态分布的概率正确性。这个方法通过用最大运算来代替高斯混合模型中的求和运算来规避增加的计算复杂度，即对于每一个观测值，它会在高斯混合模型中选择单一的高斯分量，在给定了当前状态估计值的情况下，应用最大似然选择使观测值的可能性最大化的分量。在参考文献[14]中提到，最大运算作为一个选择器，可以将优化过程简化为残差平方和的加权总和，即非线性最小二乘问题。与参考文献[14]中讨论的其他鲁棒状态估计公式类似，最大混合算法的实现也可以解释为通过协方差自适应来实现鲁棒性。

3試验及分析

本文试验使用佐治亚理工平滑和地图绘制（Georgia tech smoothing and mapping，GTSAM）作为核心组件，作为目前在因子图增量平滑方面研究成果的集成，应用于因子图优化，GTSAM能发挥很大的作用，所以本文试验中因子图的构建和优化都使用GTSAM来进行。

本节将对上文提到的两种不同算法进行比对，为了让试验结果不具有特殊性，将采用不同质量的GNSS信号进行试验。在试验中使用软件无线电可以更改接收机GPS伪距和载波相位可观测值的精度，以测地型GNSS接收机的观测质量作为参考基准来量化，将观测数据调谐成低质量和高质量两种不同的精度，其中低质量数据集的平均三维误差为16.20m，高质量的为0.59m。并且为了更明显地比较出两种算法的鲁棒性优劣，还在观测值中人为加入随机的模拟故障信息。最后在Matlab中对经过两种优化算法后的数据与原始数据的拟合程度进行了误差比较，并计算了两种算法的误差的均值、方差以及两种算法的运行时间。

3.1低质量

在图4中进行地面轨迹比较后，在图5和图6中分别对经过两种算法优化后的东向和北向数据集与原始数据集之间的误差进行比较，可以看出当使用低质量观测值时，L2和最大混合的偏差都比较大，但是最大混合的拟合程度更高，更准确地还原了真实的地面轨迹。

3.2高质量

从图7可以看出，当使用高质量观测值时，两种算法与初始数据集的拟合程度明显提高，但最大混合算法的轨迹更接近于真实数据集的地面轨迹。东向和北向的导航误差结果如图8和图9所示，可以看出高质量观测值的数据下，两者拟合精度都有所提高，且最大混合整体的误差更小一些。统计结果见表1、表2，在相同环境下测试时两种算法运行的时间不同，L2的运行时间更快，但最大混合的误差较小，估计精度更高一些，其算法复杂度较高。

在试验中人为地加入了模拟故障信息用于更明显的对比两种算法的鲁棒性，图10和图11是高质量信号下两种算法在受到随机故障信息影响而产生突变性的误差时的图像，可以看出在受到影响时，L2方法得到的优化估计结果较大的偏离真实状态，而最大混合算法对故障信息具有更好的鲁棒性。

4结论

目前因子图优化在视觉SLAM中得到了极好应用，但对于GNSS导航中还不多见。在自主导航系统中，需要系统对不同的情况和较复杂的环境做出我们期望的判断和推断出较为准确的状态，为此，本试验将因子图优化应用到了GNSS导航处理中，并采用不同质量的数据集评估了两种鲁棒优化后的系统性能，结果表明因子图优化应用在GNSS数据处理中有优异的表现，且在优化算法方面，基于因子图的最大混合算法较L2拥有更好的数据处理能力，在算法鲁棒性上也强于L2，最大混合算法具有更好的状态估计精度。

参考文献

[1]史萌，连晓棠，王汉平，等.惯导智能故障诊断系统设计[J].航空科学技术，2020，31（10）：51-56. Shi Meng， Lian Xiaotang， Wang Hanping， et al. Design of intelligent fault diagnosis for inertial navigation system [J]. Aeronautical Science & Technology， 2020， 31（10）： 51-56. （in Chinese）

[2]Watson R M，Gross J N. Robust navigation in GNSS degraded environment using graph optimization[C]//ION GNSS+2017，Portland，2017：2906-2918.

[3]Indelman V，Williams S，Kaess M，et al. Factor graph based incremental smoothing in inertial navigation systems[C]// International Conference on Information Fusion，2012：2154-2161.

[4]Sunderhauf N，Protzel P. Switchable constraints for robust pose graph SLAM[C]// IEEE/RSJ International Conference on Intelligent Robots & Systems. IEEE，2012.

[5]Agarwal P，Tipaldi G D，Spinello L，et al. Robust map optimization using dynamic covariance scaling[C]// Robotics and Automation（ICRA），2013 IEEE International Conference on. IEEE，2013.

[6]Olson E，Agarwal P. Inference on networks of mixtures for robust robot mapping[J]. International Journal of Robotics Research，2013，32（7）：826-840.

[7]戴海發，卞鸿巍，马恒，等.基于因子图的鲁棒性增量平滑算法的水面无人艇组合导航方法[J].中国惯性技术学报，2018， 26（6）：779-786. Dai Haifa， Bian Hongwei， Ma Heng， et al. Application of robust incremental smoothing algorithm based on factor graph in integrated navigation of unmanned surface vehicle[J]. Journal of Chinese Inertial Technology， 2018，26（6）：779-786.（in Chinese）

[8]Wiberg N. Codes and iterative decoding on general graphs[J]. European Transactions on Telecommunications，1995，6（5）：513-525.

[9]Wiberg N，Loeliger H A，Kotter R. Codes and iterative decoding on general graphs[C]// Proceedings of 1995 IEEE International Symposium on Information Theory. IEEE，2002.

[10]高军强，汤霞清，张环，等.基于因子图的车载INS/GNSS/OD组合导航算法[J].系统工程与电子技术，2018，40（11）：2548-2553. Gao Junqiang， Tang Xiaqing， Zhang Huan， et al. Vehicle INS/ GNSS/OD integrated navigation algorithm based on factor graph[J]. Systems Engineering and Electronics， 2018，40（11）：2548-2553.（in Chinese）

[11]Graham M C. Robust Bayesian state estimation and mapping[D]. Massachusetts Institute of Technology，2015.

[12]PfeiferT，LangeS，ProtzelP.Dynamiccovariance estimation：a parameter free approach to robust sensor fusion[C]// 2017 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems（MFI）. IEEE，2017.

[13]Morton R，Olson E. Robust sensor characterization via maxmixture models：GPS sensors[C]//IEEE/RSJ International Conference on Intelligent Robots & Systems. IEEE，2013.

[14]Watson R M，Gross J N，Taylor C N，et al. Enabling robust stateestimationthroughmeasurementerrorcovariance adaptation[J]. IEEE Transactions on Aerospace and Electronic Systems，2019，99：1.

作者简介

吴坤（1998-）男，硕士研究生。主要研究方向：导航制导与控制。

Tel：15625666199

E-mail：904705610@qq.com

刘海颖（1980-）男，副教授。主要研究方向：导航制导与控制。

Tel：13951716495

E-mail：liuhaiying@nuaa.edu.cn

谢阳光（1983-）男，高级工程师。主要研究方向：导航制导与控制。

E-mail：xiesunshine@163.com

罗曼（1997-）男，硕士研究生。主要研究方向：导航制导与控制。

E-mail：2676830149@qq.com

Analysis on GNSS Robust Optimization Algorithms Based on Factor Graph

Wu Kun1，Liu Haiying1，3，*，Xie Yangguang2，Luo Man1

1. Nanjing University of Aeronautics and Astronautics，Nanjing 210016，China 2. Aviation Key Laboratory of Science and Technology on Inertia，AVIC Xian Fight Automatic Control Research Institut，Xian 710065，China 3. Nanjing Center for Applied Mathematics，Nanjing 211135，China

Abstract： Accurate navigation and positioning systems have become an important part of aircraft navigation and autonomous driving of automobiles. However， due to the complex environments， Global Navigation Satellite System（GNSS） will be affected by many factors （e. g.， signal occlusion， multipath effect）， resulting in reduced usability. Traditional navigation and positioning algorithms （e.g.， Kalman Filtering） may make it difficult to achieve the desired performance. In this paper， the factor graph optimization algorithm in Simultaneous Localization and Mapping （SLAM） is applied to GNSS which can improve the accuracy of navigation and positioning， and uses the Max-mixture （MM） algorithm to improve the robustness of navigation. Through the evaluation and verification of the actual test data and the comparative analysis with the traditional method， the result shows that the maximum mixed graph optimization algorithm used in this paper has higher accuracy and stronger robustness in GNSS navigation than the traditional algorithm.

Key Words： GNSS； navigation and positioning algorithm； factor graph optimization； Max-mixture； robust navigation