ライントレースロボットのシミュレーション

はじめに

ロボットランサーはじめました（昔ばなし）

機械学会主催のロボットグランプリのロボットランサー競技と言うトラック上の白線コース上をロボットが走行して、トラック周辺にある的を槍で突くことで点数が加算され、制限時間でトラックを周回する中で点数を競い合う競技がありました。

その競技に1999年に参加しました。

当時修士の学生（既に社会人でした）だった僕は隣の学部のロボットの講義などをつまみ食いしていた時だったと思います。同じ学部の同期がニヤニヤしながらロボットランサーのルールブックを持ってきました。

「でない？」

「でる！」

この瞬間が、その後の僕の生きる方向性を大幅に変えることになるとは全く思っていませんでした。

槍騎兵なので「YariYari」と名付けました。ロボット開発と並行してホームページを作り始めました。後でそのホームページの内容の一部は書籍で紹介されました。当時はホームページ事態が珍しかったのか本の編集者の目に留まったのかと思います。

僕のホームページ「こうへいのホームページ」はもうなくなってしまいましたが、たまに競技の懇親会等で「見てました」とお声がけされるとすこぶる嬉しいです。

最初にやったのは、秋月電子にH8マイコンを買いに行きました。ラジコンサーボモータを接続して、アセンブラを勉強しタイマーカウンタを駆使ししてサーボ用のPWM信号を作って、はじめてサーボが動いたときは嬉しかったですよ。

ロボットやってる人は何かしらこのような瞬間があるのだろうと思います。

友人と一緒に学校の屋上に行って買って来たベニヤ板に黒いペンキを塗って、コース全長を作りました。

学校の製図室にコース展開して、夜中までデバッグ。

ほんとに楽しかった。

大会前日は徹夜で一睡もしないで朝を迎えフラフラになりながら友人が車を運転してくれて会場に行きました。たしか、大会が地元開催だったんだと思います。

あまり覚えていないのですが、結局コースアウトして終わったような気がします。

その後、打ち上げは友人の奥さんも交えて３人でカラオケで「飛べガンダム」を熱唱しました。寝てないのでかなりハイテンションでしたね。

この年は１９３こと土井さんが優勝。でも実は自分たちの番が終わったらすぐ帰ったので覚えていません。（すまぬ）まあ、よわよわの1年目はそのような雲の上の人の成績を気にしている余裕は無かったです。次の年からは過去の成績分析しましたけど。

２年目、そして、びっくり

2000年3月、修士課程が終わって、それぞれの任地に再び配属されることになったので友人とはバラバラになりました。

僕は青森の下北半島に配置になりました。ずいぶん遠くです。そこで日夜「ドッカン、ドッカン」の実験を繰り広げていました。死を覚悟しろと言われていましたね。

今年は結果を残そうとYariYari2000の開発に着手しました。ホームセンターにボール盤買いに行ったり、この時期に家に道具が充実してきました。

修士時代研究室で一緒に勉強会でお話を聞かせて頂いた日産の４WSや、研究室で話題にしていたミサイルの双翼操舵に影響を受けて、YariYari2000は４WDで４WSにしようと決心しました。

そんなんで出来上がったのがYariYari2000です。

昨年とった写真なのでH8-3052などは取れてます。車体を作る技術が無いので田宮の高級ラジコンシャーシ（TA03だったか？）を２個一して前後のステアリングを実現しました。

各部はこんな感じです。

シャシはキットそのままでは長すぎたので高価なショートホイールベースのCFRPのフレームを買ってみたのですが、それも長すぎたので最後はアルミで自作しました。ボール盤、糸鋸、やすりがけの大作です（笑）

写真には無いのですが、後ろに計測用のエンコーダ輪を引っ張ってました。走行距離と速度の制御に使っていました。

センサーはマイクロマウスの森永さんのセンサー回路を使わせてもらいました。何分素人だったので回路もこのころからきっちり勉強し始めました。

当時、ラインの位置をデジタル的にとることも考えましたが、それでは制御がかくかくしてしまい高速には走れないと判断しましたので、センサーは2個のペアでアナログ的にラインの位置を検知して、うまくセンサの組の中心からの距離を足し算して、センサーモジュール一杯の幅でラインの位置を連続的に取得できました。

