基于變結(jié)構(gòu)多模型的SINS/DVL組合導(dǎo)航算法

李 倩，王健成，尹冬寒，臧新樂(lè)，奔粵陽(yáng)，趙玉新

(1.哈爾濱工程大學(xué)智能科學(xué)與工程學(xué)院，哈爾濱 150001；2.中國(guó)船舶工業(yè)集團(tuán)公司第七○八研究所，上海 201100)

0 引言

捷聯(lián)慣性導(dǎo)航系統(tǒng)(Strapdown Inertial Navigation System, SINS)是一種自主式導(dǎo)航系統(tǒng)，被認(rèn)為是水下導(dǎo)航領(lǐng)域中最可靠的導(dǎo)航系統(tǒng)，在短時(shí)間內(nèi)具有較高的導(dǎo)航精度。但是由于陀螺漂移和加速度計(jì)零偏的存在， 導(dǎo)致其誤差不斷積累，因此無(wú)法長(zhǎng)時(shí)間單獨(dú)工作。對(duì)于水下環(huán)境，由于全球定位系統(tǒng)(Global Positioning System，GPS)信號(hào)會(huì)迅速衰減[1-3]，因此常用多普勒計(jì)程儀(Doppler Velocity Log, DVL)輔助SINS為無(wú)人水下航行器提供精確的導(dǎo)航信息[4]。DVL有兩種工作模式：底跟蹤模式和水跟蹤模式。DVL一般工作在底跟蹤模式，將DVL提供的相對(duì)地面的速度與SINS融合，為載體提供精確的導(dǎo)航信息。然而，DVL受作用深度限制，在超過(guò)一定范圍時(shí)，DVL將工作在水跟蹤模式下，此時(shí)DVL只能為載體提供相對(duì)于水的速度。若將相對(duì)于水的速度作為相對(duì)地面的速度信息提供給卡爾曼濾波器(Kalman Filter, KF)對(duì)SINS的系統(tǒng)誤差進(jìn)行估計(jì)，此時(shí)洋流速度會(huì)成為DVL的測(cè)量誤差，將導(dǎo)致SINS / DVL組合導(dǎo)航性能降低，甚至導(dǎo)航失敗[5-7]。

為了提高SINS/DVL組合導(dǎo)航系統(tǒng)的性能，文獻(xiàn)[8]提出了一種解決方案，將洋流速度建模為常數(shù)，考慮其對(duì)系統(tǒng)的影響，并將其增廣到KF的狀態(tài)量中， 基于長(zhǎng)基線/超短基線(Long Baseline/Ultra-short Baseline， LBL/USBL)和DVL提供的位置和速度信息，采用KF估計(jì)洋流速度。文獻(xiàn)[9]采用模型輔助慣性導(dǎo)航系統(tǒng)對(duì)自主水下航行器(Autono-mous Underwater Vehicle, AUV)導(dǎo)航解算方法進(jìn)行改進(jìn)，該模型可以在線估計(jì)洋流速度參數(shù),但需要提供精確的AUV動(dòng)態(tài)模型以及外部定位信息。以上方法均需一些外部定位信息，而對(duì)于水下環(huán)境，連續(xù)的外部位置信息有時(shí)難以獲得。此外，簡(jiǎn)單地將洋流速度建模為常數(shù)以補(bǔ)償DVL水跟蹤模式下的測(cè)量誤差，在短時(shí)間內(nèi)有一定的效果，但不適用于長(zhǎng)航時(shí)和廣海域環(huán)境。為了進(jìn)一步降低洋流常值模型對(duì)SINS/DVL組合導(dǎo)航系統(tǒng)造成的影響，文獻(xiàn)[10]使用一階馬爾可夫過(guò)程來(lái)描述洋流速度的變化，并通過(guò)KF實(shí)時(shí)估算洋流速度，從而校正系統(tǒng)誤差。在很多情況下，一階馬爾可夫過(guò)程洋流速度模型中的相關(guān)時(shí)間常數(shù)會(huì)隨著季節(jié)、位置、鹽度和天氣條件等外部因素的變化而變化[11-12]。因此，單一的洋流速度模型往往無(wú)法表征其流速變化。

