2D Grid Map에서의 A* 최단 경로 탐색

문제 개요

• 환경: 0(이동 가능)과 1(장애물)로 구성된 N×M 2차원 그리드 맵.
• 목표: 시작점(Start)에서 목표점(Goal)까지 충돌 없이 도달하는 최단 경로 좌표 리스트 반환.
• 제약: 4방향(상하좌우) 이동만 허용 (대각선 이동 불가 가정).
• 핵심 평가 항목:
  1. Heuristic 함수 설계: 맨해튼 거리(Manhattan Distance) 등을 적절히 사용하여 탐색 효율을 높였는가?
  2. 자료구조 활용: Priority Queue (최소 힙)를 사용하여 $O(1)$로 최소 비용 노드를 꺼낼 수 있는가?
  3. 예외 처리: 도달 불가능한 경우나 맵 밖으로 나가는 경우를 처리했는가?

import heapq

# -----------------------------------
# 1. Heuristic Function
# -----------------------------------
def heuristic(a, b):
    # 상하좌우 이동만 가능하므로 맨해튼 거리(L1 Norm) 사용
    return abs(a[0] - b[0]) + abs(a[1] - b[1])

# -----------------------------------
# 2. A* Algorithm
# -----------------------------------
def a_star_search(grid, start, goal):
    # grid: 2D list (0: free, 1: obstacle)
    rows, cols = len(grid), len(grid[0])
    
    # 4방향 이동 (상, 하, 좌, 우)
    directions = [(-1, 0), (1, 0), (0, -1), (0, 1)]
    
    # Priority Queue: (f_score, current_node)
    open_list = []
    heapq.heappush(open_list, (0, start))
    
    # 방문 여부 및 비용 저장
    # g_score: 출발점에서 현재까지의 실제 비용
    g_score = {start: 0}
    
    # 경로 복원을 위한 부모 노드 기록
    came_from = {}
    
    while open_list:
        # f_score가 가장 낮은 노드 꺼내기
        current_f, current = heapq.heappop(open_list)
        
        # 목표 도달 시 경로 복원 및 반환
        if current == goal:
            path = []
            while current in came_from:
                path.append(current)
                current = came_from[current]
            path.append(start)
            return path[::-1] # 역순으로 뒤집어 반환

        x, y = current
        
        # 이웃 노드 탐색
        for dx, dy in directions:
            nx, ny = x + dx, y + dy
            neighbor = (nx, ny)
            
            # 맵 범위 내에 있고, 장애물이 아닌 경우
            if 0 <= nx < rows and 0 <= ny < cols and grid[nx][ny] == 0:
                new_g = g_score[current] + 1 # 이동 비용은 항상 1로 가정
                
                # 더 짧은 경로를 발견했거나 처음 방문하는 경우
                if neighbor not in g_score or new_g < g_score[neighbor]:
                    g_score[neighbor] = new_g
                    f_score = new_g + heuristic(neighbor, goal)
                    heapq.heappush(open_list, (f_score, neighbor))
                    came_from[neighbor] = current
                    
    return None # 경로 없음

# -----------------------------------
# 3. Execution Example
# -----------------------------------
def main():
    # 0: 길, 1: 장애물
    grid_map = [
        [0, 0, 0, 0, 1],
        [1, 1, 0, 1, 0],
        [0, 0, 0, 0, 0],
        [0, 1, 1, 1, 0],
        [0, 0, 0, 1, 0]
    ]
    
    start_pos = (0, 0)
    goal_pos = (4, 4)
    
    path = a_star_search(grid_map, start_pos, goal_pos)
    
    if path:
        print(f"Path Found: {path}")
        # 시각화 (Optional)
        for r in range(len(grid_map)):
            line = ""
            for c in range(len(grid_map[0])):
                if (r, c) == start_pos: line += "S "
                elif (r, c) == goal_pos: line += "G "
                elif (r, c) in path: line += "* "
                elif grid_map[r][c] == 1: line += "X "
                else: line += ". "
            print(line)
    else:
        print("No Path Found.")

if __name__ == "__main__":
    main()

 

