最短経路導出の実装例


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

問題

本稿では,長い直線上はまとめ加減速しながらスラローム走行で迷路を駆け抜けるロボットを考える.このロボットが最短時間でゴールに到達できる経路を導出することが問題である.

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

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

以下のような直進走行と90度スラームを使って迷路を駆け抜けるロボットを考える.
Sura1

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

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

この時,理想的な軌道の性質で紹介した公式を使い,直進・90度スラロームに必要な時間は以下のようにして求めることができる.

まず,90度ターンに必要な時間は,\frac{r\pi/2}{v}=0.08[sec]である.

次にn\displaystyleマス分直進するために必要な時間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*}

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

  • ロボットは,0.08[sec]で,右側90度に半径45mmでスラローム可能.
  • ロボットは,0.08[sec]で,左側90度に半径45mmでスラローム可能.
  • ロボットは,0.10[sec]で自分が向いている方向に1区画だけ移動可能
  • ロボットは,0.18[sec]で自分が向いている方向に2区画まとめて移動可能(壁がなければ)
  • ロボットは,0.25[sec]で自分が向いている方向に3区画まとめて移動可能(壁がなければ)
  • ロボットは,0.32[sec]で自分が向いている方向に4区画まとめて移動可能(壁がなければ)
  • ロボットは,0.40[sec]で自分が向いている方向に5区画まとめて移動可能(壁がなければ)
  • ロボットは,0.45[sec]で自分が向いている方向に6区画まとめて移動可能(壁がなければ)
  • ロボットは,0.49[sec]で自分が向いている方向に7区画まとめて移動可能(壁がなければ)
  • ロボットは,0.54[sec]で自分が向いている方向に8区画まとめて移動可能(壁がなければ)
  • ロボットは,0.58[sec]で自分が向いている方向に9区画まとめて移動可能(壁がなければ)
  • (途中省略)
  • ロボットは,1.57[sec]で自分が向いている方向に31区画まとめて移動可能(壁がなければ)

定式化

前節で述べたロボットと迷路をグラフとしてモデル化する.この迷路・ロボットは,最短経路の求め方#速度を考慮した走行の例と似た構造のグラフとしてモデル化できる.(超信地旋回を利用した走行ではなく,スラローム走行なのでアークのつき方・重みコストが異なる)

なお,アークのコストは,0.01[sec]を1単位時間としてint型に正規化した.

  • ロボットは,8単位時間で,右側90度に半径45mmでスラローム可能.
  • ロボットは,8単位時間で,左側90度に半径45mmでスラローム可能.
  • ロボットは,10単位時間で自分が向いている方向に1区画だけ移動可能
  • ロボットは,18単位時間で自分が向いている方向に2区画まとめて移動可能(壁がなければ)
  • ロボットは,25単位時間で自分が向いている方向に3区画まとめて移動可能(壁がなければ)
  • ロボットは,32単位時間で自分が向いている方向に4区画まとめて移動可能(壁がなければ)
  • ロボットは,40単位時間で自分が向いている方向に5区画まとめて移動可能(壁がなければ)
  • ロボットは,45単位時間で自分が向いている方向に6区画まとめて移動可能(壁がなければ)
  • ロボットは,49単位時間で自分が向いている方向に7区画まとめて移動可能(壁がなければ)
  • ロボットは,54単位時間で自分が向いている方向に8区画まとめて移動可能(壁がなければ)
  • ロボットは,58単位時間で自分が向いている方向に9区画まとめて移動可能(壁がなければ)
  • (途中省略)
  • ロボットは,157単位時間で自分が向いている方向に31区画まとめて移動可能(壁がなければ)

グラフに対してダイクストラ法を適用し,x=0,y=31のマスからx=7,y=24までの最短経路を求める.

ソースコード

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

これを図示すると以下のようになる
最短経路サンプル迷路

実行結果

実行結果は以下のとおり

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

int runtimeruntime[35配列の中身をロボットの走行速度にあわせて設定すると,与えられた速度パラメータに対する最短経路を求めることができる.このソースは,空間計算量として,intのbyte数×2^13=16kbyte強のRAMを要求している.ダイクストラ法はRAMを要求するデメリットがあるが,最短経路の近似解法開発の必要がない点で優れている.ハーフマウスにおいては,RAMを増設するかしないかで最短経路の質が変わると思われる.あるいは,もうすこし時間がたてば,ワンチップで実装しきれるマイコンが出現するかもしれない.

RAM使用量を節約した実装

RAMを節約した実装.
また,壁の判定条件を2通り切り替えられるようにした.

prev[]変数がなくなったかわりにコスト変数から経路の復元に手間がかかるようになっている.実行結果は変わらない.