本發(fā)明涉及一種基于三維模糊ptp算法的單體機器魚行為控制策略，屬于智能控制技術領域。

背景技術：

機器人運動規(guī)劃問題是任何機器人研究者都不會忽略的問題，運動規(guī)劃對機器人高效運動有重要作用。然而，水下機器魚運動規(guī)劃同陸地機器人相比具有特殊的難點：

(1)機器魚特殊的運動學模型帶來的難點，即由于慣性作用，機器魚在水中無法立刻停止，或者像陸地機器人一樣自由的后退。再者，目前仍然未能為機器魚的運動建立精確的動力學模型，因此發(fā)出控制命令后，只能大致地預測機器魚的反應；

(2)同陸地環(huán)境相比，水下環(huán)境具有更多的不確定性及干擾。當機器魚游動時帶來的擾動水波，不僅將影響到機器魚本身及其他機器魚的運動，而且會影響到傳感器的精確測量；

(3)另外，被運輸?shù)奈矬w可能會由于水波的沖擊發(fā)生漂移和轉(zhuǎn)動，這將會給機器魚的運輸運動控制帶來很大的困難。因此，大多數(shù)陸地機器人的協(xié)作算法不能直接應用到機器魚上，需要提出更新的、抗干擾能力更強的控制算法。

在機器魚頂球的任務中，要求機器魚在水池環(huán)境中，將半徑為r的水球從初始位置移動到目標位置，沒有其他輔助裝置，機器魚只允許通過頭部或尾鰭頂球。理論上來講，是運動規(guī)劃和行為選擇問題，不僅涉及到底層的運動控制，而且涉及到上層的規(guī)劃和控制。因為水中環(huán)境會因機器魚的游動而產(chǎn)生水波，因此采集水中環(huán)境信息，即機器魚、水球、目標點的位置和角度信息，進行圖像識別、處理是很重要的。

本專利假設初始情況下水球和機器魚的位置可以是任意的?，F(xiàn)有針對單體機器魚頂球運動規(guī)劃問題雖然能夠使得機器魚很快找到水球位置，該行為是游向行為，該算法較為復雜，尤其針對機器魚轉(zhuǎn)彎策略設計中，較難實現(xiàn)。因此，在運動性能提升方面及算法復雜度方面均存在很大改進空間。

本專利致力于從運動學角度對單體機器魚頂球運動規(guī)劃問題進行了改進，采用面向不同“區(qū)域”的控制算法，實現(xiàn)機器魚頂球到達目標點的簡單任務。該算法的基本思想是：根據(jù)頂球點劃分水池區(qū)域，設計了對應區(qū)域的具體行為。

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

本發(fā)明的目的在于克服現(xiàn)有單體機器魚頂球運動規(guī)劃算法較為復雜且轉(zhuǎn)彎策略難以實現(xiàn)的技術缺陷，提出了一種基于三維模糊ptp算法的單體機器魚行為控制策略。

本發(fā)明的核心思想為：采用面向不同“區(qū)域”的控制算法，實現(xiàn)機器魚頂球到達目標點的簡單任務；具體根據(jù)頂球點劃分水池區(qū)域，設計了機器魚在對應區(qū)域的頂球運動行為。

一種基于三維模糊ptp算法的改進單體機器魚行為控制策略，包括如下步驟：

步驟1初始化采樣時間、劃分水池區(qū)域以及頂球點位置為主的參數(shù)，具體為：

步驟1.1初始化采樣時間δt；

步驟1.2將機器魚進行水下運動所處的水池區(qū)域劃分為區(qū)域1、區(qū)域2、區(qū)域3和區(qū)域4；

其中，區(qū)域1為進攻區(qū)pr(pushingregion)；

區(qū)域2為上邊界區(qū)ubr(upperboundaryoftheregion)；

區(qū)域3為下邊界區(qū)lbr(lowerboundaryoftheregion)；

