基于因子圖的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