NEW水中機器人使用說明書策略部分

1、水中機器魚說明書(策略部分詳解)緒論 在機器魚實驗室多年的過程，收獲頗多，也希望大家能夠后浪推前浪，在機器魚實驗室中收獲知識，收獲成長。機器魚的控制，相當于參照一條真正魚的行動方式，需要“眼睛”來識別，需要“尾巴”來獲得動力，需要一個“腦子”做決策。我們針對實體魚來說明一下，“眼睛”就是我們魚池上方的攝像頭，動力就是魚體內(nèi)的舵機，決策的“腦子”就是我們的程序。本次所寫內(nèi)容是根據(jù)自己的編程的經(jīng)驗，總結(jié)給大家，主要內(nèi)容是機器魚程序的講解和編程思路的分析。我們重要講解的是關(guān)于策略部分的內(nèi)容。首先說明程序用到的參數(shù)的定義和一些參數(shù)的使用含義，接著是對于程序中用到關(guān)鍵方向的具體解釋，在然后是解釋控制程序

2、中一些基本的程序，例如點到點函數(shù)，計算角度的函數(shù)，計算距離的函數(shù)。其中重點講解的是點到點函數(shù)，往后就是具體函數(shù)的簡單實例，像場地追逐的程序，1VS1程序，具體說明程序的編寫思想和魚在對應情況的執(zhí)行方式。希望大家一定要掌握好關(guān)鍵用到的方向的參數(shù)定義，還有點到點函數(shù)的作用，可以從場地追逐這個最簡單的程序中開始進行程序的編寫，最后理解好1VS 1程序，以后編寫其他的不同項目的程序時應該能夠應付過來。最后，希望這個倉促中編寫的程序講解能夠給大家?guī)韼椭?，從中出現(xiàn)的錯誤也希望指正，一些關(guān)于編程的思想也一起交流。 知識導航緒論2機器魚控制的基本定義4魚的基本表示方式，如圖一。4魚池的基本模型定義，如圖二4

3、基本的方向設(shè)定，如圖三5機器魚基本控制策略講解6類函數(shù)的定義6最基本的定義變量說明61）程序的定義的最基本變量有四個：62）基本變量在程序中的獲取7 三個基本服務函數(shù)81）角度函數(shù) 此函數(shù)所測算的角度建立于絕對坐標系基礎(chǔ)上82）距離函數(shù)93）轉(zhuǎn)換角度函數(shù)9 點到點函數(shù)的講解10點對點底層函數(shù)詳解10具體程序的編寫舉例13 場內(nèi)追逐的程序示例131V1綜合測試實例15機器魚控制的基本定義我們需要先了解機器魚的一些最基本的定義的東西，這樣后面的學習中會更簡單一些。魚的基本表示方式，如圖一。側(cè)視圖 線段就是天線的高度，而程序中魚中心的位置就是定義為天線的位置。段就是魚頭的長度（這個是重要概念），通常

4、在程序中定義為多少個像素，或者。魚池的基本模型定義，如圖二程序中定義坐標系的方式為，以點為坐標系原點，為軸且右為正向，為軸且下為正向。就是魚池的長度，在大平臺軟件中表現(xiàn)為像素的多少，在到的范圍內(nèi)。就是魚池寬度大平臺表現(xiàn)像素的大小，在到的范圍內(nèi)。點就是比賽的發(fā)球點，在中線和中線交點上。魚池分左半場與右半場，和上下半場。共有兩個球門。在程序中判斷魚在左還是右半場，就是判斷魚的坐標是否大于中線的坐標，同理判斷魚在上還是下半場就是判斷魚的坐標是否大于中線的坐標。基本的方向設(shè)定，如圖三對于方向的設(shè)定，存在兩個坐標系，一個是絕對坐標系，就是以魚池左上角為原點的坐標系，還有一個是相對參考坐標系，就是一魚中心

