基于因子圖的AUV多傳感器組合導航算法

馬曉爽，劉錫祥，張同偉，劉賢俊，許廣富

（1.東南大學 儀器科學與工程學院，南京 210096；2.微慣性儀表與先進導航技術教育部重點實驗室，南京 210096；3.國家深海基地管理中心，青島 266237）

對于自主水下航行器（Autonomous Underwater Vehicle,AUV）而言，隨著水下導航傳感器的數量和種類不斷增加，怎樣有效融合多種導航傳感器提供的量測信息來提高水下組合導航精度一直是國內外備受關注的研究重點[1-2]。國內外眾多研究學者已經對水下組合導航信息融合技術做了大量的工作，并在導航精度上有了一定的提高。而分布式結構的聯邦卡爾曼濾波技術及其各類改進方法以其容錯性好、計算量小、設計靈活等特點在多源信息融合技術中應用最為廣泛，尤其是在水下導航領域中[3]。

文獻[4]利用分布式的聯邦卡爾曼濾波技術有效提高了AUV“T1”的導航性能，避免了單一傳感器的局限性。文獻[5]提出了一種基于聯邦濾波的地形輔助捷聯慣導系統并應用于水下航行器中，能夠提高長航導航的精度和可靠性。文獻[6]在聯邦卡爾曼濾波的基礎上，引入了一個基于奇異值分解的動態信息共享因子，既解決了傳統聯邦濾波器信息分配因子的盲目性，又減少了計算量。

然而，各導航傳感器的信息更新頻率通常異步，而聯邦卡爾曼濾波只能等待所有傳感器的量測信息全部到達之后才可以進行信息融合，這無疑會對AUV 組合導航系統的精度和實時性造成影響。而且AUV 水下作業環境復雜多變，外部干擾的統計特性具有不確定性，在不能滿足其假設的前提下，亦會對聯邦卡爾曼濾波的導航性能造成影響。另外，某些導航傳感器通常會由于一些特殊原因出現不活躍甚至失效的情況，或者只在短時間內有信號，這時聯邦卡爾曼濾波方法就會面臨信息故障以及信息源變化后的系統重構等難題。

針對以上問題，本文提出了一種基于因子圖的多傳感器組合導航算法，并應用于AUV 水下組合導航系統。因子圖作為一種概率圖模型，最早應用于編碼領域，目前在人工智能、信號處理、神經網絡等領域也逐漸開始使用因子圖方法來解決一系列復雜問題。因子圖方法為導航系統的信息融合提供了一個新思路。它可以將可用導航傳感器的量測信息抽象成相應的因子節點，根據非線性優化理論對狀態變量節點進行遞推與更新，能夠實現慣性導航系統與不同導航傳感器的非等間隔融合，滿足不同類型水下導航傳感器的即插即用。

1 因子圖算法

因子圖[7-8]是用來表達隨機變量的聯合概率分布的二分圖模型G=(F,X ,E)。它包括兩類節點：一類是因子節點fi∈F，是指因式分解中的局部函數；另一類是變量節點x j∈X，是指全局多元函數中的變量。邊緣eij∈E是指當且僅當因子節點fi與狀態變量 節點xj相關時存在一條連接邊。

那么因子圖G就可以定義為函數f(X)的因式分 解[9-10]：其中X是與因子節點xi有 關的所有變量節點的集合。我們通常將每一個因子f i(xi)表示成一個誤差函數fi(xi) =d(erri(xi,zi))，其 中d(·) 代表相應的代價函數，zi是實際量測值。

我們可以將因子圖的概念引入AUV 多源導航系統的信息融合問題中，用變量節點xi表示AUV 在ti時刻的導航狀態，并定義當前時刻tk的導航狀態集用zi表示ti時刻由不同導航傳感器得到的實際量測信息，并定義當前時刻tk的量測集由此定義得到所有狀態變量和量測的聯合概率密度（Probability Distribution Function,PDF）及其因式分解可表示為：

其中，P(x0)表示所有變量的初始狀態的先驗信息；表示量測模型中變量節點的一個集合；ziIMU是為了區分來自慣性組件IMU 的量測信息，用于描述一個過程模型P(xi|xi-1,ziIMU)。

