吳坤 劉海穎 謝陽光 羅曼

摘要：精確的導(dǎo)航定位系統(tǒng)是飛機(jī)導(dǎo)航、汽車自主駕駛等領(lǐng)域的重要組成部分，但由于全球?qū)Ш叫l(wèi)星系統(tǒng)（GNSS）在復(fù)雜環(huán)境中會受到多種因素的影響（如信號遮擋、多路徑效應(yīng)等）而導(dǎo)致可用性降低，傳統(tǒng)的導(dǎo)航定位算法（如卡爾曼濾波）有可能使性能難以達(dá)到預(yù)期效果。本文將同步定位與地圖構(gòu)建（SLAM）中的因子圖優(yōu)化算法應(yīng)用于GNSS中，可提高導(dǎo)航定位精度，并采用最大混合（max-mixture，MM）算法提高導(dǎo)航的魯棒性。通過對實(shí)測試驗(yàn)數(shù)據(jù)的評估驗(yàn)證并與傳統(tǒng)方法進(jìn)行對比分析，結(jié)果表明本文所采用的最大混合圖優(yōu)化算法相比傳統(tǒng)算法，在GNSS導(dǎo)航中的精度更高、魯棒性更強(qiáng)。

關(guān)鍵詞：全球?qū)Ш叫l(wèi)星系統(tǒng)；導(dǎo)航定位算法；因子圖優(yōu)化；最大混合；魯棒導(dǎo)航

中圖分類號：TN967.1文獻(xiàn)標(biāo)識碼：ADOI：10.19452/j.issn1007-5453.2021.04.011

全球?qū)Ш叫l(wèi)星系統(tǒng)（GNSS）在航空航天、自動駕駛以及機(jī)器人等領(lǐng)域中發(fā)揮著重要作用。但是在森林峽谷、高樓林立等復(fù)雜環(huán)境中會受到各種因素的影響（如信號遮擋、多路徑效應(yīng)等），導(dǎo)致GNSS觀測數(shù)據(jù)的質(zhì)量下降、導(dǎo)航系統(tǒng)的觀測能力受到限制，進(jìn)而影響GNSS精確導(dǎo)航定位的能力。而且在飛行安全中，導(dǎo)航系統(tǒng)的性能優(yōu)劣不僅影響著飛機(jī)的定位精度、控制性能，而且直接關(guān)系到飛機(jī)的安全性[1]。因此，將GNSS的觀測值安全準(zhǔn)確地納入現(xiàn)有的推理算法中非常必要，特別是在面臨有可能降低GNSS可觀測性的環(huán)境之中[2]。

為了解決這個問題，參考文獻(xiàn)[2]利用了機(jī)器人研究領(lǐng)域中同步定位與地圖構(gòu)建（SLAM）所取得的進(jìn)展來高效可靠地處理GNSS數(shù)據(jù)，其中一個較新的研究方向即因子圖的概率推理。目前的方法如通用圖優(yōu)化（G2O）最終都是將SLAM問題轉(zhuǎn)化為最小二乘問題，并通過高斯-牛頓（Gauss-Newton，GN）迭代法等來進(jìn)行求解。但是最小二乘法容易受到離群值的影響，會導(dǎo)致計(jì)算的推斷結(jié)果出現(xiàn)嚴(yán)重錯誤，所以必須有更好的魯棒優(yōu)化方法來降低離群值的影響。目前的魯棒方法包括傳統(tǒng)的M估計(jì)[3]，適用于單峰高斯噪聲分布的開關(guān)約束（SC）[4]、動態(tài)協(xié)方差標(biāo)度（DCS）[5]法，以及適用于混合高斯分布最大混合法（MM）[6-7]。目前這些魯棒方法主要還是應(yīng)用在SLAM中，用于GNSS數(shù)據(jù)處理中的性能未進(jìn)行過直接比較，特別是基于因子圖的方法。