これ作っているときは、車に乗ると中央ラインがトレースラインに見えるんですよ。どうしたらうまくトレースできるのか考えながら運転してた。危ないですね。

結果はびっくりしました。できすぎでした。ロボコンマガジンの表紙にも出ました。ほんとうは土井さんのために用意された表紙だったと思います。（すまぬ）

動画は（既出ですが）その年のは無くて、次の年の動画です。次の年は成績は少し残念だったんですが、走りは良い走りです。モータを強力なものに変えたのでモータドライバが追い付かず、性能を発揮できていなかったのが心残りでした。

動画の最後の観客のどよめきは開発していた当時も「これ上手くいったら受けるだろうな」と実は思っていて、少しばかし快感でした。

YariYari2000F

前置きが長かったですが

前置きが長かったですが、前回、壁沿い走行のシミュレーションプログラム作ったのでそれを改造してライントレースのシミュレーションしてみようというのが今日のお話です。

ライントレースの話をしようと思ったので、ライントレースに取り組んだ昔話をしたくなり長くなりました。

rikei-tawamure.com

はじめに
シミュレーションでのラインとセンサの模擬について
シミュレーション計算式
シミュレーション
- 比例制御の速度による変化
  - 1m/s
  - 3m/s
  - 5m/s
- 比例微分制御による改善
  - 5m/s
  - 10m/s
横滑りを考慮したモデルでのシミュレーション
- 計算モデル
  - コーナリングパワーとサイドフォース
  - 横滑りを含む運動の計算式
- 横滑りモデルによるシミュレーション
おわりに
付録
- 横滑りを考慮しない場合
- 横滑りを考慮する場合

シミュレーションでのラインとセンサの模擬について

シミュレーションの中でラインをどう表現して、センサの位置とでラインとの距離関係をどう表現するかを考えてみます。

今回のシミュレーションでは直線と円弧を結ぶコースを想定します。

円弧とセンサ位置

次の図のように円弧をトレースしているところを想像してみます。

センサモジュールの傾きなどは無視してしまうと、センサの位置とラインとの位置誤差は、センサと円弧中心との距離から円弧の半径を引いたものになります。この方法は誤差の正負でラインのどちら側にいるかもわかるので便利です。

シミュレーションではこの様に円弧をトレースしているときの誤差の計算を行いたいと思います。センサの位置の計算については、前回の壁沿い走行の際にも説明しましたので、そちらを参照ください。

直線を巨大な円弧で近似する

直線との位置誤差についてはセンサ位置と直線の距離を求めることで誤差を算出できます。しかし、距離は必ず正で求まるので改めてラインの左か右か判定する必要があります。

そこで今回は少し変わった試みとして、直線は半径が大きな円の一部であるとしてみました。これで誤差計算アルゴリズムを変えなくても良くてすっきりします。

ちなみに半径のそこそこ大きい円弧で半径の小さい円弧を繋いだイメージが次です。

次に半径の非常に大きい円弧で半径の小さい円弧を繋いだイメージが次です。

円弧を円弧で繋いでいるのですが、円弧を直線で結んでいるように見えます。

今回はこの方法でラインとセンサの位置関係を扱います。

シミュレーション中のラインの切り替え

実際の走行ではそこにラインが書かれているのでラインの切り替えなど考えないのですが、シミュレーションでは仮想空間で走行しているのでセンサの状態に応じてラインの数式を切り替えていく必要があります。

今回の計算では、下図のように４つのエリアを設けて、センサの位置が①のエリアに入ったときには、青い円弧ラインとセンサとの距離を計算し、センサが②のエリアに入ったときは、オレンジ色の直線ラインとセンサとの距離を計算します、他のラインに関しても同様にします。

シミュレーション計算式

壁沿い走行の時はモータの応答を考慮した遅れを無視しましたが、今回は１次遅れで表現してみます。

以下の式を用いて計算を行います。微分方程式はルンゲ・クッタ法を用いて計算します。

センサとロボットの位置

これは、壁沿い走行で記述したものと同じです。

