A* アルゴリズム

A*アルゴリズムとは

近場から探索を開始して，（さすがに障害物は避けるけど）ゴールに着くまで手当たり次第に探索＆記録していった幅優先探索では，迷路の大きさ＝探索範囲の広さだった．

当たり前だが，探索範囲が広くなればなるほど計算コストも増加する．
つまり，広い迷路において「待てばゴールに着けるし，最短経路も出せるけど，コスパが悪い」のが前回行った幅優先探索だった．

一方でA*アルゴリズムとは，ゴールのありそうな方向に向かって探索を進めていくことで，探索範囲を絞り，コスパよくゴールを探してくれるアルゴリズムのことである．

A*アルゴリズムの解説

A*アルゴリズムでは，トータルコスト F(n) が小さくなるように次のノード（探索点）を選び探索を進めていく．

F(n) は以下の式によってあらわされ，特に H(n) のおかげで探索範囲は小さくなる．（H(n) がゼロの場合，ダイクストラ法と呼ばれる手法になる）
☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆

\begin{align}
& F(n) = G(n) + H(n) \\
 ただし，\\
& F(n):トータルコスト \\
& G(n):スタートから現在地までの移動にかかったコスト(移動コスト) \\
& H(n):現在地からゴールまでにかかるだろうコスト(ヒューリスティック距離) \\
\end{align}

☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆

• 移動コストG(n) ：これまでに移動してきた実際の経路における移動コストであり，経路の長さや移動経路にもとづく各マス間の移動コストの和となる．

• ヒューリスティック距離H(n) ：正確な距離ではなく仮に見積もった距離．つまり，「現在地からゴールまで，どのような経路をたどるか分からないけど，大体このくらい」という大雑把な距離のこと．現在地からゴールまでの直線距離を使うことが多い．

探索方法（イメージ）

ノードの選択方法を下図を例に述べる．現在地は黄色い丸で，赤矢印で表されたように上下左右にのみ移動できるとする．

1. 現在地までの移動コストG(n) は青い移動経路の長さ
2. 現在地から移動できる(n+1番目の)ノードは，赤矢印で示されている４点
3. 各点のヒューリスティック距離H(n) は緑色の直線で表される
4. トータルコストF(n) を計算
5. 探索点候補に加えるか検討する
   • 左隣の点：ここは(n-1番目に)一度通った点．
     H は一度目も二度目も同じだが，行って戻る分，G は２度目のほうが大きくなる．
     つまり，左隣に移動する場合，F(n-1)<F(n+1)となるから，戻らない方がいい！ と判断
   • 右隣＆上下の点：一度も通っていない点で，スタート＆ゴールでも，障害物でもない点
     とりあえず，探索候補点に追加する
6. 探索候補点の中から，次の探索点を決定
   例えば，探索候補点が先ほど追加した３点のみとすると，トータルコストは$F(下)>F(右)>F(上)$ の順に小さくなる．したがって，次の探索点には上の点が選ばれる．

このように，A*アルゴリズムでは，ゴールと反対側にある（今回では下の点）ような，明らかに最短経路に関係なさそうな点の探索は後回しにすることで，効率よく探索が可能である．

不変の迷路探索

まず，形の変化しない迷路についてA*を用いて探索を行った．

上図の左側では，各ノードのF(n) の値を表示している．見ての通り，左下のスタートから右上のゴールに近づくにつれて，F(n) の値が小さくなっているのが分かる．

また，探索した範囲も最短距離ではなさそうな左上や右下を含んでおらず，探索範囲を絞れていることが分かる．（ちなみに青×が探索したノード，赤が最短経路）

コードの全容

manual_main.py

import numpy

import matplotlib.pyplot as plt
from matplotlib import patches
from matplotlib.animation import FuncAnimation