本文將SLAM領(lǐng)域中新興的因子圖優(yōu)化算法應(yīng)用到GNSS導(dǎo)航定位中。首先描述因子圖的構(gòu)建和優(yōu)化，給出因子圖優(yōu)化算法來提高導(dǎo)航精度，其次采用兩種不同的魯棒優(yōu)化方法提高導(dǎo)航魯棒性，最后通過試驗(yàn)數(shù)據(jù)對所提方法進(jìn)行評估分析。

1因子圖優(yōu)化

因子圖是Wiberg等在參考文獻(xiàn)[8]和參考文獻(xiàn)[9]中提出的Tanner圖的一個簡單的推廣。它作為一種數(shù)學(xué)工具，是用因子描述多變量復(fù)雜函數(shù)的二部圖，通常被用于SLAM視覺中。圖優(yōu)化就是將優(yōu)化問題轉(zhuǎn)換成圖的形式，圖由邊和頂點(diǎn)組成，邊連接著頂點(diǎn)，表示頂點(diǎn)之間的一種關(guān)系。變量表示估計(jì)問題中的未知隨機(jī)變量，而因子表示有關(guān)這些變量的概率信息，這些信息是從測量或先驗(yàn)信息中得出的。

圖2中x為狀態(tài)變量，e為對變量x的概率約束，ψ為因子節(jié)點(diǎn)，若在模型中加入其他傳感器，只需將其添加到框架中相關(guān)的因子節(jié)點(diǎn)處即可，如添加慣性測量單元（IMU）fIMU。

在本文所介紹的試驗(yàn)中選擇的是高斯-牛頓法來進(jìn)行迭代優(yōu)化，高斯-牛頓法是解決非線性最小二乘問題的最基本方法，并且僅用于處理這類問題，達(dá)到數(shù)據(jù)擬合、參數(shù)估計(jì)和狀態(tài)估計(jì)的目的。最小二乘問題可分為線性和非線性兩類，對于線性的最小二乘問題，通過一般的理論推導(dǎo)即可得到其解析解，但是對于非線性最小二乘問題，則需要依靠迭代優(yōu)化的方法來解決。

2魯棒優(yōu)化算法

在復(fù)雜多變的實(shí)際環(huán)境中GNSS導(dǎo)航的精度會受到多種因素的影響，為了能夠使GNSS導(dǎo)航具有更好的魯棒性能，必須將優(yōu)化性能更好的算法加入到系統(tǒng)的數(shù)據(jù)處理中。在本節(jié)下文中會對傳統(tǒng)優(yōu)化算法與新興的魯棒優(yōu)化技術(shù)最大混合進(jìn)行介紹。

2.1 L2-norm

L2范數(shù)能夠有效地避免過擬合，在參數(shù)較小的情況下，即使樣本的數(shù)據(jù)發(fā)生比較大的變化，模型的預(yù)測值受到的影響也會很小。這主要是因?yàn)樽孡2范數(shù)的規(guī)則項(xiàng)||W||2盡可能的?。╓為一個參數(shù)矩陣），可以把W中的每個元素都變得很小而又不會等于0，這是和L1不同的地方所在，得到的模型抗干擾能力比原來的模型強(qiáng)。傳統(tǒng)的狀態(tài)估計(jì)技術(shù)（如卡爾曼濾波）的基礎(chǔ)代價函數(shù)是估計(jì)誤差的L2范數(shù)。當(dāng)假設(shè)的模型能夠精確地表征所提供的觀測值時，這種代價函數(shù)可實(shí)現(xiàn)準(zhǔn)確且有效的估計(jì)。但是L2范數(shù)代價函數(shù)的一個主要的缺點(diǎn)是有一個漸進(jìn)崩潰性質(zhì)，具體來說就是任何一個僅使用L2范數(shù)代價函數(shù)的估計(jì)量都有一個漸進(jìn)崩潰點(diǎn)零，可以通過讓任意觀測值偏離模型來更直觀的理解這一性質(zhì)，即當(dāng)所利用的模型不能準(zhǔn)確地表征所提供的測量時，估計(jì)框架可能由于單個觀測值的偏離使?fàn)顟B(tài)估計(jì)的解產(chǎn)生偏差。

