Informed RRT*는 기존의 RRT,RRT* 계열의 알고리즘을 개선하여, 초기 solution을 찾은 뒤, 이후에 좀 더 효율적인 탐색을 통해서 경로 비용을 단축하고자 하는 기법이다.
특히, sampling을 불필요한 전체 공간을 하는것이 아니라, solution에 의미있는 공간만을 집중적으로 sampling하는 방법을 사용한다.
ex) Informed RRT는 초기 경로를 찾은뒤, 해당 경로의 비용을 c*라고 하자. 그러면 c*보다 더 좋은 경로를 찾을 수 있는 지역만 집중적으로 sampling한다.
장점
1. 탐색 효율 개선 : 이미 발견한 path보다 개선의 여지가 없는 공간은 sampling을 하지않는다.
2. 빠른 비용 수렴 : RRT와 결합시, 단순 RRT와 대비하여 적은 samples로 더 좋은 경로를 발견하게 된다.
3. 일반화 용이 : 구성 공산의 차원이 높아도 동일한 수학적 원리를 적용해서, sampling 범위를 줄일수있다.
단점
1. 초기 경로 필요 : 타원체를 정의하기 위해서는 '최초 경로의 비용'이 필요하므로, 처음부터 informed 샘플링을 사용하기는 어렵다.
2. 장애물이 많은 복잡한 환경 : 장애물이 많은 환경이라면, 여전히 많은 노드를 만들어야 할 수 있다.
3. 고차원에서도 여전히 샘플링 비용이 부담
핵심 아이디어
informed RRT가 목표하는 것은, 우선 임의의 경로를 하나 찾은 뒤, 그 해보다 비용이 더 좋은 해를 찾을 가능성이 있는 부분만 선택적으로 샘플링하여, 빠르게 최적성을 개선하는 것이다.
1. sampling 공간의 축소
-> 일반적은 RRT*는 전역에서 randomSampling()을 한다. ---> 하지만 이미 현재까지 찾은 경로의 비용 c*가 존재한다면, 이보다 비용이 작아질 가능성이 없는 sampling은 최적 경로 탐색에 도움이 안된다. ---> 그래서 informed rrt*는 '이보다 나은 경로'가 존재할 법한, 유의미한 부분 공간만을 골라서 sampling함으로써, sampling효율을 크게 끌어올림.
2. Ellipsoidal 영역
start와 goal을 잇는 shortest path가 c*라고 할때, 이는 두 점을 잇는 '직선'일수도, 동역학이나 장애물을 고려할수 있으나 우선은 유클리드 거리로 이해한다. ---> informed rrt에서 핵심적으로 사용하는 것은, start와 goal을 초점으로 하는 타원체를 정의하는것이다. ---> 이때 타원체는 '만약 어떠한 점이, 이 타원체 안에 존재한다면, 이 점을 연결하는 path의 length가 c* 이하일 가능성이 있다'는 점을 바탕으로 설정된다.
위의 솔명을 단순화해서 유클리드 거리를 기준으로 예를 들면, start부터 goal까지의 최소 비용이 c*일때, 이것보다 짧은 경로가 존재하려면, "start와 goal까지의 거리합이 c*이하"라는 제약이 존재한다. 이 공간을 "에러 바운드가 있는 유효 샘플링 영역"으로 잡고, 여기에 대해서 균등분포로 샘플링한다.
타원영역에 대한 정의 예시... 우선 가정을 위와 같이 하자만약에 현재 최적 비용보다 적은 비용의 경로가 나타났을 경우...
알고리즘의 대략적인 개요
1. initial search
기본 RRT*와 같은 탐색 알고리즘으로 빠르게 경로를 찾는다. 그리고 이 경로의 비용을 c*라고 한다.
2. informed samping 영역 설정
현재까지 발견된 최적 비용 c*을 기준으로, start와 goal을 초점으로 하는 고차원 타원체를 정의.
정의한 타원체 범위 내에서만 sampling을 진행한다.
3. RRT 확장 및 개선
새로 뽑은 샘플들을 기존 트리에 연결(Extend)하여 경로를 업데이트한다.
더 짧은 경로를 발견하면, c*를 갱신하고, 타원체 범위를 다시 재설정하여 탐색을 이어감.
4. 종료 조건
시간 제한, 반복횟수 등에 도달하면 종료
이론적으로 샘플링을 무한히 증가시키면 최적해에 수렴
타원체 샘플링 기법
표준 분포 --> 타원체 매핑
1. 우선 단위 N차원 구의 sample을 균등하게 얻는다. ( e.g. 가우시안 표본을 정규화 )
2. 샘플를 타원체로 선형 변환한다.
이때 고유벡터와 고유값 or SVD 등을 이용해서, start~goal방향을 정렬하고, 필요에 따라서 축의 비율을 맞춘다.
3. 변환된 샘플은 "Start-goal에서 정의되는 최대 비용 c*을 제한"을 만족하는 공간에 위치한다.
import random
import math
import matplotlib.pyplot as plt
import time
class Map2D:
def __init__( self , width=10 , height=10 ):
self.width = width
self.height = height
self.obstacles = [ (2, 10, 3, 2) , (6,0,7,8,) ]
def is_in_obstacle( self , x , y ):
for ( x_min , y_min , x_max , y_max ) in self.obstacles:
if( x_min <= x_max) and ( y_min <= y_max ):
if ( x_min < x < x_max ) and ( y_min < y < y_max ):
return True
elif ( x_max <= x_min ) and ( y_min <= y_max ):
if ( x_max < x < x_min ) and ( y_min < y < y_max ):
return True
elif ( x_max <= x_min ) and ( y_max <= y_min ):
if ( x_max < x < x_min ) and ( y_max < y < y_min ):
return True
elif ( x_min <= x_max ) and ( y_max <= y_min ):
if ( x_min < x < x_max ) and ( y_max < y < y_min ):
return True
return False
def plot_map( self ):
fig , ax = plt.subplots()
ax.set_xlim( 0 , self.width )
ax.set_ylim( 0 , self.height )
ax.set_aspect("equal" , adjustable="box" )
ax.set_title("2d map")
for ( x_min , y_min , x_max , y_max ) in self.obstacles:
rect_width = x_max - x_min
rect_height = y_max - y_min
obstacle_rect = plt.Rectangle( (x_min,y_min) , rect_width , rect_height , fill=True , alpha=0.4 ) # alpha는 불투명도
ax.add_patch( obstacle_rect )
return fig , ax
class Node:
def __init__( self , x , y , parent=None , cost=None ):
self.x , self.y , self.parent , self.cost = x , y, parent , cost
self.children = []
class RRTInformed:
def __init__( self , map_2d , start , goal , step_size=0.5 , max_iter=100 , goal_sample_rate=0.05 , dist_threshold=0.3 , near_neighbor_dist=1 ):
self.map = map_2d
self.start , self.goal = Node( start[0] , start[1] , cost=0 ) , Node( goal[0] , goal[1] )
self.step_size , self.max_iter = step_size , max_iter
self.goal_sample_rate , self.dist_threshold = goal_sample_rate , dist_threshold
self.tree = [ self.start ]
self.fig , self.ax = self.map.plot_map()
self.near_neighbor_dist = near_neighbor_dist
self.c_min = None
self.x_center = None
self.C = None
# ============================= 유틸리티 =====================================
def distance( self , from_node , to_node ):
return math.hypot( to_node.x - from_node.x , to_node.y - from_node.y )
def collision_free( self , node1 , node2 ):
steps = max( int( self.distance( node1 , node2 ) / (self.step_size * 0.5 ) ) , 1 )
for i in range( steps + 1):
t = i/steps
x = node1.x + t*( node2.x - node1.x )
y = node1.y + t*( node2.y - node1.y )
if self.map.is_in_obstacle( x , y ):
return False
return True
def _update_subtree_cost( self , node ):
for child in node.children:
new_cost = node.cost + self.distance( node , child )
child.cost = new_cost
self._update_subtree_cost(child)
def rotation_to_world_frame_2d( self ):
# 1. 목표 방향 벡터를 구한다
dx = self.goal.x - self.start.x
dy = self.goal.y - self.start.y
# 2. 목표 방향의 단위 벡터를 구한다
a1x = dx/self.c_min
a1y = dy/self.c_min
theta = math.atan2( a1y , a1x )
cos_t = math.cos( theta )
sin_t = math.sin( theta )
C = [
[ cos_t , -sin_t],
[ sin_t , cos_t]
]
return C
# ============================= RRT*로직 =====================================
# RRT*-1
def random_sampling( self ):
if random.random() < self.goal_sample_rate:
return Node( self.goal.x , self.goal.y )
else:
x , y = random.uniform( 0 , self.map.width ) , random.uniform( 0 , self.map.height )
return Node( x , y )
# RRT*-2
def get_near_node_WhereInTree( self , tree , q_rand ):
return min( tree , key=lambda nd: self.distance( nd , q_rand ) )
# RRT*-3
def steer( self , from_node , to_node ):
dist = self.distance( from_node , to_node )
if dist < self.step_size:
q_new = Node( to_node.x , to_node.y , parent=from_node )
return q_new
else:
theta = math.atan2( to_node.y - from_node.y , to_node.x - from_node.x )
new_x = from_node.x + self.step_size * math.cos( theta )
new_y = from_node.y + self.step_size * math.sin( theta )
q_new = Node( new_x , new_y , parent=from_node )
return q_new
# RRT*-5
def qNew_choose_bestParent( self , q_new ):
rst = []
for nd in self.tree:
if self.distance( q_new ,nd ) <= self.near_neighbor_dist:
if self.collision_free( q_new , nd ):
rst.append( nd )
if len(rst)==0:
q_new.cost = q_new.parent.cost + self.distance( q_new.parent , q_new )
q_new.parent.children.apprend(q_new)
self.tree.append(q_new)
return rst
else:
best_parent = q_new.parent
best_cost = q_new.parent.cost + self.distance( q_new.parent , q_new )
for parent_candidate in rst:
candidate_cost = parent_candidate.cost + self.distance( parent_candidate , q_new )
if candidate_cost < best_cost:
best_parent , best_cost = parent_candidate , candidate_cost
q_new.parent= best_parent
q_new.cost = best_cost
q_new.parent.children.append(q_new)
self.tree.append(q_new)
return rst
# RRT*-6
def rewire( self , q_new , nodes ):
for child_candidate in nodes:
candidate_cost = q_new.cost + self.distance( q_new , child_candidate )
if candidate_cost < child_candidate.cost:
if child_candidate.parent:
child_candidate.parent.children.remove( child_candidate )
child_candidate.parent = q_new
q_new.children.append( child_candidate )
child_candidate.cost = candidate_cost
self._update_subtree_cost( child_candidate)
# ============================= Informed RRT로직 =====================================
def eillpsoidal_random_sampling( self ):
a = self.goal.cost / 2.0 # 장축의 절반
c = self.c_min / 2.0 # 초점간의 거리의 절반
b = math.sqrt( max( a*a - c*c , 0 ) )/2 # 단축의 절반
# 단위원( 반지름이 1인 원)안에서 면적을 균등하게 점을 샘플링하는 부분이다.
# 그래서 우선 단위원 내부에서 균등하게 샘플링을 수행하고, 이 점을 타원체의 형태에 맞게 선형 변환을 수행하여 실제 타원 내부의 점으로 변환한다.
u = random.random()
r = math.sqrt( u ) # 단위원의 반지름
phi = random.uniform( 0 , 2*math.pi ) # 단위원 내부의 각도
x_u = r*math.cos( phi )
y_u = r*math.sin( phi )
x_s = a*x_u
y_s = b*y_u
# 4) 회전 변환 C @ [x_s, y_s]
x_r = self.C[0][0]*x_s + self.C[0][1]*y_s
y_r = self.C[1][0]*x_s + self.C[1][1]*y_s
# 5) 월드 좌표로 이동: + x_center
x_w = x_r + self.x_center.x
y_w = y_r + self.x_center.y
return Node(x_w, y_w)
# =============================== build =====================================
def build(self):
start_time = time.time()
self.c_min = self.distance( self.goal , self.start )
self.x_center = Node( (self.goal.x + self.start.x)/2 , (self.goal.y + self.goal.x)/2 )
self.C = self.rotation_to_world_frame_2d()
for i in range( self.max_iter ):
# 1. 우선 RRT-star로 경로를 찾는다
if self.goal.cost==None:
q_rand = self.random_sampling() # RRT*-1 : 랜덤샘플링
q_near = self.get_near_node_WhereInTree( self.tree , q_rand ) # RRT*-2 : tree에서 q_rand와 가장 가까운 노드 찾기
q_new = self.steer( q_near , q_rand )# RRT*-3 : tree의 q_near에서 q_rand방향으로 뻗어가는 q_new를 찾기
if not self.collision_free( q_new , q_near ): # RRT*-4 : q_new와 q_near사이에 장애물 존재시 ==> 처음부터 다시 random sampling
continue
qNew_bestParent_candidate = self.qNew_choose_bestParent( q_new ) # RRT*-5 : q_new의 현재 부모보다 더 가까운 부모가 있는지 찾아본다.
self.rewire( q_new , qNew_bestParent_candidate ) # RRT*-6 : 트리에 존재하는 q_new근처 노드에서 q_new가 부모가 되었을때 최적인 경우를 찾아본다.
self.ax.plot( [q_new.parent.x , q_new.x] , [q_new.parent.y , q_new.y] , '-g' , linewidth=2.5 , alpha=0.5)
if self.distance( q_new , self.goal ) < self.dist_threshold:
end_time = time.time()
print(f'informed RRT를 위한 최초 경로를 찾음! (사용 알고리즘 RRT* ) 걸린 시간 : {end_time - start_time: .4f}초')
self.goal.parent = self.tree[-1]
self.goal.cost = self.goal.parent.cost + self.distance( self.goal.parent , self.goal )
path=self.reconstruct_path()
print('아래서부터는 informed rrt를 적용합니다....\n==============================\n')
else:
# print("여기서부터 eillipsoidal random sampling를....\n==============================\n")
q_rand = self.eillpsoidal_random_sampling() # 타원체안에서 샘플을 생성.
# =========아래는 기존과 동일 =====
q_near = self.get_near_node_WhereInTree( self.tree , q_rand ) # RRT*-2 : tree에서 q_rand와 가장 가까운 노드 찾기
q_new = self.steer( q_near , q_rand )# RRT*-3 : tree의 q_near에서 q_rand방향으로 뻗어가는 q_new를 찾기
if not self.collision_free( q_new , q_near ): # RRT*-4 : q_new와 q_near사이에 장애물 존재시 ==> 처음부터 다시 random sampling
continue
qNew_bestParent_candidate = self.qNew_choose_bestParent( q_new ) # RRT*-5 : q_new의 현재 부모보다 더 가까운 부모가 있는지 찾아본다.
self.rewire( q_new , qNew_bestParent_candidate ) # RRT*-6 : 트리에 존재하는 q_new근처 노드에서 q_new가 부모가 되었을때 최적인 경우를 찾아본다.
self.ax.plot( [q_new.parent.x , q_new.x] , [q_new.parent.y , q_new.y] , '-g' , linewidth=2.5 , alpha=0.5)
if self.distance( q_new , self.goal ) < self.dist_threshold:
if ( q_new.cost + self.distance( q_new , self.goal ) ) < self.goal.cost:
self.goal.parent = self.tree[-1]
self.goal.cost = self.goal.parent.cost + self.distance( self.goal.parent , self.goal )
path=self.reconstruct_path()
print("!")
return path
def reconstruct_path( self ):
path = [ ]
cur = self.goal
# print(cur.parent)
while cur is not None:
path.append( ( cur.x , cur.y ) )
cur = cur.parent
path.reverse()
print(f'최종적인 경로의 길이 {len(path)} , {self.goal.cost}')
return path
def visualize_path(self, path):
if path is None:
print("error")
return
# 1. 트리의 모든 노드 위치를 검정색 점으로 표시
node_x = [node.x for node in self.tree]
node_y = [node.y for node in self.tree]
self.ax.scatter(node_x, node_y, c='k', s=5, label='Nodes') # 'k'는 검정색, s는 점 크기
# 2. 경로가 있으면 경로 시각화
px, py = zip(*path)
self.ax.plot(px, py, '-r', linewidth=1, label='Final Path') # 최종 경로(빨간색 선)
self.ax.plot(self.start.x, self.start.y, 'bo', label='Start') # 시작점(파란 원)
self.ax.plot(self.goal.x, self.goal.y, 'bx', label='Goal') # 목표점(파란 X)
# self.ax.legend() # 필요하면 범례 활성화
plt.show()
if __name__ == "__main__":
# 맵 생성
my_map = Map2D()
# 시작점, 목표점
start_pos = (1, 9)
goal_pos = (9, 1)
# RRT-Connect 객체 생성
rrt_connect = RRTInformed(
map_2d=my_map,
start=start_pos,
goal=goal_pos,
step_size=0.3, # 확장 시 한 번에 이동할 거리
max_iter=3000, # 최대 반복 횟수
goal_sample_rate=0.1 # goal을 직접 샘플링할 확률(10%)
)
# 알고리즘 실행
final_path = rrt_connect.build()
# print(final_path)
# 결과 시각화
rrt_connect.visualize_path(final_path)
