RRT (Rapidly-Exploring Random Tree)

RRT (Rapidly-Exploring Random Tree)란?

• 고차원 상태 공간에서 빠르게 탐색할 수 있도록 설계된 샘플 기반 탐색 알고리즘.
  • 특히 비선형 시스템의 경로 계획을 위해서 개발됨
• 핵심 아이디어
  • 무작위 샘플링으로 빠르게 넓은 공간을 커버하고, 이를 기반으로 트리를 확장해서 경로를 찾는 방식 
• 이론적 성질
  • 1. 완전성 probabilistic completeness
    • rrt는 확률적 완전성을 가진다. 이는 탐색 가능한 경로가 존재할 경우, 무한히 많은 샘플을 생성하면 결국에는 그 경로를 찾게 된다는 것을 의미
  • 2. 최적성
    • rrt는 최적 경로를 보장하지 않는다. 이를 보완하기 위해서 등장한 것이 RRT* 이다.
• 장점
  • 트리의 샘플링이 진행됨에 따라서 탐색 공간을 균등하게 커버한다.
    • n개의 노드가 존재시, 탐색 속도는 O( n log n )에 근접한다.
• 단점
  • 탐색 공간이 커질수록 샘플수가 기하급수적으로 증가한다. 그래서 이럴때는 guided sampling을 사용.

 

---

 

RRT (Rapidly-Exploring Random Tree)의 수학적 정의

• 상태 공간 state space
  • 로봇의 모든 가능한 상태를 나타내는 공간이다.
    • 즉, 자유공간안에서 시작점에서 목표까지 연속적으로 연결하는 경로를 찾는 문제

공간에 대한 표현

• 경로는 연속함수이며 아래와 같이 나타낸다.
  • τ(0)은 시작점이며 τ(1)은 목표점이다.

 

 

 

---

 

 

RRT (Rapidly-Exploring Random Tree)의 작동 원리

• RRT는 트리를 점진적으로 확장하여 목표 지점에 도달하는 구조이다.
• 1. 랜덤 샘플링 random sampling ( 그림 1 참고 )
  • 탐색 공간 C에서 무작위 샘플 q rand를 생성한다. 
    이는 탐색 공간을 고르게 커버하기 위해서 중요하다. 

샘플링 함수

• 2. 최근접 노드 탐색 nearest neighbor search ( 그림 2 참고 )
  • 기존 트리의 노드중 q rand에 가장 가까운 노드인 q near를 찾는다.
    • 보통은 유클리드 거리를 탐색한다.

유클리드 거리 공식

최근접 노드 탐색

• 3. 트리 확장 tree expansion ( 그림 3 참고 )
  • 최근접 노드 q near에서 q rand방향으로 일정한 거리만큼 이동하여, 새로운 노드 q new를 생성함.

방향 벡터 계산

새로운 노드 생성

• 4. q near와 q new를 연결하는 선분이 C obs와 충동하는지 확인. 충돌이 없다면 트리에 추가함.
  • 장애물과 노드간의 충돌 여부를 확인가기 위해서 유클리드 거리의 제곱을사용한다.
    • 이는 점과 원의 중심간의 거리가 원의 radius보다 작거나 같은 경우를 의미하며, 이경우에는 노드가 원 안에 존재함을 의미.

유클리드 거리의 제곱

• 5. 목표 근접 확인
  • q new가 목표지점인 q goal에 충분히 가까워지면 탐색을 종료함.

 

 

 

 

 

그림1

• 왜 일정확률로 목표지점을 샘플링하나??
  • RRT는 무작위 샘플링 기반으로 트리를 확장하는데, 목표 지점 근처로 확장하지 않으면 경로를 찾는데 오랜 시간이 걸릴수있다. 그래서 목표지점을 샘플링하면 트리가 목표지점 방향으로 성장해서 빠르게 수렴할 가능성이 높아진다.
    • 무작위 샘플링만 한다면 탐색이 지나치게 불규칙 해지며, 목표로 가는 효율적인 경로를 찾기가 어려울수있다.
      반대로 목표지점만 샘플링하면 장애물 회피가 어려워질수있다.
      ==> 그래서 탐색과 수렴 사이의 균형을 맞춰야함 
      • 확률 높을수록 속도는 빠르나 장애물 회피 부족
      • 확률 낮을수록 속도는 느리나 탐색을 잘할수있다.

그림1

그림1

 

 

 

 

그림2

그림2

그림2

 

 

 

 

그림3

그림3

 

 

 

 

import math
import random
import matplotlib.pyplot as plt

# 노드 클래스
class Node:
    def __init__( self , x , y ):
        self.x = x 
        self.y = y
        self.parent = None

