斜め走行を考慮した最短経路導出の実装例


本稿では最短経路の求め方|最短経路導出法の実装例を紹介する.

問題

本記事では,斜め走行で迷路を駆け抜けるロボットを考える.このロボットが最短時間でゴールに到達できる経路を導出することが問題である.

対象とするロボット・迷路

本稿ではハーフサイズ迷路を対象とする.

以下のような直進走行(GO1,DIA_GO1,etc),ターン(DIA_TURNR,DIA_TURNL),緩和曲線(DIA_CLOTHOIDR,DIA_CLOTHOIDL)を使って迷路を駆け抜けるロボットを考える.

Diaarc1
Diaarc2
Diaarc3

ロボットは以下のように走行するとする.

  • ロボットは,初速,終端速度v_e=800\displaystyle[mm/sec],加速度a=4000\displaystyle[mm/sec/sec],最高速度v_{max}=2000\displaystyle[mm/sec]のパラメータのもと,台形加速で直進する.
  • ロボットは,半径r=\frac{90}{2\sqrt{2}}\displaystyle[mm]の円弧上を,v_e=800\displaystyle[mm/sec]で走りながら90度ターン可能.
  • ロボットは,v_e=800\displaystyle[mm/sec]で緩和曲線を走る.

(ターン速度は速すぎる気がしますが,きりのいい数字を選びました)

この時,理想的な軌道の性質で紹介した公式を使い,直進,ターン,緩和曲線に必要な時間は以下のようにして求めることができる.

まず,ターン(DIA_TURNR,DIA_TURNL)に必要な時間は,\frac{r\pi/2}{v}=0.06[sec]である.

次にn\displaystyleマス分直進(GO1)するために必要な時間t\displaystyleを求めよう.

(1)    \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*}

が成立する.ただし,\ell\displaystyleは移動距離である.

場合分けの条件式に各種数値を代入すると,\frac{v_{max}^2-v_e^2}{a}/90[mm]=42/9\displaystyleより,1~4マスの直進は前者の場合,それ以上の直進は後者の場合で必要時間が求まる.各種変数に数値を代入すると,

(2)    \begin{align*} t =\left\{ \begin{array}{ll} \displaystyle \frac{\sqrt{16+9n}-4}{10}&\displaystyle (n\geq 42/9)\\ \displaystyle \frac{4.5n+18}{100}&\displaystyle (n <   42/9)\\ \end{array}\right. \end{align*}

m\displaystyleマス斜め走行(DIA_GO1,etc)するためには,\frac{v_{max}^2-v_e^2}{a}\sqrt{2}/90[mm]=\frac{42\sqrt{2}}{9}\displaystyleより,1~6マスの直進は前者の場合,それ以上の直進は後者の場合で必要時間が求まる.各種変数に数値を代入すると,

(3)    \begin{align*} t =\left\{ \begin{array}{ll} \displaystyle \frac{\sqrt{16+9m/\sqrt{2}}-4}{10}&\displaystyle (n\geq \frac{42\sqrt{2}}{9})\\ \displaystyle \frac{4.5m/\sqrt{2}+18}{100}&\displaystyle (n <   \frac{42\sqrt{2}}{9})\\ \end{array}\right. \end{align*}

緩和曲線(DIA_CLOTHOIDR,DIA_CLOTHOIDL)は適当な計算方法がないので,3/4マス分の距離を速度v_e\displaystyleで走っているとみなす.

(4)    \begin{align*} t=90*3/4/v_e=0.08[sec] \end{align*}

これをまとめると以下通り.

  • ロボットは,0.06[sec]で,DIA_TURNR,DIA_TURNL可能.
  • ロボットは,0.10[sec]で,GO1可能.
  • ロボットは,0.18[sec]で,GO2可能.
  • ロボットは,0.25[sec]で,GO3可能.
  • ロボットは,0.32[sec]で,GO4可能.
  • ロボットは,0.38[sec]で,GO5可能.
  • ロボットは,0.40[sec]で,GO6可能.
  • ロボットは,0.49[sec]で,GO7可能.
  • ロボットは,0.54[sec]で,GO8可能.
  • ロボットは,0.58[sec]で,GO9可能.
  • (途中省略)
  • ロボットは,1.57[sec]で,GO31可能.
  • ロボットは,0.07[sec]で,DIA_GO1可能.
  • ロボットは,0.13[sec]で,DIA_GO2可能.
  • ロボットは,0.19[sec]で,DIA_GO3可能.
  • ロボットは,0.24[sec]で,DIA_GO4可能.
  • ロボットは,0.29[sec]で,DIA_GO5可能.
  • ロボットは,0.33[sec]で,DIA_GO6可能.
  • (途中省略)
  • ロボットは,2.18[sec]で,DIA_GO62可能.
  • ロボットは,0.08[sec]で,DIA_CLOTHOIDR,DIA_CLOTHOIDL可能.

定式化

まず,迷路とロボットをグラフとしてモデル化する.以下の図のように,1区画あたり12状態のノードを用意する.32*32*12=12288ノードのグラフを考える.
Dianode

アークは直進走行,ターン,緩和曲線の関係にそって伸びているとする.重みコストは適切に設定されているとする.

このようにしてモデル化したグラフに対してダイクストラアルゴリズムを適用する.

ソースコード

maze.datの内容は次のとおり.迷路の表現は座標系・迷路表現の定義#迷路表現の例で解説しているものを採用している.

これを図示すると以下のようになる

[[ファイル:最短経路サンプル迷路.JPG]]

結果

実行結果は以下のとおり.

斜め走行・速度を考慮して最短経路を求めると,与えられたパラメータの下では5.14秒でゴールする軌道が最短であることがわかった.

考察

計算量

prev,cost,lis変数が最もRAMを使用している.int型は2byteとすると,2*2^14*2=64Kby程度のRAMを要求する.ただし,コードはより複雑になるものの,prev変数を使わない実装を行えば半分の32kbyte程度の空間計算量に抑えることができる.すなわち,最新のSTM32マイコンに対してはこのアルゴリズムが実装できることがわかる.

コーディングの難易度

ダイクストラアルゴリズム部分は最短経路導出の実装例と同じである.最短経路導出の実装例との違いは,アークの伸びている条件式のコーディングである.最短経路導出の実装例から順番にステップアップすればプログラミングが苦手でもコーディングできなくはない.

より実機に近付けた実装をしたい場合は

大回り90度・180度ターンが実装したい場合は,単にアークの伸び方を増やすだけで良い.さらに細かく速度を考慮したい場合は,同じ場所でも速度が違えば別ノードであるとみなせばよい.

すなわち,どのようにしてグラフ構造に落とすかによって,経路の質が変わる.
第3世代のマイクロマウスにおいて,最短経路の導出法は,モデル化法部分が腕の見せ所になると思われる.