本發(fā)明涉及的是一種無人艇安全航行船位推算方法。

背景技術(shù)：

無人艇作為一種新型高性能船，隨著其在航運和科學研究等領(lǐng)域越來越廣泛的應(yīng)用，各種各樣的無人艇也應(yīng)運而生。因為無人艇通常在較高速度下航行，所以其容易受到惡劣環(huán)境的影響，穩(wěn)定性能不好，在航行軌跡上產(chǎn)生偏離導致實際航行路線和預期航行軌跡出現(xiàn)誤差。GPS信號用來對船舶進行定位，針對其航行誤差進行修正，但是在環(huán)境干擾下容易導致無人艇的GPS信號數(shù)據(jù)丟失，若在GPS信號丟失的情況下船舶面臨碰撞，觸礁等危險情況時，無人艇很難進行避讓。現(xiàn)階段主要是通過基于卡爾曼濾波進行船位推算來對航行軌跡進行預估，船位推算指的是通過無人艇之前的歷史航行數(shù)據(jù)參數(shù)(實船的航行方向、航行速度以及航行時間等)來進行船體位置推算，將推算出來的結(jié)果當成信號丟失時刻GPS信號所對應(yīng)的航行運動參數(shù)，這樣就可以使船舶正常行駛。

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

本發(fā)明的目的在于提供一種可以在GPS信號丟失情況下推算出無人艇的安全航行船位的無人艇安全航行船位推算方法。

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

步驟一，獲取無人艇數(shù)據(jù)；

步驟二，利用最小二乘法多項式擬合原理對獲取的原始數(shù)據(jù)進行濾波，消除野值；

步驟三，進行強跟蹤卡爾曼濾波；

步驟四，從GPS信號丟失時刻的位置開始，根據(jù)該點的航行速度、航行方向和航行時間對下一時刻的位置信息進行推算。

本發(fā)明還可以包括：

1、所述消除野值的具體步驟為：

①對已知的測量數(shù)據(jù)(x_i,y_i)(i＝1,2,...,n)，選取合適的次數(shù)n進行最小二乘多項式擬合。其中x_i為海流測量時間，y_i為海流速度大小；

②求出系數(shù)矩陣α₀,α₁,…,α_n，得到擬合多項式

③通過求得的多項式推算出相對應(yīng)的擬合值p_i(i＝1,2,...,n)；

④將求出的擬合值與實際值做差，生成擬合殘差序列y＝p_i-y_j(j＝1,2,...n)，

⑤計算擬合殘差序列的均方誤差σ：

⑥用3σ準則判斷并剔除野值,剔除野值后的數(shù)據(jù)為y_j(j＝1,2,...,m,m≤n)：

2、所述強跟蹤卡爾曼濾波的濾波器具有如下一般結(jié)構(gòu):

其中k——采樣時刻；

Y(k+1)——為濾波器的量測值；

V(k+1)——為估計殘差；

K(k+1)——為時變增益陣。

最后通過引入漸消因子λ(k+1)來實現(xiàn)強跟蹤濾波器的特性。

3、所述根據(jù)該點的航行速度、航行方向和航行時間對下一時刻的位置信息進行推算具體包括：

設(shè)已知起始位置已知，無人艇根據(jù)無人艇的運動參數(shù)經(jīng)過系統(tǒng)的采樣時間，計算得到無人艇下一時刻的位置，起始位置用(x₀,y₀)表示，通過GPS得到在t_n時刻的位置用(x_n,y_n)來表示，則無人艇載體航行速度在正東方向和正北方向的分量如下：

結(jié)合圖2通過下式得到：

式中v_E、v_N——分別為載體航行速度在正東方向和正北方向的分量；

v_x、v_y——分別為DVL測得的載體相對大地的前向速度和左向速度；

ψ_i——無人艇載體的航向角,順時針為正，逆時針為負，可由羅經(jīng)測得。

Δx,Δy——分別表示在時刻t_i-1和t_i單位時間內(nèi)無人艇分別在X軸和Y軸方向上的位移差；

T——船位推算的時間間隔；

利用強跟蹤卡爾曼濾波后：

則狀態(tài)方程為：

4、獲取無人艇數(shù)據(jù)包括利用姿態(tài)傳感器測量船的航向、回轉(zhuǎn)率、縱搖、橫搖、垂向加速度；利用位置參考系統(tǒng)測量無人艇的位置；利用風傳感器測量風速和風向器。

本發(fā)明提供了一種無人艇可以在GPS信號丟失情況下安全航行的船位推算方法。主要包括：

1、無人艇數(shù)據(jù)獲取

無人艇由外殼、推進器、DVL、姿態(tài)傳感器、羅經(jīng)、GPS和控制計算機組成，在運動時可通過以下幾種系統(tǒng)實時獲取數(shù)據(jù)：利用姿態(tài)傳感器測量船的航向、回轉(zhuǎn)率、縱搖、橫搖、垂向加速度；利用位置參考系統(tǒng)測量無人艇的位置；利用風傳感器測量風速和風向器。

