📜 주제
Rapidly-exploring Random Tree에 대해서 알아보자
💡 path planning이란
경로계획(path planning)이란 출발지점에서 도착지까지 갈 수 있는 경로를 찾아야 하는 문제이다. 로보틱스 분야나 자율주행분야가 발달하면서 꾸준히 연구가 이뤄지는 분야로써 다음과 같은 알고리즘들이 존재한다.
- 다익스트라 알고리즘
- A* 알고리즘
- RRT 알고리즘
- RRT* 알고리즘
이번에는 현재 가장 인기를 모으는 RRT 알고리즘을 주로 다뤄볼 예정이다.
✏️ RRT란?
1) RRT에 대해서..
RRT는 Rapidly-exploring Random Tree의 줄임말로 샘플링 기반 경로계획법 중 하나이다.
RRT는 탐색 영역 전체에 대해서 랜덤으로 노드를 샘플점을 생성하고 시작점부터 트리를 서서히 성장시켜 가면서 도착지점까지 경로를 생성하는 알고리즘이다. 이 알고리즘은 고차원 환경에서도 사용가능하기 때문에 활용성이 높다는 장점이 있다. 하지만 이미 생성된 트리를 수정하는 과정이 없기 때문에 경로의 최단거리는 보장되지 않는데 이를 보완하기 위해 만들어진 알고리즘이 RRT*이다.
이번에는 RRT에 대해서만 알아볼 예정이다.
2) RRT 구현법
1. 우선 트리를 초기화 한다. 그리고 시작점 q_init이 트리의 루트가 된다.
-
먼저 공간에서 샘플점을 랜덤하게 한개 발생시킨다. 그리고 트리에서 랜덤하게 발생시킨 q_rand와 가장 가까운 노드 q_near를 만든다. 예를 들어 초기화했을땐 가장 가까운 노드는 q_init이 되니 q_near는 q_init이 되는것이다.
-
그 다음 노드 q_near에서 q_rand 방향으로 delta q만큼 떨어진 점을 새로운 노드, 즉, q_new로 삼고 q_near에서 q_new 사이에 장애물이 없다면 q_new를 트리로 편입시킨다.
아래는 자세한 코드 내용이다.
"""
Path planning Sample Code with Randomized Rapidly-Exploring Random Trees (RRT)
author: AtsushiSakai(@Atsushi_twi)
"""
import math
import random
import matplotlib.pyplot as plt
import numpy as np
show_animation = True
class RRT:
"""
Class for RRT planning
"""
class Node:
"""
RRT Node
"""
def __init__(self, x, y):
self.x = x
self.y = y
self.path_x = []
self.path_y = []
self.parent = None
class AreaBounds:
def __init__(self, area):
self.xmin = float(area[0])
self.xmax = float(area[1])
self.ymin = float(area[2])
self.ymax = float(area[3])
def __init__(self,
start,
goal,
obstacle_list,
rand_area,
expand_dis=3.0,
path_resolution=0.5,
goal_sample_rate=5,
max_iter=500,
play_area=None
):
"""
Setting Parameter
start:Start Position [x,y]
goal:Goal Position [x,y]
obstacleList:obstacle Positions [[x,y,size],...]
randArea:Random Sampling Area [min,max]
play_area:stay inside this area [xmin,xmax,ymin,ymax]
"""
self.start = self.Node(start[0], start[1])
self.end = self.Node(goal[0], goal[1])
self.min_rand = rand_area[0]
self.max_rand = rand_area[1]
if play_area is not None:
self.play_area = self.AreaBounds(play_area)
else:
self.play_area = None
self.expand_dis = expand_dis
self.path_resolution = path_resolution
self.goal_sample_rate = goal_sample_rate
self.max_iter = max_iter
self.obstacle_list = obstacle_list
self.node_list = []
def planning(self, animation=True):
"""
rrt path planning
animation: flag for animation on or off
"""
self.node_list = [self.start]
for i in range(self.max_iter):
rnd_node = self.get_random_node()
nearest_ind = self.get_nearest_node_index(self.node_list, rnd_node)
nearest_node = self.node_list[nearest_ind]
new_node = self.steer(nearest_node, rnd_node, self.expand_dis)
if self.check_if_outside_play_area(new_node, self.play_area) and \
self.check_collision(new_node, self.obstacle_list):
self.node_list.append(new_node)
if animation and i % 5 == 0:
self.draw_graph(rnd_node)
if self.calc_dist_to_goal(self.node_list[-1].x,
self.node_list[-1].y) <= self.expand_dis:
final_node = self.steer(self.node_list[-1], self.end,
self.expand_dis)
if self.check_collision(final_node, self.obstacle_list):
return self.generate_final_course(len(self.node_list) - 1)
if animation and i % 5:
self.draw_graph(rnd_node)
return None # cannot find path
def steer(self, from_node, to_node, extend_length=float("inf")):
new_node = self.Node(from_node.x, from_node.y)
d, theta = self.calc_distance_and_angle(new_node, to_node)
new_node.path_x = [new_node.x]
new_node.path_y = [new_node.y]
if extend_length > d:
extend_length = d
n_expand = math.floor(extend_length / self.path_resolution)
for _ in range(n_expand):
new_node.x += self.path_resolution * math.cos(theta)
new_node.y += self.path_resolution * math.sin(theta)
new_node.path_x.append(new_node.x)
new_node.path_y.append(new_node.y)
d, _ = self.calc_distance_and_angle(new_node, to_node)
if d <= self.path_resolution:
new_node.path_x.append(to_node.x)
new_node.path_y.append(to_node.y)
new_node.x = to_node.x
new_node.y = to_node.y
new_node.parent = from_node
return new_node
def generate_final_course(self, goal_ind):
path = [[self.end.x, self.end.y]]
node = self.node_list[goal_ind]
while node.parent is not None:
path.append([node.x, node.y])
node = node.parent
path.append([node.x, node.y])
return path
def calc_dist_to_goal(self, x, y):
dx = x - self.end.x
dy = y - self.end.y
return math.hypot(dx, dy)
def get_random_node(self):
if random.randint(0, 100) > self.goal_sample_rate:
rnd = self.Node(
random.uniform(self.min_rand, self.max_rand),
random.uniform(self.min_rand, self.max_rand))
else: # goal point sampling
rnd = self.Node(self.end.x, self.end.y)
return rnd
def draw_graph(self, rnd=None):
plt.clf()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
if rnd is not None:
plt.plot(rnd.x, rnd.y, "^k")
for node in self.node_list:
if node.parent:
plt.plot(node.path_x, node.path_y, "-g")
for (ox, oy, size) in self.obstacle_list:
self.plot_circle(ox, oy, size)
if self.play_area is not None:
plt.plot([self.play_area.xmin, self.play_area.xmax,
self.play_area.xmax, self.play_area.xmin,
self.play_area.xmin],
[self.play_area.ymin, self.play_area.ymin,
self.play_area.ymax, self.play_area.ymax,
self.play_area.ymin],
"-k")
plt.plot(self.start.x, self.start.y, "xr")
plt.plot(self.end.x, self.end.y, "xr")
plt.axis("equal")
plt.axis([-2, 15, -2, 15])
plt.grid(True)
plt.pause(0.01)
@staticmethod
def plot_circle(x, y, size, color="-b"): # pragma: no cover
deg = list(range(0, 360, 5))
deg.append(0)
xl = [x + size * math.cos(np.deg2rad(d)) for d in deg]
yl = [y + size * math.sin(np.deg2rad(d)) for d in deg]
plt.plot(xl, yl, color)
@staticmethod
def get_nearest_node_index(node_list, rnd_node):
dlist = [(node.x - rnd_node.x)**2 + (node.y - rnd_node.y)**2
for node in node_list]
minind = dlist.index(min(dlist))
return minind
@staticmethod
def check_if_outside_play_area(node, play_area):
if play_area is None:
return True # no play_area was defined, every pos should be ok
if node.x < play_area.xmin or node.x > play_area.xmax or \
node.y < play_area.ymin or node.y > play_area.ymax:
return False # outside - bad
else:
return True # inside - ok
@staticmethod
def check_collision(node, obstacleList):
if node is None:
return False
for (ox, oy, size) in obstacleList:
dx_list = [ox - x for x in node.path_x]
dy_list = [oy - y for y in node.path_y]
d_list = [dx * dx + dy * dy for (dx, dy) in zip(dx_list, dy_list)]
if min(d_list) <= size**2:
return False # collision
return True # safe
@staticmethod
def calc_distance_and_angle(from_node, to_node):
dx = to_node.x - from_node.x
dy = to_node.y - from_node.y
d = math.hypot(dx, dy)
theta = math.atan2(dy, dx)
return d, theta
def main(gx=6.0, gy=10.0):
print("start " + __file__)
# ====Search Path with RRT====
obstacleList = [(5, 5, 1), (3, 6, 2), (3, 8, 2), (3, 10, 2), (7, 5, 2),
(9, 5, 2), (8, 10, 1)] # [x, y, radius]
# Set Initial parameters
rrt = RRT(
start=[0, 0],
goal=[gx, gy],
rand_area=[-2, 15],
obstacle_list=obstacleList,
# play_area=[0, 10, 0, 14]
)
path = rrt.planning(animation=show_animation)
if path is None:
print("Cannot find path")
else:
print("found path!!")
# Draw final path
if show_animation:
rrt.draw_graph()
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
plt.grid(True)
plt.pause(0.01) # Need for Mac
plt.show()
if __name__ == '__main__':
main()
🤔 정리 및 참고 문서
RRT는 다양한 형태의 장애물이나 환경에서도 효율적으로 경로를 생성할 수 있지만 샘플링 개수가 충분하지 않다면 경로를 찾지 못한다는 단점이 있다.
또한 위에서 말했듯이 항상 최적의 경로가 아니라는 점이 있다.
때문에 경로를 찾고 다시 최적의 경로를 찾아야 할수도 있기 때문에 다양한 알고리즘들이 파생되었다.
https://github.com/joonyeolsim