因子圖發(fā)展及其在定位與導航的應用技術

周雅婧, 曾慶化, 2, 3, 劉建業(yè), 2, 3, 孫克誠

(1. 南京航空航天大學 導航研究中心,江蘇 南京 211106;2.先進飛行器導航、控制與健康管理工業(yè)和信息化部重點實驗室,江蘇 南京 211106;3. 衛(wèi)星通信與導航江蘇高校協(xié)同創(chuàng)新中心,江蘇 南京 210016)

0 引 言

圖模型是一種可用于描述自然界和人類社會中的多個事物之間關系的可見模型,采用圖模型以及圖論對各種系統(tǒng)特別是復雜系統(tǒng)進行分析是一種有效而直觀的方法．因子圖是圖模型中的一種,用于表示函數(shù)的因式分解[1]．在復雜系統(tǒng)中,系統(tǒng)建模與計算時需要處理具有大量不同變量的復雜全局函數(shù),因此,將全局函數(shù)進行因式分解,并改寫成多個簡單局部函數(shù)的乘積,直觀地表示哪些變量屬于哪些局部函數(shù),提高計算效率．和-積算法是因子圖的消息傳遞規(guī)則,該算法能夠表示因子節(jié)點和變量節(jié)點之間傳遞的消息,是基于因子圖模型的各種算法的基礎．因子圖作為一種圖模型可以用來表示任何動態(tài)狀態(tài)空間系統(tǒng),能夠清楚直觀地表示系統(tǒng)狀態(tài)量之間的信息傳遞關系．因此,因子圖模型在統(tǒng)計推斷、譯碼編碼、消息傳遞、濾波平滑等方面都有所應用．

因子圖理論起源于編碼理論,1981年,Tanner[2]引入了一種二分圖來描述Gallager提出的低密度奇偶校驗LDPC代碼的校驗矩陣,稱為“Tanner圖”,并且還基于該圖模型解釋了表示消息傳遞的算法．從因子圖的角度來看,編碼的Tanner圖表示代碼的特征函數(shù)的特定因式分解．1995年,Wiberg等[3]擴展了Tanner圖的定義,將迭代解碼算法用廣義Tanner圖表示,并實現(xiàn)了基于改進Tanner圖模型的Viterbi解碼算法與BCJR解碼．2001年,Kschischang等[4]在用于分析信道編碼的Tanner圖、Wiberg圖等圖模型的基礎上做了一定的改進和推廣,抽象并提煉出了因子圖的概念與和-積算法．人工智能,信號處理和數(shù)字通信中開發(fā)的各種算法可以作為和-積算法的具體應用．Loeliger[5]詳細地將卡爾曼濾波器與最小二乘法兩種最優(yōu)估計方法與基于因子圖的和-積算法統(tǒng)一起來,通過因子圖的模型描述了估計算法．

上述數(shù)學層面上的理論分析,建立了因子圖建模及算法理論,為因子圖理論在導航等實際問題上的應用奠定了基礎．導航系統(tǒng)實際上是一個多變量的控制系統(tǒng),通常以狀態(tài)空間形式表示,所以導航系統(tǒng)也能夠以因子圖模型的形式進行建模．在用因子圖的建模方法解算系統(tǒng)位置與姿態(tài)等導航信息時,組合導航系統(tǒng)的狀態(tài)方程和觀測方程可以用因子圖形式表示,為此,研究導航狀態(tài)量的計算與融合算法也就轉換成了研究導航系統(tǒng)因子圖上的信息傳遞方法．

本文在說明因子圖基本數(shù)學理論的基礎上,闡述了因子圖算法在典型工程領域中的應用,說明了因子圖在定位與導航系統(tǒng)中的算法,對因子圖及其在導航系統(tǒng)中的應用進行了綜述.

1 因子圖基本理論

因子圖是用來表示因式分解的二分圖模型,二分圖的特點是圖的頂點能夠劃分為兩個不相交集,使每一條邊都分別連接兩個不相交集中的節(jié)點,即圖中的頂點可以分為兩個不同的類型．因子圖可以用來對各類系統(tǒng)進行建模,基于因子圖建模的主要目的是將復雜的系統(tǒng)進行因式化分解．根據(jù)實際求解的問題,采用相應的因子圖模型進行建模,可以提高解決問題的效率．當因子圖的節(jié)點表示隨機變量和概率分布時,因子圖也是一種概率圖模型,在導航上的應用也多為概率圖．

