本發(fā)明涉及的是一種氣墊船船位推算方法，具體地說(shuō)是一種氣墊船在GPS信號(hào)丟失情況下基于IMM算法的船位推算控制方法。

背景技術(shù)：

由于氣墊船是高速船，在高速航行過(guò)程中對(duì)突發(fā)情況的處理能力較弱，對(duì)其進(jìn)行船位推算是很有必要的。

國(guó)內(nèi)外對(duì)船位推算方法的研究，主要集中在對(duì)歷史船舶位置數(shù)據(jù)的處理，常見(jiàn)處理方法為灰色預(yù)測(cè)，自適應(yīng)卡爾曼等；關(guān)于AUV的航跡推算，彭樹(shù)萍在2011年的哈爾濱工程大學(xué)碩士論文中，提出一種在AUV速度傳感器DVL失效的情況下用AUV運(yùn)動(dòng)學(xué)模型解算的速度信息代替DVL給出的速度信息；而對(duì)于氣墊船的船位推算方法的研究目前還是空白狀態(tài)。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的在于提供一種在氣墊船GPS信號(hào)丟失的情況下能使氣墊船能安全地航行，安全地到達(dá)目的地的氣墊船船位推算控制方法。

本發(fā)明的目的是這樣實(shí)現(xiàn)的：

(1)在氣墊船GPS數(shù)據(jù)丟失的情況下，基于氣墊船的速度及航行信息，用傳統(tǒng)船位推算的方法計(jì)算出船的位置；

(2)根據(jù)氣墊船的運(yùn)動(dòng)參數(shù)，建立勻速模型、勻加速模型、相關(guān)噪聲模型模型，建立基于勻速勻速模型、勻加速模型、相關(guān)噪聲模型模型的互式多模型模型，用互式多模型模型來(lái)描述氣墊船的機(jī)動(dòng)運(yùn)動(dòng)；

(3)根據(jù)先驗(yàn)信息確定馬爾可夫轉(zhuǎn)移概率矩陣，模型間的轉(zhuǎn)換由轉(zhuǎn)移概率矩陣決定；根據(jù)互式多模型算法進(jìn)行循環(huán)運(yùn)算，一個(gè)循環(huán)周期內(nèi)進(jìn)行：狀態(tài)估計(jì)輸入交互、模型濾波處理、模型和狀態(tài)的概率更新、濾波結(jié)果加權(quán)輸出，輸出結(jié)果即為推算的船位；

(4)將推算的船位作為GPS測(cè)量數(shù)據(jù)反饋給航跡控制器，實(shí)現(xiàn)安全航行。

本發(fā)明的氣墊船船位推算控制方法，在氣墊船GPS信號(hào)丟失的情況下，通過(guò)船位推算得到的推算位置代替GPS位置信息，保證了氣墊船能安全地航行，安全地到達(dá)目的地。本發(fā)明的技術(shù)方案的要點(diǎn)在于：

1、根據(jù)氣墊船的航速，艏向角及歷史位置數(shù)據(jù)，通過(guò)傳統(tǒng)推算算法得到實(shí)時(shí)位置估計(jì)數(shù)據(jù)；

2、結(jié)合氣墊船特性建立基于氣墊船的勻速(CV)模型、勻加速(CA)模型及Singer模型；

3、建立交互式多模型(IMM)，設(shè)計(jì)相應(yīng)的強(qiáng)跟蹤卡爾曼濾波器；

4、推算數(shù)據(jù)反饋給航跡控制器，實(shí)現(xiàn)安全航行。

