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

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

對(duì)于自主水下航行器（Autonomous Underwater Vehicle,AUV）而言，隨著水下導(dǎo)航傳感器的數(shù)量和種類不斷增加，怎樣有效融合多種導(dǎo)航傳感器提供的量測信息來提高水下組合導(dǎo)航精度一直是國內(nèi)外備受關(guān)注的研究重點(diǎn)[1-2]。國內(nèi)外眾多研究學(xué)者已經(jīng)對(duì)水下組合導(dǎo)航信息融合技術(shù)做了大量的工作，并在導(dǎo)航精度上有了一定的提高。而分布式結(jié)構(gòu)的聯(lián)邦卡爾曼濾波技術(shù)及其各類改進(jìn)方法以其容錯(cuò)性好、計(jì)算量小、設(shè)計(jì)靈活等特點(diǎn)在多源信息融合技術(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ǔ)上，引入了一個(gè)基于奇異值分解的動(dòng)態(tài)信息共享因子，既解決了傳統(tǒng)聯(lián)邦濾波器信息分配因子的盲目性，又減少了計(jì)算量。

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

針對(duì)以上問題，本文提出了一種基于因子圖的多傳感器組合導(dǎo)航算法，并應(yīng)用于AUV 水下組合導(dǎo)航系統(tǒng)。因子圖作為一種概率圖模型，最早應(yīng)用于編碼領(lǐng)域，目前在人工智能、信號(hào)處理、神經(jīng)網(wǎng)絡(luò)等領(lǐng)域也逐漸開始使用因子圖方法來解決一系列復(fù)雜問題。因子圖方法為導(dǎo)航系統(tǒng)的信息融合提供了一個(gè)新思路。它可以將可用導(dǎo)航傳感器的量測信息抽象成相應(yīng)的因子節(jié)點(diǎn)，根據(jù)非線性優(yōu)化理論對(duì)狀態(tài)變量節(jié)點(diǎn)進(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é)點(diǎn)：一類是因子節(jié)點(diǎn)fi∈F，是指因式分解中的局部函數(shù)；另一類是變量節(jié)點(diǎn)x j∈X，是指全局多元函數(shù)中的變量。邊緣eij∈E是指當(dāng)且僅當(dāng)因子節(jié)點(diǎn)fi與狀態(tài)變量 節(jié)點(diǎn)xj相關(guān)時(shí)存在一條連接邊。

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

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

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

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

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

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

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

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

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

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

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

2.1 IMU 因子節(jié)點(diǎn)

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

2.2 DVL 因子節(jié)點(diǎn)

DVL 量測方程可以表示為：

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

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

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

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

目前大多采用無反饋模式的聯(lián)邦卡爾曼濾波算法以提高AUV 組合導(dǎo)航系統(tǒng)的容錯(cuò)性能[13,15]。以SINS 作為公共參考系統(tǒng)，3 個(gè)子濾波器分別為SINS/ DVL、SINS/TAN、SINS/MCP，均采用標(biāo)準(zhǔn)的卡爾曼濾波結(jié)構(gòu)，各子濾波器獨(dú)立工作，僅在主濾波器中進(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)邦卡爾曼濾波器能夠有效避免由于某個(gè)子濾波器出現(xiàn)故障而污染到其他子濾波器的情況，提高系統(tǒng)的容錯(cuò)性能。但是當(dāng)某個(gè)傳感器故障或失效時(shí)，需對(duì)其相應(yīng)子濾波器單獨(dú)隔離并進(jìn)行系統(tǒng)重構(gòu)，大大降低了其靈活性和擴(kuò)展性。

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

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

4.1 仿真條件設(shè)置

對(duì)于AUV 多傳感器組合導(dǎo)航系統(tǒng)而言，慣性組件在實(shí)際工作之前已經(jīng)做了大量的反復(fù)試驗(yàn)，其噪聲統(tǒng)計(jì)特性較為穩(wěn)定，且精確已知。設(shè)置IMU 更新頻率為100 Hz，陀螺儀常值漂移誤差為0.02 (°)/h，角度隨 機(jī)游走系數(shù)為0.01 (°/h)·Hz0.5；加速度計(jì)常值漂移誤差為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ū)域性和階段性的特點(diǎn)，如DVL 因其聲學(xué)工作環(huán)境及回波信號(hào)的特殊性易發(fā)生短時(shí)失效或間歇性失效的情況，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)，抗干擾能力強(qiáng)，因此設(shè)置IMU 信息在整個(gè)仿真時(shí)間段內(nèi)始終有效。

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

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

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

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

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

由圖4～6 可以看出，在整個(gè)仿真時(shí)間段內(nèi)因子圖方法與聯(lián)邦卡爾曼濾波方法始終都能保持輸出相對(duì)較高精度的導(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)，驗(yàn)證了本文所提方法的有效性與可靠性。在傳感器可用性動(dòng)態(tài)改變時(shí)，由于可利用的信息源減少， 與其他時(shí)間段相比，兩者的導(dǎo)航精度都會(huì)有所降低，與本文第3 節(jié)分析一致。

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

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

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

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

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

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

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

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

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

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

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