量測野值與波束故障條件下SINS/DVL緊組合導航方法

徐 博，王連釗，李盛新

（哈爾濱工程大學 智能科學與工程學院，哈爾濱 150001）

復雜的水下環(huán)境給航行器長時間、高精度導航帶來了巨大挑戰(zhàn)，在某些特殊應用領域又要求水下導航系統(tǒng)具有隱蔽性和自主性。捷聯(lián)慣性導航系統(tǒng)（Strapdown Inertial Navigation System, SINS）因其可靠性高、自主性強，能夠提供姿態(tài)、速度、位置等多維度導航參數(shù)被廣泛應用于水下導航領域，但是其特殊的導航原理決定著導航參數(shù)的誤差會隨著時間累積，這一特點限制了慣性導航系統(tǒng)長時間的應用。由于水下環(huán)境電磁信號衰減嚴重，因此，SINS/全球衛(wèi)星導航系統(tǒng)（Global Navigation Satellite System, GNSS）組合導航系統(tǒng)在水下導航難以實現(xiàn)[1]。相對電磁信號，聲波在水下環(huán)境中具有衰減小、傳播范圍廣等優(yōu)勢，因此聲學導航設備在水下環(huán)境得到大量應用。但基于聲學基線的水下定位系統(tǒng)應用條件苛刻，需要在附近水面或海面設置接收應答器[2]，自主性較差。多普勒計程儀（Doppler Velocity Log, DVL）可以利用發(fā)射信號與接收信號之間的多普勒頻移測量載體的速度，具備相對穩(wěn)定的誤差特性，且不受工作范圍約束、自主性強、可靠性高。因此，DVL與SINS構成的SINS/DVL組合導航系統(tǒng)被廣泛應用于水下導航領域[3,4]。

傳統(tǒng)的水下組合定位方式是基于羅經(jīng)和DVL的航位推算方法，因為其沒有充分考慮各子導航系統(tǒng)的誤差，所以通常定位精度較低[5]。SINS/DVL組合導航都采用基于導航系下速度匹配方式，?yvind Hegren?s等對AUV水下動力學模型進行分析簡化，提出了水下航行器模型輔助SINS/DVL組合導航系統(tǒng)的方案，利用3自由度的AUV動力學模型對DVL輸出速度進行改善[6-8]，并證明了所提出的方法，提高了水下導航能力。Asaf T. 等人從Janus配置構型的原理出發(fā)，直接采用4個DVL波束的測量信息作為匹配量[9-11]，構建了SINS/DVL緊組合模型。?yvind Hegren?s在新型DVL驗證中提到對每一個波束信息進行校正補償可以提高測速精度。Xu等人對SINS/DVL緊組合系統(tǒng)做出研究，通過對四波束DVL模型分析提出了DVL波束出現(xiàn)故障情況下的解決方案[12]，但所提出的波束重構方案并未考慮船舶顛簸、安裝偏差角等引起的波束構造誤差。

以上都是通過完善模型的方式提高導航精度的方案，所提解決方案無法對誤差量精準地建模，同時會引入新的速度量測噪聲等問題。長時間的先驗信息與噪聲統(tǒng)計特性不匹配將導致組合導航結果精度變低[13]，針對水下導航過程中DVL測量異?；蚬收锨闆r，單一的波束重構方案是不夠的，還應該從信息融合方面出發(fā)，對帶有變化測量噪聲的速度信息進行合理的融合利用。為應對不確定干擾等因素，有學者提出了魯棒濾波算法，其核心為以下三個方面：1、在假設模型下估計值最優(yōu)或接近最優(yōu)；2、實際模型與假設模型有微小差異時，其估計值或統(tǒng)計參數(shù)值所受影響較??；3、當實際模型與假設模型有嚴重偏離時，其估計結果不嚴重偏離。

Huber魯棒濾波器是基于廣義極大似然估計提出的一種魯棒濾波器[14,15]，文章[15]采用Huber濾波器信息融合方法提高了復雜環(huán)境下移動機器人的定位精度。朱兵等針對水下導航中DVL量測信息受混合高斯分布和高強度野值污染的問題提出了一種基于馬氏距離算法的Huber魯棒自適應算法[16]。但以上改進算法都是在傳統(tǒng)Huber算法的基礎上，調(diào)節(jié)因子是狀態(tài)殘差的函數(shù)，先驗估計的準確程度將一定程度上影響最終的估計精度。

