面向城市復(fù)雜環(huán)境的GNSS/INS 高精度圖優(yōu)化算法

韓勇強(qiáng)，于瀟穎，紀(jì)澤源，陳家斌

（1.北京理工大學(xué) 自動(dòng)化學(xué)院，北京 100081；2.北京理工大學(xué) 光電學(xué)院，北京 100081）

慣性/衛(wèi)星組合導(dǎo)航由于其高精度、高魯棒性等諸多優(yōu)點(diǎn)，成為地面、空中等多類移動(dòng)平臺(tái)廣泛采用的重要導(dǎo)航模式。但城市復(fù)雜環(huán)境下，衛(wèi)星信號(hào)易被建筑物遮擋且受多路徑和非視距等效應(yīng)影響[1]，導(dǎo)致GNSS 信號(hào)出現(xiàn)粗差或者信號(hào)丟失情況。

近年來，圖優(yōu)化算法(Factor Graph Optimization,FGO)相比擴(kuò)展卡爾曼濾波算法(Extended Kalman Filter,EKF)在數(shù)據(jù)融合方面體現(xiàn)出了明顯優(yōu)勢[2]。IMU 預(yù)積分對圖優(yōu)化算法的精度具有重要影響，現(xiàn)有圖優(yōu)化算法應(yīng)用背景多采用低精度IMU，往往忽略慣導(dǎo)解算中的地球自轉(zhuǎn)與有害加速度積分項(xiàng)。為了提高預(yù)積分精度，文獻(xiàn)[3,4]提出了考慮地球自轉(zhuǎn)的IMU 預(yù)積分模型。文獻(xiàn)[5]驗(yàn)證了即使對于消費(fèi)級(jí)MEMS-IMU，現(xiàn)有的IMU 預(yù)積分也不夠精確，必須改進(jìn)；并將改進(jìn)的預(yù)積分模型應(yīng)用到GNSS/INS 組合導(dǎo)航系統(tǒng)中，構(gòu)建了基于優(yōu)化的GNSS/INS 集成導(dǎo)航系統(tǒng) (Optimization-Based GNSS/INS Integrated Navigation System,OB-GINS)，該系統(tǒng)顯著提高了基于FGO 的GNSS/INS 積分的精度，但OB-GINS 系統(tǒng)并沒有考慮衛(wèi)星信號(hào)在復(fù)雜環(huán)境下的局限性。

當(dāng)衛(wèi)星信號(hào)出現(xiàn)短暫粗差時(shí)，圖優(yōu)化算法無法感知傳感器性能變化，認(rèn)為異常的觀測值依舊在可控范圍內(nèi)，進(jìn)而導(dǎo)致估計(jì)誤差增大；EKF 算法對異常的觀測值具備一定的判別能力，可降低粗差對系統(tǒng)估計(jì)造成的影響，但當(dāng)衛(wèi)星信號(hào)恢復(fù)正常時(shí)，無法及時(shí)作出響應(yīng)，進(jìn)而影響系統(tǒng)后續(xù)的估計(jì)精度?？偠灾瑹o論是濾波算法還是優(yōu)化算法，衛(wèi)星粗差觀測值一旦作用到系統(tǒng)，都會(huì)導(dǎo)致嚴(yán)重估計(jì)誤差。針對上述問題，國內(nèi)外的研究學(xué)者開展了大量研究。文獻(xiàn)[6]提出了一種基于殘差卡方檢驗(yàn)方法容錯(cuò)卡爾曼濾波器方案，包括三個(gè)子濾波器和一個(gè)主濾波器。文獻(xiàn)[7]設(shè)計(jì)了一種冗余量測噪聲協(xié)方差估計(jì)方法，可有效剔除多路徑等影響產(chǎn)生的野值。文獻(xiàn)[8]基于模糊理論的隸屬度函數(shù)，提出了一種模糊強(qiáng)跟蹤EKF 算法，可以快速適應(yīng)衛(wèi)星信號(hào)突變。文獻(xiàn)[9]使用卡爾曼濾波新息值，設(shè)計(jì)了一種基于高斯檢驗(yàn)的魯棒估計(jì)方法。文獻(xiàn)[10]結(jié)合慣導(dǎo)與衛(wèi)星原始信息進(jìn)行導(dǎo)航系統(tǒng)故障檢測，而后基于故障檢測結(jié)果對故障因子節(jié)點(diǎn)進(jìn)行自適應(yīng)隔離。本文針對高精度IMU/GNSS 數(shù)據(jù)融合問題，重點(diǎn)解決預(yù)積分模型不準(zhǔn)與衛(wèi)星定位粗差抑制問題，提高組合導(dǎo)航系統(tǒng)的位姿解算精度。