5、位置為原點，魚頭方向為軸方向的坐標系。對于相對參考坐標系，為魚頭方向，以魚頭方向為參照，程序中定義是逆時針為正向角度，順時針為負向角度。所以，表示的話為°，為°。另外對于絕對坐標系，它的方向正負判斷則是相反的，順時針為正向角度，逆時針為負向角度，具體原因會在后期解釋Angle函數(shù)時提道，平常我們經(jīng)常用到的就是絕對坐標系下的角度，因此綜上，這是我們程序編寫必須要了解的基本參數(shù)的意義大家需要重點掌握的是基本方向的設(shè)定，我們后面理解程序的時候會覺得簡單一些。機器魚基本控制策略講解類函數(shù)的定義程序中變量都定義為C+語言中“類”的方式。至于什么是“類”，簡單理解是定義的對象、變量包含

6、同樣的特性，作為一種形式提供給我們使用的一種函數(shù)類型，定義一種類需要進行實例化，說明類的具體內(nèi)容，包含的參數(shù)，變量，對象是什么。更多的大家可以具體上網(wǎng)查，我們需要知道有這種東西，能用就行。它的基本形式是：Cpoint+變量名；通道臨時目標點：指所控制的魚在一定的區(qū)域范圍內(nèi)需要到達的位置。最基本的定義變量說明1）程序的定義的最基本變量有四個：1、魚中心坐標；2、球中心坐標；3、魚頭方向；4、球門中心坐標下面具體說明定義魚中心坐標是：CPoint f_pt ：f是fish的簡寫，pt表示坐標，f_pt是魚的中心坐標，魚中心坐標 f_pt。定義球的中心坐標是：CPoint b_pt ：b是Ball的

7、簡寫，pt是坐標，b_pt就是球的中心坐標。定義球門中心坐標是： CPoint g_pt ：g是goal（球門）的簡寫，pt是坐標，所以g_pt代表球門坐標。定義魚頭方向是： double f_dir ：f就是fish簡寫，dir是direction的簡寫，這是一個雙精度的變量。絕對坐標系中范圍是-，所以程序中魚頭方向的范圍就是-，。大家或許有疑問，為什么魚頭方向是雙精度的變量而其他的三個是類呢。因為其他三個都是包含這x和y坐標，也就是類包含相同的參數(shù)（對象），而魚頭方向沒有包含這些，僅僅是一個-，的變化范圍。2）基本變量在程序中的獲取我們定義了最基本的四個變量，那么程序中是怎么獲得的呢，我們

8、下面來說明。針對三個類（魚中心坐標，球中心坐標，球門中心坐標），我們大平臺已經(jīng)寫好對這些這些類的定義和例化函數(shù)，具體我們不需要了解。獲取的函數(shù)也已經(jīng)寫好，就是簡單的調(diào)用一個函數(shù)就行。具體程序是對魚中心坐標的獲取 f_pt=m_FishInfo0.GetCenterPt() ;m_FishInfo0.GetCenterPt()是一個類函數(shù)，不需要具體了解函數(shù)具體內(nèi)容，只需要記住其實現(xiàn)的功能。功能是獲取魚的中心坐標。m_FishInfo0m_是標識作用，fish代表“魚”，info代表“輸入”數(shù)字數(shù)字代表魚的編號這個獲取的是0號魚的，GetCenterPt() 代表獲得中心坐標。那么2 vs 2

9、程序獲取0號是：f_pt0=m_FishInfo0.GetCenterPt() ; 程序獲取1號是；f_pt1=m_FishInfo1.GetCenterPt() ;對球中心坐標的獲取是： b_pt=m_goalinfo.GetBallPt();m_goalinfo.GetBallPt()是一個類函數(shù)，功能是獲得球中心坐標m_goalinfo0同時在2013的新平臺上也需要輸入魚的編號m_ 是標識作用，goal代表球門，info是“輸入”，GetBallPt() 代表球中心坐標對球門中心坐標的獲取是：g_pt=m_Channel0.center ;數(shù)字 中括號內(nèi)部的數(shù)字代表獲取的是第幾個通道的

