２輪差動式移動ロボットの横滑り運動について

マイクロマウスの横滑り運動のアイキャッチ画像

はじめに

マイクロマウスの運動についてタイヤの力学を含めて考えてみたいと思います。

マイクロマウスを完全な６自由度の運動をすると思うと、考察がかなり厳しくなるので、今回は平面内での３自由度に限定して考えていきたいと思います。

その中で横滑りのメカニズムを解き明かせればと思いますがはたしてどうなりますでしょうか。

はじめに
剛体の運動方程式
- ６自由度の運動方程式
- ３自由度の運動方程式
マイクロマウスの横滑りについて
走行シミュレーション
おわりに
参考文献
付録　シミュレーションソースコード
- 運動方程式を解いてスラロームを計算するバージョン
- 簡易スラローム計算バージョン

剛体の運動方程式

６自由度の運動方程式

いずれ剛体の運動方程式を最初からひも解いてみたいと思いますが、今日は省略して結論からのべます。いつもは数式を $\TeX$ 書式で手打ちしていますが剛体の６自由度の運動方程式を成分ごとに記述すると長いので今日は手書きしたのを暫定的に貼らせていただきます。長くて大変ですよね。これらの式は座右の書である加藤寛一郎先生の「航空機力学入門」からの写しです。ベクトルで記述すると次のようにたった２本の式で書けてすっきりするんですが、それを展開すると上のようになるわけです。

$\displaystyle{ m(\frac{d^* \vec{V_c}}{dt} + \vec \omega \times \vec{V_c})= \vec F }$

$\displaystyle{ \frac{d^* \vec{h}}{dt} + \vec \omega \times \vec h= \vec M }$

ここで $\vec{V_c}$ は剛体の重心周りの速度、 $\vec{h}$ は角運動量、 $\vec \omega$ は角速度です。

３自由度の運動方程式

そこで、思い切ってマイクロマウスは平面にへばりついてピッチングやローリングや上下に動いたりしないとしてしまいましょう。すると上の書き下しは３本無くなり、各項もロール（ｐ）、ピッチ（ｑ）、上下運動（ｗ）は無くなり下のように削除できます。

３自由度の運動方程式

残った式は清書しますね。

$\begin{eqnarray} m (\dot u - r v)&=& F_x\\ m (\dot v + r u )&=&F_y\\ I_{zz} \dot r &=&N\\ \end{eqnarray}$

上式において $m$ は質量、 $I_{zz}$ はＺ軸周りの慣性モーメント、 $u$ はＸ軸方向速度成分、 $v$ はＹ方向速度成分、 $r$ はヨーレートです。

さらに外力については $F_x$ がＸ軸方向の力、 $F_y$ がＹ軸方向の力、 $N$ がヨーモーメントです。

マイクロマウスの座標は前後方向がＸ軸、Ｘ軸に対して左方向がＹ軸方向です。また、上から見て反時計回りが回転の正方向とします。

またこれらの運動方程式はロボットに固定された座標系に対するものです。マイクロマウスの走行軌跡を描きたいときは、これらの運動方程式を解いて速度や角速度が求められたとしても、座標変換をして迷路座標系における速度になおしてから積分して位置を計算する必要があります。ここでも座標変換は重要となります。

rikei-tawamure.com

マイクロマウスの横滑りについて

横滑りと横滑り角

さて、 $u$ は前後方向の速度ですのでわかりますがマイクロマウスに横方向の速度 $v$ はあるのでしょうか？

本記事のアイキャッチ画像に横方向の速度と、縦方向の速度を合成した斜めの速度が描かれています。その図の $\beta$ は横滑り角と呼ばれるものです。

タイヤの発生する力

ここでタイヤについての考察をしなければ謎は解けないので、タイヤについてお話します。タイヤは図のようにタイヤの回転面方向に対して進行方向が傾いたときの角度を横滑り角またはスリップ角と呼びます。これはマイクロマウスの図の横滑りと同じです。