針對城市復(fù)雜環(huán)境下高精度、高魯棒性導(dǎo)航定位的需求，本文在構(gòu)建IMU 因子、GNSS 因子的基礎(chǔ)上，搭建了基于因子圖的慣性/GNSS 數(shù)據(jù)融合算法架構(gòu)(GNSS-Inertial navigation system with Gross error Detection,GD-GINS)，通過改進(jìn)預(yù)積分模型，提升了因子圖優(yōu)化算法精度；基于衛(wèi)星信息關(guān)聯(lián)性的特性，設(shè)計(jì)了一種在線滑動(dòng)窗口粗差檢測算法，可有效提升衛(wèi)星觀測粗差影響條件下的組合導(dǎo)航精度。最后通過仿真和試驗(yàn)驗(yàn)證本算法的優(yōu)越性。

1 多源融合圖優(yōu)化理論

因子圖是由變量和因子節(jié)點(diǎn)組合而成的雙邊圖模型，通過對狀態(tài)隨時(shí)間的后驗(yàn)概率進(jìn)行編碼，來解決不同頻率異構(gòu)傳感器融合問題，如圖1 所示。

圖1 基于圖優(yōu)化的GNSS/INS 融合框架Fig.1 GNSS/INS fusion framework base on factor garph optimization

在GNSS/INS 融合導(dǎo)航系統(tǒng)中，第k時(shí)刻的狀態(tài)誤差變量節(jié)點(diǎn)的最小化誤差函數(shù)為：

兩個(gè)相鄰時(shí)刻變量節(jié)點(diǎn)之間的約束定義為二元狀態(tài)轉(zhuǎn)移因子：

基于圖優(yōu)化的組合導(dǎo)航算法，最小化所有測量殘差的先驗(yàn)和馬氏距離之和，以獲得最大后驗(yàn)概率估計(jì)。

2 基于GNSS/INS 的高精度圖優(yōu)化算法

2.1 算法方案

如圖2 所示，本文提出的圖優(yōu)化算法主要分為衛(wèi)星、慣導(dǎo)和非線性優(yōu)化三個(gè)模塊。衛(wèi)星數(shù)據(jù)進(jìn)入衛(wèi)星模塊做預(yù)處理，慣導(dǎo)數(shù)據(jù)進(jìn)入慣導(dǎo)模塊做預(yù)處理，預(yù)處理后的數(shù)據(jù)插入到非線性優(yōu)化模塊的滑動(dòng)窗口內(nèi)；在非線性優(yōu)化模塊中，新的傳感器數(shù)據(jù)加入到滑動(dòng)窗口后，進(jìn)行局部狀態(tài)優(yōu)化，如果滑動(dòng)窗口內(nèi)的狀態(tài)數(shù)量達(dá)到閾值，則進(jìn)行邊緣化處理。

圖2 算法方案Fig.2 Algorithm diagram

2.2 坐標(biāo)系與待優(yōu)化狀態(tài)定義

如圖1 所示，導(dǎo)航系統(tǒng)中的待優(yōu)化狀態(tài)變量定義為X=[x0,x1…xn]。其中xk為：

其中，b系為載體坐標(biāo)系，x、y、z軸分別指向載體前、右、下方，且與載體固連。w系為導(dǎo)航坐標(biāo)系，x軸指向正北，y軸指向正東，z軸指向地。為tk時(shí)刻載體在w系下的位置向量、姿態(tài)四元數(shù)和速度向量，和分別為陀螺儀和加速度計(jì)的零偏。

