PRM star

알고리즘

PRM star

정지홍 2025. 2. 18. 14:26

KDTree( K-Dimensional Tree )

다차원 공간에서 가까운 이웃을 빠르게 찾을 수 있는 공간 분할 자료 구조이다.
- 공간 분할 자료: 공간을 여러개의 작은 부분으로 나누어서 데이터를 효율적으로 저장하고 검색하는 구조.
기본 개념
- KDTree는 이진트리 형태로 공간을 분할하는 방식이다.
  - 1. x축 or y축을 기준으로 데이터를 반씩 나누어서 정렬
  - 2. 트리를 구성하면서 빠르게 검색이 가능하도록 구조화
  - 3. 새로운 노드가 들어오면, 트리를 따라가면서 최근접 이웃 탐색.
    - ==>즉, KDTree는 특정 위치에서 가까운 점을 빠르게 찾도록 공간을 정리하는 방법

PRM star

PRM*는 PRM ( probabilistic Roadmap )의 항상된 버전이며, 최적성 보장을 강화한 sampling-based path planning이다.
- PRM과 마찬가지로 비정형 환경에서도 다자유도 로봇의 경로 계획을 수행할 수 있으며, 최적 경로 보장 성질을 가짐.
PRM과의 차이점
- 1. 연결 반경 조정
  - PRM에서는 sample을 생성후, 미리 정해진 고정된 반경 or 고정된 갯수의 이웃 도느만을 연결한다.
    하지만, PRM * 에서는 연결 반경을 환경 크기 및 노드 갯수에 따라서 동적으로 조정하며, 경로가 길수록 더 최적화될 수 있게 설계함.
- 2. 최적성 보장
  - PRM*는 sample수가 증가할수록, 경로가 점점 최적에 가까워지게 설계된다.
  - sample의 갯수가 무한대로 증가할 경우, PRM*는 전역 최적 경료를 수렴하게 된다.
    - 위의 속성이 PRM*가 기존의 PRM보다 좋은 성능을 내는 이유다.
장점
- sample수가 증가할수록 최적 경로를 찾을 가능성이 높아짐
- PRM보다 불필요한 edge생성을 줄임 -> 성능 향상
- 2d , 3d 등 다양한 환경에서 가능
단점
- 연결 반경을 조정하는 과정에서 추가적인 연산이 필요하여 실행 시간이 길어질 수 있음.
- 샘플링 품질에 따라서 성능 차이 발생 ( sampling이 적거나, 편향되면 최적 경로 보장이 어려움 )
- 정적인 환경에서만 사용 가능

**PRM*의 과정**

1. sampling
- 환경 내에서 랜덤한 nodes를 sampling해서 후보 경로를 만든다.
  ( 이는 로봇이 이동할 수 있는 공간에서 무작위로 sample node를 만드는 것이다. )
  - a. 무작위 sample을 generate하려면, map_size 내에서 무작위 x,y좌표를 선택한다.
    b. 장애물과 너무 가까운 sample은 제거해야함.
    c. 보통은 균일한 확률로 sample을 generate하지만, 필요에 따라서 weight를 부여할 수 있음.

2. Roadmap 생성
- sample node들을 연결하여 roadmap을 만든다. ( PRM*의 핵심은 동적으로 연결 반경을 설정하는 것 )
  - a. 기존 PRM에서는 '고정된 반경'을 사용해서 이웃 노드를 찾았다.( KDTree 사용)
    하지만 PRM*에서는 sample수에 따라서 반경을 조정하는 것이 가능하다.

3. collision checking
- 2개의 node간에 장애물이 없는 경우에만 연결한다.

4. 시작점과 목표점을 연결
- sampling한 node들 중에서 시작점과 목표점과 가장 가까운 node를 찾아서 연결해야 한다.
5. 경로 탐색
- ( a* or 다익스트라 같은 알고리즘 사용 )

import numpy as np
import matplotlib.pyplot as plt
from scipy.spatial import KDTree
import networkx as nx  
import math

class PRMStar:
    def __init__( self , start , goal , map_size , num_samples , obstacles ):
        self.start = start
        self.goal = goal
        self.map_size = map_size
        self.num_samples = num_samples
        self.obstacles = obstacles
        self.nodes = [ start , goal ]
        self.edges = []
        self.graph = nx.Graph()

    def sample_nodes( self ):
        while len(self.nodes) < self.num_samples:
            x , y = np.random.uniform( 0 , self.map_size , 2 )
            if not self.is_in_obstacle( (x,y) ):
                self.nodes.append( (x,y) )
                
    def is_in_obstacle( self , node ):
        for ox , oy , r in self.obstacles:
            if np.linalg.norm( np.array(node) - np.array( (ox,oy) ) ) < r:
                return True
        return False

    def connect_nodes( self ):
        for node in self.nodes:
            neighbors = self.find_neighbors( node )
            for neighbor in neighbors:
                if self.is_collision_free( node , neighbor ):
                    self.edges.append( ( node , neighbor ) )
                    distance = np.linalg.norm( np.array(node) - np.array(neighbor) )
                    self.graph.add_edge( node , neighbor , weight = distance )

    def find_neighbors( self , node ):
        n = len( self.nodes ) # 현재 샘플된  노드 개수
        d = 2 # 차원 수. 현재는 2차원으로  설정
        eta = 2.0 # 조정 상수이며 조정이 가능하다.