10、坐標。如果很多個通道可以代替臨時目標點的作用嗎？對魚頭方向的獲取是： f_dir=m_FishInfo0.GetDirection() ;數(shù)字也是同樣的含義，代表獲取的方向是第幾條魚的。 三個基本服務函數(shù)程序中還有最簡單的三個服務函數(shù)。1獲取角度函數(shù)Angle(CPoint point,CPoint aimer);2獲取距離函數(shù)Distance(x,y)；其中形參都是定義為“類”。3轉(zhuǎn)換角度函數(shù)Checkangle(double dir)形參類型是雙精度。大家有英文字母的含義應該就了解函數(shù)的作用了。請記住三個函數(shù)的功能。1）角度函數(shù) 此函數(shù)所測算的角度建立于絕對坐標系基礎(chǔ)上Angle(CPoi

11、nt point,CPoint aimer)具體定義如下：Angle(CPoint point,CPoint aimer) /聲明函數(shù)，兩個類的形參point和aimer double a;a=atan2(double)(aimer.y-point.y),(double)(aimer.x-point.x);/就是求一個反正切的角度return a; /返回一個角度值范圍是-/2,/2(這個很重要，記住范圍) 紅色為point 黑色為aimer 2）距離函數(shù)Distance(CPoint point,CPoint aimer)函數(shù)具體定義如下：Distance(CPoint point,CPoi

12、nt aimer)double a;a=sqrt(double)(point.x-aimer.x)*(point.x-aimer.x)+(double)(point.y-aimer.y)*(point.y-aimer.y);/ 就是一個求距離的平方根公式return a; /返回距離值，雙精度類型3）轉(zhuǎn)換角度函數(shù)Checkangle(double dir) 具體定義如下：Checkangle(double dir) /把dir換算成-pipi之間的數(shù)if(dir>PI)dir-=2*PI;else if(dir<-PI)dir+=2*PI;return dir;請認真理解函數(shù)的內(nèi)容

13、，具體在后面不需要書寫書寫這些程序，這些函數(shù)在平臺底層已經(jīng)寫好，大家需要的只是合理的去調(diào)用它就行了。 點到點函數(shù)的講解這是程序中核心的函數(shù)，機器魚所執(zhí)行的動作都是調(diào)用這個函數(shù)來實現(xiàn)的，所以請大家一定要理解函數(shù)內(nèi)容，并且能夠修改優(yōu)化這個程序。先給出函數(shù)功能說明，如圖四。點對點底層函數(shù)詳解 重在理解點對點函數(shù)是如何運行的，底層函數(shù)研究根據(jù)自身能力酌情處理。void Roundp2p(CPoint aimpt,CFishAction &action,int fishID,CFishInfo m_FishInfo)/定義一個函數(shù)，包含四個參變量，第一個是aimpt作為目標點的類，第二個是魚的動

14、作模式action（包括速度、方向以及運動模式）第三個是魚的ID，第四個是魚的入口參數(shù)（包含魚中心坐標，魚頭方向等）。其中，魚的ID是整數(shù)類型外，其他的都是“類”。 CPoint FishPt;/ 魚中心的位置double FishDir;/ 魚頭的方向CPoint FishHeadPt;/魚頭的位置CPoint LastFishPt;CPoint FishRotatePt;/;/魚的轉(zhuǎn)動位置，程序未用到，作為預留變量待開發(fā)使用。CPoint FishTailPt; /魚尾巴的位置，程序未用到，作為預留變量待開發(fā)使用。CPoint GoalPt1, GoalPt2;/臨時目標點，GoalPt2

15、沒有用到CPoint Pt1, Pt2, Pt3, Pt4;/CPoint f_headpt;/point of fish's head/f_headpt=m_FishInfo0.GetHeadPoint();double dir1=0;double dir0;double dist0;double dist1=0;double dir2=0;double dist2=0;/臨時用到的方向和距離CPoint centerpt1,centerpt2;/作為魚要繞的中心來用CPoint centerpt3,centerpt4,centerpt5,centerpt6;CPoint cente

