Page 289 - Introduction to Autonomous Mobile Robots

Chapter 6
274
Practical application: example of Bug2. Because of the popularity and simplicity of
Bug2, we present a specific example of obstacle avoidance using a variation of this tech-
nique. Consider the path taken by the robot in figure 6.8. One can characterize the robot’s
motion in terms of two states, one that involves moving toward the goal and a second that
involves moving around the contour of an obstacle. We will call the former state GOAL-
SEEK and the latter WALLFOLLOW. If we can describe the motion of the robot as a function
of its sensor values and the relative direction to the goal for each of these two states, and if
we can describe when the robot should switch between them, then we will have a practical
implementation of Bug2. The following pseudocode provides the highest-level control
code for such a decomposition:

public void bug2(position goalPos){
boolean atGoal = false;
while( ! atGoal){
position robotPos = robot.GetPos(&sonars);
distance goalDist = getDistance(robotPos, goalPos);
angle goalAngle = Math.atan2(goalPos, robotPos)-robot.GetAngle();
velocity forwardVel, rotationVel;
if(goalDist < atGoalThreshold){
System.out.println("At Goal!");
forwardVel = 0;
rotationVel = 0;
robot.SetState(DONE);
atGoal = true;
}
else{
forwardVel = ComputeTranslation(&sonars);
if(robot.GetState() == GOALSEEK){
rotationVel = ComputeGoalSeekRot(goalAngle);
if(ObstaclesInWay(goalAngle, &sonars))
robot.SetState(WALLFOLLOW);
}
if(robot.GetState() == WALLFOLLOW){
rotationVel = ComputeRWFRot(&sonars);
if( ! ObstaclesInWay(goalAngle, &sonars))
robot.SetState(GOALSEEK);
}
}
robot.SetVelocity(forwardVel, rotationVel);
}
}