참고자료
https://choiseokwon.tistory.com/210
파이썬 A* (A-star) 최단 경로 찾기 알고리즘
A* 길 찾기 알고리즘은 1968년에 Peter Hart, Nils Nilsson, Bertram Raphael에 의해 최초 공개됐다. 우선 Node 클래스는 아래와 같이 구현될 것이며, 아래 클래스를 보고 글을 읽으면 좀 더 편할 것이다. class Nod
choiseokwon.tistory.com
코드 참고는 다음의 블로그를 참고하였습니다.
파이썬으로 작성되었으며, C++로 작성된 코드는 후에 시간이 남는다면 작성하도록 하겠습니다.
A* 알고리즘이란?
최단거리 탐색알고리즘으로 가장유명한 알고리즘입니다. Grid Map (모논종이) 상에서 8방향의 방향성에 대해 장애물을 고려하여 Cost를 계산하고 이 Cost 비용 최소화를 통해 최적의 길을 찾아내는 알고리즘입니다.
트리구조
트리구조란 경로탐색에서 주로 활용되는 자료구조입니다. 부모와 자식 노드간의 관계를 나타냅니다.
이를 통해서 빠른 경로탐색을 가능하게 합니다.
A*는 그래프를 기준으로 한다.
A* 알고리즘을 적용하기 전에 Grid map에 대해 알아보자.
앞서 설명했던 글을 참고하면 맵파일을 2차원의 Grid 형식으로 나눠서 접근을 했던 것을 알 수 있습니다.
저번시간에 만들었던 맵의 이미지입니다. 바닥을 잘 확인해보시면 횐색의 사각형이 있는 것을 확인할 수 있습니다. 이 횐색의 사각형으로 이미지를 잘라서 맵행렬을 얻었습니다. (저는 맵행렬이라고 불렀는데, 이를 Grid Map이라고 하는 것 같습니다)
이를 실제맵과 비교해보면 다음과 같습니다.
이 이미지의 크기는 가로 250픽셀, 세로 250픽셀을 가지고 있습니다. 우리가 얻은 Grid 맵은 20x20 의 크기를 가지는 맵행렬입니다.
물론 250x250의 Grid 맵을 얻을 수도 있습니다. 이렇게 된다면 더 복잡한 계산이 가능하게될 것이지만, 계산량이 증가할 것이라는 것을 예상할 수 있습니다.
A* 알고리즘의 이론
A*(A star) 알고리즘 정의 및 개념 (tistory.com)
탐색 시작
다음과 같은 예시를 들어보겠습니다.
| 장애물 | |||||||||
| 장애물 | |||||||||
| 장애물 | |||||||||
| 출발지 | 장애물 | 목적지 | |||||||
| 장애물 | |||||||||
여기서 등장하는 공식이 하나 있습니다. 바로 아래와 같은 공식입니다.
F = G + H
여기서 F는 총비용이라고 하는 놈으로 현재까지 이동하는데 걸린 비용과 예상비용을 합친 것을 의미합니다.
G는 시작점으로부터 현재 사각형까지의 경로를 따라 이동하는 데에 소요되는 비용을 의미합니다.
H는 현재 사각형에서 목적지까지의 예상 이동 비용을 의미합니다. (여기서 H는 대각선 이동을 고려하지 않습니다.) (장애물을 무시합니다)
(여기서 H를 계산하는 방법은 여러가지가 있다고 합니다. )
그러면 간단하게 계산을 한 번 해보도록 합시다. 하나의 그리드를 10이라고 가정합시다. 그러면 우선 출발지 주변의 모든 값들을 계산합니다. (왼쪽은 G 오른쪽은 H를 나타냅니다.)
| 장애물 | |||||||||
| 장애물 | |||||||||
| 74(14, 60) | 60(10, 50) | 54(14, 40) | 장애물 | ||||||
| 60(10, 50) | 출발지 | 40(10, 30) | 장애물 | 목적지 | |||||
| 74(14, 60) | 60(10, 50) | 54(14, 40) | 장애물 | ||||||
출발지 주변의 8개의 그리드를 열린 목록에 추가됩니다. 여기서 우리는 출발지의 오른쪽 40(10, 30)을 선택할 수 있습니다. 그러면 출발지와, 40(10, 30)을 열린목록에서 제거하고 닫힌 목록에 추가합니다.
열린목록 간의 비교는 G값만 가지고 비교를 합니다. 낮은 경우가 이기는 것이죠.
| 장애물 | |||||||||
| 장애물 | |||||||||
| 74(14, 60) | 60(10, 50) | 54(14, 40) | 장애물 | ||||||
| 60(10, 50) | 출발지 | 40(10, 30) | 장애물 | 목적지 | |||||
| 74(14, 60) | 60(10, 50) | 54(14, 40) | 장애물 | ||||||
그러면 초록색 주변으로 사각형들을 검사합니다. 40이 선택된 상태에서 우측하단의 G값은 10 +10 이 되므로 20이 됩니다.
출발지의 바로 위 아래는 40을 거친 후에 지나가게 되면 10은 아닙니다. 그러니 G가 개선되는 것이 없으니 40을 버리고 다시 열린목록중에서 선택을 하면됩니다.
열린목록 중에서 54 노드가 두 개 있으므로 끌리는 것 아무거나 택하면 됩니다. 우리는 우측하단을 선택합니다.
| 장애물 | |||||||||
| 장애물 | |||||||||
| 74(14, 60) | 60(10, 50) | 54(14, 40) | 장애물 | ||||||
| 60(10, 50) | 출발지 | 40(10, 30) | 장애물 | 목적지 | |||||
| 74(14, 60) | 60(10, 50) | 54(14, 40) | 장애물 | ||||||
그러면 우측하단 54에 인접한 사각형의 값을 계산합니다.
| 장애물 | |||||||||
| 장애물 | |||||||||
| 74(14, 60) | 60(10, 50) | 54(14, 40) | 장애물 | ||||||
| 60(10, 50) | 출발지 | 40(10, 30) | 장애물 | 목적지 | |||||
| 74(14, 60) | 60(10, 50) | 54(14, 40) | 장애물 | ||||||
| 80(20, 60) | 74(24, 50) | ||||||||
(24인 이유는 대충 1.4를 20에 곱해서 24로 치는거 원래는 22.36정도)
이제 우측하단 54는 닫힌 목록에 들어갑니다. 여기서 54의 왼쪽 60은 G값으로 비교를 하면 14 +10 으로 더 크게 변하므로 제외합니다.
그러면 이제 80과 74값 노드를 열린목록에 넣고, 가장 작은 F를 가지는 74를 선택합니다.
| 장애물 | |||||||||
| 장애물 | |||||||||
| 74(14, 60)↘ | 60(10, 50) | 54(14, 40) | 장애물 | ||||||
| 60(10, 50) | 출발지 | 40(10, 30) | 장애물 | 목적지 | |||||
| 74(14, 60) | 60(10, 50) | 54(14, 40) | 장애물 | ||||||
| 80(20, 60) | 74(24, 50) | 74(34, 40) | |||||||
| 108(38, 70) | 94(34, 60) | 98(48, 50) |
74가 선택되었고, 74를 닫힌 목록에 넣습니다. 그리고 74 주변의 노드들의 F값을 계산합니다.
좌측 하단을 보면 G값이 38입니다. 그 이유는 출발지 40->54->74->108 까지의 G값을 모두 더했을 때 값이 38이기 때문입니다.
열린목록간의 계산은 G값으로 비교를 진행합니다. 여튼 그래서 60과 80을 비교하니 G값이 개선되지 않습니다. 그러니 새롭게 추가된 노드들을 확인합니다.
새롭게 계산된 노드들을 열린목록에 추가합니다. F값으로 새롭게 추가된 노드들을 비교하니 74(오른쪽이 최선입니다.)
| 장애물 | |||||||||
| 장애물 | |||||||||
| 74(14, 60) | 60(10, 50) | 54(14, 40) | 장애물 | ||||||
| 60(10, 50) | 출발지 | 40(10, 30) | 장애물 | 목적지 | |||||
| 74(14, 60) | 60(10, 50) | 54(14, 40) | 장애물 | ||||||
| 80(20, 60) | 74(24, 50) | 74(34, 40) | 74(44, 30) | ||||||
| 108(38, 70) | 94(34, 60) | 98(48, 50) | 88(48, 40) |
별도의 설명없이 오른쪽의 74가 선택됩니다.
| 장애물 | |||||||||
| 장애물 | |||||||||
| 74(14, 60) | 60(10, 50) | 54(14, 40) | 장애물 | ||||||
| 60(10, 50) | 출발지 | 40(10, 30) | 장애물 | 목적지 | |||||
| 74(14, 60) | 60(10, 50) | 54(14, 40) | 장애물 | 74(54, 20) | 68(58, 10) | ||||
| 80(20, 60) | 74(24, 50) | 74(34, 40) | 74(44, 30) | 74(54, 20) | |||||
| 108(38, 70) | 94(34, 60) | 98(48, 50) | 88(48, 40) | 88(58, 30) |
별도의 설명없이 68이 선택됩니다.
| 장애물 | |||||||||
| 장애물 | |||||||||
| 74(14, 60) | 60(10, 50) | 54(14, 40) | 장애물 | ||||||
| 60(10, 50) | 출발지 | 40(10, 30) | 장애물 | 82(72, 10) | 목(68, 68, 0) | 82(72, 10) | |||
| 74(14, 60) | 60(10, 50) | 54(14, 40) | 장애물 | 74(54, 20) | 68(58, 10) | 88(68, 20) | |||
| 80(20, 60) | 74(24, 50) | 74(34, 40) | 74(44, 30) | 74(54, 20) | 102(72, 30) | ||||
| 108(38, 70) | 94(34, 60) | 98(48, 50) | 88(48, 40) | 88(58, 30) |
별도의 설명없이 68을 가지는 목적지가 선택됩니다. 여기서 주목할 점은
화살표는 현재 이런식으로 가리키고 있게 됩니다. 뭐 이런 이야기는 잠시 접어두고 목적지에서 화살표를 따라가면 출발지가 나오게 됩니다.
화살표는 부모를 가리키는 것으로 이해하면됩니다. A* 알고리즘을 구할때, G의 값을 비교하기도 하고, F의 값을 비교하면서 차례대로 진행했던 것을 알 수 있습니다. 차례대로 진행을 하면서 부모를 표시한 것입니다.
결국 path는 목적지에서 출발지까지 부모노드를 따라가면서 연결이 될 때까지 찾는 것을 의미합니다.
이전 시간의 A* 알고리즘 코드 분석
코드의 출처는 파이썬 A* (A-star) 최단 경로 찾기 알고리즘 :: PROGRAMMING PER SE (tistory.com)
파이썬 A* (A-star) 최단 경로 찾기 알고리즘
A* 길 찾기 알고리즘은 1968년에 Peter Hart, Nils Nilsson, Bertram Raphael에 의해 최초 공개됐다. 우선 Node 클래스는 아래와 같이 구현될 것이며, 아래 클래스를 보고 글을 읽으면 좀 더 편할 것이다. class Nod
choiseokwon.tistory.com
위의 블로그이며, 위의 블로그의 코드를 참고하여 모듈화를 진행했습니다.
class Astar:
def __init__(self, parent=None, position=None):
self.parent = parent
self.position = position
self.g = 0
self.h = 0
self.f = 0
def __eq__(self, other):
return self.position == other.position
def heuristic(self, node, goal, D=1, D2=2 ** 0.5): # Diagonal Distance
dx = abs(node.position[0] - goal.position[0])
dy = abs(node.position[1] - goal.position[1])
return D * (dx + dy)
def aStar(self, maze, start, end):
# startNode와 endNode 초기화
startNode = Astar(None, start)
endNode = Astar(None, end)
# openList, closedList 초기화
openList = []
closedList = []
# openList에 시작 노드 추가
openList.append(startNode)
# endNode를 찾을 때까지 실행
while openList:
# 현재 노드 지정
currentNode = openList[0]
currentIdx = 0
# 이미 같은 노드가 openList에 있고, f 값이 더 크면
# currentNode를 openList안에 있는 값으로 교체
for index, item in enumerate(openList):
if item.f < currentNode.f:
currentNode = item
currentIdx = index
# openList에서 제거하고 closedList에 추가
openList.pop(currentIdx)
closedList.append(currentNode)
# 현재 노드가 목적지면 current.position 추가하고
# current의 부모로 이동
if currentNode == endNode:
path = []
current = currentNode
while current is not None:
# maze 길을 표시하려면 주석 해제
x, y = current.position
maze[x][y] = 2
path.append(current.position)
current = current.parent
return path[::-1] # reverse
children = []
# 인접한 xy좌표 전부
for newPosition in [(0, -1), (0, 1), (-1, 0), (1, 0)]:
# 노드 위치 업데이트
nodePosition = (
currentNode.position[0] + newPosition[0], # X
currentNode.position[1] + newPosition[1]) # Y
# 미로 maze index 범위 안에 있어야함
within_range_criteria = [
nodePosition[0] > (len(maze) - 1),
nodePosition[0] < 0,
nodePosition[1] > (len(maze[len(maze) - 1]) - 1),
nodePosition[1] < 0,
]
if any(within_range_criteria): # 하나라도 true면 범위 밖임
continue
# 장애물이 있으면 다른 위치 불러오기
if maze[nodePosition[0]][nodePosition[1]] != 0:
continue
new_node = Astar(currentNode, nodePosition)
children.append(new_node)
# 자식들 모두 loop
for child in children:
# 자식이 closedList에 있으면 continue
if child in closedList:
continue
# f, g, h값 업데이트
child.g = currentNode.g + 1
child.h = ((child.position[0] - endNode.position[0]) **
2) + ((child.position[1] - endNode.position[1]) ** 2)
# child.h = heuristic(child, endNode) 다른 휴리스틱
# print("position:", child.position) 거리 추정 값 보기
# print("from child to goal:", child.h)
child.f = child.g + child.h
# 자식이 openList에 있으고, g값이 더 크면 continue
if len([openNode for openNode in openList
if child == openNode and child.g > openNode.g]) > 0:
continue
openList.append(child)
def run(self, maze, start, end):
path = self.aStar(maze, start, end)
return maze, path
윗줄부터 코드를 분석해나가도록 하겠습니다.
class Astar:
def __init__(self, parent=None, position=None):
self.parent = parent
self.position = position
self.g = 0
self.h = 0
self.f = 0우선Class 형식으로 선언된 Astar 이며, 위와같이 선언된 이유는 A* 알고리즘을 모듈화해서 사용하기 위함입니다.
(지금보니 코드가 조금 이상하네요.. 후에 수정해서 다시 올리겠습니다. 메인에서 선언하는데, 함수내에서 또 선언이 되고 있다니, 아마 class 모듈화를 다시 진행해야할 것으로 보입니다.)
처음은 init 함수 즉 생성자라고 볼 수 있는 부분입니다. 우선 넘어갑니다.
def aStar(self, maze, start, end):
# startNode와 endNode 초기화
startNode = Astar(None, start)
endNode = Astar(None, end)
# openList, closedList 초기화
openList = []
closedList = []
# openList에 시작 노드 추가
openList.append(startNode)aStar 함수의 모습입니다. 우선 스타트 노드와 엔드 노드를 정의합니다. Astar를 거치게되면서
class Astar:
def __init__(self, parent=None, position=None):
self.parent = parent
self.position = position
self.g = 0
self.h = 0
self.f = 0다음의 클래스 형식을 가지는 Astar 노드 start와 end가 만들어집니다. 우선 부모는 없네요. 그리고 maze(grid map) start의 위치와 end의 위치입니다. start의 값과 end의 값은 따로 유저가 넣어주게됩니다.
openList에 startNode를 넣습니다.
# endNode를 찾을 때까지 실행
while openList:
# 현재 노드 지정
currentNode = openList[0]
currentIdx = 0
# 이미 같은 노드가 openList에 있고, f 값이 더 크면
# currentNode를 openList안에 있는 값으로 교체
for index, item in enumerate(openList):
if item.f < currentNode.f:
currentNode = item
currentIdx = index
# openList에서 제거하고 closedList에 추가
openList.pop(currentIdx)
closedList.append(currentNode)
# 현재 노드가 목적지면 current.position 추가하고
# current의 부모로 이동
if currentNode == endNode:
path = []
current = currentNode
while current is not None:
x, y = current.position
maze[x][y] = 2
path.append(current.position)
current = current.parent
return path[::-1] # reverse
children = []
# 인접한 xy좌표 전부
# 4가지만 표시한 이유는 대각선 진행을 배제하기 위함
for newPosition in [(0, -1), (0, 1), (-1, 0), (1, 0)]:
# 노드 위치 업데이트
nodePosition = (
currentNode.position[0] + newPosition[0], # X
currentNode.position[1] + newPosition[1]) # Y
# 미로 maze index 범위 안에 있어야함
within_range_criteria = [
nodePosition[0] > (len(maze) - 1),
nodePosition[0] < 0,
nodePosition[1] > (len(maze[len(maze) - 1]) - 1),
nodePosition[1] < 0,
]
if any(within_range_criteria): # 하나라도 true면 범위 밖임
continue
# 장애물이 있으면 다른 위치 불러오기
if maze[nodePosition[0]][nodePosition[1]] != 0:
continue
new_node = Astar(currentNode, nodePosition)
children.append(new_node)
# 자식들 모두 loop
for child in children:
# 자식이 closedList에 있으면 continue
if child in closedList:
continue
# f, g, h값 업데이트
child.g = currentNode.g + 1
child.h = ((child.position[0] - endNode.position[0]) **
2) + ((child.position[1] - endNode.position[1]) ** 2)
# child.h = heuristic(child, endNode) 다른 휴리스틱
# print("position:", child.position) 거리 추정 값 보기
# print("from child to goal:", child.h)
child.f = child.g + child.h
# 자식이 openList에 있으고, g값이 더 크면 continue
if len([openNode for openNode in openList
if child == openNode and child.g > openNode.g]) > 0:
continue
openList.append(child)이론에서 설명한 동작을 진행하게 만듭니다.
여기서 heurisitic이란 H값을 계산할 때 쓰는 공식을 의하며 4가지의 경우가 있다고 합니다.
(코드 수정이 필요한 것으로 확인됩니다. class 형식으로 만드는 과정에서 문제가 생겼습니다.)
여기까지 A* 알고리즘에 대해서 알아보았습니다. 다음 시간에는 pure pursuit에 대해서 조사를 진행하겠습니다.
'공부#Robotics#자율주행 > (ROS2)Path Planning' 카테고리의 다른 글
| ROS2로 turtlebot3 제어하기 5장. turtlebot이 생성된 경로를 따라 움직이게 하자 (0) | 2023.06.03 |
|---|---|
| ROS2로 turtlebot3 제어하기 4장. map파일을 가지고 전역경로 생성하기 (0) | 2023.06.03 |
| ROS2로 turtlebot3 제어하기 3장. Nav2 이해하기와 파이썬 환경설정 (0) | 2023.06.03 |
| ROS2로 turtlebot3 제어하기 2장. MAP 만들기 (0) | 2023.06.02 |
| ROS2로 turtlebot3 제어하기 1장. 경로계획(Path planning)의 개요 (0) | 2023.06.02 |