何がしたいの？
僕はロボカップジュニア レスキューメイズという競技に普段出場しているのですが、この競技では自分がフィールドのどこにいるのか自己位置推定することが必須になります。すべてのフィールドは30cm四方のタイルで区切られているという前提がある競技なので、30cm四方のグリッドを検出できれば事実上の自己位置推定となります。
詳しいルールはこちらの記事をみてください。僕の個人ブログです。
使うもの
- LiDAR LD06
- UARTが二系統以上あるマイコンボード
マイコンボードはなんでもいいんですが、今回は持ち運びやすさの観点と、ディスプレイがあって便利という理由でM5Stack Core2を使いました。
やったこと
本来であればすべての処理をマイコンボードで処理させたいのですが、とりあえず実験として試している段階なので現段階では
- マイコンでデータを読み取ってパソコンに流す
- そのデータをNumbersを使ってヒストグラムに変換する
- Pythonを使ってデータを処理してプロットする
という流れです。
原理
動作の例を実際に示します。
例えばこのようなフィールドにロボットが置かれたとします。簡単のためにロボットは真っ直ぐタイルと平行に向いているとします。（実際にはこの辺は方位センサを用いて線形変換するだけなので大した問題ではないです。）
これをプロットしたのが赤い点群です。強いCPUスペックがあればこれをゴリゴリにマッチングしてもいいのですが、マイコンには処理負荷が重すぎるので今回は別の方法を考えます。
まずこのように、一旦ヒストグラムに変換します。このヒストグラムは横軸がX軸、縦軸が点群の個数を示しています。
このヒストグラムを見ると、150mmらへんと-150mm、-700mmらへんが盛り上がっていることがわかります。壁があると点群が集中するので、ここに壁があると推測できます。
点群が集中している場所を見つけて位相を出すために、今回は位相が30cmの正弦波を用意しました。地道ですがこの正弦波の位相をずらしながらそれぞれの共分散を求めて、それが一番デカくなる時の位相がグリッドに重なっている→壁という判定をしています。
これが実際に計算してみた値です。位相が3.09[rad]、147[mm]ずれているという判定が出ました。上の写真と照らし合わせてみると、ちゃんとタイルの真ん中ら辺にLiDARがあるので正しく計算できていることがわかります。
実装の詳細
実測データによるヒストグラムdata_nと参照ヒストグラムref_nの共分散を求める。その共分散が一番デカくなるタイミングがいわゆる壁の波。
1. 平均を出す
refは基準波です。色々な位相のrefを用意して、それぞれに共分散を求めていきます。計算量は多いですが、ギリマイコンで捌ける量でしょう。
$$
\begin{split}
\mu_{d}&=\frac{1}{n}\sum_{k=1}^n\mathit{data}_k\
\end{split}
$$
$$
\begin{split}
\mu_r&=\frac{1}{n}\sum_{k=1}^n\mathit{ref}_k
\end{split}
$$
そりゃぁそう
2. 偏差積和を出す
$$
\begin{split}
S_{dr}=\sum_{k=1}^n(\mathit{data_k}-\mu_d)(\mathit{ref}_k-\mu_r)
\end{split}
$$
3. 共分散を出す
$$
\begin{split}
Cov.&=\frac{S_{dr}}{n}\
&=\frac{1}{n}\sum_{k=1}^n(\mathit{data}_k-\mu_d)(\mathit{ref}_k-\mu_r)
\end{split}
$$
後は頑張って共分散がデカくなるタイミングを探そう！！！！！（ゴリ押し回数ゲー）
実際のコード
なんか理論武装して共分散のコードを書きましたが、現状僕はnumpyしてチートしています。ごめんなさい。
import math
import numpy as np
import matplotlib.pyplot as plt
import lidar
from sklearn.preprocessing import StandardScaler
freq = 6
samples = 181 # 90 + 90 + 0
t = np.arange(samples) / samples
def main():
rawData = np.zeros((len(t), 2))
# よっ！お待ちかね測定データのヒストグラムだ！
rawData[:, 1] = lidar.lidar
# NOTE: 位相推定
maxCov = -1000.0
phase = 0
for i in range(628):
rawData[:, 0] = dataInit(i / 100)
cov = calcCov(rawData)
print(cov)
if cov > maxCov:
maxCov = cov
phase = i
# 最終プロット
rawData[:, 0] = dataInit(phase / 100)
c = np.cov(np.transpose(rawData))
place = (phase / 100) / (2 * math.pi) * 300
if place > 150:
place = place - 300
print("maxCov: ", maxCov, "phase:", phase / 100)
print("place:", place)
plot(rawData)
def plot(data):
plt.figure()
plt.title("Data")
plt.plot(data[1:samples, :])
plt.figure(figsize=(4, 4))
plt.title("Phase Diagram")
plt.scatter(data[1:samples, 0], data[1:samples, 1])
plt.show()
def calcCov(data):
c = np.cov(np.transpose(data))
return c[0, 1]
def dataInit(phase):
data = np.cos(2 * math.pi * freq * t + phase)
return data
if __name__ == "__main__":
main()
lidarリストにはヒストグラムを入れておきます。
lidar = [
0,
0,
0,
0,
0,
0,
0,
# 省略
12
]
終わりに
終わりです