class Maze:
    def __init__(self, map_size=(10,10), object_num=25):
        self.map_size = map_size
        self.object_num = object_num
        self.maze = [[0] * self.map_size[0] for i in range(self.map_size[1])]
        pass

    def generation_map(self, object_list, start, goal):
        
        # set cost of maze outline
        for i in range(self.map_size[1]):
            self.maze[0][i] = object_cost
            self.maze[self.map_size[0] - 1][i] = object_cost

        for j in range(self.map_size[0]):
            self.maze[j][0] = object_cost
            self.maze[j][self.map_size[1] - 1] = object_cost

        # set cost of objects (like wall...). Number of object is set at first
        if object_list:
            for object in object_list:
                self.maze[object[0]][object[1]] = object_cost

        # set start&goal position with position class
        self.goal = goal
        self.start = start
        # set cost
        self.maze[start.x][start.y] = -1
        self.maze[goal.x][goal.y] = 1
        # set full cost
        self.goal.set_ini_cost(self.goal)
        self.start.set_ini_cost(self.goal)
        return self.maze, self.start, self.goal

    def twice_graph(self, route_list, trace_list):
        fig = plt.figure(figsize=self.map_size)

        #グラフを描画するsubplot領域を作成。
        ax1 = fig.add_subplot(1, 2, 1)
        ax2 = fig.add_subplot(1, 2, 2)

    
        ax1.set_xticks(list(range(self.map_size[0]+1)))
        ax1.set_yticks(list(range(self.map_size[1]+1)))
        ax1.grid()
        ax1.set_aspect('equal')

        
        ax2.set_xticks(list(range(self.map_size[0]+1)))
        ax2.set_yticks(list(range(self.map_size[1]+1)))
        ax2.grid()
        ax2.set_aspect('equal')


        # for before
        for x in range(self.map_size[0]):
                for y in range(self.map_size[1]):
                    if self.maze[x][y] == object_cost:
                        r = patches.Rectangle( xy=(x,y), width=1, height=1, color="grey", alpha=0.5) # 四角形のオブジェクト
                        # 4. Axesに図形オブジェクト追加・表示
                        ax1.add_patch(r)

        # # show start&goal position with box
        # r = patches.Rectangle( xy=(self.start.x,self.start.y), width=1, height=1, color="blue", alpha=0.5) # 四角形のオブジェクト
        # ax1.add_patch(r)
        # r = patches.Rectangle( xy=(self.goal.x,self.goal.y), width=1, height=1, color="red", alpha=0.5) # 四角形のオブジェクト
        # ax1.add_patch(r)
        for p in trace_list:
            ax1.text(p.x + 0.1, p.y + 0.1, round(p.h,1))

        # show start&goal position with star
        s2g = [[self.start.convert_coordinate().x, self.goal.convert_coordinate().x],
                 [self.start.convert_coordinate().y, self.goal.convert_coordinate().y]]
        
        ax1.scatter(s2g[0], s2g[1], c="orange", marker="*", s=350)
        ax2.scatter(s2g[0], s2g[1], c="orange", marker="*", s=350, zorder=5)
        
        # for after
        for x in range(self.map_size[0]):
            for y in range(self.map_size[1]):
                if self.maze[x][y] == object_cost:
                    r = patches.Rectangle( xy=(x,y), width=1, height=1, color="grey",alpha=0.5) # 四角形のオブジェクト
                    # 4. Axesに図形オブジェクト追加・表示
                    ax2.add_patch(r)

        # show all node explored with blue x
        for i in range(len(trace_list)):
            ax2.scatter(trace_list[i].convert_coordinate().x, trace_list[i].convert_coordinate().y, c="blue", marker="x")
        
        # # show all node explored with yellow box
        # for i in range(len(trace_list)):
        #     r = patches.Rectangle( xy=(trace_list[i].x, trace_list[i].y), width=1, height=1, color="yellow", alpha=0.5) # 四角形のオブジェクト
        #     # 4. Axesに図形オブジェクト追加・表示
        #     ax2.add_patch(r)

        # show route node with red x
        for i in range(len(route_list)):
            ax2.scatter(route_list[i].convert_coordinate().x, route_list[i].convert_coordinate().y, c="red", marker="x")
        
        # # show route node with red green box
        # for i in range(len(route_list)):
        #     r = patches.Rectangle( xy=(route_list[i].x, route_list[i].y), width=1, height=1, color="green", alpha=0.5) # 四角形のオブジェクト
        #     # 4. Axesに図形オブジェクト追加・表示
        #     ax2.add_patch(r)

        # # show start&goal position with box
        # r = patches.Rectangle( xy=(self.start.x,self.start.y), width=1, height=1, color="blue", alpha=0.5) # 四角形のオブジェクト
        # ax2.add_patch(r)
        # r = patches.Rectangle( xy=(self.goal.x,self.goal.y), width=1, height=1, color="red", alpha=0.5) # 四角形のオブジェクト
        # ax2.add_patch(r)

        plt.show()
    
    def set_maze_graph(self):
        fig, ax = plt.subplots(figsize=self.map_size)
    
        ax.set_xticks(list(range(self.map_size[0]+1)))
        ax.set_yticks(list(range(self.map_size[1]+1)))
        ax.grid()
        ax.set_aspect('equal')

        # set maze
        for x in range(self.map_size[0]):
                for y in range(self.map_size[1]):
                    if self.maze[x][y] == object_cost:
                        r = patches.Rectangle( xy=(x,y), width=1, height=1, color="grey", alpha=0.5) # 四角形のオブジェクト
                        # 4. Axesに図形オブジェクト追加・表示
                        ax.add_patch(r)
        # set star&goal position
        s2g = [[self.start.convert_coordinate().x, self.goal.convert_coordinate().x],
                 [self.start.convert_coordinate().y, self.goal.convert_coordinate().y]]
        
        ax.scatter(s2g[0], s2g[1], c="orange", marker="*", s=350)
        return fig, ax

    def animation(self, frame):
        ax.clear()
        # draw maze
        for x in range(self.map_size[0]):
                for y in range(self.map_size[1]):
                    if self.maze[x][y] == object_cost:
                        r = patches.Rectangle( xy=(x,y), width=1, height=1, color="grey", alpha=0.5) # 四角形のオブジェクト
                        # 4. Axesに図形オブジェクト追加・表示
                        ax.add_patch(r)
        # set start&goal
        s2g = [[self.start.convert_coordinate().x, self.goal.convert_coordinate().x],
                 [self.start.convert_coordinate().y, self.goal.convert_coordinate().y]]
        
        ax.scatter(s2g[0], s2g[1], c="orange", marker="*", s=350)
        plt.text(self.start.x + 0.7, self.start.y + 0.7, "S")
        plt.text(self.goal.x + 0.7, self.goal.y + 0.7, "G")
        
        # trace the route
        for i in range(frame+1):
            ax.scatter(trace_list[i].convert_coordinate().x, trace_list[i].convert_coordinate().y, c="blue", marker="x", zorder=0)

        # show route list at the end
        if frame+1 == len(trace_list):
            for i in range(1,len(route_list)-1):
                ax.scatter(route_list[i].convert_coordinate().x, route_list[i].convert_coordinate().y, c="red", marker="x", zorder=1)
        
        ax.set_xlim([0, self.map_size[0]])
        ax.set_ylim([0, self.map_size[1]])
        
        ax.set_xticks(list(range(self.map_size[0]+1)))
        ax.set_yticks(list(range(self.map_size[1]+1)))
        ax.grid()
        ax.set_aspect('equal')