區(qū)域4為射門區(qū)gr(goalregion)；

步驟1.3設定不同區(qū)域?qū)捻斍螯c坐標；

其中，頂球點，記為point點；對應于步驟1.2的不同水池區(qū)域設定的頂球點位置如下：

(1)向下頂球點：對應上邊界區(qū)ubr，設定的向下頂球點坐標為(b_pt.x,b_pt.y-r)；

(2)向右頂球點：對應進攻區(qū)pr，設定的向右頂球點坐標為(b_pt.x-r,b_pt.y)；

(3)向上頂球點：對應下邊界區(qū)lbr，設定的向上頂球點坐標為(b_pt.x,b_pt.y+r)；

(4)射門頂球點：對應射門區(qū)gr，設定的射門頂球點坐標為：

(b_pt.x-rcos(b_to_g_dir),b_pt.y-rsin(b_to_g_dir))；

步驟2更新機器魚的位置、方向信息及環(huán)境信息，計算機器魚同頂球點的距離差和方向差，并判斷機器魚是否到達目標點，決定是否結束本發(fā)明所提策略，具體為：

其中，距離差，記為l；方向差，記為θe；

若(l＜δl)∩(θe＜δθe)，表明機器魚已經(jīng)到達目標點，結束本發(fā)明所提策略；否則，轉(zhuǎn)步驟3；

其中，δl為距離絕對誤差，δθe為方向絕對誤差；

步驟3根據(jù)步驟2機器魚的方向差，提取轉(zhuǎn)角，對機器魚進行方向控制，即決定調(diào)用轉(zhuǎn)彎模態(tài)還是速度策略，具體為：

步驟3.1獲取機器魚當前的方向差θe，并根據(jù)方向差與角度閾值的大小關系，進行如下操作：

其中，角度閾值，記為θf，其取值范圍為85到95度，優(yōu)選的θf為90度；

3.1a獲取機器魚當前的方向差θe，如果|θe|＜θf，轉(zhuǎn)步驟4調(diào)用設計的機器魚速度策略；

3.1b若|θe|≥θf，則轉(zhuǎn)步驟3.2；

步驟3.2調(diào)用轉(zhuǎn)彎模態(tài)進行大幅度方向改變；

其中，轉(zhuǎn)彎模態(tài)，記為mode1，具體為：

3.2a若θe＜-θf，機器魚進行左急轉(zhuǎn)彎，轉(zhuǎn)角為負60度到負30度，轉(zhuǎn)步驟4；

3.2b若θe＞θf，機器魚進行右急轉(zhuǎn)彎，轉(zhuǎn)角為30度到60度，轉(zhuǎn)步驟4；

步驟4：根據(jù)步驟2中距離差進行速度控制，設計機器魚的速度策略，具體為：

4.1若距離差滿足l＞ld，則設置機器魚的速度為speed＝15，跳至步驟5；

4.2若距離差滿足ls＜l≤ld，則設置機器魚的速度為speed＝12，跳至步驟5；

4.3若距離差l≤ls，則設置機器魚的速度為speed＝8，跳至步驟5；

其中，ls為近距離閾值，ld為遠距離閾值，且ls＜ld；

步驟5：將機器魚的轉(zhuǎn)角、速度值和采樣時間進行三維模糊化處理，轉(zhuǎn)化為不同的控制模式，通過無線方式發(fā)送給機器魚，轉(zhuǎn)步驟2，具體為：

步驟5.1對機器魚的轉(zhuǎn)角、速度值和采樣時間進行三維模糊化處理，具體為：

5.1a在不同的采樣時間下，機器魚的轉(zhuǎn)角和速度值劃分區(qū)間不同；

5.1b將轉(zhuǎn)角、速度和采樣時間轉(zhuǎn)換為運動模式；

其中，運動模式用(θa，va，ta)表示,θa為角度檔位，va為速度檔位，ta為采樣時間檔位；

