徐 璟，楊功流，孫藝丁

（北京航空航天大學(xué)儀器科學(xué)與光電工程學(xué)院，北京100191）

0 引言

目前，以GPS為代表的衛(wèi)星導(dǎo)航系統(tǒng)已成為現(xiàn)代軍事、民用導(dǎo)航的重要信息基礎(chǔ)設(shè)施。但是，衛(wèi)星導(dǎo)航信號難以穿透地面和建筑物等密度較大的物質(zhì)，在都市、室內(nèi)、地下等環(huán)境中信號的衰減現(xiàn)象非常嚴重［1］。針對衛(wèi)星導(dǎo)航在一些特定條件和環(huán)境下應(yīng)用的局限性，國內(nèi)外均對衛(wèi)星信號拒止環(huán)境下的新導(dǎo)航技術(shù)開展了多項研究，其中就包括了基于因子圖的導(dǎo)航定位技術(shù)。

2012年，美國Georgia理工學(xué)院的Indelman等針對美國國防高級研究計劃局（DARPA）提出的全源定位導(dǎo)航項目，參照Dellaert［2］等的SAM系列算法和一些平滑定位新算法［3?7］，提出了基于因子圖（Factor Graph，F(xiàn)G）框架的多源導(dǎo)航傳感器融合方法［8?10］。2013 年，美國 SRI 國際公司的 Chiu 等［11］在美國Georgia理工學(xué)院的研究基礎(chǔ)上，針對機器人自主導(dǎo)航問題給出了基于因子圖的組合導(dǎo)航系統(tǒng)，該系統(tǒng)包括57種不同的傳感器。2013年，德國Chemnitz工業(yè)大學(xué)的Lange等［12］將基于因子圖框架的增量平滑多源導(dǎo)航傳感器融合方法應(yīng)用到裝配有慣性測量單元、光流傳感器和無線電高度表的四旋翼無人機中，并與傳統(tǒng)的擴展Kalman濾波（Extended Kalman Filter，EKF）方法及其變體方法作對比，仿真實驗和物理實驗結(jié)果證明基于因子圖的增量平滑方法比傳統(tǒng)濾波方法更具有優(yōu)勢。2015年，西北工業(yè)大學(xué)的戚麗萍［13］開展了基于因子圖的INS/GPS/VO組合導(dǎo)航方法研究，給出了因子圖框架下INS/GPS/VO組合導(dǎo)航系統(tǒng)的算法流程，采用固定滯后平滑優(yōu)化的方法對該組合導(dǎo)航系統(tǒng)進行優(yōu)化求解，并在不同導(dǎo)航數(shù)據(jù)更新頻率和不同導(dǎo)航傳感器組合模式下完成了仿真試驗。

綜上所述，國外已經(jīng)展開了基于因子圖框架的多源數(shù)據(jù)融合方法，并且能夠較好地處理異步、多速率數(shù)據(jù)，但是算法的應(yīng)用背景多以小場景、低動態(tài)的SLAM問題為主，還未在大場景下進行過驗證。國內(nèi)雖然也展開了該技術(shù)的研究，但都以無人機為應(yīng)用背景，且以仿真試驗居多，沒有在實地大場景下進行過驗證。本文利用因子圖框架對不同頻率的傳感器量測信息進行融合，通過因子節(jié)點的增加和刪減來實現(xiàn)多傳感器即插即用，實地車載實驗驗證了所用算法的有效性。

1 因子圖概論

“因子圖”實際上是一種概率圖模型，其應(yīng)用廣泛，不僅可以用來解決數(shù)學(xué)概率問題，還被用來解決人工智能、信號處理、數(shù)字通信等其他領(lǐng)域的復(fù)雜問題［10，14?15］。

因子圖指的是將一個具有多個變量的全局函數(shù)因子分解成幾個局部函數(shù)的積而形成的一個二分圖模型G＝（F，Θ，ε），其中包含變量節(jié)點θj∈Θ、因子節(jié)點 fi∈ F 和連線 eij∈ ε［16］。設(shè) g（θ1，θ2，θ3，θ4）是具有4個變量的全局函數(shù)，將其分解成3個局部函數(shù)f1、f2、f3之積，即

式（1）對應(yīng)的因子圖如圖1所示。在圖1中，每個變量對應(yīng)一個變量節(jié)點，每個局部函數(shù)對應(yīng)一個因子節(jié)點，因子圖的邊線由變量節(jié)點和因子節(jié)點的連線組成。當(dāng)且僅當(dāng)因子圖中變量節(jié)點θj是局部函數(shù)fi的自變量時，它們之間才會有一條連線eij連接。

圖1 因子圖示例Fig.1 Example of factor graph

2 基于因子圖的多傳感器融合設(shè)計

2.1 融和原理

基于因子圖的信息融合算法的基本思想是：通過構(gòu)建某一時間段內(nèi)的因子圖模型，將系統(tǒng)狀態(tài)與導(dǎo)航信息相關(guān)聯(lián)，再基于后驗估計理論實現(xiàn)數(shù)據(jù)的融合。即在給定所有可用量測值后，計算所有狀態(tài)的聯(lián)合概率分布函數(shù)的最大后驗概率估計（MAP）［17?18］，如下所示

因式分解可將 p（X0∶k｜Z1∶k）分解為多個局部函數(shù)之積，如下所示

2.2 各傳感器量測模型及因子

現(xiàn)代組合導(dǎo)航系統(tǒng)中包含有多種傳感器，針對其不同的量測模型需提出不同的因子節(jié)點配置。下面對之后試驗中包含的幾類傳感器列出其因子節(jié)點表達式。

（1）慣組（IMU）量測因子

