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

(1.東南大學(xué) 儀器科學(xué)與工程學(xué)院,江蘇 無(wú)錫,214026;2.微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室,江蘇 南京,210096)

0 引言

準(zhǔn)確的定位和導(dǎo)航信息是自主水下航行器(autonomous undersea vehicle,AUV)安全、高效作業(yè)的關(guān)鍵[1-2]。然而,由于海洋環(huán)境的多樣性和AUV 的局限性,AUV 很難實(shí)現(xiàn)高精度的導(dǎo)航和定位,不能滿足日益增長(zhǎng)的應(yīng)用需求。慣性導(dǎo)航系統(tǒng)(inertial navigation system,INS)具有強(qiáng)自主性和強(qiáng)隱蔽性等優(yōu)點(diǎn),通常作為AUV 的核心導(dǎo)航系統(tǒng),但單個(gè)傳感器無(wú)法應(yīng)對(duì)AUV 面臨的復(fù)雜環(huán)境。通過(guò)利用各種傳感器的互補(bǔ)和特性,進(jìn)行信息融合和濾波校正,可以在較長(zhǎng)時(shí)間內(nèi)獲得更高的導(dǎo)航性能和穩(wěn)定性[3-5]。

多普勒測(cè)速儀(Doppler velocity log,DVL)利用多普勒效應(yīng)進(jìn)行速度測(cè)量,可以為捷聯(lián)式慣導(dǎo)系統(tǒng)(strapdown inertial navigation system,SINS)提供高精度的速度信息,但位置誤差仍然隨時(shí)間發(fā)散[6-7]。超短基線(ultra-short baseline,USBL)定位系統(tǒng)利用聲學(xué)定位技術(shù)實(shí)現(xiàn)對(duì)水下目標(biāo)的定位和跟蹤,可以獲得高精度位置信息,能有效抑制AUV導(dǎo)航系統(tǒng)的位置誤差,但受換能器作用距離限制[8-11]。SINS/DVL/USBL 導(dǎo)航系統(tǒng)能充分利用各傳感器的特點(diǎn),取長(zhǎng)補(bǔ)短,提高系統(tǒng)外界量測(cè)的穩(wěn)定性,更能滿足AUV 高精度、長(zhǎng)航時(shí)及廣區(qū)域內(nèi)的導(dǎo)航與定位的需求。

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

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

基于上述分析,為使AUV 能在復(fù)雜水下環(huán)境獲得更長(zhǎng)時(shí)間、更高精度以及更穩(wěn)定的導(dǎo)航定位信息以滿足作業(yè)需求,提出一種基于緊組合的SINS/DVL/USBL 組合導(dǎo)航算法。首先構(gòu)建了SINS/DVL的4 維頻移信息量測(cè)模型和SINS/USBL 的相對(duì)測(cè)量信息量測(cè)模型2 種緊組合模型,并和不同組合模型下的定位精度作比較,仿真驗(yàn)證該算法能有效提高系統(tǒng)的導(dǎo)航精度。為了能夠及時(shí)有效地處理異常環(huán)境帶來(lái)的失效數(shù)據(jù),保證量測(cè)信息的穩(wěn)定性,文中在此基礎(chǔ)上對(duì)DVL 和USBL 數(shù)據(jù)失效情況下的定位精度進(jìn)行故障判斷和隔斷處理,最終通過(guò)理論仿真驗(yàn)證了文中方法在DVL 和USBL 的數(shù)據(jù)失效情況下具有更高的容錯(cuò)性和穩(wěn)定性。

1 SINS/DVL/USBL 組合導(dǎo)航模型

考慮集中濾波方法能夠獲得全局的最優(yōu)估計(jì),故選取該方法實(shí)現(xiàn)SINS/DVL/USBL 緊組合導(dǎo)航,組合導(dǎo)航系統(tǒng)結(jié)構(gòu)如圖1 所示。

圖1 SINS/DVL/USBL 組合導(dǎo)航系統(tǒng)結(jié)構(gòu)圖Fig.1 SINS/DVL/USBL integrated navigation system structrue

導(dǎo)航坐標(biāo)系(n系)選取為東北天地理坐標(biāo)系,慣導(dǎo)載體坐標(biāo)系(b系)、多普勒載體坐標(biāo)系(d系)和聲學(xué)基陣坐標(biāo)系(u系)均選取右前上方向。選擇SINS/DVL 四波束方向頻移信息量測(cè)和SINS/USBL 相對(duì)位置信息量測(cè)來(lái)實(shí)現(xiàn)緊組合的集中濾波。

