基于緊組合的SINS/DVL/USBL 導航算法

章彩霞 ,劉錫祥 * ,黃永江 ,陳世杰 ,陶育杰

(1.東南大學 儀器科學與工程學院,江蘇 無錫,214026;2.微慣性儀表與先進導航技術教育部重點實驗室,江蘇 南京,210096)

0 引言

準確的定位和導航信息是自主水下航行器(autonomous undersea vehicle,AUV)安全、高效作業的關鍵[1-2]。然而,由于海洋環境的多樣性和AUV 的局限性,AUV 很難實現高精度的導航和定位,不能滿足日益增長的應用需求。慣性導航系統(inertial navigation system,INS)具有強自主性和強隱蔽性等優點,通常作為AUV 的核心導航系統,但單個傳感器無法應對AUV 面臨的復雜環境。通過利用各種傳感器的互補和特性,進行信息融合和濾波校正,可以在較長時間內獲得更高的導航性能和穩定性[3-5]。

多普勒測速儀(Doppler velocity log,DVL)利用多普勒效應進行速度測量,可以為捷聯式慣導系統(strapdown inertial navigation system,SINS)提供高精度的速度信息,但位置誤差仍然隨時間發散[6-7]。超短基線(ultra-short baseline,USBL)定位系統利用聲學定位技術實現對水下目標的定位和跟蹤,可以獲得高精度位置信息,能有效抑制AUV導航系統的位置誤差,但受換能器作用距離限制[8-11]。SINS/DVL/USBL 導航系統能充分利用各傳感器的特點,取長補短,提高系統外界量測的穩定性,更能滿足AUV 高精度、長航時及廣區域內的導航與定位的需求。

基于此,張濤等[12]以SINS/USBL/DVL/磁羅經導航(magnetic compass pilot,MCP)組合導航系統為研究對象,較好地判斷并隔斷處理USBL 故障。王彬等[13]采用單水聲應答器和慣導組合的方法抑制INS/DVL 導航誤差的發散趨勢,有效提高了導航定位精度。張亞文等[14]建立了SINS/DVL 的3 維速度量測模型和SINS/USBL 相對測量信息量測模型后,提出了基于集中濾波的SINS/DVL/USBL組合導航算法,對比SINS/DVL/USBL 聯邦濾波的算法具有明顯精度優勢,但未驗證說明復雜水下環境下的精度影響和系統穩健性。徐博等[15]建立了SINS/USBL 子濾波器和SINS/DVL 子濾波器,構建了基于精度因子信息分配方法的SINS/USBL/DVL 聯邦濾波器,對于各子濾波器進行動態信息分配,驗證了USBL 突然故障的容錯性,但對DVL的故障情況并未驗證。

分析復雜水下環境對AUV 的影響,應考慮SINS/DVL/USBL 導航系統中DVL 和USBL 的數據出現失效情況,并進行系統精度和穩定性驗證。目前公開的SINS/DVL/USBL 組合導航系統在構建SINS/DVL 的量測模型時都選擇采用3 維速度量測信息的松組合方法,在DVL 受異常環境影響出現數據失效的情況下,導航精度也會被明顯影響。在SINS/DVL 系統中DVL 出現故障的改進方案中,Wang 等[16]提出了基于波束域4 維速度信息的SINS 和DVL 緊組合方法,并驗證了所提方法明顯優于傳統的松組合方法,可以將SINS/DVL的定位精度提高30%以上。Liu 等[17]構建了基于四波束頻移信息的組合模型來實現對慣導系統誤差和DVL 誤差的估計和校正,仿真和實驗驗證了基于多普勒頻移的緊組合模型在水下復雜環境下具有較強的魯棒性。在DVL 和SINS 的數據融合時,基于DVL 波束信息上的緊組合方法在部分波束失效時能更有效地利用正常的波束信息,提高DVL 數據的利用率。SINS/DVL/USBL 導航在進行數據融合時,也可以采用SINS/DVL 緊組合的數據融合方法以提高導航精度。

