首先拿转向动作做例子
首先拿转向动作做例子:
o 90
o0
我们
计划
项目进度计划表范例计划下载计划下载计划下载课程教学计划下载
让robot从90度转到0度的方向。如果我们设定一个固定的左右轮速差比如(vl=20,vr=-20),那么robot会向右转,但是由于惯性,他又会转到小于0度的位置。(红线所示)结果robot将会在0度位置上下摆动。这样的转动是不合理的,因为它不能停留在0度。我们应该这样考虑,TagetAngle为目标角度,CurrentAngle为robot目前角度。 1当TagetAngle和CurrentAngle相差较大的时候,我们设定一个较大的左右轮速差,这样robot转动很快,是一个加速的动作,CurrentAngle和TagetAngle逐渐靠近。
2当CurrentAngle 和TagetAngle差距减小到某一个值的时候,我们应该减小左右轮速差,减小加速度,或者甚至采取“刹车”减速,使其最终停留在TagetAngle。 3尽管我们考虑的很周到,但是由于某种原因比如误差或者碰撞,我们的robot的角度CurrentAngle有可能超过TagetAngle,这样相当于又回到了2,只要我们的模型参数正确,就可以把误差减小并且实现转向的动作。
下面看代码
void Angle( Environment *env, int robot,double angle)
{
Mydata * p;
p=(Mydata *)env->userData;
double speed = 0; //和pangle接轨
double accuracy=1;
double turnangle=0,nextangle=0;
double FF=125; //最大减速度
//FF为左右轮速差的一半
turnangle = angle -p->robot[robot].rotation;
RegulateAngle(turnangle);
//根据目标角度和当前角度的差来选择适当的左右轮速的差 FF
if(turnangle < 1 && turnangle >-1)
{
Velocity(env,robot,0,0);
return ;
}
else if(turnangle < 2 && turnangle >-2)
FF=10;
else if( turnangle >-3 && turnangle < 3)
FF=15;
else if( turnangle >-5 && turnangle < 5)
FF=30;
double v=p->robot[robot].rotation - p->myoldpos[robot].z ;
//v是当前robot的转动角速度
RegulateAngle(v);
double v1=v;
double f=0; //相当于减速时,右轮速度,
// int n=0;
bool turnleft=true; //判断小车是否是该向左转
double a=ANGLE_A;
double b=ANGLE_B;
//因为robot的头尾是等效的,所以我们应该判断是头更接近目标角度还是尾更接近目标角
度
if(turnangle > 90)
{
turnleft=false;
turnangle-=180;
}
else if(turnangle >0)
{
turnleft=true;
}
else if(turnangle > -90)
{
turnleft=false;
}
else
{
turnleft=true;
turnangle+=180;
}
if(turnleft)
{//
f=-FF;
v1=AngleOne(v1,speed+f,speed-f); //v1+=a *( -b *f-v1);
nextangle+=v1;
//计算如果按最大加速度转动后robot的角度
do{//whether to reduce
//收敛!!
v1 =AngleOne(v1,speed-f,speed+f);//+= a *( b *f-v1); // v1
nextangle+=v1;
}while( v1 > 0 );
//这个循环的是模拟让robot以最大减速度减速到转动开始反向是转动过的角度 //下面将会拿这个角度和目标角度比较如果不超过目标角度,则可以继续以最大速度加速旋转
nextangle-=v1;
if(nextangle < turnangle)
{//不满足减速条件 所以 f 取相反数
Velocity(env,robot,speed+f,speed-f);
}
else
//这时,如果再继续加速旋转的话,则来不及“刹车”,因此要开始刹车减速了
{//reduce
v1 = AngleOne(v,speed-f,speed+f); //v + a *( b *f-v);
//选取适当的刹车力度ff,使得robot刚好不超过设定的方向
if( v1 < 0 )
{
do{//该降低功率了
f++;
v1 = AngleOne(v,speed-f,speed+f); //v + a *( b *f-v);
}while( v1 < turnangle && f
turnangle)
{//不满足减速条件 所以 f 取相反数
Velocity(env,robot,speed+f,speed-f);
}
else
{//reduce
v1 = AngleOne(v,speed-f,speed+f); //v + a *( b *f-v);
if( v1 > 0 )
{
do{//该降低功率了
f--;
v1 = AngleOne(v,speed-f,speed+f); //v + a *( b *f-v);
}while( v1 > turnangle && f >-FF);
}
Velocity(env,robot,speed-f,speed+f);
}
}
}
下面是 跑位 动作的例子
跑位比转向要复杂一点,因为它不仅有转向还有平移。
目标点
我们实现一点到另外一点的运动可以有两种方式,1先旋转后直线跑位2跑位同时旋转。但是第一种有很多不利因素,相对来说,第二种方式效率更高。因此,通常我们的运动轨迹不是一条直线,而是一条曲线。基本的思路是这样的:
vl=v0+v2;
vr=v0-v2;
只要v0!=0,左右轮速之和vl+vr!=0,这样可以保证robot可以在旋转的同时前进或者后退。而且只要v0/v2的值小于某一个值(与robot和目标点之间的距离和偏离的角度有关,与robot的运动速度也有关)就可以以目标点为中心,robot不断靠近目标点,而且方向不断接近目标点。
最简单的曲线跑位方式是这样的
theta
d Target
R R
o
就是让robot通过一段光滑的圆周曲线到达目标点,其左右轮速可以这样求得到: 设robot的平移轮速为V0,robot左右轮之间距离L;
通过robot和目标点之间的夹角theta和距离可以确定出这条曲线的圆心位置o和圆周半径R。因为既然robot通过圆周曲线到达目标点,那么robot的方向总是和圆周向切的,换句话说,圆心在robot的垂直方向上。2 *R *sin(theta) =d; R,L,vl,vr有这样的关系:
vl/(R+L/2)=vr/(R-L/2);
又因为vl=V0+v2;vr=V0-v2;则可以计算出v2.这样这个跑位动作所需要的动作参数我们就到确定下来了。
同Angle函数一样,如果我们不采取减速措施,可能robot会在目标点来回晃动。
另外,读者可以想象一下,如果v2的绝对值小于上面的计算值,那么robot的运动轨迹会是什么样的,大于呢,
实际上以上两种假设的结果如下
第一种,“小于”,
robot的实际路线
theta
d Target
R R
o
第二种,“大于”,
robot的实际路线
(绿色+红色)
theta
d Target
R R
o
如果有兴趣当然可以自己动手
设计
领导形象设计圆作业设计ao工艺污水处理厂设计附属工程施工组织设计清扫机器人结构设计
一下,可以观察到实际robot的运动路线。 看上去只有最开始的标准圆周型路线才是“合格”的,实际上不是,我们的程序采用了第二种“大于”思想,在theta较大时,用“大于”让robot更快的转向,绿色部分,当转动到robot朝向目标点时,我们采用直线前进的方式~我们可以直观的看到这样的路程要比圆周
短。至于短到什么程度,有赖于程序中参数的设定。
请看下面的跑位函数
void PositionAndStop(Environment *env,int robot,Vector3D pos ,double bestangle,double limit)
{ //考虑到可能的 急停和 急快速加速
//特别作了优化
//还有就是 被碰转后的转角过程 不能耽搁时间!!!
//转角是最危险的过程
Mydata * p;
p=(Mydata *)env->userData;
double anglespeedmax=0; //控制转交速度的变量
double vmax=125; //默认的跑位加速度
double Limitedangle=2; //默认减速范围
//limit 的 含义是: 目标点应该是一个范围,而limit就是这个范围的半径
if( limit < 0.5 )
limit =0.5;
double Limiteddis=limit; //减速范围有一个下限,保证不会来回跑动
double distance; //robot和目标点的距离
double turnangle,posangle,vangle; //转动角度 ,目标点相对robot的角度,速度的绝对角度
double dx,dy; //pos 和robot的坐标差
double a=SPEED_A; //参数
double b=SPEED_B;
double v,v1; //临时保存速度的大小!!!
double f=vmax; //加速度变量
double s=0; //预测的减速位移(路程)
int n=0; //跑位的步数
bool face=true; //判断小车是否是正面前进
v= sqrt(p->myspeed[robot].x * p->myspeed[robot].x +
p->myspeed[robot].y*p->myspeed[robot].y);
//临时保存速度的大小!!!
dx = pos.x - p->robot[robot].pos.x ; //pos 和robot的坐标差
dy = pos.y - p->robot[robot].pos.y ;
distance = Distance(p->robot[robot].pos , pos);
posangle = Atan(dy,dx);
turnangle = p->robot[robot].rotation - posangle; //转动角度
RegulateAngle(turnangle);
//考虑是正面跑还是倒退,因为robot的头尾是等效的
if(turnangle > 90)
{//判断小车是否是正面前进
face=false;
turnangle-=180;
}
else if(turnangle < -90)
{
face=false;
turnangle+=180;
}
else
{
face=true;
}
vangle = p->myspeed[robot].z - p->robot[robot].rotation; //速度的方向和robot正面
的夹角
RegulateAngle(vangle); //主要用于最后控制减速度的大小
if( vangle <-90 || vangle > 90 ) //同时判断v的正负
v=-v;
//下面判断robot运动是否远离目标点
if(face)
{//forward 跑位,如果后退的话 就v=0
//设vl,vr=0 还是vl,vr=125 有一个条件有一个临界条件那就是
//v = SPEED_ZERO
if(v < -SPEED_ZERO)
{
Velocity(env,robot,0,0);
return ;
}
}
else if(v > SPEED_ZERO)
{//back 跑位,如果后退的话 就v=0
Velocity(env,robot,0,0);
return ;
}
v1=v; //v1 is changing while program running
//whlie, v is not
//Limiteddis:大于Limiteddis时,不考虑减速
if(distance > Limiteddis )
{//it is too early to count the steps
//but the Limiteddis should be tested!! to do...
if(turnangle > Limitedangle || turnangle < -Limitedangle)
{//adjust angle
/////////////////测试这一段
//对于goalie这一段应该特别注意
//发生变向 1.knock the robot,especially the opponent
// 2.knock the wall
// so the anglespeedmax is allowed ++ more!!
if(turnangle > 20 || turnangle < -20)
anglespeedmax = 0;
else if(turnangle > 10 || turnangle < -10)
anglespeedmax = 125;
else if(turnangle > 5 || turnangle < -5)
anglespeedmax = 180;
else
anglespeedmax = 200;
///////////////测试这一段
PAngle(env,robot,posangle,anglespeedmax);
}
else
{
if(face)
Velocity(env,robot,f,f);
else
Velocity(env,robot,-f,-f);
}//it is time to rush
}
else //开始考虑角度的偏差了
{
if(distance > 1)
{ //调整角度 return!!!!!!
//radious of robot is about 1.5 ,so the distance is very short
if(turnangle > Limitedangle || turnangle < -Limitedangle)
{
Angle(env,robot,posangle);
return ;
}
}
if(distance < 0.4)
{ //停止并转向 return!!!!!!
//radious of robot is about 1.5 ,so the distance is very short
if( v<0.1 && v>-0.1)
{ //the range of v should be tested
Angle(env,robot,bestangle);
return ;
}
}
//如果不需要调整角度,那么考虑减速~
//跑位函数的减速过程和Angle是一样的,所以不再赘述
if(true)
{
vmax=125;
if(face)
{
f=-vmax; //减速度 为 0000000
v1=VelocityOne(v1,-f,-f); //加速一步
s=v1;
do{//whether to reduce //之所以有与SPEED_ZERO比较,是因为v1=VelocityOne(v1,0,0)刹车和v1=VelocityOne(v1,f,f)
效果不同
if(v1 > SPEED_ZERO) //as i said,this is limited
v1=VelocityOne(v1,0,0);
else
v1=VelocityOne(v1,f,f);
s+=v1;
}while( v1 > 0 );
s-=v1;
if(s < distance)
{//不满足减速条件加速
Velocity(env,robot,-f,-f);
}
else
{
if(v > SPEED_ZERO)
Velocity(env,robot,0,0);
else
{
v1=VelocityOne(v,f,f); //减速一步
if( v1 < 0 )
{
do{//该降低功率了
f++; //f=-vmax;
v1 = VelocityOne(v,f,f);
}while( v1 < distance && f < vmax);
}
Velocity(env,robot,f,f);
}
}
}
else
{
f=vmax; //减速度!!!!!
v1=VelocityOne(v1,-f,-f);
s=v1;
do{//whether to reduce
if(v1 < -SPEED_ZERO) //as i said,this is limited
v1=VelocityOne(v1,0,0);
else
v1=VelocityOne(v1,f,f);
s+=v1;
}while( v1 < -0.1 );
s-=v1;
if(s > -distance)
{//不满足减速条件加速
Velocity(env,robot,-f,-f);
}
else
{
if(v < -SPEED_ZERO)
Velocity(env,robot,0,0);
else
{
v1=VelocityOne(v,f,f); //减速一步
if( v1 > 0 )
{
do{//该降低功率了
f--; //f=-vmax;
v1 = VelocityOne(v,f,f);
}while( v1 > -distance && f > -vmax);
}
Velocity(env,robot,f,f);
}
}
}
}
}
}
另外以下函数和上面跑位函数类似
void PAngle( Environment *env, int robot,double angle,double speed);
void PositionAndStopX(Environment *env,int robot,Vector3D pos ,double Xangle,double
limit) ;
void PositionAndThrough(Environment *env,int robot,Vector3D pos ,double MAX); void PositionBallX(Environment *env,int robot,Vector3D pos ,double Xangle,double limit) ; void GoaliePosition(Environment *env,int robot,Vector3D pos ,double bestangle,double limit) ;
还有一个很重要的动作,就是Kick!
Kick和Position相比有两个特点1:robot只能从一个方向跑到目标点 2:这个目标点可能是变化的。
因此这个
踢球方向
theta
theta
下
一
踢球方向 个theta 周
期
theta
下
一
踢球方向 个
周
期
一个简单的Kick函数,如图所示:robot和ball的连线与踢球方向之间有夹角theta,如果让robot的运动方向与这个连线也成theta角度,并且在连线的同侧,那么就可以实现踢球这个动作。当然这个theta角度在不断变小,因此robot和ball的连线就越来越和踢球方向趋向一致。最终冲向球,达到踢球的目的。
实际上的Kick函数,有更多的考虑,主要是:1:
踢球方向
theta
theta
如果theta〉90,也就是说,robot在ball的“前面”,按照上面的算法就不合适了。 2
踢球方向
如果robot和ball的距离太近,而且方向不对,那么在转向的时候可能已经把ball推开了,这样就达不到kick的目的了。
因此这时更好的办法是:
踢球方向
先拉开一定距离,在调整为同方向踢球。
3如果robot和ball距离很远,也可以先缩小他们的距离再调整同一方向Kick。如:
踢球方向
下面是程序中的Kick函数
void Kick2(Environment *env , int robot , Vector3D ToPos ) {
Mydata * p;
p=(Mydata *)env->userData;
double LimitedCircle = 3;
Vector3D ball = Meetball_p(env,robot); //use the predictball position
Vector3D RobotToBall; //人和球的相对位置
RobotToBall.x = ball.x - p->robot[robot].pos.x ;
RobotToBall.y = ball.y - p->robot[robot].pos.y ;
RobotToBall.z = Atan(p->robot[robot].pos , ball);
Vector3D BallToGate ; //球和球门的相对位置
BallToGate.x = ToPos.x - ball.x ;
BallToGate.y = ToPos.y - ball.y ;
BallToGate.z = Atan(ball , ToPos);
double gateangle=BallToGate.z;
double RunAngle ;
RunAngle = RobotToBall.z - BallToGate.z;
RegulateAngle(RunAngle);
double dis= Distance(ball,p->robot[robot].pos);
if(dis > 3*LimitedCircle){
Vector3D Center;
if(RunAngle >0){
BallToGate.z -=90;
}
else{
BallToGate.z +=90;
}
RegulateAngle(BallToGate.z);
Center.x = ball.x + LimitedCircle*cos(BallToGate.z /180.0);
Center.y = ball.y + LimitedCircle*sin(BallToGate.z /180.0);
Center.z = 0;
double distance = Distance(Center,p->robot[robot].pos);
if(distance < 2*LimitedCircle)
{
RunAngle = RobotToBall.z + RunAngle /2; // 可以调整 2
}
else{
double CenAngle= Atan(p->robot[robot].pos,Center);
if(RunAngle <0){
RunAngle =
CenAngle-180*LimitedCircle*asin(LimitedCircle/distance)/3.142;
RegulateAngle(RunAngle);
}
else{
RunAngle =
CenAngle+180*LimitedCircle*asin(LimitedCircle/distance)/3.142;
RegulateAngle(RunAngle);
}
}
}
else{
RunAngle = RobotToBall.z + RunAngle /2; // 可以调整 2
RegulateAngle(RunAngle);
}
double paraA=gateangle - p->robot[robot].rotation;
if(paraA<0){
paraA=-paraA;
}
if(paraA>90){
paraA=180-paraA;
}
if(0.1>paraA)
{
paraA=0.1;
}
double paraB=125*dis/3*LimitedCircle*10/paraA;
if(paraB>125){
paraB=125;
}
PAngle(env,robot,RunAngle,paraB);
}