HC-SR04の測定値を安定化｜外れ値除去・移動平均・中央値フィルタを比較【超音波センサー#3】

Contents

概要
プログラムコード
1. マイコン（ESP32）
2. Python

概要

今回は、超音波距離センサー「HC-SR04」の測定値を安定させるために、Pythonでフィルタ処理を試します。

外れ値除去、移動平均、中央値フィルタを使って、距離データをどのように安定化できるかを比較します。

フィルタ処理は、グラフを見やすくするだけでなく、次回以降の速度計算や状態判定を安定させるためにも重要です。

ちょっとしたデジタル化のトライアルをお考えの方に、少しでも参考になればうれしいです。

詳しくは、以下のYouTube動画をご覧ください。

HC-SR04の測定値を安定化｜外れ値除去・移動平均・中央値フィルタを比較【超音波センサー#3】

プログラムコード

マイコン（ESP32）

超音波を発射
対象物で反射
戻ってくるまでの時間を測定
音速から距離を計算

#define TRIG_PIN 13
#define ECHO_PIN 14

// 最大測定距離[cm]
#define MAX_DISTANCE 700

// タイムアウト時間[μs]
float timeOut = MAX_DISTANCE * 60;

// 音速[m/s]
const int SOUND_VELOCITY = 340;

void setup()
{
    pinMode(TRIG_PIN, OUTPUT);
    pinMode(ECHO_PIN, INPUT);

    Serial.begin(115200);
}

void loop()
{
    float distance = getDistance();

    Serial.print("Distance: ");
    Serial.print(distance);
    Serial.println(" cm");

    delay(100);
}


/*************************************************
 * 距離測定関数
 *************************************************/
float getDistance()
{
    unsigned long pingTime;

    // 超音波パルス送信
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);

    // 反射波の時間測定
    pingTime = pulseIn(
        ECHO_PIN,
        HIGH,
        timeOut
    );

    // 距離計算
    float distance =
        (float)pingTime *
        SOUND_VELOCITY /
        2 /
        10000;

    return distance;
}

Python

ESP32から距離データを受信
距離値を抽出
外れ値を除去
移動平均と中央値を計算
生データ・外れ値除去後・移動平均・中央値をリアルタイム表示
測定結果をCSVに保存

import serial
import time
import re
import csv
from collections import deque

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation


# =========================================================
# ▼ ユーザー設定：ここだけ変更すればOK
# =========================================================

# ESP32のCOMポート
# Arduino IDEの「ツール」→「ポート」で確認してください
PORT = "COM5"

# ESP32側の Serial.begin() と同じ値にします
BAUDRATE = 115200

# CSV保存ファイル名
EXPORT_CSV_FILE = "ultrasonic_filter_realtime_export.csv"

# グラフに表示する最新データ数
DISPLAY_POINTS = 100

# 外れ値除去のしきい値 [cm]
# 前回値との差がこの値を超えたら、外れ値として前回値を使います
OUTLIER_THRESHOLD = 50.0

# 移動平均・中央値フィルタの窓幅
# 例：5なら直近5点を使って計算します
WINDOW_SIZE = 5

# グラフ更新周期 [ms]
# 100ms = 0.1秒ごとに画面を更新
UPDATE_INTERVAL_MS = 100


# =========================================================
# シリアル接続
# =========================================================

ser = serial.Serial(
    PORT,
    BAUDRATE,
    timeout=1
)

time.sleep(2)


# =========================================================
# CSV保存準備
# =========================================================

csv_file = open(
    EXPORT_CSV_FILE,
    mode="w",
    newline="",
    encoding="utf-8-sig"
)

csv_writer = csv.writer(csv_file)

csv_writer.writerow([
    "elapsed_time_s",
    "raw_cm",
    "outlier_removed_cm",
    "moving_average_cm",
    "median_cm",
    "raw_text"
])


# =========================================================
# データ保存用
# =========================================================

time_plot = deque(maxlen=DISPLAY_POINTS)

raw_plot = deque(maxlen=DISPLAY_POINTS)
outlier_plot = deque(maxlen=DISPLAY_POINTS)
avg_plot = deque(maxlen=DISPLAY_POINTS)
median_plot = deque(maxlen=DISPLAY_POINTS)