1.1 因子圖的一般模型

在因子圖G=(F,X,E)里,包含兩種類型的節(jié)點:一種是因子節(jié)點fi∈F,代表因式分解中的局部函數(shù);一種是變量節(jié)點xj∈X,代表全局多元函數(shù)中的變量．邊緣eij∈E,當且僅當因子圖中變量節(jié)點xj是與之相應的因子節(jié)點fi的自變量時,它們之間存在一條連接邊eij[6]．

因子圖的構建有兩種常用方式,如圖1所示的學者Forney提出的因子圖與如圖2所示的學者Loeliger提出的因子圖[1]．兩種圖都可以表示一個多元函數(shù)的因式分解,且兩者的本質(zhì)是一致的,僅僅是符號表示的方法不同．Forney式因子圖的優(yōu)點在于支持分層建模,兼容標準框圖,Loeliger式因子圖的優(yōu)點在于方便轉換為概率模型．圖1與圖2都表示了f(x1,x2,x3,x4)的因式分解,可分解為3個因式乘積的形式,即

f(x1,x2,x3,x4)=f1(x1,x2,x3)×

f2(x3,x4)×f3(x4).

(1)

式中:f(x1,x2,x3,x4)表示全局函數(shù);f1(x1,x2,x3)、f2(x3,x4)、f3(x4)表示局部函數(shù).

圖1 Forney式因子圖示例

圖2 Loeliger式因子圖示例

從兩種因子圖中可以總結出,每個因子圖都是由節(jié)點和邊組成的,每個局部函數(shù)都可以由唯一的一個因子節(jié)點表示;每個變量由唯一的一個變量節(jié)點表示;當某一個局部函數(shù)與某一個變量相關時,相應的因子節(jié)點與變量節(jié)點通過邊相連．

1.2 概率圖與因子圖模型

概率圖模型是應用因子圖模型解決工程問題時最為常見的模型．概率圖模型是一種以圖模型表示變量概率依存關系的理論,它是對復雜不確定性問題進行建模和計算的重要工具之一[7],概率圖被廣泛應用于需要進行概率推理的場合[8]．基本的概率圖一般可以分為兩個類別:貝葉斯網(wǎng)絡和馬爾可夫隨機場．貝葉斯網(wǎng)絡采用有向圖來表達因果關系,馬爾可夫隨機場則采用無向圖來表達變量間的相互作用．

與貝葉斯網(wǎng)絡與馬爾科夫隨機場不同,因子圖是從變量全局函數(shù)分解的角度考慮,是一種更為精細的模型表示方法,借助因子圖的概念可以實現(xiàn)多類模型表示方法上的統(tǒng)一．換而言之,貝葉斯網(wǎng)絡與馬爾科夫隨機場都能與因子圖相互轉換．舉例來說,令g(x1,x2,x3,x4,x5)表示一組隨機變量的聯(lián)合概率密度函數(shù),其可以分解成式(2)的形式:

g(x1,x2,x3,x4,x5)=g1(x1)×g2(x2)×

g3(x3|x1,x2)×

g4(x4|x3)×

g5(x5|x3).

(2)

圖3是式2用因子圖、貝葉斯網(wǎng)絡、馬爾科夫隨機場分別表示的概率圖示例．由三種圖對比可以看出,因子圖是一種比貝葉斯網(wǎng)絡和馬爾科夫隨機場更為精細的模型表示．例如在圖3(a)所示的因子圖中,可以用因子節(jié)點表示并區(qū)分不同的具體分布形式,能夠根據(jù)因子圖寫出唯一對應的聯(lián)合概率密度函數(shù);而在圖3(b)貝葉斯網(wǎng)絡與圖3(c)馬爾可夫隨機場中,表示的方法更為籠統(tǒng),對應的聯(lián)合概率密度函數(shù)并不唯一．

普遍的基于概率圖推理算法一般是使用貝葉斯網(wǎng)絡或者馬爾科夫網(wǎng)絡,其節(jié)點之間的條件獨立性是重要的先決條件,但這兩種圖模型在表達節(jié)點間條件獨立性方面存在著不足[9]．由于因子圖結構能夠顯式地表示出構造概率分布的因子,因此特別適合通過變量與因子之間傳遞消息的推理算法的執(zhí)行．

