羅子巖，陳 帥，王國棟，趙海飛，馬永犇

(1.南京理工大學(xué)自動化學(xué)院，南京 210094；2.北京航天控制儀器研究所，北京 100039)

0 引言

導(dǎo)航定位技術(shù)是現(xiàn)代戰(zhàn)爭中不可或缺的關(guān)鍵技術(shù)，在精確打擊和協(xié)同作戰(zhàn)中發(fā)揮著重要作用，同時也在無人駕駛等民用領(lǐng)域廣泛應(yīng)用。目前，隨著對導(dǎo)航定位性能的要求越來越高以及導(dǎo)航環(huán)境的日益惡劣，集成在一起的傳感器種類和數(shù)量也不斷增加，導(dǎo)航手段已從單一導(dǎo)航方法發(fā)展到多源、全源組合導(dǎo)航系統(tǒng)階段，多傳感器的組合導(dǎo)航系統(tǒng)已經(jīng)逐漸成為各種復(fù)雜環(huán)境下長時間高精度導(dǎo)航的主要方式。

在多傳感器數(shù)據(jù)融合方面，目前采用較多的方法是聯(lián)邦Kalman濾波算法。但是，多傳感器數(shù)據(jù)融合系統(tǒng)往往各個傳感器的頻率不同，誤差特性也不同，傳統(tǒng)的Kalman濾波算法為了保持數(shù)據(jù)的同步往往需要丟棄一部分測量值，這樣會造成信息的浪費。同時，標準Kalman濾波器只能解決線性問題，而大多數(shù)傳感器模型都包含了非線性成分。擴展Kalman濾波算法應(yīng)用于非線性狀態(tài)估計系統(tǒng)中雖然已經(jīng)得到了學(xué)術(shù)界認可并被廣泛使用，然而該種方法也帶來了兩個缺點：1)當強非線性時其違背局部線性假設(shè)，Taylor展開式中被忽略的高階項帶來大的誤差時，擴展Kalman濾波算法可能會使濾波發(fā)散；2)由于在線性化處理時需要用Jacobi矩陣，其繁瑣的計算過程導(dǎo)致該方法實現(xiàn)相對困難。

然而在因子圖框架下，每個傳感器的測量值被編碼成為一個因子，只需在產(chǎn)生測量值時加入組成框架圖，通過對這些連接起來的因子使用Bayes推理來完成數(shù)據(jù)融合和參數(shù)估計。這種方法顯然適用于處理異質(zhì)異構(gòu)非周期數(shù)據(jù)融合問題，具有很高的靈活性，能夠以即插即用的方式配置、組合傳感器，如果有傳感器失效，也可以從因子圖框架中及時刪除。因此在多源組合導(dǎo)航中，因子圖的身影出現(xiàn)得越來越頻繁，其在無人機、自主水下潛器(Autonomous Underwater Vehicle，AUV)、無人駕駛等領(lǐng)域都有相應(yīng)的應(yīng)用，并且逐漸出現(xiàn)了各種基于因子圖的改進算法[1]。本文將對因子圖模型理論以及因子圖算法的應(yīng)用情況進行研究，并展望其發(fā)展前景。

1 因子圖模型

將一個具有多變量的全局函數(shù)因子分解，得到幾個局部函數(shù)的乘積，以此為基礎(chǔ)得到的一個雙向圖叫做因子圖。因子圖是概率圖模型的一種，其不僅可以被用來解決數(shù)學(xué)概率問題，還能被用來解決人工智能、信號處理、數(shù)字通信等其他領(lǐng)域的復(fù)雜問題[2]。當處理包含許多變量的全局函數(shù)時，通常采用的方法是利用給定函數(shù)的因子作為一個局部函數(shù)，然后由許多個局部函數(shù)的乘積表示全局函數(shù)。因子圖便是表示這樣的因式分解的二分圖模型[3]，因子圖模型由變量節(jié)點、因子節(jié)點以及聯(lián)系兩者的邊組成。若包含多變量的全局函數(shù)g(x1，x2，…，xn)可被分解為局部函數(shù)的乘積，則有

