지난 시간에는 A* 알고리즘의 작동 방식을 알아보았습니다.
그럼 오늘은 실제로 A* 알고리즘을 구현해 보도록 합시다.

💻구현

우선 이전 시간에 자료로 사용한 미로를 리스트로 표현해봅시다. 7 by 7 리스트가 되겠네요. 길은 True, 나머지는 False로 표현하겠습니다.

matrix = [
    [True, True, True, False, False, False, False],
    [True, False, True, False, False, False, False],
    [True, False, True, True, True, True, True],
    [True, False, True, False, False, False, True],
    [True, False, True, False, True, True, True],
    [True, False, True, False, True, False, False],
    [True, True, True, True, True, True, True],
]

그리고 시작점과 도착점을 각각 start, dest(destination) 라는 변수로 선언하겠습니다.

Coord = Tuple[int, int]

start = (2, 2)
dest = (6, 6)

개발 편의성을 위해 타입 힌트를 사용합니다.

a_star 함수 구현

A* 알고리즘을 수행할 함수는 아래와 같이 구현할 것입니다.

total_cost, paths, vis, heuristic_cost = a_star(matrix, start, dest)

미로와 시작 지점, 목표 지점을 파라미터로 넘기면 최단 거리와 최단 경로, 방문한 길, 그리고 휴리스틱 코스트를 리턴해주는 형태가 될 것입니다. 그럼 A* 알고리즘의 함수를 구현해 봅시다.

from typing import List, Tuple, Callable
import math
import heapq

# 방향 벡터
d_row = (-1, 0, 1, 0)
d_col = (0, 1, 0, -1)

def a_star(
    matrix: List[List[int]], start: Coord, dest: Coord
) -> Tuple[int, List[Coord]]:
    global d_row
    global d_col

    h = len(matrix)
    w = len(matrix[0])

    # 휴리스틱 코스트 테이블
    heuristic_cost = [[float("inf")] * w for _ in range(h)]

    # 휴리스틱 코스트 구하기
    for i in range(h):
        for j in range(w):
            if matrix[i][j]:
                heuristic_cost[i][j] = round(get_euclidean_distance((i, j), dest))

    row, col = start
    dest_y, dest_x = dest

    vis = [[False] * w for _ in range(h)]

    heap = []
    heapq.heappush(heap, (heuristic_cost[row][col] + 0, row, col))

    total_cost = 0
    # 어떤 노드에서 어떤 노드로 이동하는지 저장할 리스트
    came_from = []

    """
    heap이 비거나 목적 지점에 도착할 때까지 반복:
        heap에서 cost가 최소인 값을 꺼내서 방문처리를 한 후,
        유효한 인접 노드가 있으면 코스트를 계산해 힙에 넣는다.
    """
    while heap and (row, col) != (dest_y, dest_x):
        total_cost, row, col = heapq.heappop(heap)

        # Total Cost 에서 휴리스틱 코스트를 빼면 시작 지점에서 현재 지점까지의 실제 거리를 구할 수 있음
        depth = total_cost - heuristic_cost[row][col]

        # 방문 처리
        vis[row][col] = True

        # 유효한 인접 노드가 있으면 코스트를 계산해 힙에 넣는다.
        for i in range(4):
            adjy = row + d_row[i]
            adjx = col + d_col[i]
            if is_vaild(matrix, vis, adjy, adjx):
                total_cost = heuristic_cost[adjy][adjx] + depth + 1
                came_from.append(((row, col), (adjy, adjx)))
                heapq.heappush(heap, (total_cost, adjy, adjx))

    # came_from을 역순으로 추적하여 최단 경로를 찾음
    from_y, from_x = came_from[-1][0]
    paths = []

    for i in range(len(came_from) - 1, -1, -1):
        from_coord, to_coord = came_from[i]
        to_y, to_x = to_coord

        if from_y == to_y and from_x == to_x:
            from_y, from_x = from_coord
            paths.insert(0, to_coord)

    return total_cost, paths, vis, heuristic_cost

코드가 길죠? 위에서부터 차근차근 봅시다. 우선 미로는 상, 하, 좌, 우의 네 방향으로 이동 가능하므로, 방향 벡터를 선언해 줍니다.

# 방향 벡터
d_row = (-1, 0, 1, 0)
d_col = (0, 1, 0, -1)

휴리스틱 코스트 구하기