横滑り角が発生すると図のように横方向の力が発生します。横力の速度方向に垂直な成分がコーナリングフォースで水平成分がコーナリング抵抗です。これは航空工学における揚力と抗力と同じような見方で楽しいですね。コーンリングフォースは横滑り角の関数となりますが、一番簡単なモデルは比例モデルです。 横滑り角にコーンリングフォースが比例するというものです。このモデルを当てはめて運動方程式を解いて、マイクロマウスの走行軌跡にどのような影響があるのか検討してみたいと思います。ちなみに、コーナリングフォースは物体が動いている方向とは逆向きに発生する動摩擦と同じものです。

横滑り発生のメカニズム

ここで運動方程式を変形して以下のように微分＝の形にします。

$\begin{eqnarray} \dot u &=& \frac{F_x}{m} + rv\\ \dot v &=&\frac{F_y}{m} - ru\\ \dot r &=&\frac{N}{I_{zz}}\\ \end{eqnarray}$

ここでマイクロマウスが横滑りを起こすメカニズムを解き明かしたいと思います．

ロボットが旋回するとヨーレート $r$ が発生します。すると上の運動方程式の２式目の $ru$ は０ではなくなります。そのため上式の２式目の横力 $F_y$ が０であっても $\dot v$ は０とならず，横方向の速度 $v$ が発生し、これが横滑りです。横滑りが発生すると今度はタイヤが横力を発生します。下の図は正の旋回をしようとしたときの，横滑りが発生しそれにより横力が発生する流れを図示してみました。

横滑りが発生するのは旋回するためのヨーモーメントによるヨーレート発生がきっかけとなります，見かけの力である遠心力の正体はこれです．そして、オレンジ色の速度ベクトルは旋回方向とは逆を向いているのを奇妙に感じるかもしれません．これは、この図がロボット座標系に注目しているせいです．この座標は正のヨーレートが発生した場合、反時計方向に回転します。それに伴ってオレンジ色の速度ベクトルも反時計方向に回転するので、迷路座標から見ればちゃんと旋回しているように見えます。この、逆方向を向いている分大回りをすることになります。マイクロマウスは２輪の発生する駆動力の差で旋回モーメントを発生することができますがその結果発生した横力が向心力となり旋回運動を続けます。

走行シミュレーション

外力と横滑り角の計算式

マイクロマウスは２本のタイヤが駆動力、制動力を発しします。それだけではなく横滑りが発生すると横力も発生します。横滑り角が小さいと横力とコーナリングフォースは等しいとします。今回はマイクロマウスはすでに一定の速度で走行しており、駆動力は転がり抵抗と釣り合っているとしますので、基本的には $F_x=0$ です。横力は横滑り角に比例するとし、この比例係数 $K$ をコーナリングパワーと呼びます。ヨーモーメントは任意とします．したがって書ける外力について以下に示すと。

$\begin{eqnarray} F_x&=&F_x (t)\\ F_y&=&-K \beta\\ \end{eqnarray}$

横滑り角 $\beta$ については図から以下のようになります。

$\begin{eqnarray} \beta=\tan^{-1}\frac{v}{u} \end{eqnarray}$

迷路座標系への座標変換

ロボットに固定した座標系での速度や角速度の変化を見ることである程度はこれまで説明したことの検証はできると思いますが、やはり走行軌跡などが判ったほうがよいのでロボット座標の速度と角速度を迷路座標に変換する式を以下に掲載します。

$\begin{eqnarray} \dot X &=& u \cos \Psi - v \sin \Psi\\ \dot Y &=& u \sin \Psi + v \cos \Psi\\ \dot \Psi &=& r \end{eqnarray}$

$X$ 、 $Y$ 、 $\psi$ は迷路座標におけるロボットの位置と方向を表しています。

これで、式は出そろいました。これらの $u$ 、 $v$ 、 $w$ 、 $X$ 、 $Y$ 、 $\Psi$ に関する微分方程式をルンゲ・クッタ法で解いていきます。

スラローム走行のキネマティクスと理想走行軌跡

以上で説明した、３自由度剛体の運動においてタイヤの力学を考慮した計算と以下で説明するスラローム旋回とを比較してみたいと思います。

