距離と速度をカラーマップ化｜超音波センサーの動きを見える化してみた【超音波センサー#5】

Contents

概要
プログラムコード

概要

今回は、HC-SR04で取得した距離と速度のデータを、カラーマップで見える化します。

前回は、距離データから速度を計算し、接近・停止・離反をリアルタイムに表示しました。
第5回では、その距離と速度の変化を色で表現し、対象物の動きを直感的に確認できるようにします。

折れ線グラフでは細かい変化を確認できますが、カラーマップにすると、距離や速度の変化パターンを一目で見やすくなります。

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

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

プログラムコード

マイコン（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（CSV読み込み版）

CSVファイルから、時間・距離・速度データを読み込み
数値化できないデータや欠損値を除外
時間順にデータを並べ替え
指定した時間幅ごとにデータを分割
各時間区間ごとに、距離の平均値を計算
各時間区間ごとに、速度の平均値を計算
グラフ表示

import pandas as pd
import numpy as np
import matplotlib.pyplot as plt


# ============================================
# ▼ここだけ設定すればOK
# ============================================

CSV_FILE = "ultrasonic_motion_analysis.csv"

DISTANCE_COLUMN = "distance_smooth_cm"
VELOCITY_COLUMN = "velocity_smooth_cm_s"

TIME_BIN = 0.2

DISTANCE_MIN = 0
DISTANCE_MAX = 100

VELOCITY_MIN = -50
VELOCITY_MAX = 50


# ============================================
# CSV読込
# ============================================

df = pd.read_csv(CSV_FILE)

df["elapsed_time_s"] = pd.to_numeric(df["elapsed_time_s"], errors="coerce")
df[DISTANCE_COLUMN] = pd.to_numeric(df[DISTANCE_COLUMN], errors="coerce")
df[VELOCITY_COLUMN] = pd.to_numeric(df[VELOCITY_COLUMN], errors="coerce")

df = df.dropna(
    subset=["elapsed_time_s", DISTANCE_COLUMN, VELOCITY_COLUMN]
).reset_index(drop=True)

df = df.sort_values("elapsed_time_s").reset_index(drop=True)

time_s = df["elapsed_time_s"].values
distance = df[DISTANCE_COLUMN].values
velocity = df[VELOCITY_COLUMN].values


# ============================================
# 時間ビン作成
# ============================================

t_start = np.min(time_s)
t_end = np.max(time_s)

time_bins = np.arange(t_start, t_end + TIME_BIN, TIME_BIN)

if time_bins[-1] < t_end:
    time_bins = np.append(time_bins, t_end)

n_time = len(time_bins) - 1


# ============================================
# 時間ビンごとの平均値を作る関数
# ============================================

def make_time_map(time_s, value, time_bins):
    value_map = np.full((1, len(time_bins) - 1), np.nan)

    for i in range(len(time_bins) - 1):
        if i == len(time_bins) - 2:
            mask = (time_s >= time_bins[i]) & (time_s <= time_bins[i + 1])
        else:
            mask = (time_s >= time_bins[i]) & (time_s < time_bins[i + 1])

        if np.any(mask):
            value_map[0, i] = np.mean(value[mask])

    return value_map


distance_map = make_time_map(time_s, distance, time_bins)
velocity_map = make_time_map(time_s, velocity, time_bins)


# ============================================
# 距離マップ表示
# ============================================

fig = plt.figure(figsize=(14, 6))

gs = fig.add_gridspec(
    2,
    2,
    width_ratios=[30, 1],
    height_ratios=[1, 3],
    hspace=0.25,
    wspace=0.08
)

ax_map = fig.add_subplot(gs[0, 0])
ax_trend = fig.add_subplot(gs[1, 0], sharex=ax_map)
cax = fig.add_subplot(gs[0, 1])

mesh = ax_map.pcolormesh(
    time_bins,
    [0, 1],
    distance_map,
    cmap="turbo_r",
    vmin=DISTANCE_MIN,
    vmax=DISTANCE_MAX,
    shading="flat"
)

