基于因子圖的車(chē)載INS/GNSS/OD組合導(dǎo)航算法

高軍強(qiáng), 湯霞清, 張 環(huán), 武 萌

(陸軍裝甲兵學(xué)院兵器與控制系, 北京 100072)

0 引 言

隨著導(dǎo)航技術(shù)的不斷發(fā)展,多傳感器組合導(dǎo)航已經(jīng)逐漸成為各種復(fù)雜環(huán)境下長(zhǎng)時(shí)間高精度導(dǎo)航的主要方式[1-2]。在陸用車(chē)載導(dǎo)航系統(tǒng)中,慣性導(dǎo)航系統(tǒng)(inertial navigation system,INS)、全球?qū)Ш叫l(wèi)星系統(tǒng)(global navigation satellite system,GNSS)、里程計(jì)(odometer,OD)構(gòu)成的組合導(dǎo)航系統(tǒng)被廣泛應(yīng)用,其中OD可以在GNSS信號(hào)因遮擋、多路徑效應(yīng)等因素導(dǎo)致不可用時(shí),有效減緩INS的誤差積累,從而實(shí)現(xiàn)車(chē)輛的長(zhǎng)時(shí)間高精度穩(wěn)定導(dǎo)航。

目前,INS/GNSS/OD組合導(dǎo)航算法大多以卡爾曼濾波為核心,為提高導(dǎo)航精度及穩(wěn)定性進(jìn)行了很多方面的改進(jìn)。但在處理傳感器異步及時(shí)延問(wèn)題時(shí),卡爾曼濾波需要采用外推內(nèi)插的方法將量測(cè)統(tǒng)一到同一時(shí)刻,顯然會(huì)引入人為誤差,并增加計(jì)算負(fù)擔(dān)。此外,卡爾曼濾波必須要建立組合導(dǎo)航系統(tǒng)的狀態(tài)空間模型,當(dāng)組合導(dǎo)航系統(tǒng)中的傳感器有效性改變時(shí),其狀態(tài)空間模型就需要進(jìn)行相應(yīng)的調(diào)整,這無(wú)疑增加了算法設(shè)計(jì)的復(fù)雜性[3-5],也一定程度限制了使用更多有效傳感器信息提高導(dǎo)航性能的可能性。

因子圖是概率圖模型的一種,其對(duì)導(dǎo)航狀態(tài)量與導(dǎo)航系統(tǒng)測(cè)量值之間的概率關(guān)系進(jìn)行編碼,將狀態(tài)量定義為變量節(jié)點(diǎn),各傳感器測(cè)量值定義為因子節(jié)點(diǎn),多傳感器組合導(dǎo)航便轉(zhuǎn)換為在因子圖中相應(yīng)的變量節(jié)點(diǎn)上連接這些因子節(jié)點(diǎn)[6-7]。因子圖模型具有很強(qiáng)的靈活性,能夠?qū)崿F(xiàn)傳感器的即插即用,已經(jīng)在導(dǎo)航領(lǐng)域受到廣泛關(guān)注[8-10]。

本文將基于因子圖的組合導(dǎo)航算法應(yīng)用于車(chē)載INS/GNSS/OD系統(tǒng)中,構(gòu)建了組合導(dǎo)航系統(tǒng)因子圖模型,推導(dǎo)了因子圖增量推理方法,并對(duì)其在處理傳感器有效性動(dòng)態(tài)改變、傳感器存在異步和時(shí)延等方面的性能進(jìn)行了深入研究。

1 組合導(dǎo)航系統(tǒng)因子圖模型

以INS因子圖模型為主體,當(dāng)GNSS、OD等傳感器測(cè)量信息有效時(shí),將與之對(duì)應(yīng)的因子節(jié)點(diǎn)連接進(jìn)INS因子圖模型中,構(gòu)成組合導(dǎo)航系統(tǒng)因子圖模型。

1.1 INS因子圖模型