그 다음은 a_star 함수의 내부를 봅시다. 우선 휴리스틱 코스트를 저장할 리스트를 초기화합니다. 리스트의 각 엘리먼트들은 무한으로 설정합니다. 그 후, 각각의 길을 돌면서 matrix의 값이 True라면(해당 노드가 미로의 길에 해당한다면), 휴리스틱 코스트를 계산해서 반올림한 후에 할당해 줍니다.

# 휴리스틱 코스트 테이블
heuristic_cost = [[float("inf")] * w for _ in range(h)]

# 휴리스틱 코스트 구하기
    for i in range(h):
        for j in range(w):
            if matrix[i][j]:
                heuristic_cost[i][j] = round(get_euclidean_distance((i, j), dest))

이와 같이 미리 휴리스틱 코스트 테이블을 초기화하는 방법도 있지만, 직접 탐색을 하는 과정에서 구하는 편이 연산량이 적을 것입니다. 이 포스트에서는 편의상 미리 구하도록 하겠습니다.

이렇게 구하고 나면, 휴리스틱 코스트 테이블은 아래와 같이 됩니다.

- Heuristic Cost -
8 8 7 . . . .
8 . 6 . . . .
7 . 6 5 4 4 4
7 . 5 . . . 3
6 . 4 . 3 2 2
6 . 4 . 2 . .
6 5 4 3 2 1 0

여기까지 잘 따라오고 계신다면, get_euclidean_distance 라는 함수에 대한 설명이 없는 것에 눈치채셨을 겁니다. get_euclidean_distance는 두 Coord 사이의 유클리드 거리를 리턴하는 함수입니다.

def get_euclidean_distance(pq1: Coord, pq2: Coord) -> float:
    p1, q1 = pq1
    p2, q2 = pq2

    return math.sqrt((p1 - p2) ** 2 + (q1 - q2) ** 2)

탐색 시작

그 다음은 휴리스틱 코스트를 바탕으로 탐색을 시작할 차례입니다.

row, col = start
dest_y, dest_x = dest

vis = [[False] * w for _ in range(h)]

heap = []
heapq.heappush(heap, (heuristic_cost[row][col] + 0, row, col))

total_cost = 0
# 어떤 노드에서 어떤 노드로 이동하는지 저장할 리스트
came_from = []

방문한 길을 저장하기 위한 vis(visited)와 탐색할 길을 저장할 heap을 초기화해 줍니다. Python의 heap은 min-heap이므로, 가까운 길부터 탐색하기에 최적인 자료구조입니다.
그리고 최단 거리를 저장할 total_cost와, 어디서 왔는지를 저장할 리스트인 came_from도 초기화해 줍니다.

Python의 heapq 라이브러리에서는 튜플이 저장될 때, 튜플의 첫 번째 값을 기준으로 min-heap이 구성됩니다.

그럼 이제 탐색을 시작해 봅시다.

"""
heap이 비거나 목적 지점에 도착할 때까지 반복:
    heap에서 cost가 최소인 값을 꺼내서 방문처리를 한 후,
    유효한 인접 노드가 있으면 코스트를 계산해 힙에 넣는다.
"""
while heap and (row, col) != (dest_y, dest_x):
    total_cost, row, col = heapq.heappop(heap)

    # Total Cost 에서 휴리스틱 코스트를 빼면 시작 지점에서 현재 지점까지의 실제 거리를 구할 수 있음
    depth = total_cost - heuristic_cost[row][col]

    # 방문 처리
    vis[row][col] = True

    # 유효한 인접 노드가 있으면 코스트를 계산해 힙에 넣는다.
    for i in range(4):
        adjy = row + d_row[i]
        adjx = col + d_col[i]
        if is_vaild(matrix, vis, adjy, adjx):
            total_cost = heuristic_cost[adjy][adjx] + depth + 1
            came_from.append(((row, col), (adjy, adjx)))
            heapq.heappush(heap, (total_cost, adjy, adjx))

heap에 탐색할 수 있는 길이 남아있거나, 목적 지점에 도착할 때까지 탐색을 반복합니다.

