安全運転支援のためのフーリエ解析による合図の認識 〜腕の回転〜

## はじめに
自動車、鉄道、航空機など、様々なモビリティの形態において運転の自動化が進んでいます。自動運転技術では、車載前方に搭載されたカメラを使用して、前方の車や歩行者などの可動障害物を「バウンディングボックス」と呼ばれる矩形領域で認識することで実現していることが主流です。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3572173/bc5f3f7a-635d-2de3-99d0-42651f742913.png)

ここで注意すべき点として、バウンディングボックスの中で人間がどんな動きをしているのかの判別までは行っていない事例がほとんどです。しかし、例えば、

・警備員・交通誘導員の「止まれ」の合図
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3572173/520d20f7-b0f8-2e86-7a8e-ee7ed57687b9.png)
・列車が非常停止させるために行う「列車防護」と呼ばれる発煙筒を回す合図
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3572173/82480a3c-95cf-f9ca-1602-ccd35af4cfd2.png)
・飛行機を滑走路で安全に誘導するためのマーシャラー「停止せよ」の合図
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3572173/05e714bb-f2dd-9f71-067a-cc08556e177a.png)

このような合図が出されている場合、運転者の一刻も早い気づきと適切な対応が必要であり、認知・判断・操作のどれかが一つでも遅れたり欠けたりすると、人身事故や衝突事故などの重大事故につながる危険性があります。

そこで、今回は、周囲の人間をバウンディングボックスとして「いるかないか」を認識するだけでなく、骨格推定データを解析することで、その人がどのような動作をしているのか、どんなメッセージを伝えたいのかを推定し、運転する人に正しく早く伝えることで安全な運転を支援することを目指します。

## 今回想定するケース
今回は、前方に人がいるという前提で、AIによる骨格推定を行い人間の関節の時系列データを抽出します。その上で、「腕を回している」ことを検出するためのツールの開発の基礎検討を行います。

## 骨格推定データについて
今回は MediaPipe という Google が提供している骨格推定のライブラリを使用しました。二次元の人間の画像から３次元の骨格の位置を推定することが可能です。今回は左手で腕を周期的に回している合図を、左手の手先のx,z座標の軌跡を解析することで認識できないか模索してみました。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3572173/a0809c95-e189-75f5-19e9-8b66c45a5dc2.png)


## 実際に撮った動画を骨格推定したもの

https://youtu.be/TEUfyJzpmuY

再生位置としては、最初の2～7秒で左回転、8秒以降は右回転に切り替え、11秒以降はダミーとして回転動作ではない動きを入れてみました。

## 左手の手先の骨格推定データを見てみる

以下の図は、上の動画を骨格推定して得られた左手の手先の座標です。奥行方向の移動は無視して $x,z$　座標に注目してみました。

``` python
df = pd.read_csv("kokkakusuitei_data.csv")

left_hand_x = "17.left_pinky.x"
left_hand_z = "17.left_pinky.z"

plt.plot(df["time1"], df[left_hand_x], label="Left Hand x")
plt.plot(df["time1"], df[left_hand_z], label="Left Hand z")

plt.xlabel("time [s]")
plt.ylabel("x, z")

plt.legend()
plt.show()
```

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3572173/9e4bdb78-402d-7f99-f947-870dc45b2ab5.png)

### 座標データを正規化してみる

そこで、両者の波形の特徴を詳しく比べるために、「正規化」をしてあげましょう。正規化とは、データを特定の範囲に変換するプロセスのことで、異なるスケールや単位のデータを比較する際に問題が生じるのを防ぐことができます。

今回は Min-Max Scaling という手法を用います。各データポイントから最小値を引いて、範囲（最大値 - 最小値）で割ることで、データが指定された範囲（0から1）に収まります。

先ほどのデータを正規化して、手を回している部分に限定したデータを下に表示します。

```math
\boldsymbol{x}_{scaled}= \frac{\boldsymbol{x}−x_{min}}{x_{max}−x_{min}}
```

```python
data_x = df["17.left_pinky.x"]
data_z = df["17.left_pinky.z"]

min_x, max_x = data_x.min(), data_x.max()
min_z, max_z = data_z.min(), data_z.max()

df[left_hand_x + "_norm"] = (data_x - min_x) / (max_x - min_x) 
df[left_hand_z + "_norm"] = (data_z - min_z) / (max_z - min_z)

plt.plot(df["time1"], df[left_hand_x + "_norm"], label="Left Hand x")
plt.plot(df["time1"], df[left_hand_z + "_norm"], label="Left Hand z")

plt.xlabel("time [s]")
plt.ylabel("x, z")
plt.xlim([3,8])

plt.legend()
plt.show()
```

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3572173/c4e45e52-98ef-520b-2dd6-2b2a4c3ffb63.png)