class RRT:
    def __init__( self , start , goal , obstacle_list , rand_area , max_iter=500 , step_size=5.0 , goal_sample_rate=0.05 , goal_radius=5.0 ):
        self.start = Node( start[0] , start[1] ) # 시작 노드
        self.goal = Node( goal[0] , goal[1] ) # 목표 노드
        self.obstacle_list = obstacle_list # 장애물 목록 ( x , y , radius )
        self.rand_area = rand_area # 샘플링이 가능한 범위 ( min , max )
        self.max_iter = max_iter # 최대 반복 횟수
        self.step_size = step_size # 한번에 확장하는 거리
        self.goal_sample_rate = goal_sample_rate # 목표지점을 샘플링할 확률 
        self.goal_radius = goal_radius # 목표 반경 (목표 도달 조건을 완화)
        self.node_list = [ self.start ] # 트리의 초기 노드 리스트 ( 시작 노드 포함 )

    # 탐색 공간에서 무작위 샘플 q를 생성한다.
    def get_random_node( self ):
        rand_x = random.uniform( self.rand_area[0] , self.rand_area[1] ) # 맵 사이즈 범위 내에서 랜덤한 숫자 발생 
        rand_y = random.uniform( self.rand_area[0] , self.rand_area[1] )
        return Node( rand_x , rand_y )

    # 기존 노드 중에서 가장 가까운 노드 찾기 
    def get_nearest_node_index( self , rand_node ):
        distances = [ self.calc_distance( node , rand_node ) for node in self.node_list ]
        return distances.index( min( distances ) )

    # 거리 계산. 유클리드 거리를 이용 
    def calc_distance( self , node1 ,node2 ):
        return math.hypot( node1.x - node2.x , node1.y - node2.y )

    # 최근접 노드에서 랜덤 노드 방향으로 새로운 노드 생성
    def steer( self , near_node , rand_node ):
        theta = math.atan2( rand_node.y - near_node.y , rand_node.x - near_node.x )
        new_node = Node( 
            near_node.x + self.step_size * math.cos( theta ) ,
            near_node.y + self.step_size * math.sin( theta )
            )
        new_node.parent = near_node
        return new_node

    # 충돌 검사 함수 : 장애물과의 충돌 여부 확인 
    def is_collision_free( self , node ):
        for ( ox , oy , size ) in self.obstacle_list:
            # 유클리드 거리의 제곱을 계산하는 방정식이다.
            if ( ox - node.x)**2 + (oy - node.y )**2 <= size**2:
                return False # 충돌 있음
        return True # 충돌 없음

    def generate_final_path( self , goal_node ):
        path = [ (goal_node.x , goal_node.y ) ]
        node = goal_node
        while node.parent is not None:
            node = node.parent
            path.append( (node.x , node.y) )
        return path[ : : -1 ] # 경로를 역순으로 반환

    def planning( self ):
        for i in range( self.max_iter ):
            # 1. 랜덤 샘플링.  탐색공간C에서 무작위 샘플 q rand 를 생성한다.
            if random.random() < self.goal_sample_rate:
                q_rand = self.goal # 목표 지점으로 샘플링
            else:
                q_rand = self.get_random_node()
                
            # 2. 최근접 노드 탐색. 기존 트리의 노드중 q rand에 가장 가까운 노드인 q near을 찾는다.
            q_near_index = self.get_nearest_node_index( q_rand )
            q_near = self.node_list[ q_near_index ]

            # 3. 트리 확장. 최근접 노드 q near에서 q rand 방향으로 일정한 거리만큼 이동해서 새로운 q new를 생성함.
            q_new = self.steer( q_near , q_rand )

            # 4. 충돌하는지 확인. 충돌이 없다면 트리에 추가.
            if self.is_collision_free( q_new ):
                self.node_list.append( q_new )

            # 5. q new가 목표지점인 q goal에 가까워지면 탐색을 종료
            if self.calc_distance( q_new , self.goal ) <= self.goal_radius:
                return self.generate_final_path( q_new )
        return None

# 시각화 함수 정의
def draw_rrt(rrt, path=None):
    
    plt.figure(figsize=(10, 10))
    plt.grid(True)
    
    # 장애물 그리기 (빨간 원)
    for (ox, oy, size) in rrt.obstacle_list:
        circle = plt.Circle((ox, oy), size, color='r')
        plt.gca().add_patch(circle)

    # RRT 트리 시각화 (초록색 선)
    for node in rrt.node_list:
        if node.parent:
            plt.plot([node.x, node.parent.x], [node.y, node.parent.y], '-g')

    # 최종 경로 시각화 (파란색 선)
    if path:
        path_x, path_y = zip(*path)
        plt.plot(path_x, path_y, '-b', linewidth=2)

    # 시작점과 목표점 표시
    plt.plot(rrt.start.x, rrt.start.y, "ro", markersize=8, label='Start')
    plt.plot(rrt.goal.x, rrt.goal.y, "bo", markersize=8, label='Goal')
    plt.legend()
    plt.show()
    
    
    if __name__ == '__main__':
    start = (0, 0)
    goal = (90, 90)
    obstacle_list = [(30, 30, 10), (60, 60, 10), (70, 20, 8)]  # 장애물 (x, y, radius)
    rand_area = (0, 100)  # 샘플링 가능한 범위

    # RRT 알고리즘 실행
    rrt = RRT(start, goal, obstacle_list, rand_area)
    path = rrt.planning()

    # 결과 시각화
    draw_rrt(rrt, path)

