Obstacle Avoidance in the Real World

All I wanted was to have my robot roam about the house without bumping into things or getting stuck. What could be easier I thought? Well, it turned out to be surprisingly difficult to find a system that works regardless of the typically cluttered state of my apartment, so I thought I'd share my experience in the form of a short tutorial.

Most readers here are familiar with infrared and sonar sensors for measuring the distance to an object. For a very small robot, a single such sensor mounted on the front and pointing forward might be all you need: if the sensor detects an object within a certain distance, then turn left or right to avoid it. The trouble arises when your robot has some significant distance between its wheels or it is more than few inches tall.

The wheels on the robot shown below are 9.5 inches apart and it stands 19.5 inches tall. In this case, a single range sensor won't cut it: either the wheels get stuck on objects that the ranger doesn't detect because they are outside its lateral range, or the top of the robot runs into things like chair seats since a low-lying range sensor will send its beam right under the seat without an echo.



There are at least two solutions to this problem: use more sensors or mount a sensor on a pan and tilt servo mechanism so your single sensor can sweep through a greater lateral and vertical range. If we want our robot to move fairly quickly, a single sweeping single sensor might be looking the wrong way at just the wrong moment. So let's opt for multiple fixed sensors. The photos below show their arrangement.



The three IR sensors on the bottom of the robot detect low lying obstacles such as pillows on the ground, walls, pets, etc. The two sensors on the sides act as infrared "bumpers". The beams are oriented so as to pass just in front of the opposite wheel. If an object cuts this beam by getting too close to the front of the wheel, then we know we should turn away from that direction. See the next photo for an illustration:



The front-facing sonar sensor detects obstacles straight ahead. Since sonar sensors tend to have a wider beam and range than infrared sensors, it makes a good general purpose obstacle detector. However, there is one "gotcha" about sonar sensors that had me puzzled for days before I figured it out. If you mount such a sensor too close to the ground, it will detect small irregularities in the floor surface that you wouldn't consider obstacles. For example, my dining room carpet is barely ½ an inch above the tile kitchen floor. Nonetheless, if the front facing sonar sensor is mounted less than about 4" off the floor, its beam bounces back off this ½ rise and the poor robot can't get out of the kitchen. IR sensors can be mounted much closer to the floor without suffering from the same problem.

You would think this would be enough sensors to handle even the most cluttered household environment. But no: no fewer than five different kinds of object can avoid detection with this system: chairs, tables, beds, ottomans and cadenzas. I know because my robot ran into all of these things multiple times. What do all these pieces of furniture have in common? There are spaces beneath them that do not touch the ground and may even be fairly high above it. On the other hand, the upper parts of the objects are just floating there above the floor waiting to whack the robot.

Take a look at the chair in the picture below. The sensors we have so far can detect the chair's support where it meets the floor, but the chair seat sticks far out from the support. So while the lower sensors think the robot still has a ways to go before running into something, crunch, the upper part of the robot runs into the seat.



So how can we detect all these threats when they could be different heights above the ground? We can't simply mount a forward facing sensor at some height up the robot because it will only detect objects near that particular height. One solution that seems to work well is to mount a front-facing IR sensor pointing upward about 45 degrees as shown in the photo below:



As you can see from the picture, the beam can now be interrupted by objects of various heights. The beam can even reflect from the underside of an object if it happens to miss it from head on.

So we now have an arrangement of sensors that seems to have us covered. There is one more IR sensor mounted directly under the Christmas ornament and facing forward, simply as extra protection against cracking the ball against something.

What kind of algorithm should we now use to make use of the sensors? Of course, this depends on what we want our robot to do. Suppose we just want our robot to explore the house at random but without running into things or ever getting stuck. Then the following set of rules should do the trick:

  • obstacle on the left, turn right

  • obstacle on the right, turn left

  • obstacle straight ahead, turn left or right at random

  • obstacle straight ahead and really close, back up a bit, then turn left or right

To avoid oscillating back and forth in a corner, we can add another rule:

  • if we have just made a turn, and we need to turn again, don't turn back in the opposite direction. Instead, keep rotating in the same direction as the last turn

The following video shows our robot following these rules while moving about on a clutter floor:




As you can see, the robot comes very close to various objects without running into them or getting stuck. Just how close the robot is willing to get to obstacles can be adjusted using a number of parameters as describe below.

The following code snippet, written in C#, is one way to implement this algorithm. First let's define a number of parameters that determine how close we are willing to come to an object before we make an avoidance maneuver:

// 12" threshold for our forward facing sonar
private int fowardSonarThreshold = 12;
// 16" for the forward IR sensor (under the robot)
private int fowardIRThreshold = 16;
// 17" for the IR sensor mounted under the Christmas ball
private int towerIRThreshold = 17;
// 20" for the IR "bumpers"
private int sideSensorThreshold = 20;
// 18" for the 45-degree upward pointing IR sensor
private int upwardSensorThreshold = 18;
 // Within 7" we back up