圖3 三種概率圖示例

1.3 因子圖模型的和-積算法

在因子圖中,變量節(jié)點到因子節(jié)點或者因子節(jié)點到變量節(jié)點的過程是通過信息交互．和-積算法是基于因子圖的消息傳遞算法,該算法首先將有向圖、無向圖轉化為因子圖,然后基于因子圖框架進行推導求解．

在圖2所示因子圖中,已知聯(lián)合概率密度函數(shù)為公式1,隨機變量x3的邊緣概率可由x1,x2,x3,x4的聯(lián)合概率求解,具體如公式(3)所示,對x3外的其它變量的概率求和,最終剩下x3的概率．

(3)

由于聯(lián)合概率的因式分解已知,因此,式(3)可以做如下變化:

=μf1→x3(x3)·μf2→x3(x3),

(4)

式中:μf1→x3(x3)表示因子圖中x3左邊所有變量的信息匯總；μf2→x3(x3)表示因子圖中x3右邊所有變量的信息匯總．一個全局的信息匯總可以由連續(xù)的子系統(tǒng)的局部信息匯總得到．

因子圖與和-積算法的本質(zhì)即沿著變量節(jié)點從因子節(jié)點傳遞出的信息是因子和沿著除該變量以外其余所有變量傳入的信息的乘積,然后對除該變量以外其余所有相關變量進行求和結果．基于因子圖的消息傳遞算法是一個高效的求變量的邊緣概率分布的算法．

1.4 因子圖在通信編碼和概率推理方面的應用

因子圖在通信編碼和概率推理方面已經(jīng)有了廣泛的應用．根據(jù)應用領域的不同,因子圖變量節(jié)點與因子節(jié)點的類型也有所不用．在系統(tǒng)概率模型中,因子圖可用于表示包括所述系統(tǒng)中變量的聯(lián)合概率密度函數(shù)．該函數(shù)的因子分解可以提供關于這些變量之間的統(tǒng)計依賴性的重要信息,如馬爾科夫鏈、后驗概率分布都能以因子圖的方式表示．除此以外,在動態(tài)系統(tǒng)建模中因子圖可用于表示給定行為的特征函數(shù),該特征函數(shù)的因子分解可以給出關于模型的重要結構信息,如Tanner圖．在某些應用中,如系統(tǒng)狀態(tài)的優(yōu)化與估計中,將概率模型與狀態(tài)空間模型相結合用因子圖方法表示具有更好的適用性．

1.4.1 因子圖在通信編碼領域的應用

因子圖在通信編碼與信號處理領域的相關研究不斷發(fā)展起來．文獻[10]將Turbo編碼與LDPC碼的譯碼相結合,對Turbo采用基于其因子圖表示的和-積譯碼算法進行譯碼,很大程度地降低了譯碼復雜度,圖4為LDPC的譯碼算法圖例,采用因子圖模型能夠更好地表述和解決問題．文獻[11]將因子圖與粒子濾波相結合,研究了在較強的相位噪聲的情況下基于因子圖模型的載波同步算法．文獻[12]將因子圖算法與Turbo均衡聯(lián)合起來,研究了基于因子圖的高斯消息傳遞算法．文獻[13]提出了一個基于因子圖的均衡器,用于軟輸入軟輸出信道,降低了均衡器的復雜度．

圖4 LDPC譯碼因子圖示例

1.4.2 因子圖在概率推理領域的應用

隨著概率圖模型理論的不斷發(fā)展與完善,因子圖也逐步應用于基于概率模型的推理與預測理論方面．馬爾可夫隨機場與貝葉斯網(wǎng)絡的狀態(tài)空間被分成幾個狀態(tài)空間的乘積,可以以因子圖的形式表示并通過適當?shù)乃惴ㄟM行求解[14]．越來越多的學者不斷將因子圖模型與梯度算法、期望最大化算法、蒙特卡洛算法、濾波與平滑等其它技術進行融合,因子圖作用越來越凸顯．