2、數(shù)據(jù)預處理

利用最小二乘法多項式擬合原理對獲取的原始數(shù)據(jù)進行濾波，消除野值。

3、強跟蹤卡爾曼濾波

利用卡爾曼濾波原理，對無人艇設(shè)計一個強跟蹤卡爾曼濾波，根據(jù)此方法可以對無人艇進行船位推算。

4、無人艇位置信息的推算

無人艇在GPS信號丟失的情況下為了正常安全航行，從GPS信號丟失時刻的位置開始，根據(jù)該點的航行速度、航行方向和航行時間對下一時刻的位置信息進行合理推算。

5、模擬無人艇航行

通過Matlab軟件模擬GPS信號正常傳輸和丟失情況下無人艇的航行情況，分析基于強跟蹤卡爾曼濾波的船位推算方法是否適用與無人艇在水面的航行。

本發(fā)明相對現(xiàn)有技術(shù)具有如下的優(yōu)點及效果：

1.本發(fā)明能夠?qū)崿F(xiàn)對無人艇獲取的數(shù)據(jù)信息進行基于最小二乘法的數(shù)據(jù)預處理；

2.本發(fā)明能夠?qū)崿F(xiàn)無人艇在GPS信號丟失的情況下，采用基于強跟蹤卡爾曼濾波的船位推算方法對下一時刻位置信息進行推測。

3.通過仿真可以看出此方法可以對危險進行規(guī)避，具有實用性。

附圖說明

圖1是本發(fā)明的無人艇船位推算系統(tǒng)圖；

圖2是本發(fā)明的無人艇位置推算示意圖；

圖3是本發(fā)明的無人艇的GPS信號正常傳輸時的航跡圖。

具體實施方式

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

附圖2是本發(fā)明的無人艇位置推算示意圖，已知起始位置(x₀,y₀)、總速度V_i和對應(yīng)的大地坐標系下的航向角ψ_i，可以推測下一時刻的位置信息。

無人艇運動學方程以向量形式表示：

其中位置向量為η＝[x y ψ]^T。通常在描述船舶或半潛平臺的水面運動時只需用到縱蕩、橫蕩和艏搖三個方向的運動，因此簡化后的無人艇三自由度運動學方程如下：

式中x，y——無人艇在橫縱軸位置

u，v——無人艇在橫縱方向的速度

r——無人艇的艏搖角速度

ψ——無人艇的艏搖角度

利用無人艇的運動數(shù)學模型模擬實船的航行情況，為后續(xù)的研究提供理論基礎(chǔ)。

本發(fā)明的無人艇安全航行船位推算方法主要包括如下步驟：

步驟一：無人艇數(shù)據(jù)獲取

利用姿態(tài)傳感器測量船的航向、回轉(zhuǎn)率、縱搖、橫搖、垂向加速度；位置參考系統(tǒng)包括GPS，北斗導航系統(tǒng)，利用位置參考系統(tǒng)測量無人艇的位置；利用風傳感測量風速和風向。

步驟二：數(shù)據(jù)預處理

無人艇在水面航行的時候，由于儀器的自噪聲以及姿態(tài)、位置等傳感器的誤差，特別是海面上的風浪對無人艇的航行有較為明顯的影響，使得獲取的數(shù)據(jù)中有異常數(shù)據(jù)(野值)和隨機誤差。為了保證得到的數(shù)據(jù)更接近真實數(shù)據(jù)，則必須對這些原始數(shù)據(jù)進行濾波，消除野值，并對測量的隨機誤差進行平滑修正，提高數(shù)據(jù)的精度。

利用最小二乘法多項式擬合原理對獲取的原始數(shù)據(jù)進行濾波，需要對已知的觀測數(shù)據(jù)擬合一個多項式，再利用三倍擬合殘差序列的均方誤差準則進行判別剔除，較其它方法易于操作。該方法對數(shù)據(jù)野值剔除步驟如下：

①利用步驟一中的測量值(x_i,y_i)(i＝1,2,...,n)，選取合適的次數(shù)n進行最小二乘多項式擬合，其中x_i為海流測量時間，y_i為海流速度大??；

②求出系數(shù)矩陣α₀,α₁,…,α_n，得到擬合多項式

③通過求得的多項式推算出相對應(yīng)的擬合值p_i(i＝1,2,...,n)；

④將求出的你何止與實際值做差，生成擬合殘差序列y＝p_i-y_j(j＝1,2,...n)，

⑤計算擬合殘差序列的均方誤差σ：

⑥用3σ準則判斷并剔除野值,剔除野值后的數(shù)據(jù)為y_j(j＝1,2,...,m,m≤n)：