借助魯棒濾波的思想，本文提出了一種適用于SINS/DVL緊組合的改進Huber魯棒濾波算法。不同于現(xiàn)有濾波方法，所提出的魯棒濾波算法在屏蔽大粗差的同時，還對量測噪聲分布特性進行估計。首先，根據(jù)四波束DVL工作特點建立了SINS/DVL緊組合模型，針對DVL波束故障提出了故障檢測及故障處理方案；然后，針對DVL量測誤差及系統(tǒng)誤差異常情況提出了適用于SINS/DVL緊組合的基于改進Huber的魯棒濾波方法；所提出方法通過了水面實驗驗證。

1 SINS/DVL緊組合導航系統(tǒng)模型及容錯處理

現(xiàn)被廣泛應用的SINS/DVL松組合方式是建立在利用DVL輸出載體系速度的基礎上，而在實際中，DVL是通過獲取波束方向的頻移及速度信息后進一步計算獲取的載體坐標系的3維度速度信息。對于四波束配置的DVL，其相當于對信息進行了降維度利用，這樣的方案可以在一定程度上抑制載體動態(tài)引起的測量誤差，在做航位推算時具有較好的優(yōu)勢，但是沒有充分使用所有可利用信息。

在復雜的應用環(huán)境下，DVL也會存在測速異常等故障情況[17]：

1、當航行器航行在溝渠上時，海底與航行載體之間的距離超過DVL發(fā)射聲波的測量范圍，波束測量的速度信息包含對水速度。

2、當航行器航行過海洋生物時，DVL發(fā)出的聲波受到其遮擋，無法完全到達海底，受遮擋回波不能準確反映航行器速度信息。

3、當水下機器人執(zhí)行大角度運動時，在橫滾和俯仰的情況下，DVL可能會中斷。

在松組合中，若DVL波束方向測速出現(xiàn)故障將直接引起DVL速度信息的失效，單一波束測速的噪聲特性變化也將直接引起量測噪聲誤差特性的變化。合理的處理方式能給導航精度帶來較低的損失，本文針對以上DVL輔助SINS在復雜水下環(huán)境出現(xiàn)故障及測速異常導致導航精度下降的問題，提出SINS/DVL緊組合導航系統(tǒng)故障處理方法，為特殊情況下提高組合系統(tǒng)的導航精度提供了新的思路，SINS/DVL緊組合導航系統(tǒng)信息流程圖如圖2所示。

圖2 SINS/DVL緊組合導航系統(tǒng)信息流程圖Fig.2 Information flow chart of SINS/DVL deeply integrated system

1.1 SINS/DVL緊組合系統(tǒng)量測模型

基于波束信息匹配的SINS/DVL緊組合模型更充分地利用了DVL測量信息，因此具有更良好的可靠性。SINS/DVL緊組合仍然采用SINS導航參數(shù)誤差作為狀態(tài)量，與松組合相比，僅在量測方程上存在區(qū)別，本文也只對SINS/DVL緊組合系統(tǒng)量測方程進行介紹?？紤]DVL四波束Janus配置結構如圖3所示。

圖3 四波束Janus陣列配置結構Fig.3 Four beam Janus array configuration

忽略刻度系數(shù)誤差及安裝偏差角影響，載體坐標系三維速度和每個波束方向速度的關系為：

其中，α是由DVL結構決定的固定波束發(fā)射角度，β是四波數(shù)Janus配置安裝角，“+”配置時為0 °，“×”配置時為45 °，C為波束方向矩陣，表示由波束方向測量速度構成的速度矢量信息。緊組合方法即利用波束方向的測速信息作為量測量，對SINS誤差狀態(tài)進行估計。因為SINS輸出導航坐標系下的速度，需要利用慣性測速信息構造DVL波束方向速度信息，根據(jù)SINS輸出導航參數(shù)有：

以DVL輸出速度為匹配量的SINS/DVL緊組合量測方程如下：

1.2 SINS/DVL緊組合導航系統(tǒng)故障診斷及容錯處理技術

1) 基于新息的χ2故障檢測

根據(jù)四波束Janus陣列結構有對稱性，定義測速固有誤差公式來量化評估底部回波信號的質(zhì)量，通常也用來判斷測量速度的有效性。以載體前向為起點逆時針定義四個波束方向分別為beam1、beam2、beam3、beam4，測速誤差為ve，則測速誤差公式可以表示為：