• *Dijkstra vs A:**
  • "다익스트라(Dijkstra)는 모든 방향을 균일하게 탐색하지만, A는 휴리스틱(Heuristic)을 이용해 목표 방향으로의 탐색에 가중치를 둡니다. 실시간성이 중요한 로봇 주행에서는 연산량을 줄이기 위해 A가 필수적입니다."
• Admissible Heuristic (허용 가능한 휴리스틱):
  • "휴리스틱 함수 $h(n)$이 실제 남은 거리보다 크지 않아야(과대평가하지 않아야) 최적의 경로를 보장합니다. Grid 환경에서는 맨해튼 거리가 이에 해당합니다."
• Local Minima:
  • "단순 Greedy Search는 장애물(Local Minima)에 갇힐 수 있지만, A*는 g(n)(지금까지 온 거리)을 함께 고려하므로 돌아가더라도 결국 탈출구를 찾아냅니다."

 

 

 

 

[A] Grid Path Planning (결과 시각화 포함)

import heapq
import matplotlib.pyplot as plt
import numpy as np

def heuristic(a, b):
    # Manhattan Distance
    return abs(a[0] - b[0]) + abs(a[1] - b[1])

def a_star_search(grid, start, goal):
    rows, cols = len(grid), len(grid[0])
    directions = [(-1, 0), (1, 0), (0, -1), (0, 1)] # 상하좌우
    
    open_list = []
    heapq.heappush(open_list, (0, start))
    g_score = {start: 0}
    came_from = {}
    
    while open_list:
        current_f, current = heapq.heappop(open_list)
        
        if current == goal:
            path = []
            while current in came_from:
                path.append(current)
                current = came_from[current]
            path.append(start)
            return path[::-1]

        x, y = current
        for dx, dy in directions:
            nx, ny = x + dx, y + dy
            neighbor = (nx, ny)
            
            if 0 <= nx < rows and 0 <= ny < cols and grid[nx][ny] == 0:
                new_g = g_score[current] + 1
                if neighbor not in g_score or new_g < g_score[neighbor]:
                    g_score[neighbor] = new_g
                    f_score = new_g + heuristic(neighbor, goal)
                    heapq.heappush(open_list, (f_score, neighbor))
                    came_from[neighbor] = current
    return None

def visualize_path(grid, path, start, goal):
    grid_array = np.array(grid)
    plt.figure(figsize=(6, 6))
    
    # 0은 흰색, 1(장애물)은 검은색
    plt.imshow(grid_array, cmap='Greys', origin='upper')
    
    # Start(Green), Goal(Red)
    plt.plot(start[1], start[0], 'go', markersize=15, label='Start')
    plt.plot(goal[1], goal[0], 'ro', markersize=15, label='Goal')
    
    # Path(Blue Line)
    if path:
        y_coords = [p[0] for p in path]
        x_coords = [p[1] for p in path]
        plt.plot(x_coords, y_coords, 'b-', linewidth=3, label='Path')
        plt.plot(x_coords, y_coords, 'b.', markersize=8) # 경로 점
    
    plt.legend()
    plt.title(f"A* Path Planning (Length: {len(path) if path else 0})")
    plt.grid(True)
    plt.show()

def main():
    # 0:길, 1:장애물 (10x10 Map)
    grid_map = [
        [0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
        [1, 1, 0, 1, 1, 0, 1, 1, 1, 0],
        [0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
        [0, 1, 1, 1, 1, 1, 1, 0, 1, 0],
        [0, 0, 0, 0, 0, 0, 1, 0, 0, 0],
        [0, 1, 0, 1, 1, 0, 1, 1, 1, 0],
        [0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
        [0, 1, 1, 1, 1, 0, 1, 1, 1, 0],
        [0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
        [0, 0, 0, 1, 1, 1, 1, 1, 1, 0]
    ]
    
    start_pos = (0, 0)
    goal_pos = (9, 9)
    
    print("Finding Path...")
    path = a_star_search(grid_map, start_pos, goal_pos)
    
    if path:
        print(f"Path Found! Length: {len(path)}")
        visualize_path(grid_map, path, start_pos, goal_pos)
    else:
        print("No Path Found.")

if __name__ == "__main__":
    main()