class Position:
    def __init__(self, x, y, depth=0, parent_x=-1, parent_y=-1):
        self.x = x                  # x座標 
        self.y = y                  # y座標
        self.depth = depth          # 移動回数
        self.parent_x = parent_x    # 一つ前に通ったマスのx座標
        self.parent_y = parent_y    # 一つ前に通ったマスのy座標

        # cost for A* algorithm
        self.g = self.depth         # cost from the start to the current
        # cost from the current to the goal
        self.h = 0
        self.f = self.g + self.h    # total cost

    def set_ini_cost(self, goal):
        # cost for A* algorithm
        self.g = self.depth         # cost from the start to the current
        # cost from the current to the goal
        self.h = numpy.sqrt((goal.x -self.x)**2 + (goal.y - self.y)**2)
        self.f = self.g + self.h    # total cost

    def cal_cost(self):
        # cost for A* algorithm
        self.g = self.depth         # cost from the start to the current
        # cost from the current to the goal
        self.h = numpy.sqrt((goal.x -self.x)**2 + (goal.y - self.y)**2)
        self.f = self.g + self.h    # total cost
        
    def add_around(self):
        x, y, depth = self.x, self.y, self.depth
        left_pos = Position(x-1, y, depth+1, x, y)
        left_pos.cal_cost()
        # print(left_pos.h)
        right_pos = Position(x+1, y, depth+1, x, y)
        right_pos.cal_cost()
        # print(right_pos.h)
        up_pos = Position(x, y-1, depth+1, x, y)
        up_pos.cal_cost()
        down_pos = Position(x, y+1, depth+1, x, y)
        down_pos.cal_cost()
        
        around_list = []
        
        around_list.extend([left_pos, right_pos, up_pos, down_pos])
        return around_list

    def convert_coordinate(self):
        graph_coordinate = Position(self.x+0.5, self.y+0.5)
        return graph_coordinate
    
