背景
研究生实训课程《水中机器人实训》结课考核为机器鱼比赛，比赛规则如下：
池子设计如下：
需控制鱼从初始位置避让障碍游到球位置，然后将球顶回鱼的初始位置，计时完成，时间短得分高，时长不超过5分钟。
所编写的策略的思想
对机器鱼的控制分为以下四个阶段来进行：
- 以障碍为界，分为左右两侧。球在左侧，鱼在右侧时，定义为初始阶段，此时鱼的任务是避开障碍游到左侧。因此本阶段鱼的目标位置为障碍开口位置的中心点。本控制算法中使用简便方法，即将障碍定义为一个偏移的球门，球门中心位置即障碍开口的中心点，因此在此阶段可以直接使用现有的鱼游动控制程序，目标位置为球门；
- 球在左侧，鱼在左侧时，为顶球过门阶段。此阶段又分为两个子阶段：
a.球在鱼左侧，这时鱼不可直接顶球，需要控制鱼游到球的左侧临时目标位置。而临时目标位置不能直接设为球的正左侧一定距离，因为若鱼与球y坐标相同时，鱼仍然会顶到球。因此本控制程序将这个临时目标点设为球的左上角；
b.球在鱼右侧，这时可以直接顶球，并可以直接调用鱼顶球过门的程序，球门点设为障碍物开口中心点。 - 球在右侧，鱼在左侧时，与初始阶段相同，鱼的目标仍为穿过障碍物，目标点为障碍开口的中心点；
- 球在右侧，鱼在右侧时，为顶球到初始位置阶段。此阶段类似于顶球过门阶段，又分为两个子阶段：
a.球在鱼下侧，这时鱼不可直接顶球，需要控制鱼游到球的下侧临时目标位置。而临时目标位置不能直接设为球的正下侧一定距离，因为若鱼与球x坐标相同时，鱼仍然会顶到球。因此本控制程序将这个临时目标点设为球的左下角；
b.球在鱼上侧，这时可以直接顶球，并可以直接调用鱼顶球过门的程序，球门点设为初始位置。
本组粗糙的策略
//Purpose: 鱼从起始点避障至球处，并运球避障至与起始点（一个障碍）
//Author:Jace, cheng
//Data:2022.1.7
#include "../Header/StdAfx.h"
#include "../Header/Strategy.h"
CStrategy::CStrategy(void)
{
m_StatePeriod=15;
m_TurnYuan=0;
}
CStrategy::~CStrategy(void)
{
}
BOOL CStrategy::Strategy(IplImage* imgOrig,IplImage* imgRecog ,CFishAction action[], int n_action, CFishInfo fishinfo[], int n_fishinfo,
CBallInfo ballinfo[], int n_ballinfo, OBSTAINFO obst[],int n_obst, CHANNEL channel[], int n_channel)
{
return TRUE;
}
//以下id及鱼信息、鱼动作信息数组相关
void CStrategy::BasicActionStop(CFishInfo fish[],CFishAction action[],int id)
{
action[id].speed=0;
action[id].direction=7;
action[id].mode=0;
action[id].state++;
if(action[id].state==m_StatePeriod)
{
action[id].state=0;
m_TurnYuan=0;
}
fish[id].SetAction(action[id]);
}
void CStrategy::BasicActionGo(CFishInfo fish[],CFishAction action[],int id,int speed,int direction,int mode)
{
action[id].speed=speed;
action[id].direction=direction;
action[id].mode=mode;
if(m_TurnYuan)//正在执行原始转弯模式
{
action[id].state++;
if(action[id].state>=m_StatePeriod)
{
action[id].state=0;
m_TurnYuan=0;
}
}
else
{
if(mode)
{
action[id].state=0;
m_TurnYuan=mode;
}
else
{
action[id].state++;
if(action[id].state>=m_StatePeriod)
action[id].state=0;
}
}
fish[id].SetAction(action[id]);
}
BOOL CStrategy::Strategy0(CFishAction m_action[], CFishInfo m_FishInfo[], CBallInfo &m_goalinfo, OBSTAINFO m_obst[], CHANNEL m_Channel[])
{
//初始化动作参数
m_action[0].state = 0;
m_action[0].mode = 0;
CPoint temp0,temp2;
CPoint temp11;
CPoint temp12;
CPoint temp211;
CPoint temp212;
CPoint temp22;
CPoint f_pt;//鱼的中心点坐标
f_pt=m_FishInfo[0].GetCenterPoint();//GetCenterPt()返回中心点坐标
double f_dir;//鱼的方向，[-PI,PI]
f_dir=m_FishInfo[0].GetDirection();
CPoint f_headpt;//point of fish's head
f_headpt=m_FishInfo[0].GetHeaderPoint();
CPoint b_pt;//球的中心点坐标
b_pt=m_goalinfo.GetBallPoint();
CPoint g_pt;//球门中心点坐标//distance
g_pt=m_Channel[0].center;
CPoint start;//起始点
start.x = 562;
start.y = 100;
bool m_right;
if (start.x>312)
m_right = true;
else
m_right = false;
if(m_right)
{
if(f_pt.x>g_pt.x&&b_pt.x<g_pt.x)//鱼在B区且球在A区
{
temp0.x = g_pt.x-50;//第一阶段和第三阶段鱼的目标点是球门，临时目标点为球门向左一点
temp0.y = g_pt.y;
//**********全速游过球门**********
m_action[0].mode=0;
m_action[0].speed=14;
Roundp2p1(temp0,m_action[0],0,m_FishInfo[0]);//全速游向球门
}
else if(f_pt.x<g_pt.x&&b_pt.x<g_pt.x)//鱼在A区且球在A区
{
g_pt.x = g_pt.x + 50;
double dirfishtoball;
dirfishtoball = this->Angle(f_pt, b_pt);
double disfishtoball;
disfishtoball = this->Distance(f_pt, b_pt);
double dirballtogoal;
dirballtogoal = this->Angle(b_pt, g_pt);
double dirgoaltoball;
dirgoaltoball = this->Distance(g_pt, b_pt);
dirfishtoball -= f_dir;
dirfishtoball = this->Checkangle(dirfishtoball);
double dir = dirfishtoball * 180 / PI;
double Cf_dir=this->Checkangle(f_dir);
double r = 6;
temp11.x = b_pt.x - r*cos(dirballtogoal);//第二阶段鱼的目标是顶球过球门，临时目标点为顶球位置
temp11.y = b_pt.y - r*sin(dirballtogoal);
temp12.x = b_pt.x - r - 30;//鱼在球的右侧时的临时目标点
temp12.y = b_pt.y - r - 40;
if(f_headpt.x<b_pt.x+r)//鱼在球左上边，开始顶球
{
if(disfishtoball<80)//低速进近
{
m_action[0].mode=0;
m_action[0].speed=4;
}
else//高速推进
{
m_action[0].mode=0;
m_action[0].speed=8;
}
Roundp2p1(temp11,m_action[0],0,m_FishInfo[0]);
}
else
{
m_action[0].mode=0;
m_action[0].speed=14;
Roundp2p1(temp12,m_action[0],0,m_FishInfo[0]);
}
}
else if(f_pt.x<g_pt.x&&b_pt.x>g_pt.x)//鱼在A区且球在B区
{
temp0.x = g_pt.x+50;//第一阶段和第三阶段鱼的目标点是球门，临时目标点为球门向右一点
temp0.y = g_pt.y;
//**********全速游向球门**********
m_action[0].mode=0;
m_action[0].speed=12;
Roundp2p1(temp0,m_action[0],0,m_FishInfo[0]);//全速游向球门
}
else if(f_pt.x>g_pt.x&&b_pt.x>g_pt.x)//鱼在B区且球在B区
{
//**********原版顶球进门策略降速版**********
double dirfishtoball;
dirfishtoball = this->Angle(f_pt, b_pt);
double disfishtoball;
disfishtoball = this->Distance(f_pt, b_pt);
double dirballtostart;
dirballtostart = this->Angle(b_pt, start);
double dirstarttoball;
dirstarttoball = this->Distance(start, b_pt);
dirfishtoball -= f_dir;
dirfishtoball = this->Checkangle(dirfishtoball);
double dir = dirfishtoball * 180 / PI;
double r=6;
temp211.x = b_pt.x - r*sin(dirballtostart);//第四阶段鱼的目标是顶球进起始点，临时目标点为顶球位置
temp211.y = b_pt.y + r*cos(dirballtostart);
temp212.x = b_pt.x + r*sin(dirballtostart);//第四阶段鱼的目标是顶球进起始点，临时目标点为顶球位置
temp212.y = b_pt.y + r*cos(dirballtostart);
temp22.x = b_pt.x - r - 30;//鱼在上侧时的临时目标点
temp22.y = b_pt.y + r + 40;
if (f_headpt.y>b_pt.y + r)//鱼在球下边，直接顶球
{
if (disfishtoball<80)//the velosity change easy to approach
{
m_action[0].mode = 0;
m_action[0].speed = 4;
}
else
{
m_action[0].mode = 0;
m_action[0].speed = 8;
}
if (start.x>b_pt.x)//球在起始点左边
{
Roundp2p1(temp211, m_action[0], 0, m_FishInfo[0]);
}
else//球在起始点右边
{
Roundp2p1(temp212, m_action[0], 0, m_FishInfo[0]);
}
}
else//鱼在球上边，先游到下边临时点
{
m_action[0].mode = 0;
m_action[0].speed = 14;
Roundp2p1(temp22, m_action[0], 0, m_FishInfo[0]);
}
}
}
return TRUE;
}
double CStrategy::Distance(CPoint point,CPoint 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;
}
double CStrategy::Checkangle(double dir)//把dir换算成[-pi~pi]之间的数
{
if(dir>PI)
dir-=2*PI;
else if(dir<-PI)
dir+=2*PI;
return dir;
}
void CStrategy::Roundp2p(CPoint aimpt,CFishAction &action,int fishID,CFishInfo m_FishInfo)
{
CPoint FishPt;// 鱼中心的位置
double FishDir;// 鱼头的方向
CPoint FishHeadPt;//鱼头的位置
CPoint LastFishPt;
CPoint FishRotatePt;//鱼的转动位置
CPoint FishTailPt;//鱼尾巴的位置
CPoint GoalPt1, GoalPt2;//临时目标点，GoalPt2没有用到
CPoint Pt1, Pt2, Pt3, Pt4;
//CPoint f_headpt;//point of fish's head
//f_headpt=m_FishInfo[0].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 centerpt0,centerpt00;
double dis1,dis2;
dis1=0;
dis2=0;
double radius;///需要游动的半径,可以随意的设置.
FishPt=m_FishInfo.GetCenterPoint();
FishDir=m_FishInfo.GetDirection();
dist1=Distance(FishPt,aimpt);
dir1=Angle(FishPt,aimpt);
dir1-=FishDir;
dir1=Checkangle(dir1)*180/PI;
action.speed=15;
m_FishInfo.SetAction(action);
if(dist1>100)
{
if(dir1>-5&&dir1<5)
action.direction=7;
else if(dir1<-5&&dir1>-10)
action.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 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>80&&dir1<90)
action.direction=14;
else
action.direction=14;
}
else
{
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&&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.direction=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);
}
void CStrategy::con2tempt(CPoint aimpt,CFishAction m_action[],CFishInfo m_FishInfo[])//游到一个临时点后退出
{
//获取鱼的信息
CPoint f_pt;//鱼的中心点坐标
f_pt=m_FishInfo[0].GetCenterPoint();//GetCenterPt()返回中心点坐标
double f_dir;//鱼的方向，[-PI,PI]
f_dir=m_FishInfo[0].GetDirection();
CPoint f_headpt;//point of fish's head
f_headpt=m_FishInfo[0].GetHeaderPoint();
double disheadtoaimpt;
disheadtoaimpt=this->Distance(f_headpt,aimpt);
bool tem=true;
if(tem)
{
Roundp2p(aimpt,m_action[0],0,m_FishInfo[0]);
if(disheadtoaimpt<10)
tem=false;
}
}
double CStrategy::Angle(CPoint point,CPoint aimer)
{
double a;
a=atan2((double)(aimer.y-point.y),(double)(aimer.x-point.x));
return a;
//return point.x;
}
void CStrategy::Point2point(CPoint aimpt,CFishAction &action, int fishID, CFishInfo m_FishInfo)
{
CPoint FishPt;// 鱼中心的位置
double FishDir;// 鱼头的方向
CPoint FishHeadPt;//鱼头的位置
CPoint LastFishPt;
CPoint FishRotatePt;//鱼的转动位置
CPoint FishTailPt;//鱼尾巴的位置
CPoint GoalPt1, GoalPt2;//临时目标点，GoalPt2没有用到
CPoint Pt1, Pt2, Pt3, Pt4;
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 centerpt0,centerpt00;
double dis1,dis2;
dis1=0;
dis2=0;
double radius;///需要游动的半径,可以随意的设置.
FishPt=m_FishInfo.GetCenterPoint();
FishDir=m_FishInfo.GetDirection();
dist1=Distance(FishPt,aimpt);
dir1=Angle(FishPt,aimpt);
dir1-=FishDir;
dir1=Checkangle(dir1)*180/PI;
action.speed=15;
if(dir1>-5&&dir1<5)
action.direction=7;
else if(dir1<-5&&dir1>-10)
action.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 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>80&&dir1<90)
action.direction=14;
else
action.direction=14;
}
效果
控制较为粗糙，且有BUG，实际效果依运气，最快可以47秒完成任务，最慢会超过5分钟。
备注与反思
- 本控制程序比较粗糙，原因是其中的角度调用函数使用方法未知，手上也没有相应的例程，编写的BUG无法排除，又临近课题验收，因此只能就此结束。若有时间可以通过角度函数实现精准的多的控制方法；
- 更进一步，由于鱼顶球的时候，球同样在运动，因此顶球控制程序实际应当可以将球的移动速度作为考虑因素之一，利用一些滤波估计算法得到下一时刻球的估计位置，可以实现更为精准的控制。
2万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
