貝葉斯網絡增強型多模型AUV組合導航算法

王 磊，程向紅，冉昌艷，陳紅梅，胡 杰

（1．東南大學儀器科學與工程學院，江蘇南京210096；2．東南大學微慣性儀表與先進導航技術教育部重點實驗室，江蘇南京210096）

貝葉斯網絡增強型多模型AUV組合導航算法

王 磊1，2，程向紅1，2，冉昌艷1，2，陳紅梅1，2，胡 杰1，2

（1．東南大學儀器科學與工程學院，江蘇南京210096；2．東南大學微慣性儀表與先進導航技術教育部重點實驗室，江蘇南京210096）

針對復雜環境下自主水下航行器（autonomous underwater vehicle，AUV）組合導航系統中存在噪聲不確定或者易發生變化的情況，提出一種貝葉斯網絡增強型交互式多模型（interactive multiple model filter based on Bayesian network，BN-IM M）濾波算法。該算法在多模型估計基礎上，引入特征變量，并根據變量與系統模型之間存在的因果關系建立貝葉斯網絡；利用貝葉斯網絡參數修正多模型估計中的模型切換概率，能夠降低多模型算法中真實模式識別對先驗知識的依賴性。該算法能夠解決交互式多模型（interactive multiple model，IMM）算法中模型轉換存在滯后、模型概率易發生跳變等問題，增強多模型算法的自適應能力。以陀螺和加速度計的輸出作為特征變量建立貝葉斯網絡，對AUV組合導航系統進行仿真，結果表明所提出的BN-IM M算法相比于傳統的IMM算法能夠顯著提高機動狀態時模型轉換速度和估計精度。

自主水下航行器；組合導航；多模型估計；貝葉斯網絡

0 引 言

基于混合系統的多模型（multiple model，MM）估計是一種強有力的自適應估計方法，尤其適用于結構或參數易發生變化的系統，在機動目標跟蹤［1］、圖像識別［2］、故障診斷［3］以及組合導航［4］等領域得到了廣泛的研究。其主要思想是在對象和擾動的數學模型不完全確定或者模型變化不確定的情況下，設計多個模型來逼近系統復雜的時變或非線性過程，從而使在建模條件下分析得到的系統性能保持或接近最優。文獻［5］提出了一種以廣義偽貝葉斯算法為框架、具有Markov切換系數的交互式多模型（interactive multiple model，IM M）算法。IM M方法使用一個更好的假設管理技術，兼具了廣義偽貝葉斯（generalized pseudo Bayes，GPB）一階算法（GPB1）計算復雜度低和二階算法（GPB2）估計精度高的優勢，被認為是一種最有效的混合估計方案［6］。

IMM與傳統隨機系統估計方法的主要區別是當前計算模式會隨著模型的Markov鏈轉移，由先驗的Markov轉移概率和量測信息共同確定模型轉移概率，求得的模型轉移概率與估計結果在下一時刻的估計過程中進行輸入交互，進而對下一時刻的狀態估計產生影響。實際應用表明，當系統模式發生變化時，由于濾波系統的慣性使得IM M算法對于實際系統模式切換的辨識有一定的滯后，文獻［7］指出，IMM算法利用先驗信息來確定模型轉移概率，而忽略了系統當前時刻的量測信息，通過先驗信息確定的濾波參數是模式切換與模式未切換情況下的折中。文獻［8］提出了一種利用當前的量測信息在線推導模型轉移概率的方法，能夠有效降低先驗知識不足造成的影響，提高了算法的有效性。實際上，混合系統模式發生變化是事物之間存在因果關系的一種體現，通常的多模型估計方法均能夠用貝葉斯網絡（Bayesian network，BN）進行表示，文獻［9］指出，利用BN的圖形論表示方法可能建立更具廣泛意義的混合系統，從而更加清晰直觀地表征異類信息的關系以及多個特征級別的信息融合問題。

