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

高軍強, 湯霞清, 張 環, 武 萌

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

0 引 言

隨著導航技術的不斷發展,多傳感器組合導航已經逐漸成為各種復雜環境下長時間高精度導航的主要方式[1-2]。在陸用車載導航系統中,慣性導航系統(inertial navigation system,INS)、全球導航衛星系統(global navigation satellite system,GNSS)、里程計(odometer,OD)構成的組合導航系統被廣泛應用,其中OD可以在GNSS信號因遮擋、多路徑效應等因素導致不可用時,有效減緩INS的誤差積累,從而實現車輛的長時間高精度穩定導航。

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

因子圖是概率圖模型的一種,其對導航狀態量與導航系統測量值之間的概率關系進行編碼,將狀態量定義為變量節點,各傳感器測量值定義為因子節點,多傳感器組合導航便轉換為在因子圖中相應的變量節點上連接這些因子節點[6-7]。因子圖模型具有很強的靈活性,能夠實現傳感器的即插即用,已經在導航領域受到廣泛關注[8-10]。

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

1 組合導航系統因子圖模型

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

1.1 INS因子圖模型

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

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

圖1 INS因子圖模型構建方案Fig.1 Construct scheme of INS factor graph model

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

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

(1)

根據INS航位推算的原理,可進一步得到tk+1時刻載體位置、速度、姿態為

(2)

式中,gn表示重力矢量。

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

,αk,zk)

(3)

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

(4)

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

(5)

慣性器件偏差隨時間不斷變化,假設其轉移函數為

αk)

(6)

則偏差因子節點定義為

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

(7)

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

1.2 GNSS因子節點

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

(8)

(9)

1.3 OD因子節點

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

(10)

從而得到OD因子節點為

(11)

1.4 INS/GNSS/OD因子圖模型

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

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

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

上述構建組合導航系統因子圖模型的過程表明,組合導航系統中各傳感器的因子節點在因子圖模型中是相互獨立的、互不影響的。以GNSS的有效性改變為例,當GNSS突然失效時,只需要抑制GNSS因子節點的繼續添加,便轉換為INS/OD因子圖模型;而當GNSS變為有效時,只需要重新添加GNSS因子節點,便恢復為INS/GNSS/OD因子圖模型。說明采用因子圖模型描述組合導航問題可以靈活地處理傳感器有效性的動態改變,基于因子圖的組合導航算法具有很強的擴展性和靈活性。

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

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

2.1 因子圖算法

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

G=(F,X,E)

(12)

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

(13)

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

(14)

(15)

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

(16)

2.2 增量推理方法

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

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

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

(17)

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

圖4 貝葉斯網絡和貝葉斯樹Fig.4 Bayes net and Bayes tree

對比發現,加入新的節點,僅對部分狀態變量的估計產生影響,因此在當前時刻的推理中只需要對受到影響的部分狀態變量及新添加的狀態變量進行估計。

(18)

當因子圖中加入新的因子節點時,首先根據新因子節點中包含的狀態量確定貝葉斯樹中受影響的部分以及涉及到的狀態量,原因子圖中包含這些狀態量的部分與新因子節點構成新的因子圖,然后轉化為新的貝葉斯樹,再與未受影響的部分相接,從而實現貝葉斯樹的增量更新。

3 仿真分析

在Matlab環境下,進行仿真分析,仿真條件設置如下:

(1)仿真軌跡起始位置為緯度39°、經度116°、海拔35 m,初始速度為0,航向正北,經加速、轉彎、爬坡、下坡、減速等動作后到達終點為緯度39.02°,經度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的經度、緯度測量噪聲均方差為1 m,高度測量噪聲均方差為3 m,3個方向速度測量噪聲均方差都為0.1 m/s；

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

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

3.1 傳感器有效性動態改變仿真

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

實驗1GNSS數據始終可用;

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

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

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

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

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

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

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

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

3.2 傳感器異步和時延仿真

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

實驗4無異步、無時延;

實驗5有異步、無時延;

實驗6有異步、有時延。

其中,“有異步”是指系統輸出頻率為5 Hz,而GNSS、OD測量頻率為1 Hz;“有時延”是指GNSS與OD數據更新時刻相對于系統輸出整點時刻分別存在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次仿真實驗的位置誤差基本處于同一水平,當存在異步和時延時,位置誤差出現了一定程度的增大。這主要是由于系統輸出頻率高于GNSS、OD測量頻率,在沒有GNSS、OD數據的時刻,只能單獨依賴INS。而對于因子圖算法而言,其完全不需要針對傳感器異步和時延進行調整,只需要按部就班地將接收到的傳感器測量值定義為因子節點連接到因子圖中對應的變量節點上,然后采用增量推理算法對因子圖進行推理,便可以得到導航狀態量的最優估計值。

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

4 結 論

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

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