16、rpt0,centerpt00;double dis1,dis2;dis1=0;dis2=0; double radius;/需要游動的半徑,可以隨意的設(shè)置.FishPt=m_FishInfo.GetCenterPt();/獲取魚的中心位置FishDir=m_FishInfo.GetDirection();/獲取魚頭方向dist1=Distance(FishPt,aimpt);/計算魚中心位置和所要游動目標點的距離dist1變量dir1=Angle(FishPt,aimpt);/計算魚中心位置和所要游動目標點連線的夾角（絕對坐標系）dir1-=FishDir;/這個語句的作用是把絕對坐標系表示

17、的角度轉(zhuǎn)換為相對坐標系 /就是以魚和目標點的連線為參考軸，dir1代表此情況的魚頭方向dir1=Checkangle(dir1)*180/PI;/調(diào)用函數(shù)，把dir1的從弧度換為角度表示action.speed=15;/定義魚的速度為最快的15執(zhí)行動作m_FishInfo.SetAction(action);/發(fā)送動作設(shè)置狀態(tài)給魚if(dist1>100) /下面是魚和目標點的距離大于100像素的情況if(dir1>-5&&dir1<5)action.direction=7;else if(dir1<-5&&dir1>-10)act

18、ion.direction=5;else if(dir1<-10&&dir1>-30)action.direction=4;else if(dir1<-30&&dir1>-50)action.direction=2;else if(dir1<-50&&dir1>-70)action.direction=1;else if(dir1<-70&&dir1>-90)action.direction=0;else if(dir1<-90)action.direction=0;else

19、if(dir1>5&&dir1<10)action.direction=9;else if(dir1>10&&dir1<20)action.direction=10;else if(dir1>20&&dir1<40)action.direction=12;else if(dir1>40&&dir1<50)action.direction=12;else if(dir1>50&&dir1<80)action.direction=14;else if(dir1

20、>80&&dir1<90)action.direction=14;else action.direction=14;else /以下是魚和目標點的距離小于100像素的情況。 if(dir1>-5&&dir1<5)action.direction=7;else if(dir1<-5&&dir1>-20)action.direction=4;else if(dir1<-20&&dir1>-40)action.direction=1;else if(dir1<-40&&

21、;dir1>-60)action.direction=0;else if(dir1<-60&&dir1>-70)action.direction=0;else if(dir1<-70&&dir1>-90)action.direction=0;else if(dir1<-120)action.direction=0;else if(dir1>5&&dir1<20)action.direction=10;else if(dir1>20&&dir1<30)action.dire

22、ction=12;else if(dir1>30&&dir1<50)action.direction=13;else if(dir1>50&&dir1<70)action.direction=14;else if(dir1>70&&dir1<90)action.direction=14;else if(dir1<120)action.direction=14;else action.direction=14;m_FishInfo.SetAction(action);具體程序的編寫舉例下面我們進行一些簡單程

23、序編寫講解，主要有兩個，一個是最簡單的場內(nèi)追逐比賽的程序，另一個是1的程序。請大家好好理解，學會如何編程 場內(nèi)追逐的程序示例我們把魚池共分為八個部分，每個部分的判定條件是根據(jù)X坐標和Y坐標來劃分的。魚在每個區(qū)域都會有一個目標點，魚在該區(qū)域時就會游向該目標點。需要編寫的就是魚在當前區(qū)域下魚游向目標點的程序。下面具體說明區(qū)域A程序：if（f_pt.x<40&&f_pt.y<240） goal_pt.x =40; /A目標點的x坐標 goal_pt.y = 40; / y坐標 m_action0.speed=15; m_action0.mode=0； /魚的速度設(shè)置為最大

24、 15 Roundp2p(goal_pt,m_action0,0,m_FishInfo0); /調(diào)用點到點函數(shù)，執(zhí)行動作 區(qū)域B程序：if（f_pt.x>40&&f_pt.x<320&&f_pt.y<240） goal_pt.x =320; /B目標點的x坐標 goal_pt.y = 20; / y坐標m_action0.speed=15; /魚的速度設(shè)置為最大 15m_action0.mode=0； /魚的速度模式為0 Roundp2p(goal_pt,m_action0,0,m_FishInfo0); /調(diào)用點到點函數(shù)，執(zhí)行動作 區(qū)域C程序