マイクロマウスがスラローム旋回をするときは下図のようなターン角速度のプロファイルを考えることが多いようです。その場合に、マイクロマウスは等速直線運動、等角加速度旋回、等角速度旋回、等角減速度旋回、等速直線運動をすると考えます。走行軌跡的には①直進、②クロソイド曲線、③円弧、④クロソイド曲線、⑤直進と描きます。

旋回の角速度・角加速度プロファイルのグラフ — 旋回の角速度・角加速度プロファイル

このように走るための角加速度 $\dot \omega$ と $\psi_2$ を決めることをパラメータ決定という事にします。これらの値が決められると機械的にクロソイド走行の期間や円弧走行の期間がきまり、時間的にどう切り替えていけばも自動的に決まります。これらのパラメータからどの様にマイクロマウスに指令するかはいろいろあると思います。例えば $\dot \omega$ での等角加速度運動を $\psi_2$ になるまで走らせることや、 $\dot \omega$ での等角加速度運動を $t_2$ 秒続けるなどです。

上の図では終端の角度が９０度の旋回を示していますが、終端の角度は任意で構いません。終端の角度が目標角tex:\psi_{ref}]に達すればよく、終端位置はとりあえず問わないのであれば割と簡単にパラメータが求められます。しかし、終端の角度と位置の３つの値を指定したいとするとパラメータ決めは解析的に解けないため、数値計算で行います。

走行期間の求め方

以下では、終端の角度だけ任意の角度になれば良い場合に走行期間の求め方について説明します。

図の②の部分の角度を $\psi_2$ 、角加速度を $\dot \omega$ とすると

$\begin{eqnarray} \psi_2= \frac{1}{2} \dot \omega t_2^2 \end{eqnarray}$

したがって、走行時間は

$\begin{eqnarray} t_2= \sqrt{\frac{2 \psi_2}{ \dot \omega }} \end{eqnarray}$

その間は角加速度運動で徐々に角速度が大きくなり最終角速度は以下の速度になります．

$\begin{eqnarray} \omega_2&=&\dot \omega t_2\\ &=&\dot \omega \sqrt{\frac{2 \psi_2}{ \dot \omega }}\\ \end{eqnarray}$

③の部分は等角速度 $\omega_2$ で角度 $\psi_3$ まで回ります。また、角度 $\psi_3$ については図から次の式のように書けることが判ります。

$\begin{eqnarray} \psi_3=\frac{\pi}{2} - 2 \psi_2 \end{eqnarray}$

したがって、③の部分の走行時間を $t_3$ とすると、以下のようになります。

$\begin{eqnarray} t_3&=&\frac{\psi_3}{\omega_2}\\ &=&\frac{1}{\dot \omega} \sqrt{\frac{\dot \omega}{2 \psi }} \left( \frac{\pi}{2} -2 \psi_2\right) \end{eqnarray}$

このように、②の部分の角度 $\psi_2$ と角加速度 $\dot \omega$ を決めてしまえば、②③④の走行時間が求まりこの時間で走行させると９０度ターンします。ただし本物のマウスでは最終到達地点を所望の座標にしなければならないので、角度 $\psi_2$ と角加速度 $\dot \omega$ を決め反復して目標到達地点に至る組を探し当てます。

ここで、走行速度は $V$ 、初期方向は $\Psi_0$ としてそのようなスラローム旋回を段階に分けて、微分方程式で以下に表してみます。

①最初の直進

$\begin{eqnarray} \dot X &=& V \cos \Psi_0\\ \dot Y &=& V \sin \Psi_0 \\ \end{eqnarray}$

この走行を終了したときのロボットの位置を $X_1$ 、 $Y_1$ とします。

②最初のクロソイド

$\begin{eqnarray} \dot X &=& V \cos (\frac{1}{2} \dot \omega t^2 + \Psi_0)\\ \dot Y &=& V \sin (\frac{1}{2} \dot \omega t^2 + \Psi_0)\\ \end{eqnarray}$

この走行を終了したときのロボットの位置を $X_2$ 、 $Y_2$ 、方向を $\Psi_2$ 、角速度を $\omega_2$ とします。

③円弧