$\begin{eqnarray} x_s = x_{ms} \cos \psi - y_{ms} \sin \psi + x_m\\ y_s = x_{ms} \sin \psi + y_{ms} \cos \psi + y_m\\ \\ \end{eqnarray}$

記号	意味
$x_s$	センサ位置のｘ座標
$y_s$	センサ位置のｙ座標
$x_{ms}$	ロボット位置のｘ座標
$y_{ms}$	ロボット位置のｙ座標

センサとラインとの位置誤差

一つ前のところでお話しした事を数式化しました。

$\begin{eqnarray} e = \sqrt{(x_s - x_o)^2 +　(y_s - y_o)^2　} - r_l\\ \\ \end{eqnarray}$

記号	意味
$e$	位置誤差
$r_l$	ラインの半径
$x_o$	円弧の中心のx座標
$y_o$	円弧の中心のu座標

ロボットの位置

$\begin{eqnarray} \dot{x}_m = V \cos \psi \\ \dot{y}_m = V \sin \psi \\ \\ \end{eqnarray}$

記号	意味
$V$	ロボットの速度（定数）
$\psi$	ロボットの角度

ロボットの角度

$\begin{eqnarray} \dot{\psi} = \omega \\ \end{eqnarray}$

記号	意味
$\omega$	ロボットの角速度

ロボットの角速度と角速度指令（１次遅れ）

$\begin{eqnarray} \dot{\omega}=\frac{-\omega+\omega_{com}}{\tau}\\ \\ \end{eqnarray}$

記号	意味
$\omega_{com}$	指令角速度
$\tau$	回転運動の時定数

制御則

制御則は比例制御と比例微分制御の二つを試します。

比例制御

$\begin{eqnarray} \omega_{com} = K_p e\\ \\ \end{eqnarray}$

記号	意味
$K_p$	比例ゲイン

比例微分制御

微分は近似微分を用います

$\begin{eqnarray} \omega_{com} = K_p e_n + K_d \frac{e_n - e_{n-1}}{T_c}\\ \\ \end{eqnarray}$

記号	意味
$K_d$	微分ゲイン
$e_n$	現時点の位置誤差
$e_{n+1}$	１制御周期前の位置誤差
$T_c$	制御周期

シミュレーション

センサーはロボットの重心位置から前方に15cm前にあるとします。

制御周期は１ｍｓです。

角速度制御系の時定数をロボットの質量やモータの時定数などを想定して３５ｍｓとしてみました。この値は軽量化したり、ギヤ比を大きくしたりすると小さくできます。

まずは比例制御で速度を徐々に上げていきます。

比例制御の速度による変化

比例ゲイン１２５０にしたうえで、比例制御で速度を上げていきます。

1m/s

上から、センサのy座標、ロボットの角度、ロボットの角速度とその指令値、ラインとの位置誤差、走行軌跡です。

この速度だと余裕ですね。

ですが、ゲインが大きいのか誤差は振動的に出ています。

3m/s

速度３m/sです。そろそろ限界が近そうです。

5m/s

発散してますね。

比例微分制御による改善

比例制御だけでは限界なので比例微分制御に切り替えます。

比例ゲインを６０００、微分ゲインを１１０．７にしました。この微分ゲインの決定方法は壁沿い走行と同じ方法で行いました。

5m/s

劇的に改善されました。角速度指令に対して角速度がなまって応答していますので、微分ゲインは小さくても良いのかもしれません。

10m/s

速度を１０ｍ/sにしてみましたが、この場合でもうまくいきます。

円弧の半径が１ｍと大きいのだけれど、実機でこの速度で回れるかは疑問がのこります。

横滑りを考慮したモデルでのシミュレーション

ここまでの結果は実はかなりの速度でも周回できてしまいまして、ちと現実的ではありませんでした。

そこで横滑りを考慮したシミュレーションに切り替えてみました。

rikei-tawamure.com rikei-tawamure.com

計算モデル

コーナリングパワーとサイドフォース

$\begin{eqnarray} F_y&=&-K \beta\\ \\ \end{eqnarray}$