rrt.ipynb

0.25MB

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

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 ):
        self.x , self.y , self.parent = x , y, parent
        
        
        
 class RRT:
    def __init__( self , map_2d , start , goal , step_size=0.5 , max_iter=100 , goal_sample_rate=0.05 , dist_threshold=0.3):
        self.map = map_2d
        self.start , self.goal = Node( start[0] , start[1] ) , Node( goal[0] , goal[1] )
        self.step_size = step_size
        self.max_iter = max_iter
        self.goal_sample_rate = goal_sample_rate
        self.dist_threshold = dist_threshold
        self.tree = [ self.start ]
        self.fig , self.ax = self.map.plot_map()
        print(self.tree)
    
    def distance( self , node_from , node_to ):
        return math.hypot( node_to.x - node_from.x , node_to.y - node_from.y )

    def get_random_node( self ):
        x = random.uniform( 0 , self.map.width )
        y = random.uniform( 0 , 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 , node_from , node_to ):
        steps = max( int( self.distance( node_from , node_to ) / (self.step_size * 0.5 ) ) , 1)
    #    print(f'{steps}')
        for i in range( steps + 1 ):
            t = i/steps
            x = node_from.x + t*( node_to.x - node_from.x )
            y = node_from.y + t*( node_to.y - node_from.y )
            if self.map.is_in_obstacle( x , y ):
                return False
        return True

    def steer( self , node_from , node_to ):
        dist = self.distance( node_from , node_to )
        if dist < self.step_size:
            new_node = Node( node_to.x , node_to.y )
            new_node.parent = node_from
            return new_node
        else:
            theta = math.atan2( node_to.y - node_from.y , node_to.x - node_from.x )
            new_x = node_from.x + self.step_size*math.cos(theta)
            new_y = node_from.y + self.step_size*math.sin(theta)
            new_node = Node( new_x , new_y , parent=node_from )
            return new_node

    def extend( self , tree , q_rand ):
        q_near = self.nearest_node( tree , q_rand )
        q_new = self.steer( q_near , q_rand )
        if self.collision_free( q_near , q_new ):
            tree.append( q_new )
            self.ax.plot( [q_near.x , q_new.x] , [q_near.y , q_new.y] , '-g' , linewidth=3 , alpha=0.5)
            return q_new
        return None
    
    def build( self ):
        print("build 시작")
        start_time = time.time()
        for i in range( self.max_iter ):
            q_rand = self.get_random_node()
           #print(q_rand)
            q_new = self.extend( self.tree , q_rand )
            if q_new is not None:
             #   print(f"{q_new.parent}")
                dist_for_goal = self.distance( q_new , self.goal )
                if dist_for_goal < self.dist_threshold:
                    end_time = time.time()
                    print(f"경로 찾음! 걸린 시간: {end_time - start_time:.4f}초")
                    self.goal.parent = self.tree[-1]
                    path = self.reconstruct_path()
                    print(f'====={i}번 반복 및 트리의 길이 {len(self.tree)} ')
                    return path
        return None

    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)}')
        return path
        
    def visualize_path( self , path ):
        if path is None:
            print("error")
            return
        # 경로가 있으면 경로 시각화
        px, py = zip(*path)                           # (x, y) 리스트에서 x, y 별도 튜플로 분리
        print()
      #  print(f'  ({px}\n\n\n{py})  ')
        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, 1)
    goal_pos = (9, 1)

    # RRT-Connect 객체 생성
    rrt_connect = RRT(
        map_2d=my_map,
        start=start_pos,
        goal=goal_pos,
        step_size=0.5,       # 확장 시 한 번에 이동할 거리
        max_iter=20000,       # 최대 반복 횟수
        goal_sample_rate=0.1 # goal을 직접 샘플링할 확률(10%)
    )

    # 알고리즘 실행
    final_path = rrt_connect.build()
#    print(final_path)
    # 결과 시각화
    rrt_connect.visualize_path(final_path)

 

 

 

1. 동향

• RRT가 해결 해야할 한계는 '경로의 비최적성'과 '느린 수렴속도'가 존재한다.
  • 그래서 최근에는 경로최적화 , 수렴 속도 개선 , 동적환경 대응이 핵심 목표다.
  • 최적화를 위한 것들
    • RRT*
  • 탐색 효율 증대를 위한 것들
    • 목표 편향 기법
    • 중요도 샘플링
  • 하이브리드
    • gray wolf optimizer
    • a*
    • apf
    • 강화학습 or 딥러닝으로 샘플링 전략을 학습
  • Batch Informed Trees
    
    RRT-Smart
    
    Quick-RRT*
    
    RRT-X