2.2最大混合

目前的魯棒優(yōu)化算法有開關(guān)約束、動態(tài)協(xié)方差約束，還有參考文獻(xiàn)[12]中提出的動態(tài)協(xié)方差估計(jì)（DCE）等。前兩者雖然也都對異常值具有魯棒性，但是它們都有一個缺點(diǎn)在于它們都先假設(shè)所有不確定度都是遵循高斯單峰分布的，在傳統(tǒng)的用因子圖表示位姿圖SLAM時也是如此。而DCE由于優(yōu)化曲面是非凸的，該方法仍然局限于初始值良好、離群值適中的問題。為了能夠放寬前面說到的限制，可以使用高斯混合模型（Gaussian mixture model，GMM）來表示這個不確定度模型，但是采用高斯混合模型來表示這個不確定度時，雖然可以很大程度上提高描述誤差的精度，但是其不能將高斯分量的加權(quán)和轉(zhuǎn)化為最小二乘問題來進(jìn)行計(jì)算，所以會大大增加其計(jì)算量，優(yōu)化問題的復(fù)雜性也隨之增加。

為了能夠使用高斯混合模型，又要使優(yōu)化過程的計(jì)算量降到最低，在參考文獻(xiàn)[6]中Olson和Agarwal提出了最大混合模型。與前面提到的方法不同的是，這允許處理非對稱或多模態(tài)分布的概率正確性。這個方法通過用最大運(yùn)算來代替高斯混合模型中的求和運(yùn)算來規(guī)避增加的計(jì)算復(fù)雜度，即對于每一個觀測值，它會在高斯混合模型中選擇單一的高斯分量，在給定了當(dāng)前狀態(tài)估計(jì)值的情況下，應(yīng)用最大似然選擇使觀測值的可能性最大化的分量。在參考文獻(xiàn)[14]中提到，最大運(yùn)算作為一個選擇器，可以將優(yōu)化過程簡化為殘差平方和的加權(quán)總和，即非線性最小二乘問題。與參考文獻(xiàn)[14]中討論的其他魯棒狀態(tài)估計(jì)公式類似，最大混合算法的實(shí)現(xiàn)也可以解釋為通過協(xié)方差自適應(yīng)來實(shí)現(xiàn)魯棒性。

3試驗(yàn)及分析

本文試驗(yàn)使用佐治亞理工平滑和地圖繪制（Georgia tech smoothing and mapping，GTSAM）作為核心組件，作為目前在因子圖增量平滑方面研究成果的集成，應(yīng)用于因子圖優(yōu)化，GTSAM能發(fā)揮很大的作用，所以本文試驗(yàn)中因子圖的構(gòu)建和優(yōu)化都使用GTSAM來進(jìn)行。

本節(jié)將對上文提到的兩種不同算法進(jìn)行比對，為了讓試驗(yàn)結(jié)果不具有特殊性，將采用不同質(zhì)量的GNSS信號進(jìn)行試驗(yàn)。在試驗(yàn)中使用軟件無線電可以更改接收機(jī)GPS偽距和載波相位可觀測值的精度，以測地型GNSS接收機(jī)的觀測質(zhì)量作為參考基準(zhǔn)來量化，將觀測數(shù)據(jù)調(diào)諧成低質(zhì)量和高質(zhì)量兩種不同的精度，其中低質(zhì)量數(shù)據(jù)集的平均三維誤差為16.20m，高質(zhì)量的為0.59m。并且為了更明顯地比較出兩種算法的魯棒性優(yōu)劣，還在觀測值中人為加入隨機(jī)的模擬故障信息。最后在Matlab中對經(jīng)過兩種優(yōu)化算法后的數(shù)據(jù)與原始數(shù)據(jù)的擬合程度進(jìn)行了誤差比較，并計(jì)算了兩種算法的誤差的均值、方差以及兩種算法的運(yùn)行時間。

3.1低質(zhì)量