def RouteRecord(trace_list):
    # get number of all traced position to achieve the goal
    num_trace = len(trace_list)
    # num_goal = num_trace - 1
    n = num_trace - 1
    route_list = []
    cnt = 0
    while True:
        child = trace_list[n]
        route_list.append(child)
        # print(cnt)
        if (cnt>len(trace_list)):
            print("Error: count is over list length")
            return route_list
        else:
            cnt = cnt+1
            if child.parent_x > 0:
                # find the list number of goal's parent position and update "n" to it
                for i in range(num_trace):
                    p = trace_list[i]
                    if p.x == child.parent_x and p.y == child.parent_y:
                            n = i
                            break
                    else:
                        continue
                    
            else:
                break
    return route_list


# 探索関数：ゴールしたらそのときの位置・移動数を返す
def Search(start):
    # スタート位置（x座標, y座標, 移動回数）をセット
    ## this can be called as open list
    explore_list = [start]
    # set list to trace path
    ## this can be called as closed list
    trace_list = [start]
    route_list = []
    
    # while len(explore_list) > 0:# 探索可能ならTrue(listが空ならfalse)
    while len(explore_list)>0:
        # リストから探索する位置を取得
        ## the point with smallest cost should be explored
        f_min = 100000
        for index, listed in enumerate(explore_list):
            if listed.f < f_min:
                # print("HI")
                f_min = listed.f
                last_point = explore_list.pop(index) 
            else:
                continue

        # update explore list
        around_list = last_point.add_around()
        for i in range(len(around_list)):
            p = around_list[i]
            # print(p.h)
            # if the point is outside of maze or on object, skip
            if maze[p.x][p.y] == object_cost:
                continue
            # if the point is start, skip
            elif maze[p.x][p.y] == -1:
                continue
            # if the point is already on the trace list, compare the cost
            ## if the cost of the point in current route is smaller than that of listed one,
            ## remove listed one from trace list, and add the current to explore&trace list as new
            elif maze[p.x][p.y] == 2:
                # find the point in trace_list
                for listed in trace_list:
                    if listed.x == p.x and listed.y == p.y:
                        # compare the cost
                        if p.f < listed.f: # if current route cost smaller
                            trace_list.remove(listed) # remove listed one
                            # add the current to explore&trace list
                            explore_list.append(p)
                            trace_list.append(p)
                        else:
                            continue

            # if the point is goal, FINISH
            elif maze[p.x][p.y] == 1: 
                print("Find goal & Record it")               
                trace_list.append(p)
                print(len(trace_list))
                route_list = RouteRecord(trace_list)
                # route_list = []
                return route_list, trace_list
            
            # if the point should be explored, add to the explore list         
            elif maze[p.x][p.y] == 0:
                maze[p.x][p.y] = 2
                # update explore list
                explore_list.append(p)
                trace_list.append(p)

            # if the point is arounded by objects&explored point, skip 
            else:
                print("No route to go")
                continue
    
    print("No route was found")
    route_list=[], trace_list=[]
    return route_list, trace_list