2.3 衛(wèi)星慣導(dǎo)模塊

(1) 衛(wèi)星滑動(dòng)窗口粗差檢測

GNSS 觀測信號(hào)在多幀之間是相關(guān)的，基于該特性，本文構(gòu)建滑動(dòng)窗口，根據(jù)窗口內(nèi)數(shù)據(jù)的統(tǒng)計(jì)概率值檢測新的測量值是否為粗差。

計(jì)算窗口內(nèi)測量值的均值和標(biāo)準(zhǔn)差：

其中，l=1,2…L，L是觀測序列的維數(shù)。

如果m+1 時(shí)刻測量值被剔除，使用窗口內(nèi)歷史觀測值擬合出新的觀測值代替粗差。本文假設(shè)導(dǎo)航系統(tǒng)前100 s 內(nèi)衛(wèi)星信號(hào)處于開闊視野中，通過前100 s 內(nèi)觀測值的統(tǒng)計(jì)概率特性確定λ的取值。

(2) 構(gòu)建GNSS 因子

GNSS 位置和速度因子的殘差函數(shù)分別為：

(3) 慣導(dǎo)模塊

IMU 測量模型：

其中，e系為地心地固坐標(biāo)系，i系為慣性坐標(biāo)系。本文參考文獻(xiàn)[7]中的IMU 預(yù)積分模型，該模型相比于傳統(tǒng)的IMU 預(yù)積分模型，考慮了地球自轉(zhuǎn)加速度，進(jìn)而引入科氏加速度以提高模型精度。載體運(yùn)動(dòng)引起w系相對于e系的轉(zhuǎn)動(dòng)角速度在w系內(nèi)的投影，記為，由于其引起的誤差小于本文使用的傳感器誤差，所以本文忽略了該項(xiàng)。

IMU 預(yù)積分因子的殘差函數(shù)定義為：

2.4 非線性優(yōu)化模塊

(1) 滑動(dòng)窗口優(yōu)化

基于圖優(yōu)化的組合導(dǎo)航算法，最小化所有測量殘差的先驗(yàn)和馬氏距離之和，以獲得最大后驗(yàn)概率估計(jì)。

根據(jù)上述推導(dǎo)得到的矢量形式的表達(dá)式：

令A(yù)=JTΣ-1J，B=

為使上式的值最小，即使其一階導(dǎo)數(shù)為0，可得：

則狀態(tài)變量的解為：

最后使用LM 迭代法求解最優(yōu)值，獲得增量ΔX。

(2) 狀態(tài)邊緣化

當(dāng)窗口內(nèi)的狀態(tài)數(shù)量超出閾值時(shí)，將最舊的狀態(tài)邊緣化，并將邊緣化狀態(tài)對應(yīng)的IMU 預(yù)積分測量和GNSS 位置和速度測量轉(zhuǎn)化為先驗(yàn)因子。

根據(jù)最優(yōu)化理論，采用LM 線性優(yōu)化方法對殘差進(jìn)行優(yōu)化時(shí)，每次迭代等價(jià)于求解增量方程

兩邊同乘舒爾項(xiàng)：

通過舒爾補(bǔ)過程，將要消去的變量Δx1構(gòu)建的約束關(guān)系疊加到和其有觀測關(guān)系的變量Δx2上，保留了先驗(yàn)信息。

3 GNSS/INS 圖優(yōu)化算法實(shí)現(xiàn)

綜上，本文算法流程如圖3 所示。首先判斷GNSS信號(hào)是否拒止，如若拒止，則等待下一時(shí)間節(jié)點(diǎn)；如沒有拒止，則將該時(shí)刻觀測值插入粗差檢測滑動(dòng)窗口進(jìn)行處理，經(jīng)過粗差檢測后，將GNSS 數(shù)據(jù)從全局坐標(biāo)轉(zhuǎn)換為局部坐標(biāo)，構(gòu)建GNSS 位置因子、GNSS 速度因子，同時(shí)通過初始的位置得到重力加速度、地球自轉(zhuǎn)角速度參數(shù)；利用INS 數(shù)據(jù)構(gòu)建IMU 預(yù)積分因子。將構(gòu)建的各傳感器因子插入到因子圖中進(jìn)行局部狀態(tài)圖優(yōu)化，得到優(yōu)化狀態(tài)值。