其中，三維模糊化處理遵循如下規(guī)則：在如下條件下對轉(zhuǎn)角、速度、采樣時間模糊化處理，轉(zhuǎn)換為同一運動模式：⑴相同的區(qū)間，不同的采樣時間，⑵相同的速度區(qū)間，不同的速度值，⑶相同的轉(zhuǎn)角區(qū)間，不同的轉(zhuǎn)角值；而不同的轉(zhuǎn)角區(qū)間、速度區(qū)間或采樣時間區(qū)間，對轉(zhuǎn)角、速度或采樣時間模糊化處理，轉(zhuǎn)換為不同的運動模式；

步驟5.2規(guī)劃機器魚頂球行為，即：針對步驟5.1輸出的機器魚所處區(qū)域及其運動模式，確定機器魚的頂球行為；

其中，頂球行為由一系列動作組成，如實施例4所示；

步驟5.3規(guī)劃機器魚的基本動作，即：針對步驟5.2中機器魚的頂球行為確定機器魚的機器魚動作；

其中，機器魚的機器魚動作由電機控制；

步驟5.4確定機器魚的控制參數(shù)：將步驟5.3規(guī)劃的機器魚的基本動作轉(zhuǎn)換為電機控制參數(shù)；

步驟5.5將步驟5.4輸出的電機控制參數(shù)經(jīng)無線方式傳給機器魚，控制機器魚的頂球行為，轉(zhuǎn)步驟2；

至此，經(jīng)過了步驟1到步驟5，完成了一種基于三維模糊ptp算法的單體機器魚行為控制策略。

有益效果

一種基于三維模糊ptp算法的單體機器魚行為控制策略，與現(xiàn)有的單體機器魚行為控制方法相比，具有如下有益效果：

1.本發(fā)明針對不同區(qū)域劃分設計相應的機器魚行為控制策略，可適應于多種機器魚在含有障礙、噪音等多種比較復雜的水下環(huán)境中，應用范圍廣；

2.本發(fā)明所提策略中將速度、角度、采樣時間，采用三維模糊化處理，并轉(zhuǎn)化為控制模式，便于機器魚控制模式的直接調(diào)用，大大簡化了算法流程，降低了算法的復雜度；

3.本發(fā)明所提策略中使用的基于三維模糊的ptp算法，該算法流程簡單，因此響應速度快，機器魚靈敏度高，機器魚的運動性能得到進一步提高；

4.本發(fā)明所提策略簡化了大量的運算過程，可以設定較高的采樣頻率，具有數(shù)據(jù)更新速度快、精確度高以及實時性好的優(yōu)勢。

附圖說明

圖1是本發(fā)明一種基于三維模糊ptp算法的單體機器魚行為控制策略的步驟1及實施例1中不同水池區(qū)域劃分及相應區(qū)域頂球點設定；

圖2是本發(fā)明一種基于三維模糊ptp算法的單體機器魚行為控制策略及實施例2中的總體流程圖；

圖3是本發(fā)明一種基于三維模糊ptp算法的單體機器魚行為控制策略的實施例3中機器魚不同區(qū)域頂球行為的示意圖；

圖3(a)bpr行為示意圖(有效進攻情況)，圖3(b)bpr行為示意圖(過沖情況)，圖3(c)bubr行為示意圖，圖3(d)blbr行為示意圖，圖3(e)bgr行為示意圖；

圖4是本發(fā)明一種基于三維模糊ptp算法的單體機器魚行為控制策略實施例4中機器魚頂球動作示意圖；

圖4(a)向下頂球動作示意圖，圖4(b)向下頂球動作示意圖，圖4(c)射門動作示意圖

圖5是本發(fā)明一種基于三維模糊ptp算法的單體機器魚行為控制策略實施例4中機器魚過程動作示意圖；

圖5(a)到達臨時返回點后轉(zhuǎn)彎動作示意圖，圖5(b)尾部波動推進水球動作示意圖。

