考慮信息滯后的AUV因子圖多源信息融合定位算法

黃紫如，柴洪洲，向民志，李 達，杜禎強，王棟民

（1. 信息工程大學 地理空間信息學院，鄭州 450001；2. 自然資源部海洋測繪重點實驗室，青島266590；3. 天津航海儀器研究所，天津 300131）

海洋已成為人類可持續發展和資源開發的重要領域[1]。自主水下潛航器作為獲取海洋資源和拓展海洋軍事領域的重要工具，保證其成功高效運行的關鍵問題是進行實時精確定位[2]。目前，水下傳感器的種類不斷增多，最常用的自主水下潛航器（Autonomous underwater vehicle, AUV）定位方式是利用捷聯慣導系統（Strapdown Inertial Navigation System, SINS），其狀態參數全面，自主性好且隱蔽性高，但定位誤差會隨著時間積累，需要輔助傳感器進行修正。水下聲學測速與定位技術因其傳輸距離遠、衰減損失小，在現階段水下定位中必不可少[3]，其中多普勒測速儀（Doppler Velocity Log, DVL）為AUV航行提供速度觀測信息，反應敏捷、穩定性好；超短基線（Ultra-Short Base Line, USBL）定位系統使用簡易靈活、可移動范圍大，精度較高，得到了廣泛應用。但聲學數據傳輸速率較低，輸出參數連續性較差，為了增強定位系統的可靠性、提高AUV定位精度，需要融合多源信息，實現優勢互補、互為余度、協調超越。

在水下組合導航方面，現有研究融合了多種傳感器信息進行定位[4-6]。但在復雜環境中，水聲傳播速度慢, 脈沖串和通信數據包發送、接收與處理時間長，易造成傳感器信息延遲到達，對AUV定位的影響不容忽視。針對信息滯后的問題，基于濾波的數據處理方法主要為擬合推估，將滯后數據擬合歸算至當前時刻，作為等效觀測值參與融合[7]，但此類方法會引入推估誤差，無法進行高效完整的信息利用；此外，還可以維持用于儲存歷史狀態的滑動窗口，當滯后信息到達時從窗口原點重新解算，可以無損利用時延信息，但計算負載大大增加。

近年，因子圖方法逐漸興起。因子圖是一種概率圖模型，可以表示隨機變量的聯合概率分布的全局分解，依照各傳感器信息的時間標定義對應時刻的因子節點來擴展因子圖，即可實現傳感器即插即用。馬曉爽等采用因子圖算法融合AUV多傳感器信息進行定位[8]，有效解決了傳感器更新頻率異步的問題?？紤]到因子圖的全局解算特性，可以實時追溯歷史時刻變量節點，能夠高效快速地融合滯后測量信息，高軍強等將因子圖算法應用于GPS/INS組合導航系統中，降低了GPS信息滯后產生的定位誤差[9]。目前尚缺乏利用因子圖優化（Factor graph optimization, FGO）算法處理水下傳感器延遲信息的研究。

綜上，本文提出了一種考慮信息滯后的AUV因子圖多源信息融合定位算法，來處理水下傳感器延遲信息。研究基于USBL/DVL/SINS融合定位的因子圖模型構建及推理方法，提出處理滯后信息的算法策略，并采集水下航行器在海上測試實驗數據進行算法驗證。

1 基于因子圖的多源信息融合方法

1.1 系統模型與算法原理

因子圖是用來表達隨機變量的聯合概率分布的圖模型[10,11]。它由因子節點fi、變量節點xi以及邊組成，邊是指當且僅當因子節點與變量節點相關聯時的連接邊。在構建多源信息融合定位因子圖框架時，首先要將時間序列中的狀態變量建模為變量節點：

其中，x、y、z分別表示北向、東向、地向載體位置變量；vN、vE、vD分別表示北向、東向、地向速度變量；ψ、θ、γ表示三軸姿態變量；εx、εy、εz表示陀螺儀漂移；?x、?y、?z表示加速度計零偏。

設在ti時刻輸出的定位狀態量為xi，慣性傳感器偏置量為α1，則tk時刻的狀態變量節點的集合為：

將系統中各傳感器在ti時刻的測量值表示為則tk時刻系統接收到的所有測量值的集合為：