自主水下航行器（autonomous underwater vehicle，AUV）長時間在水下工作，其工作環境中存在很多不確定因素，例如洋流干擾、海水溫度和鹽度變化等。因此，一般采用組合導航的方法來確保自主水下航行器具有較高的可靠性和定位精度［1012］。本文將過程噪聲和觀測噪聲作為模型參數，利用多模型思想對組合導航系統狀態及其噪聲變化進行估計，將陀螺和加速度計的輸出作為特征變量建立BN，利用網絡參數與先驗知識共同確定多模型算法中當前時刻的模型轉移概率，解決IMM算法由于人為先驗知識不足造成的系統模式切換滯后以及估計精度降低等問題，提高AUV機動狀態時組合導航算法估計性能。

1 BN交互式多模型算法

1.1 BN

BN又被稱為貝葉斯因果網，是一種利用概率統計理論為復雜系統、不確定性推理和數據分析提供直觀、緊湊的圖形表示方法。它主要由有向無環圖（directed acyclic graph，DAG）和條件概率分布表兩個部分構成，其中，DAG的節點代表隨機變量，兩個節點之間帶箭頭的連線稱為有向邊，表示隨機變量之間的概率依賴關系，BN更詳細的定義可參考文獻［13］。

設變量集U＝｛X1，X2，…，Xn｝，則BN的聯合概率分布可以表示為每個節點Xi相對于其父節點Pa（Xi）的條件概率分布的乘積，表示為

BN既能夠定性描述網絡中節點之間的概率依賴關系和條件獨立關系，又可以定量的描述節點和其父節點之間的概率依賴程度。因此，BN很適合用于具有時序相關性的復雜系統中，進行多特征建模，可以準確地跟蹤系統的機動性、時變性等。

1.2 多模型估計的圖論方法描述

MM估計和BN均是由貝葉斯理論發展而來，兩者有較好的相容性，利用圖論的概率和方法可以更加清晰地認識二者之間的關系。MM估計可以采用支撐有向圖來表示，系統的模式為支撐有向圖中的頂點，有向圖的邊代表可能的模式切換，每條邊所對應的權重即為模式切換概率。固定結構的MM算法在所有時刻都使用同一個支撐有向圖，變結構的MM算法使用時變的支撐有向圖。

MM估計系統中，事物之間的相關性可以通過在BN中建立共同的隱節點實現。由此構建的多模型估計系統中，模型切換概率體現了系統過去結構與現在結構因果關系的強弱，可以被推廣為BN的網絡參數。該參數不僅可以體現系統結構的突變性，而且可以描述節點之間的相關性、因果關系的強弱。因此，本文把BN與MM估計結合起來，對IM M算法中模型概率更新過程進行改進，使得到的模型概率能夠更加清晰地描述轉換模型之間的內在關系。

1.3 BN-IMM算法

設模型集合為M，共包含r個模型，k時刻的有效模式為mk，動態系統可描述為

式中，xk為狀態向量；zk為觀測向量；Γk為過程噪聲矩陣；Hk為量測矩陣；wk與Ψk分別為過程噪聲和觀測噪聲序列。初始Markov轉移概率πji滿足條件

BN-IMM算法的步驟同IMM算法［7-8］一樣，區別是在模型概率更新過程中引入對貝葉斯概率的更新過程，算法結構如圖1所示，每一個遞推主要由以下4個步驟組成：

步驟1 模型條件重新初始化。假定k－1時刻的匹配模型是mik－1，k時刻的匹配模型為對k－1時刻各濾波器的估計進行混合得到與匹配的濾波器的輸入。

（1）概率混合，由k－1時刻的模型概率與先驗的Markov轉移概率πji進行交互，計算混合概率為

（2）估計混合，對于第j（j＝1，2，…，r）個模型，重新初始化狀態與協方差陣為

圖1 BN-IMM算法結構圖（模型個數r＝3）

步驟2 模型濾波。在獲得新的量測zk之后，利用步驟1計算得到的重新初始化狀態和協方差陣，進行狀態估計更新，可采用卡爾曼、擴展卡爾曼或其他高斯濾波器進行估計。

（1）狀態預測，對于每一個模型濾波器j（j＝1，2，…，r），分別計算

