알고리즘

PRM ( probabilistic Roadmap )

정지홍 2025. 2. 6. 13:33

코드 1
코드 2
코드 3

 

PRM ( probabilistic Roadmap )이란?

  • 로봇 경로 계획에서 사용되는 샘플 기반 접근법이다. 특히 고차원 공간에서 효율적인 탐색을 위해 설계되었다.
  • 탐색 공간의 구조를 사전에 학습한 뒤 이를 활용해서 경로를 찾는 방식으로 작동한다.
  • 기본적인 개념
    • Learning phase와 query phase의 2개의 단계로 나뉜다.
      • 학습단계 Learning phase
        • 로봇의 탐색 공간 내에서 무작위로 샘플링 한 후, 이 샘플들을 그래프( roadmap) 형태로 연결해서 환경의 구조를 모델링함.
      • 쿼리 단계 query phase
        • 시작지점과 목표지점을 roadmap에 연결한후, 그래프 탐색알고리즘으로 최적의 경로를 찾음.
    • 위와 같은 구조는 로봇이 같은 환경안에서 여러번 경로를 탐색해야 할때 효율적이다. 왜냐면 학습 단계에서 만들어진 roadmap은 재사용이 가능하기 때문.
  • 장점
    • 고차원 공간에서 효율적.
    • 환경이 고정되어 있다면 여러번 재사용이 가능하며 병렬화가 용이
  • 단점
    • 환경이 동적이면 성능 저하
    • 복잡한 지형에서는 샘플링이 부족해 경로가 불연속적일 수 있음
    • narrow passage problem에 취약
    • 충분히 많은 샘플을 사용해야만 최적의 경로를 찾을 확률이 1에 수렴함.
      즉, 항상 최적해를 보장하지 않는다.
  • 변형 알고리즘
    • prm* 
    • lazy prm

 

 

PRM ( probabilistic Roadmap )의 수학적 모델

  • 1. 탐색 공간 Configuration space
    • PRM에서 로봇이 움직이는 공간을 아래와 같이 정의한다.
      • 탐색 공간 : 로봇이 이동할 수 있는 전체 공간
      • 충돌 공간 : 장애물로 인해 로봇이 들어갈 수 없는 공간
      • 자유 공간 : 로봇이 이동할 수 있는 공간 

PRM은 C free에서 무작위 샘플링을 수행해서 그래프를 구성한다.

 

  • 2. 확률적 샘플링 random sampling
    • PRM은 확률적으로 자유 공간에서 점을 생성한다. ( 코드 1 참고 ) 
      • q = ( x , y )는 무작위로 생성된 샘플 좌표
      • Uniform( C free )는 자유 공간에서 균등 분포를 따르는 확률 분포

즉, 자유공간에서 점을 랜덤하게 뽑는것이다.

 

  • 3. 노드와 그래프 모델링
    • PRM은 샘플링한 점들로 그래프 G = ( V , E )를 만든다. ( 코드 2의 라인4 참고 ) 
      • 노드( Vertices , V ) : 샘플링 된 점들
      • 간선( Edges , E ) : 충돌이 없는 점들끼리 연결하여 생성

collision_free함수는 두 점을 이어주는 edge가 장애물과 충돌하지 않는지 검사하는 함수이다.

 

 

  • 4. k-nearest neighbors ( 코드 2의 라인 5~8 참고 )
    • 각 샘플링된 노드에 대해서 가장 가까운 k개의 노드를 찾고 연결하는 과정이 필요하다.
      즉, 가장 가까운 k개의 노드를 찾고 연결하는 과정이다.
      • 이때 가장 가까운 노드를 찾는 과정은 유클리드 거리로 정의한다.
        • 식1을 좀더 풀어보자면...
          1. 샴풀랑 된 점들의 집합에서 q를 제회한 모든 점에 대해서 유클리드 거리를 구함.
          2. 거리순으로 정렬 ( 아마도 오름차순...? )
          3. 가장 가까운 k개를 선택한다.