if __name__ == '__main__':
    object_cost = 100
    object_list = [(2,2), (2,3), (3,2), (3,3), (6,2), (6,3), (7,2), (7,3), (6,6), (6,7), (7,6), (7,7)]
    # object_list = [(4,4), (5,4), (6,4), (4,5), (5,5), (6,5), (4,6), (5,6), (6,6)]
    start = Position(1, 1, 0)
    goal = Position(8, 8, 0)
    
    create_maze = Maze()
    maze, start, goal = create_maze.generation_map(object_list, start, goal) # generate map
    fig, ax = create_maze.set_maze_graph()
    route_list, trace_list= Search(start)  # 探索
    # Check if the return value Search func returned
    ## if 0 was returned, not route was found
    if route_list == [] and trace_list == []:
        print("Try again")
    else:
        create_maze.twice_graph(route_list, trace_list)
        # アニメーション化
        anim = FuncAnimation(fig, create_maze.animation, frames=len(trace_list), interval=100, repeat=False)
        anim.save("test.gif", writer="pillow")
        plt.show()

コードの概説

このコードは，主に以下の４つからなる．

• Mazeクラス
• Positionクラス
• RouteRecord() 関数
• Search() 関数

説明の便宜上，Positionクラスから開設する

Positionクラス

追記＆変更したのは以下の３つ

• 初期値（__init__関数）
• set_ini_cost(), cal_cost() 関数
• add_around() 関数

__init__関数

[x,y]座標情報に加えて，A*アルゴリズムに基づくコストを設定した．ただし，ここでいうコストとは，Mazeクラスで設定しているobject_costなどとは異なる．（object_costは通れるか否かの判断にのみ使い，ここでいうコストは最適経路選択の際の判断材料となる）

コストの設定方法は解説のとおり．

Position class

    def __init__(self, x, y, depth=0, parent_x=-1, parent_y=-1):
        ・・・
        # cost for A* algorithm
        self.g = self.depth         # cost from the start to the current
        # cost from the current to the goal
        self.h = 0
        self.f = self.g + self.h    # total cost

set_ini_cost(), cal_cost() 関数

この二つの関数は，現在位置のトータルコストを計算する．
ただし，

• set_ini_cost() ：Mazeクラス内でスタート＆ゴール位置のコストを計算する用
• cal_cost() ：スタート＆ゴール位置をグローバル変数として利用するので，迷路を設定したあとにのみ使用できる

という違いがある．

Position class

    def set_ini_cost(self, goal):
        # cost for A* algorithm
        self.g = self.depth         # cost from the start to the current
        # cost from the current to the goal
        self.h = numpy.sqrt((goal.x -self.x)**2 + (goal.y - self.y)**2)
        self.f = self.g + self.h    # total cost

    def cal_cost(self):
        # cost for A* algorithm
        self.g = self.depth         # cost from the start to the current
        # cost from the current to the goal
        self.h = numpy.sqrt((goal.x -self.x)**2 + (goal.y - self.y)**2)
        self.f = self.g + self.h    # total cost
    

add_around() 関数

この関数では，現在地の（上下左右）周囲にあるグリッドの座標を取得し，リストとして返す．