基于上述分析,為使AUV 能在復雜水下環境獲得更長時間、更高精度以及更穩定的導航定位信息以滿足作業需求,提出一種基于緊組合的SINS/DVL/USBL 組合導航算法。首先構建了SINS/DVL的4 維頻移信息量測模型和SINS/USBL 的相對測量信息量測模型2 種緊組合模型,并和不同組合模型下的定位精度作比較,仿真驗證該算法能有效提高系統的導航精度。為了能夠及時有效地處理異常環境帶來的失效數據,保證量測信息的穩定性,文中在此基礎上對DVL 和USBL 數據失效情況下的定位精度進行故障判斷和隔斷處理,最終通過理論仿真驗證了文中方法在DVL 和USBL 的數據失效情況下具有更高的容錯性和穩定性。

1 SINS/DVL/USBL 組合導航模型

考慮集中濾波方法能夠獲得全局的最優估計,故選取該方法實現SINS/DVL/USBL 緊組合導航,組合導航系統結構如圖1 所示。

圖1 SINS/DVL/USBL 組合導航系統結構圖Fig.1 SINS/DVL/USBL integrated navigation system structrue

導航坐標系(n系)選取為東北天地理坐標系,慣導載體坐標系(b系)、多普勒載體坐標系(d系)和聲學基陣坐標系(u系)均選取右前上方向。選擇SINS/DVL 四波束方向頻移信息量測和SINS/USBL 相對位置信息量測來實現緊組合的集中濾波。

1.1 SINS/DVL/USBL 組合導航狀態模型

1.1.1 SINS 子系統狀態誤差模型

SINS 能夠獨立自主地提供速度、姿態和位置信息,但會受加速度計和陀螺儀的慣性器件誤差影響,故選取傳統的15 維慣導系統誤差作為SINS子系統的誤差狀態量

構建SINS 子系統的狀態誤差方程

式中:WSINS為 零均值高斯白噪聲;FSINS為慣導系統矩陣[18]。

1.1.2 DVL 子系統誤差模型

影響DVL 測速精度的主要因素有DVL 與SINS 之間的安裝角誤差、比例因子誤差和隨機量測誤差。因此,選取DVL 比例因子誤差和DVL 安裝角誤差為DVL 子系統的誤差狀態量

因為安裝角誤差 δθd為常值,比例因子誤差δκd也可近似為1 個常數。建立DVL 子系統的狀態誤差方程

式中:FDVL=04×4為 DVL 子系統的系統矩陣;WDVL為DVL 的相關噪聲矩陣。

1.1.3 USBL 子系統誤差模型

由于設備安裝原因導致的USBL 安裝角誤差是USBL 的主要誤差源之一。根據USBL 定位原理可知,USBL 的自身測角誤差和測距誤差也一定程度上影響了USBL 的定位精度。因此,選取USBL 與SINS 之間的安裝誤差角、USBL 的測角誤差和測距誤差系數作為USBL 子系統的誤差狀態量,即

USBL 設備中一般需要結合深度計(pressure sensor,PS)進行深度測量,考慮PS 輸出模型為

式中:h為高度真值;δbPS為PS 的常值偏置誤差;ωPS為零均值高斯噪聲。

故將δbPS加 入到USBL 子系統狀態變量中,得

因為 δα、δβ和 δκu均為小量,可視為隨機常數,建立USBL 子系統的狀態誤差方程為

式中:FUSBL=07×7為USBL 子系統的系統矩陣;WUSBL為USBL 和PS 的相關噪聲矩陣。

1.1.4 SINS/DVL/USBL 狀態誤差模型

綜上所述,構建SINS/DVL/USBL 集中濾波系統時選取狀態變量為

建立系統的狀態誤差方程為

1.2 SINS/DVL/USBL 組合導航量測模型

文中設計的SINS/DVL/USBL 組合導航系統的量測模型主要由SINS/DVL 四波束方向頻移信息量測和SINS/USBL 相對測量信息量測組成。

1.2.1 SINS/DVL 頻移量測模型

DVL 采用十字型四波束Janus 配置,4 個換能器波束發射方向與d系z軸反方向的夾角為為DVL 測量的三向速度信息,如圖2 所示。