基于因子圖的推理算法是以概率圖推理算法發(fā)展而來的,其中最大后驗概率估計法(MAP)是最為常見的,在故障診斷、信息融合、狀態(tài)估計、即時定位與地圖構建(SLAM)等領域都有所應用．文獻[15]提出一種采用因子圖模型描述鏈路狀態(tài)和路徑狀態(tài)間的聯(lián)合概率分布以估計鏈路狀態(tài)當前分布的方法;文獻[16]提出使用有向因子圖描述系統(tǒng)組成部件間的因果關系,應用概率推理實現(xiàn)故障診斷的方法;文獻[17]提出了一種基于因子圖的分布式變分稀疏貝葉斯壓縮感知算法;文獻[18]將因子圖作為估計和控制問題的統(tǒng)一表示框架和計算工具并將其應用于無人機避障方向;文獻[19]提出了一種基于可信度函數(shù)的改進因子圖加權信息融合算法;文獻[20]提出一種基于因子圖消息傳遞的衛(wèi)星姿態(tài)角速度估計方法;文獻[21]提出了一種基于因子圖的搜索廣告轉化預測模型．

因子圖作為一種不斷發(fā)展的理論,其優(yōu)勢在不同領域的研究中也有所發(fā)展．近年來,因子圖在導航領域中也有所發(fā)展與應用．

2 因子圖在定位與導航領域應用

近年來,因子圖作為一種著重于表示函數(shù)分解的系統(tǒng)數(shù)學模型,與基于因子圖的推理算法一起,在定位與導航領域中也有所研究與應用．定位與導航的目的是實時精準地得到物體的位置、速度與姿態(tài)等信息．基于因子圖的算法在某些定位與導航應用環(huán)境中對定位與導航系統(tǒng)的實時性和精確性有著重要的影響．其中,SLAM問題是因子圖的一個重要方向,因子圖作為一種狀態(tài)估計與推理方法,用于解決移動機器人運行時定位導航與地圖構建中的優(yōu)化、建圖等問題．因子圖在導航領域另一個應用是基于因子圖的多傳感器信息融合定位與導航系統(tǒng),因子圖能夠實現(xiàn)系統(tǒng)傳感器的即插即用,已經(jīng)在組合導航領域受到廣泛關注．

2.1 基于因子圖模型的SLAM定位與導航問題

SLAM是當前移動機器人自主定位與導航的重要方法．SLAM的概念可以表述為機器人在一個未知環(huán)境中運動,移動過程中自身根據(jù)運動模型和傳感器所得環(huán)境觀測信息進行定位,同時對周圍環(huán)境描述地圖的一個過程[22-23]．SLAM過程主要分為前端與后端兩個過程,前端主要處理傳感器獲取的數(shù)據(jù),后端則主要是對前端的結果進行優(yōu)化,后端本質(zhì)上是一個狀態(tài)估計問題,因此,基于因子圖的狀態(tài)估計推理算法在特定的SLAM后端優(yōu)化的過程中有著廣泛的應用．

2.1.1 基于因子圖表示的SLAM優(yōu)化基本模型

SLAM問題中機器人的每個位姿及地圖中每個環(huán)境特征都具有不確定性,SLAM的本質(zhì)其實是一個概率估計問題,也就是對機器人位姿和環(huán)境地圖特征位置的推算與估計,對它的求解,即是對概率估計問題的求解．

對于給定測量值Z,對未知狀態(tài)變量X(例如位姿或地標)的概率估計問題,最常用的估計方法是最大后驗概率,其原理是在給定測量值Z的情況下,使狀態(tài)變量X的后密度p(X/Z)最大．最大后驗概率估計在最大似然估計的基礎上又增加了先驗概率的輔助,而先驗概率的加入使得估計在較小的數(shù)據(jù)集中能夠保持良好的泛化性能[24],是基于因子圖建模的估計問題中最主要使用的估計算法．最大后驗概率估計的公式為

(5)

引入因子圖模型的主要原因是:1)因子圖能夠準確區(qū)分量測變量與狀態(tài)變量及其之間的關系;2)因子圖的結構為解決大規(guī)模推理問題的計算提供了更為有效的新策略．

圖5為一個基于SLAM特征圖構建的因子圖．圖中明確地引入了一個額外的節(jié)點類型來表示后驗概率密度p(X|Z)中的每個因子．在基于圖5優(yōu)化的SLAM 中,機器人的位姿被表示為圖中的變量節(jié)點,觀測信息在經(jīng)過處理后轉變?yōu)闄C器人位姿間的約束關系,并通過連接節(jié)點間的邊來表示．未知狀態(tài)變量X以變量節(jié)點(白色空心圓)表示,包含xt(t=1,…,i)和lt(t=1,…,i),分別表示機器人的位姿和路標．因子節(jié)點(黑色和灰色實心方框)分別代表位姿和路標、位姿和位姿間的概率關系,也稱之為里程計因子和路標觀測因子．