在聯合概率密度函數P(X k|Zk)的因式分解中，每個因子都代表一個獨立的項，即，

對于高斯噪聲分布，我們可將每一個代表量測模型的因子fi表示成一個誤差函數：

將每一個代表過程模型的因子fi表示成一個誤差函數：

為了根據所有可用量測信息獲得所有狀態變量的最優估計，我們可以通過最大后驗概率密度（Maxi- mum a Posteriori,MAP）估計，將AUV 多傳感器信息問題轉化為一個等價的非線性優化問題。此時，對式(1)取負對數的最小值可以獲得所有狀態變量的最大后驗概率密度估計X*，即：

為了簡單起見，在這里我們略去了之前的先驗信息P(x0)。根據非線性優化理論[12]，通過調整變量Xk使之最小化，那么這個最優估計值Xk*就可以使整個誤差f(X)最小。

2 基于因子圖的AUV 多傳感器信息融合框架

由于AUV 實際工作環境及其自身一些特點，本文采用捷聯慣性導航系統（Strapdown Inertial Navigation System,SINS）、多普勒計程儀（Doppler Velocity Log,DVL）、磁航向儀（Magnetic Compass Pilot,MCP）以及地形輔助導航設備（Terrain Aided Navigation,TAN）作為導航設備來實現AUV 的高精確導航定位[13-14]。水下航行器組合導航系統的高度信息通常采用深度計（Depth Meter,DM）來測量，這是由于慣性導航系統的垂直通道不穩定，而且多普勒計程儀輸出的垂直速度信息也相對不準確，因此本文不對垂直通道作討論。圖1為AUV 多源信息融合的因子圖框架，定義AUV水下組合導航系統在ti時刻的狀態變量xi為變量節點，包括AUV 的失準角、速度誤差、位置誤差以及慣性儀表誤差狀態量，即x=[ΦδVδPε ?]T，將IMU、DVL、MCP、TAN 導航傳感器的量測信息抽象成相對應的因子節點fIMU、fDVL、fMCP、fTAN。圖1中IMU 是SINS 的測量單元，后文對SINS 與IMU 不加以區分。

圖1 基于因子圖的AUV 多源信息融合框架 Fig.1 Framework of AUV multi-source information fusion based on factor graph

2.1 IMU 因子節點

式(5)即為IMU 因子節點的表達式。當在因子圖中添加新的變量節點xk+1時，需要合理的，這個值可以從預測中得到。

2.2 DVL 因子節點

DVL 量測方程可以表示為：

其中，nDVL是量測噪聲，hDVL是量測函數。

在tm時刻接收到DVL 量測信息后，添加新的因子節點fDVL。DVL 因子節點只與變量節點xm，即tm時刻的導航狀態相關。因此，可以將因子節點fDVL定義為一個一元邊因子：

隨著時間的推移，在其他時刻接收到MCP、TAN傳感器的量測信息后，定義因子節點fMCP、fTAN擴展因子圖，根據不同傳感器的量測方程以及利用相應的代價函數進行變量節點的遞推和更新。當傳感器可用性發生改變時，只需在因子圖模型中刪除或加入因子節點，便可以實現AUV 組合導航系統工作模式的無縫切換。

3 基于因子圖與聯邦濾波的融合過程比較分析

目前大多采用無反饋模式的聯邦卡爾曼濾波算法以提高AUV 組合導航系統的容錯性能[13,15]。以SINS 作為公共參考系統，3 個子濾波器分別為SINS/ DVL、SINS/TAN、SINS/MCP，均采用標準的卡爾曼濾波結構，各子濾波器獨立工作，僅在主濾波器中進行信息融合。AUV 多源信息融合聯邦卡爾曼濾波器結構圖如圖2所示。

圖2 聯邦卡爾曼濾波器結構圖 Fig.2 Structure of the federal Kalman filter

無反饋模式的聯邦卡爾曼濾波器能夠有效避免由于某個子濾波器出現故障而污染到其他子濾波器的情況，提高系統的容錯性能。但是當某個傳感器故障或失效時，需對其相應子濾波器單獨隔離并進行系統重構，大大降低了其靈活性和擴展性。