우선 heap에서 가장 가까운 길을 pop합니다. 그리고 시작 지점에서 현재 지점까지의 거리를 구하기 위해, 총 거리 total_cost에서 휴리스틱 코스트 heuristic_cosr[row][col] 를 뺀 depth를 구해서 초기화합니다. 그 후에 현재 길을 방문 처리 한 후, 현재 지점에서 갈 수 있는 길이 있다면 heap에 현재 depth에 1을 더한 코스트와 해당 좌표값을 push합니다.
또한 어디서 왔는지 저장하기 위해, came_from 리스트에 현재 지점과 다음 지점의 쌍을 append해 줍니다.

현재 지점에서 갈 수 있는지를 판별하는 is_valid 함수는 아래와 같습니다.

def is_vaild(
    matrix: List[List[bool]], vis: List[List[bool]], row: int, col: int
) -> bool:
    h = len(matrix)
    w = len(matrix[0])

    # out of bound 처리
    if not (0 <= row < h and 0 <= col < w):
        return False

    # 유효하지 않은 노드 처리
    if not matrix[row][col]:
        return False

    # 이미 방문한 노드 처리
    if vis[row][col]:
        return False

    return True

matrix와 vis, row, col을 인자로 받아 도달할 수 있으면 True, 그렇지 않으면 False를 리턴합니다.

도달할 수 있는 조건은

• matrix를 벗어나지 않았고
• 그곳이 길이며
• 방문한 적이 없어야 함

의 세 가지 조건을 만족해야합니다.

최단 경로 역추적

위와 같이 탐색을 계속하다 보면, 목표 지점에 도달해서 반복이 종료됩니다. 이제 came_from을 통해 목표 지점에서 시작 지점까지 역으로 추적하면, 최단 경로를 구할 수 있게 됩니다.

came_from에는 그림과 같이 (from, to)의 쌍이 저장되어 있습니다. 그러므로 목표 지점의 from으로부터 역추적해서, 현재 지점의 from과 이전 지점의 to가 같은 쌍을 경로를 저장하기 위한 paths 리스트에 append 해줍니다.

그리고 마지막으로 최단 거리, 최단 경로, 방문 테이블, 휴리스틱 코스트 테이블을 리턴해 줍니다. 코드는 아래와 같습니다.

# came_from을 역순으로 추적하여 최단 경로를 찾음
    from_y, from_x = came_from[-1][0]
    paths = []

    for i in range(len(came_from) - 1, -1, -1):
        from_coord, to_coord = came_from[i]
        to_y, to_x = to_coord

        if from_y == to_y and from_x == to_x:
            from_y, from_x = from_coord
            paths.insert(0, to_coord)

    return total_cost, paths, vis, heuristic_cost

보기 좋게 출력

이로써 a_star 함수의 구현은 끝났습니다! 추가적으로, 로직에는 상관없지만 실행 결과를 보기 좋게 프린트하기 위한 함수들을 선언해 줍시다.

def _print_cost(matrix: List[List[int]]) -> None:
    h = len(matrix)
    w = len(matrix[0])

    print("- Heuristic Cost -")
    for i in range(h):
        for j in range(w):
            print("." if math.isinf(matrix[i][j]) else matrix[i][j], end=" ")
        print()
    print()


def _print_path(
    matrix: List[List[bool]], start: Coord, dest: Coord, title: str
) -> None:
    h = len(matrix)
    w = len(matrix[0])

    print(f"---- {title} ----")
    for i in range(h):
        for j in range(w):
            if (i, j) == start:
                print("S", end=" ")
            elif (i, j) == dest:
                print("G", end=" ")
            else:
                print("O" if matrix[i][j] else ".", end=" ")
        print()
    print()


_print_shortest_distance: Callable[
    [Coord, Coord, int], None
] = lambda start, dest, total_cost: print(f"{start} -> {dest} 최단 거리 : {total_cost}")


def _print_shortedst_path(
    matrix: List[List[bool]], paths: List[Coord], start: Coord, dest: Coord
) -> None:
    h = len(matrix)
    w = len(matrix[0])
    matrix = [["."] * w for _ in range(h)]

    for i in range(h):
        for j in range(w):
            if (i, j) == start:
                matrix[i][j] = "S"
            elif (i, j) == dest:
                matrix[i][j] = "G"

    prev_y, prev_x = start
    for path in paths:
        cur_y, cur_x = path
        # 해당 방향으로 화살표를 그림
        if prev_y < cur_y:
            matrix[cur_y][cur_x] = "↓"
        elif prev_y > cur_y:
            matrix[cur_y][cur_x] = "↑"
        elif prev_x < cur_x:
            matrix[cur_y][cur_x] = "→"
        elif prev_x > cur_x:
            matrix[cur_y][cur_x] = "←"

        prev_y, prev_x = cur_y, cur_x

    print("-- Shortest Path --")
    for i in range(h):
        for j in range(w):
            print(matrix[i][j], end=" ")
        print()
    print()