記号	意味
$F_y$	サイドフォース[N]
$K$	コーナリングパワー[N/rad]
$\beta$	横滑り角[rad]

コーナリングパワー $K$ はタイヤの性能で単位横滑り当たりのタイヤのサイドフォースを表して大きければ大きいほどがっちりグリップしてロボットをガシガシ曲げてくれます。これを計算に加えたことで、高速移動ロボットで最も大事な部品であるタイヤの要素を考慮することができます。

横滑りを含む運動の計算式

$\begin{eqnarray} \dot \beta &=& - \frac{K}{mV} \beta -\omega \\ \dot \psi &=& \omega\\ \dot X &=& V\cos( \psi + \beta)\\ \dot Y &=& V \sin ( \psi + \beta )\\ \end{eqnarray}$

センサーの位置の計算や角速度の制御則は同じです。

横滑りモデルによるシミュレーション

今回のモデルには質量がありますので５００ｇに設定してみました。制御系の応答も時定数３５ｍｓに設定しています。コーナリングパワーは２０N/radでまず始めてみました．その他の設定は横滑りを考慮しない場合と同じです。

10m/sの場合

このコーナリングパワーのでも実機ではかなり大きな値だと思いますが、この速度では計算でも回れない事が判ります。まえシミュレーションでは回れてしまって、どうも腑に落ちませんでしたが、タイヤのサイドフォースを計算に反映されているので、らしい感じになったと思っています。

グラフについてですが、一番上のグラフをy軸方向の位置ysの代わりに横滑り角Betaに変えています。また５番目をサイドフォースにしてます。

速度をさげて、回れるところを探ってみます。

5m/sの場合

6m/sまで下げましたが、コースアウトしましたので。 5m/sでやってみたらうまくいきました。

センサーはしっかりラインをとらえながら、機体は大回りしている感じで、実機のような感じに計算できているのではないでしょうか。

さらに速度を落として1m/sの場合

ゆっくり着実に回ってますね。

最後にどこまでコーナリングパワーを大きくすれば10m/sを回るのか

この計算の様にコーナリングパワーを大きくすることができれば、早い速度でも回れるという事です。

コーナリングパワーはグリップの良いタイヤや垂直荷重を大きくすることで大きくできると思いますので実機でここまではいけないかもしれませんが、最近はダウンフォースを大きくする様々な工夫がありますから、もしかしたら行けるかなあ。

おわりに

今回は解析的な話にはあまり踏み込まず「計算してみた」的な感じにしました。

実物との対比をしてシミュレーションがどのくらい「らしい」のか考察してみたいですね。ライントレースロボットを久々に作って検証できると良いですね。

比例制御だけでは限界があり、微分制御を入れるとよくなる傾向はうまく出ていると思います。

やはり、横滑り運動を考慮することで現実的なシミュレーションになったと思っています。横滑りを組み入れた計算をしなければならないことがこれで判りました。

今回はゲインの決定居ついてほとんど触れていませんが、ゲインの設定によっては比例微分でも発振したりもしています。改めてゲインの決定についても書きたいと思っています。

計算機シミュレーションではロボットの運動の計算も難しいですが環境をどう表現するかもかなり難しく、ライントレースロボットのラインにしても存外やってみると頭を悩ますののです。今回はすべて円弧で近似する感じで統一感をもってプログラムにできました、

しかし、ラインの切り替えについてもう少しエレガントにできないものかなあ。

そのうちマイクロマウスのシミュレーションもするつもりですが、壁の表現とセンサの計算についてがやはり課題です。

それでは今回はこれで終わります。

また次回！

付録

シミュレーションのプログラムです。

横滑りを考慮しない場合

%matplotlib inline
#%matplotlib widget
import matplotlib.pyplot as plt
import numpy as np
from tqdm.notebook import tqdm as tqdm #Jupyter labの場合
#from tqdm import tqdm