private int backupSensorThreshold = 7;

The forwardSonarThreshold parameter is set to 12 inches. This means that if an object is pinged to be within 12 inches by the forward-facing sonar sensor, then we will make a turn. The forwardIRSensor is set to 16 inches because the forward-facing IR sensor under the robot is offset 4 inches back from the front edge of the robot. Setting the backupSensorThreshold to 7 means that if an object straight ahead somehow got to within 7 inches, then back up a bit before making a turn. And so on for the other parameters. You can play with these values to make the robot more or less cautious regarding how close it comes to obstacles.

Here now is our motor control loop. Note that the code below depends on some other code I won't list here because it has a lot of other extraneous stuff not relevant to our tutorial. All we really need to know is that the Sensors class contains methods for getting back the current distance values from the various range sensors. For example, Sensors.SonarFront() returns the current reading from the front-facing sonar sensor.

while (true)
{
   // Start with the assumption that we can go straight.
   nextMove = "Straight";

   // Reset the closest obstacle distance to a large value.
   minDistance = 10000;

   // Do any of the front facing range sensors detect an obstacle closer than their
   // threshold?  If so, then prepare to turn left or right.
   if (Sensors.SonarFront() < fowardSonarThreshold ||
      Sensors.IRfrontCenter() < fowardIRThreshold ||
      Sensors.IRfrontTower() < towerIRThreshold ||
      Sensors.IRfrontUpward() < upwardSensorThreshold)
   {
      nextMove = "LeftRight";
   }

   // What about the two sideways looking IR detectors?
   if (Sensors.IRleft() < sideSensorThreshold && Sensors.IRright() <  sideSensorThreshold)
   {
       nextMove = "LeftRight";
   }
   elseif (Sensors.IRleft() < sideSensorThreshold)
   {
      nextMove = "Right";
   }
   elseif (Sensors.IRright() < sideSensorThreshold)
   {
      nextMove = "Left";
   }

   // How close is the closest object in front of us?  Note how we subtract
   // 4" from the front center IR sensor (under the robot) and 5" from the
   // the tower IR sensor.  This is because these two sensors are offset by
   // these distances back from the front of the robot.
   minDistance = Math.Min(Sensors.SonarFront(), Sensors.IRfrontCenter() - 4,
   Sensors.IRfrontTower() - 5, Sensors.IRfrontUpward(), Sensors.IRleft(), Sensors.IRright());

   // If the closest object is too close, then get ready to back up.
   if (minDistance < backupSensorThreshold) nextMove = "Backup";

   // If we can't go forward, stop and take the appropriate evasive action.
   if (nextMove != "Straight")
   {
       DriveMotors.pidDrive.Stop();

       if (turnDirection == "Backward")
       {
           // Keep track of the last action.
           lastMove = "Backward";
           DriveMotors.pidDrive.Speed = driveSpeed / 2;
           DriveMotors.pidDrive.Distance = -4; // Back up 4 inches.
           DriveMotors.pidDrive.TravelDistance();
      }
      else
      {
          // Make sure we don't oscillate back and forth in a corner.
          if (lastMove == "Left" || lastMove == "Right") nextMove = lastMove;
          if (nextMove == "Left")  nextTurn = -1;
          elseif (nextMove == "Right") nextTurn = 1;
          elseif (nextMove == "LeftRight") nextTurn = turnChoice.Next(-1, 1);
          //Random left or right.

          // Keep track of the last turn.
          if (nextTurn == -1) lastMove = "Left";
          else lastMove = "Right";

          DriveMotors.pidDrive.Speed = driveSpeed;
          // Turn 45 degrees left or right.
          DriveMotors.pidDrive.RotationAngle = Math.Sign(nextTurn) * 45;
          DriveMotors.pidDrive.Rotate();
      }
   }
   else
   {
       // If no obstacles are detected close by, keep going straight ahead.
       lastMove = "Straight";
       DriveMotors.pidDrive.Speed = driveSpeed;
       DriveMotors.pidDrive.TravelAtSpeed();
   }        

    // Let the thread sleep for 1/10 of a second before repeating the loop.
    Thread.Sleep(100);
}

And that's all there is to it. Beginning with this simple loop, a number of variations come to mind. For example, you can have the robot slow down a bit as it gets closer to objects which gives it more time to maneuver if necessary. You could also have the robot turn a variable amount instead of a fixed 45 degrees. For example, one could make the turn angle inversely proportional to the distance of the nearest object (minDistance) so that closer obstacles result in greater rotation angles.

 Hardware List

In case some of you are interested in the hardware used in the robot featured in this tutorial, here is a list: