RRT*

 

rrt star.ipynb

0.22MB

RRT*

• RRT와 같은 확장 방식으로 공간을 탐색하면서도, 경로를 점진적으로 최적화하는 'rewiring'과정을 추가한다.
  ==> 이를 통해서 단순한 경로 탐색이 아니라 비용 최적화 경로 탐색이 가능해진다.
• 특징
  • 1. cost function을 적용 : 새로운 sampling point를 단순히 부모 노드에 연결하는 것이 아니라, 주변 노드중에서 최적의 부모를 선택
  • 2. Rewiring 과정 : 새로운 노드를 추가하고 주변 노드들의 부모를 다시 설정해서 비용이 더 낮아지는 경우에 트리를 재구성
  • 3. 최적성이 존재

 

 

RRT*의 동작원리

• 1. random sampling

 

• 2. 가장 가까운 노드 찾기 ( nearest neighbor )

 

• 3. 새로운 노드 확장 ( steer )

 

• 4. 새로운 노드 주변에서 근처 노드를 찾음 ( near neighbors )

 

• 5. 최적의 부모를 선택 ( choose best parent )

 

• 6. 트리 재구성 ( rewiring )

 

• 7. 목표 지점에 도달하였는지 검사

 

• 8. 위의 과정을 반복하면서 목표 지점에 도달
  
  
  
  

 

 

 

 

 

 

 

 

 

 

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 RRTStar:
    def __init__( self , map_2d , start , goal , step_size=0.5 , max_iter=100 , goal_sample_rate=0.05 , dist_threshold=0.3 , knn_dist=1 ,rewire_range=1):
        self.map = map_2d
        self.start , self.goal = Node( start[0] , start[1] , cost=0 ) , Node( goal[0] , goal[1] , cost=-1)
        self.step_size , self.max_iter , self.goal_sample_rate , self.dist_threshold = step_size , max_iter , goal_sample_rate , dist_threshold
        self.tree = [ self.start ]
        self.knn_dist = knn_dist
        self.rewire_range = rewire_range
        self.fig , self.ax = self.map.plot_map()

    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 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 nearest_node( self , tree , node):
        return min( tree , key=lambda nd: self.distance( nd , node ) )

    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  )
            new_node.parent = from_node
            print(f'   입력받은 from노드의 좌표 {from_node.x} , {from_node.y} 리턴할 노드의 좌표값 {new_node.x} , {new_node.y}')
            return new_node
        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 , parent=from_node )
            print(f'   --입력받은 from노드의 좌표 {from_node.x} , {from_node.y} 리턴할 노드의 좌표값 {new_node.x} , {new_node.y}')
            return new_node
            
    def knn(self, q_new, q_near):
        min_cost = q_near.cost + self.distance(q_near, q_new)
        q_best = q_near
        
        for node in self.tree:
            if self.distance(node, q_new) <= self.knn_dist:
                if self.collision_free(node, q_new):
                    total_cost = node.cost + self.distance(node, q_new)
                    if total_cost < min_cost:
                        min_cost = total_cost
                        q_best = node
        return q_best
        
    def rewire( self , q_new ):
        # 1. 우선 반경 내부의 노드들 찾기
  #      near_nodes = []
   #     for tree_node in self.tree:
    #        if (tree_node != q_new):
    #            if ( self.distance( tree_node , q_new ) <= self.rewire_range ):
     #               near_nodes.append(tree_node)
        near_nodes = [ node for node in self.tree if node != q_new and self.distance( node , q_new ) <= self.rewire_range ]              
        # 2. 반경 노드들의 부모 변경시 cost가 개선되면 부모를 변경 
        for check_node in near_nodes:
            dist = q_new.cost + self.distance( q_new , check_node )
            if dist < check_node.cost and self.collision_free( q_new , check_node ):
                # 부모를 q_new로 설정하여 cost를 갱신
                check_node.cost = dist
                check_node.parent = q_new
                print('rewire')
                self.update_children_cost( check_node )
                
    def update_children_cost( self , node ):
        for child in self.tree:
            if child.parent == node:
                child.cost = node.cost + self.distance( node , child )
                self.update_children_cost(child)
    
    def build(self):
        print('rrt star알고리즘 시작 ')
        start_time = time.time()
        for i in range( self.max_iter ):
            print(f'{i}번째 반복')
            q_rand = self.get_random_node() # 랜덤 샘플링
            q_nearest = self.nearest_node( self.tree , q_rand ) # 샘플링한 노드에서 가장 가까운 노드 찾고
            q_new = self.steer( q_nearest , q_rand ) # 샘플링 노드 방향으로 생성할 노드 생성
            if self.collision_free( q_nearest , q_new ): # 만약에 생성할 노드가 충돌하지 않는다면
                # 생성할 노드중에서 일정 반경 내에서 best parent를 찾는다.
                q_best_parent = self.knn(  q_new , q_nearest )
                if q_best_parent != q_nearest:
                    q_new.parent = q_best_parent
                    q_new.cost = ( q_new.parent.cost + self.distance( q_new , q_new.parent ) )
                else:
                    q_new.cost = ( q_new.parent.cost + self.distance( q_new , q_new.parent ) )
                self.tree.append( q_new )
                self.rewire( q_new )
                self.ax.plot(  [q_nearest.x , q_new.x] , [q_nearest.y , q_new.y] , '-g' , linewidth=3 , alpha=0.5)
                if self.distance( q_new , self.goal ) < self.dist_threshold:
                    end_time = time.time()
                    print(f'경로 찾음! 걸린 시간 : {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()
                    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)} , {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()