基于精化預積分的因子圖GNSS/INS松組合定位

楊顯賜,喬書波,肖國銳,彭華東,李松偉,徐海龍

(1.信息工程大學 地理空間信息學院,鄭州 450001:2.31121部隊, 南京 210001)

GNSS/INS組合導航在軍事、測繪領域一直都有廣泛應用。慣性導航系統(inertial navigation system,INS)通過測量載體的角運動和線運動信息,在初始姿態、速度和位置已知的情況下,根據力學編排方程,依次遞推出載體在各個時刻的姿態、速度和位置,具有自主性高、隱蔽性強、數據更新速率高[1],且不受外界環境干擾的優點,但由于加速度計和陀螺儀存在零偏等各種誤差,定位精度隨時間發散。全球導航衛星系統可以為用戶提供全天候、連續、實時、精確的三維坐標信息[2],但衛星信號容易受到外界環境干擾發生中斷。在GNSS/INS組合導航中,引入GNSS定位結果對INS進行校正,結合兩種導航系統的優點,可以有效提高組合導航系統的穩定性與定位精度。

在GNSS/INS組合導航中,擴展卡爾曼濾波(extended kalman filter,EKF)是一種理論成熟、應用廣泛的非線性濾波方法。文獻[3]通過仿真實驗驗證了基于EKF的組合導航能有效跟蹤載體軌跡的速度、姿態、位置,穩定性良好。文獻[4]在EKF的基礎上建立微機電系統(micro electro mechanical system,MEMS)INS/GNSS松組合(loosely coupled,LC)數學模型,可以明顯提高組合導航的定位精度和穩定性。但是,EKF假設了馬爾可夫性,認為當前時刻的狀態只與上一時刻有關,忽略了上一時刻之前的信息,是增量遞推過程,并且EKF的一次線性化不能做到近似整個函數,會導致精度損失。FGO利用非線性優化方法批量處理數據,求取一個滑動窗口內的最大后驗概率估計(maximum a posteriori,MAP),使FGO算法可以追溯歷史信息,獲取全局最優解。文獻[5]通過仿真實驗驗證了當多傳感器可用性改變和產生異步時延時,因子圖具有更好的容錯性與擴展性。文獻[6]通過仿真實驗對比分析FGO算法與EKF算法,實驗結果表明在模型非線性化特征明顯情況下,FGO算法迭代求解能夠減小誤差,定位精度更高,誤差曲線更加平滑。文獻[7]使用香港地區城市峽谷中的實測數據進行實驗分析,結果表明優化窗口的大小會影響因子圖信息融合性能,并且通過將FGO優化器退化為“類EKF估計器”,分析驗證FGO精度改進的原因之一是多次迭代。文獻[8]在IMU預積分過程中考慮地球自轉的影響,實驗證明對于工業級MEMS模塊,考慮地球自轉時可以有效提高松組合的定位性能。

因此,本文基于FGO算法進行GNSS/INS組合導航的狀態估計。考慮到因子圖算法在同時定位與建圖領域中使用的慣性測量單元(inertial measurement unit,IMU)精度較低,忽略了地球自轉的影響,進一步設計精化預積分(refined pre-integration,RP)提高組合導航定位精度。采集三組包含導航級、MEMS級別的慣性導航系統進行實驗驗證,GNSS數據為精密單點定位(precise point positioning,PPP)產品。

1 基于因子圖的GNSS/INS組合導航1.1 因子圖算法原理

因子圖是用來表達隨機變量聯合概率分布的二分圖模型H=(F,X,E)[9-10],由因子節點f∈F和變量節點x∈X組成,邊e∈E為存在于因子節點與變量節點之間的一條連接邊。在貝葉斯網絡中,所有變量聯合的概率密度是由每個節點關聯的條件概率密度相乘得到,類推到因子圖,將聯合概率密度表示為一系列因子的乘積,即:

(1)

隨機系統狀態空間模型,由一個運動方程和一個觀測方程構成,即:

(2)

(3)

將式(1)、式(3)聯合起來,即:

(4)

根據最大后驗概率估計,由量測信息計算狀態變量最優值,將式(4)轉化為一個等價的最小二乘優化問題,即:

XMAP=argmax(P(Xk∣Zk))=

(5)

1.2 IMU精化預積分因子構建

基于上節理論基礎,首先構建IMU預積分因子。在INS經典力學編排方程[11](mechanization equations,由慣性傳感器輸出求解導航參數的一套計算方案)中,姿態、速度、位置更新是逐歷元進行的,在組合導航時,GNSS的采樣頻率遠小于IMU,為了提高計算效率,使用IMU預積分理論對INS進行狀態更新,預積分的思想常用在同時定位與建圖領域中,但INS與相機等數據融合時,IMU的精度較低,沒有考慮地球自轉的影響,在GNSS/INS組合導航中,IMU陀螺儀的零偏穩定性小于或遠小于地球自轉角速率,容易受到地球自轉的影響,因此本文進一步設計了精化預積分。

首先構建IMU測量模型:

(6)

根據INS運動學模型,可以得到位置、速度、姿態對時間的微分方程:

(7)

(8)

(9)

將式(9)的離散形式表示為:

(10)

其中:

這兩項體現出考慮地球自轉時對速度、位置的修正。

由式(9)減去式(10),就得到了IMU精化預積分的殘差向量,由此構建IMU精化預積分因子:

(11)

1.3 GNSS PPP因子構建

(12)

式中:l為b系下IMU的幾何中心相對于GNSS接收機天線相位中心的空間桿臂向量。

1.4 因子圖模型遞推方法

基于1.2、1.3節,構建IMU精化預積分因子和GNSS PPP因子,將對應的因子節點添加至因子圖模型中,當接收到GNSS、IMU的測量信息時,更新因子圖框架,進行變量節點更新。GNSS/INS組合導航因子圖框架如圖1所示,IMU精化預積分因子連接相鄰兩時刻的狀態變量,各個時刻的GNSS PPP因子通過連接邊與對應的狀態變量相連接。

圖1 GNSS/INS松組合因子圖框架

圖1中,先驗因子包括運載體的初始位置等,定義為均值ux、方差∑x的高斯分布模型,即:

fprior(x)=x-ux.

(13)

綜上,得到狀態變量的最大后驗概率估計:

(14)

式中:WZ表示優化窗口內預積分因子個數;M表示優化窗口內PPP因子個數;當GNSS未發生中斷時,WZ=M。

當使用非線性優化方法批量處理數據時,觀測值以一個時間序列到達,如果每次都進行完整的批量優化會導致計算效率低,無法保證計算的實時性,通過使用增量非線性最小二乘估計算法iSAM2[12],當新的因子添加進來時,只優化被新的因子影響的那一部分,從而提高解算效率。

2 基于FGO與EKF的融合過程對比分析

目前大多數導航系統狀態估計的理論基礎是卡爾曼濾波算法,在衛星導航、組合導航、多源數據融合中均有廣泛應用[14]。松組合情況下,INS根據IMU輸出信息解算出運載體的位置、速度、姿態,與PPP獲取的定位產品在EKF濾波器中進行信息融合。GNSS/INS組合導航擴展卡爾曼濾波框架如圖2所示。

圖2 GNSS/INS組合導航擴展卡爾曼濾波框架

EKF通過預測、更新兩個步驟,可以較為簡潔地實現組合導航信息融合,在計算資源受限、待估量簡單的情況下,是一種有效的信息融合手段,但其假設的馬爾可夫性無法追溯歷史信息,也存在線性化不足的缺點。

因子圖使用非線性優化方法[13-15],可以獲得全局最優解,充分利用一段時間內的全部信息,并通過高斯牛頓法或列文伯格-馬夸爾特法做迭代優化,當迭代一次狀態估計發生改變后,重新對新的估計點進行泰勒展開,而不是像EKF只在固定點做一次泰勒展開,這就使得FGO的優化范圍更廣、效果更佳。因此本文認為,相較于EKF,FGO的精度和可靠性都會更優。

3 車載實測實驗與性能分析