圖5 SLAM中因子圖建模方法

在SLAM問題中,全局函數(shù)用(Z)表示,通過求解每個因子的結果再求其乘積,來求解全局函數(shù)的值．乘積運算通常在對數(shù)空間進行更為簡單,只須將各個因子函數(shù)的結果進行相加．如果因子是連續(xù)變量中的可微函數(shù),基于梯度的方法可以快速找到后驗的局部最大值．

具體來說,高斯噪聲模型的SLAM問題的MAP推斷等價于求解非線性最小二乘問題．事實上,對于任意因子圖,MAP推斷歸結為最大化所有因子的乘積,公式如下:

(6)

在高斯噪聲模型假設的前提下,可以將每一個因子都可以寫為式(7)的形式,表示獲取預測的量測信息和實際量測信息的差值.

(7)

式中：hi(Xi)是量測函數(shù),是與狀態(tài)變量相關的函數(shù)；zi是由路標信息得到的實際量測值．對式(7)取負對數(shù),并將式(7)帶入式(6),MAP推斷則變化為最小化所有非線性最小二形式因子乘的求和:

(8)

求解該目標函數(shù)最小值的方法是通過組合量測變量以及先驗信息執(zhí)行傳感器信息融合,可以唯一地確定未知數(shù)的最大后驗概率解．在SLAM過程中,量測函數(shù)hi(Xi)通常是非線性的．基于可靠的初始值,通過非線性優(yōu)化方法(如高斯-牛頓迭代法或Levenberg-Marquardt算法)將能夠解算得出收斂到式(8)的全局最小值．求解式(8)的一系列線性近似來達到最小值[25]．

綜上,基于因子圖模型表示的SLAM優(yōu)化過程中,通過將全局變量的最優(yōu)估計轉化成因子相乘的形式,在非線性優(yōu)化算法的具體計算中,有著簡化計算量的優(yōu)點．

2.1.2 因子圖模型在SLAM定位與導航中的應用

因子圖模型作為一種可以清楚表示節(jié)點之間概率關系概率圖模型,能夠簡化SLAM過程中非線性優(yōu)化算法中的計算量,有著越來越多的應用．

1) 因子圖在SLAM密集3D建圖優(yōu)化中的應用.

3D建圖是SLAM定位與導航中的重要過程．雖然稀疏點云對于保持機器人本地化是足夠的,但是與環(huán)境的交互需要更完整的地圖表示,因此因子圖模型在SLAM建圖中的系統(tǒng)建模有著越來越多的應用．

Whelan等[26-27]提出了一個基于密集SLAM解決方案,它將局部密集方法Kinect Fusion三維重建法擴展到大型環(huán)境,將環(huán)閉合方法應用于表示軌跡姿勢的因子圖中,并且通過應用網(wǎng)格變形將相應的校正傳遞到地圖,表示為三角形網(wǎng)格．

Salas-Moreno等[28]提出了SLAM++,即一種基于對象的SLAM系統(tǒng)．系統(tǒng)中的地標對應于環(huán)境中的椅子之類的對象,這些對象可以獲得先前的密集模型． SLAM++系統(tǒng)的整個推理過程是基于因子圖優(yōu)化方法進行計算的．Trevor等人[29]在因子圖優(yōu)化的背景下對平面SLAM下信息融合進行研究． Kaess等[30-31]使用無限平面作為因子圖中的界標,在大型環(huán)境的SLAM建圖中,可以實時地進行優(yōu)化．

2) 因子圖在水下機器人SLAM定位與導航中的應用.

因子圖算法已經(jīng)應用在水下機器人定位與導航的研究中． Hover等[32]應用它們來檢查船體和港口基礎設施與懸停的自主水下航行器(HAUV)．

對于自主水下航行器的定位與導航方式,聲吶定位、視覺定位都是其常用技術．因子圖在聲吶[33-34]、視覺定位[35]以及Mosaics技術[36]中都有所應用．Carlevaris-Bianco等人提出的稀疏化算法[37]已用于水下情景中的長航時和多重區(qū)段任務執(zhí)行． Beall等[38]使用因子圖來描述水下三維重建的大規(guī)模束調(diào)整．Bichucher等[39]提供了測深因子圖SLAM算法,該算法使用從多普勒速度測井生成的稀疏點云．

