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

背景技術(shù)：

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

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

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

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

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

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

步驟三，進(jìn)行強(qiáng)跟蹤卡爾曼濾波；

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

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

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

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

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

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

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

⑤計(jì)算擬合殘差序列的均方誤差σ：

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

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

其中k——采樣時刻；

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

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

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

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

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

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

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

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

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

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

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

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

利用強(qiáng)跟蹤卡爾曼濾波后：

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

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

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

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

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

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

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

3、強(qiáng)跟蹤卡爾曼濾波

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

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

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

5、模擬無人艇航行

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

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

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

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

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

附圖說明

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

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

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

具體實(shí)施方式

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

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

無人艇運(yùn)動學(xué)方程以向量形式表示：

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

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

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

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

ψ——無人艇的艏搖角度

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

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

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

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

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

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

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

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

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

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

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

⑤計(jì)算擬合殘差序列的均方誤差σ：

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

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

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

步驟三：強(qiáng)跟蹤卡爾曼濾波

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

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

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

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

4)適中的計(jì)算復(fù)雜性

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

式中k——采樣時刻；

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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