而因子圖方法完全不需要針對傳感器異步問題與可用性時變問題進行調整，每接收一次傳感器量測信息，可將其抽象成因子節點連接到對應的變量節點，當某個傳感器失效時，可拒絕加入該因子節點至因子圖中。比如，DVL 信號失效時，AUV 組合導航系統由SINS/DVL/TAN/MCP 工作模式切換為SINS/TAN/MCP工作模式，此時因子圖模型刪除了因子節點fDVL，速度誤差得不到修正，其精度會降低，從而導致位置誤差精度也有所下降；而DVL 信號重新有效時，AUV 組合導航系統由 SINS/TAN/MCP 工作模式切換為SINS/DVL/TAN/MCP 工作模式，此時因子節點fDVL又被重新添加進因子圖中，速度誤差將會得到修正。其他傳感器與DVL 情況類似，在這里不再贅述。因此基于因子圖的方法可以簡單地通過增減對應導航傳感器的因子節點實現即插即用功能，與聯邦濾波算法相比，因子圖算法具有更好的靈活性和可擴展性。

4 試驗結果與分析

4.1 仿真條件設置

對于AUV 多傳感器組合導航系統而言，慣性組件在實際工作之前已經做了大量的反復試驗，其噪聲統計特性較為穩定，且精確已知。設置IMU 更新頻率為100 Hz，陀螺儀常值漂移誤差為0.02 (°)/h，角度隨 機游走系數為0.01 (°/h)·Hz0.5；加速度計常值漂移誤差為100 μg，速度隨機游走系數為10 (μg/Hz)0.5。其他傳感器的更新頻率一般低于IMU 更新頻率，且異步，設置DVL 更新頻率為2 Hz，量測噪聲為幅值0.1 m/s 的高斯白噪聲；TAN 更新頻率為0.2 Hz，量測噪聲為幅值10 m 的高斯白噪聲；MCP 更新頻率為1 Hz，量測噪聲為幅值5°的高斯白噪聲。

由于AUV 各導航傳感器在復雜水下環境存在區域性和階段性的特點，如DVL 因其聲學工作環境及回波信號的特殊性易發生短時失效或間歇性失效的情況，MCP 精度不高且易收到環境干擾，TAN 地形匹配定位數據不能大范圍覆蓋，因此設置各傳感器失效區間如下：DVL 失效區間1000～1400 s；TAN 失效區間2000～2600 s；MCP 失效區間700～900 s；SINS 作為一種自主工作的導航系統，抗干擾能力強，因此設置IMU 信息在整個仿真時間段內始終有效。

模擬AUV 在海底作業的三維真實運動軌跡如圖3所示，包括直行、轉彎等機動動作，仿真總時長為3000 s。

圖3 AUV 海底作業運動軌跡 Fig.3 Trajectory of AUV underwater mission

4.2 仿真結果與分析

為了驗證本文所提理論方法在傳感器信息更新頻率異步以及傳感器可用性時變情況下的有效性與可靠性，以上述條件為基礎對聯邦濾波算法和因子圖算法分別進行數值仿真實驗。

基于因子圖方法與聯邦濾波方法的AUV 多源信息導航系統仿真結果如圖4～6 所示，紅線表示因子圖方法的仿真結果，藍線表示聯邦濾波方法的仿真結果。

由圖4～6 可以看出，在整個仿真時間段內因子圖方法與聯邦卡爾曼濾波方法始終都能保持輸出相對較高精度的導航結果，各導航參數誤差都能保持較好的穩定性，其中，姿態角誤差在-20′～+20′以內，水平速度誤差在-0.1～+0.1 m/s 以內，水平定位誤差在-5～+5 m以內，驗證了本文所提方法的有效性與可靠性。在傳感器可用性動態改變時，由于可利用的信息源減少， 與其他時間段相比，兩者的導航精度都會有所降低，與本文第3 節分析一致。

圖4 因子圖方法與聯邦濾波方法的姿態誤差角對比曲線 Fig.4 Comparison on attitude errors between factor graph and federal filter method

圖5 因子圖方法與聯邦濾波方法的速度誤差對比曲線 Fig.5 Comparison on velocity errors between factor graph and federal filter method