圖3 基于圖優(yōu)化的GNSS/INS 組合導(dǎo)航流程圖Fig.3 Flowchart of GNSS/INS integrated navigation based on factor graph optimization

4 試驗(yàn)驗(yàn)證

4.1 仿真試驗(yàn)

本文基于MATLAB 平臺(tái)生成了一條仿真路線，路線方案如圖4 所示，其中包括加速、減速、拐彎、上坡、下坡等各種運(yùn)動(dòng)情景。各傳感器的更新頻率、零偏及觀測誤差如表1 所示。

圖4 仿真軌跡Fig.4 Simulation trajectory

表1 傳感器更新頻率及誤差Tab.1 Update frequencies and errors of the sensors

基于該仿真軌跡，本文通過將本文提出方法和EKF、OB-GINS 對比，從而對提出方法的實(shí)時(shí)狀態(tài)估計(jì)的性能進(jìn)行評(píng)估。圖優(yōu)化與EKF 之間存在以下不同，第一，圖優(yōu)化算法使用多次迭代來最小化成本，而EKF 對每個(gè)狀態(tài)只進(jìn)行一次迭代；第二，與EKF執(zhí)行的單一線性化不同，圖優(yōu)化在每個(gè)狀態(tài)的每個(gè)迭代步驟都對非線性測量模型進(jìn)行線性化；第三，由于圖優(yōu)化的批處理性質(zhì)，所以其能夠更好地利用過去和現(xiàn)在時(shí)期之間的時(shí)間相關(guān)性，特別是，在批處理模式下，圖優(yōu)化相當(dāng)于每次測量更新后的前向?yàn)V波器和后向平滑器[11,12]。OB-GINS 是將慣導(dǎo)數(shù)據(jù)、衛(wèi)星接收機(jī)的位置信息作為觀測量通過圖優(yōu)化算法進(jìn)行融合，其也采用了改進(jìn)的IMU 預(yù)積分模型，GD-GINS 相比OB-GINS 來說，增加了衛(wèi)星接收機(jī)的速度信息作為觀測量，并且具備衛(wèi)星粗差處理機(jī)制。

如圖5(a)所示，在位置估計(jì)方面，GD-GINS 與OB-GINS 算法的估計(jì)效果幾乎一致；在速度和姿態(tài)估計(jì)方面，由于GD-GINS 加入了GPS 速度信息，所以相較于OB-GINS 精度有明顯提升，如圖5(b)(c)所示。此外，GD-GINS 相比于EKF 來說，在位置、速度和姿態(tài)狀態(tài)量估計(jì)上均有小幅改善。

圖5 各方法導(dǎo)航誤差對比圖Fig.5 Comparison chart of navigation error of each method

4.2 實(shí)車數(shù)據(jù)仿真試驗(yàn)

為了進(jìn)一步評(píng)估所提出方法的魯棒性和精度，利用在城市道路采集的真實(shí)導(dǎo)航數(shù)據(jù)集，本文開展了仿真試驗(yàn)。試驗(yàn)車輛裝有IMU、GPS 信號(hào)接收儀和高精度激光陀螺慣導(dǎo)/GNSS/里程計(jì)組合導(dǎo)航系統(tǒng)（作為軌跡基準(zhǔn)）。各傳感器的更新頻率、零偏及觀測誤差如表1 所示。

試驗(yàn)全程總時(shí)長987 s，跑車測試軌跡如圖6所示。100～210 s 間緯度出現(xiàn)了峰值為600 m 的突變連續(xù)性粗差信號(hào)，200～400 s 間經(jīng)度出現(xiàn)了峰值為400 m 突變連續(xù)型粗差信號(hào)，持續(xù)時(shí)間在5 s 以內(nèi)；700～800 s 間出現(xiàn)衛(wèi)星拒止情況。

圖6 跑車測試軌跡Fig.6 Test trajectory of the car

