机器人避障.pdf

《机器人避障.pdf》由会员分享，可在线阅读，更多相关《机器人避障.pdf（10页珍藏版）》请在得力文库 - 分享文档赚钱的网站上搜索。

1、机器人避障1/10 源程序如下：/*Template to access ARIA(ActivMedia Robotics Interface for Applications)using Microsoft Visual C+,console Projects.*/#include Aria.h#include#include#include#include#include#include#include usingnamespace std;/the robot ArRobot*robot;ArSonarDevice sonar;/sonar,must be added to the ro

2、bot ArSick sick;/the laser int step=0;double d0,d1,d3,d2,d4,d15;double th,r,PI=3.1415926;double minimum=0;/sonar sensor offset positions with respect to robot local coordinate frame double robotSonarX8=69,114,148,166,166,148,114,69;double robotSonarY8=136,119,78,27,-27,-78,-119,-136;double robotSona

3、rTh8=90.0,50.0,30.0,10.0,-10.0,-30.0,-50.0,-90.0;double laserData2180;double sonarData516;/last 5 sets of sonar readings double posData5;/odometry readings long frameCount=-1;/how many iterations of the sensor grabbing functions have been run/*Files for logging position and sensor data*/ofstream pos

4、itionData(positionvals.txt);ofstream sonarVals(sonarvals.txt);ofstream globalFrame(globalFrame.txt);机器人避障2/10/*Function Definitions*/void grabNewPositionData(void);void logPositionData(void);void grabNewSonarData(void);void logRawSonarData(void);void MapToGlobalFrame(void);bool flag=true;bool weWant

5、ToTurnTheRobot=true;bool decideWhichDirectionToTurn=true;bool turnRobotRight=false;bool turnRobotLeft=false;void update(void)/Your Code Here grabNewSonarData();grabNewPositionData();logPositionData();logRawSonarData();/grabNewLaserData();MapToGlobalFrame();int sequence=0,1,2,3,4,5,6,7;int leftValue=

6、200,200,200,200,-50,0,40,150;int rightValue=150,40,0,-50,200,200,200,200;int minVal=9999;/int intriger=0,0,0,0,0,0;int minSonar=0;for(int i=0;i8;i+)if(sonarData0sequencei300)&(sonarData04300)&(sonarData02250)&(sonarData05250)&(sonarData01200)&(sonarData06200)&(sonarData00150)&(sonarData07150)&(flag)

7、robot-setVel2(leftValueminSonar,rightValueminSonar);/reset which direction to turn(turn either left or right)decideWhichDirectionToTurn=true;/we do not know which direction to turn yet turnRobotRight=false;turnRobotLeft=false;else/some obstacles are very close to the robot /-2-/decide which directio

8、n to turn the robot if(decideWhichDirectionToTurn)if(sonarData02250)|(sonarData01200)|(sonarData00150)/is the nearest obstacle on the left of the robot turnRobotRight=true;/yes the nearest obstable is on the left of the robot(turn right)elseif(sonarData04300)|(sonarData03stop();/first stop the robot

9、 if(turnRobotRight)/we turn right robot-setDeltaHeading(-20);if(turnRobotLeft)/or we turn left robot-setDeltaHeading(20);flag=false;weWantToTurnTheRobot=false;/-/-4-elseif(robot-isHeadingDone()/check to see if the robot has finished turning flag=true;weWantToTurnTheRobot=true;/-/end update void MapT

10、oGlobalFrame()double r,u,v,th;double Xr,Yr,Thr;double Xo,Yo,Xg,Yg;for(int i=0;i1;i+)r=sonarData0i;u=robotSonarXi;v=robotSonarYi;th=(robotSonarThi*PI)/180;机器人避障5/10 Xr=posData0;Yr=posData1;Thr=(posData2*PI)/180;double u2=u*cos(Thr)-v*sin(Thr);double v2=u*sin(Thr)+v*cos(Thr);double a=r*cos(th+Thr);dou

11、ble b=r*sin(th+Thr);Xg=a+u2+Xr;Yg=b+v2+Yr;/*Xo=u+r*cos(th);Yo=v+r*sin(th);Xg=(Xo*cos(Thr)-Yo*sin(Thr)+Xr;Yg=(Xo*sin(Thr)+Yo*cos(Thr)+Yr;*/cout r tx:t Xg y:t Yg endl;globalFrame Xg t Yg endl;/*void MapToGlobalFrame2()double r,u,v,th;double Xr,Yr,Thr;double Xo,Yo,Xg,Yg;for(int i=1;i2;i+)r=sonarData0i;

12、u=robotSonarXi;v=robotSonarYi;机器人避障6/10 th=robotSonarThi;Xr=posData0;Yr=posData1;Thr=posData2;double u2=u*cos(Thr)-v*sin(Thr);double v2=u*sin(Thr)+v*cos(Thr);double a=r*cos(th+Thr);double b=r*sin(th+Thr);Xg=a+u2+Xr;Yg=b+v2+Yr;/Xo=u+r*cos(th);/Yo=v+r*sin(th);/Xg=(Xo*cos(Thr)-Yo*sin(Thr)+Xr;/Yg=(Xo*si

13、n(Thr)+Yo*cos(Thr)+Yr;cout x:t Xg y:t Yg endl;globalFrame Xg t Yg getX();posData1=robot-getY();posData2=robot-getTh();posData3=robot-getLeftVel();posData4=robot-getRightVel();机器人避障7/10 /*Log the current Odometry values*/void logPositionData()if(frameCount=0)positionData XtYtThtlVeltrVel endl;positio

14、nData posData0 t posData1 t posData2 t posData3 t posData4 endl;/*Saves a snapshot of the sonar readings at the start of each cycle to ensure both efficiency and consistancy.For use in the rest of a processing cycle.*/void grabNewSonarData()int i;/get the current range readings for each of the 8 son

15、ar sensors for (i=0;igetSonarRange(i);/*Saves a snapshot of the laser readings at the start of each cycle to ensure both efficiency and consistancy.For use in the rest of a processing cycle.*/*void grabNewLaserData()double angle;int i,start,end;/start angle of the first laser bucket start=-90;/end a

16、ngle of the first laser bucket 机器人避障8/10 end=start+LASER_ANGLE_RANGE_STEP-1;/for each of the laser buckets for(i=0;iSTEPS_LASER;i+)/return the range(in mm)and angle(in radians)/for the bucket starting at start degrees through to end degrees laserData0i=sick.currentReadingPolar(start,end,&angle);lase

17、rData1i=angle;/update the bucket by incremenenting the start and end laser bucket possitions by /the desired step start+=LASER_ANGLE_RANGE_STEP;end+=LASER_ANGLE_RANGE_STEP;*/*Log the current Raw Sonar Range data */void logRawSonarData()if(frameCount=0)sonarVals No.t;sonarVals frameCount t;for (int i

18、=0;i16;i+)sonarVals sonarData0i t;/cout sonarData0i t;sonarVals endl;/coutsetDeviceConnection(&con);/add the sonar to the robot robot-addRangeDevice(&sonar);/try to connect,if we fail exit if (!robot-blockingConnect()printf(Could not connect to robot.exitingn);Aria:shutdown();return 1;/user task rob

19、ot-addUserTask(update,50,&updateCB);/turn on the motors,turn off amigobot sounds robot-comInt(ArCommands:SONAR,1);robot-comInt(ArCommands:ENABLE,1);robot-comInt(ArCommands:SOUNDTOG,0);/start the robot running,true so that if we lose connection the run stops robot-run(true);机器人避障10/10 robot-lock();robot-disconnect();robot-unlock();/now exit Aria:shutdown();return 0;