本發(fā)明基于IMM算法結(jié)合氣墊船模型，能實(shí)現(xiàn)高精度的位置推算。船位推算是通過(guò)全墊升氣墊船之前的歷史航行數(shù)據(jù)參數(shù)(實(shí)船的航行方向、航行速度、航行位置以及航行時(shí)間等)來(lái)進(jìn)行船體位置推算，將推算出來(lái)的結(jié)果當(dāng)成信號(hào)丟失時(shí)刻GPS信號(hào)所對(duì)應(yīng)的航行運(yùn)動(dòng)參數(shù)，這樣就可以使船舶正常行駛。IMM算法是使用不同模型匹配目標(biāo)不同運(yùn)動(dòng)狀態(tài)，代表各個(gè)模型的濾波器并行工作，并且假設(shè)各模型之間的切換服從Markov過(guò)程，最終的估計(jì)結(jié)果為各模型估計(jì)的加權(quán)和。本發(fā)明采用IMM算法，建立基于氣墊船的勻速(CV)模型、勻加速(CA)模型及Singer模型，利用強(qiáng)跟蹤卡爾曼濾波算法對(duì)推算結(jié)果進(jìn)行了次最優(yōu)估計(jì)，提高了推算精度。

附圖說(shuō)明

圖1 傳統(tǒng)船位推算方法；

圖2 MMI算法原理圖；

圖3 基于IMM算法的氣墊船船位推算的詳細(xì)流程圖；

圖4 本發(fā)明的原理框圖。

具體實(shí)施方式

下面結(jié)合附圖舉例對(duì)本發(fā)明作進(jìn)一步描述。

1.基于IMM算法的氣墊船船位推算的詳細(xì)流程圖如圖3所示，具體步驟如下：

(1)在氣墊船GPS數(shù)據(jù)丟失的情況下，基于氣墊船的速度及航行信息，用傳統(tǒng)船位推算的方法計(jì)算出船的位置；

(2)根據(jù)氣墊船的運(yùn)動(dòng)參數(shù)，建立相應(yīng)的CV模型，CA模型，Singer模型；建立基于CV，CA，Singer模型的IMM模型，用IMM模型來(lái)描述氣墊船的機(jī)動(dòng)運(yùn)動(dòng)；

(3)根據(jù)先驗(yàn)信息確定馬爾可夫轉(zhuǎn)移概率矩陣，模型間的轉(zhuǎn)換由轉(zhuǎn)移概率矩陣決定；根據(jù)IMM算法進(jìn)行循環(huán)運(yùn)算，一個(gè)循環(huán)周期內(nèi)進(jìn)行：狀態(tài)估計(jì)輸入交互、模型濾波處理、模型和狀態(tài)的概率更新、濾波結(jié)果加權(quán)輸出，輸出結(jié)果即為推算的船位；

(4)將推算數(shù)據(jù)作為GPS測(cè)量數(shù)據(jù)反饋給航跡控制器，實(shí)現(xiàn)安全航行。

2.傳統(tǒng)推算算法過(guò)程如下：

起始位置用(x₀,y₀)表示，通過(guò)GPS可以得到在t_n時(shí)刻的位置，該時(shí)刻的位置用(x_n,y_n)來(lái)表示，通過(guò)下式得到：

在式1中，Δx,Δy分別表示在時(shí)刻t_i-1和t_i單位時(shí)間內(nèi)全墊升氣墊船分別在X軸和Y軸方向上的位移，T是船位推算系統(tǒng)的時(shí)間間隔，V_i和ψ_i分別表示在t_i-1時(shí)刻下的總速度和對(duì)應(yīng)的大地坐標(biāo)系下的航向角。

3.根據(jù)氣墊船運(yùn)動(dòng)參數(shù)建立勻速(CV)模型、勻加速(CA)模型及Singer模型

根據(jù)氣墊船的航速及艏向角，由式(1)簡(jiǎn)單推算出氣墊船的船位，基于以上參數(shù)建立氣墊船的勻速模型、勻加速模型及Singer模型。

(1)勻速(CV)模型、勻加速(CA)模型的建立

CV模型將氣墊船的運(yùn)動(dòng)簡(jiǎn)單地定義為勻速運(yùn)動(dòng)，機(jī)動(dòng)被視為隨機(jī)干擾情況，

當(dāng)氣墊船作勻加速直線運(yùn)動(dòng)時(shí)，則采用三階CA模型

(2)氣墊船運(yùn)動(dòng)的相關(guān)噪聲模型(Singer模型)的建立

