本發(fā)明涉及一種動力浮標(biāo)的自守位控制方法，屬于動力定位領(lǐng)域。

背景技術(shù)：

目前，在對水下目標(biāo)進(jìn)行定位跟蹤時，主要采用按一定陣型定點布放聲吶浮標(biāo)，通過水聲定位方法對目標(biāo)進(jìn)行定位和運動要素解算。由于布放時間的隨機性較大，因此現(xiàn)有聲吶浮標(biāo)多為無動力漂流式浮標(biāo)，這就使得浮標(biāo)在定點作業(yè)時，易受風(fēng)浪以及水流的影響而偏離目標(biāo)點，造成對水下目標(biāo)定位精度差甚至無法定位的后果。在淺海環(huán)境使用時，可通過在浮標(biāo)下方墜錨鉤來使浮標(biāo)穩(wěn)定在目標(biāo)位置，但在深海該方法就會失效。因此該發(fā)明對通過對浮標(biāo)加裝動力系統(tǒng)，設(shè)計了一種可以使浮標(biāo)自動保持在作業(yè)區(qū)域的控制方法。

哈爾濱工程大學(xué)嚴(yán)浙平等在申請?zhí)枮?01610244341.2的專利中給出了一種自守位聲納定位浮標(biāo)，并且對該浮標(biāo)的結(jié)構(gòu)進(jìn)行了詳細(xì)闡述，但是沒有對自守位浮標(biāo)的守位控制方法進(jìn)行描述。該發(fā)明在類似結(jié)構(gòu)浮標(biāo)基礎(chǔ)上設(shè)計了動力浮標(biāo)的自守位控制方法，實現(xiàn)了浮標(biāo)的自守位功能。

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

本發(fā)明的目的是針對聲吶浮標(biāo)在海上作業(yè)過程中，容易受到海浪和海流的作用使得作業(yè)位置發(fā)生變化的問題而提供一種動力浮標(biāo)的自守位控制方法。

本發(fā)明的目的是這樣實現(xiàn)的：在浮標(biāo)的主體的底部設(shè)置有底推進(jìn)器、上端對稱設(shè)置有左推進(jìn)器和右推進(jìn)器，浮標(biāo)頂端設(shè)置有g(shù)ps，包括如下步驟：

步驟1：由浮標(biāo)上搭載的gps獲取浮標(biāo)當(dāng)前位置經(jīng)度λ、當(dāng)前緯度φ、當(dāng)前速度u、當(dāng)前速度方向的信息；

步驟2：由浮標(biāo)當(dāng)前位置經(jīng)度λ、緯度φ和設(shè)定的守位點經(jīng)度λd、緯度φd得出浮標(biāo)距守位點距離d和守位點相對浮標(biāo)的方位角

步驟3：將gps給出的浮標(biāo)當(dāng)速度方向轉(zhuǎn)換到-180°～180°范圍：

步驟4：根據(jù)浮標(biāo)距守位點距離d計算期望速度ud：

ud＝a×(d-d0)

式中：a為常系數(shù)，d0為機動停止半徑；

步驟5：根據(jù)浮標(biāo)距守位點距離d來置位或清零機動標(biāo)志位flag：

式中：d0為機動停止半徑，dmax為守位區(qū)域半徑；

步驟6：根據(jù)浮標(biāo)當(dāng)前速度u、當(dāng)前速度方向期望速度ud和守位點相對浮標(biāo)的方位角得到速度控制器和航向控制器的輸入eu和eφ：

步驟7：分別計算速度控制器輸出f和航向控制器輸出n：

f＝kueu

式中：ku為速度控制器比例控制系數(shù)，kn航向控制器比例控制系數(shù)，β為航向阻尼增益系數(shù)；

步驟8：根據(jù)速度控制器輸出f和航向控制器輸出n進(jìn)行推力分配：

if(nl＜0)nl＝0,nr＝-n

if(nr＜0)nr＝0,nl＝n