ax_map.set_title("Distance Map")
ax_map.set_yticks([])
ax_map.set_xlim(t_start, t_end)
ax_map.tick_params(labelbottom=False)

cbar = fig.colorbar(mesh, cax=cax)
cbar.set_label("Distance [cm]")

ax_trend.plot(
    time_s,
    distance,
    linewidth=2,
    label="Distance"
)

ax_trend.set_title("Distance Trend")
ax_trend.set_xlabel("Time [s]")
ax_trend.set_ylabel("Distance [cm]")
ax_trend.set_xlim(t_start, t_end)
ax_trend.set_ylim(DISTANCE_MIN, DISTANCE_MAX)
ax_trend.grid(True)
ax_trend.legend(loc="upper right")

plt.show()


# ============================================
# 速度マップ表示
# ============================================

fig = plt.figure(figsize=(14, 6))

gs = fig.add_gridspec(
    2,
    2,
    width_ratios=[30, 1],
    height_ratios=[1, 3],
    hspace=0.25,
    wspace=0.08
)

ax_map = fig.add_subplot(gs[0, 0])
ax_trend = fig.add_subplot(gs[1, 0], sharex=ax_map)
cax = fig.add_subplot(gs[0, 1])

mesh = ax_map.pcolormesh(
    time_bins,
    [0, 1],
    velocity_map,
    cmap="RdYlGn",
    vmin=VELOCITY_MIN,
    vmax=VELOCITY_MAX,
    shading="flat"
)

ax_map.set_title("Velocity Map")
ax_map.set_yticks([])
ax_map.set_xlim(t_start, t_end)
ax_map.tick_params(labelbottom=False)

cbar = fig.colorbar(mesh, cax=cax)
cbar.set_label("Velocity [cm/s]")

ax_trend.plot(
    time_s,
    velocity,
    linewidth=2,
    label="Velocity"
)

ax_trend.axhline(0, linestyle="--")

ax_trend.set_title("Velocity Trend")
ax_trend.set_xlabel("Time [s]")
ax_trend.set_ylabel("Velocity [cm/s]")
ax_trend.set_xlim(t_start, t_end)
ax_trend.set_ylim(VELOCITY_MIN, VELOCITY_MAX)
ax_trend.grid(True)
ax_trend.legend(loc="upper right")

plt.show()

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
# ============================================

PORT = "COM5"
BAUDRATE = 115200

EXPORT_CSV_FILE = "ultrasonic_realtime_map.csv"

# 画面に表示する時間幅 [s]
DISPLAY_SEC = 20.0

# カラーマップの時間分解能 [s]
TIME_BIN = 0.2

# 距離マップの表示範囲 [cm]
DISTANCE_MIN = 0
DISTANCE_MAX = 100

# 速度マップの表示範囲 [cm/s]
VELOCITY_MIN = -50
VELOCITY_MAX = 50

# 平滑化の窓幅
DISTANCE_SMOOTH_WINDOW = 5
VELOCITY_SMOOTH_WINDOW = 9

# 速度計算時に無視する最小時間差 [s]
MIN_DT = 0.01

# グラフ更新周期 [ms]
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_distance_cm",
    "distance_smooth_cm",
    "velocity_cm_s",
    "velocity_smooth_cm_s",
    "raw_text"
])


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

time_data = deque()
distance_data = deque()
velocity_data = deque()

distance_buffer = deque(maxlen=DISTANCE_SMOOTH_WINDOW)
velocity_buffer = deque(maxlen=VELOCITY_SMOOTH_WINDOW)

last_time = None
last_distance_smooth = None

start_time = time.time()


# ============================================
# マップ作成関数
# ============================================

def make_time_map(time_values, value_values, t_start, t_end):
    time_bins = np.arange(
        t_start,
        t_end + TIME_BIN,
        TIME_BIN
    )

    if len(time_bins) < 2:
        time_bins = np.array([
            t_start,
            t_start + TIME_BIN
        ])

    n_time = len(time_bins) - 1

    value_map = np.full(
        (1, n_time),
        np.nan
    )

    time_array = np.array(time_values)
    value_array = np.array(value_values)

    for i in range(n_time):

        if i == n_time - 1:
            mask = (
                (time_array >= time_bins[i])
                &
                (time_array <= time_bins[i + 1])
            )
        else:
            mask = (
                (time_array >= time_bins[i])
                &
                (time_array < time_bins[i + 1])
            )

        if np.any(mask):
            value_map[0, i] = np.mean(
                value_array[mask]
            )

    return time_bins, value_map


