マイクロマウス?
迷路探索とそれにもとづいた軌道生成アルゴリズム - マイクロマウス †
概要 †
本記事では，求めた最短経路情報をもとに実際の軌道を生成する際に用いるアルゴリズムを紹介する．
前提 †
前提として以下の仮定をおく．
- ターゲットの始点と終点などを決めて，目標位置を時間の関数として生成する
- 位置入力，速度(電圧)出力の制御系
- ターゲットの位置を時間的に変化させ，位置と角度の偏差をもとに制御入力を決定する，いわゆるターゲット追尾型の位置制御を行う
- 制御入力(速度出力)から実際の機体の速度までの伝達関数は，近似的に1次遅れ系となり，(モータに対して速度制御をかけているため)その時定数は十分小さいものとする
最短経路情報から座標の抽出 †
迷路探索シミュレータの製作 - マイクロマウスのアルゴリズムでは，最短経路情報は通過する区画の番号の配列として得られる．
高速走行時には，同じ方向に続く経路は1つの直進として扱いたいので，この経路情報から曲がり角の部分だけを抽出すればよい．
基本動作 †
マイクロマウスの高速走行時の基本動作は，直進・スラローム旋回の2つである．
得られた経路上の曲がり角の座標から，
直進3マス→右旋回→直進2マス→左旋回→直進10マス・・・
のように基本動作を組み合わせて経路を生成する．
加速度の連続性と補間曲線 †
マイクロマウスにおいては，タイヤの滑りをいかに抑えるか/補正するかが大きな問題となる．
急な加減速を避けることによって，タイヤの滑りをある程度抑制することができる．
まず，1つの基本動作のみについて考えよう．
ターゲットの位置を時間による関数$r(t)$として考えると，この関数によって与えられるターゲットの加速度は，$\frac{d^2}{dt^2}r(t)$である．
これが連続であること，すなわち微積分学の言葉で表現すると，$r(t)$は$C^2$級であることが必要である．
ただし，加速度が一定値をとるようなものは現実的には使いづらいため，$C^3$級以上の関数を使用することになる．
さて，次はこの加速度連続な基本動作をつなぎあわせて全体として加速度連続な経路を生成したい．
すなわち，基本動作の境界での加速度・速度・位置がそれぞれ連続である必要がある．
このようなときによく用いられるのが，5次関数による補間曲線である．
5次関数は，一般式
$g(x)=ax^5+bx^4+cx^3+dx^2+ex+f$
で表され，未知数が6つ存在する．
すなわち，境界条件を6つ決めれば5次関数を一意に決定することができる．
今回の要件では，用いる境界条件は，
- $t=0$での位置(始点の座標)
- $t=0$での速度(始点での速度)
- $t=0$での加速度(始点での加速度)
- $t=\tau$での位置(終点の座標)
- $t=\tau$での速度(終点での速度)
- $t=\tau$での加速度(終点での加速度)
の6つである．
これが連続となるように基本動作をつなげることで，なめらかな目標軌道の生成が可能となる．
しかし，高次の関数になればなるほど，変曲点を多く含む可能性が高くなる．
変曲点まわりでは，速度増加が一旦止まった後再度加速が始まるといった動きをするため，加速度は連続であるもののふらふらとした速度変化を生むこととなり好ましくない．
したがって，なるべく低次の関数で補間したほうがよいと考えられる．
カブトガニ野郎では，4次関数を用いて軌道補間を行なっており，境界条件を1つ見捨てていることになる．
今回は終点での加速度を無視して考えた．その理由としては，
- 基本動作はすべて加速→減速のパターンであり，終点では必ず$\left.\frac{d^2}{dt^2}r(t)\right|_{t=\tau}<0$
- 直進の基本動作の後には必ず旋回の基本動作が入る
- 直進による縦滑りよりも，旋回による横滑りの方がはるかに大きいため，旋回の始点(直進の終点)における加速度の連続性はさほど重要ではない
などが挙げられる．
実装 †
今回の実装の指針は以下の様なものであった：
- 軌道をパラメトリックに設定したい
→関数オブジェクトの利用
- 軌道だけでなく，軌道上の進み方（緩急の付け方）を関数で設定したい
→関数オブジェクトを内包する合成関数オブジェクトの作成
- ターゲット座標を取り出す際に関数ライクに扱えるようにしたい
→operator()のオーバライド
- 基本単位を1つのオブジェクトとして，キューに入れて逐次処理させたい
→合成関数オブジェクトをメンバに持つ，動作の基本単位のクラスを作成．ポリモーフィズムを利用してキューに放り込む
クラス
- Position
座標と角度を示すクラス．
- EasingFunctor
緩急の付け方を規定する関数オブジェクトのクラス．
実際の時刻sに対して，変換後の時刻t(s)を出力する．
- MotionFunctor
基本動作を表す関数オブジェクトのクラス．
始点と終点のPositionから，幾何学的な軌道を生成する．
EasingFunctorからの出力を時刻入力として用いることによって，軌道の幾何学的構造を変えることなく緩急だけがつく．
- Target
ターゲットを表すクラス．
MotionFunctorを内包していて，これをキューに入れて逐次処理する．
フレームワークは以上で出来上がったので，これに対して先に述べた4次関数による補間曲線を与えることを考える．
まず，一番単純に直進を表す軌道について考える．
この直線に対して，緩急を4次関数で与える．
4次関数$f(t)=at^4+bt^3+ct^2+dt+e$について，
境界条件
- $f(0)=0$
- $f(\tau)=\tau$
- $\left.\frac{d}{dt}f(t)\right|_{t=0}=v_0$
- $\left.\frac{d}{dt}f(t)\right|_{t=\tau}=v_\tau$
- $\left.\frac{d^2}{dt^2}f(t)\right|_{t=0}=\alpha_0$
を考えると，
$f(0)=e=0$
$f(\tau)=\tau(a\tau^3+b\tau^2+c\tau+d)=\tau$
$a\tau^3+b\tau^2+c\tau+d=1$
$f'(0)=d=v_0$
$f'(\tau)=4a\tau^3+3b\tau^2+2c\tau+v_0=v_\tau$
$f''(0)=2c=\alpha_0$
から，
$a=\frac{2v_0+\alpha_0\tau/2+v_\tau-3}{\tau^3}$,
$b=\frac{4-v_\tau-3v_0-\alpha_0\tau}{\tau^2}$,
$c=\frac{\alpha_0}{2}$,
$d=v_0$,
$e=0$
導出された結果を用いると，この4次関数による緩急を実現するクラスは，EasingFunctorを継承して以下のように書ける．(制御周期が5[ms]=1/200[s]の場合)
制御側との結合 †
どこかで
std::queue<Target*> targetQueue;
として，Targetのポインタを持つキューを作成する．
基本単位ごとにキューに突っ込んでいく．
このように関数を作っておけば，
のようにして，終点座標，所要時間と軌道の形状と速度特性だけを与えてやることでどんどんマシンが動くように作れる．
位置制御をかけることを前提としているため，キューの先頭にあるTargetの位置を取得して，それを目標値とすれば，これが実現される．
![[PukiWiki]](image/pukiwiki.png "[PukiWiki]")