さらにここで，移動回数（depth）を更新したり，現在地を次のグリッドのparentとして登録したり，トータルコストを計算したりしている．

Position class

    def add_around(self):
        x, y, depth = self.x, self.y, self.depth
        left_pos = Position(x-1, y, depth+1, x, y)
        left_pos.cal_cost()
        # print(left_pos.h)
        right_pos = Position(x+1, y, depth+1, x, y)
        right_pos.cal_cost()
        # print(right_pos.h)
        up_pos = Position(x, y-1, depth+1, x, y)
        up_pos.cal_cost()
        down_pos = Position(x, y+1, depth+1, x, y)
        down_pos.cal_cost()
        
        around_list = []
        
        around_list.extend([left_pos, right_pos, up_pos, down_pos])
        return around_list

Mazeクラス

ここでは，手動で迷路を生成した．迷路生成の詳細については☟を参照．

☝のページに記載されているコードをもとに，A*を実行するために追記した部分がある．このページでは，その部分についてのみ解説する．

• generation_map() 関数
• twice_graph() 関数
• animation() 関数

generation_map() 関数

迷路を生成する関数．ここには，スタート位置とゴール位置におけるトータルコスト（f(n)）を計算するコードを追記した．

Maze class

    def generation_map(self, object_list, start, goal):
        ・・・
        # set full cost
        self.goal.set_ini_cost(self.goal)
        self.start.set_ini_cost(self.goal)

twice_graph() 関数

この関数では，☟のようにグラフを2枚横並びに表示する．

左側のグラフでは探索したグリッド全てに，各グリッドのヒューリスティック関数（h(n)）の値が表示されている．
それに対して，右側のグラフは探索したグリッドを ✕ で，最適経路を✕ で表している．

Maze class

def twice_graph(self, route_list, trace_list):
        fig = plt.figure(figsize=self.map_size)

        #グラフを描画するsubplot領域を作成。
        ax1 = fig.add_subplot(1, 2, 1)
        ax2 = fig.add_subplot(1, 2, 2)

    
        ax1.set_xticks(list(range(self.map_size[0]+1)))
        ax1.set_yticks(list(range(self.map_size[1]+1)))
        ax1.grid()
        ax1.set_aspect('equal')

        
        ax2.set_xticks(list(range(self.map_size[0]+1)))
        ax2.set_yticks(list(range(self.map_size[1]+1)))
        ax2.grid()
        ax2.set_aspect('equal')


        # for before
        for x in range(self.map_size[0]):
                for y in range(self.map_size[1]):
                    if self.maze[x][y] == object_cost:
                        r = patches.Rectangle( xy=(x,y), width=1, height=1, color="grey", alpha=0.5) # 四角形のオブジェクト
                        # 4. Axesに図形オブジェクト追加・表示
                        ax1.add_patch(r)

        # # show start&goal position with box
        # r = patches.Rectangle( xy=(self.start.x,self.start.y), width=1, height=1, color="blue", alpha=0.5) # 四角形のオブジェクト
        # ax1.add_patch(r)
        # r = patches.Rectangle( xy=(self.goal.x,self.goal.y), width=1, height=1, color="red", alpha=0.5) # 四角形のオブジェクト
        # ax1.add_patch(r)
        for p in trace_list:
            ax1.text(p.x + 0.1, p.y + 0.1, round(p.h,1))

        # show start&goal position with star
        s2g = [[self.start.convert_coordinate().x, self.goal.convert_coordinate().x],
                 [self.start.convert_coordinate().y, self.goal.convert_coordinate().y]]
        
        ax1.scatter(s2g[0], s2g[1], c="orange", marker="*", s=350)
        ax2.scatter(s2g[0], s2g[1], c="orange", marker="*", s=350, zorder=5)
        
        # for after
        for x in range(self.map_size[0]):
            for y in range(self.map_size[1]):
                if self.maze[x][y] == object_cost:
                    r = patches.Rectangle( xy=(x,y), width=1, height=1, color="grey",alpha=0.5) # 四角形のオブジェクト
                    # 4. Axesに図形オブジェクト追加・表示
                    ax2.add_patch(r)

        # show all node explored with blue x
        for i in range(len(trace_list)):
            ax2.scatter(trace_list[i].convert_coordinate().x, trace_list[i].convert_coordinate().y, c="blue", marker="x")
        
        # # show all node explored with yellow box
        # for i in range(len(trace_list)):
        #     r = patches.Rectangle( xy=(trace_list[i].x, trace_list[i].y), width=1, height=1, color="yellow", alpha=0.5) # 四角形のオブジェクト
        #     # 4. Axesに図形オブジェクト追加・表示
        #     ax2.add_patch(r)

        # show route node with red x
        for i in range(len(route_list)):
            ax2.scatter(route_list[i].convert_coordinate().x, route_list[i].convert_coordinate().y, c="red", marker="x")
        
        # # show route node with red green box
        # for i in range(len(route_list)):
        #     r = patches.Rectangle( xy=(route_list[i].x, route_list[i].y), width=1, height=1, color="green", alpha=0.5) # 四角形のオブジェクト
        #     # 4. Axesに図形オブジェクト追加・表示
        #     ax2.add_patch(r)

        # # show start&goal position with box
        # r = patches.Rectangle( xy=(self.start.x,self.start.y), width=1, height=1, color="blue", alpha=0.5) # 四角形のオブジェクト
        # ax2.add_patch(r)
        # r = patches.Rectangle( xy=(self.goal.x,self.goal.y), width=1, height=1, color="red", alpha=0.5) # 四角形のオブジェクト
        # ax2.add_patch(r)

        plt.show()