$\begin{eqnarray} \dot X &=& V \cos (\omega_2 t + \Psi_2)\\ \dot Y &=& V \sin (\omega_2 t + \Psi_2) \\ \end{eqnarray}$

この走行を終了したときのロボットの位置を $X_3$ 、 $Y_3$ 、方向を $\Psi_3$ 、角速度はそのままなので $\omega_2$ とします。

④２回目のクロソイド

$\begin{eqnarray} \dot X &=& V \cos (\frac{1}{2} \dot \omega t^2 + \omega_2 t + \Psi_3)\\ \dot Y &=& V \sin (\frac{1}{2} \dot \omega t^2 + \omega_2 t + \Psi_3) \\ \end{eqnarray}$

この走行を終了したときのロボットの位置を $X_4$ 、 $Y_4$ 、方向を $\Psi_4$ 、角速度を $\omega_4$ とします。

⑤最後の直進

$\begin{eqnarray} \dot X &=& V \cos \Psi_4\\ \dot Y &=& V \sin \Psi_4 \\ \end{eqnarray}$

理想スラローム旋回軌跡の計算結果

そこで、次のように値を決めて上式のスラローム旋回の計算をpythonにさせ、matplotlibで描画してみた。

名称	記号	値	単位
終端角度	$\psi_{ref}$	$\frac{\pi}{2}$	rad
速度	$V$	1.0	m/s
②の角度	$\psi_2$	$\frac{\pi}{6}$	rad
角加速度	$\dot \omega$	365	rad/s/s
②走行時間	$t_2$	0.0535	s
③走行時間	$t_3$	0.0267	s

運動方程式を解いたスラローム旋回計算結果

これに対して、同じ角速度プロファイルになるようにモーメント $N$ を設定した運動方程式からの計算結果を示します。

運動方程式から計算したスラローム旋回グラフ — 運動方程式から計算したスラローム旋回

こちらは、太いカラフルな線が運動方程式からの計算結果、比較として簡易計算のスラロームの計算結果が青い細い線です。運動方程式からの計算結果は、簡易計算のスラーロームと同じヨーレートが時系列で発生するように設定しているんですが、滑りが発生するため、少しだけ大回りすることが判りますね。

以下はこの走行軌跡の際の、各状態量の変化を示します。

実は、運動方程式の1式目の $u$ に関する方程式をよく見ると判りますが、ヨーレートが発生すると何もしないと $u$ が減少してしまいます。結構無視できないぐらい減るので、ＰＩ制御をかけて $F_x$ を与えています。

試しに、制御をしない場合を以下に示します。

速度制御なしのスラローム旋回計算結果のグラフ — 速度制御なしのスラローム旋回計算結果

意に反してというか、速度が遅くなったためか、外に膨らむのが抑えられて、簡易計算に近づいています。

こちらも、各状態量を載せます。

速度制御をしない場合の各状態の変化のグラフ — 速度制御をしない場合の各状態の変化

速度 $u$ が下がり続けているのが確認できます。代わりに速度誤差V-Errorが増大し続けます。

コーナリングパワーの影響

コーナリングパワー $K$ の値をどのくらいにすればいいのか、また実機でどう測ってこういったシミュレーションに反映させればいいのか、まだ確たることは言えません。その値についていろいろ文献を調査してみましたが、コーナリングフォースで見るとだいたい荷重に摩擦係数をかけたものがタイヤが出せる摩擦力の最大値です。タイヤは合わせてそれ以上の力は発生できません。これはいわゆる摩擦円という話題で取り上げられる事実です。そしてコーナリングフォースは最大値の3分の1ぐらいの値に１０度ぐらいの横滑り角でなりそうかなという感触を得ています。（まだエビデンスきっちり出せません。すみません。）これに照らし合わせるとコーナリングパワーは２０N/radぐらいかなと言った、これも感触を得ていて、今回の計算結果はこの値で計算しました。

最初、どのくらいにしたら判らず、コーナリングパワーを1にしていて、全くダメで、コードのバグかとかなりの時間悩みましたが、これを変えたら一発でそれらしくなりました。次はコーナリングパワーを1にした場合です。

