首页 首先拿转向动作做例子

首先拿转向动作做例子

举报
开通vip

首先拿转向动作做例子首先拿转向动作做例子 首先拿转向动作做例子: o 90 o0 我们计划让robot从90度转到0度的方向。如果我们设定一个固定的左右轮速差比如(vl=20,vr=-20),那么robot会向右转,但是由于惯性,他又会转到小于0度的位置。(红线所示)结果robot将会在0度位置上下摆动。这样的转动是不合理的,因为它不能停留在0度。我们应该这样考虑,TagetAngle为目标角度,CurrentAngle为robot目前角度。 1当TagetAngle和CurrentAngle相差较大的时候,我们设定一个较大的左右...

首先拿转向动作做例子
首先拿转向动作做例子 首先拿转向动作做例子: 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); }
本文档为【首先拿转向动作做例子】,请使用软件OFFICE或WPS软件打开。作品中的文字与图均可以修改和编辑, 图片更改请在作品中右键图片并更换,文字修改请直接点击文字进行修改,也可以新增和删除文档中的内容。
该文档来自用户分享,如有侵权行为请发邮件ishare@vip.sina.com联系网站客服,我们会及时删除。
[版权声明] 本站所有资料为用户分享产生,若发现您的权利被侵害,请联系客服邮件isharekefu@iask.cn,我们尽快处理。
本作品所展示的图片、画像、字体、音乐的版权可能需版权方额外授权,请谨慎使用。
网站提供的党政主题相关内容(国旗、国徽、党徽..)目的在于配合国家政策宣传,仅限个人学习分享使用,禁止用于任何广告和商用目的。
下载需要: 免费 已有0 人下载
最新资料
资料动态
专题动态
is_594886
暂无简介~
格式:doc
大小:45KB
软件:Word
页数:0
分类:生活休闲
上传时间:2017-12-02
浏览量:2