理想的な軌道の性質


本稿では二輪車両型のマイクロマウスロボットを考える.基本的に,ロボットは直進と緩和曲線,円弧の動きの組み合わせで障害物を回避する.それらの軌道の性質を解析する.

直進:減速

Gensoku
本節では直線上を等速直線運動する移動ロボットを考える.ロボットは初期状態として,速度v_{max}\displaystyleで走っており,-a\displaystyleの加速度で減速しながら,速度v_e\displaystyleまで減速するとする.この時,減速にかかる時間t\displaystyleと距離\ell\displaystyleは以下の通りである.

(1)    \begin{align*} t    &= \frac{v_{max}-v{e}}{a}\\ \ell &= v_{max}\frac{v_{max}-v_e}{a}-\frac{(v_{max}-v_e)^2}{2a}\\      &= \frac{v_{max}^2-v_e^2}{2a} \end{align*}

直線上を等加速度運動するロボットを考える場合,残り距離が\frac{v_{max}^2-v_e^2}{2a}\displaystyleになるまでは最高速度で走れば良いことが分かる.

直進:加速,等速,減速

300px-Daikeikasoku
本節では直線上を等速直線運動する移動ロボットを考える.ロボットは初速v_e\displaystyleで走行を開始し,加速度a\displaystyleで加速する.最高速度はv_{max}\displaystyleに達すると等速運動に移行し,適度な時刻で-a\displaystyleの加速度で終端速度v_e\displaystyleまで減速するとする.走行開始から終端速度v_e\displaystyleに達するまでの時間をt\displaystyle,時刻0\displaystyleからt\displaystyleまでの間にロボットが走行する距離を\ell\displaystyleとする.本節ではこれらの変数の関係を求める.

以下の式が成立する.

(2)    \begin{align*} \ell =\left\{ \begin{array}{ll} \displaystyle v_{max}t-\frac{(v_{max}-v_e)^2}{a}&\displaystyle (t\geq 2\frac{v_{max}-v_e}{a})\\ \displaystyle v_{max}t-(\frac{t}{2})^2a             &\displaystyle (t<    2\frac{v_{max}-v_e}{a})\\ \end{array}\right. \end{align*}

(3)    \begin{align*} t =\left\{ \begin{array}{ll} \displaystyle \left( \ell+\frac{(v_{max}-v_e)^2}{a}\right) /v_{max}&\displaystyle (\ell\geq\frac{v_{max}^2-v_e^2}{a})\\ \displaystyle 2\frac{-v_e+\sqrt{v_e^2+\ell a}}{a}                  &\displaystyle (\ell <  \frac{v_{max}^2-v_e^2}{a})\\ \end{array}\right. \end{align*}

t\geq 2\frac{v_{max}-v_e}{a}は最高速度まで加速してから減速している場合を,t<2\frac{v_{max}-v_e}{a}は最高速度に達する前に減速に入る場合を意味している.

回転運動

Enko
本節では,回転半径r\displaystyleの円弧上を等速運動しているロボットを考える.進行方向へのロボットの速度をv\displaystyleとすると,
\theta\displaystyle[rad]だけ回転するために必要な時間t\displaystyleと,移動した距離\ell\displaystyleは以下の通りである.

(4)    \begin{align*} t    &= \frac{r\theta}{v}\\ \ell &= r\theta \end{align*}

特に難しい点はなし.\theta\displaystyleの単位が[rad]である点にだけ注意.なお,X[deg]=Y[rad]である場合,以下の関係が成立する.

(5)    \begin{align*} \pi/180 = Y/X\\ Y=X\pi/180\\ X=Y180/\pi \end{align*}

向心加速度\alpha \displaystyle,回転半径r\displaystyleと,進行方向へのロボットの速度v\displaystyle,ロボットの回転速度\omega\displaystyleの間には以下の関係が成立する.

(6)    \begin{align*} v      &= \sqrt{r\alpha}\\ \omega &= \sqrt{\frac{\alpha}{r}}\\ \alpha &= \frac{v^2}{r}\\ r      &= \frac{v}{\omega} \end{align*}

ロボットがターン時にすべる主要因は向心力であるため,パラメータ設計は向心力と回転半径で行い,実際にロボットを制御するときは,向心力と回転半径から得られる速度・角速度を参照するといいだろう.

緩和曲線

進行方向側へは等速度v\displaystyle,回転方向側へは等加速度\alpha \displaystyleで運動しているロボットを考える.ロボットの初期位置は,(x(0),y(0),\theta(0))=(0,0,0)\displaystyleとする.この時,時刻t\displaystyleでのロボットの位置は以下の通りである.

(7)    \begin{align*} \begin{bmatrix} x(t)\\y(t)\\ \theta(t) \end{bmatrix} &= [crayon-58d5ea7e0c752604220938/] &=\begin{bmatrix} [crayon-58d5ea7e0c786901455170/] &=\begin{bmatrix} [crayon-58d5ea7e0c78d229991542/] &=\begin{bmatrix} [crayon-58d5ea7e0c792752188059/] \end{bmatrix} \end{align*}

ただし,式変形の途中で,r=\sqrt{\frac{\alpha}{2}}sの変数変換を行った(s\in[0,t]なら,r\in[0,\sqrt{\frac{2}{\alpha}}t]で,dr/ds=\sqrt{\frac{\alpha}{2}}よりds=\sqrt{\frac{2}{\alpha}}dr).また,写像C,S:\mathbb{R}\to\mathbb{R}\displaystyleフレネル積分であり,以下で定義される.

(8)    \begin{align*} S(x)&:=\sum_{n=0}^{\infty}\frac{(-1)^nx^{4n+3}}{(2n+1)!(4n+3)}\\ C(x)&:=\sum_{n=0}^{\infty}\frac{(-1)^nx^{4n+1}}{(2n)!  (4n+1)} \end{align*}

というわけで,フレネル積分を解かない限りはクロソイド曲線でのカーブは計算できません.数値計算でがんばる(フーリエ級数展開,定義のまま愚直に計算,オイラー法で積分計算)ことや,フレネル積分に関する研究を行うなどの工夫が考えられます.

9.8m/sec/sec程度の向心力を持つターンなら,すべり角を考慮した車両のダイナミクスに対してオイラー法でシミュレーションを行うことで,実機とほぼ変わらない走行軌道を得ることができます.これを利用し,ターンの適切な開始位置をあらかじめPCなどのシミュレーションで求めておくことができます.すべり角のダイナミクスは,こじまうす開発ノートで紹介されているものなどがあります.フィードフォワード制御用の定数を求める用途としてはそこそこ有力のようです.

実機でリアルタイムにシミュレーションを行うことは難しいため,フィードバック制御を考えるならもっと別の工夫が必要と思われます.

2自由度制御を使った台形加速の実現

勉強中.式だけメモしておきます.

(9)    \begin{align*} W_d&=-k_p sat(\frac{x-x_r}{\alpha})\\ \dot{x}_r&=-k_rsgn(x-x_d)\\ sat(x)&:=\left\{ \begin{array}{ll} 1&1<x\\ x&0\leq x\leq 1\\ 0&x<0 \end{array}\right. \end{align*}