Autonomous car – obstacle avoidance with ultrasonic sensor

As part of my autonomous car project (, I need the car to avoid obstacles.  This is a video of my autonomous car using ultrasonic sensor data to choose the nearest obstacle-free heading towards its destination.

Basically, an ultrasonic sensor mounted on a servo takes distance measurements at a number of angles and chooses the obstacle-free angle closest to that required to get to the next way point.

Future directions

  • Before I’m satisfied with the car’s obstacle avoidance capability, I need to do some more testing to make sure it is behaving as expected.
  • After the car is up and running, I will experiment with adding stereo cameras and fiddle with edge detection algorithms, stereo vision (when images from the two cameras are compared, the parts with lots of differences tend to be closer while the parts with more similarities tend to be farther), and optical flow (while moving, the things that move faster tend to be closer and the things that move slower tend to be farther)

For details,


Which proximity sensor will I use?
I chose to use an ultrasonic proximity sensor.  Specifically, I opted for a Parallax PING (  Here’s why I didn’t choose other sensor technologies:

  • Capacitive (range is only a few inches)
  • Inductive (range is only a few inches)
  • Infra-red (struggles in daylight)
  • Laser range-finder (expensive)

Why get a 2-D representation of the world?
If the car only had a sensor measure distance directly in front of the car, it would have to avoid obstacles by guess and check–it would drive until it sees an obstacle, back up, turn a little, and try again until it doesn’t see an obstacle.  This would take forever.  With a 2-D representation of the space in front of it, the car can choose from a variety of directions.


How get a 2-D representation of the world?
One option would be to buy a bunch of sensors and array them at different angles in front of the car, but it would be expensive to accomplish any respectable resolution with this approach.  A more attractive option would be to mount a sensor on a servo and collect readings while the sensor rotates.

Fortunately, there is a kit for mounting the ultrasonic sensor on a servo (,ProductName).  However, this kit was designed for Parallax’s Boe Bot, not my RC car.  So, I took off the front bumper of my RC car, machined a bracket for mounting the servo from an aluminium angle, and screwed the bracket into the holes where the bumper was.


What resolution?
One question to figure out is how many measurements I want the sensor to take in a single pass.  If I take too many measurements, the pass will be slow and the car could crash while waiting for the sensor to complete its pass.  If I take to few measurements, the car could fail to detect obstacles or available paths.  I chose 36 measurements per pass, because it seemed to strike the balance between these extremes.  I get a good resolution, but I am able to complete a pass in well under a second.


What distance threshold?
Another balance that needs to be struck is choosing the distance below which the car classifies something as an obstacle.  If the distance is too short, the car could be unable to stop before crashing into something.  If the distance is too far, the car could unnecessarily rule out viable paths.


How will I power the servo and sensor?
A wiring guide for the ultrasonic sensor suggests the sensor be powered by the Arduino (  The Arduino servo library also suggests that the servo be powered by the Arduino (  However, when I tried to power both devices using the Arduino, the sensor reported bad data and the servo misbehaved.

To fix the issue, I ran both of the devices off of the RC car batteries, stepping the voltage down using a voltage divider.  Thanks to DuaneB from the Arduino forum (,102369.msg767877.html#msg767877) for pointing out that inadequate power was the cause of the problem.


What obstacle-avoidance algorithm will I use?
The car will constantly cycle through these steps:

  • Sweep a proximity sensor over and back in front of the car by mounting it on a servo
  • At 36 angles along the way, the ultrasonic sensor measures distance
  • Filter out any angles where the distance is less than four feet
  • Of the remaining angles, choose the angle closest to way-point

One challenge I encountered while setting this up was that the car seemed to take forever at the end of each pass to decide where to go.  Two changes fixed this problem:  I stopped displaying all of the measurements on the Serial Monitor sped up the Serial transfer rate from 9600 baud to 11520 baud.  Thanks to dxw00d and Nick Gammon from the Arduino forum (,102369.msg768401.html#msg768401) for these fixes.


Wiring diagram



10 thoughts on “Autonomous car – obstacle avoidance with ultrasonic sensor

  1. Pingback: Autonomous car – summary | Jeff's Inventions

  2. Andrew

    This is great Jeff. I am new to programming and am trying to make a robot car that will locate an obstacle and move towards it then stop a designated distance away. I don’t understand what each part of tour code does so I cannot determine which parts of code I need to modify and what I should code in to locate and move to an object. Can you either help me understand those portions of code better or provide the rewritten sections? I am also looking at your Steering and Acceleration project to see how it differs from mine. I already have working code that will locate and move to an object but it is jerky and seems tempermental so I am trying to make it better.


  3. jeff Post author

    Hi Andrew,

    Because the code was hard to read here, I put it on github:

    In essence, the code has the following parts:
    1) Create an array of angles and distances by sweeping the servo left and right and having the ultrasonic sensor take measurements along the way (lines 155-180). The angles are measured in pulsewidths (microseconds) sent to the servo (800 is the farthest left; 1700 is the farthest right). The distances are in inches. When it’s done, it will have (without the units):

    (800us, 48″),(810us, 24″)…(1700,50″)

    2) Look through the array of angles and distances and, whenever the distance is less than the obstacle distance (minimumDistance), set the distance to 0 (lines 64-95). In the example, the array becomes:

    (800us,48″),(810us, 0″)…(1700us ,50″)

    3) Figure out how far away each angle is from the angle the car is supposed to be going (lines 96-119). Say the car is supposed to be going the angle that correspondence to a pulsewidth of 900 us, the array becomes:

    (800us,48″,100us),(810us, 0″,90us)…(1700us ,50″,800us)

    4) Look through the array to find the angle without an obstacle (distance != 0″), that is closest to the direction the car is supposed to be going (lines 120-137). In the example, it would return:


    5) If there were no angles without an obstacle, return 0 (lines 138-147).

    If you want it to go towards an obstacle, you would change step 2. Instead of setting the distance to 0 when there is an obstacle, you would set the distance to 0 when there isn’t an obstacle. Put another way, line 67 changes from this:

    if (directionsAndDistances[i][1] >= minimumDistance)

    to this:

    if (directionsAndDistances[i][1] < = minimumDistance) If the issue your are having with jerky motion involves the robot oscillating back and forth, you might consider PID control. Brett Beauregard (the author of the PID Arduino library) has done a nice write up on it: If it isn’t oscillating, you can post your code and I can take a look at it.

  4. shadi

    please help me>>>
    i make a sample of driverless car with 2 dc motors
    and i use 4 ping sensor
    and i make a control on the motion of the car using sensors only!!!
    now the car can avoid any obstacle…
    but i need the car move straight and if it face any obstacle…it avoid it and back to the straight line
    i use HMC883L compass…and i get the compass reading…
    now how i can use the compass reading for this??
    thank you and sorry for my bad english…
    the code until now:

    NewSoftSerial xbee(17, 16); // RX, TX
    NewSoftSerial GPS(19,18);
    HMC5883L compass;
    TinyGPS gps;
    void gpsdump(TinyGPS &gps);
    bool feedgps();
    void getGPS();
    long lat, lon,year,month,day,hour,second,minute;
    float LAT, LON;
    int error = 0;
    const int pingPin1 = 3;
    const int pingPin2 = 4;
    const int pingPin3 = 5;
    const int pingPin4 = 6;
    int ledPin = 13;
    int M1 = 7;
    int M2 = 8;
    int E1 = 9;
    int E2 = 10;
    int E3 = 11;
    int E4 = 12;
    long cm1,cm2,cm3,cm4;
    void setup()
    Serial.println(“Starting the I2C interface.”);
    Serial.println( “Arduino started sending bytes via XBee” );
    // set the data rate for the SoftwareSerial port
    xbee.begin( 9600 );
    pinMode(M1, OUTPUT);
    pinMode(M2, OUTPUT);
    pinMode(ledPin, OUTPUT); // Set the LED pin as output

    Serial.println(“Constructing new HMC5883L”);
    compass = HMC5883L(); // Construct a new HMC5883 compass.

    Serial.println(“Setting scale to +/- 1.3 Ga”);
    error = compass.SetScale(1.3); // Set the scale of the compass.
    if(error != 0) // If there is an error, print it out.

    Serial.println(“Setting measurement mode to continous.”);
    error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
    if(error != 0) // If there is an error, print it out.

    void loop()


    MagnetometerRaw raw = compass.ReadRawAxis();
    MagnetometerScaled scaled = compass.ReadScaledAxis();

    int MilliGauss_OnThe_XAxis = scaled.XAxis;// (or YAxis, or ZAxis)

    float heading = atan2(scaled.YAxis, scaled.XAxis);

    float declinationAngle = 0.0626;
    heading += declinationAngle;

    if(heading 2*PI)
    heading -= 2*PI;

    float headingDegrees = heading * 180/M_PI;

    Output( heading, headingDegrees);
    // send character via XBee to other XBee connected to Mac
    // via USB cable
    xbee.print(” \tHeading:\t”);
    xbee.print(” Radians \t”);
    xbee.println(” Degrees \t”);
    long lat, lon;
    unsigned long fix_age, time, date, speed, course;
    unsigned long chars;
    unsigned short sentences, failed_checksum;
    // retrieves +/- lat/long in 100000ths of a degree
    gps.get_position(&lat, &lon, &fix_age);
    // time in hh:mm:ss, date in dd/mm/yy
    gps.get_datetime(&date, &time, &fix_age);
    year = date % 100;
    month = (date / 100) % 100;
    day = date / 10000;
    hour = time / 1000000;
    minute = (time / 10000) % 100;
    second = (time / 100) % 100;
    Serial.print(“Date: “);
    Serial.print(year); Serial.print(“/”);
    Serial.print(month); Serial.print(“/”);
    Serial.print(” :: Time: “);
    Serial.print(hour); Serial.print(“:”);
    Serial.print(minute); Serial.print(“:”);

    Serial.print(“Latitude : “);
    Serial.print(” :: Longitude : “);
    xbee.print(“Latitude : “);
    xbee.print(” :: Longitude : “);

    void getGPS(){
    bool newdata = false;
    unsigned long start = millis();
    // Every 1 seconds we print an update
    while (millis() – start 150&&cm2>150)
    digitalWrite(M2, LOW);
    analogWrite(E1, 175); //PWM Speed Control
    analogWrite(E2, 0); //PWM Speed Control
    digitalWrite(M2, HIGH);
    analogWrite(E1, 150); //PWM Speed Control
    analogWrite(E2, 0); //PWM Speed Control
    analogWrite(E3, 0); //PWM Speed Control
    analogWrite(E4, 255); //PWM Speed Control
    analogWrite(E1, 150); //PWM Speed Control
    analogWrite(E2, 0); //PWM Speed Control
    analogWrite(E3, 255); //PWM Speed Control
    analogWrite(E4, 0); //PWM Speed Control
    analogWrite(E1, 0); //PWM Speed Control
    analogWrite(E2, 255); //PWM Speed Control
    analogWrite(E3, 0); //PWM Speed Control
    analogWrite(E4, 0); //PWM Speed Control

    void Output( float heading, float headingDegrees)

    Serial.print(” \tHeading:\t”);
    Serial.print(” Radians \t”);
    Serial.println(” Degrees \t”);


  5. Jeff

    Hi Shadi,

    You might use this pattern (this is pseudo-code):
    if obstacle is detected, then avoid it (using the code you already have)

    else, use this PID control library ( such that the setpoint is the direction you define as straight, the input is the direction you read in from the compass, and the output is the steering angle.

    PID is a control scheme that aims to minimize the error between a desired setpoint (e.g., the temperature you want in a room) and a measured value (e.g., the current temperature in the room), by controlling the degree to which you modify the system (e.g., by running a chiller at 50%).

    Here is a good example of how to use the PID library:

  6. norman

    Nice work you have done. but i also want to build obstacle avoidance using ultrasonic sensor and PIC. which is the best algorithm i can use. For now i am not considering reaching a goal. Only obstacle avoidance. Please help me.

    1. Jeff

      Hi Norman,

      A naive approach is:
      -mount an ultrasonic sensor on a servo such that, for each angle, you know how far away the nearest object is
      -choose a range of angles that would be in the robot’s path if it continued driving straight (e.g., if the sensor sweeps 180 degrees, anything is between 60 degrees and 120 degrees)
      -identify a distance threshold for an obstacle (e.g., if something is less than 1 feet away, start avoiding)
      -if an obstacle is detected (defined as an object between 60 to 120 degrees and less than 1 feet away), then the robot backs up one foot, steers a couple of degrees and then drives forward.

      If you want to go deeper into the topic, you might check out Sebastian Thrun’s (who led the team that won the first Autonomous car DARPA Grand Challenge) course about how to develop an autonomous car, in particular the localization lectures:

  7. arif

    I m trying to make a gps guided ultrasonic obstacle avoidance vehicle help me out .I m new to this field.I m using 2 hcsr04 sensors one at front and one at back both fixed(not on servo) and a GPS reciver and a magnetometer that is 5880 with one DC motor with a motor shield and one servo for steering all grouped to arduino mega 2560…plzz help me with program

  8. Moudjib

    Hi Jeff
    Nice work congrats
    i would to applicate your code in my robot , but it work with two wheeled motors and L293D motor drive
    where i should change in this code to put motor drive functions ( example go_forword () , turn_left() ……)
    thank you


Leave a Reply

Your email address will not be published. Required fields are marked *

You may use these HTML tags and attributes: <a href="" title=""> <abbr title=""> <acronym title=""> <b> <blockquote cite=""> <cite> <code> <del datetime=""> <em> <i> <q cite=""> <strike> <strong>