3) 因子圖在其他SLAM定位與導航中的應用.

因子圖模型也已在其他SLAM定位與導航得到應用． Tweddle等[40]使用因子圖來模擬空間中旋轉物體的剛體動力學．該系統(tǒng)已經(jīng)在國際空間站上被稱為Spheres的微型衛(wèi)星進行了測試．Zhang等[41]提出了一種視覺測距方法,其能夠使用稀疏深度信息,例如用于單獨的LIDAR傳感器,或者來自RGB-D相機,其僅具有傳感器范圍內(nèi)的部分場景,該優(yōu)化使用因子圖實現(xiàn),并使用iSAM庫進行優(yōu)化．

許多SLAM系統(tǒng)的應用中需要得到細粒度的軌跡分辨率,旋轉的激光距離傳感器也會沿著軌跡在每個測量位置稍微不同的位置進行移動,因此需要在每一秒鐘內(nèi)進行位姿估計．因為因子圖中的位姿之間可能存在插值,所以關于可以直接連接測量和圖優(yōu)化的連續(xù)時間表示的研究也應運而生．Anderson等[42]使用高斯過程回歸提出了基于因子圖的解決方案．在后續(xù)文章中[43],他們在因子圖中加入了分層小波分解思想,在需要時選擇性地提供更高的時間軌跡分辨率． Yan等[44]提出了在連續(xù)軌跡估計的背景下基于因子圖的稀疏GP回歸的增量算法．

2.2 基于因子圖在組合導航信息融合問題

隨著導航技術的不斷發(fā)展,單一導航系統(tǒng)已經(jīng)不能滿足高精度導航的要求了．為了提高導航系統(tǒng)的精度和可靠性,需要利用衛(wèi)星導航系統(tǒng)、計程儀、天文導航系統(tǒng)、視覺輔助導航等傳感器來協(xié)助導航．基于因子圖模型的融合框架可以有效解決數(shù)據(jù)融合中的異步問題,而且針對多傳感器具有很好的擴展性,可以對傳感器靈活配置．

2.2.1 基于因子圖的組合導航信息融合基本模型

因子圖算法的基本思想是通過構建系統(tǒng)某一時間段內(nèi)的圖模型,將系統(tǒng)狀態(tài)與導航信息相關聯(lián),基于后驗估計理論實現(xiàn)數(shù)據(jù)的融合．即在給定所有可用量測值后,計算所有狀態(tài)的聯(lián)合概率分布函數(shù)的最大后驗概率估計．在多源組合導航的應用中,由于慣性導航擁有較高的數(shù)據(jù)生成率和抗干擾性,所以一般被視作組合導航系統(tǒng)中的參考源．

為了實現(xiàn)導航系統(tǒng)中的數(shù)據(jù)融合,首先建立基于因子圖的組合導航系統(tǒng)模型．設k時刻的狀態(tài)變量為Xk,Zk為直到當前時刻tk接收到的所有量測值．

Xk=[pTvTqTaTgT]，

(9)

其中:P=[xyz]T為載體位置向量;v=[vxvyvz]T為載體速度向量;q=[q0q1q2q3]T為載體姿態(tài)四元數(shù)向量;a=[axayaz]T為加速度計隨機游走項;g=[gxgygz]T為陀螺儀隨機游走項．

與基于因子圖的SLAM優(yōu)化類似,基于因子圖的組合導航算法,需求解待估參數(shù)的最大后驗概率,同時根據(jù)因式分解,最大后驗估計公式為

(10)

式(10)表示因子節(jié)點獲取預測的量測信息和實際量測信息的差值．其中hi(·)是量測函數(shù),是與狀態(tài)變量相關的函數(shù),在導航框架中hi(·)可以根據(jù)給定的狀態(tài)估計預測傳感器的量測值,而zi是由各類傳感器得到的實際量測值．

圖6 組合導航因子圖建模方法