其中，Vbeami表示第i個波束方向上測得的速度，從四波束Janus陣列結構配置可以看出，以上公式適用于“+”或“×”型配置方式。通常DVL在式(5)基礎上設定有效閾值Vth對故障情況進行判斷，若ve趨近于零則認為回波有效；若ve大于Vth則認為回波信號不理想，測得速度不可用；當DVL測得速度為對水速度時，ve反應了洋流場的不均勻程度。在本文中，不僅要對故障情況進行判斷，還要求檢測出故障波束，以針對不同故障波束實施不同的應對方案，因此采用基于新息的χ2檢驗方法對故障波束進行確定。

設SINS/DVL緊組合導航系統(tǒng)離散化系統(tǒng)方程和量測方程如下：

SINS/DVL緊組合系統(tǒng)卡爾曼濾波殘差為[18]：

2) SINS/DVL緊組合導航容錯處理方案

由式(1)可以看出當采用DVL四波束測速作為匹配信息時存在冗余信息，即三個波束獲得有效測量數(shù)據(jù)時就可以辨識出載體系下的三維速度矢量，以下將分別為不同的故障環(huán)境提供不同的解決方案，以提高在波束出現(xiàn)故障條件下的SINS/DVL緊組合導航系統(tǒng)導航精度。

a. 單波束故障

在單個波束故障時，故障波束測速信息可以等價為其他三波束測速信息的線性組合，可以重構故障波束方向測速Vres為：

i、j表示與故障波束相鄰的波束，k為與故障波束構成對角線關系的波束。通過式(9)方式既可以完成故障量測信息的重構，且無需對組合系統(tǒng)進行調(diào)整就可以應對單波束故障情況，同時可以發(fā)現(xiàn)這一方法可以適用于四波束DVL的“+”和“×”型配置方式。

b. 多波束故障

在多波束故障時，有效波束個數(shù)低于波束方向矩陣C的秩，故障波束速度信息不能準確通過有效波束速度信息構造，在這種情況下需要結合載體運動約束來構造故障波束信息，現(xiàn)有提出的解決方案多是基于DVL波束Janus結構提出的直接速度重構[10-12],這些方法都忽略了載體的運動狀態(tài)，直接采用替換方式對波束速度信息進行重構，這是十分不合理的，因為DVL和SINS之間存在著安裝誤差角，同時載體也不能保持水平姿態(tài)為零，載體天向運動約束不等于DVL的垂向速度為零。當載體存在2 °傾角時，以上基于載體運動約束的方法將造成構造波束存在6.98%vcosα的速度誤差，v表示載體合速度。本文同樣以載體運動約束出發(fā)，將載體速度誤差視為約束對象，即認為SINS天向速度誤差為零，由SINS的速度推導出重構波束的速度信息，在一定程度上提高了重構速度的精度。

當一個波束方向速度信息有效時，波束方向矩陣C(i, :),(i? {1,2,3,4})的秩等于1，需要同時引入兩個維度的SINS速度信息才能重構所有的波束速度，考慮載體在垂向及側(cè)向速度較小，所造成側(cè)向速度誤差及垂向速度誤差也較小，所以通常情況下僅采用引入來重構四波束DVL速度，而在“+”型Janus配置結構波束2或4波束方向信息有效時，此時有效波束信息中不包含載體前向速度信息，故需引入進行波束速度信息重構，由于分類比較多，詳細的方案不具體給出。

綜合以上分析，可得四波束Janus配置波束故障處理方案如圖4所示，其中不同顏色的線型描述了不同重構方案及所用的SINS速度信息。

圖4 四波束Janus配置波束故障處理方案Fig.4 Janus configuration beam fault processing scheme

由于引入慣性速度會存在一定誤差，以上通過重構的速度信息不能準確反映真實值，同時引入越高維數(shù)的SINS速度重構速度的準確性越低，即波束故障數(shù)量越多，組合精度也會越低，這一點在后面實驗中得到了驗證。因此，在重構測速信息的同時應適當調(diào)節(jié)相應量測對應的噪聲先驗信息，以降低構造測速誤差對導航精度的影響。

1.3 基于改進Huber的SINS/DVL魯棒濾波算法

