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()
