基于因子圖的AUV多傳感器組合導(dǎo)航算法

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

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

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

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

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

針對以上問題，本文提出了一種基于因子圖的多傳感器組合導(dǎo)航算法，并應(yīng)用于AUV 水下組合導(dǎo)航系統(tǒng)。因子圖作為一種概率圖模型，最早應(yīng)用于編碼領(lǐng)域，目前在人工智能、信號處理、神經(jīng)網(wǎng)絡(luò)等領(lǐng)域也逐漸開始使用因子圖方法來解決一系列復(fù)雜問題。因子圖方法為導(dǎo)航系統(tǒng)的信息融合提供了一個新思路。它可以將可用導(dǎo)航傳感器的量測信息抽象成相應(yīng)的因子節(jié)點，根據(jù)非線性優(yōu)化理論對狀態(tài)變量節(jié)點進(jìn)行遞推與更新，能夠?qū)崿F(xiàn)慣性導(dǎo)航系統(tǒng)與不同導(dǎo)航傳感器的非等間隔融合，滿足不同類型水下導(dǎo)航傳感器的即插即用。

1 因子圖算法

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

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

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

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

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

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

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

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

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

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

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

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

2.1 IMU 因子節(jié)點

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

2.2 DVL 因子節(jié)點

DVL 量測方程可以表示為：

其中，nDVL是量測噪聲，hDVL是量測函數(shù)。

在tm時刻接收到DVL 量測信息后，添加新的因子節(jié)點fDVL。DVL 因子節(jié)點只與變量節(jié)點xm，即tm時刻的導(dǎo)航狀態(tài)相關(guān)。因此，可以將因子節(jié)點fDVL定義為一個一元邊因子：

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

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

目前大多采用無反饋模式的聯(lián)邦卡爾曼濾波算法以提高AUV 組合導(dǎo)航系統(tǒng)的容錯性能[13,15]。以SINS 作為公共參考系統(tǒng)，3 個子濾波器分別為SINS/ DVL、SINS/TAN、SINS/MCP，均采用標(biāo)準(zhǔn)的卡爾曼濾波結(jié)構(gòu)，各子濾波器獨立工作，僅在主濾波器中進(jìn)行信息融合。AUV 多源信息融合聯(lián)邦卡爾曼濾波器結(jié)構(gòu)圖如圖2所示。

圖2 聯(lián)邦卡爾曼濾波器結(jié)構(gòu)圖 Fig.2 Structure of the federal Kalman filter

無反饋模式的聯(lián)邦卡爾曼濾波器能夠有效避免由于某個子濾波器出現(xiàn)故障而污染到其他子濾波器的情況，提高系統(tǒng)的容錯性能。但是當(dāng)某個傳感器故障或失效時，需對其相應(yīng)子濾波器單獨隔離并進(jìn)行系統(tǒng)重構(gòu)，大大降低了其靈活性和擴(kuò)展性。

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

4 試驗結(jié)果與分析

4.1 仿真條件設(shè)置

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

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

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

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

4.2 仿真結(jié)果與分析

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

基于因子圖方法與聯(lián)邦濾波方法的AUV 多源信息導(dǎo)航系統(tǒng)仿真結(jié)果如圖4～6 所示，紅線表示因子圖方法的仿真結(jié)果，藍(lán)線表示聯(lián)邦濾波方法的仿真結(jié)果。

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

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

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

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

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

從表1可以看出，因子圖方法的導(dǎo)航精度與聯(lián)邦卡爾曼濾波方法相當(dāng)，基本處于同一水平，與從曲線中得出的結(jié)論一致。在表1中，緯度與經(jīng)度、北向速度與東向速度均屬于直接觀測量，因子圖法能夠克服異步影響，及時將觀測量引入網(wǎng)絡(luò)中，因而精度稍高。橫滾與俯仰兩者均屬于間接觀測量，兩者精度相當(dāng)；但與航向相比，因航向?qū)儆谥苯佑^測量，因子圖法能夠及時將觀測量引入濾波網(wǎng)絡(luò)，因而精度稍高。

表1 因子圖方法與聯(lián)邦濾波方法的各導(dǎo)航參數(shù) 平均誤差值與均方根誤差值 Tab.1 AME and RMSE of navigation parameters of the factor graph and the federal filter methods

4.3 半物理車載實驗

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

圖7～9所示為基于本文所提方法的AUV多傳感器組合導(dǎo)航跑車試驗誤差結(jié)果。由圖7～9 可知，經(jīng)過車載試驗驗證，基于因子圖的AUV 多傳感器組合導(dǎo)航算法能夠提供準(zhǔn)確的導(dǎo)航信息，各導(dǎo)航參數(shù)誤差均保持在較小的范圍內(nèi)，水平定位誤差在-10～+10 m 以內(nèi)?；谝蜃訄D的AUV 多傳感器組合導(dǎo)航跑車試驗各導(dǎo)航參數(shù)平均誤差值與均方根誤差值如表2所示。根據(jù)表2中的統(tǒng)計結(jié)果，亦可得到類似表1及4.2 節(jié)的結(jié)論。

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

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

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

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

5 結(jié) 論

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