25、：if（f_pt.x>320&&f_pt.x<600&&f_pt.y<240） goal_pt.x =600; /C目標點的x坐標 goal_pt.y = 20; / y坐標m_action0.speed=15; /魚的速度設(shè)置為最大 15m_action0.mode=0； /魚的速度模式為0 Roundp2p(goal_pt,m_action0,0,m_FishInfo0); /調(diào)用點到點函數(shù)執(zhí)行動作 區(qū)域D程序：if（f_pt.x>600&&f_pt.y<240） goal_pt.x = 620; /D目標點的x

26、坐標 goal_pt.y = 240; / y坐標m_action0.speed=15; /魚的速度設(shè)置為最大 15m_action0.mode=0； /魚的速度模式為0 Roundp2p(goal_pt,m_action0,0,m_FishInfo0); /調(diào)用點到點函數(shù)，執(zhí)行動作 對于下半部分，也是相似的結(jié)構(gòu)，需要注意的是y坐標的變化。區(qū)域E程序：if（f_pt.x>600&&f_pt.y>240） goal_pt.x = 600; /E目標點的x坐標 goal_pt.y = 440; / y坐標m_action0.speed=15; /魚的速度設(shè)置為最大 15

27、m_action0.mode=0； /魚的速度模式為0 Roundp2p(goal_pt,m_action0,0,m_FishInfo0); /調(diào)用點到點函數(shù)，執(zhí)行動作 區(qū)域F程序：if（f_pt.x>320&&f_pt.x<600&&f_pt.y>240） goal_pt.x =320; /F目標點的x坐標 goal_pt.y = 470; / y坐標m_action0.speed=15; /魚的速度設(shè)置為最大 15m_action0.mode=0； /魚的速度模式為0 Roundp2p(goal_pt,m_action0,0,m_FishI

28、nfo0); /調(diào)用點到點函數(shù)，執(zhí)行動作 區(qū)域G程序：if（f_pt.x>40&&f_pt.x<320&&f_pt.y>240） goal_pt.x =320; /G目標點的x坐標 goal_pt.y = 20; / y坐標m_action0.speed=15; /魚的速度設(shè)置為最大 15m_action0.mode=0； /魚的速度模式為0 Roundp2p(goal_pt,m_action0,0,m_FishInfo0); /調(diào)用點到點函數(shù)，執(zhí)行動作 區(qū)域H程序：if（f_pt.x<40&&f_pt.y>240）

29、 goal_pt.x = 20; / H目標點的x坐標 goal_pt.y = 240; / y坐標m_action0.speed=15; /魚的速度設(shè)置為最大 15m_action0.mode=0； /魚的速度模式為0 Roundp2p(goal_pt,m_action0,0,m_FishInfo0); /調(diào)用點到點函數(shù)，執(zhí)行動作 這就是場地追逐很簡單的程序例子，簡單易懂。程序很簡單，所以存在這一些缺點。第一個是程序?qū)龅胤诸惒粔蚓?，魚在實際游動的時候往往偏差較大，第二個是目標點根據(jù)場地就變化，修改目標點坐標過程繁瑣。這里給出簡單的形式是希望大家理解編程的方式，自己能夠編寫簡單的程序，在水

30、池讓魚游起來，實現(xiàn)追逐功能。針對程序的優(yōu)化，第一個大家可以把場地劃分更多的區(qū)域，進行精細化，第二個大家可以改用設(shè)置通道的方式代替目標點，這樣修改目標點坐標就很方便了。希望大家可以多多思考，交流，優(yōu)化程序。1V1綜合測試實例 CPoint f_pt;/魚的中心點坐標f_pt=m_FishInfo0.GetCenterPt();/GetCenterPt()返回中心點坐標double f_dir;/魚的方向，-PI,PIf_dir=m_FishInfo0.GetDirection();CPoint tempt0,tempt1,tempt2,tempt3,tempt4,tempt5;CPoint f_