車載實測實驗利用加拿大諾瓦泰公司SPAN-ISA-100C型高精度光纖慣導、邁普時空研發的POS320光纖慣導和MP-M39型MEMS慣性測量單元進行慣導數據采集。如圖3所示,將三組慣導和一臺GNSS接收機安裝在一個剛性平臺上,通過跑車試驗獲取三組GNSS/INS組合導航數據。圖4為組合導航實驗軌跡圖,實驗時長約1.8 h,環境開闊。三組IMU的采樣頻率均為200 Hz,其標定參數如表1所示,表中最后一列數據為北東地坐標系下三組慣導的空間桿臂臂長,數據處理中GNSS定位結果為PPP高精度位置產品,GNSS的數據更新頻率為1 Hz,經度、緯度、高程測量噪聲的標準差為0.052 m、0.049 m、0.106 m,IMU預積分的頻率為1 Hz。處理設備為ThinkPad P15v,FGO算法的實驗環境為ubuntu20.04,編程語言為C++,EKF算法的實驗環境為Window10,編程語言為Matlab。

表1 因子圖在6種窗口大小下松組合位置誤差

圖3 GNSS/INS組合導航車載實驗

圖4 GNSS/INS組合導航實測軌跡

3.1 因子圖定位性能分析

為驗證因子圖、IMU精化預積分的可行性和有效性,對因子圖的定位性能進行實驗分析。FGO作為一種批量處理數據的算法,需要有效控制解算規模,保證計算的實時性,通過滑動窗口將FGO固定在一個時間窗口內,隨歷元向前推進,不計算窗口之外的數據,獲取全局最優解,在保證計算效率的同時得到最理想的定位結果。本文設置窗口大小為1 s、5 s、10 s、20 s、30 s、60 s,分析不同窗口下的定位結果得到最優的開窗大小,并與濾波解算的時間做對比,分析因子圖在不同開窗大小下的時間效率,表1、表2分別為6種窗口大小下三維位置的均方根誤差(root mean squared error,RMSE)和運行時間。

表2 因子圖在6種窗口大小下松組合運行時間 s

表3 慣性導航元件標定參數及空間杠桿桿臂

窗口增大時將會增加解算規模,加重非線性優化迭代的負擔。由表2可得,隨滑動窗口變大,程序運行時間增加,結合表1可得,在窗口大小為20 s時,三維位置誤差均優于其他方向或呈基本相當的狀態。因此,結合定位精度與計算效率,本文認為在窗口大小為20 s時,FGO的導航定位結果最優。下文均以窗口大小20 s的情況開展相關實驗。

其次,分析精化預積分對因子圖定位性能的影響。精化預積分主要考慮地球自轉對陀螺的影響,通過表1數據可得,實驗所采用三組慣導元件陀螺儀的零偏穩定性均小于地球自轉角速率數值為15.041 067 178 6 °/h,即在其工作過程中均會受到地球自轉的影響,且當陀螺的零偏穩定性越高時,越容易受到其干擾。表4為地球自轉項改正前后松組合位置誤差,位置誤差曲線如圖5所示。下文圖表中將SPAN-ISA-100C簡稱為100C,將MP-M39簡稱為M39。

表4 地球自轉項改正前后松組合位置誤差 m

圖5 地球自轉項改正前后松組合位置誤差曲線

由表4分析,SPAN-ISA-100C、POS320使用精化預積分時組合導航定位精度在北、東、地三個方向上分別提升了56.13%、93.7%、39.7%和16.5%、68.9%、42.3%,但對于MP-M39,由于其陀螺儀的零偏穩定性較大,地球自轉的影響有限,在北方向、東方向分別提升了8%、7.9%,地方向持相當狀態。由此可見,對于導航級慣導,地球自轉的影響不可忽略,對于戰術級慣導,精化預積分對GNSS/INS的定位精度也有一定程度的提升。

3.2 FGO與EKF定位性能比較

通過3.1實驗分析可得,在窗口大小為20 s時,基于精化預積分的因子圖GNSS/INS松組合定位結果最優。在組合導航領域,EKF形式簡潔、應用廣泛,是一種成熟的用來估計不確定量的方法。將FGO與EKF進行對比,進一步驗證FGO的可行性與有效性。表5為三組慣導在FGO和EKF下松組合的均方根位置誤差,位置誤差曲線如圖6所示。

