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

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

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

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

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

1 基于因子圖的GNSS/INS組合導(dǎo)航1.1 因子圖算法原理

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

(1)

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

(2)

(3)

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

(4)

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

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

(5)

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

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

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

(6)

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

(7)

(8)

(9)

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

(10)

其中:

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

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

(11)

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

(12)

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

1.4 因子圖模型遞推方法

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

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

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

fprior(x)=x-ux.

(13)

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

(14)

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

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

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

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

圖2 GNSS/INS組合導(dǎo)航擴(kuò)展卡爾曼濾波框架

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

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

3 車載實(shí)測實(shí)驗(yàn)與性能分析

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

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

圖3 GNSS/INS組合導(dǎo)航車載實(shí)驗(yàn)

圖4 GNSS/INS組合導(dǎo)航實(shí)測軌跡

3.1 因子圖定位性能分析

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

表2 因子圖在6種窗口大小下松組合運(yùn)行時(shí)間 s

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

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

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

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

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

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

3.2 FGO與EKF定位性能比較

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

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

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

由圖6可得,FGO相較于EKF的誤差曲線更加平滑,由表5分析三組慣導(dǎo)松組合時(shí)的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ā)生了抖動,分析是因?yàn)楫?dāng)時(shí)車載路況原因,導(dǎo)致平臺發(fā)生了較大抖動,一方面由于光纖級慣導(dǎo)SPAN-ISA-100C精度高,對細(xì)微變化感受敏感,一方面由于SPAN-ISA-100C與平臺發(fā)生微小松動,導(dǎo)致高程方向發(fā)生位置誤差偏移。

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

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

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

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

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

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

4 結(jié) 論

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

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

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

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