'''
def rk4(func, t, h, y, *x)
ルンゲ・クッタ法を一回分計算する関数
    引数リスト
    func:導関数
    t：現在時刻を表す変数
    h：刻み幅
    y：出力変数（求めたい値）
    *x:引数の数が可変する事に対応する、その他の必要変数
※この関数では時刻は更新されないため、これとは別に時間更新をする必要があります。
'''
def rk4(func, t, h, y, *x):
    #print(t,h,y, *x, y)
    k1=h*func(t, y, *x)
    k2=h*func(t+0.5*h, y+0.5*k1, *x)
    k3=h*func(t+0.5*h, y+0.5*k2, *x) 
    k4=h*func(t+h, y+k3, *x)
    y=y+(k1 + 2*k2 + 2*k3 + k4)/6

    return y

'''
導関数の書き方
def func(t, y, *state):
    func:自分で好きな関数名をつけられます
    t:時刻変数(変数の文字はtで無くても良い) 
    y:出力変数(変数の文字はyで無くても良い)
    *state:その他の必要変数(引数の数は可変可能))
#関数サンプル
def vdot(t, y, *state):
    s1=state[0]
    s2=state[1]
    return t+y+s1+s2
    
'''

#移動ロボット緒言
def robot_spec():
    #ロボット速度[m/s]
    V=3
    #センサ位置ｘ[m]
    xms=0.15
    #センサ位置y[m]
    yms=0.0
    return V, xms, yms

#以下ロボットの位置と速度を計算するためルンゲクッタソルバに渡す導関数
def omegadot(t, omega, omegaref):
    tau=0.035
    return -omega/tau+omegaref/tau

def psidot(t, psi, omega):
    return omega

def xmdot(t, xm, psi):
    V, xms,yms=robot_spec()
    return V*np.cos(psi)

def ymdot(t, ym, psi):
    V, xms,yms=robot_spec()
    return V*np.sin(psi)

def _xs(xm,ym,xms,yms,psi):
    return np.cos(psi)*xms - np.sin(psi)*yms + xm

def _ys(xm,ym,xms,yms,psi):
    return np.sin(psi)*xms + np.cos(psi)*yms + ym


#ログ変数初期化
Xs=[]
Ys=[]
Xm=[]
Ym=[]
Psi=[]
Psi_dot=[]
Psi_dot_com=[]
Minls=[]
T=[]

#ゲイン設定
V,xms,yms=robot_spec()
#Kp=6000
#Kd=(Kp*xms-2*np.sqrt(Kp*V))/V
#print(Kd)
Kp=1250
Kd=0

#コース
r1=1
x10=0
y10=0
x20=-2.5
y20=-1e5
x30=-5
y30=0
x40=-2.5
y40= 1e5
r2=np.sqrt((x20-0)**2+(y20-r1)**2)
r3=r1
r4=r2

#状態変数等の初期化
t=0.0
xm=0
ym=r1
psi=np.pi
omega=0
yref=1.0
xs=_xs(xm,ym,xms,yms,psi)
ys=_ys(xm,ym,xms,yms,psi)


#センサがあるエリアで場合分け
if xs>=0:
    s1=np.sqrt((xs-x10)**2+(ys-y10)**2)
    ls1=s1-r1
    minls=ls1
elif xs<=-5.0:
    s3=np.sqrt((xs-x30)**2+(ys-y30)**2)
    ls3=s3-r1
    minls=ls3  
elif ys>0:
    s2=np.sqrt((xs-x20)**2+(ys-y20)**2)
    ls2=s2-r2
    minls=ls2
else:
    s4=np.sqrt((xs-x40)**2+(ys-y40)**2)
    ls4=s4-r2

psi_dot_com=Kp*minls
oldminls=minls

#刻み幅
h=1e-5

#制御周期
Cpriod=1e-3
_Cpriod=Cpriod/h

#計算時間(s)
TrackLength=16 #[m]
Tcalc=TrackLength/V
N=int(Tcalc/h)