組合導航精度一定程度上依賴于系統(tǒng)及量測噪聲先驗信息。在本文中，量測噪聲不能滿足標準的零均值高斯分布，主要體現(xiàn)為：1、量測設備出現(xiàn)故障時，我們所提出的故障處理方案會在一定程度引入量測噪聲；2、量測信息出現(xiàn)干擾噪聲（野值）引起量測噪聲特性變化。在以上情況下，基于先驗信息的高斯噪聲描述模型將不能很準確地反映實際狀態(tài)下的噪聲特性，最終導致組合導航系統(tǒng)的精度下降。為此，本文提出一種基于改進Huber魯棒濾波算法，在消除量測及狀態(tài)粗差影響的同時對誤差協(xié)方差矩陣進行估計，使誤差協(xié)方差矩陣與噪聲統(tǒng)計特性相匹配，以提高長時間組合導航中系統(tǒng)的魯棒性能。

現(xiàn)有的魯棒估計是基于加權最小二乘準則，在式(6)模型基礎上，考慮觀測向量Zk和狀態(tài)預測向量均服從污染分布，則根據(jù)加權最小二乘概念有：

其中，ρ(*)為核函數(shù)，不同的核函數(shù)反映了魯棒濾波的不同準則，一般要求具有對稱、連續(xù)、嚴凸性質(zhì)。

狀態(tài)Xk的魯棒濾波估計即滿足以上公式的極值條件的解，式(14)兩邊對Xk求導得：

特別的，當核函數(shù)為狀態(tài)向量的二次函數(shù)，魯棒濾波器退化為常規(guī)卡爾曼濾波。當核函數(shù)為高斯核函數(shù)，魯棒濾波器為最大熵卡爾曼濾波器[19]。本文在Huang[20]所描述的統(tǒng)計相似度的思想基礎上提出基于Huber權函數(shù)的魯棒濾波方法。

在式(15)基礎上定義權函數(shù)如下：

其中魯棒濾波初始狀態(tài)和狀態(tài)預測協(xié)方差為：

魯棒濾波量測誤差協(xié)方差為：

對式(24)整理可得：

利用同樣的方式對式(22)求期望可得：

魯棒濾波的狀態(tài)估計及狀態(tài)估計誤差協(xié)方差為：

本文所提出魯棒濾波器狀態(tài)估計需要對狀態(tài)預測及觀測異常同時做出判斷，因此觀測及狀態(tài)預測的異常都容易引起狀態(tài)估計的歪曲及等價權重歪曲等結果。考慮在組合導航系統(tǒng)中異常噪聲多出現(xiàn)在量測信息中，利用分步迭代方式來解決魯棒濾波器狀態(tài)估計中容易造成誤判的問題，所提魯棒濾波器分布迭代解方案過程如下所示：

?

?

以上方案中M,N分別表示觀測量和系統(tǒng)狀態(tài)維度，N1,N2分別表示魯棒濾波器狀態(tài)濾波迭代次數(shù)和量測濾波迭代次數(shù)，文章中未對迭代次數(shù)最優(yōu)值進行分析，故兩者都設置為固定值50次。右上標m1,m2分別表示當前矩陣或向量狀態(tài)濾波循環(huán)迭代次數(shù)和量測濾波迭代次數(shù)。

2 仿真分析

2.1 實驗情況概述

受條件所限，本文利用湖面試驗采集數(shù)據(jù)對本方案進行驗證。船上配備了光纖SINS、法國光纖捷聯(lián)慣性導航系統(tǒng)（PHINS）、DVL和GPS，其中PHINS與GPS組合系統(tǒng)（PHINS/GPS）作為導航基準用來評價SINS與DVL緊組合導航系統(tǒng)的精度，由于試驗在湖面進行，實驗設備中沒有水下壓力傳感器，為此我們對組合系統(tǒng)的高度通道進行阻尼處理。實驗所用船只、試驗用具及設備如圖5所示，試驗主要傳感器參數(shù)如表1所示。

表1 主要傳感器參數(shù)Tab.1 Mainsensor parameters

圖5 試驗船只及實驗設備圖Fig.5 Test vessel and equipment diagram

試驗前，SINS和DVL之間安裝偏差角及刻度系數(shù)誤差已通過文獻[21]所提方法進行標定，由于試驗中環(huán)境較為良好，沒有出現(xiàn)DVL量測異常情況，為了驗證所提方案的有效性，我們在DVL測量數(shù)據(jù)中加入包括量測野值及波束故障兩種干擾，在10000s至20000s區(qū)間以10%概率在波束測量速度信息中添加標準差為10m/s的速度量測野值干擾，在14000s至17600s設置一小時的波束故障情況。試驗軌跡及測量干擾添加情況如圖6所示。

