Sunday, January 24, 2016

Remove impulse noise from ultrasonic sonar data


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.

Mr. Stitson - Front view with the sonar

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:
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)
    // this last one is obviously unnecessary for the median
    return c;


The actual code used in the robot can be found here:

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.


Friday, January 1, 2016

"Mr. Stitson" - A kid friendly Arduino - LEGO robot

"Dad, it is almost as super as a train"

Mr. Stitson - A kid friendly Arduino - LEGO robot

This is the second version of my robot car. The first version had some problems, most importantly the power distribution was ill-designed (more precisely, it was not designed at all). 
Mr. Stitson - A kid friendly Arduino - LEGO robot
Mr. Stitson undressed

What's new

With this second version I fixed most of the problems of the first version (and probably introduced some brand new ones), and made it more versatile adding new hardware elements:

  • The car got brand new chassis, home made from MDF wood. It is bigger, nicer and more kid friendly being also a LEGO platform. 
  • The power distribution is redesigned. The robot got a nice 3S LIPO battery, and an LM2596 based switching power regulator . The power rail is regulated to 9V, which is more than what the motors require (6V), but they endure it and run faster. 
  • The microcontroller is updated to an Arduino Mega from the Arduino Nano. More pins, more timers, more possibilities...
  • A GY80 based Inertial Measurement Unit (IMU) is added. It’s a compact module that includes a gyroscope, accelerometer, digital compass, and a barometric pressure / temperature sensor. The sensors are not used as now, but I plan to utilize some of them in the future (e.g. to let the robot going more straight).
  • Nice headlights are made from 4 power leds. I also had to develop a small driver circuit as the LEDS would draw a way too much power from the Arduino. The LEDS are installed in a nice 3D printed mount.
  • Wiring is slightly improved. I used heat shrinks and made custom length jumper wires. It already looks a way less messy and safer than the previous version, even if there is room for improvement yet.

All together, it looks and works much better than the previous version. The chassis looks kind of "crafted", especially compared to the old acrylic one, and the 9V power rail makes the robot more reliable and faster. The extra sensors makes it more like a "real" robot, and provides stable ground for future experiments. And last, but not least, the new headlights are hugely popular with the kids.

Mr. Stitson in action

LED driver

I built the LED driver on half of 2x8 cm perfboard using the following parts:

1 x RFP30N06LE N-Channel MOSFET
4 x 82R resistor (for 20mA LEDS, calculate here otherwise) 
1 x 10K resistor (pull-down)
some pin headers

LED driver schematic

List of parts and cost

The chassis is inexpensive, made from wood, I already posted about it. The robot also contains many 3D printed parts which I consider as zero cost. These parts are the following:
(The list excludes ubiquitous items like jumper wires, heat sinks, etc. It also excludes general equipment like LIPO battery charger)

Part Price
Arduino Mega ~ 6$
L298N x 2 < 2$ x 2
Motors + wheels ~ 5$
PS2 controller + receiver ~ 10$
Piezo buzzer (salvage it from an old toy) < 1$
10 mm LEDs ~ 2$ (10 pieces)
LM2596 XL4015 < 2$
HC-SR04 < 1$
GY80 IMU ~ 14$
2200ma 3S LIPO battery ~ 12$
Chassis < 20$

< 76$


The power distribution works perfectly 99% of the cases now, but the microcontroller still resets when the motors are reversed at maximum speed. The problem is with the voltage regulator as it limits the current to 2A (despite that by specification it should be able to deliver 3A temporarily). The robot draws ~1.6A at 9V at full speed, so there is no much room for the inrush current when the motors start. It is probably an easy fix by adding some decoupling capacitors, or replacing the voltage regulator with e.g. an XL4015 (or adding a second voltage regulator for the electronics creating this way another power rail).  I believe it is also possible to solve this issue from software. I eliminated the problem temporarily by regulating to 6V instead of 9V.

Future work (this robot)

The hardware works as expected, now I want to concentrate on the software (including a fix for the resetting problem) as it is the weakest point currently. First of all, the individual components (wheels, lights, sensors, etc...) should register themselves instead of being hard wired. I also want some filtering on the sensor inputs (I mean e.g. Kalman filters). It would be also nice to use a PID controller and exploiting the new sensors to make the robot going more straight (the cheap wheels and the hand made chassis renders the trajectory of the robot slightly imprecise). I also would like to make some experiments with OOSMOS to find out whether it is good enough to replace the simple green thread library used currently.

Future work (next robot)

Ad-hoc wiring harness
  • Power distribution; I want a power distribution board. Mostly to avoid ad-hoc wiring harnesses, but some fuse would be useful as well. I can also imagine the voltage regulator(s) integrated. It is really strange that there are no general purpose power distribution boards available for robotics. Especially considering how helpful they are. Actually, the multirotor community already realised that.
  • Wiring: I read somewhere that robotics is all about wiring and connectors. I could not agree more. Some of my vague ideas: 3D printed wire channels, IDC header socket connectors, custom arduino shields. I really want to get rid of these unreliable dupont jumper wires.  
  • DC motors: Better motors, more torque and also more speed. It is going to be easy regarding how crappy motors I have now. I also want encoders feeding the PID controller.
  • Wheels: Mecanum wheels...
  • Software: Instead of C/C++, I would like to use a dependently typed functional language for developing the software. I believe that robotics is exactly the field where extra type safety pays back. There are not so many dependently typed languages though. I tried Idirs, but, although I really like the language, the generated code is a way too inefficient. The other option is GHC/Haskell. It is not officially dependently typed, but its type system is powerful enough to emulate it, and the upcoming version will officially provides this feature (to the extent I need it). The problem is that Haskell is lazy, what makes it unpractical for embedded applications (in the upcoming version this is optional) and it is garbage collected what renders it unusable on microcontrollers. But, why should it run on a microcontroller anyway?
  • Computer/microcontroller: I have no problem with Arduino, but it is not powerful enough to run my Haskell code. So I need a real computer, like Raspberry Pi, but I also need real time support provided by a microcontroller. I could use a Raspberry with an Arduino in pair, but actually I think Raspberry is just not a good embedded system. Mostly, because it is not an embedded system. Raspberry Zero is closer, but still. Intel Edison seems a much better choice. Powerful enough with integrated microcontroller. Sounds just perfect. 


Featured Post

Juicy-footprint: An SMD Footprint designer DSL

  Introduction    Juicy-footprint is a domain specific language written in/for Java and Scala to design/reconstruct SMD footprint...

Popular Posts