設(shè)氣墊船的加速度為a(t)，a(t)是滿足均值為0且指數(shù)相關(guān)的隨機(jī)過(guò)程，即

其中，為運(yùn)動(dòng)目標(biāo)加速度的方差，a為機(jī)動(dòng)時(shí)間常數(shù)的倒數(shù)，稱(chēng)為機(jī)動(dòng)頻率。

其中a_max最大機(jī)動(dòng)加速度，p₀為零加速度時(shí)的發(fā)生概率，p_max為最大加速度時(shí)的發(fā)生概率。

因此，機(jī)動(dòng)加速度可用白噪聲驅(qū)動(dòng)的一階時(shí)間相關(guān)模型表示為

式中W'(t)是方差為的零均值高斯白噪聲。于是Singer模型為

將上式進(jìn)行離散化,可得相應(yīng)的離散時(shí)間模型為

采樣周期h＝0.5s，因此，可以略去階次為h³及以上的項(xiàng)，于是可得系統(tǒng)方程為

式中W'(t)是方差為的零均值高斯白噪聲。

W'(t)的方差矩陣滿足

Q(k)＝E[W(k)W^T(k)]＝[q_ij]_3×3 (9)

4.建立交互式多模型(IMM)，設(shè)計(jì)相應(yīng)的強(qiáng)跟蹤卡爾曼濾波器

建立基于CV，CA，Singer模型的IMM模型，用IMM模型來(lái)描述氣墊船的機(jī)動(dòng)運(yùn)動(dòng)；根據(jù)先驗(yàn)信息確定馬爾可夫轉(zhuǎn)移概率矩陣，模型間的轉(zhuǎn)換由轉(zhuǎn)移概率矩陣決定。在每一時(shí)刻，基于上訴三個(gè)模型的各個(gè)濾波器的初始輸入值是實(shí)際模型轉(zhuǎn)移概率修正后的交互值。

交互式多模型算法有3個(gè)模型，其離散系統(tǒng)的狀態(tài)方程滿足:

X(k+1)＝Φ_j(k)X(k)+W_j(k),j＝1,2，3 (10)

其中，Φ_j(k)是第j個(gè)模型的過(guò)程矩陣；W_j(k)是均值為零、協(xié)方差矩陣為Q_j(k)的白噪聲序列。并設(shè)M＝{m₁,m₁,m₃}為離散系統(tǒng)的3個(gè)模型的集合，假設(shè)k時(shí)刻所處的模型為M(k),k時(shí)刻處在m_j模型時(shí)用m_j(k)表示，則模型的初始狀態(tài)概率

P(m_j(0))＝P(M(0)＝m_j)＝μ_j(0) (11)

模型的一步轉(zhuǎn)移概率為

第j個(gè)模型的觀測(cè)方程為

Z(k)＝H_j(k)X(k)+V_j(k) (13)

其中H_j(k)是第j個(gè)模型的觀測(cè)矩陣；V_j(k)是均值為零，協(xié)方差矩陣為R_j(k)的白噪聲序列。

馬爾科夫鏈的轉(zhuǎn)移概率矩陣為

其中，p_ij是從第i個(gè)模型到第j個(gè)模型的轉(zhuǎn)移概率。

IMM算法的一次循環(huán)可由四個(gè)步驟組成，如圖2所示，分別為:狀態(tài)估計(jì)輸入交互、模型濾波處理、模型和狀態(tài)的概率更新、濾波結(jié)果加權(quán)輸出。

(1)狀態(tài)估計(jì)輸入交互

設(shè)μ_j(k/k-1)為第j個(gè)模型的模型預(yù)測(cè)概率，有

μ_ij(k/k-1)為輸入混合概率

μ_ij(k/k-1)＝P(m_j(k)|Z(k-1))＝π_ijμ_i(k-1) (16)

從而，交互運(yùn)算后，k時(shí)刻濾波器的輸入為

交互協(xié)方差為

(2)模型濾波處理

