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

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

0 引 言

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

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

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

本文在說明因子圖基本數(shù)學(xué)理論的基礎(chǔ)上,闡述了因子圖算法在典型工程領(lǐng)域中的應(yīng)用,說明了因子圖在定位與導(dǎo)航系統(tǒng)中的算法,對因子圖及其在導(dǎo)航系統(tǒng)中的應(yīng)用進(jìn)行了綜述.

1 因子圖基本理論

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

1.1 因子圖的一般模型

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

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

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é)出,每個(gè)因子圖都是由節(jié)點(diǎn)和邊組成的,每個(gè)局部函數(shù)都可以由唯一的一個(gè)因子節(jié)點(diǎn)表示;每個(gè)變量由唯一的一個(gè)變量節(jié)點(diǎn)表示;當(dāng)某一個(gè)局部函數(shù)與某一個(gè)變量相關(guān)時(shí),相應(yīng)的因子節(jié)點(diǎn)與變量節(jié)點(diǎn)通過邊相連．

1.2 概率圖與因子圖模型

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

與貝葉斯網(wǎng)絡(luò)與馬爾科夫隨機(jī)場不同,因子圖是從變量全局函數(shù)分解的角度考慮,是一種更為精細(xì)的模型表示方法,借助因子圖的概念可以實(shí)現(xiàn)多類模型表示方法上的統(tǒng)一．換而言之,貝葉斯網(wǎng)絡(luò)與馬爾科夫隨機(jī)場都能與因子圖相互轉(zhuǎn)換．舉例來說,令g(x1,x2,x3,x4,x5)表示一組隨機(jī)變量的聯(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)絡(luò)、馬爾科夫隨機(jī)場分別表示的概率圖示例．由三種圖對比可以看出,因子圖是一種比貝葉斯網(wǎng)絡(luò)和馬爾科夫隨機(jī)場更為精細(xì)的模型表示．例如在圖3(a)所示的因子圖中,可以用因子節(jié)點(diǎn)表示并區(qū)分不同的具體分布形式,能夠根據(jù)因子圖寫出唯一對應(yīng)的聯(lián)合概率密度函數(shù);而在圖3(b)貝葉斯網(wǎng)絡(luò)與圖3(c)馬爾可夫隨機(jī)場中,表示的方法更為籠統(tǒng),對應(yīng)的聯(lián)合概率密度函數(shù)并不唯一．

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

圖3 三種概率圖示例

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

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

在圖2所示因子圖中,已知聯(lián)合概率密度函數(shù)為公式1,隨機(jī)變量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右邊所有變量的信息匯總．一個(gè)全局的信息匯總可以由連續(xù)的子系統(tǒng)的局部信息匯總得到．

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

1.4 因子圖在通信編碼和概率推理方面的應(yīng)用

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

1.4.1 因子圖在通信編碼領(lǐng)域的應(yīng)用

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

圖4 LDPC譯碼因子圖示例

1.4.2 因子圖在概率推理領(lǐng)域的應(yīng)用

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

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

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

2 因子圖在定位與導(dǎo)航領(lǐng)域應(yīng)用

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

2.1 基于因子圖模型的SLAM定位與導(dǎo)航問題

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

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

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

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

(5)

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

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

圖5 SLAM中因子圖建模方法

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

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

(6)

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

(7)

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

(8)

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

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

2.1.2 因子圖模型在SLAM定位與導(dǎo)航中的應(yīng)用

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

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

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

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

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

2) 因子圖在水下機(jī)器人SLAM定位與導(dǎo)航中的應(yīng)用.

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

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

3) 因子圖在其他SLAM定位與導(dǎo)航中的應(yīng)用.

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

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

2.2 基于因子圖在組合導(dǎo)航信息融合問題

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

2.2.1 基于因子圖的組合導(dǎo)航信息融合基本模型

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

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

Xk=[pTvTqTaTgT]，

(9)

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

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

(10)

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

圖6 組合導(dǎo)航因子圖建模方法

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

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

2.2.2 因子圖在組合導(dǎo)航中的應(yīng)用

慣性導(dǎo)航系統(tǒng)是導(dǎo)航系統(tǒng)中的重要傳感器,因?yàn)樗軌蛱峁┯嘘P(guān)載體運(yùn)動的高頻信息,但由于慣導(dǎo)的誤差會隨著時(shí)間的推移累積發(fā)散,所以,需要引入其他傳感器系統(tǒng)進(jìn)行數(shù)據(jù)融合．因此,因子圖提供了一個(gè)非常靈活的框架,可以融合這多個(gè)互補(bǔ)的信息來源[45]．

1) 因子圖在慣性/視覺組合導(dǎo)航的應(yīng)用

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

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

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

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

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

2) 因子圖在多傳感器信息融合導(dǎo)航中的應(yīng)用

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

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

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

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

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

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

3 結(jié)束語

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

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