圖6 湖試軌跡及測量干擾情況Fig.6 Lake test track and measurement interference

2.2 湖面試驗驗證

1)濾波器魯棒性驗證

為體現(xiàn)所提魯棒濾波器的有效性，我們在波束無故障條件下對標準卡爾曼濾波器及所提魯棒濾波器性能進行對比分析，其中速度誤差及定位誤差如圖7、圖8所示。

圖7 、圖8中紅色虛線為所提魯棒濾波器方法，黑色實線為卡爾曼濾波器方法。由圖7可以看出在異常量測信息添加之前，兩種方法精度幾乎相同，然而在10000s之后我們在測量速度信息中隨機加入標準差為10m/s的異常速度信息，標準卡爾曼濾波器由于不具備魯棒性，異常速度信息將導致組合導航速度受噪聲影響產(chǎn)生較大波動，進而影響定位精度；本文所提出的魯棒濾波器可以通過調(diào)整量測信息權重的方式對異常信息進行篩選排除，所以在量測異常情況下幾乎不受到干擾，在濾波器參數(shù)穩(wěn)定后，速度精度保持在0.05m/s以內(nèi)，位置精度無較大波動。

圖7 合速度誤差比較Fig.7 Comparison of total speed er ror

圖8 定位誤差比較Fig.8 Comparison of positioning error

為了在異常量測情況下定量描述兩種濾波方法的性能，我們選擇最大誤差(Maximum Error,ME)、平均定位誤差(Averaged Localization Error,ALE)、誤差均方根(Root Mean SquareError,RMSE）、標準差(Standard Deviation,STD)等作為多AUV協(xié)同定位性能指標，速度誤差和定位誤差具體的數(shù)值統(tǒng)計如表2所示。

表2 量測異常情況速度誤差及定位誤差統(tǒng)計Tab.2 Statisticsof velocity error andpositioning error in abnormal measurement

由表2可以看出所提魯棒濾波器幾乎不受量測異常的影響，在28.47 n mile的里程內(nèi)，位置誤差小于57.76m，速度誤差小于0.27m/s；卡爾曼濾波器方法雖然速度誤差均值較小，但受異常噪聲影響，先驗量測信息不能匹配噪聲協(xié)方差信息，引起速度誤差標準差較大，進而造成定位誤差較大。

2)故障處理方案驗證

在SINS/DVL松組合系統(tǒng)中，DVL波束信息缺失將導致輔助速度信息失效，僅能采用純慣性導航方式，然而在四波束DVL輔助SINS的情況下，由1.2節(jié)介紹可知，當波束測量信息未全部出現(xiàn)故障時，我們可以采用波束速度信息之間的關系重新構建速度信息，并用來輔助SINS進行導航。

為驗證我們所提出SINS/DVL緊組合故障處理方案在DVL測速出現(xiàn)故障情況下的有效性，我們在上一小節(jié)量測異?；A上，在14000s~17600s一個小時內(nèi)設置不同的DVL測量波束故障，利用所提出的故障檢測及處理方案對故障進行處理，并與在此區(qū)間內(nèi)利用純慣性導航的方案進行對比。故障設置情況如表3所示（“O”表示波束正常，“F”表示波束故障）。在以上故障情況下所提處理方案與采用慣性導航方案的速度誤差及徑向定位誤差結果對比如圖9-10所示。

表3 波束故障設置情況Tab.3 Setting of beamfault

圖9 故障情況合速度誤差及統(tǒng)計情況Fig.9 Speed error and statisticsin fault condition

圖9 中可以看出，在故障數(shù)據(jù)段采用慣性導航方案的速度產(chǎn)生較大偏移，在分布概率圖中，速度誤差在-0.2m/s~ -0.4m/s區(qū)間段內(nèi)存在較大分布概率，即速度誤差存在偏置，這是引起圖10中定位誤差較大的直接原因；在Case 1、Case2、Case 3情況下，因為波束故障情況不同，所采用的波束補償方式也不同，所以速度誤差也有較大出入，其中Case 3由于故障波束較多，經(jīng)補償之后的速度誤差也相對較大，Case 1僅有單波束出現(xiàn)故障，最終導航誤差也幾乎不受故障影響，導致這一現(xiàn)象的原因在1.2節(jié)中進行了說明。從誤差分布圖中可以看出，經(jīng)過所提波束補償方案的處理之后，組合系統(tǒng)的速度誤差呈現(xiàn)出零均值的高斯分布，即估計速度近似為真實速度的無偏估計。

