Algorithm for motion detection using ultrasonic sensor on Arduino

Question

I am using a ultrasonic sensor to detect motion by looking at the variance of the sensor readings, however my algorithm is very prone to give false positives thus erroneously rising a motion event.

Algorithm Background

What I do is send a ping 20 times a second and wait for a pulse to come back using the input capture on arduino. I have a limit of 2 metres because I don't need to detect motion beyond that. Using a modifed version of the NewPing library which has a method check_timer which returns a value greater than 1 if the pulse from the arduino took more than a corresponding wait time for 2 metres and returns 1 if no pulse has been measured yet or 0 if it successfully got a pulse.

The following conditions need to be met to rise a motion event:

• If the last measured pulse was a valid pulse (i.e check_timer returns 0) it will save it in an array and wait for another positive pulse and only then test for the variance of the two pulses. However if it failed to measure a valid pulse after the first success it will reset the array.

• If a variance greater than or equal to 500 is measured within a time frame it will rise a motion event.

The code for my algorithm is this:

#include <NewPing.h>

#define TRIGGER_PIN   10 // Arduino pin tied to trigger pin on ping sensor.
#define ECHO_PIN      9 // Arduino pin tied to echo pin on ping sensor.
#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.

unsigned int pingSpeed = 50; // How frequently are we going to send out a ping (in milliseconds). 50ms would be 20 times a second.
unsigned long pingTimer;     // Holds the next ping time.

unsigned long last = 0;
unsigned int samples = 2;
unsigned int distances[2];
unsigned int sum = 0;

void setup() {
  Serial.begin(9600); // Open serial monitor at 9600 baud to see ping results.
  pingTimer = millis(); // Start now.
}

void loop() {
  // Notice how there's no delays in this sketch to allow you to do other processing in-line while doing distance pings.
  if (millis() >= pingTimer) {   // pingSpeed milliseconds since last ping, do another ping.
    pingTimer += pingSpeed;      // Set the next ping time.
    sonar.ping_timer(echoCheck); // Send out the ping, calls "echoCheck" function every 24uS where you can check the ping status.
  }
  // Do other stuff here, really. Think of it as multi-tasking.
}

void echoCheck() { // Timer2 interrupt calls this function every 24uS where you can check the ping status.
  uint8_t rc = sonar.check_timer();
  if(rc==0) { // This is how you check to see if the ping was received.
    sum = sum + sonar.ping_result;
    distances[--samples] = sonar.ping_result; 
    if(samples == 0) {
      float avrg = (float)(sum) / 2.0f; // Get the average of the samples we recorded
      float sum1 = 0;
      for(int i=1;i>-1;--i) {
        sum1 = sum1 + pow((distances[i] - avrg),2);
      }
      if((sum1 / 2.0f) >= 500.0f) { // If the variance of the samples is greater than or equal to 500
        unsigned long now = millis();
        if(now-last <= 250) { // To filter out random false positives we want two positves within this time frame
          Serial.print("motion event, variance is ");
          Serial.println(sum1 / 2.0f);    
        }
        last = now;       
      }
      sum = 0;
      samples = 2;
    }
  }else if(rc>1) { // Was it a faulty reading (e.g it exceeded the maximum waiting time)?
    sum = 0;
    samples = 2; 
  }
}

My code works but is very unreliable. How can I improve this algorithm?

(EDIT: FYI the sensor I am refering to is a cheap HC - SR04 Ultrasonic Ranging Module.)

Answer 1

Late or spurious echoes

One thing that can happen is that during one ping interval of 50ms you can get echoes from objects up to 8.5m away (0.05[s] *354[m/s] / 2 ). If the object is a little further than that, say 9m, you will get the echo early during the next ping period instead.

As you can guess, this would be bad, it would wrongly appear as being a close echo while it in fact is distant. I'm not familiar with the strength of the ultrasonic on your chip; 8.5m may be out of range in which case this isn't the problem. But if it isn't then your library should send each ping request with a different frequency and check the frequency of the echo to know if you got the right echo, it should also check the amplitude of the echo for feasibility. Again I don't know if your library does any of this but you should check. Otherwise this might be causing your troubles. If 8.5m is just on the edge of the range it is conceivable that some times you get the echo and some times you don't.

Filtering

If you can rule out the above and you're certain your implementation is not at fault, then you can simply assume that it's a transient at work. They happen when dealing with sensors, and sound is finicky that way with echoes and everything.

Best way to deal with transients is to use some kind of filtering. Median might work well for you and I see that NewPing has a "ping_median" function, try this with a filter size of maybe 5 or thereabouts see if that helps.

Variance

Using two samples to estimate any kind of variance is way too little, the result is not in any way reliable. You are also very sensitive to transients and errors are you are calculating the square of the values. I would suggest another approach.

Another approach

If you want to detect movement you want to detect changes in the echo delay. So what you do is setup your timer to consistently ping at a fixed rate then keep a low pass filtered running average of the echo delay and look at the differential of this (the rate of change).

Some pseudocode:

float g = 0.9f
float avg_time = max_time
float avg_dt = 0.0f
float dt_hysteresis = 0.01f
for each 50ms
   if no echo for 10 iterations
      avg_time = max_time
      continue

   float echo_time = sensor.read()

   if avg_time == max_time
       avg_time = echo_time
       avg_dt = 0.0f
   else
       float prev_avg_time = avg_time;
       avg_time = avg_time * g + (1.0f - g) * echo_time
       avg_dt = avg_dt * g + (1.0f - g) * (avg_echo_time - prev_avg_time);

   if avg_dt < -dt_hysteresis 
       // Some one approaching
   else if avg_dt > +dt_hysteresis
       // Some one leaving
end for

Explanation g is a filter coefficient between 0 and 1.0, the higher it is the more "inert" your filter will be. It will be slower to react but be highly resistant to noise. If g is low the filter will react quickly but also more susceptible to noise, to put it simply. You can think of it as a low-pass filter where g implicitly controls the cut-off frequency. Normal values are in the range 0.7 to 0.95.

Every 50 ms you send out a ping and look for the echo. If you don't get an echo for 10 iterations or so, simply reset the avg_time variable so that you can start fresh the next time you get an echo and not be affected by what happened to be stored there since the last echo.

If you get an echo you low pass filter it with avg_time = avg_time * g + (1.0f - g) * echo_time and then look at the lowpass filtered change in the lowpass filtered time value avg_dt = avg_dt * g + (1.0 - g) * (avg_echo_time - prev_avg_time). If this change is larger than +- the hysteresis threshold dt_hysteresis you then deduce that some one is approaching or moving away from the sensor. You can also consider looking directly at avg_echo_time - prev_avg_time the extra lowpass filtering is simply to get rid of noise which is always present when we differentiate a real signal.

Hope this was of some help :s

Answer 2

I can't see anything wrong with the code, its a fairly standard way of doing it, but I have some queries about your design:

• Are you sure they are false positives and not echos and reflections?
• do you need to call the echoCheck function every 24uS? that's around 8mm resolution.
• Does echoCheck() execute within 24uS? (If it doesn't finish executing what happens to the next call does it get dropped or queued?)
• Is the ping send duration <24uS?
• What happens when you slow the ping rate down say 10/sec does it improve?