圖7 為實(shí)時(shí)狀態(tài)估計(jì)的效果圖，在100-400 s 之間衛(wèi)星定位出現(xiàn)了粗差，在700-800 s 之間衛(wèi)星信號(hào)拒止。在100-400 s 之間，GD-GINS 的粗差檢測機(jī)制可有效降低衛(wèi)星粗差帶來的干擾。700-800 s 之間衛(wèi)星拒止時(shí)，三種算法均只有純慣導(dǎo)解算，因三者慣導(dǎo)解算方式不同，所以三者誤差積累有所不同。EKF 為捷聯(lián)導(dǎo)航算法，雖GD-GINS 和OB-GINS 同為預(yù)積分方式，但初始狀態(tài)不同。

圖7 實(shí)時(shí)狀態(tài)估計(jì)對比圖Fig.7 Comparison of state estimation under gross error interference

表2 所示，在衛(wèi)星存在粗差的時(shí)間段GD-GINS相比OB-GINS，在緯度和經(jīng)度方向上精度分別提升了97.92%和97.57%，相比EKF 在緯度和經(jīng)度方向上精度分別提升了91.38%和90.81%。驗(yàn)證了本文提出的粗差檢測窗口，能夠有效地減少因衛(wèi)星粗差造成的較大誤差波動(dòng)，實(shí)現(xiàn)狀態(tài)估計(jì)的魯棒性。

表2 粗差干擾下100-400 s 時(shí)間段位置估計(jì)誤差標(biāo)準(zhǔn)差/mTab.2 The standard deviation of position estimation errors（100-400 s）under gross interference

EKF 算法的估計(jì)結(jié)果是不可逆的，而圖優(yōu)化算法可在衛(wèi)星信號(hào)恢復(fù)后，對之前狀態(tài)再優(yōu)化，通過這種后處理手段，可極大提高衛(wèi)星拒止時(shí)的估計(jì)精度。本文對衛(wèi)星拒止時(shí)間段內(nèi)的狀態(tài)進(jìn)行后處理，如圖8 所示，GD-GINS 和OB-GINS 相對于EKF，估計(jì)更平滑，估計(jì)精度更高。

圖8 GD-GINS 與EKF 優(yōu)化效果比較Fig.8 Comparison of optimization effects between GD-GINS and EKF

經(jīng)過后處理，表3 給出了GD-GINS 和EKF 在衛(wèi)星中止階段的狀態(tài)估計(jì)誤差對比情況，該圖優(yōu)化算法在衛(wèi)星信號(hào)100 s 長時(shí)間拒止情況下，可通過后處理手段，使估計(jì)具有較強(qiáng)的魯棒性和平滑性。相比于EKF，GD-GINS 可將位置估計(jì)誤差降低30%以上。

表3 GD-GINS 與EKF 位置估計(jì)誤差的標(biāo)準(zhǔn)差統(tǒng)計(jì)Tab.3 The standard deviation statistics of position estimation error of GD-GINS and EKF

5 結(jié)論

本文針對城市復(fù)雜環(huán)境下的GNSS/INS 高精度魯棒組合導(dǎo)航問題，提出了一種優(yōu)化因子圖數(shù)據(jù)融合算法?；谛l(wèi)星信息關(guān)聯(lián)性的特性，設(shè)計(jì)了一種滑動(dòng)窗口粗差檢測算法，實(shí)現(xiàn)衛(wèi)星粗差檢測與融合預(yù)處理，抑制粗差對導(dǎo)航結(jié)果的影響；構(gòu)建考慮地球自轉(zhuǎn)項(xiàng)的IMU 預(yù)積分因子、衛(wèi)星位置、速度因子，提高了因子圖優(yōu)化模型精度。車載數(shù)據(jù)處理結(jié)果表明，GD-GINS的定位精度相比OB-GINS和EKF算法均有小幅提升；針對衛(wèi)星信號(hào)中存在粗差的情況，GD-GINS 的定位誤差精度相比OB-GINS 和EKF 算法提升90%以上,可以輔助導(dǎo)航系統(tǒng)獲得較好的狀態(tài)估計(jì)效果；針對GNSS拒止的情況，GD-GINS 的位置定位精度相比EKF 提升30%以上。