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

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

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

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

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

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

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

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

(1)

隨機系統(tǒng)狀態(tài)空間模型,由一個運動方程和一個觀測方程構(gòu)成,即:

(2)

(3)

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

(4)

根據(jù)最大后驗概率估計,由量測信息計算狀態(tài)變量最優(yōu)值,將式(4)轉(zhuǎn)化為一個等價的最小二乘優(yōu)化問題,即:

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

(5)

1.2 IMU精化預積分因子構(gòu)建

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

首先構(gòu)建IMU測量模型:

(6)

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

(7)

(8)

(9)

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

(10)

其中:

這兩項體現(xiàn)出考慮地球自轉(zhuǎn)時對速度、位置的修正。

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

(11)

1.3 GNSS PPP因子構(gòu)建

(12)

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

1.4 因子圖模型遞推方法

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

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

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

fprior(x)=x-ux.

(13)

綜上,得到狀態(tài)變量的最大后驗概率估計:

(14)

式中:WZ表示優(yōu)化窗口內(nèi)預積分因子個數(shù);M表示優(yōu)化窗口內(nèi)PPP因子個數(shù);當GNSS未發(fā)生中斷時,WZ=M。

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

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

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

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

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

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

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

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

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

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

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

3.1 因子圖定位性能分析

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

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

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

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

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

表4 地球自轉(zhuǎn)項改正前后松組合位置誤差 m

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

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

3.2 FGO與EKF定位性能比較

通過3.1實驗分析可得,在窗口大小為20 s時,基于精化預積分的因子圖GNSS/INS松組合定位結(jié)果最優(yōu)。在組合導航領(lǐng)域,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)出現(xiàn)的誤差偏移現(xiàn)象,結(jié)合圖5(b)與圖6(b)、圖5(c)與圖6(c)可得,在5 600s左右高程位置均發(fā)生了抖動,分析是因為當時車載路況原因,導致平臺發(fā)生了較大抖動,一方面由于光纖級慣導SPAN-ISA-100C精度高,對細微變化感受敏感,一方面由于SPAN-ISA-100C與平臺發(fā)生微小松動,導致高程方向發(fā)生位置誤差偏移。

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

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

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

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

圖8 GNSS中斷60 s時EKF和FGO姿態(tài)誤差曲線

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

4 結(jié) 論

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

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

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

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