알고리즘
[RRT] Informed RRT star
정지홍
2025. 4. 12. 10:38
Informed RRT *
- 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효율을 크게 끌어올림.
- -> 일반적은 RRT*는 전역에서 randomSampling()을 한다. --->
- 2. Ellipsoidal 영역
- start와 goal을 잇는 shortest path가 c*라고 할때, 이는 두 점을 잇는 '직선'일수도, 동역학이나 장애물을 고려할수 있으나 우선은 유클리드 거리로 이해한다. --->
informed rrt에서 핵심적으로 사용하는 것은, start와 goal을 초점으로 하는 타원체를 정의하는것이다. --->
이때 타원체는 '만약 어떠한 점이, 이 타원체 안에 존재한다면, 이 점을 연결하는 path의 length가 c* 이하일 가능성이 있다'는 점을 바탕으로 설정된다. - 위의 솔명을 단순화해서 유클리드 거리를 기준으로 예를 들면, start부터 goal까지의 최소 비용이 c*일때, 이것보다 짧은 경로가 존재하려면, "start와 goal까지의 거리합이 c*이하"라는 제약이 존재한다.
이 공간을 "에러 바운드가 있는 유효 샘플링 영역"으로 잡고, 여기에 대해서 균등분포로 샘플링한다.
- start와 goal을 잇는 shortest path가 c*라고 할때, 이는 두 점을 잇는 '직선'일수도, 동역학이나 장애물을 고려할수 있으나 우선은 유클리드 거리로 이해한다. --->
- 알고리즘의 대략적인 개요
- 1. initial search
- 기본 RRT*와 같은 탐색 알고리즘으로 빠르게 경로를 찾는다. 그리고 이 경로의 비용을 c*라고 한다.
- 2. informed samping 영역 설정
- 현재까지 발견된 최적 비용 c*을 기준으로, start와 goal을 초점으로 하는 고차원 타원체를 정의.
- 정의한 타원체 범위 내에서만 sampling을 진행한다.
- 현재까지 발견된 최적 비용 c*을 기준으로, start와 goal을 초점으로 하는 고차원 타원체를 정의.
- 3. RRT 확장 및 개선
- 새로 뽑은 샘플들을 기존 트리에 연결(Extend)하여 경로를 업데이트한다.
- 더 짧은 경로를 발견하면, c*를 갱신하고, 타원체 범위를 다시 재설정하여 탐색을 이어감.
- 새로 뽑은 샘플들을 기존 트리에 연결(Extend)하여 경로를 업데이트한다.
- 4. 종료 조건
- 시간 제한, 반복횟수 등에 도달하면 종료
- 이론적으로 샘플링을 무한히 증가시키면 최적해에 수렴
- 1. initial search
- 타원체 샘플링 기법
- 표준 분포 --> 타원체 매핑
- 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 , axclass 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)