A* 알고리즘을 이용한 경로 계산

1. 개요 

  맵-이미지 파일을  갈수 영역(free)을 255(백색), 갈 수 없는 영역(obstacle)을 0(검은색)으로 표현하고, 초기 위치와 목표위치를 설정하고,  A* 알고리즘을 이용하여 장애물의 피하면서 갈 수 있는 최단 코스를 계산한다. 

 

2. 파이썬 코드

import cv2
import numpy as np
import heapq
# image value 255: free area
# image value 0:  obstacle area

def draw_path_on_image(img, points, line_color=(0, 255, 0), line_thickness=1):
    for i in range(len(points) - 1):
        cv2.line(img, points[i], points[i+1], line_color, line_thickness)
    return img

def is_valid_move_with_margin(img, point, margin=0):
    """ 해당 포인트와 주변 마진의 테두리가 이미지 내부에 있고 갈 수 있는 지역인지 확인 """
    x, y = point
    for dx in range(-margin, margin + 1):
        for dy in range(-margin, margin + 1):
            # 마진의 테두리만 체크하도록 조건 수정
            if abs(dx) == margin or abs(dy) == margin:
                nx, ny = x + dx, y + dy
                if not ((0 <= nx < img.shape[1]) and (0 <= ny < img.shape[0]) and (img[ny, nx] == 255)):
                    return False
    return True

def get_neighbors(point):
    """ 해당 포인트의 이웃 포인트들을 반환 """
    x, y = point
    return [(x-1, y), (x+1, y), (x, y-1), (x, y+1)]  # 상, 하, 좌, 우

def heuristic(a, b):
    """ 휴리스틱 함수: a와 b 사이의 유클리디언 거리 """
    return np.sqrt((b[0] - a[0]) ** 2 + (b[1] - a[1]) ** 2)

def astar_search_with_margin(img, start, goal, margin=0):
    """ 마진을 고려한 A* 알고리즘을 사용한 경로 탐색 """
    open_set = []
    heapq.heappush(open_set, (0, start))
    came_from = {}
    g_score = {start: 0}
    f_score = {start: heuristic(start, goal)}

    while open_set:
        current = heapq.heappop(open_set)[1]
        if current == goal:
            path = []
            while current in came_from:
                path.append(current)
                current = came_from[current]
            return path[::-1]

        for neighbor in get_neighbors(current):
            if is_valid_move_with_margin(img, neighbor, margin):
                tentative_g_score = g_score[current] + 1
                if tentative_g_score < g_score.get(neighbor, float("inf")):
                    came_from[neighbor] = current
                    g_score[neighbor] = tentative_g_score
                    f_score[neighbor] = tentative_g_score + heuristic(neighbor, goal)
                    heapq.heappush(open_set, (f_score[neighbor], neighbor))
    return []

if __name__ == '__main__':
    # 이미지 로드 (0: 이동 불가능, 255: 이동 가능)
    #resolution: 20cm
    img = cv2.imread('p20cm_map.png', cv2.IMREAD_GRAYSCALE)
    print('width=',img.shape[1],' height=',img.shape[0])
    # 시작점과 목적지 설정
    # 1000,1000  => 1270, 600
    start_point = (1000, 1000)  # 시작점 좌표
    goal_point = (1270,600)  # 목적지 좌표

    # 마진을 고려한 A* 알고리즘으로 경로 찾기
    path = astar_search_with_margin(img, start_point, goal_point, 5)
    # 결과 출력
    print("final Path:", path)
    img= draw_path_on_image(img, path, line_color=(0, 255, 0), line_thickness=1)
    cv2.imshow('image', img)
    cv2.waitKey(0)
    cv2.destroyAllWindows()

 

3. 실행결과 예시

 최종결과를 이미지 위에 그려보면 아래와 같은 결과를 얻는다.