dijkstra

10×10のマス目の土地でスタート地点、ゴール地点、障害物の位置が既知とします。アルゴリズムがスタート地点からゴール地点までの最短経路を探索します。ダイクストラ法はスタート地点からの距離の短さを手掛かりに、割としらみつぶしに当たって行く方法です。matlabで実装したものをpythonに書き換えてみました。

アルゴリズムを簡単に言葉で書くと以下のようになります。
・スタート地点から始める
・全セルのスタート地点からの距離を大きな値に設定、スタート地点は0に設定
・今いるセルの上下左右のセルについてスタートからの距離を自セルの距離プラス1とする
・自セルの距離を最大値に戻す
・全体を見て距離最小のセルを選びそれをカレントのセルに更新
・カレントセルは前回のカレントセルを自分の親として覚えておく
・上3行を繰り返しゴールに到達したら終了
・ゴール地点から親をたどっていくと最短経路となる

下のコードはそのまま実行できると思います。距離のマトリクスとステータスのマトリクスを次々に表示します。最短経路はステータスのマトリクス中に数字0で表示されます。

import numpy as np
import time

# 1: フリーのセル
# 2: 障害物
# 3: 探索済
# 4: 探索リスト
# 5: スタート
# 6: ゴール
# 0: 解の経路

# 10x10のフィールドを作成
map = np.ones(100).reshape(10,10)
# 障害物を生成
map[0,7] = map[1,7] = map[2,7] = map[3,7] = map[4,7] = map[5,7] = 2
map[5,4] = map[6,4] = map[7,4] = 2
# スタート、ゴール座標
start_coords = np.array([5, 1])
dest_coords = np.array([7,8])
map[start_coords[0],start_coords[1]] = 5
map[dest_coords[0],dest_coords[1]] = 6
# 行数、列数
nrows = map.shape[0]
ncols = map.shape[1]
# 線形インデックス
start_node = ncols * (start_coords[0]) + start_coords[1]
dest_node = ncols * (dest_coords[0]) + dest_coords[1]
# 各セルのスタート座標からの距離 とりあえず100に設定
distanceFromStart = np.ones(nrows * ncols).reshape(nrows,ncols)*100
# スタート地点は距離0
distanceFromStart[start_coords[0],start_coords[1]] = 0
# 探索用 親セル
parent = np.zeros(nrows*ncols).reshape(nrows,ncols)

while True:
    map[start_coords[0],start_coords[1]] = 5
    map[dest_coords[0],dest_coords[1]] = 6
    # スタート座標から最短距離のセルを探す(最初はスタート座標)
    # 線形インデックスが得られる
    min_dist = np.min(distanceFromStart)
    current = np.argmin(distanceFromStart)
    # ゴールに達したら抜ける
    if current == dest_node or min_dist == 100:
        break
    # 通常の座標に変換
    i = int(current // ncols)
    j = int(current - (current // ncols) * ncols)
    # 探索済とする
    map[i, j] = 3
    # 次回の探索から外す
    distanceFromStart[i][j] = 100
    # カレントセルの上下左右を調べる
    for ii in range(1,5):
        if ii == 1:
            if i > 0:
                neighbor_node_r = i - 1
                neighbor_node_c = j
        elif ii == 2:
            if i < nrows - 1:
                neighbor_node_r = i + 1
                neighbor_node_c = j
        elif ii == 3:
            if j > 0:
                neighbor_node_r = i
                neighbor_node_c = j - 1
        elif ii == 4:
            if j < ncols - 1:
                neighbor_node_r = i
                neighbor_node_c = j + 1
        # 探索済/障害物/ゴールかどうか
        if map[neighbor_node_r,neighbor_node_c] != 3 and\
            map[neighbor_node_r,neighbor_node_c] != 2 and\
            map[neighbor_node_r,neighbor_node_c] != 5:
            # 距離をプラス1
            dist = min_dist + 1
            # 最短なら更新
            if dist < distanceFromStart[neighbor_node_r,neighbor_node_c]:
                distanceFromStart[neighbor_node_r,neighbor_node_c] = dist
                parent[neighbor_node_r,neighbor_node_c] = current
                map[neighbor_node_r,neighbor_node_c] = 4
        print('distance')
        print(distanceFromStart)
        print('status')
        print(map)
        time.sleep(0.3)
        print('')
# 最短経路を表示する
# ゴールの線形インデックス座標
print('found shortest path!')
route = np.array([dest_coords[0] * ncols + dest_coords[1] - 1])
# 普通の座標に変換
route_r = int(route[0] // ncols)
route_c = int(route[0] - (route[0] // ncols) * ncols)
# スタート地点まで
while parent[route_r,route_c] != 0:
    # 親座標まで1コマ戻る
    route = np.append(parent[route_r,route_c],route)
    # 0で表示
    map[route_r,route_c] = 0
    # 座標を更新
    route_r = int(route[0] // ncols)
    route_c = int(route[0] - (route[0] // ncols) * ncols)
    print(map)
    print('')
    time.sleep(0.5)