유클리드 거리
샘플링 된 점 q에 대해서 가장 가까운 k개의 이웃을 찾고 연결하는 식 ( 식1)
인자들 설명

 

 

  • 5. 충돌 검출 collision checking ( 코드 2의 라인 7 참고 및 코드 3 참고 ) 
    • 두 점을 연결할때 충돌 여부를 검사해야한다.
      • 검사하는 방법은 아래의 식을 따른다.
        • 연속적인 작은 스텝으로 점을 생성하면서 장애물과의 충돌 여부를 검사 
          • qt는 q i 에서 q j 로 선형보간 ( linear interpolation )을 한 점이다.
          • t 간격으로 증가시키면서 중간 점들이 장애물 안에 존재하는지 검사 

q t 가 장애물과 충돌하는지 검사. 이러한 방법으로 모든 중간점이 장애물과 출돌 하지않으면 간선을 추가함.

 

 

 

시간 복잡도

 

 

 

 

PRM ( probabilistic Roadmap )의 알고리즘 단계별 과정

  • 1-1. 학습단계-샘플링  learning_phase-sampling
    • 자유공간에서 무작위로 N개의 샘플 노드를 생성한다.
    • 샘플링 방식은 uniform random , gaussian , bridge test 등이 존재한다.
      • 충돌 검사를 통해서 장애물 내부에 위치한 샘플은 폐기한다.
  • 1-2. 노드 연결 
    • 각 샘플을 인접한 이웃 노드들과 연결한다. 이웃은 주로 knn or r-nearst를 사용한다.
      • 연결된 경로가 충돌이 없는지 검사도 해야함
  • 1-3. 로드맵 생성 roadmap creation
  • 2. 쿼리 단계
    • 2-1. 시작 지점과 목표지점을 샘플링 된 로드맵에 연결한다.
    • 2-2. 그래프 탐색으로 최적 경로 찾는다.
Pseudo code

def sample_Free( space , num_samples ):
	samples = []
    while len( samples ) < num_samples:
    	q = random_point( space )
        if not is_in_obstacle( q ):
        	samples.append( q )
    return samples
    
def connect_nodes( samples , k=5 )
	G = nx.Graph()
    for q in samples:
    	G.add_node( q )
        neighbors = find_k_nearest( q , samples , k )
        for neighbor in neighbors:
        	if collision_free( q , neighbor ):
            G.add_edge( q , neighbor , weight=distance( q , neighbor ) )
    return G
 
 
 
 
samples = sample_free(space, 1000)
roadmap = connect_nodes(samples)
path = nx.shortest_path(roadmap, source=start, target=goal, weight='weight')

 

 

import random
import math
import networkx as nx
import matplotlib.pyplot as plt

# 자유공간에서 점을 생성한다.
def random_sampling( space ):
    x_min , x_max , y_min , y_max = space # 탐색 공간의 경계정의
    return ( random.uniform(x_min,x_max) , random.uniform(y_min,y_max) )
    
    
    
 # 두 점 간의 거리를 계산
def distance( p1 , p2 ):
    return math.hypot( p1[0] - p2[0] , p1[1] - p2[1] )
    
    
# 충돌하는지 검사하는 함수.
def collision( point , obstacles ):
    x , y = point # 점의 좌표를 분리시킴
    for ( ox , oy , radius ) in obstacles: # 각 장애물의 중심과 반지름을 가져옴
        if math.hypot( ox - x , oy - y ) <= radius: # 점이 장애물 내부에 존재시 True를 반환. 즉, 충돌하는 경우 
            return True
    return False # 충돌이 없는경우.
    
    
def collision_checking( p1 , p2 , obstacles , step_size=0.1 ):
    dist = distance( p1 , p2 )
    if dist == 0:
        return True
    steps = max( 1 , int( dist / step_size ) )
    for i in range( steps + 1 ):
        x = p1[0] + (p2[0] - p1[0]) * i / steps
        y = p1[1] + (p2[1] - p1[1]) * i / steps
        if collision( (x,y) , obstacles ):
            return False
    return True
    
    
    
