ライントレースの制御。

　こんばんはヌキヲです。
今回は私流ですが、ライントレース制御についてメモを残しておきます。
前からやろうかなぁとは思っていたのですが、
Twitter でお話をする機会があり、とても面白かったので、
こんな方法もあるヨみたいな感じです。

ヌキヲさんはすべて独学ですのでトンデモ説を唱えてる場合があります。
その場合はそっと指摘してやってください。



●ラインセンサ
　私のラインセンサは現在二種類あります。

一つはラインの中心を検出するモノ。
センサを等間隔に並べているので、ラインからかなり外れてもコースアウトしません。
一番白いセンサに注目し両隣と差分を取るというごく一般的なものです。
旋回半径のきついロボトレで使用しています。




もう一つはラインの両端を検出するモノです。
こちらは微細なラインの位置を検出できます。
ランサーで高速走行するのに役立っています。





●PID制御
　ラインセンサの PID 制御にはマシンによって二種類を使い分けています。

一つは一般的な PID 制御のモノ。
ステアリングの無いマシン用です。
I-PD 制御も面白カモと思い実装はしてあります。

もう一つは PD-I 制御です。
ステアリングがあるマシンは絶対これが安定しますよ！

速度制御については普通の PID 制御です。



●機体制御
Twitter で角速度と並進速度のベクトルモデルで計算していることを聞きました。
私のプログラムでは 曲率 を使います。

仮想曲率で、+-4096 とっています。
本来は tan(θ) なのですが、45°くらいまでは線形でもほとんど一緒なので省略してます。
ラインセンサの値から PID 制御をし、この仮想曲率を出します。
次に仮想曲率から旋回半径を出します。

旋回半径 = 4096 * ホイールベース / 仮想曲率

この仮想曲率が 4096 のとき 旋回半径 = ホイールベース となります。

ホイールベース / tan(45°)

の状態で tan(45°) は 1 なので合ってますね。
最大旋回角度は 45° 固定です。
トレッドとホイールベースはパラメータのほうに放り込んであるので微調整したり
マシン毎(ロボトレ、ランサー)に切り替えたりします。
続いて左右輪の速度へと落とし込んでいきます。

Lw = 旋回半径 - (トレッド / 2)
Rw = 旋回半径 + (トレッド / 2)

Lv = Lw * 並進速度 / 旋回半径
Rv = Rw * 並進速度 / 旋回半径

ここでホイールベース、トレッドは mm、並進速度は mm/s になっているので
出てきた Lv、Rv は mm/s 単位になっています。
そのまま各輪個別に速度制御のフィードバックに入れてやります。
毎回 3回 の割り算をするところが気に食わないところですが、致し方ないのかな。

というわけでフィードフォワードは全くと言っていいほど出てきませんな。