概要
今回は、HC-SR04で取得した距離と速度のデータを使って、対象物の状態をルールベースで分類します。
前回では、距離と速度をカラーマップで見える化しました。
第6回では、距離の平均値や変化量、速度の平均値などの特徴量を使い、接近・停止・離反などの状態をリアルタイムに判定します。
機械学習を使う前に、まずは人が決めたしきい値でどこまで分類できるのかを確認します。
ちょっとしたデジタル化のトライアルをお考えの方に、少しでも参考になればうれしいです。
詳しくは、以下の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
- ESP32から距離データをリアルタイム受信
- 距離データを平滑化し、距離の変化から速度を計算
- 直近の一定時間の距離・速度データから特徴量を計算
- 特徴量を使って、EMPTY、PARKING_IN、OCCUPIED、PARKING_OUTなどにルールベースで分類
- 距離・速度・特徴量・分類結果を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_rule_classification.csv"
# 画面に表示する時間幅 [s]
DISPLAY_SEC = 20.0
# カラーマップの時間分解能 [s]
TIME_BIN = 0.2
# 距離マップの表示範囲 [cm]
DISTANCE_MIN = 0
DISTANCE_MAX = 30
# 速度マップの表示範囲 [cm/s]
VELOCITY_MIN = -5
VELOCITY_MAX = 5
# 平滑化の窓幅
DISTANCE_SMOOTH_WINDOW = 5
VELOCITY_SMOOTH_WINDOW = 9
# 速度計算時に無視する最小時間差 [s]
MIN_DT = 0.01
# グラフ更新周期 [ms]
UPDATE_INTERVAL_MS = 100
# ============================================
# ▼ルールベース分類の設定
# ============================================
# 特徴量を計算する時間幅 [s]
FEATURE_WINDOW_SEC = 1.0
# 空車とみなす距離 [cm]
EMPTY_DISTANCE_THRESHOLD = 15.0
# 駐車中とみなす距離 [cm]
OCCUPIED_DISTANCE_THRESHOLD = 5.0
# 停止とみなす速度範囲 [cm/s]
VELOCITY_STOP_THRESHOLD = 1.0
# 入庫中・出庫中とみなす速度 [cm/s]
VELOCITY_MOVE_THRESHOLD = 1.0
# 入庫中・出庫中とみなす距離変化量 [cm]
DISTANCE_DELTA_THRESHOLD = 1.0
# 不安定とみなす速度ばらつき [cm/s]
UNSTABLE_VELOCITY_STD = 20.0
# ============================================
# 状態表示用
# ============================================
STATE_COLORS = {
"EMPTY": "#91E0FF",
"PARKING_IN": "#FEFF35",
"OCCUPIED": "#FF5154",
"PARKING_OUT": "#B2EC5D",
"STOP_MIDDLE": "#e7e6e6",
"UNSTABLE": "#ffd966",
"UNKNOWN": "#d9d9d9",
}
# 日本語フォント文字化け対策として英語表記にしています
STATE_LABELS = {
"EMPTY": "EMPTY",
"PARKING_IN": "PARKING IN",
"OCCUPIED": "OCCUPIED",
"PARKING_OUT": "PARKING OUT",
"STOP_MIDDLE": "STOP",
"UNSTABLE": "UNSTABLE",
"UNKNOWN": "UNKNOWN",
}
# ============================================
# シリアル接続
# ============================================
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",
"distance_mean_cm",
"distance_delta_cm",
"velocity_mean_cm_s",
"velocity_abs_mean_cm_s",
"velocity_std_cm_s",
"state",
"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
current_state = "UNKNOWN"
current_features = {}
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
# ============================================
# 特徴量計算
# ============================================
def calculate_features(time_array, distance_array, velocity_array, current_time):
start_t = current_time - FEATURE_WINDOW_SEC
mask = time_array >= start_t
t_win = time_array[mask]
d_win = distance_array[mask]
v_win = velocity_array[mask]
if len(t_win) < 2:
return {
"distance_mean": np.nan,
"distance_delta": np.nan,
"velocity_mean": np.nan,
"velocity_abs_mean": np.nan,
"velocity_std": np.nan,
}
distance_mean = float(np.mean(d_win))
distance_delta = float(d_win[-1] - d_win[0])
velocity_mean = float(np.mean(v_win))
velocity_abs_mean = float(np.mean(np.abs(v_win)))
velocity_std = float(np.std(v_win))
return {
"distance_mean": distance_mean,
"distance_delta": distance_delta,
"velocity_mean": velocity_mean,
"velocity_abs_mean": velocity_abs_mean,
"velocity_std": velocity_std,
}
# ============================================
# ルールベース分類
# ============================================
def classify_state(features):
d_mean = features["distance_mean"]
d_delta = features["distance_delta"]
v_mean = features["velocity_mean"]
v_abs_mean = features["velocity_abs_mean"]
v_std = features["velocity_std"]
if np.isnan(d_mean):
return "UNKNOWN"
# --------------------------------------------
# 最優先：空車判定
# 距離がEMPTY_DISTANCE_THRESHOLD以上なら、
# 入庫中・出庫中よりも空車を優先する
# --------------------------------------------
if d_mean >= EMPTY_DISTANCE_THRESHOLD:
return "EMPTY"
# --------------------------------------------
# 入庫中：距離が短くなり、速度がマイナス
# --------------------------------------------
if (
v_mean < -VELOCITY_MOVE_THRESHOLD
and d_delta < -DISTANCE_DELTA_THRESHOLD
):
return "PARKING_IN"
# --------------------------------------------
# 出庫中：距離が長くなり、速度がプラス
# --------------------------------------------
if (
v_mean > VELOCITY_MOVE_THRESHOLD
and d_delta > DISTANCE_DELTA_THRESHOLD
):
return "PARKING_OUT"
# --------------------------------------------
# 停止中の判定
# --------------------------------------------
if v_abs_mean <= VELOCITY_STOP_THRESHOLD:
if d_mean <= OCCUPIED_DISTANCE_THRESHOLD:
return "OCCUPIED"
return "STOP_MIDDLE"
# --------------------------------------------
# ばらつきが大きい場合
# --------------------------------------------
if v_std >= UNSTABLE_VELOCITY_STD:
return "UNSTABLE"
return "UNKNOWN"
# ============================================
# 1画面レイアウト
# ============================================
fig = plt.figure(
figsize=(16, 9)
)
fig.suptitle(
"Ultrasonic Sensor Real-time Monitoring & Rule-based Classification",
fontsize=14
)
gs = fig.add_gridspec(
3,
4,
width_ratios=[30, 1, 30, 1],
height_ratios=[1, 3, 1.2],
hspace=0.45,
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])
# 下段：状態表示
ax_state = fig.add_subplot(gs[2, :])
# ============================================
# 距離グラフ初期設定
# ============================================
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_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_trend.axhline(
0,
linestyle="--",
linewidth=1
)
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")
# ============================================
# 状態表示 初期設定
# ============================================
ax_state.set_xticks([])
ax_state.set_yticks([])
ax_state.set_title("Current State")
state_text = ax_state.text(
0.02,
0.65,
"UNKNOWN",
transform=ax_state.transAxes,
fontsize=22,
fontweight="bold",
va="center"
)
feature_text = ax_state.text(
0.02,
0.25,
"",
transform=ax_state.transAxes,
fontsize=11,
va="center"
)
ax_state.set_facecolor(
STATE_COLORS["UNKNOWN"]
)
# ============================================
# カラーバー初期化用ダミー
# ============================================
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
global current_state
global current_features
# --------------------------------
# シリアルデータ受信
# --------------------------------
while ser.in_waiting > 0:
line_text = (
ser.readline()
.decode("utf-8", errors="ignore")
.strip()
)
# 例: Distance: 52.34cm
match = re.search(
r"Distance:\s*([0-9]+(?:\.[0-9]+)?)\s*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)
# 特徴量と状態判定
time_array = np.array(time_data)
distance_array = np.array(distance_data)
velocity_array = np.array(velocity_data)
current_features = calculate_features(
time_array,
distance_array,
velocity_array,
elapsed_time
)
current_state = classify_state(
current_features
)
csv_writer.writerow([
f"{elapsed_time:.3f}",
f"{raw_distance:.2f}",
f"{distance_smooth:.2f}",
f"{velocity:.2f}",
f"{velocity_smooth:.2f}",
f"{current_features['distance_mean']:.2f}",
f"{current_features['distance_delta']:.2f}",
f"{current_features['velocity_mean']:.2f}",
f"{current_features['velocity_abs_mean']:.2f}",
f"{current_features['velocity_std']:.2f}",
current_state,
line_text
])
csv_file.flush()
print(
f"{elapsed_time:.2f}s | "
f"distance={distance_smooth:.2f}cm | "
f"velocity={velocity_smooth:.2f}cm/s | "
f"state={current_state}"
)
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)
# ============================================
# 状態表示更新
# ============================================
label = STATE_LABELS.get(
current_state,
"UNKNOWN"
)
color = STATE_COLORS.get(
current_state,
STATE_COLORS["UNKNOWN"]
)
ax_state.set_facecolor(color)
state_text.set_text(
label
)
if current_features:
feature_text.set_text(
"Features | "
f"distance_mean={current_features['distance_mean']:.1f} cm "
f"distance_delta={current_features['distance_delta']:.1f} cm "
f"velocity_mean={current_features['velocity_mean']:.1f} cm/s "
f"velocity_abs_mean={current_features['velocity_abs_mean']:.1f} cm/s "
f"velocity_std={current_features['velocity_std']:.1f} cm/s"
)
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()
コメント