白鑞のカラス日記

マイクロマウスの進捗や事件を書いていきまーす。

Pythonでターンシミュレータ

この記事はMicro Mouse Advent Calendar 2019 - Adventarの19日目の記事です。

 

昨日の記事はけりさんの「自動復帰マウスの紹介」でした。

 

自動復帰ってすごいですね。初めてみたときはびっくりしました。どんな感じでやってるのか気になっていたので後日じっくり読みたいと思います。

 

では本題に入りたいと思います。

 

今日の自分の内容は「Pythonでターンシミュレータ」です。

 

アンケートで1位だったので書こうと思います。

 

技術力ないマンが作るものなんでそんなに期待はしないでね…

 

今回使った総合開発環境は「Pycharm」です。

 

使ったライブラリは「numpy」と「matplotlib」です。

 

これを使ってターンシミュレータを作ります。

 

  • 関数の紹介 

まず使う関数の紹介です.

 

Numpy(as np)

 

np.array()

配列

np.sin(角度[rad])

sin波

np.cos(角度[rad])

cos波

np.pi       

円周率

                                       

matplotlib(as plt)

 

plt.axis(‘equal’)

x軸,y軸のスケールを合わせる

plt.plot(x,y,線幅,線色,線種)

表に点や線をプロットするやつ

plt.show()

グラフの出力

 

 

  • 壁を書く

こんなのを使っていきます。まず壁を書きましょう。

 

まじで適当に作ったのでもっとうまくできる人は頑張ってみてください

壁を書くプログラムリスト

import matplotlib.pyplot as plt

import numpy as np

 

x = np.array([0, 360])

y = np.array([0, 0])

plt.plot(x, y, linewidth=2, color="blue")

 

x = np.array([0, 0])

y = np.array([0, 360])

plt.plot(x, y, linewidth=2, color="blue")

 

x = np.array([360, 0])

y = np.array([360, 360])

plt.plot(x, y, linewidth=2, color="blue")

 

x = np.array([360, 360])

y = np.array([0, 360])

plt.plot(x, y, linewidth=2, color="blue")

 

x = np.array([180, 180])

y = np.array([0, 180])

plt.plot(x, y, linewidth=2, color="blue")

 

plt.axis('equal')

plt.show()

 

 

こんなんで壁は引けます。あとは補助線を自分で入れてみてください。リストが長くなりそうなので今回は壁だけで

 

 f:id:shinshinmice:20191217191045p:plain

 

最終的にはここまで補助線入れるといいと思います。

 

 

  • 入力値と変数

下準備が終わったので次は実際にどのような計算を行っていくのかを書きたいと思います。

 

今回はeuler法の陽解法でやりました。

 

まずプロットする前に入力値に関して考えます。

 

初期位置

xstartystart

初期角度

 θstart

重心速度

 vg

最大角速度

 a

角加速度

ωmax 

角度

  θgoal

オフセット

 Lin,Lout

時間幅

 dt

 

次に途中で使う変数や計算で求めた値について

現在時間

 tnow

現在位置

xnowynow

現在角度

 θnow

現在角速度

 ωnow 

各種の角加速度が変わる時間(便宜上)

 t2,t3,t4,t5

f:id:shinshinmice:20191217191725p:plain

 

めんどくさいの手書きの図で各時間はこれです。えっ…字が汚くて読めない?気合で読んで

 

じゃ、変数を定義はここまでにして計算していきましょう.

 

 

  • 軌道の計算

まずターンの軌道は①オフセット領域,②等角加速度領域(クロソイド曲線),③等角速度領域(円弧),④等角加速度領域,⑤オフセット領域となっています。一つ一つ追っていきましょう。

 

便宜上は下記の図のように置く

 

 

f:id:shinshinmice:20191217191805p:plain

 

全体の計算として,

 
時間をdtずつ足していく
現在の角速度\omega_{now}を求める.
現在角度 \theta_{now}\omega_{now}dtを足していく.
xの座標にv_gdt\sin{ \theta_{now}}を足していく.
yの座標にv_gdt\cos{\theta_{now}}を足していく.

 

という流れを繰り返すものになっています。


①オフセット領域
\omega_{now}dt=\frac{d \theta}{dt}dt=0

これを \theta_{now}に足していく.その \theta_{now}を用いて,

dx=v_gdt\sin{ \theta_{now}}=v_gdtsin{ \theta_{start}}
dy=v_gdt\cos{ \theta_{now}}=v_gdtcos{ \theta_{start}}

これをx_{start}y_{start}に足していく.

終了判定 \sqrt{x_{start}^2+y_{start}^2}=v_gdtL_{in}を超えたとき

 

②等角加速度領域
\frac{d\omega}{dt}=a
\omega=at+C_1
\omega_{now}=\frac{d\theta}{dt}=a\left(t-t_2\right)

これを \theta_{now}に足していく.その \theta_{now}を用いて,

dx=v_gdt\sin{ \theta_{now}}
dy=v_gdt\cos{ \theta_{now}}

これをx_{start},y_{start}に足していく.

終了判定\omega=a\left(t-t_2\right)が\omega_{max}超えたとき,三角加速なら現在角度 \theta_{now}が目標角度 \theta_{goal}の半分を超えたときとか?

 

 

