前回のダイクストラ(Dijkstra)法に続いてA*をやってみました。違いは、探索候補を決めるときにダイクストラ法ではスタート地点からの距離が最小になるものを選んでいたのに対し、A*ではそれに加えてゴール地点からの距離（マンハッタン距離）も考慮するというものです。考慮といっても単純に加算するのみです。マンハッタン距離とは縦横の移動のみを認めて斜め移動は禁止する道のりのことです。斜め移動しない設定にしているからです。
結果はダイクストラ法と同じになりますが、あさっての方向を探索しなくなるのでかなり効率的になります。マトリクスを見ても未探索のフリーのセルが結構残っています。
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に設定
s_dist = np.ones(nrows*ncols).reshape(nrows,ncols) * 100
# 各セルのゴールからのマンハッタン距離をつくる
x,y = np.meshgrid(np.linspace(0,ncols-1,ncols),np.linspace(0,nrows-1,nrows))
g_dist = abs(x - dest_coords[1]) + abs(y - dest_coords[0])
# トータル距離
t_dist = np.ones(nrows*ncols).reshape(nrows,ncols) * 100
# スタート地点は距離0
s_dist[start_coords[0],start_coords[1]] = 0
t_dist[start_coords[0],start_coords[1]] = g_dist[start_coords[0],start_coords[1]]
# 探索用 親セル
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(t_dist)
current = np.argmin(t_dist)
# ゴールに達したら抜ける
if current == dest_node or min_dist == 100:
break
# 通常の座標に変換
i = int(current//ncols)
j = int(current-(current//ncols)*ncols)
# 探索済とする
map[i, j]=3
# 次回の探索から外す
t_dist[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:
# 距離をプラス１
s_dist[neighbor_node_r,neighbor_node_c] = s_dist[i][j] + 1
# 最短なら更新
if (s_dist[neighbor_node_r,neighbor_node_c]+g_dist[neighbor_node_r,neighbor_node_c]) < t_dist[neighbor_node_r,neighbor_node_c]:
t_dist[neighbor_node_r,neighbor_node_c] = s_dist[neighbor_node_r,neighbor_node_c]+g_dist[neighbor_node_r,neighbor_node_c]
parent[neighbor_node_r,neighbor_node_c] = current
map[neighbor_node_r,neighbor_node_c] = 4
print('distance')
print(t_dist)
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:
# 親座標まで１コマ戻る
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)