根據(jù)組合導航系統(tǒng)的實際特點以及可選擇的導航傳感器設備,構建基于因子圖的組合導航框架．圖6所示為一個基于因子圖的組合導航系統(tǒng)框架,采取慣性導航系統(tǒng)(INS)為主要參考源,輔以全球衛(wèi)星導航系統(tǒng)(GNSS)以及超寬帶(UWB)定位系統(tǒng)三種導航方式,將三種導航系統(tǒng)量測方程抽象為三類因子節(jié)點．圖6中,圓圈代表狀態(tài)變量節(jié)點,黑色方塊代表因子節(jié)點,Xk代表系統(tǒng)的導航狀態(tài),f代表各傳感器量測信息．fIMU表示來自IMU的量測信息,與tk時刻和tk+1時刻的導航狀態(tài)相關,fGNSS和fUWB也分別是來自GNSS和UWB的量測信息．

基于因子圖構架的組合導航系統(tǒng)能夠有效地實現(xiàn)導航傳感器的即插即用．若需在導航系統(tǒng)中加入其他導航傳感器量測信息如氣壓高度計、磁力計等,只需要定義相應的新因子節(jié)點拓展因子圖,根據(jù)傳感器的觀測方程以及相應的代價函數(shù)進行變量邊緣的狀態(tài)更新．基于因子圖的多傳感器信息融合方法,是解決多傳感器觀測數(shù)據(jù)異步問題的一種有效方法,當接收到傳感器的輸出數(shù)據(jù)后,對因子圖節(jié)點進行擴充,根據(jù)系統(tǒng)的狀態(tài)方程和量測方程快速有效地進行系統(tǒng)狀態(tài)的更新,實現(xiàn)導航系統(tǒng)多傳感器的數(shù)據(jù)綜合處理．

2.2.2 因子圖在組合導航中的應用

慣性導航系統(tǒng)是導航系統(tǒng)中的重要傳感器,因為它能夠提供有關載體運動的高頻信息,但由于慣導的誤差會隨著時間的推移累積發(fā)散,所以,需要引入其他傳感器系統(tǒng)進行數(shù)據(jù)融合．因此,因子圖提供了一個非常靈活的框架,可以融合這多個互補的信息來源[45]．

1) 因子圖在慣性/視覺組合導航的應用

由于對基于慣性導航的組合導航以及SLAM兩方面的研究越來越深入,越來越多的研究人員將SLAM圖優(yōu)化算法使用在組合導航信息融合過程中,在慣性/視覺為主的信息組合導航領域中對它的使用也越來越多．

Indelman等[46]提出了一種基于因子圖的慣性/視覺信息融合方法,該方法將信息融合問題用因子圖模型來表示．因子圖將未知變量節(jié)點和已知測量值的關系進行編碼,融合來自不同的、非同步的傳感器觀測值的問題就變成在因子圖中連接測量值定義的因子與相應節(jié)點的問題．使用因子圖可以使系統(tǒng)具有即插即用的功能,當有新的傳感器接入系統(tǒng)時,只需要在因子圖中增加新的節(jié)點;同樣的,當某個傳感器由于信號丟失或者傳感器故障變得不可用時,系統(tǒng)只需要限制增加相應的因子,而不需要特別的處理．

Indelman等[47]提出了基于因子圖的增量平滑和映射(iSAM)算法．該方法能夠自動確定每個步驟重新計算的狀態(tài)數(shù),有效地充當自適應固定滯后平滑器．這為信息融合提供了有效且通用的框架,提供了近乎最優(yōu)的狀態(tài)估計．該算法提出的構架基于IMU/GPS/立體視覺測量組合導航系統(tǒng)進行仿真驗證,和傳統(tǒng)的擴展卡爾曼濾波器獲得的最優(yōu)解進行比較,能夠顯著提高計算效率．

為了處理基于因子圖的組合導航系統(tǒng)中IMU更新率較大的問題,Lupton等[48]提出了一種IMU因子預積分算法,在其他傳感器(如攝像機和激光雷達)的低速率測量之間預先集成IMU測量,使用IMU預積分算法是平衡因子圖的平滑計算效率的好方法．

Forster等[49]將IMU預積分算法應用在了基于因子圖的視覺/慣性融合中,提出更為復雜的積分方案并且將其與固定滯后平滑相結合,以產(chǎn)生更優(yōu)的性能．Leutenegger等[50]還提出了基于可視化因子圖的VIO/SLAM算法．Usenko等[51]將半直接測距算法與慣性信息相結合,提出了基本遵循基于因子圖的iSAM算法,應用邊際化來估計最近的狀態(tài)．

