NEW水中机器人使用说明书策略部分

1、水中机器鱼说明书(策略部分详解)绪论 在机器鱼实验室多年的过程，收获颇多，也希望大家能够后浪推前浪，在机器鱼实验室中收获知识，收获成长。机器鱼的控制，相当于参照一条真正鱼的行动方式，需要“眼睛”来识别，需要“尾巴”来获得动力，需要一个“脑子”做决策。我们针对实体鱼来说明一下，“眼睛”就是我们鱼池上方的摄像头，动力就是鱼体内的舵机，决策的“脑子”就是我们的程序。本次所写内容是根据自己的编程的经验，总结给大家，主要内容是机器鱼程序的讲解和编程思路的分析。我们重要讲解的是关于策略部分的内容。首先说明程序用到的参数的定义和一些参数的使用含义，接着是对于程序中用到关键方向的具体解释，在然后是解释控制程序

2、中一些基本的程序，例如点到点函数，计算角度的函数，计算距离的函数。其中重点讲解的是点到点函数，往后就是具体函数的简单实例，像场地追逐的程序，1VS1程序，具体说明程序的编写思想和鱼在对应情况的执行方式。希望大家一定要掌握好关键用到的方向的参数定义，还有点到点函数的作用，可以从场地追逐这个最简单的程序中开始进行程序的编写，最后理解好1VS 1程序，以后编写其他的不同项目的程序时应该能够应付过来。最后，希望这个仓促中编写的程序讲解能够给大家带来帮助，从中出现的错误也希望指正，一些关于编程的思想也一起交流。 知识导航绪论2机器鱼控制的基本定义4鱼的基本表示方式，如图一。4鱼池的基本模型定义，如图二4

3、基本的方向设定，如图三5机器鱼基本控制策略讲解6类函数的定义6最基本的定义变量说明61）程序的定义的最基本变量有四个：62）基本变量在程序中的获取7 三个基本服务函数81）角度函数 此函数所测算的角度建立于绝对坐标系基础上82）距离函数93）转换角度函数9 点到点函数的讲解10点对点底层函数详解10具体程序的编写举例13 场内追逐的程序示例131V1综合测试实例15机器鱼控制的基本定义我们需要先了解机器鱼的一些最基本的定义的东西，这样后面的学习中会更简单一些。鱼的基本表示方式，如图一。侧视图 线段就是天线的高度，而程序中鱼中心的位置就是定义为天线的位置。段就是鱼头的长度（这个是重要概念），通常

4、在程序中定义为多少个像素，或者。鱼池的基本模型定义，如图二程序中定义坐标系的方式为，以点为坐标系原点，为轴且右为正向，为轴且下为正向。就是鱼池的长度，在大平台软件中表现为像素的多少，在到的范围内。就是鱼池宽度大平台表现像素的大小，在到的范围内。点就是比赛的发球点，在中线和中线交点上。鱼池分左半场与右半场，和上下半场。共有两个球门。在程序中判断鱼在左还是右半场，就是判断鱼的坐标是否大于中线的坐标，同理判断鱼在上还是下半场就是判断鱼的坐标是否大于中线的坐标。基本的方向设定，如图三对于方向的设定，存在两个坐标系，一个是绝对坐标系，就是以鱼池左上角为原点的坐标系，还有一个是相对参考坐标系，就是一鱼中心

5、位置为原点，鱼头方向为轴方向的坐标系。对于相对参考坐标系，为鱼头方向，以鱼头方向为参照，程序中定义是逆时针为正向角度，顺时针为负向角度。所以，表示的话为°，为°。另外对于绝对坐标系，它的方向正负判断则是相反的，顺时针为正向角度，逆时针为负向角度，具体原因会在后期解释Angle函数时提道，平常我们经常用到的就是绝对坐标系下的角度，因此综上，这是我们程序编写必须要了解的基本参数的意义大家需要重点掌握的是基本方向的设定，我们后面理解程序的时候会觉得简单一些。机器鱼基本控制策略讲解类函数的定义程序中变量都定义为C+语言中“类”的方式。至于什么是“类”，简单理解是定义的对象、变量包含