（2）量測預測殘差及其協方差計算，對于每一個模型濾波器j（j＝1，2，…，r），分別計算

（3）濾波更新，對于每一個模型濾波器j（j＝1，2，…，r），分別計算

步驟3 模型概率更新。分為MM模型概率更新和BN概率更新，更新之后求和即為廣義的BN概率。

（1）MM模型概率更新，利用步驟2中所求得的殘差估值εjk、殘差協方差Sjk以及匹配模型混合后的概率進行計算

（2）BN概率更新，通過對BN中變量Xτ（τ＝1，2，…，S）所有父節點Pa（Xτ）的條件概率求積，得到更新的BN概率為

（3）將MM模型概率與BN概率進行加權求和，權值系數為η，并歸一化，得到新的模式切換概率為

步驟4 輸出交互。對各濾波器估計值進行概率加權融合得到輸出結果為

2 BN AUV組合導航系統

在MM估計系統中，模型概率更新步驟隱含著對系統當前模式的辨識，辨識的結果即為模型切換概率，其數值大小體現了模型與真實模式的匹配程度。這種隱含的對系統模式辨識的方法反應速度慢，容易導致模型切換滯后，在模式發生變化時會降低估計精度。因此，本文在MM算法基礎上考慮引入與系統模式存在因果關系的特征變量，通過特征變量直接參與對系統模式的預測。在AUV組合導航系統中，對載體當前機動狀態的準確預測非常有利于導航定位，靜止或勻速運動時傳感器噪聲較??；加速或轉彎機動時則會激發出一些未知的誤差因素，例如機械振動、電磁干擾以及桿臂效應分量等。對于某些誤差因素，可以利用疊加高斯白噪聲的方法進行等效，通過實驗的方法可以找到等效高斯白噪聲的參數。與AUV機動狀態存在聯系的特征變量都可以用來確定載體的機動情況，例如，電機的轉速、推進器的力矩大小以及傳感器輸出等。

本文考慮三軸陀螺、三加速度計的捷聯慣性導航系統（strapdown inertial navigation system，SINS）組合導航系統，以陀螺、加速度計的輸出作為特征變量進行分析，當某個方向上加速度計的輸出超過閾值λe，陀螺輸出未達到閾值λb時，認為載體處于機動較弱的模式2；當某方向上陀螺輸出超過閾值λb時，認為載體處于機動較強的模式3；二者都小于閾值時，認為載體處于比較平穩的狀態模式1，反之，載體處于機動較強的模式3。由此建立如圖2所示的BN，網絡中主要包括5個變量，分別為：加速度計輸出大于閾值λe（E）、陀螺輸出大于閾值λb（B）、模式1（M1）、模式2（M2）、模式3（M3）。每個變量分別具有兩種取值：1和0，分別代表“是”和“否”兩種狀態。

圖2 組合導航系統BN

BN是對組合導航系統蘊含的因果關系的描述，當加速度計輸出大于閾值λe，即事件E發生時，載體處于加速模式，即M2發生的概率為0.96；當陀螺輸出大于閾值λb，即事件B發生時，載體處于轉彎模式，即M3發生的概率為0.98；事件E和B均未發生時，載體處于平穩狀態，即M1發生的概率為0.97。實際系統中，對載體所處機動狀態的辨識可以借助更多特征變量，辨識的結果即為BN概率，按照第1．3節所描述的方法將BN概率引入到多模型估計中，對MM模型概率更新過程進行改進，即可把多模型估計擴展為廣義的BN多模型估計。

3 SINS／DVL／MCP／TAN組合導航

由SINS、多普勒測速儀（Doppler velocity log，DVL）、磁航向儀（magnetic compass，MCP）和地形匹配（terrain aided navigation，TAN）設備構成的AUV組合導航系統如圖3所示，DVL提供AUV在載體坐標系中的速度，MCP提供航向信息，TAN提供位置信息。傳感器信息均輸入到集中BN-IMM濾波器中進行信息融合，得到位置、速度和失準角估計。

圖3 AUV組合導航系統

3.1 狀態方程

