Introduction
Mr. Stitson has an ultrasonic sonar mounted in the front. namely a HC-SR04; it is inexpensive, and ubiquitous, but unfortunately not quite reliable. I have a single reason for having a sonar installed, I want to keep away my boys playing the “bumping” game, what is, according to some scientists, has an unhealthy effect on robots.
To avoid hitting the wall, in the main loop there is some logic that, based on the current speed and the latest sonar readings, slows down the vehicle.
Noisy sensor data
Not rocket science, but it works reasonably well. until the sonar returns valid data. Unfortunately, this is not the case. Standing still, these are common sonar readings:
30
30
30
31
5
29
31
31
5
33
4
30
31
31
31
31
31
30
30
30
30
I do not care about the slight fluctuation around 30 cm, but the 4 and 5 cm readings (it is called impulse noise I believe) are showstoppers, they immediately let the robot stop. Most people would cry Kalman filter here, but that would be like using a sledgehammer to crack a nut; a simple median filter will do the job perfectly.
Implementing the median filter
Such erroneous readings appear quite often, but I did not observe them more than twice in a row. A median filter takes the median value from the last N readings (a.k.a the window size), N = 5 is enough to filter out two extremely bad readings (like we have here) in a row.
All we have to do is to sort the last five readings and pick up the third one. First, store the last readings in a cyclic buffer:
// The cyclic buffer
int lastReadings[5];
// MAX_DISTANCE will be the the result until the filter warms up
for(int i=0; i<5; i++) lastReadings[i] = MAX_DISTANCE;
// Number of readings since the beginning
int nrReadings = 0;
// Add an element to the cyclic buffer
lastReadings[nrReadings++ % 5] = sonar->read();
Next, we need a function to calculate the median value based on this buffer (do not sort the buffer itself, then you loose cyclicity). That can be done very nicely and efficiently as there exist optimal sorting network for N = 5 which uses only nine comparisons (and one can be trivially ignored as we need the third element only). There is also an algorithm for computing the median value of five elements with six comparisons, but that is anything, but nice and I want to avoid premature optimization anyway.
#define swap(a,b) a ^= b; b ^= a; a ^= b;
#define sort(a,b) if(a>b){ swap(a,b); }
int median(int a, int b, int c, int d, int e)
{
sort(a,b);
sort(d,e);
sort(a,c);
sort(b,c);
sort(a,d);
sort(c,d);
sort(b,e);
sort(b,c);
// this last one is obviously unnecessary for the median
//sort(d,e);
return c;
}
Remarks
The actual code used in the robot can be found here: https://github.com/domoszlai/robotcar/blob/master/sonarnp.cpp
Some remarks about the code. It is based on the NewPing library. The class continuously reads the sonar (every 33 milliseconds only to avoid interference with the previous measurement) in an asynchronous manner. The measure() method always returns immediately with the latest (filtered) measurement. Although the results come asynchronously using a timer based mechanism, the measurements must be initialized from the main “thread” in the loop() method. This loop() method is part of the green thread library I use (notice the Thread base class). Read the comments in the code.
Finally, some remarks to median filter.
Median filters introduce a delay proportional to N. For N = 5, the filter is on average two values behind the actual measurements, that is ~60 milliseconds. I have to be only faster than my kids, so it is acceptable for me. If you think you have real-time requirements, then buy a proper sonar and use Kalman filter. If you want to smooth the sensor data fluctuations, use a Kalman filter. Median filters are very efficient, much faster than Kalman filters; if you do not have any of the previous requirements, stick with the median one.
This article has some nice figures related to the topic.
Conclusion and future work
I’m quite satisfied with the current implementation, it solves my main problem in a very efficient way with minimal effort. It may make sense, though, to smooth the sensor data as well. Furthermore, I definitely want to implement prediction of future data. These can be done by Kalman filters. However, although, Kalman filter is an incredible tool for sensor fusion, when one can provide a proper physical model, I feel it too much otherwise. I would rather try something new, like alpha-trimmed mean filters. Or just a simple linear prediction on top of the median filtered data.
As for the sensor itself, I think ultrasonic sensors are not the best tool to avoid obstacles, they just do not provide enough information for a proper plan. I will rather try using two cameras for creating stereoscopic 3D images. Of course, than I need a proper processor, but this is my plan anyway. Lidars seem also very great, but they are a way too expensive just for having fun.
Do you have similar experience with HC-SR04?