animation() 関数

この関数をFuncAnimation() 関数の引数とすることで，グリッドを探索する様子をアニメーションで表示する．

✕ が探索グリッドを，✕ が最短経路を表しているのは先ほどと変わらない．スタート位置からゴール位置に向かって探索エリアが延びているのが分かるよう，探索した順番がtrace_listに格納されている必要がある．

Maze class

    def animation(self, frame):
        ax.clear()
        # draw maze
        for x in range(self.map_size[0]):
                for y in range(self.map_size[1]):
                    if self.maze[x][y] == object_cost:
                        r = patches.Rectangle( xy=(x,y), width=1, height=1, color="grey", alpha=0.5) # 四角形のオブジェクト
                        # 4. Axesに図形オブジェクト追加・表示
                        ax.add_patch(r)
        # set start&goal
        s2g = [[self.start.convert_coordinate().x, self.goal.convert_coordinate().x],
                 [self.start.convert_coordinate().y, self.goal.convert_coordinate().y]]
        
        ax.scatter(s2g[0], s2g[1], c="orange", marker="*", s=350)
        plt.text(self.start.x + 0.7, self.start.y + 0.7, "S")
        plt.text(self.goal.x + 0.7, self.goal.y + 0.7, "G")
        
        # trace the route
        for i in range(frame+1):
            ax.scatter(trace_list[i].convert_coordinate().x, trace_list[i].convert_coordinate().y, c="blue", marker="x", zorder=0)

        # show route list at the end
        if frame+1 == len(trace_list):
            for i in range(1,len(route_list)-1):
                ax.scatter(route_list[i].convert_coordinate().x, route_list[i].convert_coordinate().y, c="red", marker="x", zorder=1)
        
        ax.set_xlim([0, self.map_size[0]])
        ax.set_ylim([0, self.map_size[1]])
        
        ax.set_xticks(list(range(self.map_size[0]+1)))
        ax.set_yticks(list(range(self.map_size[1]+1)))
        ax.grid()
        ax.set_aspect('equal')

RouteRecord() 関数

ゴールからparent情報をたどっていくことで，A*によって発見されたルートが分かる．ここでは，探索したグリッド情報（trace_list）を受け取ってそのルートをroute_listとして返す．