針對(duì)洋流速度模型的不確定性，文獻(xiàn)[13]利用交互式多模型(Interactive Multiple Model, IMM)算法估計(jì)洋流速度，以減小其對(duì)導(dǎo)航精度的影響。在該算法中，兩個(gè)KF在不同的洋流速度模型下并行運(yùn)行，然后進(jìn)行輸出融合，得到洋流速度的估計(jì)值。然而，在實(shí)際應(yīng)用中，僅僅建立一個(gè)由兩個(gè)洋流速度模型組成的小模型群是不夠的。為了提高估計(jì)效果就要使用較大的模型群，這樣就會(huì)增大計(jì)算量，難以滿(mǎn)足導(dǎo)航系統(tǒng)的實(shí)時(shí)性要求[14]。同時(shí)，某些不必要的模型間會(huì)存在過(guò)度競(jìng)爭(zhēng)的問(wèn)題，而在狀態(tài)估計(jì)過(guò)程中利用太多的模型也會(huì)降低算法的性能[15]。

利用IMM算法進(jìn)行組合導(dǎo)航時(shí)，一方面要保證模型群有足夠多的模型來(lái)覆蓋系統(tǒng)的實(shí)際模型；另一方面，為了保證計(jì)算速度，需要盡量使用較小的模型數(shù)量。顯然，這兩個(gè)要求是矛盾的。針對(duì)這一問(wèn)題，1992年，X.Li和Y. Bar-Shalom提出了一種變結(jié)構(gòu)多模型(Variable Structure Multiple Model, VSMM)方法[16-17]。在本文中，采用有向圖切換的變結(jié)構(gòu)多模型(Digraph Switching-Variable Structure Multiple Model, DS-VSMM)算法來(lái)實(shí)時(shí)估計(jì)洋流速度。基于有向圖切換方法，設(shè)置洋流速度模型群，利用所設(shè)置的模型群切換策略對(duì)模型群進(jìn)行激活和終止，使用較少的模型數(shù)量實(shí)現(xiàn)了洋流速度的準(zhǔn)確估計(jì)，進(jìn)而消除了其對(duì)SINS/DVL組合導(dǎo)航的影響[18]。

1 SINS/DVL組合導(dǎo)航系統(tǒng)模型建立

SINS/DVL組合導(dǎo)航系統(tǒng)誤差狀態(tài)方程為

(1)

式中，狀態(tài)變量X為14維列向量；狀態(tài)矩陣F為14×14維矩陣；系統(tǒng)噪聲輸入矩陣G和系統(tǒng)噪聲矩陣W為14維列向量。

X=[δLδλδvEδvNφxφyφz

ΔAxΔAyεEεNεUθCEθCN]T

(2)

式中，δL、δλ分別為緯度和經(jīng)度誤差；δvE、δvN分別為東向和北向速度誤差；φx、φy、φz為平臺(tái)失準(zhǔn)角；ΔAx、ΔAy分別為東向和北向加速度計(jì)零位偏移；εE、εN、εU分別為東向、北向和天向陀螺漂移；θCE、θCN分別為洋流的東向和北向速度。本文主要對(duì)洋流速度進(jìn)行模型群建模，故此處只給出洋流狀態(tài)矩陣塊Fon，其余將省略。

(3)

式中，βE、βN分別為東向和北向洋流模型的時(shí)間常數(shù)。

W=[0 0 0 0 0 0 0ωdEωdNωgE

ωgNωgUωEωN]T

(4)