avg_buffer = deque(maxlen=WINDOW_SIZE)
median_buffer = deque(maxlen=WINDOW_SIZE)

last_valid = None
start_time = time.time()


# =========================================================
# グラフ準備
# =========================================================

fig, ax = plt.subplots(figsize=(12, 6))

line_raw, = ax.plot(
    [],
    [],
    color="C0",
    linewidth=2,
    label="Raw",
    zorder=10
)

line_outlier, = ax.plot(
    [],
    [],
    color="C1",
    linewidth=2,
    label="Outlier Removal",
    zorder=5
)

line_avg, = ax.plot(
    [],
    [],
    color="C2",
    linewidth=2,
    label="Moving Average",
    zorder=4
)

line_median, = ax.plot(
    [],
    [],
    color="C3",
    linewidth=2,
    label="Median",
    zorder=3
)

ax.set_title("Real-time Ultrasonic Sensor Filter Comparison")
ax.set_xlabel("Time [s]")
ax.set_ylabel("Distance [cm]")
ax.grid(True)
ax.legend()


# =========================================================
# グラフ更新処理
# =========================================================

def update(frame):
    global last_valid

    while ser.in_waiting > 0:

        line_text = (
            ser.readline()
            .decode("utf-8", errors="ignore")
            .strip()
        )

        # 例: Distance: 52.34cm
        match = re.search(
            r"Distance:\s*(\d+\.?\d*)cm",
            line_text
        )

        if not match:
            continue

        raw = float(match.group(1))
        elapsed_time = time.time() - start_time

        # ---------------------------
        # 外れ値除去
        # ---------------------------

        if last_valid is None:
            outlier_removed = raw
        else:
            if abs(raw - last_valid) > OUTLIER_THRESHOLD:
                outlier_removed = last_valid
            else:
                outlier_removed = raw

        last_valid = outlier_removed

        # ---------------------------
        # 移動平均
        # ---------------------------

        avg_buffer.append(outlier_removed)
        moving_avg = float(np.mean(avg_buffer))

        # ---------------------------
        # 中央値フィルタ
        # ---------------------------

        median_buffer.append(outlier_removed)
        median_val = float(np.median(median_buffer))

        # ---------------------------
        # グラフ表示用データ
        # ---------------------------

        time_plot.append(elapsed_time)

        raw_plot.append(raw)
        outlier_plot.append(outlier_removed)
        avg_plot.append(moving_avg)
        median_plot.append(median_val)

        # ---------------------------
        # CSV保存
        # ---------------------------

        csv_writer.writerow([
            f"{elapsed_time:.3f}",
            f"{raw:.2f}",
            f"{outlier_removed:.2f}",
            f"{moving_avg:.2f}",
            f"{median_val:.2f}",
            line_text
        ])

        csv_file.flush()

        print(
            f"{elapsed_time:.2f}s | "
            f"raw={raw:.2f} | "
            f"outlier={outlier_removed:.2f} | "
            f"avg={moving_avg:.2f} | "
            f"median={median_val:.2f}"
        )

    # ---------------------------
    # グラフ更新
    # ---------------------------

    if len(time_plot) > 0:

        line_raw.set_data(time_plot, raw_plot)
        line_outlier.set_data(time_plot, outlier_plot)
        line_avg.set_data(time_plot, avg_plot)
        line_median.set_data(time_plot, median_plot)

        ax.set_xlim(
            max(0, time_plot[0]),
            time_plot[-1] + 1
        )

        scale_values = (
            list(outlier_plot)
            + list(avg_plot)
            + list(median_plot)
        )

        y_min = min(scale_values) - 5
        y_max = max(scale_values) + 5

        if y_min == y_max:
            y_min -= 1
            y_max += 1

        ax.set_ylim(y_min, y_max)

    return (
        line_raw,
        line_outlier,
        line_avg,
        line_median
    )


# =========================================================
# 終了処理
# =========================================================

def on_close(event):
    print("終了処理を実行します。")

    csv_file.close()
    ser.close()

    print(f"CSVを保存しました: {EXPORT_CSV_FILE}")


fig.canvas.mpl_connect(
    "close_event",
    on_close
)


# =========================================================
# アニメーション開始
# =========================================================

ani = FuncAnimation(
    fig,
    update,
    interval=UPDATE_INTERVAL_MS,
    cache_frame_data=False
)

plt.show()