在慣導(dǎo)系統(tǒng)中，導(dǎo)航狀態(tài)x的時間更新可以用如下方程描述

這種預(yù)測與狀態(tài)xk估計之間的差別就是因子節(jié)點中所代表的誤差函數(shù)，即

由式（8）可以看出，IMU量測因子與相鄰時刻的導(dǎo)航狀態(tài)相關(guān)。

（2）北斗衛(wèi)星導(dǎo)航系統(tǒng)（BDS）量測因子

BDS量測方程可以表示為

式（9）中，nBDS為量測噪聲，hBDS為量測函數(shù)，由上述量測方程可以定義BDS量測因子

由式（10）可以看出，BDS因子只與當(dāng)前時刻的導(dǎo)航狀態(tài)有關(guān)。Baro?altimeter（氣壓高度計）的量測因子可參考BDS量測因子的定義。

（3）里程計量測因子

對各類傳感器的量測因子進行定義后，就可以得到導(dǎo)航系統(tǒng)信息融合的因子圖框架，如圖2所示。其中，IMU、BDS、高度計和里程計的頻率分別為100Hz、1Hz、5Hz和1Hz。

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

由圖2可知，基于因子圖的多傳感器信息融合方法可以對不同頻率、不同線性特征的傳感器數(shù)據(jù)進行融合處理。當(dāng)導(dǎo)航系統(tǒng)接收到新的傳感器輸出數(shù)據(jù)時，在因子圖框架上擴充變量節(jié)點及相應(yīng)的因子節(jié)點。而當(dāng)某傳感器失效時，在因子圖中不再添加該傳感器的量測因子節(jié)點即可。而對于基于傳統(tǒng)Kalman濾波的多源融合方法，當(dāng)導(dǎo)航系統(tǒng)中新加入一種導(dǎo)航傳感器時，濾波器的框架需要進行一定的修改。如聯(lián)邦濾波器的主濾波器中全局估計的協(xié)方差陣需要重新計算，這使得聯(lián)邦濾波器需要一段時間來進行結(jié)構(gòu)的調(diào)整以適應(yīng)新的導(dǎo)航傳感器量測信息輸入。

因此，基于因子圖的多傳感器信息融合方法可以實現(xiàn)導(dǎo)航系統(tǒng)中傳感器信息的即插即用，便于載體在衛(wèi)星拒止環(huán)境下快速有效地進行系統(tǒng)狀態(tài)的更新，保證載體的導(dǎo)航定位精度。

3 試驗驗證

采用實地跑車試驗對文中提出的方法在衛(wèi)星拒止環(huán)境下進行分析驗證，試驗車輛裝有IMU、北斗衛(wèi)星信號接收儀、氣壓高度計和里程計，如圖3所示。各傳感器的更新率、零偏及觀測誤差如表1所示。

表1 傳感器更新率及誤差Table 1 Update frequencies and errors of the sensors

圖3 車載導(dǎo)航系統(tǒng)實物圖Fig.3 Structure of the ground vehicle navigation system

車載試驗全程為27383m，總時長為1359s。由于試驗前半段路程為直線行駛，其結(jié)果不能很好地說明本文提出的方法的有效性。本文選擇含有幾段轉(zhuǎn)彎的后半段跑車試驗來進行分析驗證，時間范圍為750s～1350s。在后半段跑車試驗中，令5個時間段內(nèi)北斗衛(wèi)星信號消失，消失時間段的始末時間如表2所示。

表2 北斗衛(wèi)星信號消失始末時間Table 2 Signal disappearance beginning and end time of the Beidou satellite

跑車試驗結(jié)果在谷歌地圖上的顯示如圖4所示。從局部放大圖中可以看出，在開始的100s內(nèi)，基于因子圖融合的導(dǎo)航路線誤差較明顯。這是因為算法中將載體初始速度設(shè)為靜止，而載體真實速度并不為零所致。但在之后的試驗過程中，因子圖融合路線與真實路線重合度高，其位置誤差曲線如圖5所示。由于東向和北向位置誤差在前100s內(nèi)較大，為了便于觀測，將850s以后的位置誤差結(jié)果顯示在圖5中。

圖4 因子圖融合曲線與真實路線對比圖Fig.4 Trajectories of the proposed method and the ground truth

圖5 基于因子圖的導(dǎo)航系統(tǒng)位置誤差曲線Fig.5 Position error curves of the integrated navigation system

由圖5可知，試驗車輛經(jīng)過600s行駛后，在位置誤差方面，東向和北向的誤差在10m以內(nèi)，高度誤差在6m以內(nèi)?？傮w來說，試驗車輛在導(dǎo)航衛(wèi)星信號變更的情況下，導(dǎo)航精度依舊可以得到有效保障。因此，本文提出的基于因子圖的多傳感器信息融合方法能在衛(wèi)星信號拒止環(huán)境下將剩余傳感器信息進行快速有效融合，實現(xiàn)多傳感器信息即插即用，確保載體的導(dǎo)航精度。

4 結(jié)論

本文針對現(xiàn)有載體導(dǎo)航系統(tǒng)多依賴衛(wèi)星信號，而衛(wèi)星信號在特定環(huán)境中容易失效導(dǎo)致系統(tǒng)導(dǎo)航精度下降的問題，提出了一種基于因子圖的多傳感器信息融合方法。首先介紹了因子圖的原理，并通過各傳感器的量測方程得到導(dǎo)航系統(tǒng)信息融合的因子圖框架。實地跑車試驗結(jié)果表明，該方法能在衛(wèi)星拒止環(huán)境下實現(xiàn)多傳感器信息的快速有效融合，確保載體導(dǎo)航精度。