吳坤 劉海穎 謝陽(yáng)光 羅曼

摘要：精確的導(dǎo)航定位系統(tǒng)是飛機(jī)導(dǎo)航、汽車(chē)自主駕駛等領(lǐng)域的重要組成部分，但由于全球?qū)Ш叫l(wèi)星系統(tǒng)（GNSS）在復(fù)雜環(huán)境中會(huì)受到多種因素的影響（如信號(hào)遮擋、多路徑效應(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)航的魯棒性。通過(guò)對(duì)實(shí)測(cè)試驗(yàn)數(shù)據(jù)的評(píng)估驗(yàn)證并與傳統(tǒng)方法進(jìn)行對(duì)比分析，結(jié)果表明本文所采用的最大混合圖優(yōu)化算法相比傳統(tǒng)算法，在GNSS導(dǎo)航中的精度更高、魯棒性更強(qiáng)。

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

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

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

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

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

1因子圖優(yōu)化

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

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

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

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

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

2.1 L2-norm

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

2.2最大混合

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

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

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

3.1低質(zhì)量

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

3.2高質(zhì)量

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

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

4結(jié)論

目前因子圖優(yōu)化在視覺(jué)SLAM中得到了極好應(yīng)用，但對(duì)于GNSS導(dǎo)航中還不多見(jiàn)。在自主導(dǎo)航系統(tǒng)中，需要系統(tǒng)對(duì)不同的情況和較復(fù)雜的環(huán)境做出我們期望的判斷和推斷出較為準(zhǔn)確的狀態(tài)，為此，本試驗(yàn)將因子圖優(yōu)化應(yīng)用到了GNSS導(dǎo)航處理中，并采用不同質(zhì)量的數(shù)據(jù)集評(píng)估了兩種魯棒優(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ā)，卞鴻巍，馬恒，等.基于因子圖的魯棒性增量平滑算法的水面無(wú)人艇組合導(dǎo)航方法[J].中國(guó)慣性技術(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)，等.基于因子圖的車(chē)載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.

作者簡(jiǎn)介

吳坤（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

謝陽(yáng)光（1983-）男，高級(jí)工程師。主要研究方向：導(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