1.1 SINS/DVL/USBL 組合導(dǎo)航狀態(tài)模型

1.1.1 SINS 子系統(tǒng)狀態(tài)誤差模型

SINS 能夠獨(dú)立自主地提供速度、姿態(tài)和位置信息,但會(huì)受加速度計(jì)和陀螺儀的慣性器件誤差影響,故選取傳統(tǒng)的15 維慣導(dǎo)系統(tǒng)誤差作為SINS子系統(tǒng)的誤差狀態(tài)量

構(gòu)建SINS 子系統(tǒng)的狀態(tài)誤差方程

式中:WSINS為 零均值高斯白噪聲;FSINS為慣導(dǎo)系統(tǒng)矩陣[18]。

1.1.2 DVL 子系統(tǒng)誤差模型

影響DVL 測(cè)速精度的主要因素有DVL 與SINS 之間的安裝角誤差、比例因子誤差和隨機(jī)量測(cè)誤差。因此,選取DVL 比例因子誤差和DVL 安裝角誤差為DVL 子系統(tǒng)的誤差狀態(tài)量

因?yàn)榘惭b角誤差 δθd為常值,比例因子誤差δκd也可近似為1 個(gè)常數(shù)。建立DVL 子系統(tǒng)的狀態(tài)誤差方程

式中:FDVL=04×4為 DVL 子系統(tǒng)的系統(tǒng)矩陣;WDVL為DVL 的相關(guān)噪聲矩陣。

1.1.3 USBL 子系統(tǒng)誤差模型

由于設(shè)備安裝原因?qū)е碌腢SBL 安裝角誤差是USBL 的主要誤差源之一。根據(jù)USBL 定位原理可知,USBL 的自身測(cè)角誤差和測(cè)距誤差也一定程度上影響了USBL 的定位精度。因此,選取USBL 與SINS 之間的安裝誤差角、USBL 的測(cè)角誤差和測(cè)距誤差系數(shù)作為USBL 子系統(tǒng)的誤差狀態(tài)量,即

USBL 設(shè)備中一般需要結(jié)合深度計(jì)(pressure sensor,PS)進(jìn)行深度測(cè)量,考慮PS 輸出模型為

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

故將δbPS加 入到USBL 子系統(tǒng)狀態(tài)變量中,得

因?yàn)?δα、δβ和 δκu均為小量,可視為隨機(jī)常數(shù),建立USBL 子系統(tǒng)的狀態(tài)誤差方程為

式中:FUSBL=07×7為USBL 子系統(tǒng)的系統(tǒng)矩陣;WUSBL為USBL 和PS 的相關(guān)噪聲矩陣。

1.1.4 SINS/DVL/USBL 狀態(tài)誤差模型

綜上所述,構(gòu)建SINS/DVL/USBL 集中濾波系統(tǒng)時(shí)選取狀態(tài)變量為

建立系統(tǒng)的狀態(tài)誤差方程為

1.2 SINS/DVL/USBL 組合導(dǎo)航量測(cè)模型

文中設(shè)計(jì)的SINS/DVL/USBL 組合導(dǎo)航系統(tǒng)的量測(cè)模型主要由SINS/DVL 四波束方向頻移信息量測(cè)和SINS/USBL 相對(duì)測(cè)量信息量測(cè)組成。

1.2.1 SINS/DVL 頻移量測(cè)模型

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

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

四波束方向多普勒頻移構(gòu)成的列向量為

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

利用SINS 測(cè)量的導(dǎo)航系速度信息計(jì)算出四波束方向的頻移值為

DVL 實(shí)際頻移測(cè)量值為

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

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

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

1.2.2 SINS/USBL 相對(duì)測(cè)量信息量測(cè)模型

參考文獻(xiàn)[11],將USBL 輸出的方位角、高度角和斜距測(cè)量值與慣性導(dǎo)航系統(tǒng)計(jì)算的測(cè)量信息進(jìn)行對(duì)比,得到的差值作為SINS/USBL 的量測(cè)量。

USBL 測(cè)量的方位角、高度角和斜距為

SINS 計(jì)算的USBL 高度角誤差δ αSINS,方位角誤差 δβSINS和 斜距誤差系數(shù) δρSINS為

式中,L和λ分別為載體的緯度和經(jīng)度。

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

根據(jù)PS 的輸出模型,將SINS 的高度信息和PS 的深度信息輸出之差作為高度信息量測(cè)

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

量測(cè)矩陣為

1.2.3 SINS/DVL/USBL 量測(cè)模型

