マイクロマウスの探索シミュレーション2

緑の点：ロボット
赤い点：目標点
青い点：目標点のうち、現在地に一番近いもの
青い線：暫定のk最短経路
ピンクの線：最終的に走行する経路
灰色のマス：未探索の壁のあるマス
(4個の壁のうち、わかっている壁の数が多くなると色が薄くなる)
以下のようなアルゴリズムで動いてます。
1.まずはゴールに向かう
2.未探索の壁は壁が無いものとして暫定k最短経路を求める
3.暫定最短経路上の未探索の壁を目標点として調べに行く
4.暫定最短経路上の未探索の壁がなくなるまで2.3を繰り返す
5.スタートに戻る