Arduino读取HC-SR04超声波测距传感器数据附带滤波

目录

前言

程序

1.无滤波程序

2.低通滤波器滤波程序

3.低通滤波器滤波程序（弃掉高幅噪声）

4.中位值滤波

5.中位值平均滤波

总结

---

前言

传感器的接线非常简单，一共四根线

vcc和gnd按照要求接线即可。TRIG接数字引脚8号口，ECHO接数字引脚9号口。

程序

1.无滤波程序

#define TRIGGER 8
#define ECHO 9
long duration;
float distance;

#define LONGEST_DISTANCE 200 // 200 cm = 2 meters
float farTime =  LONGEST_DISTANCE*2/0.034;

void setup() {
  pinMode(TRIGGER, OUTPUT); // Sets the trigPin as an Output
  pinMode(ECHO, INPUT); // Sets the echoPin as an Input
  Serial.begin(57600); // Starts the serial communication
}

long counter=0;
void loop() {
  // Clears the trigPin
  digitalWrite(TRIGGER, LOW);
  delayMicroseconds(2);

  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(TRIGGER, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(ECHO, HIGH, farTime);

  //计算距离（厘米）
  distance = duration * 0.034 / 2; // 10^-6 * 34000 cm/s
  
  Serial.println(distance);
  delay(10);
}

可以看到，当前方障碍物距离变化时，会产生尖峰，这是噪声，我们需要去掉。

2.低通滤波器滤波程序

我们通过低通滤波器进行滤波，再看看效果。

#define TRIGGER 8
#define ECHO 9
long duration;
float distance;

#define LONGEST_DISTANCE 200 // 200 cm = 2 meters
float farTime =  LONGEST_DISTANCE*2/0.034;

class LowPassFilte{
  public:
    LowPassFilte(float Tf);//低通滤波器时间常量
    ~LowPassFilte() = default;
    float operator() (float x);
    float Tf; //!< 低通滤波器时间常量
  protected:
    unsigned long timestamp_prev;  //!< 上次执行时间戳
    float y_prev; //!< 经过上次执行后过滤到的值
};
 
LowPassFilte::LowPassFilte(float time_constant)
    : Tf(time_constant)
    , y_prev(0.0f)
{
    timestamp_prev = micros();
}
 
float LowPassFilte::operator() (float x)
{
    unsigned long timestamp = micros();
    float dt = (timestamp - timestamp_prev)*1e-6f;
 
    if (dt < 0.0f || dt > 0.5f)
        dt = 1e-3f;
 
    float alpha = Tf/(Tf + dt);
    float y = alpha*y_prev + (1.0f - alpha)*x;
 
    y_prev = y;
    timestamp_prev = timestamp;
    return y;
}
 
LowPassFilte us_filter(0.1);

void setup() {
  pinMode(TRIGGER, OUTPUT); // Sets the trigPin as an Output
  pinMode(ECHO, INPUT); // Sets the echoPin as an Input
  Serial.begin(57600); // Starts the serial communication
}

long counter=0;
void loop() {
  // Clears the trigPin
  digitalWrite(TRIGGER, LOW);
  delayMicroseconds(2);

  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(TRIGGER, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(ECHO, HIGH, farTime);

  //计算距离（厘米）
  distance = duration * 0.034 / 2; // 10^-6 * 34000 cm/s
  //摒弃突变值
  Serial.print(distance);
  Serial.print(",");
  Serial.println(us_filter(distance));
  delay(10);
}

可以看到，红色的波形是经过滤波之后的结果，已经变得平滑许多了，但是由于蓝色波形（原始数据）有时会有较大突变，还是会影响我们获取到的结果，我们下一步，将这个突变较大的突变值给弃掉。

3.低通滤波器滤波程序（弃掉高幅噪声）

由于正常情况下，自然界或者传感器的噪声是高频低幅的，因此高幅噪声主要是传感器内部造成的，这些垃圾数据，我们将其摒弃。

#define TRIGGER 8
#define ECHO 9
long duration;
float distance_last;
float distance;
 
#define LONGEST_DISTANCE 200 // 200 cm = 2 meters
float farTime =  LONGEST_DISTANCE*2/0.034;
 
class LowPassFilte{
  public:
    LowPassFilte(float Tf);//低通滤波器时间常量
    ~LowPassFilte() = default;
    float operator() (float x);
    float Tf; //!< 低通滤波器时间常量
  protected:
    unsigned long timestamp_prev;  //!< 上次执行时间戳
    float y_prev; //!< 经过上次执行后过滤到的值
};
 
LowPassFilte::LowPassFilte(float time_constant)
    : Tf(time_constant)
    , y_prev(0.0f)
{
    timestamp_prev = micros();
}
 
float LowPassFilte::operator() (float x)
{
    unsigned long timestamp = micros();
    float dt = (timestamp - timestamp_prev)*1e-6f;
 
    if (dt < 0.0f || dt > 0.5f)
        dt = 1e-3f;
 
    float alpha = Tf/(Tf + dt);
    float y = alpha*y_prev + (1.0f - alpha)*x;
 
    y_prev = y;
    timestamp_prev = timestamp;
    return y;
}
 
LowPassFilte us_filter(0.1);
LowPassFilte us_filter1(0.001);

long time_last=0;

void setup() {
  pinMode(TRIGGER, OUTPUT); // Sets the trigPin as an Output
  pinMode(ECHO, INPUT); // Sets the echoPin as an Input
  Serial.begin(57600); // Starts the serial communication
 
  digitalWrite(TRIGGER, LOW);
  delayMicroseconds(2);
 
  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(TRIGGER, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(ECHO, HIGH, farTime);
 
  //计算距离（厘米）
  distance_last = distance = duration * 0.034 / 2; // 10^-6 * 34000 cm/s
  time_last=millis();
}
long counter=0;
byte lvbo=0;
void loop() {
  // Clears the trigPin
  digitalWrite(TRIGGER, LOW);
  delayMicroseconds(2);
 
  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(TRIGGER, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(ECHO, HIGH, farTime);
 
  //计算距离（厘米）
  distance = duration * 0.034 / 2; // 10^-6 * 34000 cm/s
  Serial.print(distance);
  //摒弃高幅数据
  if((abs(distance-distance_last))>10)
  {
    if(lvbo<10)
    {
      distance = distance_last;
      lvbo++;
    }
    else
      lvbo=0;
  }
  
  Serial.print(",");
  Serial.println(us_filter(distance));
  distance_last = distance;
  delay(10);
}

可以看到滤波效果非常好， 首先，我们去除了高频低幅噪声，同时也去掉了高频高幅噪声，只剩下了我们的目标值（低频低幅数据），因为正常情况下，障碍物距离我们传感器的位置是均匀低俗变化的。

红色为滤波之后的效果，蓝色为原始数据。

4.中位值滤波

 连续采样N次（N取奇数），把N次采样值按大小排列，取中间值为本次有效值。我这里采样11次。

#define TRIGGER 8
#define ECHO 9
long duration;
float distance;
 
#define LONGEST_DISTANCE 200 // 200 cm = 2 meters
float farTime =  LONGEST_DISTANCE*2/0.034;

//中值滤波器
class MidFilter{
  public:
    int Sum;
    MidFilter(int Sum);//数据总数
    float operator() (float Num[11]);
};

MidFilter::MidFilter(int Sum){
  this->Sum=Sum;
}

float MidFilter::operator() (float Num[11]){
  
  for(int i=0;i(Num[j]))
      {
        float t=Num[i];
        Num[i]=Num[j];
        Num[j]=t;
      }
    }
  }
  return Num[(int(Sum/2))];
}

//实例化中值滤波器
MidFilter mid5(11);

void setup() {
  pinMode(TRIGGER, OUTPUT); // Sets the trigPin as an Output
  pinMode(ECHO, INPUT); // Sets the echoPin as an Input
  Serial.begin(57600); // Starts the serial communication
}


long counter=0;
void loop() {
  float distance_s[11]={0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
  // Clears the trigPin
  for(int i=0;i<=10;i++)
  {
    digitalWrite(TRIGGER, LOW);
    delayMicroseconds(2);
   
    // Sets the trigPin on HIGH state for 10 micro seconds
    digitalWrite(TRIGGER, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIGGER, LOW);
    // Reads the echoPin, returns the sound wave travel time in microseconds
    duration = pulseIn(ECHO, HIGH, farTime);
   
    //计算距离（厘米）
    distance_s[i] = duration * 0.034 / 2; // 10^-6 * 34000 cm/s
    
//    Serial.println(distance_s[i]);
  }
//  Serial.println();
//  Serial.print(distance_s[0]);
//  Serial.print(",");
  Serial.println(mid5(distance_s));
  delay(10);
}

滤波效果发现不是很好，还是会出现尖峰和低谷这种情况。不知是否我的代码有问题？ 

5.中位值平均滤波

总结

我们通过上述第三个程序，可以获得比较理想的滤波后的传感器数据。

但是，从上图中可以看到，我们观察滤波之后的红色波形，相比蓝色波形有一定的滞后性，这个我们如何解决呢？