コーナリングパワーを1にした場合の結果のグラフ — コーナリングパワーを1にした場合の結果

コーナリングパワーを1にした場合の状態の変化のグラフ — コーナリングパワーを1にした場合の状態の変化

全く回れていません、タイヤが横力を発生できないので、向心力がなくて回れません。

だめ押しでコーナリングパワーが１０の場合について載せます。

コーナリングパワーを10にした場合の結果の結果 — コーナリングパワーを10にした場合の結果

コーナリングパワーを10にした場合の状態の変化のグラフ — コーナリングパワーを10にした場合の状態の変化

やはり、少し膨らみました。

しっかりグリップしてくれるタイヤの重要性が再確認できます。

ずいぶん昔は、横滑りはタイヤのグリップが負けて起こっていると思っていましたが。それは間違いで、旋回するための向心力を発生するためのメカニズムとして横滑りは常に発生しているという事が確認できました。

１８０度旋回

ツイッターでけりさんの書き込みをみて１８０度旋回ではY方向はずれないとの事でした。最初はバグがあり、そうはならなかったんですが、何度かディスカッションさせて頂いてバグが取れて同じようにY方向は誤差がなくなる結果が出るようになりました。以下の結果はコーナリングパワーが２０の時の結果です。 コーナリングパワーが小さいとやはりY方向もずれますので、タイヤの特性に左右されます。以下に１８０度旋回の場合の結果を掲載します。

名称	記号	値	単位
終端角度	$\psi_{ref}$	$\pi$	rad
速度	$V$	1.0	m/s
②の角度	$\psi_2$	$\frac{\pi}{4}$	rad
角加速度	$\dot \omega$	385	rad/s/s
②走行時間	$t_2$	0.0639	s
③走行時間	$t_3$	0.0639	s

参考としてコーナリングパワーを５にした時の軌跡を掲載します。

コーナリングパワーを5にした場合の１８０度旋回の結果のグラフ — コーナリングパワーを5にした場合の１８０度旋回の結果

更に、コーナリングパワーが大きくなったらどうなるのかという事で、コーナリングパワーが５０の場合を計算してみました。

コーナリングパワーを５０にした場合の１８０度旋回の結果のグラフ — コーナリングパワーを５０にした場合の１８０度旋回の結果

いかがでしょう。コーナリングパワーを大きくすると、横滑りが小さく抑えられるので、終端の位置が理想の曲線に近づいていくのが判りました。コーナリングパワーはタイヤの特性なので、ロボット制作者は基本的に変えられないので、この考察はあくまで参考ですが、タイヤと走行の関係について改めて考えるきっかけにはなったと思います。

タイヤの能力の見極め

旋回の実験をするときは１８０度旋回の実験をして初期角度と走行が終わった後の角度が平行になるか確認して、平行ならばコーナリングパワーが十分だという事でその速度での走行は可能で、平行にならない場合はタイヤの性能が走行速度に追い付いて無いという判断に使えそうです。

計算に使ったコードは一番最後に付録として掲載します。全くコメントもなく、マジックナンバーばかりで恥ずかしいですが・・・・

おわりに

一応、書き上げることができました。土曜日ぐらいから書き始めて、コードがまだ添付されていない状態で12000文字を超えました・・・・大作です。

１０年ぐらい前だったか「こじまうす」で有名な小島さんがスリップ角の話をされているのを見かけて気にしてたんですが、１０年越しに確認してみることができて自分としては満足です。３自由度シミュレーションのプログラムも作れたし、もう少し発展させたいなと思っています。

この記事も書きなぐった感が強いので、もう少し時間がたって見直したら書き直すことや、新たな考察もしたいですね。もしかしたら間違いもあるかもしれません。（いや、きっとあるな・・・）

マイクロマウスの制御はタイヤの速度を制御していることから始まり、車体の速度や角速度を制御する方向に発展しています。僕の作った３式改は計測輪から得られた、速度と角速度をフィードバックする仕組みでした。最終的には駆動力や旋回モーメントを陽に直接制御したい欲求にとらわれて早１０年以上経過しました。ずっと追及することや妄想することがあってマウスは楽しいです。楽しみ方も人それぞれだと思います。

