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

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

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

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

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

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

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

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

1.1 系統(tǒng)模型與算法原理

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

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

設(shè)在ti時刻輸出的定位狀態(tài)量為xi，慣性傳感器偏置量為α1，則tk時刻的狀態(tài)變量節(jié)點的集合為：

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

根據(jù)貝葉斯估計原理，從t0時刻到tk時刻，時間序列中各個節(jié)點狀態(tài)的聯(lián)合后驗概率密度為：

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

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

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

1.2 因子節(jié)點模型

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

1.2.1 DVL因子節(jié)點

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

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

AUV速度誤差和姿態(tài)誤差可以表示為：

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

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

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

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

1.2.2 USBL因子節(jié)點

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

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

SINS遞推得到的載體系下應(yīng)答器的位置矢量為：

AUV位置誤差和姿態(tài)誤差可以表示為：

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

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

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

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

1.2.3 SINS因子節(jié)點

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

1.3 圖模型遞推方法

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

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

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

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

2.1 信息滯后模型

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

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

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

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

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

2.2 算法流程

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

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

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

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

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

3 半實物仿真試驗結(jié)果與性能分析

3.1 半實物仿真條件設(shè)置

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

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

表1 試驗設(shè)備參數(shù)Tab.1 Experimental equipment parameters

圖6 實測實驗現(xiàn)場Fig.6 The experiment site

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

3.2 試驗與分析

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

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

表2 試驗條件設(shè)置Tab.2 The conditions of each test

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

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

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

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

表3 試驗1、2、3定位誤差統(tǒng)計結(jié)果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累積的誤差。與此同時，同等條件下曲線擬合法向外推斷的誤差已經(jīng)達到±10 m，已經(jīng)無法滿足高精度定位需求。綜上，相比于曲線擬合法，因子歷史信息匹配方法在滯后時間較長情況下更優(yōu)越，但長時間的誤差積累也使其絕對精度下降。

表4 試驗1、4、5定位誤差統(tǒng)計結(jié)果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的平面位置誤差統(tǒng)計，位置誤差曲線如圖12所示。針對信息時延且出序的問題，傳統(tǒng)的補償方法在判斷出序信息后通常選擇直接丟棄，等待下一個順序信息到達后再進行更新。與丟棄數(shù)據(jù)法相比較，因子圖匹配法在整體滯后5 s且每2 s發(fā)生一次出序的情況下可以提高19.7%的平面精度。因此，使用因子圖法能夠使出序信息被合理利用，提高信息利用率。

表5 試驗1、6、7定位誤差統(tǒng)計結(jié)果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

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

4 結(jié) 論

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