以東北天（east-north-up，ENU）坐標系為導航坐標系（n），右前上（right-front-up，RFU）坐標系為載體坐標系（b）。列寫15維的系統狀態方程為

式中，狀態變量

δVE、δVN、δVU為速度誤差；φE、φN、φU為東向、北向和天向失準角；δL、δλ、δh為位置誤差；εbx、εby、εbz為陀螺的常值漂移；為加速度計常值漂移；F（t）為狀態轉移矩陣［14］；W（t）為狀態噪聲，方差為Q（t）。

3.2 系統量測方程

將捷聯慣導輸出與其他傳感器測量之差作為集中濾波器的量測值，其中DVL輸出的速度是在載體坐標系（b）系中得到的，需要變換到導航坐標系（n）中［15-16］。觀測方程可以表示為

式中，VDE、VDN和VDU是由載體坐標系轉換到導航坐標系下的DVL的速度；VSE、VSN和VSU為SINS輸出的速度；φM和φS為MCP和SINS所測得的航向角；LTN、λTN和hTN為TAN所測量的經度、緯度和高度；LSN、λSN和hSN為SINS得到的位置信息；Ψ（t）為觀測噪聲向量，方差為R（t）。觀測矩陣H（t）可表示為

式中，（v×）為速度向量v＝［vEvNvU］的反對稱矩陣，可表示為（v×）＝為東北天坐標系下載體的速度。

4 仿真實驗

將提出的BN-IMM算法應用于AUV組合導航系統，并進行仿真驗證。假設AUV的初始位置為東經118°，北緯32°，高度0 m，初始位置誤差為0 m；初始速度為0 m／s，初始速度誤差為0．1 m／s；SINS初始水平姿態誤差角為6′，航向誤差角為10′。設載體勻速運動時各傳感器誤差為：陀螺的隨機常值漂移為0．03°／h，陀螺隨機游走系數為加速度計常值偏置誤差為2×10－4g，加速度計量測白噪聲標準差為DVL測速誤差的均方根為0．05 m／s；TAN的水平位置均方誤差為10 m，深度測量均方誤差為8 m；MCP偏航角均方誤差為0．3°。SINS的采樣周期為5 ms，DVL、TAN和MCP的采樣周期為1 s。

AUV運動模擬海底典型的“割草機”運動［17］，軌跡如圖4所示，仿真過程中，假設過程噪聲Q和量測噪聲R會隨著載體機動狀態的改變而發生變化，如表1所示。

圖4 AUV航跡仿真曲線

表1 不同狀態時噪聲情況

表1中，Q0和R0分別為｛（0．05 m／s）2，（0．05 m／s）2，（0．05 m／s）2，（10 m）2，（10 m）2，（8 m）2，（0．3°／h）2｝。分別采用IMM算法和本文提出的BN-IMM算法進行濾波估計，采用的模型集包含3個模型，為了清楚地呈現模型轉換的過程，將過程噪聲和量測噪聲分別取值為Q0，R0，3Q0、6R0和6Q0、12R0。Markov轉移概率設為Ωij＝BN-IMM算法中，按照圖2所示方法建立BN，根據陀螺和加速度計輸出來判定AUV所處的運動模式；在進行模型概率更新時，式（17）中MM模型概率與BN概率進行求和的權值系數η設為0.5。

仿真時間為3 600 s，圖5、圖6給出了采用IMM、BN-IMM算法時系統中3個模型的概率，圖7～圖9分別為采用兩種算法得到的姿態（縱搖角P、橫搖角R和航向角H）、速度、位置估計誤差。

圖5 IMM模型概率

圖6 BN-IMM模型概率

圖7 IMM與BN-IMM姿態估計誤差

圖8 IMM與BN-IMM速度估計誤差

圖9 IMM與BN-IMM位置估計誤差

對比圖5、圖6可以看出，IM M算法中模型概率切換出現了滯后現象，模型概率變化幅度很大，模型轉換過程時間較長；BN-IMM算法由于引入了特征變量直接對系統的模式進行辨識，降低了對前一時刻模型概率和觀測量的依賴程度，模型轉換速度快，模型概率變化幅度較小。