INS性能極大地依賴(lài)于慣性測(cè)量單元(inertial measurement unit,IMU)對(duì)載體運(yùn)動(dòng)狀態(tài)的高頻測(cè)量,該頻率一般遠(yuǎn)高于導(dǎo)航系統(tǒng)的輸出頻率,即相對(duì)于導(dǎo)航系統(tǒng)輸出而言,INS測(cè)量數(shù)據(jù)存在大量冗余信息。因此,在建立INS因子圖模型時(shí),可以采用智能因子的思想[11],僅將需要輸出的狀態(tài)量添加為變量節(jié)點(diǎn),相鄰兩變量節(jié)點(diǎn)對(duì)應(yīng)時(shí)刻之間的IMU測(cè)量數(shù)據(jù)進(jìn)行綜合,定義智能因子節(jié)點(diǎn),從而降低因子節(jié)點(diǎn)的更新頻率,提高算法實(shí)時(shí)性。

圖1所示為INS因子圖模型構(gòu)建方案。根據(jù)實(shí)際系統(tǒng)應(yīng)用需求確定導(dǎo)航狀態(tài)量輸出頻率,進(jìn)而決定智能因子更新周期,并將需要輸出的導(dǎo)航狀態(tài)量定義為目標(biāo)變量,在因子圖中添加變量節(jié)點(diǎn)。為了對(duì)IMU中慣性傳感器測(cè)量誤差進(jìn)行修正,將慣性傳感器偏差變量也添加到變量節(jié)點(diǎn)中,并根據(jù)偏差傳播函數(shù)定義偏差因子節(jié)點(diǎn)。對(duì)一個(gè)更新周期內(nèi)的IMU測(cè)量數(shù)據(jù),采用預(yù)積分和INS解算分別建立預(yù)積分因子節(jié)點(diǎn)和解算結(jié)果因子節(jié)點(diǎn),連接到因子圖中對(duì)應(yīng)的變量節(jié)點(diǎn)上。

圖1 INS因子圖模型構(gòu)建方案Fig.1 Construct scheme of INS factor graph model

INS通過(guò)對(duì)加速度、角速度測(cè)量值進(jìn)行積分,得到載體的位置、速度、姿態(tài)信息,是一種航位推算系統(tǒng)。在建立智能因子節(jié)點(diǎn)時(shí),可以采用預(yù)積分方法對(duì)一個(gè)更新周期內(nèi)的IMU測(cè)量數(shù)據(jù)進(jìn)行綜合。組合導(dǎo)航系統(tǒng)中,INS初始對(duì)準(zhǔn)可以在其他導(dǎo)航方式的輔助下快速完成,以獲得載體的初始位置、速度、姿態(tài)信息。然后,將加速度、角速度測(cè)量值直接在載體系下進(jìn)行預(yù)積分,可以極大地降低計(jì)算量,提高算法實(shí)時(shí)性[12]。

設(shè)導(dǎo)航狀態(tài)量輸出時(shí)刻為tk,k=0,1,…,則智能因子更新周期T=tk+1-tk。對(duì)一個(gè)更新周期tk～tk+1內(nèi)的IMU測(cè)量數(shù)據(jù)在tk時(shí)刻載體系下進(jìn)行預(yù)積分,得到位置、速度、姿態(tài)增量的計(jì)算公式為

(1)

根據(jù)INS航位推算的原理,可進(jìn)一步得到tk+1時(shí)刻載體位置、速度、姿態(tài)為

(2)

式中,gn表示重力矢量。

式(2)即為INS系統(tǒng)狀態(tài)方程離散形式,可以統(tǒng)一表示為

,αk,zk)

(3)

,xk,αk)d(xk+1-h(xk,αk,zk))

(4)