式中，W為零均值的高斯白噪聲，它的協(xié)方差矩陣為Q；ωdE、ωdN分別為加速度計(jì)東向和北向噪聲；ωgE、ωgN、ωgU分別為陀螺東向、北向和天向噪聲；ωE、ωN分別為洋流速度模型對(duì)應(yīng)的東向和北向白噪聲。

在水下環(huán)境中，如果DVL工作在水跟蹤模式下，則其測(cè)得的是對(duì)水的相對(duì)速度，即

(5)

式中，vE、vN分別為慣導(dǎo)系統(tǒng)計(jì)算的東向和北向速度；vDVL(E)、vDVL(N)分別為DVL測(cè)得的相對(duì)于水的東向和北向速度，對(duì)式(5)整理可得

(6)

本文利用慣導(dǎo)解算的速度與DVL測(cè)得的速度之差作為濾波器的量測(cè)量，則SINS/DVL組合導(dǎo)航系統(tǒng)的量測(cè)方程為

(7)

式中，量測(cè)噪聲V為零均值白噪聲，它的協(xié)方差矩陣為R，式(7)中量測(cè)矩陣H為

H=

(8)

2 SINS/DVL組合導(dǎo)航系統(tǒng)模型群的建立

基于DS-VSMM算法對(duì)導(dǎo)航參數(shù)進(jìn)行估計(jì)，首先建立系統(tǒng)的模型群。根據(jù)第1節(jié)，洋流速度模型設(shè)置為一階馬爾可夫過(guò)程，然而在實(shí)際中，洋流速度模型中的一階馬爾科夫過(guò)程相關(guān)時(shí)間常數(shù)是未知的，它會(huì)隨著外部因素的變化而變化。因此，為了能夠準(zhǔn)確地估計(jì)洋流的速度，在可能的范圍內(nèi)設(shè)置一個(gè)大的洋流速度模型群(不同模型對(duì)應(yīng)不同的相關(guān)時(shí)間常數(shù))。此外，由于水下環(huán)境復(fù)雜，SINS/DVL組合導(dǎo)航系統(tǒng)中量測(cè)噪聲的統(tǒng)計(jì)特性也會(huì)隨機(jī)變化，因此量測(cè)噪聲協(xié)方差矩陣R也是一個(gè)未知的參數(shù)。

本文基于不同的相關(guān)時(shí)間常數(shù)β和量測(cè)噪聲協(xié)方差矩陣R，設(shè)置了13個(gè)模型。根據(jù)式(3)，洋流速度模型相關(guān)時(shí)間常數(shù)β對(duì)應(yīng)狀態(tài)轉(zhuǎn)移矩陣塊Fon，各個(gè)模型中對(duì)應(yīng)的狀態(tài)矩陣塊Fon和量測(cè)噪聲方差矩陣R設(shè)置如表1所示。

表1 各個(gè)模型與洋流模型狀態(tài)矩陣塊、量測(cè)噪聲方差矩陣的對(duì)應(yīng)關(guān)系

假設(shè)系統(tǒng)模型的跳變不會(huì)出現(xiàn)斷層式的情況，即只在相鄰模型之間切換。例如：系統(tǒng)不會(huì)出現(xiàn)從Fon直接越過(guò)2Fon跳變到4Fon的情況，R的跳變情況同上，那么SINS/DVL組合導(dǎo)航系統(tǒng)模型跳變的拓?fù)溆邢驁D如圖1所示。

圖1 SINS/DVL組合導(dǎo)航系統(tǒng)整體模型群的有向圖

圖1中，箭頭所指兩個(gè)模型可以互相進(jìn)行切換。將SINS/DVL組合導(dǎo)航系統(tǒng)可能出現(xiàn)的跳變分為四種類(lèi)型:β變大的跳變、β變小的跳變、R變大的跳變和R變小的跳變。