圖2 DVL 十字型四波束Janus 配置Fig.2 DVL cross-type four-beam Janus configuration

四波束方向多普勒頻移構成的列向量為

根據DVL 的3 維速度信息解算四波束方向的頻移信息公式為

利用SINS 測量的導航系速度信息計算出四波束方向的頻移值為

DVL 實際頻移測量值為

式中,vd為多普勒測速儀的頻移量測噪聲。

因此,根據式(13)和 式(14)建 立SINS/DVL 的頻移量測為

由式(15)可得量測矩陣為

1.2.2 SINS/USBL 相對測量信息量測模型

參考文獻[11],將USBL 輸出的方位角、高度角和斜距測量值與慣性導航系統計算的測量信息進行對比,得到的差值作為SINS/USBL 的量測量。

USBL 測量的方位角、高度角和斜距為

SINS 計算的USBL 高度角誤差δ αSINS,方位角誤差 δβSINS和 斜距誤差系數 δρSINS為

式中,L和λ分別為載體的緯度和經度。

所以,根據式(17)和式(20)建立SINS/USBL的相對測量信息量測為

根據PS 的輸出模型,將SINS 的高度信息和PS 的深度信息輸出之差作為高度信息量測

由式(25)和(26)可得SINS/USBL 子系統的量測方程為

量測矩陣為

1.2.3 SINS/DVL/USBL 量測模型

文中構建的SINS/DVL/USBL 緊組合導航系統的量測量為

對應構建的量測方程為

2 SINS/DVL/USBL 故障判斷及處理

2.1 基于新息的 χ2故障檢測

為了保證集中卡爾曼濾波的量測穩定性,當DVL 和USBL 的數據出現異常時,對其進行隔斷處理。文中采用 χ2檢驗法來判斷DVL 和USBL 的測量值是否出現異常。

新息序列定義為

新息的協方差矩陣為

2.2 DVL 失效數據處理

在判斷DVL 存在失效數據或野值時,采取數據隔斷處理,系統的量測方程簡化為

2.3 USBL 失效數據處理

當USBL 受環境影響出現野值時,采取數據隔斷處理,系統的量測方程簡化為

3 理論數據仿真分析

為驗證算法有效性,進行如下仿真研究。仿真時間為2 185 s,載體初始地理位置為L=39.8?,λ=116.2?,h=0 m,以4 m/s 的速度按圖3 所示軌跡勻速航行,其中應答器的位置為Pt=[39.802 026°116.198 185° -150 m]。為確保算法定位精度,屏蔽各類誤差,設置初始姿態角均為 0?,無初始速度誤差和位置誤差。模擬中等海平面設置搖擺運動如下[17]

圖3 仿真軌跡Fig.3 Simulation track

式中: θm=3?;γm=4?;ψm=3?;初始相位角 θ0=3?;γ0=4?;ψ0=3?;搖擺周期分別為5、6、7 s。

SINS 的數據更新頻率為200 Hz,DVL、USBL和PS 的數據更新頻率均為1 Hz[19-20]。各傳感器的誤差參數設置如表1 所示。

表1 傳感器誤差參數Table 1 The error parameters of sensors

3.1 不同組合模式仿真對比

按照文中的SINS/DVL/USBL 緊組合導航算法進行仿真,并對SINS/DVL 頻移量測緊組合導航算法、SINS/USBL 相對位置信息量測緊組合導航算法、文獻[14]中SINS/DVL/USBL 的SINS/DVL速度量測和SINS/USBL 相對測量信息量測集中濾波方法定位結果進行對比。SINS/DVL 緊組合濾波方法表示為SDT,SINS/USBL 緊組合濾波方法表示為SUT,文獻[14]的方法用SDLUT表示,文中方法用SDTUT表示。定位誤差曲線如圖4～圖6 所示。

圖4 東向位置誤差Fig.4 East position error

圖5 北向位置誤差Fig.5 North position error

圖6 水平定位誤差Fig.6 Level position error