具體實施方式

下面結合附圖和實施例對本發(fā)明做進一步說明和詳細描述。

實施例1

本實施例敘述了在本發(fā)明在具體實施時針對不同水池區(qū)域劃分及相應區(qū)域頂球點設定。圖1是本發(fā)明一種基于三維模糊ptp算法的單體機器魚行為控制策略的步驟1中不同水池區(qū)域劃分及相應區(qū)域頂球點設定。圖1中，oxy為直角坐標系。

從圖1中可以看出，水池區(qū)域被劃分為區(qū)域1、區(qū)域2、區(qū)域3和區(qū)域4；每一個區(qū)域中都有一個球，球上有個黑點即是頂球點，不同區(qū)域中設定的頂球點坐標，區(qū)域1中包含向右頂球點，其坐標為(b_pt.x-r,b_pt.y)；區(qū)域2中包含向下頂球點，其坐標為(b_pt.x,b_pt.y-r)；區(qū)域3中包含向上頂球點，其坐標為(b_pt.x,b_pt.y+r)；區(qū)域4中包含射門頂球點，其坐標為(b_pt.x-rcos(b_to_g_dir),b_pt.y-rsin(b_to_g_dir))。其中cos(b_to_g_dir)是球心坐標與目標點之間連線和x軸夾角的余弦值，(sin(b_to_g_dir)是球心坐標與目標點之間連線和x軸夾角的正弦值。

實施例2

本實施例詳細闡述了一種基于三維模糊ptp算法的單體機器魚行為控制策略的總體流程。

圖2為本發(fā)明一種基于三維模糊ptp算法的單體機器魚行為控制策略的三維模糊策略下的ptp算法設計流程圖。

從圖2可以看出，本發(fā)明具體實施時包括如下步驟：

步驟a.初始化；

將采樣時間初始化為1s；如圖1所示，水池區(qū)域劃分為4個區(qū)域；小球坐標為(103cm，47cm)，位于區(qū)域1，point點坐標為(101cm，47cm)；

步驟b.信息更新計算；

其中，信息更新參數(shù)為l和θe；

步驟c.判斷是否|θe|＜δθ，并根據(jù)判斷結果進行相應操作，具體為：

c.1若|θe|＜δθ，即對應圖2中的y1，則跳至步驟e；

c.2若|θe|≥δθ，即對應圖2中的n1，則跳至步驟d；

步驟d.判斷是否|θe|＜θf，并根據(jù)判斷結果進行相應操作，具體為：

d.1若|θe|＜δθ，即對應圖2中的y2，則調(diào)用轉(zhuǎn)向策略，再跳至步驟e；

d.2若|θe|≥δθ，即對應圖2中的n2，則調(diào)用轉(zhuǎn)彎策略，再跳至步驟e；

步驟e.判斷是否|l|＜δl，并根據(jù)判斷結果進行相應操作，具體為：

d.1若|θe|＜δθ，即對應圖2中的y3，則跳至步驟f；

d.2若|θe|≥δθ，即對應圖2中的n3，則調(diào)用速度控制策略，再跳至步驟f；

步驟f.輸出機器魚的速度、轉(zhuǎn)向，具體為；

根據(jù)步驟d、e中的轉(zhuǎn)向、轉(zhuǎn)彎、速度控制策略，輸出機器魚的速度和轉(zhuǎn)向。

步驟g.計算模糊控制模式，具體為；

根據(jù)步驟f機器魚輸出的速度、轉(zhuǎn)向以及步驟a中的采樣時間，三維模糊化處理轉(zhuǎn)換為模糊控制模式；

步驟h.若(l＜δl)∩(θe＜δθe)結束，否則轉(zhuǎn)步驟b；

實施例3

本實施例詳細闡述了在本發(fā)明在具體實施時針對不同區(qū)域設置相應的機器魚頂球行為規(guī)劃策略，具體對應著本發(fā)明所提策略的步驟5.1中的機器魚頂球行為規(guī)劃。

頂球行為規(guī)劃的目標是為機器魚規(guī)劃適當?shù)倪\動行為，以引導機器魚頂球到達目標點位置。對應于各種區(qū)域情況，相應的行為設計使得機器魚在不同區(qū)域內(nèi)選擇相應的行為向目標點運動，其具體過程如下：

