ロボットカーのモデリング

　東北大のやり方”車輪移動ロボット”でロボットカーのモデリングを試したいと思います。

(1) ロボットカーの運動モデル

(1.1) 速度と角速度の計算

　ロボットカー後軸中心位置Pをロボットの位置とし、位置Pの移動速度 $v$ がロボットカーの移動速度となります。左右輪回転速 $v_L$ と $v_R$ として、位置Pの移動速度は式①になります。

$v = \frac {(v_L + v_R)} {2}$ ...........①

　また、ロボットカー幅を $2 \cdot l$ として、左輪を固定し、右輪が $v_R$ の速度で回転する場合、ロボットカーは左輪を軸に角速度 $\omega = \frac {v_R} {2 \cdot l}$ で回転します。また、右輪を固定して、左輪が $v_L$ の速度で回転する場合、ロボットカーは右輪を軸に角速度 $\omega = \frac {v_L} {2 \cdot l}$ で回転します。両輪がそれぞれ $v_L$ と $v_R$ で回転する場合、ロボットカーの回転速度は、片方の輪だけが回転する場合の田幸せとなります。

$\omega = \frac {(v_R - v_L)} {2 \cdot l}$ ...........②

さらにロボットカーを制御する場合、制御器の出力は両輪を駆動する両モータの角速度 $\frac {d\phi_L} {dt}$ と $\frac {d\phi_R} {dt}$ であるため、車輪半径が $r$ のとき、モータ角速度と両輪回転速間の関係は式③と式④となります。

$v_L = \frac {d\phi_L} {dt} \cdot r$ ...........③
$v_R = \frac {d\phi_R} {dt} \cdot r$ ...........④

(1.2) 座標の計算

　ロボットカー進行方向とx軸がなす角度を $\theta$ とし、車速を $v$ とした場合、ロボットカーの位置は式⑤、⑥、⑦によって表現されます。

$\frac {dx} {dt} = v cos\theta$ ...........⑤
$\frac {dy} {dt} = v sin\theta$ ...........⑥
$\frac {d\theta} {dt} = \omega$ ...........⑦

(1.3) ロボットカーの運動モデル

　式?から式?をまとめるとロボットカーの運動モデルとなります。

(2) ロボットカーの制御
　ロボットカーを指定軌跡に沿うように制御しようとすると、その位置と指定軌跡間の差を最小とする必要があります。ロボットカー位置と指定軌跡間の差は、位置と軌跡間距離 $L$ と、車両進行方向と軌跡方向間角度 $\psi$ によって表現されます。

　ロボットカーの制御は、 $L$ 、 $\frac {dL} {dt}$ 、 $\psi$ 、 $\frac {d\psi} {dt}$ を入力とし、それぞれに制御パラメータ $K_L$ 、 $K_DL$ 、 $K_T$ 、 $K_DT$ を設け、式⑧〜式⑩の制御則で入力を最小化するようにするものです。

$v_L = v - \Delta v$ ...........⑧
$v_R = v + \Delta v$ ...........⑨
$\Delta v = K_L \cdot L + K_DL \cdot \frac {dL} {dt} + K_T \cdot \psi + K_DT \cdot \frac {d\psi} {dt}$ ...........⑩

　式⑧から式⑩を整理すると、ロボットカーのコントローラとなります。

　上記コントローラと運動モデルをhttps://github.com/GerryZhang0925/AgvPythonで実装してみました。