圖1 水下復雜工作環(huán)境Fig.1 Underwater complex working environment

圖10 中可以看出在波束故障時段內(nèi)采用慣性導航方式，在短時間內(nèi)定位誤差呈正弦趨勢增長，一個小時定位誤差增長約550 m。同時在速度信息恢復時，位置誤差并沒有減小，也說明了速度匹配情況下，初始位置誤差是不可觀測的。本文提出的故障處理方案在定位精度上相較于采用慣性導航方案有了較大提高，其中Case1在故障時間段誤差幾乎沒有增加，在Case 2中定位誤差約增長55m，Case3中定位誤差增長約90m，約為純慣性方案定位誤差的6.96%、12.95%、19.95%。

圖10 故障情況下徑向定位誤差Fig.10 Radial positioning error under fault condition

為具體分析定位誤差的來源，東向定位誤差與北向定位誤差情況在圖11給出，由1.2節(jié)可知，針對故障Case 2的應對方案是將天向速度認為是零，天向速度誤差經(jīng)俯仰角的投影轉(zhuǎn)為載體前向的速度誤差，由此造成前向定位誤差，從軌跡圖6可知，載體在東向位移較大，北向位移較小，所以定位誤差主要表現(xiàn)在東向定位誤差約為75m；針對Case3的應對方案將載體側(cè)向及天向速度認為是零，此時造成了側(cè)向及前向速度的不準確，由圖11可知，東向及北向定位誤差分別接近100m及50m。

圖11 東向、北向定位誤差Fig.11 East andnorth positioning error

進一步說明所提誤差處理方案的有效性，徑向定位誤差累計概率圖由圖12給出，曲線描述了緊組合在導航過程中定位誤差分布情況，曲線越接近直線，斜率越大表明定位誤差分布越均勻且定位誤差較小。由圖12可以看出在波束故障段，采用慣性導航方法曲線呈明顯階梯型，即波束故障段定位誤差增長較快；而采用本文所提故障處理方案，曲線階梯狀不明顯，累計概率更快速地接近1，即定位誤差最大值較小。

圖12 徑向定位誤差累積概率Fig.12 Cumulative probability of radialpositioning err or

一小時故障段的定位誤差及速度誤差統(tǒng)計情況如表4所示，由表4可以看出，在1小時的DVL波束異常情況下，采用純慣性導航方案將產(chǎn)生最大0.49m/s的速度誤差，平均速度誤差為-0.20m/s，造成位置誤差最大值為577.93m，平均位置誤差為241.50m；采用本文所提故障處理方案，速度最大誤差小于0.15m/s，平均速度誤差小于0.06m/s，Case 1、Case 2、Case 3最大定位誤差分別為40.20 m、74.83m、115.31m，僅是采用慣性導航方案定位誤差的6.96%、12.95%、19.95%，由此也可以看出在多波束故障情況下，所提出故障處理方案是對部分速度做出了補償，實現(xiàn)了一部分速度誤差的估計。均方根誤差和標準差分別反應了統(tǒng)計量偏離真實值程度及均勻程度，可以看出，本文所提的故障處理方案所得速度均方根誤差值小于0.07m/s，標準差值小于0.04 m/s，遠小于純慣性導航方式的相對應數(shù)值，定位誤差也有同樣的結論。

表4 量測故障段速度誤差及定位誤差統(tǒng)計Tab.4 Statisticsof speederror andlocationerror in fault section

3 結論

本文以SINS/DVL水下組合導航系統(tǒng)量測信息異常及故障情況為背景，傳統(tǒng)松組合方式中，單一DVL波束出現(xiàn)故障將導致速度量測信息無效，僅能采用純慣性進行接替導航方式，長時間的慣性導航將引起較大的速度誤差。本文針對四波束DVL輔助SINS緊組合導航系統(tǒng)提出了一種故障處理方案，實現(xiàn)了對故障波束速度信息的重構，并用重構后的速度信息對SINS誤差項進行校正，針對量測異常情況、速度重構過程中產(chǎn)生的模型不精確誤差及引入的不確定性噪聲提出了改進Huber魯棒濾波器，最終通過試驗驗證了所提故障處理方案的有效性，試驗結果表明，波束故障數(shù)量分別為1、2、3時，對應的1小時定位誤差分別為采用純慣性定位的6.96%、12.95%、19.95%。