根據貝葉斯估計原理，從t0時刻到tk時刻，時間序列中各個節點狀態的聯合后驗概率密度為：

其中，P(X0)表示所有變量的初始狀態的先驗信息；表示 SINS狀態轉移模型，分別表示觀測更新模型。根據可用觀測信息，通過最大后驗概率密度（Maximum a Posteriori, MAP）估計獲得時間序列中所有狀態變量的最優估計：

對于服從高斯噪聲分布的測量，我們通常將每一個測量模型P(Xk|Zk)定義為一個因子用誤差函數來表示，其中表示馬氏距離的平方，Σ表示協方差矩陣。于是可以將最優估計問題轉化為非線性最小二乘問題：

至此，可以利用非線性優化方法對式(7)進行一系列的線性近似，解算得到狀態變量的最優估計。

1.2 因子節點模型

由上節可知，構建多源信息融合因子圖框架還需要將各個傳感器觀測信息建模為因子節點。下面分別討論AUV多源信息融合定位中所用傳感器因子模型。

1.2.1 DVL因子節點

基于多普勒效應原理，DVL利用載體移動時發射頻率和反射頻率的不一致性以及其差值與相對速度呈現線性關系的特點，可以長航時提供精確的對地速度測量信息，且信息的時間延遲很小，可以忽略處理。

SINS所估計出載體坐標系下速度如下：

AUV速度誤差和姿態誤差可以表示為：

將式(10)(11)代入式(8)可得：

取其差值作為觀測誤差，觀測方程如下：

為方便描述，設在tk時刻接收到DVL的速度信息時添加 DVL因子，觀測方程為：

線性化目標函數中的雅可比矩陣HDVL為：

1.2.2 USBL因子節點

使用USBL傳感器進行定位時，將基陣安裝在AUV上，應答器安裝在無人水面載體（Unmanned Surface Vehicle, USV）上，USV浮于水面，此時GNSS觀測條件良好，USV可以獲得高精度位置信息。AUV利用基陣發射脈沖到應答器的往返傳播時間與相位差計算斜距與方位角，從而計算出應答器相對基陣坐標系的三維相對位置，以此構建觀測方程對AUV自身位置進行修正[4,12]。

忽略USBL坐標系與載體坐標系間的誤差，設s與p分別為導航坐標系下USV上應答器與AUV上基陣的位置矢量。其中s為己知位置矢量，設r為應答器在載體系下的位置矢量，載體系到導航系的變換矩陣用表示，nUSBL代表USBL系統的位置測量噪聲。可得AUV的位置觀測方程為：

SINS遞推得到的載體系下應答器的位置矢量為：

AUV位置誤差和姿態誤差可以表示為：

將式(11)與式(18)代入式(16)可得：

忽略二階小量的影響，USBL與SINS位置矢量測量值誤差方程可以表示為：

當tk時刻接收到USBL的位置信息，觀測方程可列為其誤差函數可表示為：

線性化目標函數中的雅可比矩陣HUSBL為：

1.2.3 SINS因子節點

因子圖算法中通常使用預積分技術將一段時間內的SINS測量值進行積分后轉化為一個因子，以指定的頻率添加，可以提高計算效率。

1.3 圖模型遞推方法

當接收到各傳感器的測量信息時，定義因子節點并更新因子圖框架，進行變量節點更新。AUV多源信息融合定位因子圖框架如圖1所示，各個時刻的USBL因子與DVL因子通過連接邊與對應狀態變量相關聯，SINS因子連接了前后相鄰的狀態變量，為與程序框架相匹配，將Bias因子單獨列出[13]。圖中虛線部分表示tk時刻新添加到圖中的邊與狀態變量：

圖1 多源信息融合定位因子圖框架Fig.1 Multi-source information fusion location factor graph framework

考慮到解算過程中的效率，若在每次接收到新的測量時執行全體狀態變量的重新優化，會產生計算負載大，無法實現實時解算的問題。實際上，在圖中添加一個新的因子相當于在整體雅可比矩陣中添加一個新行，因此在優化時只有部分變量被更新?；贕TSAM開源庫的iSAM2增量平滑技術[14]利用了此特點，每次更新只優化狀態變量中受到新因子影響的部分節點，在保留歷史變量節點、可以實時添加因子的前提下，實現了更快的解算速度，為靈活處理時延的傳感器信息創造了條件。