式(1)的因式分解可以使用因子圖表示，且在此雙向圖中，xn表示為一個變形節(jié)點，局部函數(shù)fj表示為因子節(jié)點，Xj表示第j個局部函數(shù)fj(·)的自變量點集，再由表示相互關(guān)系的邊將因子節(jié)點與相應(yīng)的變量節(jié)點聯(lián)系起來，下面給出一個具體的例子。

設(shè)g(x1，x2，x3，x4，x5)為一個包含五個變量的全局函數(shù)，同時該函數(shù)還可以被表示為：g(x1，x2，x3，x4，x5) ＝fA(x1，x2)fB(x2，x3，x4)fC(x4，x5)， 則其對應(yīng)的因子圖如圖1所示。

圖1 因子圖結(jié)構(gòu)示意圖Fig.1 Structure diagram of factor graph

導(dǎo)航系統(tǒng)中，量測函數(shù)通?？梢愿鶕?jù)狀態(tài)變量估計預(yù)測出傳感器的量測值。而因子節(jié)點計算預(yù)測的測量值和實際測量值之間的差值通過構(gòu)建相應(yīng)函數(shù)獲得狀態(tài)變量估計，因子節(jié)點獲取預(yù)測的量測信息和實際量測信息的差值構(gòu)建相應(yīng)的指標函數(shù)從而獲取代價函數(shù)，即

式(3)中，hi(Xi)為與狀態(tài)變量有關(guān)的量測函數(shù)，zi為由傳感器得到的實際測量值，L(·)為代價函數(shù)。對于Gauss噪聲分布，L(·)為平方馬氏距離，定義為L(e)＝eT∑－1e，∑通常取量測噪聲方差陣R[4]。

2 基于因子圖的多傳感器信息融合實現(xiàn)

2.1 衛(wèi)星慣性組合導(dǎo)航實現(xiàn)與因子圖算法可行性分析

文獻[5]以發(fā)射慣性系為導(dǎo)航坐標系，選取SINS基本誤差和慣性器件誤差參數(shù)構(gòu)成15維系統(tǒng)狀態(tài)向量，在全軟化仿真平臺系統(tǒng)中通過軌跡發(fā)生器和衛(wèi)星信號模擬器依次模擬生成導(dǎo)彈的慣組數(shù)據(jù)和GPS中頻信號，再對生成的角速率和比力數(shù)據(jù)進行慣導(dǎo)解算，將GPS中頻信號注入軟件接收機進行導(dǎo)航解算，建立發(fā)射慣性坐標系下慣性／衛(wèi)星松組合導(dǎo)航狀態(tài)方程，采用傳統(tǒng)的Kalman濾波算法和因子圖算法進行信息融合。實驗顯示，運用因子圖融合算法的三軸位置均方誤差絕對值均比傳統(tǒng)Kalman濾波算法的誤差略小，較傳統(tǒng)濾波結(jié)果有一定的改善。結(jié)果表明，因子圖算法可以保證導(dǎo)航系統(tǒng)的正常使用且導(dǎo)航精度可以得到保障。文獻[6]則以慣性衛(wèi)星緊組合導(dǎo)航為背景對因子圖算法代替Kalman濾波的可行性進行了研究，假設(shè)每個衛(wèi)星就是一個傳感器，提供一組量測信息，同時通過因子圖來表示狀態(tài)的遞推與更新。通過Gauss-Newton迭代法求解優(yōu)化方程，得到最優(yōu)的狀態(tài)修正量，實驗結(jié)果同樣證明了因子圖算法的有效性。

2.2 因子圖算法國外研究現(xiàn)狀

2012年，美國Georgia理工學(xué)院的Indelman等針對美國國防高級研究計劃局(DARPA)提出的全源定位導(dǎo)航項目并參照Dellaert等的一些平滑定位算法提出了基于因子圖框架的多源導(dǎo)航傳感器融合算法[7-8]。2013年，美國SRI國際公司的Chiu等[1]在美國Georgia理工學(xué)院研究的基礎(chǔ)上針對機器人自主導(dǎo)航問題給出了基于因子圖的組合導(dǎo)航系統(tǒng)，該系統(tǒng)包含了57種不同的傳感器。其后，德國Chemnitz工業(yè)大學(xué)的Lange等[6]將基于因子圖框架的增量平滑多源導(dǎo)航傳感器融合方法應(yīng)用到裝配有慣性測量單元、光流傳感器和無限反高度表的四旋翼無人機中，并與傳統(tǒng)的擴展Kalman濾波方法進行對比，仿真實驗結(jié)果驗證了基于因子圖的增量平滑算法相比傳統(tǒng)濾波算法的優(yōu)勢。