# ============================================
# 1画面4分割レイアウト
# ============================================

fig = plt.figure(
    figsize=(16, 8)
)

fig.suptitle(
    "Ultrasonic Sensor Real-time Distance & Velocity Map",
    fontsize=14
)

gs = fig.add_gridspec(
    2,
    4,
    width_ratios=[30, 1, 30, 1],
    height_ratios=[1, 3],
    hspace=0.35,
    wspace=0.15
)

# 左上：距離マップ
ax_dist_map = fig.add_subplot(gs[0, 0])

# 左下：距離推移
ax_dist_trend = fig.add_subplot(
    gs[1, 0],
    sharex=ax_dist_map
)

# 距離カラーバー
cax_dist = fig.add_subplot(gs[0, 1])

# 右上：速度マップ
ax_vel_map = fig.add_subplot(gs[0, 2])

# 右下：速度推移
ax_vel_trend = fig.add_subplot(
    gs[1, 2],
    sharex=ax_vel_map
)

# 速度カラーバー
cax_vel = fig.add_subplot(gs[0, 3])


# ============================================
# 距離グラフ初期設定
# ============================================

dist_mesh = None

line_dist, = ax_dist_trend.plot(
    [],
    [],
    linewidth=2,
    label="Distance"
)

ax_dist_map.set_title("Distance Map")
ax_dist_map.set_yticks([])
ax_dist_map.set_ylabel("(map)")

ax_dist_trend.set_title("Distance Trend")
ax_dist_trend.set_xlabel("Time [s]")
ax_dist_trend.set_ylabel("Distance [cm]")
ax_dist_trend.set_ylim(DISTANCE_MIN, DISTANCE_MAX)
ax_dist_trend.grid(True)
ax_dist_trend.legend(loc="upper right")


# ============================================
# 速度グラフ初期設定
# ============================================

vel_mesh = None

line_vel, = ax_vel_trend.plot(
    [],
    [],
    linewidth=2,
    label="Velocity"
)

ax_vel_map.set_title("Velocity Map")
ax_vel_map.set_yticks([])
ax_vel_map.set_ylabel("(map)")

ax_vel_trend.axhline(
    0,
    linestyle="--"
)

ax_vel_trend.set_title("Velocity Trend")
ax_vel_trend.set_xlabel("Time [s]")
ax_vel_trend.set_ylabel("Velocity [cm/s]")
ax_vel_trend.set_ylim(VELOCITY_MIN, VELOCITY_MAX)
ax_vel_trend.grid(True)
ax_vel_trend.legend(loc="upper right")


# ============================================
# カラーバー初期化用ダミー
# ============================================

dummy_dist = ax_dist_map.pcolormesh(
    [0, TIME_BIN],
    [0, 1],
    np.array([[np.nan]]),
    cmap="turbo_r",
    vmin=DISTANCE_MIN,
    vmax=DISTANCE_MAX,
    shading="flat"
)

cbar_dist = fig.colorbar(
    dummy_dist,
    cax=cax_dist
)

cbar_dist.set_label("Distance [cm]")


dummy_vel = ax_vel_map.pcolormesh(
    [0, TIME_BIN],
    [0, 1],
    np.array([[np.nan]]),
    cmap="RdYlGn",
    vmin=VELOCITY_MIN,
    vmax=VELOCITY_MAX,
    shading="flat"
)

cbar_vel = fig.colorbar(
    dummy_vel,
    cax=cax_vel
)

cbar_vel.set_label("Velocity [cm/s]")


# ============================================
# 更新処理
# ============================================