2 傳感器滯后信息處理策略

2.1 信息滯后模型

在實際應用場景中，按照應答器的返回信息是否能夠以AUV上的基陣向應答器發射脈沖詢問信號的時間順序依次到達定位系統，可將信息滯后分為信息順序到達和信息出序到達兩種類型[15]。

信息順序到達如圖2所示，應答器發送的通信數據包按照脈沖串播發的前后順序到達AUV。USBL發送脈沖串后，雖然tk+1時刻的返回信號在tk+δ時刻才到達AUV的濾波器，但早于下一次發送的數據包到達，即為測量信息有延遲但沒有出序。

圖2 順序滯后信息Fig.2 Sequential lagging information

信息出序到達如圖3所示，應答器發送的通信數據包未按照順序到達AUV。應答器在tk和tk+1時刻發送的返回信號于tk+δ和tk+1+ε時刻到達AUV，tk+δ時刻晚于tk+1+ε時刻，即為測量信息出序到達。

圖3 出序滯后信息Fig.3 Out-of-sequence lagging information

2.2 算法流程

由上節已知，因子圖算法能夠在一個信息融合時刻處理各信息源不同時刻的測量信息[6]，具有便捷地融合滯后信息的能力。此外，不同于擴展卡爾曼濾波對于順序與出序情況需要分別采用不同的模型，因子圖方法能夠靈活處理兩種類型滯后信息。

利用因子圖算法處理滯后信息的步驟如圖4所示，當接收到傳感器數據時，判斷其是否有效，若無效則予以剔除；信息正常到達時，在因子圖框架中添加對應因子節點與變量節點進行解算；若判斷信息存在時延，則匹配到歷史時刻的變量節點，在該時刻變量節點上添加因子節點進行重新解算。

圖4 因子圖信息滯后算法處理流程Fig.4 The steps of processing lagging information by factor graph algorithm

設USBL測量信息未及時到達，滯后時間為1個歷元，即本應該在tk1-時刻到達的信息在tk時刻才被接收。接收到滯后信息的圖模型如圖5所示，虛線部分表示在接收滯后信息時圖模型增添的節點和連接邊。

圖5 存在信息滯后情況的因子圖框架Fig.5 Factor graph framework with information lag

3 半實物仿真試驗結果與性能分析

3.1 半實物仿真條件設置

在天津塘沽海面實驗中，使用了GNSS、SINS、DVL等多種傳感器，獲取了一艘自主式水下航行器在水面行駛的數據。

為驗證本文所提算法在實際應用中的可用性，提取AUV航行期間的SINS與DVL觀測數據，所使用設備的標稱參數如表1所示。由于缺乏可用的USBL定位數據，本文將GNSS位置觀測信息轉換到載體系下模擬USBL應答器在基陣坐標系下的位置觀測信息，更新頻率為1 Hz，設置0.2%斜距的高斯白噪聲。

表1 試驗設備參數Tab.1 Experimental equipment parameters

圖6 實測實驗現場Fig.6 The experiment site

圖7 AUV軌跡圖Fig.7 The AUV trajectory

3.2 試驗與分析

在以往的研究中，濾波算法常用曲線擬合法來處理順序信息，AUV航行速度較慢，因此運動參數在短時間內變化不大，可以用曲線函數來近似描述載體的位置信息并推估當前測量值。對于出序信息，在保證實時性的前提下，可以選擇丟棄出序數據[11]，簡單易行且計算量小，但大量的數據棄用會降低信息利用率，影響定位精度。

為了分析存在因子圖信息滯后處理算法的性能，比較在不同USBL信息滯后情況下的效果，本文在海測數據的基礎上模擬了順序、出序以及不同滯后時長下的7組試驗進行對比，如表2所示。試驗1為傳感器信息不存在滯后的情況，可以作為比較基準。

表2 試驗條件設置Tab.2 The conditions of each test

