A*알고리즘 구현

코드가 너무 길어서 구현은 이 부분에서 진행하려고 한다. 

 

일단 전체 코드를 보여주고 하나씩 차근차근 설명하겠다. 

 

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

# 이동방향
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, int]:
    #이동방향 설정
    global d_row
    global d_col
    
    # matrix의 가로, 세로 길이 
    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_euclidian_dist((i, j), dest))

    # 시작지점, 목적지 설정
    row, col = start
    dest_x, dest_y = dest

    # 방문여부 매트릭스
    vis = [[False] * w for _ in range(h)]

    # 방문 히스토리 저장 리스트
    came_from = []

    # 힙을 사용하여 전체 코스트가 가장 작은 노드를 먼저 방문한다.
    heap = []
    heapq.heappush(heap, (heuristic_cost[row][col] + 0, row, col))
    total_cost = 0

    while heap and (row, col) != (dest_x, dest_y):
        total_cost, row, col = heapq.heappop(heap)
        
        # 전체 코스트 - 휴리스틱 코스트 = 시작지점으로부터의 거리
        depth = total_cost - heuristic_cost[row][col]
        
        # 방문 체크
        vis[row][col] = True
        
        #상하좌우 탐색
        for i in range(4):
            adjr = row + d_row[i]
            adjc = col + d_col[i]
            if is_valid(matrix, vis, adjr, adjc):
                # total cost 업데이트 후 heap에 추가
                total_cost = heuristic_cost[adjr][adjc] + depth + 1
                came_from.append(((row, col), (adjr, adjc)))
                heapq.heappush(heap, (total_cost, adjr, adjc))

    # 이동 경로 리스트 만들기 
    from_r, from_c = came_from[-1][0]
    paths = []

    for i in range(len(came_from)-1, -1, -1):
        from_coord, to_coord = came_from[i]
        to_r, to_c = to_coord
        if to_r == from_r and to_c == from_c:
            from_r, from_c = from_coord
            paths.insert(0, to_coord)

    return total_cost, paths, vis, heuristic_cost



# 두 지점의 유클리드 거리를 구하는 함수
def get_euclidian_dist(pq1:Coord, pq2:Coord) -> float: 
    p1, q1 = pq1
    p2, q2 = pq2
    return math.sqrt((p1 - p2)**2 + (q1 - q2)**2)

# 입력된 위치가 이동할 수 있는 노드인지 확인하는 함수 
def is_valid(matrix, vis, adjr, adjc):
    h = len(matrix)
    w = len(matrix[0])

    if adjr < 0 or adjr >= h or adjc < 0 or adjc >= w:
        return False

    if vis[adjr][adjc]:
        return False

    if not matrix[adjr][adjc]:
        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)

 

흐음.. 일단 코드가 매우 길기 때문에 차근차근 설명해보겠다. 

 

우선 구현을 위한 함수가 아닌 시각화 함수는 이름 첫 부분에 _를 붙였다. 

 

    # 휴리스틱 코스트 초기화
    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_euclidian_dist((i, j), dest))

 

우선 우리는 주어진 heuristic cost를 사용하여 맵을 탐색해야 하기 때문에 휴리스틱 맵에 코스트를 입력해준다. 

 

get_euclidian_dist함수는 (i, j)좌표와 목적지 사이의 유클리드 거리를 반환하는 함수이다. 

 

이 함수의 반올림 값을 휴리스틱 코스트 맵에 입력한다. 

 

 # 시작지점, 목적지 설정
    row, col = start
    dest_x, dest_y = dest

    # 방문여부 매트릭스
    vis = [[False] * w for _ in range(h)]

    # 방문 히스토리 저장 리스트
    came_from = []

    # 힙을 사용하여 전체 코스트가 가장 작은 노드를 먼저 방문한다.
    heap = []
    heapq.heappush(heap, (heuristic_cost[row][col] + 0, row, col))
    total_cost = 0

 

시작지점과 목적지를 설정한 후에 방문 여부를 체크하는 vis 매트릭스를 만든다. 

 

이후 다익스트라 알고리즘과 동일하게 heap을 사용하여 전체 코스트가 가장 작은 노드를 먼저 방문한다. 

 

전체 코스트 = 휴리스틱 코스트 + 시작지점으로부터의 거리

 

min heap은 첫 번째 원소를 기준으로 가장 작은 값을 root에 올리기 때문에 코스트가 가장 작은 값부터 삭제가 진행된다. 

 

    while heap and (row, col) != (dest_x, dest_y):
        total_cost, row, col = heapq.heappop(heap)

        # 전체 코스트 - 휴리스틱 코스트 = 시작지점으로부터의 거리
        depth = total_cost - heuristic_cost[row][col]

        # 방문 체크
        vis[row][col] = True

        #상하좌우 탐색
        for i in range(4):
            adjr = row + d_row[i]
            adjc = col + d_col[i]
            if is_valid(matrix, vis, adjr, adjc):
                # total cost 업데이트 후 heap에 추가
                total_cost = heuristic_cost[adjr][adjc] + depth + 1
                came_from.append(((row, col), (adjr, adjc)))
                heapq.heappush(heap, (total_cost, adjr, adjc))

 

이후에 현재 위치인 row, col이 목적지가 아니라면 while문을 계속해서 반복해준다. 

 

while문 안에서 상하좌우로 이동하면서 해당구간으로 이동할 수 있는 지 확인하고 만약 이동할 수 있다면 

이 노드를 heap과 came_from 리스트에 추가해준다. 

 

# 입력된 위치가 이동할 수 있는 노드인지 확인하는 함수
def is_valid(matrix, vis, adjr, adjc):
    h = len(matrix)
    w = len(matrix[0])

    if adjr < 0 or adjr >= h or adjc < 0 or adjc >= w:
        return False

    if vis[adjr][adjc]:
        return False

    if not matrix[adjr][adjc]:
        return False

    return True

 

is_valid함수는 해당 좌표로 이동할 수 있는지 확인해주는 함수이다. 

 

    # 이동 경로 리스트 만들기
    from_r, from_c = came_from[-1][0]
    paths = []

    for i in range(len(came_from)-1, -1, -1):
        from_coord, to_coord = came_from[i]
        to_r, to_c = to_coord
        if to_r == from_r and to_c == from_c:
            from_r, from_c = from_coord
            paths.insert(0, to_coord)

    return total_cost, paths, vis, heuristic_cost

 

이후에 came from리스트를 거꾸로 탐색하며 목적지까지의 최단 경로를 paths 리스트에 추가해준다. 

 

이후에 목적지까지의 total_cost와 최단경로, 방문여부, 휴리스틱 코스트 맵을 반환해준다. 

 

전체 코드를 실행하면 다음과 같은 결과물이 나온다. 

 

---- 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 


Process finished with exit code 0

 

 

이상 포스팅을 마치겠다!