6、同样的特性，作为一种形式提供给我们使用的一种函数类型，定义一种类需要进行实例化，说明类的具体内容，包含的参数，变量，对象是什么。更多的大家可以具体上网查，我们需要知道有这种东西，能用就行。它的基本形式是：Cpoint+变量名；通道临时目标点：指所控制的鱼在一定的区域范围内需要到达的位置。最基本的定义变量说明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坐标，也就是类包含相同的参数（对象），而鱼头方向没有包含这些，仅仅是一个-，的变化范围。2）基本变量在程序中的获取我们定义了最基本的四个变量，那么程序中是怎么获得的呢，我们

8、下面来说明。针对三个类（鱼中心坐标，球中心坐标，球门中心坐标），我们大平台已经写好对这些这些类的定义和例化函数，具体我们不需要了解。获取的函数也已经写好，就是简单的调用一个函数就行。具体程序是对鱼中心坐标的获取 f_pt=m_FishInfo0.GetCenterPt() ;m_FishInfo0.GetCenterPt()是一个类函数，不需要具体了解函数具体内容，只需要记住其实现的功能。功能是获取鱼的中心坐标。m_FishInfo0m_是标识作用，fish代表“鱼”，info代表“输入”数字数字代表鱼的编号这个获取的是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()是一个类函数，功能是获得球中心坐标m_goalinfo0同时在2013的新平台上也需要输入鱼的编号m_ 是标识作用，goal代表球门，info是“输入”，GetBallPt() 代表球中心坐标对球门中心坐标的获取是：g_pt=m_Channel0.center ;数字 中括号内部的数字代表获取的是第几个通道的

10、坐标。如果很多个通道可以代替临时目标点的作用吗？对鱼头方向的获取是： f_dir=m_FishInfo0.GetDirection() ;数字也是同样的含义，代表获取的方向是第几条鱼的。 三个基本服务函数程序中还有最简单的三个服务函数。1获取角度函数Angle(CPoint point,CPoint aimer);2获取距离函数Distance(x,y)；其中形参都是定义为“类”。3转换角度函数Checkangle(double dir)形参类型是双精度。大家有英文字母的含义应该就了解函数的作用了。请记住三个函数的功能。1）角度函数 此函数所测算的角度建立于绝对坐标系基础上Angle(CPoi

11、nt point,CPoint aimer)具体定义如下：Angle(CPoint point,CPoint aimer) /声明函数，两个类的形参point和aimer double a;a=atan2(double)(aimer.y-point.y),(double)(aimer.x-point.x);/就是求一个反正切的角度return a; /返回一个角度值范围是-/2,/2(这个很重要，记住范围) 红色为point 黑色为aimer 2）距离函数Distance(CPoint point,CPoint aimer)函数具体定义如下：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）转换角度函数Checkangle(double dir) 具体定义如下：Checkangle(double dir) /把dir换算成-pipi之间的数if(dir>PI)dir-=2*PI;else if(dir<-PI)dir+=2*PI;return dir;请认真理解函数的内容

13、，具体在后面不需要书写书写这些程序，这些函数在平台底层已经写好，大家需要的只是合理的去调用它就行了。 点到点函数的讲解这是程序中核心的函数，机器鱼所执行的动作都是调用这个函数来实现的，所以请大家一定要理解函数内容，并且能够修改优化这个程序。先给出函数功能说明，如图四。点对点底层函数详解 重在理解点对点函数是如何运行的，底层函数研究根据自身能力酌情处理。void Roundp2p(CPoint aimpt,CFishAction &action,int fishID,CFishInfo m_FishInfo)/定义一个函数，包含四个参变量，第一个是aimpt作为目标点的类，第二个是鱼的动