if(nl＞m)nl＝m

if(nr＞m)nr＝m

nb＝d×(nl+nr)

式中：nl、nr、nb分別左推進(jìn)器、右推進(jìn)器和底推進(jìn)器的轉(zhuǎn)速，m為推進(jìn)器的最高轉(zhuǎn)速，d為與底推進(jìn)器距浮標(biāo)重心距離有關(guān)的系數(shù)；

步驟9：根據(jù)機動標(biāo)志位flag的值確定是否進(jìn)行推力輸出：若flag＝1，則控制推進(jìn)器輸出步驟8得到的轉(zhuǎn)速；若flag＝0，則推進(jìn)器不輸出轉(zhuǎn)速，重新進(jìn)行第一步。

與現(xiàn)有技術(shù)相比，本發(fā)明的有益效果是：可以在浮標(biāo)漂離守位區(qū)域時，自動控制浮標(biāo)穩(wěn)定駛回守位點，提高了聲吶浮標(biāo)的水下目標(biāo)定位精度，加長了浮標(biāo)的有效作用時間。具有較強的實用性，且在工程實現(xiàn)中簡單易用。

附圖說明

圖1為本發(fā)明所控制浮標(biāo)的結(jié)構(gòu)示意圖；

圖2為本發(fā)明所提控制方法控制結(jié)構(gòu)圖；

圖3為本發(fā)明所提控制方法控制流程圖；

圖4為趨近守位點過程浮標(biāo)位置變化曲線圖；

圖5為趨近守位點過程浮標(biāo)航向變化曲線圖；

圖6為趨近守位點過程浮標(biāo)速度變化曲線圖；

圖7為趨近守位點過程推進(jìn)器的推力變化圖。

具體實施方式

下面結(jié)合附圖與具體實施方式對本發(fā)明作進(jìn)一步詳細(xì)描述。

本發(fā)明通過對gps信息的解算，得到浮標(biāo)當(dāng)前速度、近似航向角以及浮標(biāo)距守位點的距離和方位，根據(jù)這些信息分別設(shè)計了航向和速度控制器，將兩個控制器的輸出通過推力分配算法轉(zhuǎn)化為三個推進(jìn)器的推力。當(dāng)浮標(biāo)距守位點距離超出設(shè)定值時，通過控制推進(jìn)器驅(qū)動使浮標(biāo)自動駛回到守位點。實現(xiàn)將浮標(biāo)保持在設(shè)定的守位區(qū)域內(nèi)，防止了浮標(biāo)受海流海浪的影響漂離作業(yè)區(qū)域。

對gps信息解算如下：

首先由浮標(biāo)上搭載的gps傳感器獲取浮標(biāo)當(dāng)前位置經(jīng)度(λ)緯度(φ)和當(dāng)前速度(u)和速度方向信息。

1.浮標(biāo)距守位點距離d和守位點相對浮標(biāo)的方位角期望速度ud以及機動標(biāo)志位取值計算；

由浮標(biāo)當(dāng)前位置經(jīng)度(λ)緯度(φ)信息和設(shè)定的守位點經(jīng)度(λd)緯度(φd)計算浮標(biāo)距守位點距離d和守位點相對浮標(biāo)的方位角可以通過下面公式計算：

根據(jù)浮標(biāo)距守位點距離d計算期望速度ud＝a×(d-d0)，其中a為常系數(shù)，d0為機動停止半徑，兩個參數(shù)可通過實驗確定最佳取值。

根據(jù)浮標(biāo)距守位點距離d來置位或清零機動標(biāo)志位：

其中，d0為機動停止半徑，dmax為守位區(qū)域半徑。

2、求速度控制器和航向控制器的輸入eu和

將gps給出的浮標(biāo)當(dāng)速度方向轉(zhuǎn)換到(-180°～180°)范圍：

求速度控制器和航向控制器的輸入eu和公式如下：

eu＝ud-u