#        k = int( eta * ( math.log(n)/n ) ** ( 1/d) )
        k = max(2, int(math.e * (1 + d / 2) * math.log(n)))
#        r = self.gamma * (math.log(n) / n)**(1/d)
        k = max( 2, k ) # 최소 2개 이상의 이웃을 보장

        tree = KDTree( self.nodes ) # KDTree를 생성
        # KDTree.query는 node에서 가장 가까운 k개의 이웃 노드를 찾는 것. k는 PRM*에서 설정한 이웃 갯수.
        # 반환값... distances는 node와 k개의 이웃 노드간의 거리 리스트. indices는 self.nodes에서 가장 가까운 k개의 노드 인덱스 리스트 
        distances , indices = tree.query( node , k=min( k , n ) ) # k개의 최근접 노드를 찾습니다. 
        return [ self.nodes[i] for i in indices if i != self.nodes.index(node) ] # 자기 자신 제외한 이웃 반환 

    # 두 노드를 연결하는 선이 장애물과 충동하는지 검사하는 함수 
    # 1. 각 장ㅇ애물의 중심과 두 노드의 거리를 확인해서, 노드가 장애물 내부에 있는지 확인
    # 2. 두 노드를 잇는 선분과 장애물 간 최단 거리를 계산
    # 3. 최단 거리가 장애물 반지름보다 작다면 충돌 발생 
    def is_collision_free( self , node1 , node2 ):
        for ox , oy , r in self.obstacles:
            v1 , v2 , center = np.array( node1 ) , np.array( node2 ) , np.array( (ox,oy) ) # 첫번째 노드 좌표 , 두번째 노드 좌표 , 장애물의 중심 좌표 
            if ( np.linalg.norm( v1-center) < r ) or ( np.linalg.norm( v2 - center ) < r ): # 노드가 장애물 내부에 존재하는지 검사 
                return False
            line_vec , center_vec = v2-v1 , center-v1 # 선분 v1-v2와 장애물 중심 사이의 최단 거리 계산. 노드1에서 노드2로 가는 벡터 , 장애물 중심에서 노드1로가는 벡터
            # 선분 v1-v2를 따라서 중심점이 어느 위치에 투영되는지 계산
            proj = np.dot( center_vec , line_vec ) / np.dot( line_vec , line_vec ) 
            # 투영된 점이 선분 내에 있도록 조정
            proj = np.clip( proj , 0 , 1 )
            closest_point = v1 + proj * line_vec # 선분 위에서 장애물 중심과 가장 가까운 점 계산
            if np.linalg.norm( closest_point - center ) < r : # 가장 가까운점과 장애물 중심 사이의 거리 검사
                return False # 충돌 발생...
        return True

    def find_shortest_path( self ):
        try:
            return nx.astar_path( self.graph , self.start , self.goal , weight='weight' )
        except nx.NetworkXNoPath:
            return []

    def run(self):
        self.sample_nodes()
        self.connect_nodes()
        return self.find_shortest_path()

    def visualize(self, path=[]):
        fig, ax = plt.subplots(figsize=(8, 8))
        ax.set_xlim(0, self.map_size)
        ax.set_ylim(0, self.map_size)
        
        # 장애물 그리기
        for ox, oy, r in self.obstacles:
            circle = plt.Circle((ox, oy), r, color='red', alpha=0.5)
            ax.add_patch(circle)
        
        # 노드 그리기
        for node in self.nodes:
            ax.plot(node[0], node[1], 'bo', markersize=3)
        
        # 엣지 그리기
        for edge in self.edges:
            n1, n2 = edge
            ax.plot([n1[0], n2[0]], [n1[1], n2[1]], 'gray', linewidth=0.5)
        
        # 최단 경로 그리기
        if path:
            path_x, path_y = zip(*path)
            ax.plot(path_x, path_y, 'g-', linewidth=2, label='Shortest Path')
        
        # 시작과 목표 위치 강조
        ax.plot(self.start[0], self.start[1], 'go', markersize=8, label='Start')
        ax.plot(self.goal[0], self.goal[1], 'ro', markersize=8, label='Goal')
        
        ax.legend()
        plt.show()

obstacles = [(30, 30, 10), (70, 70, 15), (50, 50, 10), (15, 10, 5), (10, 15, 2)]
env = PRMStar(start=(5, 5), goal=(95, 95), map_size=100, num_samples=200, obstacles=obstacles)
shortest_path = env.run()
env.visualize(shortest_path)