對(duì)模型進(jìn)行強(qiáng)跟蹤卡爾曼濾波處理，則狀態(tài)預(yù)測(cè)為

協(xié)方差預(yù)測(cè)為

殘差為

殘差協(xié)方差為

S_j(k)＝H_j(k)P_j(k|k-1)(H_j(k))^T+R_j(k) (22)

增益為

K_j(k)＝P_j(k|k-1)(H_j(k))^T(S_j(k))^-1 (23)

β(k+1)是引入的自適應(yīng)漸消因子，利用漸消因子限制卡爾曼濾波器的記憶長(zhǎng)度，調(diào)整狀態(tài)預(yù)報(bào)誤差以及相應(yīng)的增益矩陣，改善濾波器的動(dòng)態(tài)性能。次漸消因子β(k+1)的計(jì)算如下：

其中β₀為：

(3)模型和狀態(tài)的概率更新

濾波后的狀態(tài)更新為

濾波后的狀態(tài)協(xié)方差更新為

P_j(k|k)＝P_j(k|k-1)-k_j(k)S_j(k)(k_j(k))^T (27)

模型概率更新為

其中，A_j(k)為似然函數(shù)，且u_j(k|k-1)是從第i個(gè)模型到第j個(gè)模型的轉(zhuǎn)移概率。

(4)輸出交互

狀態(tài)交互

協(xié)方差交互

5.推算數(shù)據(jù)反饋給航跡控制器，實(shí)現(xiàn)安全航行。

將推算數(shù)據(jù)作為GPS測(cè)量數(shù)據(jù)反饋給航跡控制器，實(shí)現(xiàn)安全航行。

本氣墊船船位推算控制方法主要包括傳統(tǒng)船位推算方法和交互式多模型(IMM)算法。在氣墊船GPS位置信息丟失的情況下，由氣墊船的航速，艏向角，及位置歷史數(shù)據(jù)，根據(jù)氣墊船特性建立的MMI模型，推算出高精度的位置信息，獲得氣墊船的實(shí)時(shí)位置，從而實(shí)現(xiàn)安全航行。

推算過(guò)程分為兩個(gè)部分：第一部分為傳統(tǒng)船位推算方法部分，根據(jù)氣墊船的航速，艏向角及歷史位置數(shù)據(jù)，通過(guò)推算算法得到實(shí)時(shí)位置估計(jì)數(shù)據(jù)；第二部分為結(jié)合氣墊船特性建立基于氣墊船的勻速(CV)模型、勻加速(CA)模型及Singer模型的交互式多模型(IMM)，利用強(qiáng)跟蹤卡爾曼濾波算法對(duì)推算結(jié)果進(jìn)行次最優(yōu)估計(jì)。

基于氣墊船安全航行的考慮，在航行過(guò)程中突然GPS發(fā)生故障的情況下，可以這船位推算的位置信息代替實(shí)船位置。

利用推算的位置信息，反饋給航跡控制器，控制器根據(jù)規(guī)劃航跡給出舵角指令，從而實(shí)現(xiàn)安全航行。

傳統(tǒng)船位推算方法，根據(jù)船體航行方向以及航行速度的運(yùn)動(dòng)參數(shù)預(yù)判下一個(gè)位置的坐標(biāo)的導(dǎo)航過(guò)程，可以給出船體當(dāng)前以及未來(lái)的位置坐標(biāo)的估計(jì)值。

IMM算法模型間的切換是通過(guò)馬爾科夫鏈來(lái)完成的。IMM算法的一次循環(huán)可由四個(gè)步驟組成:狀態(tài)估計(jì)輸入交互、模型濾波處理、模型和狀態(tài)的概率更新、濾波結(jié)果加權(quán)輸出。

基于IMM算法建立強(qiáng)強(qiáng)跟蹤卡爾曼濾波模型，對(duì)推算結(jié)果進(jìn)行次最優(yōu)估計(jì)，實(shí)現(xiàn)高精度的推算船位推算。

船舶航跡控制算法的研究與仿真_楊樹(shù)仁(航跡控制)。