基于因子圖的衛星拒止環境下的多信息融合技術

徐 璟，楊功流，孫藝丁

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

0 引言

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

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

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

1 因子圖概論

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

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

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

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

2 基于因子圖的多傳感器融合設計

2.1 融和原理

基于因子圖的信息融合算法的基本思想是：通過構建某一時間段內的因子圖模型，將系統狀態與導航信息相關聯，再基于后驗估計理論實現數據的融合。即在給定所有可用量測值后，計算所有狀態的聯合概率分布函數的最大后驗概率估計（MAP）［17?18］，如下所示

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

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

現代組合導航系統中包含有多種傳感器，針對其不同的量測模型需提出不同的因子節點配置。下面對之后試驗中包含的幾類傳感器列出其因子節點表達式。

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

在慣導系統中，導航狀態x的時間更新可以用如下方程描述

這種預測與狀態xk估計之間的差別就是因子節點中所代表的誤差函數，即

由式（8）可以看出，IMU量測因子與相鄰時刻的導航狀態相關。

（2）北斗衛星導航系統（BDS）量測因子

BDS量測方程可以表示為

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

由式（10）可以看出，BDS因子只與當前時刻的導航狀態有關。Baro?altimeter（氣壓高度計）的量測因子可參考BDS量測因子的定義。

（3）里程計量測因子

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

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

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

因此，基于因子圖的多傳感器信息融合方法可以實現導航系統中傳感器信息的即插即用，便于載體在衛星拒止環境下快速有效地進行系統狀態的更新，保證載體的導航定位精度。

3 試驗驗證

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

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

圖3 車載導航系統實物圖Fig.3 Structure of the ground vehicle navigation system

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

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

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

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

圖5 基于因子圖的導航系統位置誤差曲線Fig.5 Position error curves of the integrated navigation system

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

4 結論

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