The Curse of the Nearsighted Robot

As you have seen from the videos we posted yesterday, there were issues getting the SONAR sensor on-line and working correctly.  The expected behavior would be basic wall-hugging always turning right (or clockwise).

(For those of you just wanting to see the Rover avoid obstacles without any of the engineering comedy, just skip to the bottom.)

The Basic Collision Avoidance Algorithm

Here is the program logic laid out in pseudo code:

• Loop Forever
  • Read Sensor – Is there an obstacle?
    • If yes – turn right
    • If no – Travel forward

Reading the SONAR Sensor

We were stashing the sensor data into a signed integer which should be an early clue for those who have done this kind of work before.

Unfortunately, the raw SONAR sensor data had a lot of noise and a typical sequence of reads would look like this:

1. SRF05Echo = 4979
2. SRF05Echo = 4950
3. SRF05Echo =    1
4. SRF05Echo =    4
5. SRF05Echo =    3
6. SRF05Echo =    1
7. SRF05Echo =    1
8. SRF05Echo =    1
9. SRF05Echo =    1
10. SRF05Echo = 5008

While our typical reads are in the 4-5 thousand range we see these outlying glitches popping up that could easily skew our results.  You can call the glitches “sensor noise”. One of the tricks to sensors is the tuning and we set up two kinds of filtering to help us clean up this noise.

Filter 1 – Toss out the Low Stuff

First, we set a lower threshold on the sensor readings to 300.  Anything lower than 300, we automatically toss out.  For the sensor, a value of 300 is equivalent to about 2 inches and with the sensor set a bit back on the chassis that gives us an ideal range for detecting obstacles.

150 sensor units = 1 inch

Filter 2 – Build a Smooth Average

Next, we wanted to smooth out the data in order to minimize the effect of noise.  By taking 4 readings and averaging the values, we can reduce the effect of any single bad sensor read.

Step 1 – Add 4 values together:

Sensor_data_1 + Sensor_data_2 + Sensor_data_3 + Sensor_data_4 = Sensor_Data

Step 2 – Divide the sum by 4 to get an average value:

Sensor_Data = Sensor_Data / 4

We must be Brilliant!

Here is another set of data with both of the filters applied:

1. SRF05Echo = 3518
2. SRF05Echo = 3518
3. SRF05Echo = 3518
4. SRF05Echo = 3518
5. SRF05Echo = 3518
6. SRF05Echo = 3518
7. SRF05Echo = 3518
8. SRF05Echo = 3568
9. SRF05Echo = 3568
10. SRF05Echo = 3568

As you can see, this is much more consistent data.

So with all this clever thinking where did we go wrong?  (Or to return to the Wile E. Coyote metaphor where did the falling anvil come from?)

Maybe not so Brilliant?

The averaging filter works great for low values.  When we did our bench tests it worked like a dream but as soon as we plopped the little guy onto the test track he starting spinning around randomly.

The SONAR has a maximum range of about 12 feet.  If one inch is equivalent to 150 from the SONAR sensor we can crank a little math to see what our long range sensor reads will look like:

12 ft. x (12 inches/1 ft.) x (150 sensor units/ 1 inch) = 21600 sensor units at 12 feet

If you have encountered this error before you are probably smirking at this point but for the benefit of everyone else let’s follow this logic to its head-on-anvil conclusion by sending the data into our averaging filter.

21600 + 21600 + 21600 + 21600 = 86400

86400 / 4 = 21600

Looks great doesn’t it?

Nope.

What we found after extensive testing is that when the sensor readings got too high, they became negative.  That is a classic sign that you have overrun your data variable which in our case was a signed integer.  A 16-bit signed integer goes from -32,768 up to 32,767.

As soon as we passed the integer’s upper limit (32,767) when we tried to add beyond it, the number stored in the integer rolled over into the negative range in the same way an old Chevy would roll the odometer back to zero when you hit 999,999 miles.

What this meant in plain English was that RoverLORD could see pretty well at short distances but would immediately panic if he couldn’t detect anything nearby.  We had created a nervous and nearsighted robot.

Nope, We’re Brilliant Again

We found the error while watching the raw sensor data and fixed RoverLORD’s vision by swapping the signed integer for a much bigger unsigned long variable.  With that change the SONAR works as planned.

You can see the successful SONAR tests working here with the same right-turning algorithm described above.

Videos of Collision Avoidance