假設(shè)系統(tǒng)當(dāng)前時(shí)刻處于模型M1，隨后可能向模型M2跳變(β增大)，也可能向模型M4跳變(β減小)，也可能向模型M3跳變(R增大)，也可能向模型M5跳變(R減小)，這種情況對(duì)應(yīng)的子模型群為A1={M1,M2,M3,M4,M5}，M1為子模型群A1的中心模型。子模型群A1的有向圖如圖2所示。

圖2 SINS/DVL組合導(dǎo)航系統(tǒng)子模型群A1的有向圖

與圖2所示的子模型群A1類(lèi)似，還有分別以M2、M3、M4和M5為中心的子模型群A2、A3、A4和A5

A2={M2,M10,M6,M1,M9}A3={M3,M11,M7,M1,M6}A4={M4,M12,M8,M1,M7}A5={M5,M13,M9,M1,M8}

3 模型群切換算法

模型群切換(Model-GroupSwitching,MGS)算法首先需要設(shè)置模型群，隨后要制定MGS算法的自適應(yīng)策略。MGS算法在運(yùn)行過(guò)程中一般分為兩部分：模型群激活與終止，最后在匹配的有向圖內(nèi)進(jìn)行IMM濾波。MGS算法步驟如下：

步驟1：將計(jì)數(shù)時(shí)間k增加1，即k→k+1。運(yùn)行VSMM[Mk,Mk-1]一個(gè)周期。

步驟2：檢測(cè)是否有候選模型群被激活。當(dāng)滿(mǎn)足以下概率條件時(shí)

μk

(9)

步驟3：如果模型群Ma被激活，則令Mo=Mk，同時(shí)進(jìn)行如下運(yùn)算：

1)運(yùn)行VSMM[Mn,Mk-1]一個(gè)周期，其中Mn=Ma-Mo是新模型的集合。

2)令Mk=Mn∪Mo=Ma∪Mo。

3)估計(jì)融合：對(duì)模型群Mk計(jì)算模式概率、狀態(tài)估計(jì)以及誤差的協(xié)方差矩陣，分別如式(10)～式(12)所示

(10)

(11)

(12)

步驟5：模型群Ml=Mo或Ml=Ma，則有式(13)～式(15)

(13)

(14)

(15)

當(dāng)式(16)或式(17)成立時(shí)

(16)

(17)

則終止模型群Ma，令Mk+1=Mo，然后跳轉(zhuǎn)到步驟1。

當(dāng)式(18)與式(19)同時(shí)成立時(shí)

(18)

(19)

則終止模型群Mo，并令Mk+1=Ma，然后跳轉(zhuǎn)到步驟1。

步驟6：將計(jì)數(shù)時(shí)間k增加1，即k→k+1，同時(shí)令Mk+1=Mk，運(yùn)行VSMM[Mk,Mk-1]一個(gè)周期，然后跳轉(zhuǎn)到步驟4。

4 仿真驗(yàn)證

為了驗(yàn)證基于DS-VSMM的SINS/DVL組合導(dǎo)航算法的性能，在仿真環(huán)境相同的情況下，對(duì)比DS-VSMM算法與IMM算法。

(20)

根據(jù)式(20)以及表1，仿真中真實(shí)的洋流速度曲線如圖3所示。

圖3 真實(shí)的洋流速度曲線圖

此外，這里還給出了仿真環(huán)境中設(shè)置的載體運(yùn)動(dòng)軌跡以及速度變化圖，如圖4和圖5所示。

圖4 載體運(yùn)動(dòng)軌跡

圖5 載體的速度變化曲線圖

根據(jù)上述仿真環(huán)境，分別應(yīng)用DS-VSMM算法和IMM算法，最終得到兩種算法在SINS/DVL組合導(dǎo)航系統(tǒng)中的誤差，仿真結(jié)果如圖6～圖11所示。

圖6 DS-VSMM速度誤差曲線圖

圖7 IMM速度誤差曲線圖

圖8 DS-VSMM位置誤差曲線圖

圖9 IMM位置誤差曲線圖

圖10 DS-VSMM姿態(tài)角誤差曲線圖

圖11 IMM姿態(tài)角誤差曲線圖

由上述仿真結(jié)果曲線對(duì)比可知，在橫滾以及俯仰角誤差方面，應(yīng)用DS-VSMM算法與IMM算法二者無(wú)明顯差別；在速度誤差、位置誤差和航向角誤差方面，應(yīng)用DS-VSMM算法明顯優(yōu)于IMM算法。綜上所述，本文設(shè)計(jì)的基于DS-VSMM的SINS/DVL組合導(dǎo)航算法可以更好地消除洋流速度對(duì)系統(tǒng)的影響，進(jìn)一步提高了組合導(dǎo)航系統(tǒng)的性能。

兩種算法運(yùn)行10次的時(shí)間均值如表2所示，表明本文提出的算法具有更優(yōu)的費(fèi)效比。

表2 兩種算法運(yùn)行10次的時(shí)間均值對(duì)比

IMM算法每次濾波要使用全部13個(gè)子模型濾波，而DS-VSMM算法每次濾波只需要使用相應(yīng)的5個(gè)子模型濾波。DS-VSMM算法運(yùn)行時(shí)間理論上是IMM算法運(yùn)行時(shí)間的38.5%，但由于需要進(jìn)行子模型群切換，進(jìn)而增加了一定的運(yùn)行時(shí)間，因此根據(jù)表2，DS-VSMM算法最終的運(yùn)行時(shí)間是IMM算法的65.4%。綜上所述，基于DS-VSMM算法的SINS/DVL組合導(dǎo)航與應(yīng)用IMM算法相比，降低了計(jì)算量，且進(jìn)一步提高了系統(tǒng)的性能。

5 結(jié)論

本文針對(duì)無(wú)人水下航行器面臨的不確定性洋流速度影響SINS/DVL組合導(dǎo)航系統(tǒng)導(dǎo)航精度的問(wèn)題，提出了一種基于DS-VSMM的組合導(dǎo)航算法。當(dāng)工作在水跟蹤模式下，DVL只能提供相對(duì)于水的速度信息，因此洋流速度會(huì)影響SINS/DVL組合導(dǎo)航系統(tǒng)的性能。為了能夠準(zhǔn)確地對(duì)洋流速度進(jìn)行估計(jì)，本文基于有向圖切換方法，建立了洋流速度模型群，通過(guò)所設(shè)置的模型群切換策略對(duì)模型群進(jìn)行激活和終止，最終實(shí)現(xiàn)了對(duì)洋流速度的準(zhǔn)確估計(jì)，進(jìn)而消除了其對(duì)SINS/DVL組合導(dǎo)航系統(tǒng)的影響。仿真結(jié)果表明：基于DS-VSMM的SINS/DVL組合導(dǎo)航算法的位置誤差控制在50m之內(nèi)，而IMM算法的位置誤差接近300m，前者的定位精度明顯優(yōu)于后者。本文提出的算法可以很好地克服多變的洋流速度引起的導(dǎo)航誤差，同時(shí)減少了一定的計(jì)算量，提高了效費(fèi)比，也提高了導(dǎo)航系統(tǒng)的精度，為進(jìn)一步實(shí)現(xiàn)水下高精度SINS/DVL組合導(dǎo)航提供了參考。為進(jìn)一步提高SINS/DVL組合導(dǎo)航系統(tǒng)的穩(wěn)定性及水下高精度導(dǎo)航能力，還需要更深入的研究：

1)本文提出的算法構(gòu)建的洋流速度模型較少，在實(shí)際應(yīng)用中，可以根據(jù)具體的導(dǎo)航精度要求設(shè)置更多的洋流速度模型；

2)本文的濾波方法為KF，并未進(jìn)行非線性近似，可使用其他非線性濾波方法進(jìn)行同條件仿真，比較不同濾波方法的優(yōu)劣，從而選出最優(yōu)的濾波算法。