2) 因子圖在多傳感器信息融合導航中的應用

傳統(tǒng)的多傳感器信息融合方法大多采用基于聯(lián)邦卡爾曼濾波器的組合導航方法,雖然該方法能夠通過數(shù)據(jù)同步處理方法融合不同速率的傳感器信息并實時計算出導航解,但是為了保持數(shù)據(jù)的同步往往需要丟棄一部分測量值,這樣就會造成信息的浪費;同時,標準卡爾曼濾波器只能夠解決線性問題,而大多數(shù)的傳感器模型都包含非線性成分．至于擴展卡爾曼濾波器,由于邊緣化了過去的狀態(tài)而只使用了最近的狀態(tài)估計,導致表示這些測量值的非線性因子在估計過程中不能夠很好地重線性化,因此,該優(yōu)化過程可能是概率不連續(xù)的,估計值并不是最優(yōu)的．此外,當某個傳感器的信息出現(xiàn)故障變得不可用時,聯(lián)邦濾波器需要進行較復雜的系統(tǒng)重構處理[52]．基于因子圖的多傳感器信息融合導航算法可解決傳統(tǒng)方法的問題,擁有即插即用的優(yōu)點．

Zeng等[53]提出了一種改進的因子圖多傳感器融合導航算法,為了實現(xiàn)多類型傳感器的測量信息融合并實時計算導航解,根據(jù)因子圖的鏈結構對全局最優(yōu)解進行因式分解．將該算法應用于無人機組合導航系統(tǒng)中,與傳統(tǒng)的聯(lián)邦濾波算法對比,能夠更快的重新構建組合導航系統(tǒng)結構,更好地減小導航傳感器精度下降或故障帶來的影響．

Chiu等[54]在因子圖構架的基礎上，進一步將滑動窗的概念引入到多源組合導航因子圖算法中．滑動窗使得因子圖的全局最優(yōu)化過程僅發(fā)生在最新生成的若干個節(jié)點間,它的引入不僅提高了融合算法的估計速度,而且減少了算法的計算量．

高軍強等[55]在因子圖算法的基礎上,采用變量消除算法將因子圖轉化為貝葉斯網(wǎng)絡,并通過貝葉斯樹的形式實現(xiàn)增量推理,降低了傳感器有效性,以及傳感器存在異步和時延對導航精度的影響．因子圖算法具有很強的魯棒性和靈活性,在處理多傳感器信息融合問題中具有較大優(yōu)勢．

針對GNSS信息滯后而導致INS組合導航系統(tǒng)實時性差的問題,高軍強等[56]利用因子圖算法可以在一個信息融合時刻處理各信息源不同時刻量測信息的特點,提出了一種基于因子圖的INS/GPS信息滯后處理方法．待系統(tǒng)接收到GPS 信息之后,再將關于 GPS 信息的因子節(jié)點添加到因子圖模型中,修正 INS 誤差,從而保證系統(tǒng)長時間高精度運行．因子圖算法在保證系統(tǒng)精度的前提下,避免了 GPS 信息滯后對 INS/GPS 組合導航系統(tǒng)實時性的不良影響．

近些年結合因子圖方法的全源導航技術發(fā)展迅猛,形成一個重要的全源導航分支,其在即插即用的方式配置組合傳感器方面[57],有望較好地解決導航系統(tǒng)中某些狀態(tài)方程和觀測方程具有不同程度的非線性問題,為實現(xiàn)高精度、魯棒性強的定位與導航技術奠定基礎．

3 結束語

因子圖作為一種能夠表示函數(shù)因子分解的圖模型,由于其數(shù)學特性,在特定的模型建立、狀態(tài)估計算法中有其獨特的優(yōu)越性．因子圖可以實時求解大規(guī)模最優(yōu)估計問題,簡化了最優(yōu)估計問題中的計算量,提高了系統(tǒng)的實時性．

基于因子圖的導航方法是全源導航系統(tǒng)的重要研究內(nèi)容和方向,結合因子圖方法的信息融合導航方法具有高度適應性的框架,可以滿足各種不同類型的傳感器的即插即用,有利于促使人們根據(jù)導航系統(tǒng)的特性,去探索更加有針對性的信息融合導航濾波方法．