### メインループ ###
for n in  tqdm(range(N)):
    Xm.append(xm)
    Ym.append(ym)
    Xs.append(xs)
    Ys.append(ys)
    Psi.append(psi*180/3.14159)
    Psi_dot.append(omega*180/3.14159)
    Psi_dot_com.append(psi_dot_com*180/3.14159)
    Minls.append(minls)
    T.append(t)

    #ライン検知
    if n%_Cpriod==0:
        #センサがあるエリアで場合分け
        if xs>=0:
            s1=np.sqrt((xs-x10)**2+(ys-y10)**2)
            ls1=s1-r1
            minls=ls1
        elif xs<=-5.0:
            s3=np.sqrt((xs-x30)**2+(ys-y30)**2)
            ls3=s3-r1
            minls=ls3
        elif ys>0:
            s2=np.sqrt((xs-x20)**2+(ys-y20)**2)
            ls2=s2-r2
            minls=ls2
        else:
            s4=np.sqrt((xs-x40)**2+(ys-y40)**2)
            ls4=s4-r2
            minls=ls4
        
        #制御
        psi_dot_com=Kp*minls+Kd*(minls-oldminls)/Cpriod
    
    #状態変数保存    
    oldminls=minls
    oldpsi=psi
    oldys=ys
    oldym=ym
    oldxm=xm
    oldomega=omega
    
    #ルンゲ・クッタ呼び出し
    xm  = rk4(xmdot, t, h, oldxm, oldpsi)
    ym  = rk4(ymdot, t, h, oldym, oldpsi)
    omega=rk4(omegadot, t, h, oldomega, psi_dot_com)
    psi = rk4(psidot, t, h, oldpsi, oldomega)

    #センサ位置算出
    xs=_xs(xm,ym,xms,yms,psi)
    ys=_ys(xm,ym,xms,yms,psi)
    
   #時間更新
    t=t+h

Xm.append(xm)
Ym.append(ym)
Xs.append(xs)
Ys.append(ys)
Psi.append(psi*180/3.14159)
Psi_dot.append(omega*180/3.14159)
Psi_dot_com.append(psi_dot_com*180/3.14159)
Minls.append(minls)
T.append(t)

plt.figure(figsize=(11,16)) 

#ysのグラフ
plt.subplot(511)
plt.plot(T[::10], Ys[::10])
plt.ylabel('ys[m]')
plt.grid()

#角度のグラフ
plt.subplot(512)
plt.plot(T[::10], Psi[::10])
plt.ylabel('Psi[deg]')
plt.grid()

#角速度のグラフ
plt.subplot(513)
plt.plot(T[::10], Psi_dot[::10], label='Psi_dot')
plt.plot(T[::10], Psi_dot_com[::10], label='Command')
plt.xlabel('Time[s]')
plt.ylabel('PsiDot[deg/s]')
plt.legend(loc='upper left')
plt.grid()

#誤差のグラフ
plt.subplot(514)
plt.plot(T[::10], Minls[::10])
plt.xlabel('Time[s]')
plt.ylabel('Error[m]')
plt.grid()

#走行軌跡のグラフ
plt.subplot(515)
th1=np.linspace(-np.pi/2,np.pi/2,100000)
x1=r1*np.cos(th1)
y1=r1*np.sin(th1)
x20=-2.5
y20=-1e4
r2=np.sqrt((x20)**2+(y20-1.0)**2)
tmp=np.arcsin(2.5/r2)
th2=np.linspace(np.pi/2-tmp, np.pi/2+tmp, 100000)
x2=r2*np.cos(th2)+x20
y2=r2*np.sin(th2)+y20

plt.plot(x1, y1, c='b', label='Line')
plt.plot(x2, y2, c='b')
plt.plot(x2, -y2, c='b')
plt.plot(-x1-5, y1, c='b')

plt.plot(Xm[::10], Ym[::10], label='CG')
plt.plot(Xs[::10], Ys[::10], label='Sensor')

plt.xlabel('X[m]')
plt.ylabel('Y[m]')
plt.legend(loc='center')
plt.grid()

plt.show()

横滑りを考慮する場合

%matplotlib inline
#%matplotlib widget
import matplotlib.pyplot as plt
import numpy as np
from tqdm.notebook import tqdm as tqdm
#from tqdm import tqdm