手を回転させているときの左手のx座標およびz座標に焦点を当てると、それらの座標が sin 関数および cos 関数のように上下に変動していることがわかりました。

## 円運動の特性
理論的には、円運動の横方向・縦方向の動きは、位相が $\pi/2$ ずれた三角関数（単振動）として表すことができます。数式で表すと以下のようになります。

```math
\begin{align}
x(t)&=r \cos(\omega t) \\
y(t)&=r \sin(\omega t)=r \cos(\omega t + \frac{\pi}{2})
\end{align}
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3572173/f13d0ed9-0f87-c104-bbc9-5b1b4748eaa6.png)

このような振動の特徴をうまく検出するアルゴリズムを作れば、回転動作も認識できるようになるのではないかと考えました。今回は Deep Learning などの学習ベースの手法を用いず、「フーリエ解析」というアプローチで信号検出に取り組んでいきます。

## 検出のアーキテクチャ
まず、左手のx座標それぞれのz座標の振動検出アルゴリズムを通します。もしどちらの振動も検出された場合、それぞれ振動の位相を比較し、$\pi/2$ ずれていたら「回転」と検出するという流れで行いたいと思います。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3572173/f20c8e37-793d-5274-b12c-dfdbfa917108.png)


## フーリエ解析の実行
関数に含まれている周波数成分を見つける操作をフーリエ解析と言います。腕を回転させている際、左手のx, z座標が比較的きれいな振動の軌跡を描いていました。そこで、その波形に対してフーリエ解析を行うことで、回転周波数の成分を顕著に抽出できるのではないかと思います。今回はそのような仮説に基づいてpythonによるプログラミングで検証を行います。今回使う「離散フーリエ変換（DFT）」は以下のような定義式になります。

```math
X[f] = \sum_{k=0}^{N-1} f[t]e^{-j\cdot 2\pi f \frac{k}{N} t} dt 
```

ここで、$f[t]$ は元の信号波形、 $f$ はサンプリング周波数、$N$ はサンプル数です。なぜ「eの○○乗」を掛けるのか？という疑問はオイラーの公式を使うとわかります。

```math
\begin{align}
X[f] &= \sum_{k=0}^{N-1} f[t]e^{-j\cdot 2\pi f \frac{k}{N} t} \\
&= \sum_{k=0}^{N-1} f[t]\cos{\frac{2\pi f k t}{N}}-\sum_{k=0}^{N-1}j \cdot f[t]\sin{\frac{2\pi f k t}{N}}
\end{align}
```

オイラーの公式を適用した上の式からも分かる通り、実部では、$f[t]$ に周波数 $fk/N$ のコサイン波を掛け、虚部は周波数 $fk/N$ のサイン波を掛けていることがわかります。実は三角関数には「直交性」という性質があり、さまざまな周波数の波が重なり合った波形と、周波数 $fk/N$ の三角関数を掛けて積分を取る（内積を取る操作と同じ）と、周波数 $fk/N$ 以外の成分は $0$ （内積がゼロ・つまり直交！）になり、周波数 $fk/N$ の波だけを取り出すことができ、周波数特性がわかります。これがフーリエ変換のざっくりとした概念的理解です。

つまり、出力される $X[f]$ は複素数で、実部・虚部はそれぞれ、周波数 $fk/N$ におけるコサイン成分の強さ、サイン成分の強さを表します。ゆえに、絶対値 $|X[f]|$ を取ることで信号の強さを抽出することが可能です。

## 実行結果
人間の腕を回転させる速度は、一回転あたり、0.5秒～5秒と仮定し、5秒間の窓で波形を取り出し、DFTを実行してみました。以下にDFTを実行するコードを記載しました。

```python
import pandas as pd
import numpy as np
def fft(t, timeFrame = 5, value, df):
  # 注目する時間の開始時刻
  t1 = t - timeFrame 

  # 注目時間を抽出するマスクを作成
  time_mask = (df['time1'] >= t1) & (df['time1'] <= t)

  # 注目時間を抽出
  time = (df["time1"][time_mask] - t1).to_numpy()

  # dfからvalueの列データを抽出
  data = df[value][time_mask].to_numpy()

  N = len(data) # データ点の数
  fs = len(data)/timeFrame # サンプリング周波数

  # FFTを実行
  amp = np.fft.fft(data) #振幅
  freq = np.fft.fftfreq(len(data), 1/(len(data)/timeFrame)) #周波数
  
  return time,data, freq[1:int(N/2)], np.abs(amp*2/N)[1:int(N/2)]