3、速度控制器和航向控制器的設(shè)計。

在浮標(biāo)守位過程中對速度精度要求較低，只要滿足在趨近守位點的過程速度逐漸降低，以防止反向超出機動停止區(qū)，導(dǎo)致浮標(biāo)不斷往復(fù)機動。因此速度控制器采用簡單的比例控制方法，其控制律為：

f＝kueu

其中：ku為速度控制器比例控制系數(shù)。

由于浮標(biāo)長寬比較小，從而導(dǎo)致浮標(biāo)的航向穩(wěn)定性較差。航向控制器通過加入航向偏差角的微分項，提高了浮標(biāo)在機動過程中的偏航阻尼，提高了航行穩(wěn)定性。航向控制器控制律為：

其中：kn航向控制器比例控制系數(shù)，β為航向阻尼增益系數(shù)。

4、推力分配設(shè)計

根據(jù)速度控制器輸出f和航向控制器輸出n進(jìn)行推力分配，分配原則為各推進(jìn)器轉(zhuǎn)速保證在0到最大轉(zhuǎn)速之間，航向調(diào)節(jié)優(yōu)于航速調(diào)節(jié)，分配的具體方法如下：

if(nl＜0)nl＝0,nr＝-n

if(nr＜0)nr＝0,nl＝n

if(nl＞m)nl＝m

if(nr＞m)nr＝m

nb＝d×(nl+nr)

其中：nl、nr、nb分別為左推進(jìn)器，右推進(jìn)器和底推進(jìn)器轉(zhuǎn)速；m為推進(jìn)器的最高轉(zhuǎn)速；d為與底部推進(jìn)器距浮標(biāo)重心距離有關(guān)的系數(shù)。

根據(jù)機動允許標(biāo)志位flag的值確定是否進(jìn)行推力輸出。若flag＝1，則控制推進(jìn)器輸出轉(zhuǎn)速；若flag＝0，則推進(jìn)器不輸出轉(zhuǎn)速。

下面結(jié)合附圖對本發(fā)明進(jìn)行詳細(xì)的描述：

圖1為動力浮標(biāo)的結(jié)構(gòu)圖。圖1中的裝置包括：浮標(biāo)主體；2左推進(jìn)器；3右推進(jìn)器；4.底部推進(jìn)器；5.gps天線；6.通訊天線；7.天線桿；8.水聽器。通過gps天線解算獲取浮標(biāo)的位置、速度和航向信息；根據(jù)通訊天線接收到守位點位置坐標(biāo)和浮標(biāo)的位置計算如何驅(qū)動浮標(biāo)運動；通過左右底三個推進(jìn)器共同推動浮標(biāo)向前運動，通過左右兩個推進(jìn)器的推力差控制浮標(biāo)轉(zhuǎn)向運動。

圖2為動力浮標(biāo)自守衛(wèi)控制方法控制結(jié)構(gòu)圖，圖3為浮標(biāo)自守衛(wèi)控制方法控制流程圖

自守位控制原理為，根據(jù)浮標(biāo)上gps信息和守位點經(jīng)緯度，解算出浮標(biāo)當(dāng)前位置、速度、航向，并根據(jù)浮標(biāo)與守位點相對位置給出浮標(biāo)期望航向與期望航速。分別由航向控制器和航速控制器對浮標(biāo)的航向和航速進(jìn)行控制。依據(jù)一定的法則將控制器輸出對各推進(jìn)器推力進(jìn)行分配。

執(zhí)行步驟具體如下：

101：gps傳感器獲取浮標(biāo)當(dāng)前位置經(jīng)度(λ)緯度(φ)和當(dāng)前速度(u)和速度方向

102：計算浮標(biāo)距守位點距離d和守位點相對浮標(biāo)的方位角期望速度ud以及機動標(biāo)志位取值；

浮標(biāo)距守位點距離

浮標(biāo)期望航向角