在運(yùn)動(dòng)過(guò)程中,載體姿態(tài)是時(shí)刻變化的,而一個(gè)智能因子更新周期內(nèi)的IMU測(cè)量數(shù)據(jù)全部在tk時(shí)刻載體系下進(jìn)行預(yù)積分,實(shí)際上是一種簡(jiǎn)化處理方式,尤其當(dāng)載體姿態(tài)變化較快時(shí)會(huì)產(chǎn)生比較大的誤差。鑒于此,在進(jìn)行預(yù)積分的同時(shí),采用經(jīng)典INS處理方式對(duì)一個(gè)更新周期內(nèi)的IMU測(cè)量數(shù)據(jù)進(jìn)行解算,解算方法[4]不再贅述。在對(duì)tk～tk+1內(nèi)的IMU測(cè)量數(shù)據(jù)進(jìn)行解算時(shí),tk時(shí)刻的位置、速度、姿態(tài)可以由組合導(dǎo)航的結(jié)果中得到,每次解算時(shí)間長(zhǎng)度僅為一個(gè)更新周期,從而避免出現(xiàn)過(guò)大的誤差積累,保證了解算結(jié)果的精度。

(5)

慣性器件偏差隨時(shí)間不斷變化,假設(shè)其轉(zhuǎn)移函數(shù)為

αk)

(6)

則偏差因子節(jié)點(diǎn)定義為

αk+1,αk)d(αk+1-g(αk))

(7)

圖2 INS因子圖模型Fig.2 Factor graph model of INS

1.2 GNSS因子節(jié)點(diǎn)

GNSS可以提供載體的位置及速度信息,其測(cè)量方程表示為

(8)

(9)

1.3 OD因子節(jié)點(diǎn)

令nOD表示OD測(cè)量噪聲,則OD測(cè)量方程可以表示為

(10)

從而得到OD因子節(jié)點(diǎn)為

(11)

1.4 INS/GNSS/OD因子圖模型

當(dāng)GNSS、OD測(cè)量頻率與系統(tǒng)輸出頻率相同且完全同步時(shí),每一個(gè)導(dǎo)航狀態(tài)量輸出時(shí)刻都存在GNSS和OD測(cè)量值,將GNSS因子節(jié)點(diǎn)、OD因子節(jié)點(diǎn)連接到INS因子圖模型中,即可得到INS/GNSS/OD因子圖模型,如圖3(a)所示。

圖3 INS/GNSS/OD因子圖模型Fig.3 Factor graph model of INS/GNSS/OD

當(dāng)GNSS、OD測(cè)量頻率與系統(tǒng)輸出頻率不同,以及存在時(shí)延時(shí),只需要將tk時(shí)刻接收到的GNSS或OD測(cè)量值定義為因子節(jié)點(diǎn),連接到INS因子圖模型中。圖3(b)所示INS/GNSS/OD因子圖模型便是由于GNSS、OD測(cè)量頻率低于系統(tǒng)輸出頻率且存在時(shí)延得到的。

上述構(gòu)建組合導(dǎo)航系統(tǒng)因子圖模型的過(guò)程表明,組合導(dǎo)航系統(tǒng)中各傳感器的因子節(jié)點(diǎn)在因子圖模型中是相互獨(dú)立的、互不影響的。以GNSS的有效性改變?yōu)槔?當(dāng)GNSS突然失效時(shí),只需要抑制GNSS因子節(jié)點(diǎn)的繼續(xù)添加,便轉(zhuǎn)換為INS/OD因子圖模型;而當(dāng)GNSS變?yōu)橛行r(shí),只需要重新添加GNSS因子節(jié)點(diǎn),便恢復(fù)為INS/GNSS/OD因子圖模型。說(shuō)明采用因子圖模型描述組合導(dǎo)航問(wèn)題可以靈活地處理傳感器有效性的動(dòng)態(tài)改變,基于因子圖的組合導(dǎo)航算法具有很強(qiáng)的擴(kuò)展性和靈活性。

2 組合導(dǎo)航因子圖模型推理方法

因子圖模型將組合導(dǎo)航中對(duì)狀態(tài)量極大后驗(yàn)估計(jì)值的求解轉(zhuǎn)化為了對(duì)因子圖的推理。因子圖推理的基本算法是和積算法[13],在實(shí)際工程應(yīng)用中各種精確推理或近似推理方法被提出,并取得了較好的效果[14]?？紤]到導(dǎo)航系統(tǒng)需要具備很強(qiáng)的實(shí)時(shí)性,而組合導(dǎo)航因子圖模型又是一個(gè)隨時(shí)間不斷遞增的動(dòng)態(tài)圖模型,故宜采用增量推理技術(shù)[15]。