前に「月曜日は書いている途中で寝落ちしてました・・・」と書きましたが、今日（水曜日）も気が付いたら寝ていて、書き上げられそうな今は、日をまたいで１時です。ランサーを職場に泊まって作ってた時は３０代で、いくらでも徹夜できたのに、人生半分を折り返すと目もぼやけてくるし徹夜は生産性が著しく下がってだめですね。若返りたいです。

しかし、この記事は自分で書いていて楽しかった。楽しいんですが記事が長くなると、だんだん味もそっけもない事実の羅列みたいな文章になるので注意せねばと思っているところです。

今回はマイクロマウスの重心はタイヤの間にあるとしてます．後日時間をとって書き足したいと思っているのは、横力がタイヤに発生するということは、重心位置次第では、横力のモーメントの方向が変わり、それによってオーバーステア、アンダーステアが発生すると思われるため計算で確認したいと思います．

それでは、また！

参考文献

マウスのためにどんだけ本買ったかな・・・

航空機力学入門

作者:加藤寛一郎
東京大学出版会

Amazon

自動車の運動と制御

作者:正人, 安部
山海堂

Amazon

自動車用タイヤの研究 (MECHANISM SERIES)

山海堂

Amazon

タイヤ工学

作者:秀男, 酒井
グランプリ出版

Amazon

Motor Vehicle Dynamics: Modeling and Simulation (SERIES ON ADVANCES IN MATHEMATICS FOR APPLIED SCIENCES)

作者:Genta, Giancarlo
World Scientific Publishing Company

Amazon

付録　シミュレーションソースコード

以下はJupyter labが実行環境です。

https://blog.rikei-tawamure.com/entry/2020/05/10/194256blog.rikei-tawamure.com

運動方程式を解いてスラロームを計算するバージョン

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

'''
def rk4(func, t, h, y, *x)
ルンゲ・クッタ法を一回分計算する関数
    引数リスト
    func:導関数
    t：現在時刻を表す変数
    h：刻み幅
    y：出力変数（求めたい値）
    *x:引数の数が可変する事に対応する、その他の必要変数
※この関数では時刻は更新されないため、これとは別に時間更新をする必要があります。
'''
def rk4(func, t, h, y, *x):
    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 uDot(t, u, m, fx ,r, v):
    return fx/m+r*v

def vDot(t, v, m, fy, r, u):
    return fy/m-r*u

def rDot(t, r, Izz, N):
    return N/Izz

def xDot(t, x, u, v, psi):
    return u*np.cos(psi) -v*np.sin(psi)

def yDot(t, y, u, v , psi):
    return u*np.sin(psi) +v*np.cos(psi)

def psiDot(t, psi, r):
    return r

#初期化
U=np.empty(0)
V=np.empty(0)
R=np.empty(0)
X1=np.empty(0)
Y1=np.empty(0)
X2=np.empty(0)
Y2=np.empty(0)
X3=np.empty(0)
Y3=np.empty(0)
X4=np.empty(0)
Y4=np.empty(0)
X5=np.empty(0)
Y5=np.empty(0)
Psi=np.empty(0)
T=np.empty(0)

u=1
v=0
r=0
x=0
y=0
psi=0
t=0
Fx=0
Fy=0
Izz=1
N=0
s=0 #積分器
beta=0

#質量
m=0.1

#コーナリングパワー
K=20

#刻み幅
h=0.00001

#制御周期
Tc=0.001

#旋回パラメータ
psiref=np.pi/2
omegadot=410 #330
psi24=np.pi/5
TW1=0.01
TW24=np.sqrt(2*psi24/omegadot)
TW3=np.sqrt(omegadot/2/psi24)*(psiref-2*psi24)/omegadot
TIME=int((TW1*2+TW24*2+TW3)/h)

#制御を1msでするためのカウンタ
cnt=0