圖6 因子圖方法與聯邦濾波方法的位置誤差對比曲線 Fig.6 Comparison on position errors between factor graph and federal filter method

為了定量地分析各導航參數誤差，分別計算因子圖方法與聯邦濾波方法的各導航參數的絕對誤差平均值（Absolute Mean Error,AME）與均方根誤差值（Root Mean Square Error,RMSE）如表1所示。

從表1可以看出，因子圖方法的導航精度與聯邦卡爾曼濾波方法相當，基本處于同一水平，與從曲線中得出的結論一致。在表1中，緯度與經度、北向速度與東向速度均屬于直接觀測量，因子圖法能夠克服異步影響，及時將觀測量引入網絡中，因而精度稍高。橫滾與俯仰兩者均屬于間接觀測量，兩者精度相當；但與航向相比，因航向屬于直接觀測量，因子圖法能夠及時將觀測量引入濾波網絡，因而精度稍高。

表1 因子圖方法與聯邦濾波方法的各導航參數 平均誤差值與均方根誤差值 Tab.1 AME and RMSE of navigation parameters of the factor graph and the federal filter methods

4.3 半物理車載實驗

為了驗證本文所述基于因子圖的AUV 多傳感器組合導航算法在實際工程應用中的有效性與可靠性，利用跑車試驗對其進行驗證。以法國IXBLUE 公司研制的PHINS 和NovAtel 公司研制的FlexPark6 接收機進行SINS/GNSS 松組合所提供的姿態、速度、位置信息作為導航參考數據，輸出頻率為200 Hz。在其航向角的基礎上增加幅值為5°的高斯白噪聲來模擬MCP量測數據，采樣時間間隔為0.1 s。利用其姿態角和速度得到PHINS 在載體坐標系下的速度，在此基礎上增加幅值為0.1 m/s 的高斯白噪聲來模擬DVL 量測數據，采樣時間間隔為0.3 s。在其位置信息的基礎上增加10 m 的高斯白噪聲來模擬TAN 量測數據，采樣時間間隔為7 s。

圖7～9所示為基于本文所提方法的AUV多傳感器組合導航跑車試驗誤差結果。由圖7～9 可知，經過車載試驗驗證，基于因子圖的AUV 多傳感器組合導航算法能夠提供準確的導航信息，各導航參數誤差均保持在較小的范圍內，水平定位誤差在-10～+10 m 以內。基于因子圖的AUV 多傳感器組合導航跑車試驗各導航參數平均誤差值與均方根誤差值如表2所示。根據表2中的統計結果，亦可得到類似表1及4.2 節的結論。

圖7 AUV 多傳感器組合導航跑車試驗姿態角誤差 Fig.7 Attitude errors of AUV multi-sensor integrated navigation in vehicle test

圖8 AUV 多傳感器組合導航跑車試驗速度誤差 Fig.8 Velocity errors of AUV multi-sensor integrated navigation in vehicle test

圖9 AUV 多傳感器組合導航跑車試驗位置誤差 Fig.9 Position errors of AUV multi-sensor integrated navigation in vehicle test

表2 AUV 多傳感器組合導航跑車試驗各導航參數平均誤差值與均方根誤差值 Tab.2 AME and RMSE of navigation parameters of AUV multi-sensor integrated navigation in vehicle test

5 結 論

本文提出了基于因子圖的AUV 多傳感器組合導航算法，實現了SINS/DVL/TAN/MCP 水下組合導航系統的信息融合。該方法將導航狀態誤差量抽象成變量節點，各導航傳感器的量測信息抽象成因子節點，根據非線性優化理論實現變量節點的遞推與更新。仿真結果表明，基于因子圖的AUV 多傳感器組合導航算法能夠連續穩定地輸出較高精度的導航結果，有效解決慣性導航系統與不同導航傳感器的非等間隔融合與傳感器可用性動態改變問題，與聯邦卡爾曼濾波算法的導航解算精度在同一水平上，但本文所提的因子圖方法具有更好的靈活性和擴展性。半物理實驗表明，在實際工作系統中，本文所提方案具有較高的可靠性和有效性。