로직과 상관없는 함수명은 _로 시작하도록 작명했습니다.

실행 결과

이렇게 최단 경로를 구하기 위한 준비는 끝났습니다. 이제 직접 최단 경로를 구해봅시다. 아래는 전체 코드입니다.

from typing import List, Tuple, Callable
import math
import heapq


"""
1. 휴리스틱 코스트 H를 구한다. (목표 지점에서 각 노드까지의 유클리드 거리를 반올림)
2. 시작 지점에서 현재 지점까지의 실제 거리 G를 구한다.
3. 이하의 과정을 목표 지점에 도달할 때까지 반복한다.
4. 현재 지점을 방문 처리 후 현재 지점에 인접한 모든 노드의 Total Cost를 계산한다. Total Cost F는 G + H로 구할 수 있다.
5. 방문하지 않은 노드 중에서 Total Cost가 최소인 노드를 찾아 현재 지점을 이동한다.
"""


# 방향 벡터
d_row = (-1, 0, 1, 0)
d_col = (0, 1, 0, -1)

Coord = Tuple[int, int]


def a_star(
    matrix: List[List[int]], start: Coord, dest: Coord
) -> Tuple[int, List[Coord]]:
    global d_row
    global d_col

    h = len(matrix)
    w = len(matrix[0])

    # 휴리스틱 코스트 테이블
    heuristic_cost = [[float("inf")] * w for _ in range(h)]

    # 휴리스틱 코스트 구하기
    for i in range(h):
        for j in range(w):
            if matrix[i][j]:
                heuristic_cost[i][j] = round(get_euclidean_distance((i, j), dest))

    row, col = start
    dest_y, dest_x = dest

    vis = [[False] * w for _ in range(h)]

    heap = []
    heapq.heappush(heap, (heuristic_cost[row][col] + 0, row, col))

    total_cost = 0
    # 어떤 노드에서 어떤 노드로 이동하는지 저장할 리스트
    came_from = []

    """
    heap이 비거나 목적 지점에 도착할 때까지 반복:
        heap에서 cost가 최소인 값을 꺼내서 방문처리를 한 후,
        유효한 인접 노드가 있으면 코스트를 계산해 힙에 넣는다.
    """
    while heap and (row, col) != (dest_y, dest_x):
        total_cost, row, col = heapq.heappop(heap)

        # Total Cost 에서 휴리스틱 코스트를 빼면 시작 지점에서 현재 지점까지의 실제 거리를 구할 수 있음
        depth = total_cost - heuristic_cost[row][col]

        # 방문 처리
        vis[row][col] = True

        # 유효한 인접 노드가 있으면 코스트를 계산해 힙에 넣는다.
        for i in range(4):
            adjy = row + d_row[i]
            adjx = col + d_col[i]
            if is_vaild(matrix, vis, adjy, adjx):
                total_cost = heuristic_cost[adjy][adjx] + depth + 1
                came_from.append(((row, col), (adjy, adjx)))
                heapq.heappush(heap, (total_cost, adjy, adjx))

    # came_from을 역순으로 추적하여 최단 경로를 찾음
    from_y, from_x = came_from[-1][0]
    paths = []

    for i in range(len(came_from) - 1, -1, -1):
        from_coord, to_coord = came_from[i]
        to_y, to_x = to_coord

        if from_y == to_y and from_x == to_x:
            from_y, from_x = from_coord
            paths.insert(0, to_coord)

    return total_cost, paths, vis, heuristic_cost


def get_euclidean_distance(pq1: Coord, pq2: Coord) -> float:
    p1, q1 = pq1
    p2, q2 = pq2

    return math.sqrt((p1 - p2) ** 2 + (q1 - q2) ** 2)


def is_vaild(
    matrix: List[List[bool]], vis: List[List[bool]], row: int, col: int
) -> bool:
    h = len(matrix)
    w = len(matrix[0])

    # out of bound 처리
    if not (0 <= row < h and 0 <= col < w):
        return False

    # 유효하지 않은 노드 처리
    if not matrix[row][col]:
        return False

    # 이미 방문한 노드 처리
    if vis[row][col]:
        return False

    return True