'''
def rk4(func, t, h, y, *x)
ルンゲ・クッタ法を一回分計算する関数
    引数リスト
    func:導関数
    t：現在時刻を表す変数
    h：刻み幅
    y：出力変数（求めたい値）
    *x:引数の数が可変する事に対応する、その他の必要変数
※この関数では時刻は更新されないため、これとは別に時間更新をする必要があります。
'''
def rk4(func, t, h, y, *x):
    #print(t,h,y, *x, y)
    k1=h*func(t, y, *x)
    k2=h*func(t+0.5*h, y+0.5*k1, *x)
    k3=h*func(t+0.5*h, y+0.5*k2, *x) 
    k4=h*func(t+h, y+k3, *x)
    y=y+(k1 + 2*k2 + 2*k3 + k4)/6

    return y

'''
導関数の書き方
def func(t, y, *state):
    func:自分で好きな関数名をつけられます
    t:時刻変数(変数の文字はtで無くても良い) 
    y:出力変数(変数の文字はyで無くても良い)
    *state:その他の必要変数(引数の数は可変可能))
#関数サンプル
def vdot(t, y, *state):
    s1=state[0]
    s2=state[1]
    return t+y+s1+s2
    
'''
#以下ロボットの位置と速度を計算するためルンゲクッタソルバに渡す導関数

def robot_spec():
    #移動ロボット
    V=5.0
    xms=0.15
    yms=0.0
    K=20
    m=0.5
    return V, xms, yms, K, m

def omegadot(t, omega, omegaref):
    tau=0.035
    return -omega/tau+omegaref/tau

def psidot(t, psi, omega):
    return omega

def xmdot(t, xm, psi, beta):
    V, xms,yms,K,m=robot_spec()
    return V*np.cos(psi+beta)

def ymdot(t, ym, psi, beta):
    V, xms,yms,K,m=robot_spec()
    return V*np.sin(psi+beta)

def betadot(t, beta, omega):
    V, xms,yms,K,m=robot_spec()
    return -K*beta/m/V - omega
    

def _xs(xm,ym,xms,yms,psi):
    return np.cos(psi)*xms - np.sin(psi)*yms + xm

def _ys(xm,ym,xms,yms,psi):
    return np.sin(psi)*xms + np.cos(psi)*yms + ym


#変数初期化
Xs=[]
Ys=[]
Xm=[]
Ym=[]
Psi=[]
Psi_dot=[]
Psi_dot_com=[]
Beta=[]
Minls=[]
Sideforce=[]
T=[]

V, xms,yms,K,m=robot_spec()

#Kp=6000
Kp=6000
Kd=(Kp*xms-2*np.sqrt(Kp*V))/V
#Kd=20
print(Kd)
#Kp=1250
#Kd=0

#コース
#円弧部の半径
r1=1.0

x10=0
y10=0
x20=-2.5
y20=-1e5
x30=-5
y30=0
x40=-2.5
y40= 1e5
r2=np.sqrt((x20-0)**2+(y20-r1)**2)
r3=r1
r4=r2

#状態変数初期化
t=0.0
xm=0.0
ym=r1
psi=np.pi
beta=0
omega=0
yref=1.0
xs=_xs(xm,ym,xms,yms,psi)
ys=_ys(xm,ym,xms,yms,psi)
sideforce=0


#センサがあるエリアで場合分け
if xs>=0:
    s1=np.sqrt((xs-x10)**2+(ys-y10)**2)
    ls1=s1-r1
    minls=ls1
elif xs<=-5.0:
    s3=np.sqrt((xs-x30)**2+(ys-y30)**2)
    ls3=s3-r1
    minls=ls3  
elif ys>0:
    s2=np.sqrt((xs-x20)**2+(ys-y20)**2)
    ls2=s2-r2
    minls=ls2
else:
    s4=np.sqrt((xs-x40)**2+(ys-y40)**2)
    ls4=s4-r2

psi_dot_com=Kp*minls
oldminls=minls


#刻み幅
h=1e-5

#制御周期
Cpriod=1e-3

_Cpriod=Cpriod/h


#計算時間(s)
#コース一周の時間より少し少ない時間
Tcalc=(10+2*r1*np.pi-0.2)/V
N=int(Tcalc/h)