def update(frame):
    global last_time
    global last_distance_smooth
    global dist_mesh
    global vel_mesh

    # --------------------------------
    # シリアルデータ受信
    # --------------------------------

    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_distance = float(match.group(1))
        elapsed_time = time.time() - start_time

        # --------------------------------
        # 距離の平滑化
        # --------------------------------

        distance_buffer.append(raw_distance)

        distance_smooth = float(
            np.mean(distance_buffer)
        )

        # --------------------------------
        # 速度計算
        # --------------------------------

        if last_time is None:
            velocity = 0.0

        else:
            dt = elapsed_time - last_time

            if dt <= MIN_DT:
                velocity = 0.0
            else:
                velocity = (
                    distance_smooth
                    - last_distance_smooth
                ) / dt

        last_time = elapsed_time
        last_distance_smooth = distance_smooth

        # --------------------------------
        # 速度の平滑化
        # --------------------------------

        velocity_buffer.append(velocity)

        velocity_smooth = float(
            np.mean(velocity_buffer)
        )

        # --------------------------------
        # データ保存
        # --------------------------------

        time_data.append(elapsed_time)
        distance_data.append(distance_smooth)
        velocity_data.append(velocity_smooth)

        csv_writer.writerow([
            f"{elapsed_time:.3f}",
            f"{raw_distance:.2f}",
            f"{distance_smooth:.2f}",
            f"{velocity:.2f}",
            f"{velocity_smooth:.2f}",
            line_text
        ])

        csv_file.flush()

        print(
            f"{elapsed_time:.2f}s | "
            f"distance={distance_smooth:.2f}cm | "
            f"velocity={velocity_smooth:.2f}cm/s"
        )

    if len(time_data) < 2:
        return []

    # --------------------------------
    # 表示時間範囲
    # --------------------------------

    current_time = time_data[-1]
    t_start = max(0, current_time - DISPLAY_SEC)
    t_end = max(DISPLAY_SEC, current_time)

    time_array = np.array(time_data)
    distance_array = np.array(distance_data)
    velocity_array = np.array(velocity_data)

    mask = time_array >= t_start

    t_show = time_array[mask]
    d_show = distance_array[mask]
    v_show = velocity_array[mask]

    # ============================================
    # 距離マップ更新
    # ============================================

    time_bins, distance_map = make_time_map(
        t_show,
        d_show,
        t_start,
        t_end
    )

    if dist_mesh is not None:
        dist_mesh.remove()

    dist_mesh = ax_dist_map.pcolormesh(
        time_bins,
        [0, 1],
        distance_map,
        cmap="turbo_r",
        vmin=DISTANCE_MIN,
        vmax=DISTANCE_MAX,
        shading="flat"
    )

    ax_dist_map.set_xlim(t_start, t_end)
    ax_dist_map.set_yticks([])
    ax_dist_map.tick_params(labelbottom=False)

    line_dist.set_data(
        t_show,
        d_show
    )

    ax_dist_trend.set_xlim(t_start, t_end)
    ax_dist_trend.set_ylim(DISTANCE_MIN, DISTANCE_MAX)

    # ============================================
    # 速度マップ更新
    # ============================================

    time_bins, velocity_map = make_time_map(
        t_show,
        v_show,
        t_start,
        t_end
    )

    if vel_mesh is not None:
        vel_mesh.remove()

    vel_mesh = ax_vel_map.pcolormesh(
        time_bins,
        [0, 1],
        velocity_map,
        cmap="RdYlGn",
        vmin=VELOCITY_MIN,
        vmax=VELOCITY_MAX,
        shading="flat"
    )

    ax_vel_map.set_xlim(t_start, t_end)
    ax_vel_map.set_yticks([])
    ax_vel_map.tick_params(labelbottom=False)

    line_vel.set_data(
        t_show,
        v_show
    )

    ax_vel_trend.set_xlim(t_start, t_end)
    ax_vel_trend.set_ylim(VELOCITY_MIN, VELOCITY_MAX)

    return []


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

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

    try:
        csv_file.close()
    except Exception:
        pass

    try:
        ser.close()
    except Exception:
        pass

    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()