```
結果は以下の通りです。

再生位置 $3<t<8$ のとき

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3572173/62dbacce-1260-835f-3011-30017ba5e21c.png)

$4<t<9$ のとき
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3572173/22a92ab0-85c5-9183-8664-ecc62938290b.png)

$6<t<11$ のとき
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3572173/a7839d18-b726-f30b-eee7-aa587a0fe3cb.png)

$14<t<19$ のとき
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3572173/42434e32-f8c9-fb68-f357-62b22ff23570.png)

それぞれのDFTを実行した時間間隔に対して、上下の２つのグラフがあると思いますが、上は時間領域信号（元々の信号）、下は周波数領域信号（DFTした信号）を表します。図からわかる通り、元の信号がきれいな振動をしているときは、DFTした結果、その振動周波数に急峻なピークが立ちます。それに対して、途中で動きが変わったりしてきれいに振動していない時は、ピークが弱くなったり複数のピークが出てしまうことがわかります。

## 極大の検出
これらのピークを検出して、実際に回転動作を判定してみようと思います。
今回は、科学技術計算ライブラリ scipy の find_peaks 関数を使用します。振幅0.01以上のしきい値でノイズを篩にかけ、0.2～10 Hz の極大点を検出していきます。極大点が一つか、極大点が複数の場合は最大の極大値が、他のすべての極大値よりも3倍以上大きい場合を「検出」としました。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3572173/f0546416-3f61-17a6-cff4-d9b6fafd7f10.png)

```python
peaks, _ = find_peaks(amp, height=0.01) # 0.01 のしきい値でピークを取得
peak_freq = freq[peaks].clip(0.2,10) #0.2～10 Hz に限定

max_peak_value = max(amp[peaks]) #極大値の最大値を取得
detected = len(peak_freq) == 1 \
            or np.count_nonzero(amp[peaks] >= max_peak_value / 3) == 1
```

## 位相の検出
三角関数には、有名な性質 $\cos^2 \theta + \sin^2 \theta = 1$ があります。この性質は言い換えると、位相が $\pi/2$ ずれた二つの正規化された波の二乗和は１になるということを意味します。この性質を利用し、円運動の縦横の波形の二乗和を求めて 1 との差を考えることで、位相が $\pi/2$ ずれている判定に使用することができます。

左手の手先の $x, z$ 座標を -1~1 に正規化した座標 $x_n, z_n$ において、$x_n^2 + z_n^2 - 1$ を計算した結果、以下のようになりました。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3572173/b790772f-a649-f991-e582-924f81d94f3a.png)

5秒間での移動平均を取ると、以下のようになります。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3572173/b4b3a3fe-77c4-d444-91c8-71203fb7b013.png)

0〜10秒が回転動作だったので、今回は $|x_n^2+z_n^2-1|<0.2$ をしきい値にすることで、位相の $\pi/2$ ずれを検出することができます。

```python
# 0〜1 に正規化したデータを -1〜1 に直し、(二乗和) - 1 を求める。
df["phase"] = (df[left_hand_x + "_norm"] * 2 - 1) ** 2 \
                + (df[left_hand_z + "_norm"] * 2 - 1) ** 2 - 1

dur, step = 22, 0.1 #動画の秒数とサンプリング時間ステップ
t_sequence = np.arange(5, dur + step, step) #
n_t = len(t_sequence)
average_phase = np.zeros(n_t)
detect_phase = np.zeros(n_t)

for i in range(n_t):
    t = t_sequence[i]
    # t - 5 ~ t 秒に該当するデータのインデックスを取得する。
    filtered_indexes = df[(df['time'] >= t - 5) & (df['time'] <= t)].index

    # 平均値を求め、 0.2 をしきい値として判定データをdetect_phaseに格納
    average_phase_d[i-1] = df.loc[filtered_indexes, 'phase'].mean()
    detect_phase[i-1] = abs(average_phase_d[i-1]) < 0.2
```

## 検出結果
縦横それぞれの検出結果と、位相 $\pi/2$ ずれの検出結果を組み合わせた結果、以下のようになりました。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3572173/da564e35-506b-efee-f51c-492fedab9b14.png)

多少の検出の遅れはあるものの、5~10秒の回転動作をしっかり検出できているのはわかります。また右回転と左回転の切り替えのタイミングもしっかりと反映されていました。
動画と重ねてみると以下のようになります。

https://youtu.be/v2WKHVUBFck

## 今後
最近はDeep Learningによる認識が流行っていますが、今回のようなプリミティブな手法を使うことにも、認識の仕組みを知る上で必要なことだとも思っています。（実際にAIもこのような特徴抽出を学習している。）今後は回転以外の周期的な合図動作を認識する手法を考えていきたいと思います。