2.1 因子圖算法

因子圖模型中,變量節(jié)點(diǎn)集合表示為X;因子節(jié)點(diǎn)集合表示為F;連接節(jié)點(diǎn)的所有邊構(gòu)成集合E,則因子圖可以表示為

G=(F,X,E)

(12)

根據(jù)因子圖理論,因子圖G描述了對(duì)函數(shù)f(X)的因式分解,表示為

(13)

式中,Xi表示與因子節(jié)點(diǎn)fi相連的變量節(jié)點(diǎn)集合,Xi?X。

(14)

(15)

因此,對(duì)式(13)取極大值等價(jià)于最小二乘問(wèn)題,即

(16)

2.2 增量推理方法

因子圖模型隨時(shí)間的不斷遞增直觀的表現(xiàn)為X中的變量越來(lái)越多,如果每一次進(jìn)行因子圖推理都對(duì)X中的所有變量重新求解最優(yōu)估計(jì)值,顯然是無(wú)法滿足系統(tǒng)實(shí)時(shí)性需求的。下面以圖2所示因子圖模型為例詳述因子圖模型增量推理方法。

圖2(a)所示t1時(shí)刻因子圖模型對(duì)應(yīng)的雅克比矩陣為Jt1。采用變量消除算法[16]按照x0→α0→x1→α1的順序進(jìn)行消除,得到貝葉斯網(wǎng)絡(luò)如圖4(a)所示,對(duì)應(yīng)的平方根信息矩陣是對(duì)Jt1進(jìn)行QR分解得到的上三角矩陣Rt1。由于通過(guò)消除算法得到的貝葉斯網(wǎng)絡(luò)是弦圖,因此可以將其表示為貝葉斯樹(shù)的形式,如圖4(b)所示。

x0α0x1α1x0α0x1α1

(17)

式中,×表示矩陣中的非零元素。

圖4 貝葉斯網(wǎng)絡(luò)和貝葉斯樹(shù)Fig.4 Bayes net and Bayes tree

對(duì)比發(fā)現(xiàn),加入新的節(jié)點(diǎn),僅對(duì)部分狀態(tài)變量的估計(jì)產(chǎn)生影響,因此在當(dāng)前時(shí)刻的推理中只需要對(duì)受到影響的部分狀態(tài)變量及新添加的狀態(tài)變量進(jìn)行估計(jì)。

(18)

當(dāng)因子圖中加入新的因子節(jié)點(diǎn)時(shí),首先根據(jù)新因子節(jié)點(diǎn)中包含的狀態(tài)量確定貝葉斯樹(shù)中受影響的部分以及涉及到的狀態(tài)量,原因子圖中包含這些狀態(tài)量的部分與新因子節(jié)點(diǎn)構(gòu)成新的因子圖,然后轉(zhuǎn)化為新的貝葉斯樹(shù),再與未受影響的部分相接,從而實(shí)現(xiàn)貝葉斯樹(shù)的增量更新。

3 仿真分析

在Matlab環(huán)境下,進(jìn)行仿真分析,仿真條件設(shè)置如下:

(1)仿真軌跡起始位置為緯度39°、經(jīng)度116°、海拔35 m,初始速度為0,航向正北,經(jīng)加速、轉(zhuǎn)彎、爬坡、下坡、減速等動(dòng)作后到達(dá)終點(diǎn)為緯度39.02°,經(jīng)度115.85°,海拔48 m,途中最大速度為20 m/s,最高海拔為230 m,總行駛時(shí)間為1 800 s；

(2) INS中陀螺的常值漂移為0.02 (°)/h、隨機(jī)漂移為0.01 (°)/h,加速度計(jì)的常值零偏為100 μg,隨機(jī)零偏為50 μg；

(3)GNSS的經(jīng)度、緯度測(cè)量噪聲均方差為1 m,高度測(cè)量噪聲均方差為3 m,3個(gè)方向速度測(cè)量噪聲均方差都為0.1 m/s；

(4)OD輸出脈沖經(jīng)刻度因子轉(zhuǎn)化為速度,噪聲均方差為0.2 m/s；