###メインループ
for n in  tqdm(range(N)):
    Xm.append(xm)
    Ym.append(ym)
    Xs.append(xs)
    Ys.append(ys)
    Psi.append(psi*180/3.14159)
    Psi_dot.append(omega*180/3.14159)
    Psi_dot_com.append(psi_dot_com*180/3.14159)
    Beta.append(beta*180/3.14159)
    Minls.append(minls)
    Sideforce.append(sideforce)
    T.append(t)

    #ライン検知
    if n%_Cpriod==0:
        #センサがあるエリアで場合分け
        if xs>=0:
            xo=x10
            yo=y10
            rc=r1
        elif xs<=-5.0:
            xo=x30
            yo=y30
            rc=r1
        elif ys>0:
            xo=x20
            yo=y20
            rc=r2
        else:
            xo=x40
            yo=y40
            rc=r2
            
        sc=np.sqrt((xs-xo)**2+(ys-yo)**2)
        minls=sc-rc
        
        
        #制御
        psi_dot_com=Kp*minls+Kd*(minls-oldminls)/Cpriod
    
    
    oldminls=minls
    oldpsi=psi
    oldbeta=beta
    oldys=ys
    oldym=ym
    oldxm=xm
    oldomega=omega
    
    xm  = rk4(xmdot, t, h, oldxm, oldpsi, oldbeta)
    ym  = rk4(ymdot, t, h, oldym, oldpsi, oldbeta)
    omega = rk4(omegadot, t, h, oldomega, psi_dot_com)
    beta = rk4(betadot, t, h, oldbeta, oldomega)
    psi = rk4(psidot, t, h, oldpsi, oldomega)

    
    xs=_xs(xm,ym,xms,yms,psi)
    ys=_ys(xm,ym,xms,yms,psi)
    
    sideforce=-K*beta
    
    t=t+h
    #print('i omega',i,omega,e,TL)
    
    
Xm.append(xm)
Ym.append(ym)
Xs.append(xs)
Ys.append(ys)
Psi.append(psi*180/3.14159)
Psi_dot.append(omega*180/3.14159)
Psi_dot_com.append(psi_dot_com*180/3.14159)
Beta.append(beta*180/3.14159)
Minls.append(minls)
Sideforce.append(sideforce)
T.append(t)

plt.figure(figsize=(11,22))    
plt.subplot(611)
plt.plot(T[::10], Beta[::10])
plt.ylabel('Beta[deg]')
#plt.legend()
plt.grid()

plt.subplot(612)
plt.plot(T[::10], Psi[::10])
plt.ylabel('Psi[deg]')
plt.grid()

plt.subplot(613)
plt.plot(T[::10], Psi_dot[::10], label='Psi_dot')
plt.plot(T[::10], Psi_dot_com[::10], label='Command')
#plt.xlabel('Time[s]')
plt.ylabel('PsiDot[deg/s]')
plt.legend(loc='upper left')
plt.grid()

plt.subplot(614)
plt.plot(T[::10], Minls[::10])
#plt.xlabel('Time[s]')
plt.ylabel('Error[m]')
plt.grid()

plt.subplot(615)
plt.plot(T[::10], Sideforce[::10])
plt.xlabel('Time[s]')
plt.ylabel('Side Force[N]')
#plt.ylim(0, 100)
plt.grid()

plt.subplot(616)

th1=np.linspace(-np.pi/2,np.pi/2,1000000)
x1=r1*np.cos(th1)
y1=r1*np.sin(th1)
x20=-2.5
y20=-1e4
r2=np.sqrt((x20)**2+(y20-r1)**2)
tmp=np.arcsin(2.5/r2)
th2=np.linspace(np.pi/2-tmp, np.pi/2+tmp, 1000000)
x2=r2*np.cos(th2)+x20
y2=r2*np.sin(th2)+y20

plt.plot(x1, y1, c='b', label='Line')
plt.plot(x2, y2, c='b')
plt.plot(x2, -y2, c='b')
plt.plot(-x1-5, y1, c='b')

plt.plot(Xm[::10], Ym[::10], label='CG')
plt.plot(Xs[::10], Ys[::10], label='Sensor')


plt.xlabel('X[m]')
plt.ylabel('Y[m]')
plt.legend(loc='center')
plt.grid()

plt.show()