從圖7～圖9可以看出，IM M與BN-IMM算法在AUV機動狀態情況下，均能夠很好地進行姿態、速度、位置估計，體現了多模型估計很強的抗干擾能力。在0～220 s，730～1 030 s，2 470～2 770 s，3 380～3 600 s勻速直線運動階段，過程噪聲和量測噪聲較小，兩種算法精度相當；在220～730 s，2 770～3 380 s加速和1 030～2 470 s轉彎機動過程中，由于IMM算法中模型概率變化幅度很大，引起的姿態、速度、位置估計誤差也存在很大的波動。將兩種算法的位置估計誤差進行比較，采用IMM算法得到的經度估計誤差均方差為21 m，最大達到82．5 m；緯度估計誤差均方差為21 m，最大達到72．8 m；高度估計誤差均方差為20 m，最大達到81．6 m。采用BN-IM M算法得到的經度估計誤差為14 m，最大為65．8 m；緯度估計誤差均方差為13．5 m，最大為59．5 m；高度估計誤差均方差為14 m，最大為53．7 m。可以看出，基于BN的BN-IMM算法性能優于IMM算法。

5 結 論

由于BN在描述非線性、時序性以及不確定性等方面具有顯著的優勢，本文將BN與當前多模型估計中應用最為廣泛的IMM算法相結合，利用BN參數修正多模型估計算法中的模型概率，得出一種廣義的BN多模型估計算法。根據AUV組合導航系統載體機動特性，將該算法應用于SINS、DVL、MCP和TAN構成的組合導航系統中，引入與載體機動狀態相關的特征變量并建立BN，提出的BN-IMM算法模型能夠提高AUV機動狀態時模型切換的速度，解決IMM算法中存在的模型切換滯后問題。通過在Matlab環境下仿真，驗證了提出的BN-IMM算法的優越性。

［1］Dunne D，Kirubarajan T．Multiple model multi-Bernoulli filters for manoeuvering targets［J］．IEEE Trans.on Aerospace and Electronic Systems，2013，49（4）：2679- 2692．

［2］Evans J S，Evans R J．Image-enhanced multiple model tracking［J］．Automatica，1999，35（11）：1769- 1786．

［3］Meskin N，Naderi E，Khorasani K．A multiple model-based approach for fault diagnosis of jet engines［J］．IEEE Trans.on Control Systems Technology，2013，21（1）：254- 262．

［4］Oliveira P．MMAE terrain reference navigation for underwater vehicles using PCA［J］．International Journal of Control，2007，80（7）：1008- 1017．

［5］Blom H A P，Bar-Shalom Y．The interacting multiple model algorithm for systems with Markov switching coefficients［J］．IEEE Trans.on Automatic Control，1988，33（8）：780- 783．

［6］Li X R，Jilkov V P，Ru J F，et al．Multiple-model estimation with variable structure part VI：expected-mode augmentation［J］．IEEE Trans.on Aerospace and Electronic systems，2005，41（3），853- 867．

［7］Liang Y，Cheng Y M，Jia Y G，et al．Analysis on the performance and properties of interacting multiple models algorithm［J］．Control Theory and Applications，2001，18（4）：487- 492．（梁彥，程詠梅，賈宇崗，等．交互式多模型算法性能分析［J］．控制理論與應用，2001，18（4）：487- 492．）

［8］Luo X B，Wang H Q，Li X．Interacting multiple model algorithm with adaptive Markov transition probabilities［J］．Journal of Electronics and Information Technology，2005，27（10）：1539 -1541．（羅笑冰，王宏強，黎湘．模型轉移概率自適應的交互式多模型跟蹤算法［J］．電子與信息學報，2005，27（10）：1539- 1541．）

［9］Liang Y，Zhou D H，Pan Q．Multiple model estimation represented by Bayesian networks［C］∥Proc.of the 4th World Congress on Intelligent Control and Automation，2002：863- 866．

［10］Guo Z，Hao Y，Sun F．A new method to improve the maneuver capability of AUV integrated navigation systems［J］．Journal of Computers，2010，5（5）：757- 764．