首先分析無滯后情況下的USBL/DVL/SINS信息融合算法性能，AUV的高度信息通常由深度計直接提供，因此只需分析平面精度。以GNSS/INS組合導航結果為真值，圖8、9分別為位置與速度誤差曲線，平面北方向與東方向的位置誤差在-2.0～+2.0 m以內，速度誤差在-0.5～+0.5 m/s以內。

圖8 試驗1平面位置誤差曲線Fig.8 Error curves of plane position in experiment 1

圖9 試驗1平面速度誤差曲線Fig.9 Error curves of plane velocity in experiment 1

為定量分析試驗結果，統計實驗1、2、3的位置均方根誤差（RMSE），如表3所示。圖10為試驗1、2、3的位置誤差曲線對比。顯而易見，當USBL信息滯后時間為1 s時，因子圖處理方法獲得了與理想情況下定位結果相當的定位精度，而曲線擬合法雖然能夠保證實時輸出定位結果，但精度有所下降。在USBL信息延遲1 s時，相比于曲線擬合法，因子圖法解算精度提高了12%。

表3 試驗1、2、3定位誤差統計結果Tab.3 Statistics of position error in experiment 1,2,3

圖10 試驗1、2、3平面位置誤差曲線對比Fig.10 Error curves of position error in experiment 1,2,3

表4為試驗1、4、5的位置均方根誤差（RMSE），圖11為試驗1、4、5的位置誤差曲線對比。當USBL信息滯后時間為5 s時，由于SINS因子預積分時間間隔增大，誤差累積導致因子圖方法的定位精度相對于無滯后情況降低41.9%，說明因子圖法雖然能夠完整利用延遲到達的信息，但無法修正測量信息空白期間的SINS累積的誤差。與此同時，同等條件下曲線擬合法向外推斷的誤差已經達到±10 m，已經無法滿足高精度定位需求。綜上，相比于曲線擬合法，因子歷史信息匹配方法在滯后時間較長情況下更優越，但長時間的誤差積累也使其絕對精度下降。

表4 試驗1、4、5定位誤差統計結果Tab.4 Statistics of position error in experiment 1,4,5

圖11 試驗1、4、5平面位置誤差曲線對比Fig.11 Error curves of position error in experiment 1,4,5

再考察在USBL信息出序的情況下因子圖算法的性能。表5為試驗1、6、7的平面位置誤差統計，位置誤差曲線如圖12所示。針對信息時延且出序的問題，傳統的補償方法在判斷出序信息后通常選擇直接丟棄，等待下一個順序信息到達后再進行更新。與丟棄數據法相比較，因子圖匹配法在整體滯后5 s且每2 s發生一次出序的情況下可以提高19.7%的平面精度。因此，使用因子圖法能夠使出序信息被合理利用，提高信息利用率。

表5 試驗1、6、7定位誤差統計結果Tab.5 Statistics of position error in experiment 1,6,7

圖12 試驗1、6、7平面位置誤差曲線對比Fig.12 Error curves of position error in experiment 1, 6, 7

以上分析可見，因子圖算法憑借其可以在當前時刻追溯歷史時刻的傳感器狀態的優勢，當USBL信息滯后時間較短時，可以實現與沒有滯后時相近的導航精度且計算量小，但是當USBL信息滯后時間較長時，修正效果變差但相比于曲線擬合法仍有明顯改進。此外，在處理出序信息的表現上，因子圖法也比棄用數據法解算效果更佳。

4 結 論

本文在考慮了AUV傳感器信息滯后問題的前提下，采用因子圖算法實現了基于SINS/USBL/DVL的多傳感器信息融合定位，提出了對滯后信息的處理策略，并進行了試驗分析并得出如下結論：當滯后傳感器信息順序到達時，相較于曲線擬合法，因子圖匹配歷史信息能夠明顯削弱時延不良影響，在USBL信息延遲1 s時精度提高12%，延遲5 s時精度提高52%，在滯后時間較長情況下更加優越，但長時間的誤差積累使其相對于無滯后情況降低41.9%，可以針對該問題開展進一步研究；當滯后傳感器信息出序到達時，相較于直接丟棄，因子圖法能夠迅速、完整地利用出序信息，提高19.7%的平面精度。綜上，該算法可以有效補償時延導致的精度下降，可以靈活應用于多種條件下的時延問題。