31、headpt;/point of fish's headf_headpt=m_FishInfo0.GetHeadPoint();CPoint b_pt;/球的中心點坐標b_pt=m_goalinfo.GetBallPt();CPoint g_pt;/球門中心點坐標g_pt=m_Channel0.center;double disfishtoball;disfishtoball=Distance(f_pt,b_pt);/魚到球的距離double dirballtogoal;dirballtogoal=Angle2(b_pt,g_pt);/球到目標點的角度double r=9.0; /右攻

32、目標點tempt0.x=b_pt.x-1.3*r*cos(dirballtogoal);tempt0.y=b_pt.y-0.2*r*sin(dirballtogoal); /在球門右攻tempt4.x=b_pt.x-2*r*cos(dirballtogoal);tempt4.y=b_pt.y-0*r*sin(dirballtogoal); /左攻目標點tempt1.x=b_pt.x+1.3*r*cos(dirballtogoal);tempt1.y=b_pt.y+0.1*r*sin(dirballtogoal); /在球門左攻tempt5.x=b_pt.x+2*r*cos(dirballtog

33、oal);tempt5.y=b_pt.y-0*r*sin(dirballtogoal); /球門目標點tempt2.x=b_pt.x-1.4*r;tempt2.y=b_pt.y-0*r*sin(dirballtogoal);/（右攻）tempt3.x=b_pt.x+1.5*r;tempt3.y=b_pt.y+0.1;/0*r*sin(dirballtogoal);/（左攻）、 bool m_right;if(g_pt.x>378)m_right=true;elsem_right=false;if(m_right) if (f_headpt.x<b_pt.x-0.5*r) /魚在球左

34、邊 if(b_pt.x>610) if( b_pt.y<=292&&b_pt.y>=212) /球門程序 if(disfishtoball<35) m_action0.mode=0; m_action0.speed=14; else m_action0.mode=0; m_action0.speed=15; Roundp2p(tempt2,m_action0,0,m_FishInfo0); else if(disfishtoball<35) m_action0.mode=0; m_action0.speed=13; else m_action0.m

35、ode=0; m_action0.speed=15; Roundp2p(tempt4,m_action0,0,m_FishInfo0); else if(disfishtoball<35) m_action0.mode=0; m_action0.speed=13; else m_action0.mode=0; m_action0.speed=15; Roundp2p(tempt0,m_action0,0,m_FishInfo0); else if(f_pt.y<b_pt.y-r)/魚在球右上面 m_action0.speed=15; if(f_dir>5/6*PI&&

36、amp;f_dir<-5/6*PI) m_action0.mode=0; m_action0.direction=7; else if(f_dir>-2/3*PI&&f_dir<PI/2) m_action0.mode=1;m_action0.direction=0; else m_action0.mode=0;m_action0.direction=f_dir<0?2:12; else if(f_pt.y>b_pt.y+r)/魚在球右下面 m_action0.speed=15; if( f_dir>5/6*PI&&f_dir

37、<-5/6*PI) m_action0.mode=0;m_action0.direction=7; else if(f_dir>-PI/2&&f_dir<2/3*PI ) m_action0.mode=1;m_action0.direction=14; else m_action0.mode=0;m_action0.direction=f_dir<0?2:12; else/魚和球的Y值相同。 m_action0.speed=15; if(f_dir<2/3*PI&&f_dir>-2/3*PI) m_action0.mode=1

38、; m_action0.direction=f_dir<0?0:14; else m_action0.mode=0;m_action0.direction=7; else /(左功)if(f_headpt.x>b_pt.x+1.4*r)/魚在球右邊 if(b_pt.x<95) if(b_pt.y<302&&b_pt.y>227) if(disfishtoball<35)/the velosity change easy to approach m_action0.mode=0; m_action0.speed=14; else m_action0.mode=0