表5 FGO和EKF松組合位置誤差 m

圖6 GNSS/INS FGO和EKF松組合位置誤差曲線

由圖6可得,FGO相較于EKF的誤差曲線更加平滑,由表5分析三組慣導松組合時的RMSE值可得,FGO相對于EKF,SPAN-ISA-100C、POS320、MP-M39的定位精度在北、東、地三個方向上分別提升了53.03%、52.35%、64.59%,60.14%、22.62%、50.06%,22.2%、40%、67.16%。

針對于圖5(a)與圖6(a)出現的誤差偏移現象,結合圖5(b)與圖6(b)、圖5(c)與圖6(c)可得,在5 600s左右高程位置均發生了抖動,分析是因為當時車載路況原因,導致平臺發生了較大抖動,一方面由于光纖級慣導SPAN-ISA-100C精度高,對細微變化感受敏感,一方面由于SPAN-ISA-100C與平臺發生微小松動,導致高程方向發生位置誤差偏移。

為了更好驗證兩種算法的適用性,本文選取一段數據,每間隔120 s,設置30 s、60 s一次的GNSS中斷,共計13次,表6為三組慣導松組合時在三維位置上的最大誤差,表7為中斷時段姿態三維均方根誤差,圖7為三組慣導松組合GNSS中斷60s時在三維位置上的最大誤差曲線圖,圖8為GNSS中斷60 s時姿態誤差曲線圖。

表6 松組合時EKF、FGO三維位置最大誤差 m

表7 松組合時EKF、FGO姿態誤差 (°)

圖7 GNSS中斷60 s時EKF和FGO位置誤差曲線

圖8 GNSS中斷60 s時EKF和FGO姿態誤差曲線

由式(5)分析可得,圖優化算法求取的是全局最優解,因此在發生GNSS信號中斷時,優化區域通過與前一時段GNSS信號正常時的定位信息做全局最優化,即圖優化算法在GNSS信號恢復后,對GNSS信號失鎖時間段內的狀態進行再優化,形成一個整體的最優結果,可以極大提高GNSS發生信號失鎖時導航定位精度,與EKF狀態遞推方法相比可以更有效地減小組合導航位置發散,其次圖優化算法采用非線性優化方法,優化時進行了多次迭代,與EKF只進行一階泰勒展開相比,多次迭代也可以有效減小誤差。如圖7、圖8所示,當發生GNSS中斷時,FGO的定位效果要明顯優于EKF,位置誤差發散慢,姿態誤差也更加穩定。三組實驗位置最大誤差如表6所示,分析可得,對于導航級慣導,使用FGO算法時,SPAN-ISA-100C和POS320在GNSS中斷30 s時最大誤差均在1.3 m以內,中斷60 s時最大誤差均在4.7 m以內,使用EKF算法時,SPAN-ISA-100C和POS320在GNSS中斷30 s時最大誤差達到7.11 m,中斷60 s時最大誤差達到40.5 m,地方向誤差發散較慢,但FGO的定位效果也優于EKF;對于戰術級慣導,由于其本身陀螺儀的零偏穩定性較差,當發生GNSS信號中斷時,位置誤差發散快,GNSS中斷時段較快的誤差發散會影響優化區間整體的精度,因此MP-M39在與GNSS松組合時定位誤差較大,但在GNSS中斷30 s和60 s時,FGO的定位效果也均優于EKF。

4 結 論

本文針對EKF狀態遞推過程無法追溯歷史信息以及線性化不足所帶來的精度損失問題,采用基于精化預積分的因子圖算法進行GNSS/INS松組合定位,通過車載實測實驗進行分析得到如下結論:

1)研究了因子圖的定位性能。通過實驗分析可得,在兼顧定位精度和計算效率的情況下,將窗口大小設置為20 s時,基于精化預積分的因子圖算法定位效果最優。

2)與EKF相比,FGO算法求取的是全局最優解,因此可以獲得更優的定位結果。

3)當發生GNSS中斷時,由于FGO算法可以追溯歷史信息,因此可以極大的抑制位置誤差發散。