def _print_cost(matrix: List[List[int]]) -> None:
    h = len(matrix)
    w = len(matrix[0])

    print("- Heuristic Cost -")
    for i in range(h):
        for j in range(w):
            print("." if math.isinf(matrix[i][j]) else matrix[i][j], end=" ")
        print()
    print()


def _print_path(
    matrix: List[List[bool]], start: Coord, dest: Coord, title: str
) -> None:
    h = len(matrix)
    w = len(matrix[0])

    print(f"---- {title} ----")
    for i in range(h):
        for j in range(w):
            if (i, j) == start:
                print("S", end=" ")
            elif (i, j) == dest:
                print("G", end=" ")
            else:
                print("O" if matrix[i][j] else ".", end=" ")
        print()
    print()


_print_shortest_distance: Callable[
    [Coord, Coord, int], None
] = lambda start, dest, total_cost: print(f"{start} -> {dest} 최단 거리 : {total_cost}")


def _print_shortedst_path(
    matrix: List[List[bool]], paths: List[Coord], start: Coord, dest: Coord
) -> None:
    h = len(matrix)
    w = len(matrix[0])
    matrix = [["."] * w for _ in range(h)]

    for i in range(h):
        for j in range(w):
            if (i, j) == start:
                matrix[i][j] = "S"
            elif (i, j) == dest:
                matrix[i][j] = "G"

    prev_y, prev_x = start
    for path in paths:
        cur_y, cur_x = path
        # 해당 방향으로 화살표를 그림
        if prev_y < cur_y:
            matrix[cur_y][cur_x] = "↓"
        elif prev_y > cur_y:
            matrix[cur_y][cur_x] = "↑"
        elif prev_x < cur_x:
            matrix[cur_y][cur_x] = "→"
        elif prev_x > cur_x:
            matrix[cur_y][cur_x] = "←"

        prev_y, prev_x = cur_y, cur_x

    print("-- Shortest Path --")
    for i in range(h):
        for j in range(w):
            print(matrix[i][j], end=" ")
        print()
    print()


if __name__ == "__main__":
    matrix = [
        [True, True, True, False, False, False, False],
        [True, False, True, False, False, False, False],
        [True, False, True, True, True, True, True],
        [True, False, True, False, False, False, True],
        [True, False, True, False, True, True, True],
        [True, False, True, False, True, False, False],
        [True, True, True, True, True, True, True],
    ]

    start = (2, 2)
    dest = (6, 6)
    total_cost, paths, vis, heuristic_cost = a_star(matrix, start, dest)

    _print_path(matrix, start, dest, "Path")
    _print_cost(heuristic_cost)
    _print_path(vis, start, dest, "Visited")
    _print_shortest_distance(start, dest, total_cost)
    _print_shortedst_path(matrix, paths, start, dest)

자 그럼 실행 결과를 볼까요? 실행 결과는 다음과 같습니다.

---- Path ----
O O O . . . .
O . O . . . .
O . S O O O O
O . O . . . O
O . O . O O O
O . O . O . .
O O O O O O G

- Heuristic Cost -
8 8 7 . . . .
8 . 6 . . . .
7 . 6 5 4 4 4
7 . 5 . . . 3
6 . 4 . 3 2 2
6 . 4 . 2 . .
6 5 4 3 2 1 0

---- Visited ----
. . . . . . .
. . O . . . .
. . S O O O O
. . O . . . O
. . O . . . O
. . O . . . .
. . O O O O G

(2, 2) -> (6, 6) 최단 거리 : 8
-- Shortest Path --
. . . . . . .
. . . . . . .
. . S . . . .
. . ↓ . . . .
. . ↓ . . . .
. . ↓ . . . .
. . ↓ → → → G

👋마치며

지금까지 Python3 으로 A* 알고리즘을 직접 구현해 봤습니다. 다소 미흡한 점이 많은 코드지만, 간단한 미로에서의 최단 경로는 잘 찾는 것을 알 수 있습니다.

오탈자 지적 및 피드백 환영합니다.

📕참고 자료

石田 保輝. 『アルゴリズム図鑑 絵で見てわかる26のアルゴリズム』. 翔泳社. 2017.

💡이 포스트의 소스 코드는 직접 작성한 것으로, 해당 책에 별도의 소스 코드는 기재되어 있지 않습니다.