目前來說，國外的學(xué)者對基于因子圖框架的多源數(shù)據(jù)融合方法已經(jīng)進行了深入的研究，并且可以較好地處理異步、多速率數(shù)據(jù)。但是，研究的應(yīng)用背景多以小場景多動態(tài)SLAM問題為主，尚未在實地大場景下進行驗證。

2.3 因子圖算法應(yīng)用

組合導(dǎo)航系統(tǒng)面臨無人機、導(dǎo)彈、水下潛器等多種使用場景，以及衛(wèi)星信號被干擾、不同傳感器的使用等各種特殊的情況。因此，因子圖算法被應(yīng)用于多種使用場景并衍生出基于因子圖算法的各種改進算法。

沒有就算了。村長適時接過話頭。八斗丘補償問題，村委過些天會研究，你不要到處找人說事。特別是不要找那樣專門戳漏洞的什么狗屁記者。

(1)無人機

王慧哲等[9]以民用無人機為背景，結(jié)合全源導(dǎo)航需要快速集成并重新配置任意導(dǎo)航傳感器的要求，對全源導(dǎo)航的關(guān)鍵技術(shù)展開研究，構(gòu)建了如圖2所示的基于因子圖的多傳感器融合框架，其中涉及的傳感器包括IMU、GPS、氣壓高度計、光流傳感器、磁航向傳感器、星敏感傳感器。

圖2 基于因子圖的多傳感器融合框架Fig.2 Diagram of multi-sensor fusion framework based on factor graph

在接收IMU觀測信息后定義因子節(jié)點fIMU，連接tk和tk+1兩個不同時刻的變量節(jié)點即導(dǎo)航狀態(tài)xk和xk+1， 因子節(jié)點IMU可表示為

在tl時刻接收到GPS觀測信息后，定義因子節(jié)點fGPS

同理，在其他時刻接收到高度計、星敏感器等傳感器觀測信息后，定義相應(yīng)因子節(jié)點拓展因子圖，根據(jù)相應(yīng)的觀測方程和代價函數(shù)進行變量節(jié)點的狀態(tài)更新。實驗以Matlab為平臺實現(xiàn)對飛行器軌跡的設(shè)計，模擬不同傳感器的數(shù)據(jù)輸出以及對文中提出的算法進行仿真驗證。運動軌跡仿真通過抬頭、爬升、平飛、轉(zhuǎn)彎、傾斜、俯沖等機動動作模擬飛行器真實的飛行狀況，并且設(shè)置GPS在500s～1000s內(nèi)不可用、星敏感器在800s～1200s內(nèi)不可用、光流傳感器在1300s～1400s內(nèi)不可用，以模擬飛行過程中的故障情況。仿真結(jié)果表明，因子圖算法能對異步異質(zhì)傳感器的數(shù)據(jù)進行有效融合，并適應(yīng)傳感器在運行過程中可用性發(fā)生變化的情況。

文獻[10]以無人機MINS／BDS／磁羅盤／氣壓高度表組合導(dǎo)航系統(tǒng)為例構(gòu)建了因子圖信息融合模型，通過車載跑車試驗采集導(dǎo)航傳感器原始數(shù)據(jù)進行離線仿真，將因子圖融合算法與分散式聯(lián)邦濾波算法進行誤差對比，驗證了因子圖算法作為無人機信息融合方法的可行性，并且該方法在高度定位以及航向角測姿上展現(xiàn)了更高的精度。

