샘플링 방식은 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() # 그래프 출력