1)bpr行為：對應為進攻區(qū)的行為。有效進攻情況：如圖3(a)所示，當機器魚中心坐標f_centerpt橫坐標小于水球坐標b_pt的橫坐標時，機器魚沿最短路徑朝向右頂球點游去，到達向右頂球點后，實施向右頂球動作。過沖情況：如圖3(b)所示，若機器魚中心坐標點f_centerpt橫坐標在b_pt的橫坐標與g_pt橫坐標之間時，即機器魚位于水球和目標點之間的區(qū)域，為過沖情況。在這種情況下，機器魚調(diào)整位姿，游向有效進攻區(qū)。

2)bubr行為：對應為上邊界區(qū)的行為。當水球處于上邊界區(qū)域時，如圖3(c)所示，機器魚游向向下頂球點，實施向下頂球動作。

3)blbr行為：對應為下邊界區(qū)的行為。同上邊界區(qū)域行為相似，如圖3(d)所示，當水球處于下邊界區(qū)域時，機器魚游向向上頂球點，實施向上頂球動作。

4)bgr行為：對應為射門區(qū)的行為。如圖3(e)所示，當水球處于射門區(qū)域時，機器魚游向射門頂球點。

實施例4

本實施例詳細闡述了在本發(fā)明中，根據(jù)不同區(qū)域的行為設計，給出相應的動作規(guī)劃。在機器魚頂球到目標點過程中，均是由這一系列動作組成，具體對應著本發(fā)明所提策略的步驟5.2中的動作規(guī)劃。

(1)頂球動作p_action

p_action1：向右頂球動作，當機器魚處于進攻區(qū)時，機器魚朝向右頂球點運動，實施向右頂球動作。

p_action2：向下頂球動作，這個動作應用于bubr行為中，如圖4(a)所示，機器魚游到向下頂球點，實施向下頂球動作。

p_action3：向上頂球動作，應用于blbr行為中，如圖4(b)所示，機器魚游到向上頂球點，實施向上頂球動作。

p_acition4：射門動作，如圖4(c)所示，當水球處于射門區(qū)時，采取了切入圓射門算法，使得機器魚將球頂入球門內(nèi)。

(2)過程動作action

為了區(qū)分機器魚所處的區(qū)域情況，本發(fā)明對游向目標行為進行了具體的動作劃分：

action1：游向向右頂球點。

action2：游向臨時返回點。

action3：到達臨時返回點后轉(zhuǎn)彎動作，如圖5(a)所示，當機器魚到達臨時返回點后，設計相應的轉(zhuǎn)彎動作，使機器魚沿著舒適圓路徑，朝向右頂球點運動，繼而實施向右頂球動作。

action4：游向向上頂球點。

action5：游向向下頂球點。

action6：游向射門頂球點。

action7：頂球后尾巴波動推動水球運動，如圖5(b)所示，當機器魚采取頂球動作后，水球會由于機器魚頂球受力，而漂離機器魚，此時機器魚采取急轉(zhuǎn)彎，并伴隨尾部波動，然后調(diào)直以速度v向前游動，即使機器魚尾部沒有接觸到水球，借助尾部波動攪起的水波，產(chǎn)生力f，也可推動水球向目標點運動。

以上所述為本發(fā)明的較佳實施例而已，本發(fā)明不應該局限于該實施例和附圖所公開的內(nèi)容。凡是不脫離本發(fā)明所公開的精神下完成的等效或修改，都落入本發(fā)明保護的范圍。