在圖4中進(jìn)行地面軌跡比較后，在圖5和圖6中分別對經(jīng)過兩種算法優(yōu)化后的東向和北向數(shù)據(jù)集與原始數(shù)據(jù)集之間的誤差進(jìn)行比較，可以看出當(dāng)使用低質(zhì)量觀測值時，L2和最大混合的偏差都比較大，但是最大混合的擬合程度更高，更準(zhǔn)確地還原了真實(shí)的地面軌跡。

3.2高質(zhì)量

從圖7可以看出，當(dāng)使用高質(zhì)量觀測值時，兩種算法與初始數(shù)據(jù)集的擬合程度明顯提高，但最大混合算法的軌跡更接近于真實(shí)數(shù)據(jù)集的地面軌跡。東向和北向的導(dǎo)航誤差結(jié)果如圖8和圖9所示，可以看出高質(zhì)量觀測值的數(shù)據(jù)下，兩者擬合精度都有所提高，且最大混合整體的誤差更小一些。統(tǒng)計(jì)結(jié)果見表1、表2，在相同環(huán)境下測試時兩種算法運(yùn)行的時間不同，L2的運(yùn)行時間更快，但最大混合的誤差較小，估計(jì)精度更高一些，其算法復(fù)雜度較高。

在試驗(yàn)中人為地加入了模擬故障信息用于更明顯的對比兩種算法的魯棒性，圖10和圖11是高質(zhì)量信號下兩種算法在受到隨機(jī)故障信息影響而產(chǎn)生突變性的誤差時的圖像，可以看出在受到影響時，L2方法得到的優(yōu)化估計(jì)結(jié)果較大的偏離真實(shí)狀態(tài)，而最大混合算法對故障信息具有更好的魯棒性。

4結(jié)論

目前因子圖優(yōu)化在視覺SLAM中得到了極好應(yīng)用，但對于GNSS導(dǎo)航中還不多見。在自主導(dǎo)航系統(tǒng)中，需要系統(tǒng)對不同的情況和較復(fù)雜的環(huán)境做出我們期望的判斷和推斷出較為準(zhǔn)確的狀態(tài)，為此，本試驗(yàn)將因子圖優(yōu)化應(yīng)用到了GNSS導(dǎo)航處理中，并采用不同質(zhì)量的數(shù)據(jù)集評估了兩種魯棒優(yōu)化后的系統(tǒng)性能，結(jié)果表明因子圖優(yōu)化應(yīng)用在GNSS數(shù)據(jù)處理中有優(yōu)異的表現(xiàn)，且在優(yōu)化算法方面，基于因子圖的最大混合算法較L2擁有更好的數(shù)據(jù)處理能力，在算法魯棒性上也強(qiáng)于L2，最大混合算法具有更好的狀態(tài)估計(jì)精度。

參考文獻(xiàn)

[1]史萌，連曉棠，王漢平，等.慣導(dǎo)智能故障診斷系統(tǒng)設(shè)計(jì)[J].航空科學(xué)技術(shù)，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]戴海發(fā)，卞鴻巍，馬恒，等.基于因子圖的魯棒性增量平滑算法的水面無人艇組合導(dǎo)航方法[J].中國慣性技術(shù)學(xué)報(bào)，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]高軍強(qiáng)，湯霞清，張環(huán)，等.基于因子圖的車載INS/GNSS/OD組合導(dǎo)航算法[J].系統(tǒng)工程與電子技術(shù)，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-）男，碩士研究生。主要研究方向：導(dǎo)航制導(dǎo)與控制。

Tel：15625666199

E-mail：904705610@qq.com

劉海穎（1980-）男，副教授。主要研究方向：導(dǎo)航制導(dǎo)與控制。

Tel：13951716495

E-mail：liuhaiying@nuaa.edu.cn

謝陽光（1983-）男，高級工程師。主要研究方向：導(dǎo)航制導(dǎo)與控制。

E-mail：xiesunshine@163.com

羅曼（1997-）男，碩士研究生。主要研究方向：導(dǎo)航制導(dǎo)與控制。

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