基于因子圖的車載INS/GNSS/OD組合導航算法

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

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

0 引 言

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

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

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

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

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

以INS因子圖模型為主體,當GNSS、OD等傳感器測量信息有效時,將與之對應的因子節(jié)點連接進INS因子圖模型中,構(gòu)成組合導航系統(tǒng)因子圖模型。

1.1 INS因子圖模型

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

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

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

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

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

(1)

根據(jù)INS航位推算的原理,可進一步得到tk+1時刻載體位置、速度、姿態(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)

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

(5)

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

αk)

(6)

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

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

(7)

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

1.2 GNSS因子節(jié)點

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

(8)

(9)

1.3 OD因子節(jié)點

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

(10)

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

(11)

1.4 INS/GNSS/OD因子圖模型

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

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

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

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

2 組合導航因子圖模型推理方法

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

2.1 因子圖算法

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

G=(F,X,E)

(12)

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

(13)

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

(14)

(15)

因此,對式(13)取極大值等價于最小二乘問題,即

(16)

2.2 增量推理方法

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

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

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

(17)

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

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

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

(18)

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

3 仿真分析

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

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

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

(3)GNSS的經(jīng)度、緯度測量噪聲均方差為1 m,高度測量噪聲均方差為3 m,3個方向速度測量噪聲均方差都為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 傳感器有效性動態(tài)改變仿真

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

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

實驗2GNSS信號在600～1 200 s不可用,INS單獨導航;

實驗3GNSS信號在600～1 200 s不可用,采用OD輔助INS進行導航。

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

圖5 實驗1、2、3位置誤差曲線對比Fig.5 Comparison of position error curves of test 1, 2, and 3

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

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

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

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

3.2 傳感器異步和時延仿真

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

實驗4無異步、無時延;

實驗5有異步、無時延;

實驗6有異步、有時延。

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

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

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

圖7 實驗4、5、6位置誤差曲線對比Fig.7 Comparison of position error curves of test 4, 5, and 6

4 結(jié) 論

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

因子圖模型良好的擴展性和靈活性,使其在處理多傳感器組合導航問題中具有較大優(yōu)勢。基于因子圖的INS/GNSS/OD組合導航算法對于其他應用環(huán)境下的多傳感器組合導航系統(tǒng)具有較高的參考價值。