def configuration_space_free( space , obstacles , num_samples ):
    free_nodes = [] # 충돌하지 않는 노드를 저장할 리스트
    while len( free_nodes ) < num_samples:
        q = random_sampling(space) # 랜덤 생성
        if not collision( q , obstacles): # 충돌하지 않으면 추가.
            free_nodes.append(q)
    return free_nodes
    
    
   
 def knn( point , samples , k ):
    samples = sorted( samples , key=lambda s: distance( point , s ) )
    return samples[ 1 : k+1 ] # 자신을 제외한 제일 근접한 k개의 노드만 반환
    
    
    
def connect_nodes( samples , obstacles , k=5 ):
    G = nx.Graph()
    for q in samples:
        G.add_node( q )
        neighbors = knn( q , samples , k )
        for neighbor in neighbors:
            if collision_checking( q , neighbor , obstacles ):
                G.add_edge( q , neighbor , weight=distance( q , neighbor ) )
    return G
# 시각화 함수
def plot_prm(G, start, goal, obstacles):
    plt.figure(figsize=(10, 10))  # 그래프 크기 설정

    # 장애물 그리기
    for (ox, oy, r) in obstacles:
        circle = plt.Circle((ox, oy), r, color='gray')  # 원형 장애물 생성
        plt.gca().add_patch(circle)  # 그래프에 추가

    # 노드 및 간선 그리기
    for (u, v) in G.edges():
        plt.plot([u[0], v[0]], [u[1], v[1]], 'b-', linewidth=0.5)  # 간선 표시
    for node in G.nodes():
        plt.plot(node[0], node[1], 'ro', markersize=3)  # 노드 표시

    # 시작점과 목표점 표시
    plt.plot(start[0], start[1], 'go', markersize=10, label='Start')  # 시작점 (녹색)
    plt.plot(goal[0], goal[1], 'mo', markersize=10, label='Goal')  # 목표점 (자홍색)
    plt.legend()  # 범례 추가
    plt.grid()  # 격자 표시
    plt.show()  # 그래프 출력
space = (0, 100, 0, 100)  
obstacles = [(30, 30, 10), (60, 60, 15), (70, 20, 8)]  

start = ( 5 , 5 )
goal = (95, 95)  

samples = configuration_space_free(space, obstacles, 500)  # 자유 공간에서 500개의 샘플 생성
samples += [start, goal]  # 시작점과 목표점도 샘플에 추가

roadmap = connect_nodes(samples, obstacles, k=10)

try:
    # 시작점과 목표점 간 최단 경로 탐색
    path = nx.shortest_path(roadmap, source=start, target=goal, weight='weight')
except nx.NetworkXNoPath:
    path = []  # 경로가 없을 경우 빈 리스트 반환

# 결과 시각화
plot_prm(roadmap, start, goal, obstacles)  # 로드맵 시각화

# 경로 표시 (옵션)
if path:
    plt.figure(figsize=(10, 10))  # 그래프 크기 설정
    for (u, v) in roadmap.edges():
        plt.plot([u[0], v[0]], [u[1], v[1]], 'b-', linewidth=0.5)  # 로드맵의 간선 표시
    for node in roadmap.nodes():
        plt.plot(node[0], node[1], 'ro', markersize=3)  # 노드 표시
    plt.plot(start[0], start[1], 'go', markersize=10)  # 시작점 표시
    plt.plot(goal[0], goal[1], 'mo', markersize=10)  # 목표점 표시

    # 최단 경로 표시
    path_x, path_y = zip(*path)
    plt.plot(path_x, path_y, 'g-', linewidth=2, label='Path')  # 최단 경로를 초록색 선으로 표시

    plt.legend()  # 범례 표시
    plt.grid()  # 격자 표시
    plt.show()  # 그래프 출력