#求解ループ
for n in tqdm(range(TIME)):
    
    #外力の計算（制御）
    Fy=-K*beta
    if t<TW1:
        N=0.0
        X1=np.append(X1, x)
        Y1=np.append(Y1, y)
    elif t<TW1+TW24:
        N=omegadot
        X2=np.append(X2, x)
        Y2=np.append(Y2, y)
    elif t<TW1+TW24+TW3:
        N=0
        X3=np.append(X3, x)
        Y3=np.append(Y3, y)
    elif t<TW1+TW24*2+TW3:
        N=-omegadot
        X4=np.append(X4, x)
        Y4=np.append(Y4, y)
    else:
        N=0.0
        X5=np.append(X5, x)
        Y5=np.append(Y5, y)
        
    if cnt==int(Tc/h):
        cnt=0
        err=1-np.sqrt(u**2+v**2)
        s=s+err
        Fx=100.0*err+0.01*s
        #Fx=0 #制御西にするときはコメントをはずす
    cnt=cnt+1
    
    
    U=np.append(U, u)
    V=np.append(V, v)
    R=np.append(R, r)

    Psi=np.append(Psi, psi)
    T=np.append(T, t)
    uold=u
    vold=v
    rold=r
    xold=x
    yold=y
    psiold=psi
    
    #数値積分（ルンゲクッタ関数呼び出し）
    u=rk4(uDot, t, h, uold, m, Fx, rold, vold)
    v=rk4(vDot, t, h, vold, m, Fy, rold, uold)
    r=rk4(rDot, t, h, rold, Izz, N)
    x=rk4(xDot, t, h, xold, uold, vold, psiold)
    y=rk4(yDot, t, h, yold, uold, vold, psiold)
    psi=rk4(psiDot, t, h, psiold, rold)
    
    t=t+h
    beta=np.arctan2(v, u)

#グラフ描画
plt.rcParams["font.size"] = 15    
plt.figure(figsize=(18,16))    
plt.subplot(421)
plt.plot(T,U)
plt.ylabel('u[m/s]')
plt.grid()
#plt.show()
plt.subplot(422)
plt.plot(T,V)
plt.ylabel('v[m/s]')
plt.grid()
#plt.show()
plt.subplot(423)
plt.plot(T,R)
plt.ylabel('r[rad/s]')
plt.grid()
#plt.show()
plt.subplot(424)
plt.plot(T,Psi)
plt.ylabel('Psi[rad]')
plt.grid()
#plt.show()
plt.subplot(425)
plt.plot(T,1-np.sqrt(U**2+V**2))
plt.ylabel('V-Error[m/s]')
plt.xlabel('Time(s)')
plt.grid()
#plt.show()
plt.subplot(426)
plt.plot(T,np.arctan2(V, U))
plt.ylabel('Beta[rad]')
plt.xlabel('Time(s)')
plt.grid()
plt.show()

plt.rcParams["font.size"] = 12
plt.figure(figsize=(6,6))
plt.plot(X1,Y1, lw=5)
plt.plot(X2,Y2, lw=5)
plt.plot(X3,Y3, lw=5)
plt.plot(X4,Y4, lw=5)
plt.plot(X5,Y5, lw=5)
plt.xlim(0, 0.11)
plt.ylim(0, 0.11)
plt.xticks( np.arange(0.0, 0.11, 0.01) )
plt.yticks( np.arange(0.0, 0.11, 0.01) )
plt.grid()
plt.show()

簡易スラローム計算バージョン

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

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

#1
def x1Dot(t, x, v, psi):
    return v*np.cos(psi)

def y1Dot(t, y, v, psi):
    return v*np.sin(psi)

#2
def x2Dot(t, x, v, omegadot, psi):
    return v*np.cos(0.5*omegadot*t**2+psi)

def y2Dot(t, y, v, omegadot, psi):
    return v*np.sin(0.5*omegadot*t**2+psi)

#3
def x3Dot(t, x, v, omega, psi):
    return v*np.cos(omega*t+psi)

def y3Dot(t, y, v, omega, psi):
    return v*np.sin(omega*t+psi)

#4
def x4Dot(t, x, v, omegadot, omega, psi):
    return v*np.cos(0.5*omegadot*t**2+omega*t+psi)

def y4Dot(t, y, v, omegadot, omega, psi):
    return v*np.sin(0.5*omegadot*t**2+omega*t+psi)