(5)INS采樣頻率為100 Hz,GNSS更新頻率為1 Hz,OD輸出頻率為1 Hz。

3.1 傳感器有效性動(dòng)態(tài)改變仿真

假定系統(tǒng)輸出頻率與GNSS、OD測(cè)量頻率相同且完全同步,仿真過(guò)程中設(shè)置部分時(shí)段GNSS數(shù)據(jù)不可用,檢驗(yàn)因子圖算法處理傳感器有效性動(dòng)態(tài)改變的能力。3次仿真實(shí)驗(yàn)的具體設(shè)置如下:

實(shí)驗(yàn)1GNSS數(shù)據(jù)始終可用;

實(shí)驗(yàn)2GNSS信號(hào)在600～1 200 s不可用,INS單獨(dú)導(dǎo)航;

實(shí)驗(yàn)3GNSS信號(hào)在600～1 200 s不可用,采用OD輔助INS進(jìn)行導(dǎo)航。

圖5(a)、圖5(b)、圖5(c)所示分別為3次仿真實(shí)驗(yàn)得到的經(jīng)度、緯度、高度誤差曲線。結(jié)果表明,實(shí)驗(yàn)1得到的位置誤差最小,實(shí)驗(yàn)3次之,實(shí)驗(yàn)2誤差最大。

圖5 實(shí)驗(yàn)1、2、3位置誤差曲線對(duì)比Fig.5 Comparison of position error curves of test 1, 2, and 3

實(shí)驗(yàn)1中,GNSS數(shù)據(jù)始終可用,組合導(dǎo)航系統(tǒng)一直處于INS/GNSS工作模式,達(dá)到了很高的位置精度。

實(shí)驗(yàn)2中,GNSS信號(hào)在600～1 200 s不可用,組合導(dǎo)航系統(tǒng)中傳感器有效性先后發(fā)生了兩次改變。當(dāng)GNSS信號(hào)在600 s失效時(shí),組合導(dǎo)航系統(tǒng)由INS/GNSS工作模式切換為INS單獨(dú)導(dǎo)航模式,因子圖模型中抑制了因子節(jié)點(diǎn)fpGNSS和fvGNSS的添加,位置誤差由于INS得不到修正而迅速積累。當(dāng)GNSS信號(hào)在1 200 s重新有效后,組合導(dǎo)航系統(tǒng)由INS單獨(dú)導(dǎo)航模式切換為INS/GNSS工作模式,因子節(jié)點(diǎn)fpGNSS和fvGNSS又被添加進(jìn)了因子圖中,位置誤差得到修正。

實(shí)驗(yàn)3中,組合導(dǎo)航系統(tǒng)中傳感器有效性也發(fā)生了兩次改變。當(dāng)GNSS信號(hào)在600 s失效時(shí),在抑制因子節(jié)點(diǎn)fpGNSS和fvGNSS的添加的同時(shí),加入了因子節(jié)點(diǎn)fOD,系統(tǒng)由INS/GNSS工作模式切換為INS/OD工作模式。當(dāng)GNSS信號(hào)在1 200 s時(shí)重新有效后,因子節(jié)點(diǎn)fpGNSS和fvGNSS又被添加進(jìn)了因子圖中,同時(shí)抑制因子節(jié)點(diǎn)fOD的添加,系統(tǒng)由INS/OD工作模式恢復(fù)為INS/GNSS工作模式,INS誤差被迅速修正。相較于實(shí)驗(yàn)2,當(dāng)GNSS信號(hào)不可用時(shí),OD的引入有效緩解了INS的誤差積累速度。

根據(jù)以上分析,INS/GNSS/OD組合導(dǎo)航系統(tǒng)有INS/GNSS、INS、INS/OD 3種工作模式。當(dāng)系統(tǒng)由于傳感器有效性改變而在這3種工作模式之間切換時(shí),只需要在因子圖模型中抑制失效傳感器的因子節(jié)點(diǎn),或接入新的有效傳感器的因子節(jié)點(diǎn),不需要對(duì)因子圖模型進(jìn)行更多的調(diào)整,從而保證了組合導(dǎo)航系統(tǒng)在各種工作模式下的無(wú)縫切換,使得系統(tǒng)具有很強(qiáng)的魯棒性。

