0
点赞
收藏
分享

微信扫一扫

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);
}

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

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

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);
}

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

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

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);
}

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

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

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

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<Sum-1;i++)
{
for(int j=i+1;j<Sum;j++)
{
if((Num[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);
}

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

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

5.中位值平均滤波

总结

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

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

举报

相关推荐

0 条评论