void Avoid(Robot *robot,Environment *env,double x,double y) { int desired_angle = 0, theta_e = 0, d_angle = 0, vl, vr, vc = 190; int j=1; double dx, dy, d_e, Ka = 10.0/90.0;//给系数 Ka 赋个较小的初值，作为默认 情况的取值 dx = x - robot->pos.x;//目标点与小车在 x 轴方向上的距离 dy = y - robot->pos.y; d_e = sqrt(dx * dx + dy * dy);//小车与目标点之间的距离 if (dx == 0 && dy == 0) desired_angle = 90;//当小车已在目标点时，期望角赋为 90 度 else desired_angle = (int)(180. / PI * atan2((double)(dy), (double)(dx)));// 小车与目标点之间的连线与水平线之间的夹角，即期望角（与 position 函数里 的期望角相同） theta_e = desired_angle - (int)robot->rotation;//用上面一句计算出的期 望角减去小车当前的角度，得到一个差角 theta_e while (theta_e > 180) theta_e -= 360; while (theta_e < -180) theta_e += 360;/*这两句与 position 函数里的相同， 都是为了使得当计算出的差角|theta_e|>180 的时候，加上或减去 360 度以使 theta_e 的绝对值始终小于 180。因为从后面的程序可以看到，theta_e 实际上 是 小车需要旋转的角度(因为小车必须旋转直到其面向目标点，此时往前运动才能 完成朝向目标点的运动)，那么与其让它旋转大于 180 度，不如只让其旋转 360-theta_e 度角（这里实在不好表达）*/ if (d_e > 100.) //当小车到目标点的距离大于 100 时，Ka 取 45.0/90（即对不 同长度的距离分别选用不同大小的系数，系数的大小据实际情况定，是试出来的。 该系数后面计算轮速时要用到） Ka=45./90.; else if (d_e > 50) Ka=65./90; else if (d_e > 30) Ka=75./90.; else if (d_e > 20) Ka=85./90.; else Ka=95./90.; if (theta_e > 95 || theta_e < -95) { j=-1; theta_e += 180;//如果差角大于 90 度，将 theta_e 转化成其补角（即如果小车 是屁股朝向目标点的话，它没必要转一大圈将脑袋对着目标点前进，而是直接转 

个很小的角度让屁股正对目标点倒退着行进就可以了，这样就节约了时间，这就 是 position 函数里的原理） if (theta_e > 180) theta_e -= 360; if (theta_e > 80) theta_e = 80; if (theta_e < -80) theta_e = -80; if (d_e < 5.0 && abs(theta_e) < 40)//当小车离目标点很近并且差角也不大 的时候，Ka 取个很小的值 Ka = 0.1; vr = (int)(-vc * (1.0 / (1.0 + exp(-3.0 * d_e))-0.3 ) + Ka * theta_e);// 左轮速 vl = (int)(-vc * (1.0 / (1.0 + exp(-3.0 * d_e))-0.3 ) - Ka * theta_e);// 右轮速 /*计算轮速的公式，其中左右轮速的-vc * (1.0 / (1.0 + exp(-3.0 * d_e))-0.3 ) 是一样的，因为这项是赋予左右轮直线行驶的速度，他们大小相同，正负也相同。 从该项里可以看到唯一变化的量是 d_e，说明直线行驶时的轮速只与小车离目标 点的距离有关，距离越大，求出的轮速就越大，当距离为零时轮速也就为零了。 后面的一项 Ka * theta_e 前的正负号，左右轮速是相反的，因为该项是赋予左 右轮不同的轮速以使小车改变方向，这样让左右轮一个快一个慢从而达到使小车 沿弧线运动的目的。 从该项里可以看到变量有两个，一个是系数 Ka 一个是差角 theta_e，当小车没 有完全正对目标点时，差角 theta_e 产生的轮速便会使小车边往前运动边转（又 因为 theta_e 有些大， 可能会有几十度，而轮速的最大值是 125，如果直接加到轮速上的话会使得左右 轮速差过大，效果不好，所以加个系数 Ka 以控制其对轮速的影响） */ /*该轮速的计算公式即为 position 中的公式，之所以没改是发现写得极好，如 果嫌速度不够快的话就将 vc 的值赋为 190（见该函数开头声明变量处）， 这样计算出的轮速可使小车在几个周期里立刻达到最大速度 125，对距离很敏感 */ } else if (theta_e < 85 && theta_e > -85) { if (d_e < 5.0 && abs(theta_e) < 40) Ka = 0.1; vr = (int)( vc * (1.0 / (1.0 + exp(-3.0 * d_e)) -0.3) + Ka * theta_e); vl = (int)( vc * (1.0 / (1.0 + exp(-3.0 * d_e))-0.3 ) - Ka * theta_e); // vr = (int)( 120 + Ka * theta_e); // vl = (int)( 120 - Ka * theta_e); } else 

{ vr = (int)(+.17 * theta_e); vl = (int)(-.17 * theta_e); } int num; int i; double temp=100;//将循环里要用到的中间变量赋个很大的值 double dis,dir,dis_temp; int ex_vl,ex_vr; for(i=0;i<5;i++)//依次遍历对方五个小车，得到离自己最近的那个小车的编号 以及与自己之间的距离 { dis=GetDis(robot->pos.x,robot->pos.y,env->opponent.pos.x,env->opponen t.pos.y); if(dis<=temp) { temp=dis; num=i; } } dir=Avoiddir(env,robot,num,x,y);//将x,y代入f(x)=kx+h-y计算，如果 f(x)>0说明对方那个机器人在小车与目标点连线的上方，小车就应该从对方机 器人的下方过去以达到壁障的目的 dis_temp=1/(temp-3.1);//小车与对方最近机器人之间的距离（经过了一下下运 算，忘了为什么这么弄了，反正大概就这么个意思） int times=70;//倍数 if(dir>=0)//对方机器人在小车与目标点连线的上方 { ex_vl=j*dis_temp*times;//系数times就有点像上面的Ka那样，起个辅助作用 ex_vr=-j*dis_temp*times;//这里计算出的是为使小车避障而产生的额外的轮 速，实质上就是让左右轮速有个差值从而绕开障碍物 } else { ex_vl=-j*dis_temp*times; ex_vr=j*dis_temp*times; } Velocity(robot, vl+ex_vl, vr+ex_vr);//将的额外的轮速分别加到vl跟vr 里一起赋给左右轮，就达到避障的目的了，实际上就是在position函数里加了 一点点罢了 } double Avoiddir(Environment *env,Robot *robot,int num,double x,double y)//小车与目标点之间的连线设为y=kx+h,解出后再令f(x)=kx+h-y.然后将对 方机器人的坐标x，y代入计算出就可以看出对方机器人在直线上方还是下方了 

{ double k,h,a,b,i; k=(y-robot->pos.y)/(x-robot->pos.x); h=y-x*k; a=env->opponent[num].pos.x; b=env->opponent[num].pos.y; i=k*a-b+h; return i; } /*看到有好多新手对平台给的那个程序都不是很了解，今天好不容易有空把注释 添加上，就索性和着position函数里面的一些东东一块儿讲了。我能力有限， 有些地方几句话也不好表达清楚，结合实际理解吧，呵呵，希望能对大家有点帮 助。我花了两个小时才写完。^_^ ～～ */ 看起来有些乱 看起来好像有些乱，最好复制到vc++里面看