③等角速度領域
\omega_{now}=\frac{d\theta}{dt}=\omega_{max}=const
d\theta=\omega_{max}dt

これを\theta_{now}に足していく.その\theta_{now}を用いて,

dx=v_gdt\sin{\theta_{now}}
dy=v_gdt\cos{\theta_{now}}

これをx_{start},y_{start}に足していく.

終了判定t-t_3\frac{\theta_{goal}-\omega_{max}\left(t_3-t_2\right)}{\omega_{max}}超えたとき,多分自分の台形加速はこんな感じの判定だったきがする.

 

④等角加速度領域
\frac{d\omega}{dt}=-a
\omega=-at+C_1
t=t_4のとき\omega_{now}=\omega_{max}より
\omega_{now}=\frac{d\theta}{dt}=\omega_{max}-a\left(t-t_4\right)
d\theta=\left(\omega_{max}-a\left(t-t_4\right)\right)dt

これを\theta_{now}に足していく.その\theta_{now}を用いて,

dx=v_gdt\sin{\theta_{now}}
dy=v_gdt\cos{\theta_{now}}

これをx_{start},y_{start}に足していく.

終了判定 まぁなんでいいとおもいます。②と同じ長さーとか,任せます。自分は,t-t_4\frac{\omega_{max}}{a}超えたときでしたね。三角加速なら②と同じ長さってしてますね。

 

 

⑤オフセット領域
\omega_{now}dt=\frac{d\theta}{dt}dt=0

これを \theta_{now}に足していく.その \theta_{now}を用いて,

dx=v_gdt\sin{\theta_{now}}=v_gdt\sin{\theta_{start}}
dy=v_gdt\cos{\theta_{now}}=v_gdt\cos{\theta_{start}}

これをx_{start}y_{start}に足していく.

終了判定 \sqrt{x_{start}^2+y_{start}^2}=v_gdtL_{out}を超えたとき

まぁさっきと全く同じです。

 

こんな感じ計算していきました。
今回は細かく領域ごとに説明しました。一般化すると最初に言ったように

時間をdtずつ足していく
現在の角速度\omega_{now}を求める.
現在角度 \theta_{now}\omega_{now}dtを足していく.
xの座標にv_gdt\sin{ \theta_{now}}を足していく.
yの座標にv_gdt\cos{ \theta_{now}}を足していく.

これです。

\omega=\omega\left(t\right)

の関数さえわかっていれば,

dt=const
d \theta=\omega dt
dx=v_gdt\sin{ \theta}
dy=v_gdtcos{ \theta}

を足し続けるの繰り返しです。簡単ですね。多分間違っていない。うん

 

台形加速②の領域を例にすると

 

while (now_t-t2)<=max_angle_speed/angle_acc:              #終了判定

    now_t+=delta_time                                 #時間足していく

    now_angle_speed = angle_acc*(now_t-t2)             #角速度を変える

    now_angle = now_angle + now_angle_speed * delta_time #角速度を角度に足していく

    x += delta_time*g_speed * np.sin(now_angle)          #dxxに足していく

    y += delta_time*g_speed * np.cos(now_angle)           #dyyに足していく

    plt.plot(x, y, marker='.', color="red",markersize=1)        #plotする

    if now_angle-start_angle>=angle/2:          #三角加速など条件あるなら…

        sankaku_mode = 1

        break

 

まぁ正直行列とか使ってないので全く制御工学っぽくなくてすみません。重心速度一定なんでそのままです。

 

 

  • 滑り角を考える?

えっとじゃあ最後に滑り角について今現在自分がやっていることを紹介したいと思います。

 

車体にかかる横方向の力はコーナリングフォースと遠心力があります。基本的に車体は横方向に動かないのでコーナリングフォースは遠心力釣り合っていると考えられます。しかしこの状態にならない場合も存在するらしいです。しかしそんなの低能な自分には無理なので簡単のためコーナリングフォースは遠心力釣り合うと仮定します。次にコーナリングフォースとスリップ角の線形モデルを用います。簡単のためコーナリングフォースとスリップ角は比例と仮定します。実際にはなんかやばい式があります。Magic Formulaモデル?みたいなのがあるらしいです。式見ましたけどあーニュートンラプソンのコードは作ったことあるからできなくはないが…めんどい無理。まぁ以上をまとめると,コーナリングフォースFは,

F=mv\omega

となり,スリップ角は,

eta=CF=Cmv\omega

となります.ここでのCは比例定数です。このスリップ角を \theta_{now}に引きます。

now_angle=now_angle+now_angle_speed*delta_time-K*m*g_speed*now_angle_speed#mいらな

さっきの一文をこんな感じで変えました。正直多くの近似をしているため速度が遅いと合う気がしますが速くなるとやばそう…これあってますかね?マイクロマウス界隈のいろんな人に質問したですね。自分のは自己流すぎて全く自信がありません。


以上がターンシミュレータのお話でした。

明日の記事は枝ピロさんの「今年度機体の反省」です。
僕のもう一つのカレンダーでも反省を書いたんですけどやっぱり反省で大事ですよね。人に反省を見て自分も次の機体で頑張りたいですね。