針對密集集群無人機的協(xié)同導(dǎo)航問題，文獻[11]提出了一種基于因子圖優(yōu)化的無人機群協(xié)同導(dǎo)航方法，結(jié)合無人機自身的導(dǎo)航信息和無人機之間的測距信息，將無人機群協(xié)同導(dǎo)航信息融合問題轉(zhuǎn)換成因子圖模型，給出了基于消息迭代策略的因子迭代方法，為每架無人機建立局部因子圖，實現(xiàn)了協(xié)同導(dǎo)航信息分布式處理和位置優(yōu)化。仿真試驗結(jié)果表明，所提出的方法可有效提高密集集群等多無人機應(yīng)用場景下的定位精度。

(2)AUV

對于自主水下潛器(AUV)的水下組合導(dǎo)航信息融合技術(shù)，國內(nèi)外眾多研究學(xué)者作了大量的研究，其中的分布式Kalman濾波應(yīng)用最為廣泛。文獻[12]則針對AUV所在水下復(fù)雜多變的環(huán)境與任務(wù)需求提出了基于因子圖的AUV多傳感組合導(dǎo)航算法，對捷聯(lián)慣性導(dǎo)航系統(tǒng)、Doppler計程儀、磁航向儀、地形輔助導(dǎo)航設(shè)備進行建模，構(gòu)建了基于因子圖的信息融合框架。仿真結(jié)果同樣表明，因子圖方法能夠連續(xù)穩(wěn)定地輸出較高精度的導(dǎo)航結(jié)果，有效實現(xiàn)了慣性導(dǎo)航系統(tǒng)與不同導(dǎo)航傳感器的非等間隔融合，且具有很好的靈活性與擴展性。

文獻[13]則提出了一種基于因子圖的多AUV協(xié)同導(dǎo)航定位算法，根據(jù)當前時刻AUV的領(lǐng)航與跟航關(guān)系構(gòu)建相應(yīng)的變量節(jié)點、函數(shù)節(jié)點和因子圖模型。之后，針對因子圖中的消息傳遞形式推導(dǎo)了和積算法，隨后對基于因子圖的多AUV協(xié)同導(dǎo)航定位算法進行描述和推導(dǎo)。最后的仿真結(jié)果表明，算法可以有效估計出AUV的位置信息，且在弱噪聲情況下的定位準確度要優(yōu)于擴展Kalman濾波算法。另外，在大規(guī)模多AUV協(xié)同導(dǎo)航定位系統(tǒng)中增減AUV時只需添加對應(yīng)的局部因子圖模型，因此因子圖方法具有很好的擴展性。

(3)衛(wèi)星拒止條件下的應(yīng)用

目前，幾乎所有軍用和民用系統(tǒng)都依靠基于衛(wèi)星的組合導(dǎo)航系統(tǒng)來獲取導(dǎo)航定位信息。但衛(wèi)星信號在復(fù)雜環(huán)境下容易受到干擾，在室內(nèi)、地下等環(huán)境中衰減嚴重。因此，針對衛(wèi)星導(dǎo)航在一些特定環(huán)境和條件下的局限性，國內(nèi)外學(xué)者均對衛(wèi)星信號拒止環(huán)境下的新導(dǎo)航技術(shù)展開了多項研究，基于因子圖的導(dǎo)航技術(shù)就包括其中。文獻[14]提出了一種基于因子圖協(xié)同定位輔助的單星定位方法，利用多個定位目標之間的高精度測距信息建立協(xié)作因子圖，并利用該協(xié)作因子圖輔助單星定位系統(tǒng)的定位結(jié)果，從而提高定位精度。

文獻[15]定義了IMU、BDS和里程計量測因子后構(gòu)建了基于因子圖的多傳感器融合框架。在跑車實驗中，令五個時間段內(nèi)北斗衛(wèi)星信號丟失，在行駛600s后導(dǎo)航精度依舊可以得到有效保障，衛(wèi)星拒止環(huán)境下可以將剩余傳感器信息快速有效融合，實現(xiàn)了多傳感器的即插即用。

2.4 改進的因子圖算法

當GNSS、OD的測量頻率與系統(tǒng)輸出頻率不同以及存在時延時，文獻[16]構(gòu)建了如圖3所示的因子圖模型。

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

