RRT*-Smart는 RRT*의 느린 수렴 속도를 획기적으로 개선하기 위해서 고안된 알고리즘이다. 그리서 아래의 2가지 기법을 사용한다.
1. path optimization
2. intelligent sampling
==> 위의 2가지 기법을 통해서, 초기 경로를 주기적으로 정제하고, 샘플링으로 유망한 구간('비콘')으로 집중시켜서 빠른 수렴과 적은 노드 수를 달성한다.
핵심 기법
1. path optimization
1-1. 삼각 부등식( truangular inequality ) 기반 단축
현재 경로 상에 서로 다른 두 노드 사이에 직접적인 충돌이 없다면, 중간 노드를 모두 제거하고 더 짧은 경로로 대체.
충돌 검사 없이도, 선분 보간( interpolation )만으로도 자유 공간 여부를 판단할 수 있어서 장애물 형상에 독립적이다.
1-2. beacons선정
최적화된 경로 상의 nodes가 beacons로 선정되며, 이후에 샘플링 시에 중심점 역할을 한다.
2. intelligent sampling
2-1. beacon 중심 편향 샘플링
각 beacons의 주변 반경 구역 내에서 샘플을 생성한다.
일정한 비율(bias)로 무작위 전역 샘플링을 병행한다.
2-2. bias비율 제어
bias가 클수록 빠른 최적화를 가져오지만, 탐색 커버리지 공간이 줄어드는 trade-off가 존재한다.
시간 복잡도
RRT*와 동일하게 nlog n이다.
메모리 복잡도
O(n)
수렴 속도
동일한 반복수와 대비해서, cost 감소율이 rrt*보다 훨씬 빠르다.
trade-off
bias가 클수록, 빠른게 경로가 감수하지만 탐색의 다양성이 내려감
bias가 작을수록, 전역탐색을 하지만 속도는 느려짐.
전체적인 의사코드path optimizationupdated beacons
1-1. 삼각 부등식 기반의 단축 - 수학적 근거
기호 ❘❘ a - b ❘❘는 점a와 점b의 거리를 나타내는 공식이다. ( 유클리드 거리 )
1-2. 삼각 부등식 기반의 단축 - 충돌검사
선분 보간 기번을 사용한다.
1. 거리 || zi - zj ||를 n개의 구간으로 나눈다.
2. 각 보간점 z( α ) = ( 1−α )zi + αzj( α=k/n )에서 장애물 충돌 유무만 검사한다.
파라미터
보간 해상도 n=⌈ ( || zi−zj || ) / δ ⌉
δ : 최대 충돌 검사 간격 ( 일반적으로 로봇의 크기와 환경의 복잡도를 고려해서 설정한다. )
==> 복잡한 장애물 형상에 독립적이라는 장점을 가짐. 보간 해상도인 n값만 조절하면 정확도와 속도의 균형을 가질 수 있다.
총 길이와 분할 개수 계산에 대한 예시. 아래 표는 보간점 계산에 대한 예시이다. 아래표의 보간점들에 대해서 충돌 검사를 하면 된다.
k
a k
z( ak ) <==이게 보간점이며, 이것들에게 충돌 검사를 수행하면 됨.
1
0.2
( 1.6 , 1.8 )
2
0.4
( 2.2 , 2.6 )
3
0.6
( 2.8 , 3.4 )
4
0.8
( 3.4 , 4.2 )
1-3. 삼각 부등식 기반의 단축 - beacons 설정
경로 최적화 후 얻은 τ_opt 상의 노드들은, 이후 지능적 샘플링 시점의 “중심점”(비콘)으로 사용된다.
아래는 선정 방법들...
1. 모든 노드를 beacons로 사용
간단하지만, node의 수가 많아지면 편향 영역이 과다해짐. ( overfitting의 우려 )
2. 등간격 샘플링
경로 길이 L을 기준으로 고정 간격 l마다 노드를 선택한다.
비콘 개수 ≈ ⌈ L / ℓ ⌉
3. 거리 기반 클러스터링
τ_opt상 인접 노드들을 클러스터로 묶고, 각 클러스터 중심만 beacons으로 설정\
4. 꺽이는 지점에 대한 노드를 beacon으로
꺽이는 지점은 일반적으로 경로가 복잡하거나 장애물을 피해서 돌아가는 지점이다.==> 그래서 이 근처에서 더 좋은 경로를 발견할 가능성이 높다.
파라미터..
l ( 간격 ): 환경의 크기와 정밀도 요구에 비례
beacons 반경 : beacons 주위 샘플링 영향 반경
출력 : B = { z beacon1 , z beacon2 , .... , z beaconN }
왜 지능적인 샘플링을 하는가?
RRT와 RRT*는 경로를 찾기 위해서 무작위로 샘플링하는 방식이며, 이것은 아래의 단점이 존재한다.
1. 샘플링이 경로와는 상관없는 곳에 너무 많이 찍힐수 있다.
2. 최적 경로를 수렴하는데, 오랜 시간이 걸린다.
3. 탐색이 비효율적으로 분산된다.
==> 위와 같은 이유로 RRT*-smart는 '목표에 가깝거나' or '이미 탐색한 유망한 경로 근처'를 의도적으로 더 많이 샘플링 하도록 개선한것이다. 그리고 이를 'beacon기반 샘플링'전략이라고 한다.
1. 비콘 중심 편향 샘플링 ( Beacon - biased Sampling )
탐색 도중에 "좋은 노드(ex: 현재까지의 최적 경로 상의 노드)"를 "beacon"으로 지정한다. 그리고 이 beacons 주변을 집중적으로 샘플링한다.
작동 방식
1-1. beacon을 중심으로 반경 R beacon을 설정한다.
1-2. 전체 샘플링 중 일부는 "반경 R beacon"안에서만 무작위로 생성된다.
1-3. 나머지는 일반 RRT처럼 전체 공간안에서 무작위 샘플링을한다.
==> 즉, 두 가지 종류의 샘플링을 혼합해서 사용한다. ( beacon편향 샘플링(β)과 전역 샘플링(1-β) )
beacons은 시간이 지나면서 새롭게 업데이트 된다. ( 예를 들자면, 더 짧은 경로가 발견되면, 새로운 beacon후보가 생기며, 이전 beacon은 무시될 수 있다. )
2. 편향 비율인 β의 의미. ( 보통 0~1의 값을 가짐 )
β가 작다면?
탐색 공간 전체를 잘 커버하지만 수렴이 느리다.
β가 크다면
현재 좋은 경로 근처를 빨리 탐색한다. 대신에 지역 최적에 갇힐 위험이 크다.
==> 따라서 β는 "탐색성"과 "이용성"사이의 밸런스를 조절하는 hyperparameter이다.
import random
import math
import matplotlib.pyplot as plt
import time
class Map2D:
def __init__( self , width=100 , height=100 ):
self.width = width
self.height = height
self.obstacles = [ (20, 100, 30, 20) , (60,0,70,80,) , (40,40,50,50) , (80,80,90,90)]
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 RRTSmart:
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.best_cost = None
self.beacons = []
def distance( self , from_node , to_node ):
return math.hypot( to_node.x - from_node.x , to_node.y - from_node.y )
def get_random_node( 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 )
def intelligent_sampling( self ):
q_rand = random.choice( self.beacons )
theta = random.uniform( 0 , 2*math.pi )
r = self.step_size*2 # 우선 step_size의 2배로 설정
x = q_rand.x + r*math.cos( theta )
y = q_rand.y + r*math.sin( theta )
# 맵 경계 검사
x = max(0, min(x, self.map.width))
y = max(0, min(y, self.map.height))
return Node( x , y )
def nearest_node( self , tree , node ):
return min( tree , key=lambda nd: self.distance( nd , node ) )
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 )
# print(f'{x} , {y}')
if self.map.is_in_obstacle( x , y ):
return False
return True
def collision_free_for_beacons( 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 ):
self.beacons.append( Node(x,y) )
return False
return True
def steer( self , from_node , to_node ):
dist = self.distance( from_node , to_node )
if dist < self.step_size:
new_node = Node( to_node.x , to_node.y )
return new_node , dist
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 )
new_node = Node( new_x , new_y )
return new_node , dist
# q_rand에서 트리와 가장 가까운 q_nearest를 구하고, q_nearest에서 q_rand방향으로 일정 거리 이동한 지점에 q_new를 찍는다.
# 그렇게 얻은 q_new가 q_nearest보다 최적의 부모가 있는지 탐색
def get_near_neighbors( self , q_new ):
rst = []
for node in self.tree:
if self.distance( q_new , node ) <= self.near_neighbor_dist:
if self.collision_free( q_new , node ):
rst.append( node )
return rst
def rewire( self , new_node , near_nodes ):
for nd in near_nodes:
potential_cost = new_node.cost + self.distance( new_node , nd )
if potential_cost < nd.cost:
if nd.parent:
nd.parent.children.remove(nd)
nd.parent = new_node
new_node.children.append(nd)
nd.cost = potential_cost
# self.ax.plot([new_node.x, nd.x], [new_node.y, nd.y],color='purple', linewidth=3, alpha=0.3)
self._update_subtree_cost(nd)
def _update_subtree_cost( self , node ):
for c in node.children:
new_cost = node.cost + self.distance( node , c )
if new_cost < c.cost:
c.cost = new_cost
self._update_subtree_cost(c)
def build(self):
print('rrt smart알고리즘 시작\n우선 rrt star를 이용하기 초기 경로를 찾습니다.' )
start_time = time.time()
for i in range( self.max_iter ):
if (self.goal.cost != None ) and (i%10==0 ):# 우선 biasing ratio를 10%로 설정한다
# print(' intelligent sampling')
# biasing radius는 로봇의 이동단위(step_size)의 2~3배 정도를 기본값으로 설정함.
q_rand = self.intelligent_sampling()
else:
q_rand = self.get_random_node() # 랜덤하게 뽑고
q_nearest = self.nearest_node( self.tree , q_rand )# 가장 가까운 지점 찾고( q_new의 부모가 될 후보... )
q_new , dist_with_qNew_qNearest = self.steer( q_nearest , q_rand ) # 곧 추가시킬 q_new를 찾고,
if not self.collision_free( q_new , q_nearest ): # q_new와 q_nearest가 부딪히는지 확인
continue
# q_new를 기준으로 일정한 반경내에서, q_new의 부모가 될 후보들을 찾는다.
near_nodes = self.get_near_neighbors( q_new )
# 우선 현재는 q_nearest가 q_new의 부모가 되기에 최적이다.
best_parent = q_nearest
# q_new가 q_nearest보다 최적의 부모가 있는지 찾는다.
best_cost = q_nearest.cost + self.distance( q_nearest , q_new )
# q_new를 기쥰으로 self.near_neighbor_dist의 반경내의 부모후보들을 순회하며, 더 최적의 부모가 있는지 탐색
for nd in near_nodes:
cost_candiate = nd.cost + self.distance( nd , q_new )
if cost_candiate < best_cost:
best_parent = nd
best_cost = cost_candiate
# 최종적인 q_new의 부모
q_new.parent = best_parent
# 최종적인 q_new의 cost
q_new.cost = best_cost
# 최적의 부모의 children리스트에 q_new를 추가하고
best_parent.children.append(q_new)
# 트리에도 추가해준다.
self.tree.append(q_new)
# rewiring으로 q_new가 부모가 될때, q_new가 더 최적의 부모인지 확인
self.rewire( q_new , near_nodes )
self.ax.plot( [q_nearest.x , q_new.x] , [q_nearest.y , q_new.y] , '-g' , linewidth=2.5 , alpha=0.5)
if self.distance( q_new , self.goal ) < self.dist_threshold:
# 아직까지 최초 경로가 없는 경우
if self.goal.cost == None:
end_time = time.time()
print(f' 최초 경로를 찾았습니다.! 걸린 시간 : {end_time - start_time: .4f}초')
#self.goal.parent = self.tree[-1]
self.goal.parent = q_new
self.goal.cost = self.goal.parent.cost + self.distance( self.goal.parent , self.goal )
self.path_optimization()
path=self.reconstruct_path()
else:
new_cost = q_new.cost + self.distance( q_new , self.goal )
if new_cost < self.goal.cost:
new_end_time = time.time()
print(f' new path 찾음! 걸린 시간 : {new_end_time -start_time: .4f}초 , new_cost is {new_cost} , 기존 cost는 {self.goal.cost}')
#self.goal.parent = self.tree[-1]
self.goal.parent = q_new
# self.goal.cost = self.goal.parent.cost + self.distance( self.goal.parent , self.goal )
self.goal.cost = new_cost
self.beacons.clear()
self.path_optimization()
path=self.reconstruct_path()
print(f' {len(self.beacons)}\n')
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 path_optimization( self ):
node = self.goal
while node.parent and node.parent.parent:
parent = node.parent
grandfa = node.parent.parent
dist_cost = self.distance( grandfa , node )
# 이미 발견된 경로가 존재하는 경우
if self.collision_free_for_beacons( grandfa , node ):# 삼각부등식 기반으로 거리 최적화
if node in parent.children: # node와 parent의 연결을 없애버림
parent.children.remove( node )
node.parent = grandfa # 다음 반복을 위해서 node의 parent를 ancestor로 설정
grandfa.children.append( node ) # ancstor의 자식에 node추가
node.cost = grandfa.cost + dist_cost
if not parent.children:# node의 기존 parent에게서 자식들이 없다면, 지워줘야함
if parent in grandfa.children:
grandfa.children.remove( parent )
if parent in self.tree:
self.tree.remove(parent)
continue
self._update_subtree_cost(node)
node = node.parent
print(f' 삼각부등식 경로 최적화 후 경로의 길이 {self.goal.cost} ')
def visualize_path(self, path):
if path is None:
print("error")
return
# print(path[0][0])
# 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() # 필요하면 범례 활성화
print(self.best_cost)
print('\n')
plt.show()
if __name__ == "__main__":
# 맵 생성
my_map = Map2D()
# 시작점, 목표점
start_pos = (10, 90)
goal_pos = (90, 10)
# RRT-Connect 객체 생성
rrt_connect = RRTSmart(
map_2d=my_map,
start=start_pos,
goal=goal_pos,
step_size=0.3, # 확장 시 한 번에 이동할 거리
max_iter=20000, # 최대 반복 횟수
goal_sample_rate=0.1 # goal을 직접 샘플링할 확률(10%)
)
# 알고리즘 실행
final_path = rrt_connect.build()
# print(final_path)
# 결과 시각화
rrt_connect.visualize_path(final_path)