3.2 傳感器異步和時(shí)延仿真

假定INS/GNSS/OD組合導(dǎo)航系統(tǒng)中所有傳感器的測(cè)量信息均穩(wěn)定有效,設(shè)置GNSS、OD的測(cè)量頻率與組合導(dǎo)航系統(tǒng)輸出頻率存在異步和時(shí)延的情況,檢驗(yàn)因子圖算法處理傳感器異步和時(shí)延的能力。3次仿真實(shí)驗(yàn)具體設(shè)置如下:

實(shí)驗(yàn)4無(wú)異步、無(wú)時(shí)延;

實(shí)驗(yàn)5有異步、無(wú)時(shí)延;

實(shí)驗(yàn)6有異步、有時(shí)延。

其中,“有異步”是指系統(tǒng)輸出頻率為5 Hz,而GNSS、OD測(cè)量頻率為1 Hz;“有時(shí)延”是指GNSS與OD數(shù)據(jù)更新時(shí)刻相對(duì)于系統(tǒng)輸出整點(diǎn)時(shí)刻分別存在0.2 s和0.6 s的時(shí)延,如圖6所示。

圖6 傳感器異步及時(shí)延Fig.6 Asynchronization and time delay of the sensors

圖7所示為3次仿真實(shí)驗(yàn)得到的位置誤差曲線,表1所示為位置誤差均方根(root mean squared,RMS)對(duì)比。整體而言,3次仿真實(shí)驗(yàn)的位置誤差基本處于同一水平,當(dāng)存在異步和時(shí)延時(shí),位置誤差出現(xiàn)了一定程度的增大。這主要是由于系統(tǒng)輸出頻率高于GNSS、OD測(cè)量頻率,在沒(méi)有GNSS、OD數(shù)據(jù)的時(shí)刻,只能單獨(dú)依賴(lài)INS。而對(duì)于因子圖算法而言,其完全不需要針對(duì)傳感器異步和時(shí)延進(jìn)行調(diào)整,只需要按部就班地將接收到的傳感器測(cè)量值定義為因子節(jié)點(diǎn)連接到因子圖中對(duì)應(yīng)的變量節(jié)點(diǎn)上,然后采用增量推理算法對(duì)因子圖進(jìn)行推理,便可以得到導(dǎo)航狀態(tài)量的最優(yōu)估計(jì)值。

圖7 實(shí)驗(yàn)4、5、6位置誤差曲線對(duì)比Fig.7 Comparison of position error curves of test 4, 5, and 6

4 結(jié) 論

本文提出了基于因子圖的INS/GNSS/OD組合導(dǎo)航算法,研究其處理傳感器有效性動(dòng)態(tài)改變、傳感器存在異步和時(shí)延等方面的性能。實(shí)驗(yàn)結(jié)果表明,當(dāng)傳感器有效性發(fā)生變化時(shí),只需要在因子圖模型中抑制或加入其因子節(jié)點(diǎn),便可以完成組合導(dǎo)航系統(tǒng)工作模式的無(wú)縫切換,使系統(tǒng)具有很強(qiáng)的魯棒性。而對(duì)于傳感器存在異步和時(shí)延的情況,因子圖算法只需要直接將接收到的傳感器測(cè)量值定義為因子節(jié)點(diǎn)連接到因子圖中對(duì)應(yīng)的變量節(jié)點(diǎn)上,不需要將不同傳感器測(cè)量值統(tǒng)一到同一時(shí)刻,從而避免引入額外的人為誤差。

因子圖模型良好的擴(kuò)展性和靈活性,使其在處理多傳感器組合導(dǎo)航問(wèn)題中具有較大優(yōu)勢(shì)?；谝蜃訄D的INS/GNSS/OD組合導(dǎo)航算法對(duì)于其他應(yīng)用環(huán)境下的多傳感器組合導(dǎo)航系統(tǒng)具有較高的參考價(jià)值。