根據(jù)式(2)可知，若殘差值小于門限值，為正常值，此值不變；若大于或等于門限值，則判斷為野值，用與其相鄰的六個數(shù)據(jù)的均值代替此野值。門限值3σ可根據(jù)實驗具體情況做出適當?shù)母?，注意在第一步求解過程中多項式系數(shù)n的確定需要結(jié)合實際的數(shù)據(jù)情況，選取最終擬合曲線結(jié)果最好的一組。

采用最小二乘法擬合的數(shù)據(jù)預處理方法對采集數(shù)據(jù)中的野值進行提出，保證采集數(shù)據(jù)更符合真實情況，減小誤差對實船航行的影響。

步驟三：強跟蹤卡爾曼濾波

卡爾曼濾波的濾波可以分為兩個部分，分別是狀態(tài)空間模型和觀測模型。狀態(tài)模型可以通過狀態(tài)方程來形容相近兩時刻內(nèi)的狀態(tài)變化規(guī)律，是用來對狀態(tài)變化規(guī)律進行反映的模型。用觀測模型來對真實觀測量和狀態(tài)變量的關(guān)系進行反映。強跟蹤濾波器的概念,它與通常的濾波器相比具有以下優(yōu)良特性:

1)較強的關(guān)于模型參數(shù)失配的魯棒性.

2)較低的關(guān)于噪聲及初值統(tǒng)計特性的敏感性.

3)極強的關(guān)于突變狀態(tài)的跟蹤能力,并在濾波器達到穩(wěn)態(tài)時保持這種能力.

4)適中的計算復雜性

一類強跟蹤卡爾曼濾波器應(yīng)具有如下一般結(jié)構(gòu):

式中k——采樣時刻；

Y(k+1)——為濾波器的量測值；

V(k+1)——為估計殘差；

K(k+1)——為時變增益陣。

最后通過引入漸消因子λ(k+1)來實現(xiàn)強跟蹤濾波器的特性。

卡爾曼濾波問題就是利用線性系統(tǒng)狀態(tài)方程來進行最優(yōu)估計。

步驟四：無人艇位置信息的推算

無人艇航行時經(jīng)常使用基于多普勒測速儀(DVL)、慣性測量單元(IMU)和全球定位系統(tǒng)(GPS)數(shù)據(jù)的傳統(tǒng)航位推算算法。DVL測得的速度一般是船體坐標系下的速度，為了推算無人艇航行時載體的經(jīng)緯度，需要把船體坐標系下無人艇的速度轉(zhuǎn)化為北東坐標系下的速度。假設(shè)已知起始位置已知，無人艇可以根據(jù)無人艇的運動參數(shù)(船體的航行方向以及船體的航行速度)經(jīng)過系統(tǒng)的采樣時間(0.3～0.5秒)計算得到無人艇下一時刻的位置。起始位置用(x0,y₀)表示，通過GPS可以得到在t_n時刻的位置，該時刻的位置用(x_n,y_n)來表示。

無人艇載體航行速度在正東方向和正北方向的分量計算如下：

結(jié)合圖2通過下式得到：

式中v_E、v_N——分別為載體航行速度在正東方向和正北方向的分量；

v_x、v_y——分別為DVL測得的載體相對大地的前向速度和左向速度；

ψ_i——無人艇載體的航向角,順時針為正，逆時針為負，可由羅經(jīng)測得。

Δx,Δy——分別表示在時刻t_i-1和t_i單位時間內(nèi)無人艇分別在X軸和Y軸方向上的位移差；

T——船位推算的時間間隔；

如果想利用強跟蹤卡爾曼濾波對船位推算進行過預測，可以先推導出正確的狀態(tài)方程和測量方程。根據(jù)船位推算的原理方程式(7)，可以得到最終結(jié)果方程式：

則狀態(tài)方程為：

利用本發(fā)明的推算結(jié)果模擬無人艇航行

GPS信號用來對船舶位置進行定位，在GPS信號正常傳輸?shù)那闆r下，無人艇根據(jù)GPS信號給出的運動參數(shù)可以正常按照預定的航行軌跡航行(如圖3所示)。假設(shè)當在A點處GPS信號丟失，無人艇運行的位置無法確定，此時可以通過船位推算針對初始使其無人艇的航行方向、速度以及時間進行估計下一時刻無人艇的位置，從無人艇GPS信號丟失到恢復GPS信號的過程中無人艇能夠平穩(wěn)的按照預期的估計滑行。經(jīng)過船位推算后的曲線和GPS信號正常傳輸時候的航行曲線是沒有太明顯的差異的，也可以理解為在GPS信號丟失的時候可以根據(jù)基于強跟蹤卡爾曼濾波的船位推算原理對下一時間的航行位置進行估計，防止GPS信號丟失到恢復的過程中無人艇無法航行或者出現(xiàn)其他危險。通過仿真驗證，可以確定選擇此方法可以使無人艇安全航行，在GPS信號丟失時間內(nèi)讓無人艇可以按照預期的軌跡航行。