RouteRecord()

def RouteRecord(trace_list):
    # get number of all traced position to achieve the goal
    num_trace = len(trace_list)
    # num_goal = num_trace - 1
    n = num_trace - 1
    route_list = []
    cnt = 0
    while True:
        child = trace_list[n]
        route_list.append(child)
        # print(cnt)
        if (cnt>len(trace_list)):
            print("Error: count is over list length")
            return route_list
        else:
            cnt = cnt+1
            if child.parent_x > 0:
                # find the list number of goal's parent position and update "n" to it
                for i in range(num_trace):
                    p = trace_list[i]
                    if p.x == child.parent_x and p.y == child.parent_y:
                            n = i
                            break
                    else:
                        continue
                    
            else:
                break
    return route_list

Search() 関数

ここでは，ゴールが見つかるまで以下の3つの手順を繰り返すことで探索を行う．ただし，ゴールにたどり着く前に探索候補リストが空になった場合は，"No Route to go"と表示される．

1. 探索候補リスト（explore_list）を更新
2. もっともトータルコストが小さくなりそうなグリッドを選ぶ
3. そのグリッドの上下左右4点について...
   • 障害物の場合：無視
   • スタート位置の場合：無視
   • すでに探索済みリスト（trace_list）に含まれている場合：リストに載っているものと今のルートのトータルコストを比較して，今のほうがコストが小さいならば，古いほうを削除&今のを新規追加する
   • ゴール位置の場合：探索終了．route_listを生成して，route_listとtrace_listを返す
   • そのほかの未探索地点の場合：探索候補リストに追加する

Search()

def Search(start):
    # スタート位置（x座標, y座標, 移動回数）をセット
    ## this can be called as open list
    explore_list = [start]
    # set list to trace path
    ## this can be called as closed list
    trace_list = [start]
    route_list = []
    
    # while len(explore_list) > 0:# 探索可能ならTrue(listが空ならfalse)
    while len(explore_list)>0:
        # リストから探索する位置を取得
        ## the point with smallest cost should be explored
        f_min = 100000
        for index, listed in enumerate(explore_list):
            if listed.f < f_min:
                # print("HI")
                f_min = listed.f
                last_point = explore_list.pop(index) 
            else:
                continue

        # update explore list
        around_list = last_point.add_around()
        for i in range(len(around_list)):
            p = around_list[i]
            # print(p.h)
            # if the point is outside of maze or on object, skip
            if maze[p.x][p.y] == object_cost:
                continue
            # if the point is start, skip
            elif maze[p.x][p.y] == -1:
                continue
            # if the point is already on the trace list, compare the cost
            ## if the cost of the point in current route is smaller than that of listed one,
            ## remove listed one from trace list, and add the current to explore&trace list as new
            elif maze[p.x][p.y] == 2:
                # find the point in trace_list
                for listed in trace_list:
                    if listed.x == p.x and listed.y == p.y:
                        # compare the cost
                        if p.f < listed.f: # if current route cost smaller
                            trace_list.remove(listed) # remove listed one
                            # add the current to explore&trace list
                            explore_list.append(p)
                            trace_list.append(p)
                        else:
                            continue

            # if the point is goal, FINISH
            elif maze[p.x][p.y] == 1: 
                print("Find goal & Record it")               
                trace_list.append(p)
                print(len(trace_list))
                route_list = RouteRecord(trace_list)
                # route_list = []
                return route_list, trace_list
            
            # if the point should be explored, add to the explore list         
            elif maze[p.x][p.y] == 0:
                maze[p.x][p.y] = 2
                # update explore list
                explore_list.append(p)
                trace_list.append(p)

            # if the point is arounded by objects&explored point, skip 
            else:
                print("No route to go")
                continue
    
    print("No route was found")
    route_list=[], trace_list=[]
    return route_list, trace_list