［11］Thurman E，Riordan J，Toal D．Real-time adaptive control of multiple colocated acoustic sensors for an unmanned underwater vehicle［J］．IEEE Journal of Oceanic Engineering，2013，38（3）：419- 432．

［12］Maki T，Matsuda T，Sakamaki T，et al．Navigation method for underwater vehicles based on mutual acoustical positioning with a single seafloor station［J］．IEEE Journal of Oceanic Engineering，2013，38（1）：167- 177．

［13］Finn V J．Bayesian networks and decision graphs［M］．Springer：Information Science and Statistics，2001．

［14］Li Y，Xu X S，Wu B X．Observable degree of information matching in AUV integrated navigation［J］．Journal of Chinese Inertial Technology，2008，16（5）：589- 594．（李瑤，徐曉蘇，吳炳祥．AUV組合導航系統信息匹配的可觀測度［J］．中國慣性技術學報，2008，16（5）：589- 594．）

［15］Han S，Wang J．A novel initial alignment scheme for low-cost INS aided by GPS for land vehicle applications［J］．Journal of Navigation，2010，63（4），663- 680．

［16］Li W，Wang J，Lu L，et al．A novel scheme for DVL-aided SINSin-motion alignment using UKF techniques［J］．Sensors，2013，13（1），1046- 1063．

［17］Hegren s，Hallingstad O．Model-aided ins with sea current estimation for robust underwater navigation［J］．IEEE Journal of Oceanic Engineering，2011，36（2）：316- 337．

Improved multiple model algorithm based on Bayesian network for AUV integrated navigation

WANG Lei1，2，CHENG Xiang-hong1，2，RAN Chang-yan1，2，CHEN Hong-mei1，2，HU Jie1，2
（1.School of Instrument Science and Engineering，Southeast University，Nanjing 210096，China；2.Key Laboratory of Micro Inertial Instrument and Advanced Navigation，
Southeast University，Nanjing 210096，China）

An improved interactive multiple model filter based on Bayesian network（BN-IMM）is proposed．The aim is to resolve the problem when the noise of the autonomous underwater vehicle（AUV）integrated navigation system in the tough environment is uncertain or time-varying．The proposed algorithm builds a Bayesian network according to the relationship of characteristic variables and the system model．The parameters of the Bayesian network are used to correct the model probabilities in the interactive multiple model（IMM）algorithm which can reduce the dependence to the prior knowledge in the real mode recognition of the system．The proposed method can solve the problems of time lag in model transformation and probability jump in the IMM algorithm．The outputs of gyros and accelerometers are used as characteristic variables to establish the Bayesian network．Simulation results show that the BN-IMM algorithm can improve the model converting speed and the precision of estimation significantly when the AUV is in maneuvering state．

autonomous underwater vehicle（AUV）；integrated navigation；multiple model cstimation；Bayesian network

U 666．1

A

10．3969／j．issn．1001-506X．2015．04．27

王 磊（1984-），男，博士研究生，主要研究方向為多傳感器信息融合技術及其在自主水下航行器中的應用。E-mail：frank_408＠163．com

程向紅（1963 ），通訊作者，女，教授，博士，主要研究方向為慣性導航技術、組合導航系統理論與方法。E-mail：xhcheng＠seu．edu．cn

冉昌艷（1974-），女，博士研究生，主要研究方向為非線性濾波及其在水下導航系統中的應用。E-mail：ranchangyan＠126．com

陳紅梅（1977-），女，博士研究生，主要研究方向為高斯過程回歸與組合導航算法研究。E-mail：ldyichm＠163．com

胡 杰（1977-），男，博士研究生，主要研究方向為捷聯慣性導航技術、自適應濾波。E-mail：hj_student＠163．com

1001-506X（2015）04-0901-06

2014- 01- 06；

2014- 09- 05；網絡優先出版日期：2014- 10- 28。

網絡優先出版地址：http：∥w ww．cnki．net／kcms／detail／11．2422．TN．20141028．1612．007．html

國家自然科學基金（61374215）資助課題