v=1.0


X1=np.empty(0)
Y1=np.empty(0)
X2=np.empty(0)
Y2=np.empty(0)
X3=np.empty(0)
Y3=np.empty(0)
X4=np.empty(0)
Y4=np.empty(0)
X5=np.empty(0)
Y5=np.empty(0)
Psi=np.empty(0)
T=np.empty(0)

x=0
y=0
psi=0.0

h=0.00001

psiref=np.pi
omegadot=385 #330
psi24=np.pi/4
TW24=int(np.sqrt(2*psi24/omegadot)/h)
TW3=int(np.sqrt(omegadot/2/psi24)*(psiref-2*psi24)/omegadot/h)


t=0
#求解ループ1
for n in tqdm(range(100)):

    X1=np.append(X1, x)
    Y1=np.append(Y1, y)
    Psi=np.append(Psi, psi)
    T=np.append(T, t)

    xold=x
    yold=y
    
    #数値積分（ルンゲクッタ関数呼び出し）

    x=rk4(x1Dot, t, h, xold, v, psi)
    y=rk4(y1Dot, t, h, yold, v, psi)

    
    t=t+h

t2=0
#求解ループ2
for n in tqdm(range(TW24)):

    X2=np.append(X2, x)
    Y2=np.append(Y2, y)
    Psi=np.append(Psi, psi)
    T=np.append(T, t)

    xold=x
    yold=y
    
    #数値積分（ルンゲクッタ関数呼び出し）

    x=rk4(x2Dot, t2, h, xold, v, omegadot, psi)
    y=rk4(y2Dot, t2, h, yold, v, omegadot, psi)

    t=t+h 
    t2=t2+h

psi=0.5*omegadot*t2**2+psi
omega=omegadot*t2
t3=0
#求解ループ3
for n in tqdm(range(TW3)):

    X3=np.append(X3, x)
    Y3=np.append(Y3, y)
    Psi=np.append(Psi, psi)
    T=np.append(T, t)

    xold=x
    yold=y
    
    #数値積分（ルンゲクッタ関数呼び出し）

    x=rk4(x3Dot, t3, h, xold, v, omega, psi)
    y=rk4(y3Dot, t3, h, yold, v, omega, psi)

    t=t+h 
    t3=t3+h    

psi=omega*t3+psi
t4=0
#求解ループ4
for n in tqdm(range(TW24)):

    X4=np.append(X4, x)
    Y4=np.append(Y4, y)
    Psi=np.append(Psi, psi)
    T=np.append(T, t)

    xold=x
    yold=y
    
    #数値積分（ルンゲクッタ関数呼び出し）

    x=rk4(x4Dot, t4, h, xold, v, -omegadot, omega, psi)
    y=rk4(y4Dot, t4, h, yold, v, -omegadot, omega, psi)

    t=t+h 
    t4=t4+h

#求解ループ5
t5=0
psi=-0.5*omegadot*t4**2+omega*t4+psi
omega=-omegadot*t4+omega
for n in tqdm(range(100)):

    X5=np.append(X5, x)
    Y5=np.append(Y5, y)
    Psi=np.append(Psi, psi)
    T=np.append(T, t)

    xold=x
    yold=y
    
    #数値積分（ルンゲクッタ関数呼び出し）

    x=rk4(x1Dot, t5, h, xold, v, psi)
    y=rk4(y1Dot, t5, h, yold, v, psi)
    
    t=t+h
    t5=t5+h
    
plt.figure(figsize=(10, 10))
plt.plot(X1, Y1, lw=5)
plt.plot(X2, Y2, lw=5)
plt.plot(X3, Y3, lw=5)
plt.plot(X4, Y4, lw=5)
plt.plot(X5, Y5, lw=5)
plt.xlim(0, 0.18)
plt.ylim(0, 0.18)
plt.xticks( np.arange(0.0, 0.18, 0.01) )
plt.yticks( np.arange(0.0, 0.18, 0.01) )
plt.grid()
plt.show()

np.savez('np_savez', X1, Y1,X2, Y2,X3, Y3,X4, Y4,X5, Y5)