浮標(biāo)期望速度ud＝a×(d-d0)，其中a為常系數(shù)，d0為機動停止半徑，兩個參數(shù)可通過實驗確定最佳取值。

103：機動標(biāo)志位

其中：d0為機動停止半徑，dmax為守位區(qū)域半徑。

104：求取速度控制器和航向控制器的輸入eu和即期望航速與實際航速的偏差和期望航向與實際航向偏差。

首先將gps給出的浮標(biāo)當(dāng)速度方向轉(zhuǎn)換到(-180°～180°)范圍內(nèi)：

求速度控制器和航向控制器的輸入eu和公式如下：

eu＝ud-u

105：根據(jù)輸入eu和求取速度控制器和航向控制器輸出：

f＝kueu

其中：ku為速度控制器比例控制系數(shù)，kn航向控制器比例控制系數(shù)，β為航向阻尼增益系數(shù)。

106：根據(jù)速度控制器輸出f和航向控制器輸出n進(jìn)行推力分配，由于推進(jìn)器只能正轉(zhuǎn)，且轉(zhuǎn)速不能超過最大轉(zhuǎn)速，因此推力分配要保證各推進(jìn)器轉(zhuǎn)速在0到最大轉(zhuǎn)速m之間。在守位過程中航向控制對守位效果影響更大，所以在航向調(diào)節(jié)與速度調(diào)節(jié)沖突時，航向調(diào)節(jié)應(yīng)該優(yōu)于優(yōu)于航速調(diào)節(jié)，推力分配的具體方法如下：

if(nl＜0)nl＝0,nr＝-n

if(nr＜0)nr＝0,nl＝n

if(nl＞m)nl＝m

if(nr＞m)nr＝m

nb＝d×(nl+nr)

其中nl，nr，nb，分別為左推進(jìn)器，右推進(jìn)器和底推進(jìn)器轉(zhuǎn)速；m為推進(jìn)器的最高轉(zhuǎn)速；d為與底部推進(jìn)器距浮標(biāo)重心距離有關(guān)的系數(shù)。

107：根據(jù)機動允許標(biāo)志位flag的值確定是否進(jìn)行推力輸出。若flag＝0，則推進(jìn)器不輸出轉(zhuǎn)速。

108：若flag＝1，則控制推進(jìn)器輸出轉(zhuǎn)速；完成一個時間拍的控制作用后，轉(zhuǎn)回到101開始下一個時間拍對浮標(biāo)的守位控制。

在仿真平臺下，使浮標(biāo)在北東坐標(biāo)下的初始位置(50m，50m)、初始速度(0m/s，0/s，0rad/s)守位位置(0m，0m)，漂移速度(0.038m/s，0.034m/s)；

機動停止半徑d0取5米，守位區(qū)域半徑dmax取50米。常系數(shù)a取0.1。速度控制器比例控制系數(shù)ku取5，航向控制器比例控制系數(shù)kn取10，航向阻尼增益系數(shù)β取4，推進(jìn)器的最高轉(zhuǎn)速m取1000；系數(shù)d取0.4。

仿真結(jié)果如圖4-圖7，圖4顯示了浮標(biāo)位置在漂移力作用下在超出守位區(qū)域，然后機動的回到守位點附近的位置變化曲線；圖5為航向曲線，由圖中可見發(fā)生機動后浮標(biāo)航向逐漸收斂到目標(biāo)航向；圖6為速度曲線，由圖中可見開始機動后浮標(biāo)加速到最高航速，隨之浮標(biāo)逐漸接近守位點，航速也逐漸下降；圖7為推進(jìn)器轉(zhuǎn)速曲線，由圖中可見左右推進(jìn)器差動保持航向，三個推進(jìn)器共同保持航速，隨之浮標(biāo)逐漸接近守位點，三個推進(jìn)器轉(zhuǎn)速逐漸下降到零。綜合來看該控制方法較好的實現(xiàn)了動力浮標(biāo)的自守位控制。