由定位誤差曲線可以看出,SINS/DVL/USBL緊組合集中卡爾曼濾波算法和基于速度量測和相對測量信息量測的SINS/DVL/USBL 集中濾波方法的定位精度都明顯優于SINS/DVL 緊組合方法和SINS/USBL 緊組合方法,具有更高精度。

計算各個方法的水平定位誤差的均方根誤差(root mean squared error,RMSE)可以發現(見表2),文中方法對比基于速度量測和相對測量信息量測的SINS/DVL/USBL 集中濾波方法具有更高精度,精度提升約23%。

表2 不同組合導航方法水平定位誤差RMSE 對比Table 2 Comparison of level positioning error RMSE

3.2 DVL 和USBL 數據失效情況下的仿真對比

在實際復雜的水下環境,DVL 會出現原始波束頻移數據中部分波束數據失效的情況,USBL 的輸出水聲信息也存在野值。為了仿真這種情況,對DVL 和USBL 的原始數據以10%概率加上原來50 倍的量測噪聲,DVL 和USBL 存在失效數據的仿真情況如圖7 和圖8 所示。

圖7 DVL 失效數據情況Fig.7 DVL failure data situation

圖8 USBL 失效數據情況Fig.8 USBL failure data situation

在DVL 和USBL 出現數據失效的情況下,通過卡方檢驗判斷出故障情況并進行隔離處理。對比2 種采用不同量測的集中卡爾曼導航算法,DVL 數據失效情況和USBL 數據失效情況下水平定位誤差曲線如圖9 和圖10 所示。計算數據失效情況下的定位誤差RMSE,并與正常情況的定位誤差作對比,如表3 所示。

表3 數據失效情況下不同量測的集中濾波方法水平定位誤差RMSETable 3 Level positioning error RMSE of the concentrated filter methods based on different measurements in case of data invalidation

圖9 DVL 數據失效情況下的水平定位誤差Fig.9 Level positioning error in case of DVL data failure

圖10 USBL 數據失效情況下的水平定位誤差Fig.10 Level positioning error in case of USBL data failure

觀察定位誤差曲線,并對比2 種方法的定位誤差RMSE,可以發現,在DVL 數據失效情況下,文中方法可以實現定位精度基本穩定,定位誤差較正常情況增長5.2%,而SDLUT方法定位誤差較正常情況增長39.6%。在USBL 數據失效情況下,定位精度會受到較大影響,文中方法定位誤差較正常情況增長165.4%,而SDLUT方法定位誤差較正常情況增長188.4%,精度仍明顯優于基于速度量測和相對測量信息量測的SINS/DVL/USBL 集中濾波方法。綜上所述,無論是DVL 數據失效還是USBL 數據失效的情況,文中方法都具有較高的精度和穩定性,更適應復雜的水下環境。

4 結束語

針對復雜水下環境下AUV 對于高精度、高穩定性和高可靠性的導航需求,提出一種基于DVL四波束方向頻移量測和USBL 相對測量信息量測的SINS/DVL/USBL 緊組合導航算法,針對數據失效情況進行實時判斷并隔斷處理。仿真驗證表明,與SINS/DVL 緊組合導航方法、SINS/USBL 緊組合導航算法及基于DVL 速度量測和USBL 相對測量信息量測的SINS/DVL/USBL 集中濾波組合導航算法相比,該方法具有較高的定位精度,可提升約23%。在DVL 和USBL 數據失效的情況下,該算法穩定性更好,在DVL 數據失效時,定位誤差較正常情況僅增長5.2%;在USBL 數據失效時,定位誤差較正常情況增長165.4%,優于基于其他量測的SINS/DVL/USBL 集中濾波導航算法,具有更強的魯棒性,更能適應復雜的水下環境。

文中方法對于DVL 和USBL 的數據直接進行數據隔斷以避免故障數據帶來的精度影響,但是如果存在長時間的數據失效情況,隔斷可能會影響定位精度,下一步將針對DVL 和USBL 的數據情況研究如何采取更有效的手段(而非數據隔斷)進行數據替換。