圖3 INS／GNSS／OD存在時延時的因子圖模型Fig.3 Factor graph modelof time delay in the presence of INS／GNSS／OD

文獻[17]提出了如下所示的自適應(yīng)因子

式(8)中，Φ(Λk)為自適應(yīng)可信度調(diào)節(jié)函數(shù)，其值為[0，1]之間的標量。從因子圖拓撲網(wǎng)絡(luò)角度來看，基于可信度的值，可以啟用或禁用與其關(guān)聯(lián)的約束邊，從而實現(xiàn)在拓撲中加入或刪除的操作。構(gòu)建新的自適應(yīng)因子圖模型，如圖4所示。

圖4 自適應(yīng)因子圖模型Fig.4 Diagram of adaptive factor graph model

在仿真實驗中，分別采用傳統(tǒng)因子圖算法和基于可信度的自適應(yīng)因子圖算法與真實軌跡及慣性／衛(wèi)星子系統(tǒng)軌跡對比，尤其通過比較衛(wèi)星拒止區(qū)域的定位精度可以看出：基于可信度的自適應(yīng)因子圖算法能夠自適應(yīng)調(diào)整因子圖拓撲結(jié)構(gòu)，從而保證了復(fù)雜動態(tài)場景下多源融合導(dǎo)航算法的魯棒性和環(huán)境自適應(yīng)性，提升了自主無人系統(tǒng)跨場景導(dǎo)航模式切換和連續(xù)可靠的導(dǎo)航定位能力。

在因子圖算法中，應(yīng)用Bayes推斷進行狀態(tài)量估計的范圍可以是整個因子圖網(wǎng)絡(luò)。但是，遍歷估計的方法使得算法在運行過程中的計算量不斷增大，而且最新得到的觀測值對早期變量估計準確性的提高也并不顯著。因此，通過對因子圖的優(yōu)化范圍加窗，即限定參與優(yōu)化的因子節(jié)點范圍，能夠有效減少算法計算量，提高算法的效率。

滑動窗使得因子圖的全局最優(yōu)化過程僅發(fā)生在最新生成的若干個節(jié)點間，它的引入不僅提高了融合算法的估計速度，而且還減少了算法的計算量。

圖5 基于滑動窗口的因子圖模型Fig.5 Diagram of factor graph model based on sliding window

3 總結(jié)與展望

如今，單一的導(dǎo)航系統(tǒng)已經(jīng)不能滿足對導(dǎo)航精度以及抗干擾能力的需求，組合導(dǎo)航技術(shù)成為載體導(dǎo)航與定位的主流方法[19]。與單個傳感器信息相比，多種傳感器組合信息顯然能夠提供更精確、更具魯棒性的導(dǎo)航結(jié)果[20]?；谝蜃訄D的多傳感器信息融合導(dǎo)航算法可以方便地融合多種傳感器的數(shù)據(jù)，可以滿足傳感器的即插即用和快速配置。從對因子圖算法的可行性進行分析到其在無人機、水下潛器等上的應(yīng)用再到多種改進的因子圖算法的提出，國內(nèi)外已經(jīng)開展了很多關(guān)于因子圖算法的研究，本文也對其進行了一定程度的闡述。但由于理論水平和技術(shù)水平有限，目前的研究仍然存在如下的一些改進空間：

1)因子圖算法中，隨著時間的增長，圖中的變量節(jié)點也逐漸增加，進行計算的時候，數(shù)據(jù)維度也逐漸增加，這需要消耗大量的空間資源和時間成本[21]。因此，需要對此進一步進行研究從而提高導(dǎo)航系統(tǒng)的實時性。

2)目前，基于因子圖的多傳感器融合算法大多都是基于松耦合結(jié)構(gòu)的，建立緊耦合融合框架理論上可以提高導(dǎo)航精度，可以作為接下來的一個發(fā)展方向。

因子圖算法在即插即用等方面的巨大優(yōu)勢注定它存在很大的發(fā)展空間和實用價值[22]，但迄今為止針對因子圖算法的研究仍只停留在實驗仿真階段，尚未應(yīng)用于實際系統(tǒng)，同時研究也存在諸多問題，因此仍需要大量的理論工作和工程實踐。