14、作模式action（包括速度、方向以及运动模式）第三个是鱼的ID，第四个是鱼的入口参数（包含鱼中心坐标，鱼头方向等）。其中，鱼的ID是整数类型外，其他的都是“类”。 CPoint FishPt;/ 鱼中心的位置double FishDir;/ 鱼头的方向CPoint FishHeadPt;/鱼头的位置CPoint LastFishPt;CPoint FishRotatePt;/;/鱼的转动位置，程序未用到，作为预留变量待开发使用。CPoint FishTailPt; /鱼尾巴的位置，程序未用到，作为预留变量待开发使用。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;/需要游动的半径,可以随意的设置.FishPt=m_FishInfo.GetCenterPt();/获取鱼的中心位置FishDir=m_FishInfo.GetDirection();/获取鱼头方向dist1=Distance(FishPt,aimpt);/计算鱼中心位置和所要游动目标点的距离dist1变量dir1=Angle(FishPt,aimpt);/计算鱼中心位置和所要游动目标点连线的夹角（绝对坐标系）dir1-=FishDir;/这个语句的作用是把绝对坐标系表示

17、的角度转换为相对坐标系 /就是以鱼和目标点的连线为参考轴，dir1代表此情况的鱼头方向dir1=Checkangle(dir1)*180/PI;/调用函数，把dir1的从弧度换为角度表示action.speed=15;/定义鱼的速度为最快的15执行动作m_FishInfo.SetAction(action);/发送动作设置状态给鱼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、序编写讲解，主要有两个，一个是最简单的场内追逐比赛的程序，另一个是1的程序。请大家好好理解，学会如何编程 场内追逐的程序示例我们把鱼池共分为八个部分，每个部分的判定条件是根据X坐标和Y坐标来划分的。鱼在每个区域都会有一个目标点，鱼在该区域时就会游向该目标点。需要编写的就是鱼在当前区域下鱼游向目标点的程序。下面具体说明区域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； /鱼的速度设置为最大

24、 15 Roundp2p(goal_pt,m_action0,0,m_FishInfo0); /调用点到点函数，执行动作 区域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; /鱼的速度设置为最大 15m_action0.mode=0； /鱼的速度模式为0 Roundp2p(goal_pt,m_action0,0,m_FishInfo0); /调用点到点函数，执行动作 区域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; /鱼的速度设置为最大 15m_action0.mode=0； /鱼的速度模式为0 Roundp2p(goal_pt,m_action0,0,m_FishInfo0); /调用点到点函数执行动作 区域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; /鱼的速度设置为最大 15m_action0.mode=0； /鱼的速度模式为0 Roundp2p(goal_pt,m_action0,0,m_FishInfo0); /调用点到点函数，执行动作 对于下半部分，也是相似的结构，需要注意的是y坐标的变化。区域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; /鱼的速度设置为最大 15

27、m_action0.mode=0； /鱼的速度模式为0 Roundp2p(goal_pt,m_action0,0,m_FishInfo0); /调用点到点函数，执行动作 区域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; /鱼的速度设置为最大 15m_action0.mode=0； /鱼的速度模式为0 Roundp2p(goal_pt,m_action0,0,m_FishI

28、nfo0); /调用点到点函数，执行动作 区域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; /鱼的速度设置为最大 15m_action0.mode=0； /鱼的速度模式为0 Roundp2p(goal_pt,m_action0,0,m_FishInfo0); /调用点到点函数，执行动作 区域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; /鱼的速度设置为最大 15m_action0.mode=0； /鱼的速度模式为0 Roundp2p(goal_pt,m_action0,0,m_FishInfo0); /调用点到点函数，执行动作 这就是场地追逐很简单的程序例子，简单易懂。程序很简单，所以存在这一些缺点。第一个是程序对场地分类不够精细，鱼在实际游动的时候往往偏差较大，第二个是目标点根据场地就变化，修改目标点坐标过程繁琐。这里给出简单的形式是希望大家理解编程的方式，自己能够编写简单的程序，在水

30、池让鱼游起来，实现追逐功能。针对程序的优化，第一个大家可以把场地划分更多的区域，进行精细化，第二个大家可以改用设置通道的方式代替目标点，这样修改目标点坐标就很方便了。希望大家可以多多思考，交流，优化程序。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