1. 샘플링 기반 경로 계획
1.1 샘플링 기반 경로 계획 이란?
- 대표적인 경로 계획 방법론 중 한 분류
- 고차원 공간에서도 짧은 시간 내에 효과적으로 적절한 경로를 찾기 위해 제안
- 경로를 탐색할 공간을 그래프 구조나 격자(Grid)로 분할하여 나타내지 않고 자유 공간에 무작위로 생성된 포인트 중에서 랜덤하게 한 포인트를 선택하여 경로를 이어가며 경로를 탐색하는 방법
- RRT, RRT*, PRM이 대표적
1.2 샘플링 기반 경로 계획 특징
- 무작위 샘플 생성
- 포인트 연결 및 트리 구조 생성
- 최적 경로 탐색
- 경로 후처리
즉, 형상 공간 내에서 샘플점을 무작위로 충분한 수만큼 발생시키고, 그 샘플점이, 혹은 두 개의 샘플점을 잇는 선이 장애물과 충돌하는지 여부를 확인하여 자유공간의 구조를 효율적으로 파악한다.
랜덤 탐색은 특히 격자 생성 방법으로는 탐색이 거의 불가능한 고차원 공간에서 강력한 힘을 발휘한다.
2. RRT 기반 경로 계획
2.1 RRT(Rapidly-exploring Random Tree) 알고리즘 개요
RRT는 LaValle에 의해 처음 제안되었다. 기본 아이디어는 트리(Tree)라고 불리는 특별한 그래프를 구축하는 것이다. 트리는 노드(vertex 또는 node)와 연결선(edge)로 구성된다. 연결선은 노드와 노드를 연결한 선이다.
트리 구조에서는 모든 노드는 부모(Parent)라 불리는 1개의 노드에 연결되며 시작점은 트리의 맨 최상위 부모가 되는 루트(root)가 된다. RRT알고리즘은 형상공간에서 시작점과 목표점을 연결한 트리를 산출한다.
RRT에서는 형상공간에서 랜덤하게 샘플점을 발생시킨 후 트리에서 가장 가까운 노드를 찾는다. 그리고 그 두점을 직선으로 연결한 선상에 트리로부터 미리 설정한 거리만큼 떨어진 새로운 점을 트리로 편입한다. 만약 새로운 노드와 기존 노드를 연결한 직선, 즉 트리의 연결선이 장애물과 충돌한다면 새로운 샘플점을 랜덤하게 다시 발생시킨 후 같은 방법을 다시 시도한다.
이런 방법을 시작점에서 시작하여 목표점이 트리에 연결될 때까지 반복 시행하면서 트리를 확장시켜서 나가는데, 트리가 고차원의 형상공간속으로 신속하고 균일하게 확장되면서 자유공간을 효과적으로 탐색하게 된다.
- 시작 지점과 목적 지점이 정해지면 반복적으로 공간의 한 점을 뽑아 트리 구조를 확장
- 장애물과 충돌이 없는 공간에서 무작위로 샘플을 추출
- 샘플과 현재까지 구축된 트리에서 가장 가까운 노드를 간선으로 연결
- 만약 샘플과 기존 노드를 연결한 직선이 장애물과 충돌한다면,
- 새로운 샘플을 다시 발생시킨 후 반복
- 시작 지점에서 목적 지점까지 하나의 연결된 궤적이 만들어질 때까지 트리를 확장
2.2 RRT 알고리즘
RRT 알고리즘을 구체적으로 살펴보자.
우선 트리를 초기화 한다. 초기화의 의미는 트리를 비운다는 뜻이다. 그리고 시작점을 트리에 편입한다. 시작점()이 트리의 루트(root)가 되는 것이다. 시작점은 루트로서 부모 노드가 없기 때문에 공집합이다.
이제 트리를 목표점에 연결될 때까지 확장시킨다. 우선 샘플점()을 한 개 발생시킨다. 그리고 트리에서 와 가장 가까운 노드 를 찾는다.
그 다음에는 노드 에서 방향으로 연결한 직선상에 일정한 거리 만큼 떨어진 점을 새로운 노드 로 선정한다.
만약 에서 까지 직선으로 연결한 선이 장애물과 충돌하지 않으면 를 트리로 편입한다.
위 과정을 반복한다. 샘플점()을 한 개 발생시킨다.
트리에서 $q{rand}와 가장 가까운 노드 $q{near}를 찾는다.
노드 에서 방향으로 연결한 직선상에 일정한 거리 만큼 떨어진 점을 새로운 노드 로 선정한다.
만약 에서 까지 직선으로 연결한 선이 장애물과 충돌하지 않으면 를 트리로 편입한다.
또 다시 샘플점()을 한 개 발생시킨다.
트리에서 와 가장 가까운 노드 를 찾는다. 노드 에서 방향으로 연결한 직선 상에 거리 만큼 떨어진 점을 새로운 노드 로 선정한다.
만약 장애물과 충돌한다면 를 버리고, 샘플점 를 다시 생성한다.
그리고 위 과정을 반복한다.
목표점이 트리의 노드와 연결되거나 혹은 매우 가까운 거리에 이르면 트리가 완성된 것으로 본다.
- 초기화: 시작 지점을 기점으로 트리 구조 초기화
- 무작위 샘플링 : 무작위로 한 점을 샘플링하고, 트리 구조의 기존 노드 중 가장 가까운 노드를 선택
- 연결 : 기존 트리에서 가장 가까운 노드에서 샘플링된 노드 방향으로 일정 거리 떨어진 지점에 새로운 노드를 만들고, 두 점 사이에 장애물이 충돌하지 않는다면, 새로운 노드를 트리에 추가
- 경로 생성 : 도착 지점에 연결될 때 까지 무작위 샘플링과 연결을 반복
BUILD RRT()
- 의사 코드(Pseudo code)
EXTEND()
- 개념도 및 의사코드
3. RRT 알고리즘 특징
- 전체 공간을 탐색하지 않아도, 충분히 많은 신규 노드가 생성되면 도착 지점까지의 경로를 빠르게 탐색 가능
- 샘플 수가 충분히 클 때, RRT알고리즘은 실현 가능한(feasible)경로를 찾아주지만. 최적성(optimality)를 보장하지는 않음
- 신규 노드의 확장 길이(Step-size)가 작으면, 도착 지점까지의 많은 반복과 노드가 필요할 수 있음
- 노드의 개수가 증가할수록 연산량과 메모리 사용량이 증가
- 반대로, 확장 길이(Step-size)가 너무 크면, 복잡한 지형에서 경로 탐색 시 장애물 등에 충돌이 많이 발생하여 적절한 경로를 탐색하기 어려울 수 있음
- 또한, 확장된 트리의 노드가 도착 지점 인근에 생성되기 어려울 수 있음
- 좁은 통로 문제의 경우에는 적절한 경로를 찾기 어려울 수 있음
4. RRT 알고리즘 코드
import numpy as np
import random
import math
import matplotlib.pyplot as plt
from matplotlib.patches import Circle, Rectangle
from matplotlib.animation import FuncAnimation
from matplotlib.lines import Line2D
class Node: # 트리의 각 노드를 나타내는 클래스를 정의
def __init__(self, x, y):
self.x = x # 노드의 x 좌표
self.y = y # 노드의 y 좌표
self.parent = None # 트리에서의 부모 노드
# 두 노드 사이의 유클리드 거리를 계산하는 함수
def euclidean_distance(node1, node2):
return math.sqrt((node1.x-node2.x)**2 + (node1.y-node2.y)**2) # 점과 점 사이 거리 공식(루트((x1-x2)제곱+(y1-y2)제곱))
# 주어진 범위 내에서 무작위로 노드를 생성하는 함수
def get_random_node(x_max, y_max):
x = random.uniform(0, x_max) # x_max 범위 내에서 무작위 x 좌표 생성
y = random.uniform(0, y_max) # y_max 범위 내에서 무작위 y 좌표 생성
return Node(x,y) # 생성된 노드 반환
# 주어진 노드에 가장 가까운 트리 노드를 찾는 함수
def get_nearest_node(rrt_nodes, rand_node):
min_distance = float('inf') # 최소 거리를 무한대로 초기화
nearest_node = None # 가장 가까운 노드를 저장할 변수
for node in rrt_nodes: # 모든 트리 노드에 대해 반복
distance = euclidean_distance(node, rand_node) # 노드와 무작위 노드 간의 거리 계산
if distance < min_distance: # 현재 거리가 최소 거리보다 작으면
min_distance = distance # 최소 거리 갱신
nearest_node = node # 가장 가까운 노드 갱신
return nearest_node # 가장 가까운 노드 반환
# 새로운 노드를 생성하는 함수
def create_new_node(nearest_node, rand_node, step_size):
# 가장 가까운 노드에서 무작위 노드 방향으로 가는 각도 계산
theta = math.atan2(rand_node.y - nearest_node.y , rand_node.x - nearest_node.x)
# 최대 이동 거리와 무작위 노드와의 거리 중 작은 값을 선택하여 이동 거리로 설정
dist = min(step_size, euclidean_distance(nearest_node, rand_node))
new_x = nearest_node.x + dist + math.cos(theta) # 새로운 노드의 x 좌표 계산
new_y = nearest_node.y + dist + math.sin(theta) # 새로운 노드의 y 좌표 계산
new_node = Node(new_x, new_y) # 새로운 좌표의 노드 생성
new_node.parent = nearest_node # 기존의 노드를 새로운 노드의 부모 노드로 갱신
return new_node # 새로운 노드 반환
# 목표 지점에 도달했는지 확인하는 함수
def goal_reached(goal_node, current_node, goal_radius):
# 현재 노드와 목표 노드 간의 거리가 목표 반경보다 작으면 도달한 것으로 판단
return euclidean_distance(goal_node, current_node) < goal_radius
# 원형 장애물 선언 클래스
class ObstacleCircle:
def __init__(self, x, y, radius):
self.x = x # 원의 x 좌표
self.y = y # 원의 y 좌표
self.radius = radius # 원의 반지름
# 주어진 점이 원의 내부에 있는지 확인하는 함수
def contains(self, point):
distance = euclidean_distance(point, self) # 원의 중심과 점(point)간의 거리 계산
return distance <= self.radius # 거리가 반지름 이하이면 원 내부에 있는 것으로 판단
# 직사각형 장애물 선언 클래스
class ObstacleRectangle:
def __init__(self, x, y, width, height):
self.x = x # 직사각형 x 좌표
self.y = y # 직사각형 y 좌표
self.width = width # 직사각형의 너비
self.height = height # 직사각형의 높이
# 주어진 점이 직사각형 내부에 있는지 확인하는 함수
def contains(self, point):
# 점이 직사각형 내부에 있으면 True, 아니면 False 반환
return (self.x < point.x <= self.x + self.width and self.y <= point.y <= self.y + self.height)
# 주어진 점이 장애물 내부에 있는지 확인하는 함수
def is_point_in_obstacle(point, obstacle):
if isinstance(obstacle, ObstacleCircle): # 주어진 객체가 원형장애물 객체인 경우
return obstacle.contains(point) # 원 내부에 있는지 확인
elif isinstance(obstacle, ObstacleRectangle): # 주어진 객체가 직사각형 장애물 객체인 경우
return obstacle.contains(point) # 직사각형 내부에 있는지 확인
return False # 그 외의 경우는 False 반환
# 노드 간의 경로가 장애물과 충돌하지 않는지 확인하는 함수
def is_collision_free(new_node, nearest_node, obstacles):
# 선분을 여러 점으로 나누어 충돌 여부 확인(거리를 0.1 간격으로 등분하여 나눠 몫만 가져옴)
num_point = int(euclidean_distance(new_node, nearest_node) // 0.1)
if num_point == 0: # 만약 선분 길이가 0이면 충돌 없음으로 판단(등분한 값이 0이면 new_node와 nearest_node거리가 충분히 짧음)
return True
# 두 노드 사이의 벡터를 계산하여 세그먼트의 길이와 방향을 결정함
vector = np.array([new_node.x - nearest_node.x, new_node.y - nearest_node.y]) / num_point
# 세그먼트의 시작점을 설정함
# 가장 가까운 노드의 위치를 시작점으로 사용함
point = np.array([nearest_node.x, nearest_node.y], dtype=float)
for i in range(num_point):
point += vector
for obstacle in obstacles:
if is_point_in_obstacle(Node(point[0], point[1]), obstacle): # 충돌이 발생하면
return False # False 반환
return True # 충돌이 없으면 True 반환
# RRT 경로 탐색 알고리즘을 실행하는 함수
def rrt_path_planning(start_node, goal_node, x_max, y_max, step_size, goal_radius, max_iterations, obstacles):
rrt_nodes = [start_node] # 시작 노드를 가진 트리 생성
for _ in range(max_iterations): # 최대 반복 횟수만큼 반복
rand_node = get_random_node(x_max, y_max) # 무작위 노드 생성
nearest_node = get_nearest_node(rrt_nodes, rand_node) # 가장 가까운 노드 찾기
new_node = create_new_node(nearest_node, rand_node, step_size) # 새로운 노드 생성
# 새로운 노드와 가장 가까운 노드 사이의 경로가 충돌하지 않으면
if is_collision_free(new_node, nearest_node, obstacles):
rrt_nodes.append(new_node) # 새로운 노드를 트리에 추가
# 목표 지점에 도달하면 경로 반환
if goal_reached(goal_node, new_node, goal_radius):
print("Goal Reached!!")
return rrt_nodes, new_node
print("Max iterations reached, goal not found!") # 최대 반복 횟수 동안 목표 지점에 도달하지 못한 경우
return rrt_nodes, None # 경로가 없으므로 None 반환
# 경로 및 트리를 시각화하는 함수
def plot_rrt(rrt_nodes, start_node, goal_node, obstacles, path_node):
fig, ax = plt.subplots()
ax.set_xlim(0, x_max)
ax.set_ylim(0, y_max)
ax.grid(True)
plot_obstacles(obstacles, ax)
# 트리 노드들을 선으로 연결하여 그리기
for node in rrt_nodes:
if node.parent:
ax.add_line(Line2D([node.parent.x, node.x], [node.parent.y, node.y], linewidth=1, color='black'))
# 경로 노드들을 선으로 연결하여 그리기
temp_node = path_node
while temp_node:
if temp_node.parent:
ax.add_line(Line2D([temp_node.parent.x, temp_node.x], [temp_node.parent.y, temp_node.y], linewidth=3, color='purple'))
temp_node = temp_node.parent
# 목표 지점과 가장 가까운 노드를 선으로 연결하여 그리기
nearest_node = get_nearest_node(rrt_nodes, goal_node)
ax.add_line(Line2D([nearest_node.x, goal_node.x], [nearest_node.y, goal_node.y], linewidth=3, color='purple'))
# 시작 지점과 목표 지점을 점으로 표시
ax.scatter([start_node.x], [start_node.y], c='blue', s=50, label='Start')
ax.scatter([goal_node.x], [goal_node.y], c='green', s=50, label='Goal')
plt.legend()
plt.show()
# 장애물을 시각화하는 함수
def plot_obstacles(obstacles, ax):
for obstacle in obstacles: # 모든 장애물에 대해 반복
if isinstance(obstacle, ObstacleCircle): # 원형 장애물인 경우
circle = Circle((obstacle.x, obstacle.y), obstacle.radius, color='gray', alpha=0.5) # 원 그리기
ax.add_patch(circle) # 원을 그래프에 추가
elif isinstance(obstacle, ObstacleRectangle): # 직사각형 장애물인 경우
Ractangle = Rectangle((obstacle.x, obstacle.y), obstacle.width, obstacle.height, color='gray', alpha=0.5)
ax.add_patch(Ractangle)
# 경로의 비용을 계산하는 함수
def calculate_cost(path_node):
path = [path_node] # 경로 노드들을 저장하는 리스트
while path_node != start_node: # 시작 노드에 도달할 때까지 반복
path_node = path_node.parent # 부모 노드를 찾아서 경로에 추가(역추적)
path.append(path_node) # 경로에 노드 추가
cost = 0 # 비용 초기화
for i in range(len(path) - 1): # 모든 경로 노드에 대해 반복
cost += euclidean_distance(path[i], path[i+1]) # 각 선분의 길이를 비용에 추가
return cost
if __name__ == "__main__":
start_node = Node(1,1) # 시작 노드 생성
goal_node = Node(9,9) # 목표 노드 생성
x_max, y_max = (10,10) # X와 Y의 최대값 설정
step_size = 0.5 # 노드 간의 최대 이동 거리 설정
goal_radius = 0.5 # 목표 반경 설정
max_iterations = 1000 # 최대 반복 횟수 설정
# 장애물 리스트 생성
obstacles = [
ObstacleCircle(3,5,1),
ObstacleCircle(6,5,1),
ObstacleCircle(8,2,1),
ObstacleRectangle(4,2,2,2)
]
# RRT 경로 탐색 알고리즘 실행
rrt_nodes, path_node = rrt_path_planning(start_node, goal_node, x_max, y_max, step_size, goal_radius, max_iterations, obstacles)
if path_node: # 만약 경로가 존재하면
cost = calculate_cost(path_node) # 경로 비용 계산
print("Cost of the final path : {0}".format(cost))
else:
print("No path found")
# 경로 및 트리플 시각화
plot_rrt(rrt_nodes, start_node, goal_node, obstacles, path_node)
Robotics