文中構(gòu)建的SINS/DVL/USBL 緊組合導(dǎo)航系統(tǒng)的量測(cè)量為

對(duì)應(yīng)構(gòu)建的量測(cè)方程為

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

2.1 基于新息的 χ2故障檢測(cè)

為了保證集中卡爾曼濾波的量測(cè)穩(wěn)定性,當(dāng)DVL 和USBL 的數(shù)據(jù)出現(xiàn)異常時(shí),對(duì)其進(jìn)行隔斷處理。文中采用 χ2檢驗(yàn)法來(lái)判斷DVL 和USBL 的測(cè)量值是否出現(xiàn)異常。

新息序列定義為

新息的協(xié)方差矩陣為

2.2 DVL 失效數(shù)據(jù)處理

在判斷DVL 存在失效數(shù)據(jù)或野值時(shí),采取數(shù)據(jù)隔斷處理,系統(tǒng)的量測(cè)方程簡(jiǎn)化為

2.3 USBL 失效數(shù)據(jù)處理

當(dāng)USBL 受環(huán)境影響出現(xiàn)野值時(shí),采取數(shù)據(jù)隔斷處理,系統(tǒng)的量測(cè)方程簡(jiǎn)化為

3 理論數(shù)據(jù)仿真分析

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

圖3 仿真軌跡Fig.3 Simulation track

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

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

表1 傳感器誤差參數(shù)Table 1 The error parameters of sensors

3.1 不同組合模式仿真對(duì)比

按照文中的SINS/DVL/USBL 緊組合導(dǎo)航算法進(jìn)行仿真,并對(duì)SINS/DVL 頻移量測(cè)緊組合導(dǎo)航算法、SINS/USBL 相對(duì)位置信息量測(cè)緊組合導(dǎo)航算法、文獻(xiàn)[14]中SINS/DVL/USBL 的SINS/DVL速度量測(cè)和SINS/USBL 相對(duì)測(cè)量信息量測(cè)集中濾波方法定位結(jié)果進(jìn)行對(duì)比。SINS/DVL 緊組合濾波方法表示為SDT,SINS/USBL 緊組合濾波方法表示為SUT,文獻(xiàn)[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緊組合集中卡爾曼濾波算法和基于速度量測(cè)和相對(duì)測(cè)量信息量測(cè)的SINS/DVL/USBL 集中濾波方法的定位精度都明顯優(yōu)于SINS/DVL 緊組合方法和SINS/USBL 緊組合方法,具有更高精度。

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

表2 不同組合導(dǎo)航方法水平定位誤差RMSE 對(duì)比Table 2 Comparison of level positioning error RMSE

3.2 DVL 和USBL 數(shù)據(jù)失效情況下的仿真對(duì)比

在實(shí)際復(fù)雜的水下環(huán)境,DVL 會(huì)出現(xiàn)原始波束頻移數(shù)據(jù)中部分波束數(shù)據(jù)失效的情況,USBL 的輸出水聲信息也存在野值。為了仿真這種情況,對(duì)DVL 和USBL 的原始數(shù)據(jù)以10%概率加上原來(lái)50 倍的量測(cè)噪聲,DVL 和USBL 存在失效數(shù)據(jù)的仿真情況如圖7 和圖8 所示。

圖7 DVL 失效數(shù)據(jù)情況Fig.7 DVL failure data situation

圖8 USBL 失效數(shù)據(jù)情況Fig.8 USBL failure data situation

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

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

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

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

觀察定位誤差曲線,并對(duì)比2 種方法的定位誤差RMSE,可以發(fā)現(xiàn),在DVL 數(shù)據(jù)失效情況下,文中方法可以實(shí)現(xiàn)定位精度基本穩(wěn)定,定位誤差較正常情況增長(zhǎng)5.2%,而SDLUT方法定位誤差較正常情況增長(zhǎng)39.6%。在USBL 數(shù)據(jù)失效情況下,定位精度會(huì)受到較大影響,文中方法定位誤差較正常情況增長(zhǎng)165.4%,而SDLUT方法定位誤差較正常情況增長(zhǎng)188.4%,精度仍明顯優(yōu)于基于速度量測(cè)和相對(duì)測(cè)量信息量測(cè)的SINS/DVL/USBL 集中濾波方法。綜上所述,無(wú)論是DVL 數(shù)據(jù)失效還是USBL 數(shù)據(jù)失效的情況,文中方法都具有較高的精度和穩(wěn)定性,更適應(yīng)復(fù)雜的水下環(huán)境。

4 結(jié)束語(yǔ)

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

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