728x90
반응형
경로 계획 알고리즘은 로봇공학과 자율 주행 시스템에서 핵심적인 역할을 수행합니다. 주요 알고리즘들의 특성과 적용 방안을 상세히 살펴보겠습니다.
1. Random Sampling-Based Algorithm (무작위 샘플링 기반 알고리즘)
확률 분포에 기반한 샘플링 전략을 통해 경로를 생성합니다. 이 알고리즘의 핵심은 주어진 공간에서 무작위로 샘플을 추출하고, 이를 연결하여 실행 가능한 경로를 찾는 것입니다. 특히 균등 분포나 가우시안 분포와 같은 확률 분포를 활용하여 효율적인 샘플링을 수행합니다. 고차원 공간에서도 적용이 가능하다는 장점이 있으나, 경로의 최적성을 보장하지 않는다는 한계가 있습니다.
- 클래스 구조: Sampler, Validator, PathPlanner
- 주요 메서드:
- sample(): 탐색 공간에서 무작위로 상태를 샘플링
- is_valid(state): 샘플링된 상태의 유효성(충돌 여부) 검사
- plan_path(start, goal): 시작 지점에서 목표 지점까지의 경로 계획
- 알고리즘 동작 과정:
- 탐색 공간에서 무작위로 상태를 샘플링합니다.
- 샘플링된 상태가 유효한지 검사합니다.
- 유효한 상태들을 연결하여 경로를 생성합니다.
- 주요 기능: 복잡한 환경에서의 경로 탐색, 계산 비용 절감
- 시각화: 무작위로 분포된 샘플 점과 그 연결선을 그래프로 표시
- 특징: 고차원 공간에서 효율적이지만, 최적 경로를 보장하지 않음
2. Probabilistic Roadmap Algorithm (PRM) ( 확률적 로드맵 알고리즘)
PRM은 로드맵 구축과 경로 탐색이라는 두 단계로 구성됩니다. 오프라인 단계에서 환경을 샘플링하여 그래프 형태의 로드맵을 구축하고, 온라인 단계에서 이를 활용하여 최적 경로를 탐색합니다. 정적 환경에서 매우 효율적이며 재사용이 가능하다는 장점이 있으나, 동적 환경에서는 추가적인 계산이 필요하다는 단점이 있습니다.
- 클래스 구조: Node, Edge, Roadmap
- 주요 메서드:
- add_node(state): 로드맵에 노드 추가
- add_edge(node1, node2): 두 노드를 연결하는 간선 추가
- find_path(start, goal): 로드맵에서 시작 지점과 목표 지점 간의 경로 탐색
- 알고리즘 동작 과정:
- 탐색 공간에서 무작위로 상태를 샘플링하여 노드를 생성합니다.
- 노드 간의 거리가 일정 기준 이하인 경우 간선을 추가하여 로드맵을 구성합니다.
- 로드맵에서 시작 지점과 목표 지점을 연결하고, 그래프 탐색 알고리즘을 통해 경로를 찾습니다.
- 주요 기능: 복잡한 환경에서의 경로 탐색, 로드맵의 재사용 가능
- 시각화: 노드와 간선으로 이루어진 그래프 형태로 표시
- 특징: 고차원 공간에서 효율적이며, 로드맵을 재사용하여 다중 쿼리에 적합하지만, 좁은 통로(narrow passage) 문제에 취약할 수 있음.
3. Potential Field Algorithm (PFM) ( 포텐셜 필드 알고리즘)
목표점에서의 인력과 장애물에서의 척력을 이용하여 경로를 생성합니다. 실시간으로 적용이 가능하며 동적 환경에서도 효과적입니다. 그러나 지역 최적해 문제에 빠질 수 있으며, 복잡한 환경에서는 성능이 저하될 수 있습니다.
- 클래스 구조: Robot, Obstacle, Goal
- 주요 메서드:
- compute_attractive_force(): 목표 지점으로부터의 인력 계산
- compute_repulsive_force(): 장애물로부터의 반발력 계산
- move_robot(force): 계산된 힘에 따라 로봇 이동
- 알고리즘 동작 과정:
- 목표 지점에서 로봇으로 작용하는 인력을 계산합니다.
- 장애물에서 로봇으로 작용하는 반발력을 계산합니다.
- 인력과 반발력을 합산하여 로봇의 이동 방향과 크기를 결정합니다.
- 로봇을 계산된 힘에 따라 이동시킵니다.
- 주요 기능: 실시간 경로 수정, 장애물 회피
- 시각화: 로봇 주위의 힘 벡터와 이동 경로를 화살표 등으로 표시
- 특징: 계산이 간단하고 실시간 적용이 가능하지만, 지역 최소점(local minima)에 빠질 수 있는 단점이 있음
4. A* Algorithm
휴리스틱 함수를 활용하여 효율적인 경로 탐색을 수행합니다. 실제 비용과 추정 비용을 결합한 평가 함수를 사용하여 최적 경로를 보장합니다. 다만 탐색 공간이 큰 경우 메모리 사용량이 증가하는 단점이 있습니다.
- 클래스 구조: Node, PriorityQueue, Heuristic
- 주요 메서드:
- calculate_cost(current, neighbor): 현재 노드에서 이웃 노드로의 이동 비용 계산
- calculate_heuristic(node, goal): 노드와 목표 지점 간의 휴리스틱 비용 계산
- reconstruct_path(came_from, current): 최적 경로 재구성
- 알고리즘 동작 과정:
- 시작 노드를 우선순위 큐에 추가합니다.
- 큐에서 비용이 가장 낮은 노드를 선택하여 확장합니다.
- 선택된 노드의 이웃 노드에 대해 이동 비용과 휴리스틱 비용을 계산하여 총 비용을 산출합니다.
- 목표 지점에 도달할 때까지 위 과정을 반복합니다.
- 도달 시, 경로를 재구성합니다.
- 주요 기능: 최단 경로 탐색, 효율적인 탐색
- 시각화: 탐색된 노드와 경로를 그래프로 표시
- 특징: 다익스트라 알고리즘에 휴리스틱을 추가하여 탐색 효율성을 향상시킨 알고리즘으로, 최적 경로를 보장하며, 휴리스틱 함수의 선택에 따라 성능이 좌우됩니다.
5. Dijkstra Algorithm (다익스트라 알고리즘)
가중 그래프에서 최단 경로를 찾는 기본적인 알고리즘입니다. 구현이 단순하고 최적성을 보장하지만, 모든 방향으로의 탐색으로 인해 A* 알고리즘보다 비효율적일 수 있습니다.
- 클래스 구조: Node, Edge, Graph
- 주요 메서드:
- initialize_single_source(graph, source): 시작 노드 초기화
- extract_min(queue): 최소 비용 노드 추출
- relax(u, v, weight): 노드 간의 거리 완화
- 알고리즘 동작 과정:
- 모든 노드의 초기 거리를 무한대로 설정하고, 시작 노드의 거리를 0으로 설정합니다.
- 우선순위 큐에서 최소 거리 노드를 추출합니다.
- 해당 노드의 모든 이웃 노드에 대해 거리를 갱신합니다.
- 모든 노드를 처리할 때까지 위 과정을 반복합니다.
- 주요 기능: 최단 경로 탐색
- 시각화: 노드와 간선으로 이루어진 그래프 형태로 표시
- 특징: 음의 가중치가 없는 그래프에서 최적 경로를 보장하며, 그리디 알고리즘과 다이나믹 프로그래밍 기법을 사용합니다.
6. RRT (Rapidly-exploring Random Tree)
무작위 샘플링을 통해 트리를 확장하며 경로를 탐색합니다. 고차원 공간에서도 빠른 초기 경로 탐색이 가능하나, 최적성을 보장하지 않습니다.
- 클래스 구조: Node, Tree, Planner
- 주요 메서드:
- add_node(tree, node): 트리에 노드 추가
- nearest_neighbor(tree, random_node): 트리에서 무작위 노드에 가장 가까운 노드 찾기
- new_state(nearest_node, random_node): 새로운 상태 생성
- 알고리즘 동작 과정:
- 탐색 공간에서 무작위로 상태를 샘플링하여 노드를 생성합니다.
- 트리에서 샘플링된 노드에 가장 가까운 노드를 찾습니다.
- 가까운 노드에서 샘플링된 노드 방향으로 새로운 노드를 생성하고 트리에 추가합니다.
- 목표 지점에 도달하거나 최대 반복 횟수에 도달할 때까지 위 과정을 반복합니다.
- 주요 기능: 복잡한 환경에서의 경로 탐색, 고차원 공간에서의 효율적인 탐색
- 시각화: 탐색된 노드와 경로를 트리 구조로 표시
- 특징: 탐색 공간을 빠르게 탐색하며, 실시간 경로 계획에 적합하지만, 최적 경로를 보장하지 않습니다.
7. Swarm NRRT*
다중 에이전트 시스템에서 RRT 알고리즘을 확장한 것입니다. 여러 에이전트가 협력하여 경로를 탐색하며, 정보 공유를 통해 더 효율적인 탐색이 가능합니다. 그러나 구현이 복잡하고 통신 오버헤드가 발생할 수 있습니다.
이러한 알고리즘들은 각각의 장단점을 가지고 있으며, 적용 환경과 요구사항에 따라 적절한 선택이 필요합니다. 실제 응용에서는 이러한 알고리즘들을 조합하거나 변형하여 사용하는 경우가 많으며, 특히 최근에는 기계학습과의 결합을 통해 성능을 향상시키는 연구도 활발히 진행되고 있습니다. 결론적으로, 각 알고리즘의 특성을 잘 이해하고 적절히 활용하는 것이 효과적인 경로 계획 시스템 구축의 핵심이라 할 수 있습니다.
클래스 구조:
- SwarmAgent: 각 로봇을 나타내는 클래스입니다.
- 속성:
- id: 에이전트의 고유 식별자
- position: 현재 위치
- goal: 목표 위치
- nrrt_star: 해당 에이전트의 NRRT* 인스턴스
- 메서드:
- plan_path(): NRRT*를 사용하여 경로 계획
- share_information(): 다른 에이전트와 정보 공유
- update_position(): 현재 위치 업데이트
- 속성:
- NRRTStar: NRRT* 알고리즘을 구현하는 클래스입니다.
- 속성:
- tree: 탐색 트리
- start: 시작 위치
- goal: 목표 위치
- 메서드:
- initialize_tree(): 트리 초기화
- expand_tree(): 트리 확장
- find_nearest_node(): 가장 가까운 노드 찾기
- rewire_tree(): 트리 재구성
- 속성:
- Environment: 로봇들이 작동하는 환경을 나타내는 클래스입니다.
- 속성:
- obstacles: 장애물 목록
- boundary: 환경의 경계
- 메서드:
- is_collision(): 충돌 여부 확인
- render(): 환경 시각화
- 속성:
주요 메서드:
- SwarmAgent 클래스:
- plan_path(): NRRT* 알고리즘을 사용하여 경로를 계획합니다.
- share_information(): 다른 에이전트와 경로 정보 및 장애물 정보를 공유합니다.
- update_position(): 계획된 경로를 따라 현재 위치를 업데이트합니다.
- NRRTStar 클래스:
- initialize_tree(): 시작 위치를 루트로 하는 탐색 트리를 초기화합니다.
- expand_tree(): 무작위 샘플링을 통해 트리를 확장합니다.
- find_nearest_node(): 트리에서 주어진 위치에 가장 가까운 노드를 찾습니다.
- rewire_tree(): 새로운 노드 추가 시, 트리의 구조를 최적화합니다.
- Environment 클래스:
- is_collision(): 주어진 위치가 장애물과 충돌하는지 확인합니다.
- render(): 현재 환경과 에이전트들의 상태를 시각화합니다.
알고리즘 동작 과정:
- 각 에이전트는 자신의 NRRT* 알고리즘을 초기화하고, 시작 위치에서 탐색 트리를 생성합니다.
- 무작위 샘플링을 통해 트리를 확장하며, 목표 위치에 도달하는 경로를 찾습니다.
- 에이전트들은 주기적으로 서로의 경로 정보와 장애물 정보를 공유하여, 전체 시스템의 인식을 향상시킵니다.
- 공유된 정보를 바탕으로 각 에이전트는 자신의 탐색 트리를 재구성하고, 경로를 최적화합니다.
- 이 과정을 반복하여, 모든 에이전트가 충돌 없이 목표 위치에 도달하도록 합니다.
주요 기능:
- 분산형 경로 계획: 각 에이전트가 독립적으로 경로를 계획하므로, 시스템의 확장성이 높습니다.
- 협력적 정보 공유: 에이전트 간의 정보 공유를 통해, 개별 에이전트의 제한된 센서 범위를 보완하고, 전체적인 경로 계획의 효율성을 향상시킵니다.
- 실시간 경로 수정: 동적 환경에서 실시간으로 경로를 수정하여, 장애물 회피 및 목표 도달을 보장합니다.
시각화:
환경과 에이전트들의 상태를 2D 또는 3D 그래픽으로 시각화하여, 경로 계획 과정과 에이전트들의 움직임을 실시간으로 모니터링할 수 있습니다. 에이전트들의 탐색 트리, 경로, 장애물 등을 시각적으로 표현하여, 알고리즘의 동작을 직관적으로 이해할 수 있습니다.
특징:
- 확장성: 에이전트 수가 증가하더라도, 분산형 구조로 인해 시스템의 성능 저하가 최소화됩니다.
- 유연성: 동적이고 예측 불가능한 환경
# Python 실습 #
1. Random Sampling-Based Algorithm (무작위 샘플링 기반 알고리즘) - Python 예제 코드
import random
import numpy as np
import matplotlib.pyplot as plt
class Node:
"""노드 클래스"""
def __init__(self, x, y):
self.x = x # x 좌표
self.y = y # y 좌표
self.parent = None # 부모 노드
class RRT:
"""RRT 알고리즘 클래스"""
def __init__(self, start, goal, obstacle_list, rand_area,
expand_distance=3.0, goal_sample_rate=5, max_iterations=500):
"""
초기화 함수
start: 시작점 [x, y]
goal: 목표점 [x, y]
obstacle_list: 장애물 리스트 [(x, y, radius), ...]
rand_area: 무작위 샘플링 영역 [min, max]
"""
self.start = Node(start[0], start[1])
self.goal = Node(goal[0], goal[1])
self.obstacle_list = obstacle_list
self.min_rand = rand_area[0]
self.max_rand = rand_area[1]
self.expand_distance = expand_distance
self.goal_sample_rate = goal_sample_rate
self.max_iterations = max_iterations
self.node_list = [self.start]
def planning(self):
"""경로 계획 실행"""
for i in range(self.max_iterations):
# 무작위 노드 생성
if random.randint(0, 100) > self.goal_sample_rate:
random_node = self.get_random_node()
else:
random_node = Node(self.goal.x, self.goal.y)
# 가장 가까운 노드 찾기
nearest_node_idx = self.get_nearest_node_index(random_node)
nearest_node = self.node_list[nearest_node_idx]
# 새로운 노드 생성
new_node = self.steer(nearest_node, random_node)
new_node.parent = nearest_node_idx
# 충돌 검사
if self.check_collision(new_node):
self.node_list.append(new_node)
# 목표 도달 확인
if self.is_near_goal(new_node):
if self.check_collision(Node(self.goal.x, self.goal.y)):
last_node = self.node_list[-1]
return self.generate_final_path(len(self.node_list) - 1)
return None
def get_random_node(self):
"""무작위 노드 생성"""
x = random.uniform(self.min_rand, self.max_rand)
y = random.uniform(self.min_rand, self.max_rand)
return Node(x, y)
def steer(self, from_node, to_node):
"""새로운 노드 생성"""
dist = self.get_distance(from_node, to_node)
angle = np.arctan2(to_node.y - from_node.y, to_node.x - from_node.x)
if dist > self.expand_distance:
dist = self.expand_distance
new_x = from_node.x + dist * np.cos(angle)
new_y = from_node.y + dist * np.sin(angle)
return Node(new_x, new_y)
def get_nearest_node_index(self, random_node):
"""가장 가까운 노드의 인덱스 반환"""
distances = [(node.x - random_node.x) ** 2 + (node.y - random_node.y) ** 2
for node in self.node_list]
return distances.index(min(distances))
def check_collision(self, node):
"""충돌 검사"""
for obstacle in self.obstacle_list:
dx = node.x - obstacle[0]
dy = node.y - obstacle[1]
dist = np.sqrt(dx**2 + dy**2)
if dist <= obstacle[2]:
return False
return True
def is_near_goal(self, node):
"""목표 근처 확인"""
return self.get_distance(node, self.goal) <= self.expand_distance
def get_distance(self, from_node, to_node):
"""두 노드 사이의 거리 계산"""
return np.sqrt((from_node.x - to_node.x)**2 + (from_node.y - to_node.y)**2)
def generate_final_path(self, goal_idx):
"""최종 경로 생성"""
path = [[self.goal.x, self.goal.y]]
node_idx = goal_idx
while self.node_list[node_idx].parent is not None:
node = self.node_list[node_idx]
path.append([node.x, node.y])
node_idx = node.parent
path.append([self.start.x, self.start.y])
return path
def main():
"""메인 함수"""
# 초기 설정
start = [0, 0]
goal = [30, 30]
obstacle_list = [(15, 15, 5), (5, 20, 3), (25, 10, 4)]
rand_area = [-2, 32]
# RRT 실행
rrt = RRT(start, goal, obstacle_list, rand_area)
path = rrt.planning()
# 결과 시각화
if path is not None:
plt.figure()
# 장애물 그리기
for obstacle in obstacle_list:
circle = plt.Circle((obstacle[0], obstacle[1]), obstacle[2], color='red')
plt.gca().add_patch(circle)
# 트리 그리기
for node in rrt.node_list:
if node.parent is not None:
parent = rrt.node_list[node.parent]
plt.plot([node.x, parent.x], [node.y, parent.y], '-g')
# 최종 경로 그리기
path = np.array(path)
plt.plot(path[:, 0], path[:, 1], '-r', linewidth=2)
plt.plot(start[0], start[1], 'ko')
plt.plot(goal[0], goal[1], 'ko')
plt.axis('equal')
plt.grid(True)
plt.show()
if __name__ == '__main__':
main()
1. Random Sampling-Based Algorithm (무작위 샘플링 기반 알고리즘) - Python 예제 코드 실행 결과
2. Probabilistic Roadmap Algorithm (PRM) - Python 예제 코드
import numpy as np
import matplotlib.pyplot as plt
from scipy.spatial import KDTree
import random
import warnings
warnings.filterwarnings('ignore')
class PRM:
def __init__(self, start, goal, obstacle_list, rand_area,
n_samples=500, n_neighbors=10, max_edge_length=5.0):
self.start = np.array(start)
self.goal = np.array(goal)
self.obstacle_list = obstacle_list
self.min_rand = rand_area[0]
self.max_rand = rand_area[1]
self.n_samples = n_samples
self.n_neighbors = n_neighbors
self.max_edge_length = max_edge_length
self.samples = []
self.roadmap = []
self.kdtree = None
def planning(self):
"""PRM 경로 계획 실행"""
# 시작점과 목표점을 포함한 샘플 생성
self.samples = self.generate_samples()
if len(self.samples) < 2:
return None
# KD-tree 생성
self.kdtree = KDTree(self.samples)
# 로드맵 생성
self.generate_roadmap()
# 경로 탐색
path = self.find_path()
return path
def generate_samples(self):
"""무작위 샘플 생성"""
samples = []
# 시작점과 목표점 추가
if not np.array_equal(self.start, self.goal):
samples.append(self.start)
samples.append(self.goal)
else:
samples.append(self.start)
modified_goal = self.goal + np.array([0.1, 0.1])
samples.append(modified_goal)
# 무작위 샘플 생성
attempts = 0
max_attempts = self.n_samples * 10
while len(samples) < self.n_samples and attempts < max_attempts:
sample = self.generate_random_point()
if self.check_collision(sample):
min_dist = min(np.linalg.norm(sample - np.array(s)) for s in samples)
if min_dist > 0.1:
samples.append(sample)
attempts += 1
return np.array(samples)
def generate_random_point(self):
"""무작위 점 생성"""
return np.array([
random.uniform(self.min_rand, self.max_rand),
random.uniform(self.min_rand, self.max_rand)
])
def check_collision(self, point):
"""충돌 검사"""
for obstacle in self.obstacle_list:
dx = point[0] - obstacle[0]
dy = point[1] - obstacle[1]
dist = np.sqrt(dx**2 + dy**2)
if dist <= obstacle[2]:
return False
return True
def check_path_collision(self, point1, point2):
"""경로 충돌 검사"""
dist = np.linalg.norm(point2 - point1)
if dist <= 0 or dist > self.max_edge_length:
return False
n_points = max(int(dist / 0.5), 1)
try:
for i in range(n_points + 1):
t = i / n_points if n_points > 0 else 0
point = point1 + t * (point2 - point1)
if not self.check_collision(point):
return False
except ZeroDivisionError:
return False
return True
def generate_roadmap(self):
"""로드맵 생성"""
self.roadmap = [[] for _ in range(len(self.samples))]
try:
for i, sample in enumerate(self.samples):
k = min(self.n_neighbors + 1, len(self.samples))
if k <= 1:
continue
distances, indices = self.kdtree.query(sample, k=k)
for j, idx in enumerate(indices[1:]):
if idx < len(self.samples):
if self.check_path_collision(sample, self.samples[idx]):
self.roadmap[i].append(idx)
self.roadmap[idx].append(i)
except Exception as e:
print(f"로드맵 생성 중 에러 발생: {e}")
for i in range(len(self.samples)):
if i > 0:
self.roadmap[i].append(i-1)
if i < len(self.samples)-1:
self.roadmap[i].append(i+1)
def find_path(self):
"""다익스트라 알고리즘을 사용한 경로 탐색"""
start_idx = 0
goal_idx = 1
distances = [float('inf')] * len(self.samples)
distances[start_idx] = 0
previous = [None] * len(self.samples)
visited = [False] * len(self.samples)
while True:
min_dist = float('inf')
current_idx = None
for i in range(len(self.samples)):
if not visited[i] and distances[i] < min_dist:
min_dist = distances[i]
current_idx = i
if current_idx is None:
break
visited[current_idx] = True
if current_idx == goal_idx:
break
for next_idx in self.roadmap[current_idx]:
if visited[next_idx]:
continue
try:
distance = distances[current_idx] + np.linalg.norm(
self.samples[current_idx] - self.samples[next_idx])
if distance < distances[next_idx]:
distances[next_idx] = distance
previous[next_idx] = current_idx
except ZeroDivisionError:
continue
if distances[goal_idx] == float('inf'):
return None
path = []
current_idx = goal_idx
while current_idx is not None:
path.append(self.samples[current_idx])
current_idx = previous[current_idx]
return path[::-1]
def plot(self, path=None):
"""결과 시각화"""
plt.figure(figsize=(10, 10))
# 장애물 그리기
for obstacle in self.obstacle_list:
circle = plt.Circle((obstacle[0], obstacle[1]), obstacle[2],
color='red', alpha=0.5)
plt.gca().add_patch(circle)
# 로드맵 그리기
for i, sample in enumerate(self.samples):
for j in self.roadmap[i]:
plt.plot([sample[0], self.samples[j][0]],
[sample[1], self.samples[j][1]], '-g', alpha=0.2)
# 샘플 그리기
samples = np.array(self.samples)
plt.plot(samples[2:, 0], samples[2:, 1], '.b')
plt.plot(self.start[0], self.start[1], 'og')
plt.plot(self.goal[0], self.goal[1], 'or')
# 경로 그리기
if path is not None:
path = np.array(path)
plt.plot(path[:, 0], path[:, 1], '-r', linewidth=2)
plt.axis([self.min_rand, self.max_rand, self.min_rand, self.max_rand])
plt.grid(True)
plt.axis('equal')
plt.show()
def main():
"""메인 함수"""
try:
# 초기 설정
start = [2, 2]
goal = [28, 28]
obstacle_list = [(15, 15, 5), (5, 20, 3), (25, 10, 4)]
rand_area = [0, 30]
# PRM 실행
prm = PRM(start, goal, obstacle_list, rand_area)
path = prm.planning()
# 결과 시각화
if path is not None:
prm.plot(path)
else:
print("경로를 찾을 수 없습니다.")
except Exception as e:
print(f"오류 발생: {e}")
if __name__ == '__main__':
main()
2. Probabilistic Roadmap Algorithm (PRM) - Python 예제 코드 실행 결과
3. Potential Field Algorithm (PFM) - Python 예제 코드
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
class PotentialFieldPlanner:
def __init__(self):
# 환경 파라미터 설정
self.KP = 2.0 # 인력 계수 (수정)
self.ETA = 50.0 # 척력 계수 (수정)
self.AREA_WIDTH = 30.0
self.ROBOT_RADIUS = 2.0 # 로봇 반경 (수정)
self.SAFETY_DISTANCE = 5.0 # 안전 거리 추가
# 시뮬레이션 파라미터
self.animation = True
self.delta = 0.5
self.max_iterations = 1000
self.min_potential_threshold = 0.1 # 최소 포텐셜 임계값 추가
def calc_attractive_potential(self, x, y, gx, gy):
"""인력 포텐셜 계산"""
dist = np.hypot(x - gx, y - gy)
if dist <= self.SAFETY_DISTANCE:
return 0.5 * self.KP * dist ** 2
else:
return self.KP * self.SAFETY_DISTANCE * dist - 0.5 * self.KP * self.SAFETY_DISTANCE ** 2
def calc_repulsive_potential(self, x, y, ox, oy):
"""척력 포텐셜 계산"""
uo = 0.0
for i in range(len(ox)):
d = np.hypot(x - ox[i], y - oy[i])
if d <= self.ROBOT_RADIUS:
if d <= 0.1:
d = 0.1
uo += 0.5 * self.ETA * (1.0 / d - 1.0 / self.ROBOT_RADIUS) ** 2
return uo
def get_motion_model(self):
"""모션 모델 정의"""
motion = [[1, 0],
[0, 1],
[-1, 0],
[0, -1],
[-1, -1],
[-1, 1],
[1, -1],
[1, 1]]
return motion
def potential_field_planning(self, sx, sy, gx, gy, ox, oy, grid_size=0.5):
"""포텐셜 필드 기반 경로 계획"""
current_x, current_y = sx, sy
rx, ry = [sx], [sy]
motion = self.get_motion_model()
previous_x, previous_y = None, None
stuck_count = 0
while True:
# 목표점 도달 확인
if np.hypot(current_x - gx, current_y - gy) < grid_size:
print("Goal!!")
break
# 현재 위치에서의 포텐셜 계산
current_potential = self.calc_attractive_potential(current_x, current_y, gx, gy) + \
self.calc_repulsive_potential(current_x, current_y, ox, oy)
# 다음 이동 위치 탐색
min_potential = float("inf")
best_move = None
for dx, dy in motion:
new_x = current_x + dx * self.delta
new_y = current_y + dy * self.delta
# 장애물과의 충돌 검사
collision = False
for obx, oby in zip(ox, oy):
if np.hypot(new_x - obx, new_y - oby) < self.ROBOT_RADIUS:
collision = True
break
if collision:
continue
# 포텐셜 계산
potential = self.calc_attractive_potential(new_x, new_y, gx, gy) + \
self.calc_repulsive_potential(new_x, new_y, ox, oy)
if potential < min_potential:
min_potential = potential
best_move = (new_x, new_y)
# 이동 불가능한 경우 처리
if best_move is None:
print("No immediate path found, trying to escape local minimum...")
# 무작위 방향으로 이동 시도
escape_attempts = 0
while escape_attempts < 8:
random_motion = motion[np.random.randint(0, len(motion))]
new_x = current_x + random_motion[0] * self.delta * 2
new_y = current_y + random_motion[1] * self.delta * 2
collision = False
for obx, oby in zip(ox, oy):
if np.hypot(new_x - obx, new_y - oby) < self.ROBOT_RADIUS:
collision = True
break
if not collision:
best_move = (new_x, new_y)
break
escape_attempts += 1
if best_move is None:
print("Failed to escape local minimum")
break
# 이동 실행
current_x, current_y = best_move
rx.append(current_x)
ry.append(current_y)
# 시각화
if self.animation:
plt.cla()
plt.plot(ox, oy, "xk")
plt.plot(sx, sy, "og")
plt.plot(gx, gy, "or")
plt.plot(rx, ry, "-r")
plt.axis("equal")
plt.grid(True)
plt.pause(0.01)
# 정체 상태 확인
if previous_x is not None:
if abs(previous_x - current_x) < self.delta/10 and \
abs(previous_y - current_y) < self.delta/10:
stuck_count += 1
else:
stuck_count = 0
if stuck_count > 10:
print("Stuck in local minimum")
break
previous_x, previous_y = current_x, current_y
return rx, ry
def main():
print("포텐셜 필드 경로 계획 시작")
# 시작점과 목표점 설정
sx = 0.0
sy = 0.0
gx = 30.0
gy = 30.0
grid_size = 0.5
# 장애물 위치 수정
ox = [15.0, 5.0, 20.0, 25.0]
oy = [25.0, 15.0, 26.0, 25.0]
# 경로 계획
planner = PotentialFieldPlanner()
if planner.animation:
plt.grid(True)
plt.axis("equal")
# 경로 생성 및 시각화
rx, ry = planner.potential_field_planning(
sx, sy, gx, gy, ox, oy, grid_size)
if rx: # 경로를 찾은 경우
plt.plot(rx, ry, "-r")
plt.pause(0.01)
plt.show()
else:
print("경로를 찾을 수 없습니다.")
if __name__ == '__main__':
main()
3. Potential Field Algorithm (PFM) - Python 예제 코드 실행 결과
4. A* Algorithm - Python 예제 코드
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.collections import PatchCollection
from matplotlib.patches import Rectangle
import heapq
class Node:
def __init__(self, x, y, cost, parent_index):
self.x = x # x 좌표
self.y = y # y 좌표
self.cost = cost # 비용
self.parent_index = parent_index # 부모 노드 인덱스
self.f = 0.0 # f = g + h
def __lt__(self, other):
return self.f < other.f
class AStarPlanner:
def __init__(self, min_x, max_x, min_y, max_y, resolution, robot_radius):
self.min_x = min_x
self.max_x = max_x
self.min_y = min_y
self.max_y = max_y
self.resolution = resolution
self.robot_radius = robot_radius
# 그리드 크기 계산
self.x_width = round((max_x - min_x) / resolution)
self.y_width = round((max_y - min_y) / resolution)
# 장애물 맵 초기화
self.obstacle_map = [[False for _ in range(self.y_width)]
for _ in range(self.x_width)]
# 이동 방향 모델 (8방향)
self.motion = [
[1, 0, 1], # 우
[0, 1, 1], # 상
[-1, 0, 1], # 좌
[0, -1, 1], # 하
[1, 1, 1.414], # 우상
[-1, 1, 1.414], # 좌상
[-1, -1, 1.414],# 좌하
[1, -1, 1.414] # 우하
]
def set_obstacle(self, ox, oy):
"""장애물 설정"""
for x, y in zip(ox, oy):
# 장애물 좌표를 그리드 인덱스로 변환
ix = self.calc_xy_index(x, self.min_x)
iy = self.calc_xy_index(y, self.min_y)
if 0 <= ix < self.x_width and 0 <= iy < self.y_width:
self.obstacle_map[ix][iy] = True
def planning(self, sx, sy, gx, gy):
"""A* 경로 계획"""
# 시작 노드와 목표 노드 생성
start_node = Node(self.calc_xy_index(sx, self.min_x),
self.calc_xy_index(sy, self.min_y), 0.0, -1)
goal_node = Node(self.calc_xy_index(gx, self.min_x),
self.calc_xy_index(gy, self.min_y), 0.0, -1)
# Open, Closed 집합 초기화
open_set = {}
closed_set = {}
# 시작 노드를 open_set에 추가
start_node.f = self.calc_heuristic(start_node, goal_node)
open_set[self.calc_index(start_node)] = start_node
while open_set:
# 현재 노드 선택 (f값이 가장 작은 노드)
current_id = min(open_set, key=lambda o: open_set[o].f)
current = open_set[current_id]
# 목표 도달 확인
if current.x == goal_node.x and current.y == goal_node.y:
print("Goal Found!")
goal_node.parent_index = current.parent_index
goal_node.cost = current.cost
break
# 현재 노드를 closed_set으로 이동
del open_set[current_id]
closed_set[current_id] = current
# 이웃 노드 탐색
for move_x, move_y, move_cost in self.motion:
node = Node(current.x + move_x,
current.y + move_y,
current.cost + move_cost,
current_id)
node_id = self.calc_index(node)
# 유효하지 않은 노드 건너뛰기
if not self.verify_node(node):
continue
if node_id in closed_set:
continue
node.f = node.cost + self.calc_heuristic(node, goal_node)
if node_id not in open_set:
open_set[node_id] = node
else:
if open_set[node_id].cost > node.cost:
open_set[node_id] = node
if current.x != goal_node.x or current.y != goal_node.y:
print("No path found to goal")
return [], []
# 최종 경로 생성
rx, ry = self.calc_final_path(goal_node, closed_set)
return rx, ry
def calc_final_path(self, goal_node, closed_set):
"""최종 경로 계산"""
rx, ry = [self.calc_position(goal_node.x, self.min_x)], \
[self.calc_position(goal_node.y, self.min_y)]
parent_index = goal_node.parent_index
while parent_index != -1:
n = closed_set[parent_index]
rx.append(self.calc_position(n.x, self.min_x))
ry.append(self.calc_position(n.y, self.min_y))
parent_index = n.parent_index
return rx, ry
def calc_heuristic(self, node1, node2):
"""휴리스틱 비용 계산 (유클리드 거리)"""
w = 1.0 # 가중치
d = w * np.sqrt((node1.x - node2.x)**2 + (node1.y - node2.y)**2)
return d
def calc_position(self, index, min_pos):
"""그리드 인덱스를 실제 위치로 변환"""
pos = index * self.resolution + min_pos
return pos
def calc_xy_index(self, position, min_pos):
"""실제 위치를 그리드 인덱스로 변환"""
return round((position - min_pos) / self.resolution)
def calc_index(self, node):
"""노드의 고유 인덱스 계산"""
return (node.y * self.x_width) + node.x
def verify_node(self, node):
"""노드의 유효성 검증"""
if node.x < 0 or node.x >= self.x_width:
return False
if node.y < 0 or node.y >= self.y_width:
return False
if self.obstacle_map[node.x][node.y]:
return False
return True
def main():
print("A* 경로 계획 시작")
# 지도 크기와 해상도 설정
min_x, max_x = -5, 55
min_y, max_y = -5, 55
resolution = 1.0
robot_radius = 1.0
# A* 플래너 초기화
planner = AStarPlanner(min_x, max_x, min_y, max_y, resolution, robot_radius)
# 장애물 설정
ox, oy = [], []
# 외벽
for i in range(-5, 55):
ox.append(i)
oy.append(-5)
for i in range(-5, 55):
ox.append(i)
oy.append(55)
for i in range(-5, 55):
ox.append(-5)
oy.append(i)
for i in range(-5, 55):
ox.append(55)
oy.append(i)
# 내부 장애물
for i in range(-5, 25):
ox.append(20)
oy.append(i)
for i in range(25, 55):
ox.append(35)
oy.append(i)
planner.set_obstacle(ox, oy)
# 시작점과 목표점 설정
sx = 0.0
sy = 0.0
gx = 50.0
gy = 50.0
# 경로 계획 실행
rx, ry = planner.planning(sx, sy, gx, gy)
# 결과 시각화
plt.figure(figsize=(10, 10))
plt.plot(ox, oy, ".k", label="Obstacles")
plt.plot(sx, sy, "og", label="Start")
plt.plot(gx, gy, "xr", label="Goal")
if rx and ry:
plt.plot(rx, ry, "-b", label="Path")
plt.grid(True)
plt.axis("equal")
plt.legend()
plt.show()
if __name__ == '__main__':
main()
4. A* Algorithm - Python 예제 코드 실행 결과
A* 경로 계획 시작
Goal Found!
5. Dijkstra Algorithm - Python 예제 코드
import sys
import subprocess
from typing import List, Tuple, Dict
def install_required_packages():
"""필요한 패키지 설치"""
required_packages = ['numpy', 'matplotlib']
for package in required_packages:
try:
__import__(package)
except ImportError:
print(f"Installing {package}...")
subprocess.check_call([sys.executable, "-m", "pip", "install", package])
# 패키지 설치
install_required_packages()
# 필요한 패키지 임포트
import numpy as np
import matplotlib.pyplot as plt
class Node:
"""경로 계획을 위한 노드 클래스"""
def __init__(self, x: float, y: float, cost: float, parent_index: int):
self.x = x
self.y = y
self.cost = cost
self.parent_index = parent_index
def __lt__(self, other):
return self.cost < other.cost
class DijkstraPlanner:
"""다익스트라 알고리즘을 이용한 경로 계획"""
def __init__(self, min_x: float, max_x: float, min_y: float, max_y: float,
resolution: float, robot_radius: float):
self.min_x = min_x
self.max_x = max_x
self.min_y = min_y
self.max_y = max_y
self.resolution = resolution
self.robot_radius = robot_radius
self.x_width = round((max_x - min_x) / resolution)
self.y_width = round((max_y - min_y) / resolution)
self.animation = True
# 장애물 맵 초기화
self.obstacle_map = [[False for _ in range(self.y_width)]
for _ in range(self.x_width)]
# 이동 방향 정의
self.motion = self.get_motion_model()
def get_motion_model(self) -> List[List[float]]:
"""8방향 이동 모델 정의"""
motion = [[1, 0, 1.0],
[0, 1, 1.0],
[-1, 0, 1.0],
[0, -1, 1.0],
[1, 1, np.sqrt(2)],
[-1, 1, np.sqrt(2)],
[-1, -1, np.sqrt(2)],
[1, -1, np.sqrt(2)]]
return motion
def planning(self, sx: float, sy: float, gx: float, gy: float) -> Tuple[list, list]:
"""
경로 계획 실행
Args:
sx: 시작점 x 좌표
sy: 시작점 y 좌표
gx: 목표점 x 좌표
gy: 목표점 y 좌표
Returns:
rx, ry: 계획된 경로 좌표
"""
start_node = self.create_node(sx, sy, 0, -1)
goal_node = self.create_node(gx, gy, 0, -1)
open_set: Dict[int, Node] = {}
closed_set: Dict[int, Node] = {}
open_set[self.calc_index(start_node)] = start_node
while True:
if not open_set:
print("경로를 찾을 수 없습니다.")
return [], []
current_id = min(open_set, key=lambda o: open_set[o].cost)
current = open_set[current_id]
# 시각화
if self.animation:
plt.plot(self.calc_position(current.x, self.min_x),
self.calc_position(current.y, self.min_y), "xc")
plt.pause(0.001)
if current.x == goal_node.x and current.y == goal_node.y:
print("목표에 도달했습니다!")
goal_node.parent_index = current.parent_index
goal_node.cost = current.cost
break
# 현재 노드를 closed set으로 이동
del open_set[current_id]
closed_set[current_id] = current
# 이웃 노드 탐색
for move_x, move_y, move_cost in self.motion:
node = self.create_node(
self.calc_position(current.x + move_x, self.min_x),
self.calc_position(current.y + move_y, self.min_y),
current.cost + move_cost,
current_id
)
node_id = self.calc_index(node)
if not self.verify_node(node):
continue
if node_id in closed_set:
continue
if node_id not in open_set:
open_set[node_id] = node
else:
if open_set[node_id].cost > node.cost:
open_set[node_id] = node
rx, ry = self.calc_final_path(goal_node, closed_set)
return rx, ry
def calc_final_path(self, goal_node: Node, closed_set: Dict[int, Node]) -> Tuple[list, list]:
"""최종 경로 계산"""
rx = [self.calc_position(goal_node.x, self.min_x)]
ry = [self.calc_position(goal_node.y, self.min_y)]
parent_index = goal_node.parent_index
while parent_index != -1:
node = closed_set[parent_index]
rx.append(self.calc_position(node.x, self.min_x))
ry.append(self.calc_position(node.y, self.min_y))
parent_index = node.parent_index
return rx, ry
def create_node(self, x: float, y: float, cost: float, parent_index: int) -> Node:
"""노드 생성"""
return Node(
self.calc_xy_index(x, self.min_x),
self.calc_xy_index(y, self.min_y),
cost,
parent_index
)
def calc_position(self, index: int, min_pos: float) -> float:
"""인덱스를 실제 위치로 변환"""
return index * self.resolution + min_pos
def calc_xy_index(self, position: float, min_pos: float) -> int:
"""위치를 인덱스로 변환"""
return round((position - min_pos) / self.resolution)
def calc_index(self, node: Node) -> int:
"""노드의 고유 인덱스 계산"""
return (node.y * self.x_width) + node.x
def verify_node(self, node: Node) -> bool:
"""노드의 유효성 검증"""
if node.x < 0 or node.x >= self.x_width:
return False
if node.y < 0 or node.y >= self.y_width:
return False
return not self.obstacle_map[node.x][node.y]
def set_obstacle(self, ox: List[float], oy: List[float]) -> None:
"""장애물 설정"""
for x, y in zip(ox, oy):
ix = self.calc_xy_index(x, self.min_x)
iy = self.calc_xy_index(y, self.min_y)
if 0 <= ix < self.x_width and 0 <= iy < self.y_width:
self.obstacle_map[ix][iy] = True
def main():
"""메인 함수"""
print("다익스트라 경로 계획 시작")
# 환경 설정
min_x, max_x = -5.0, 55.0
min_y, max_y = -5.0, 55.0
resolution = 2.0
robot_radius = 1.0
# 시작점과 목표점
sx, sy = 0.0, 0.0
gx, gy = 50.0, 50.0
# 플래너 초기화
planner = DijkstraPlanner(min_x, max_x, min_y, max_y, resolution, robot_radius)
# 장애물 설정
ox, oy = [], []
# 외벽
for i in range(-5, 55):
ox.extend([i, i])
oy.extend([-5, 55])
for i in range(-5, 55):
ox.extend([-5, 55])
oy.extend([i, i])
# 내부 장애물
for i in range(-5, 25):
ox.append(20)
oy.append(i)
for i in range(25, 55):
ox.append(35)
oy.append(i)
planner.set_obstacle(ox, oy)
# 시각화 설정
plt.figure(figsize=(10, 10))
plt.plot(ox, oy, ".k")
plt.plot(sx, sy, "og")
plt.plot(gx, gy, "xb")
plt.grid(True)
plt.axis("equal")
# 경로 계획 실행
rx, ry = planner.planning(sx, sy, gx, gy)
if rx and ry:
plt.plot(rx, ry, "-r")
plt.pause(0.01)
plt.show()
if __name__ == '__main__':
main()
5. Dijkstra Algorithm - Python 예제 코드 실행 결과
6. RRT (Rapidly-exploring Random Tree) - Python 예제 코드
import numpy as np
import matplotlib.pyplot as plt
from typing import List, Tuple, Optional
import random
import math
class RRTPlanner:
"""RRT 경로 계획 알고리즘 클래스"""
class Node:
"""트리의 노드를 표현하는 내부 클래스"""
def __init__(self, x: float, y: float):
self.x = x
self.y = y
self.parent = None
self.path_x = []
self.path_y = []
def __init__(self,
start: List[float],
goal: List[float],
obstacle_list: List[Tuple[float, float, float]],
rand_area: List[float],
expand_dis: float = 3.0,
goal_sample_rate: int = 5,
max_iter: int = 500):
"""
RRT 플래너 초기화
Args:
start: 시작점 [x, y]
goal: 목표점 [x, y]
obstacle_list: 장애물 리스트 [(x, y, radius), ...]
rand_area: 무작위 샘플링 영역 [min, max]
expand_dis: 확장 거리
goal_sample_rate: 목표점 샘플링 비율 (%)
max_iter: 최대 반복 횟수
"""
self.start = self.Node(start[0], start[1])
self.end = self.Node(goal[0], goal[1])
self.min_rand = rand_area[0]
self.max_rand = rand_area[1]
self.expand_dis = expand_dis
self.goal_sample_rate = goal_sample_rate
self.max_iter = max_iter
self.obstacle_list = obstacle_list
self.node_list = [self.start]
def planning(self, animation: bool = True) -> Tuple[List[float], List[float]]:
"""
RRT 경로 계획 실행
Args:
animation: 시각화 여부
Returns:
경로 좌표 (x_coords, y_coords)
"""
for i in range(self.max_iter):
# 무작위 노드 생성
if random.randint(0, 100) > self.goal_sample_rate:
rnd = self.Node(
random.uniform(self.min_rand, self.max_rand),
random.uniform(self.min_rand, self.max_rand)
)
else: # 목표점 방향으로 편향된 샘플링
rnd = self.Node(self.end.x, self.end.y)
# 가장 가까운 노드 찾기
nearest_ind = self.get_nearest_node_index(rnd)
nearest_node = self.node_list[nearest_ind]
# 새로운 노드 생성
new_node = self.steer(nearest_node, rnd)
if new_node and not self.check_collision(new_node):
self.node_list.append(new_node)
# 시각화
if animation:
self.draw_graph(rnd)
# 목표 도달 확인
if self.calc_dist_to_goal(self.node_list[-1].x, self.node_list[-1].y) <= self.expand_dis:
final_node = self.steer(self.node_list[-1], self.end)
if final_node and not self.check_collision(final_node):
return self.generate_final_course(len(self.node_list) - 1)
return [], [] # 경로를 찾지 못한 경우
def steer(self, from_node: Node, to_node: Node) -> Optional[Node]:
"""
두 노드 사이의 새로운 노드 생성
"""
dist = math.hypot(to_node.x - from_node.x, to_node.y - from_node.y)
if dist > self.expand_dis:
theta = math.atan2(to_node.y - from_node.y, to_node.x - from_node.x)
new_x = from_node.x + self.expand_dis * math.cos(theta)
new_y = from_node.y + self.expand_dis * math.sin(theta)
else:
new_x = to_node.x
new_y = to_node.y
new_node = self.Node(new_x, new_y)
new_node.parent = from_node
return new_node
def check_collision(self, node: Node) -> bool:
"""
노드와 장애물 간의 충돌 검사
"""
if node is None:
return True
for (ox, oy, size) in self.obstacle_list:
dx_list = [node.x]
dy_list = [node.y]
if node.parent:
dx_list.append(node.parent.x)
dy_list.append(node.parent.y)
for (dx, dy) in zip(dx_list, dy_list):
dist = math.hypot(dx - ox, dy - oy)
if dist <= size:
return True # 충돌
return False # 충돌 없음
def get_nearest_node_index(self, rnd_node: Node) -> int:
"""
무작위 노드에서 가장 가까운 노드의 인덱스 반환
"""
dlist = [(node.x - rnd_node.x) ** 2 + (node.y - rnd_node.y) ** 2
for node in self.node_list]
minind = dlist.index(min(dlist))
return minind
def generate_final_course(self, goal_ind: int) -> Tuple[List[float], List[float]]:
"""
최종 경로 생성
"""
path_x = [self.end.x]
path_y = [self.end.y]
node = self.node_list[goal_ind]
while node.parent is not None:
path_x.append(node.x)
path_y.append(node.y)
node = node.parent
path_x.append(node.x)
path_y.append(node.y)
return path_x, path_y
def calc_dist_to_goal(self, x: float, y: float) -> float:
"""
현재 위치에서 목표점까지의 거리 계산
"""
return math.hypot(x - self.end.x, y - self.end.y)
def draw_graph(self, rnd: Optional[Node] = None):
"""
그래프 시각화
"""
plt.clf()
# 장애물 그리기
for (ox, oy, size) in self.obstacle_list:
circle = plt.Circle((ox, oy), size, color='gray')
plt.gca().add_patch(circle)
# 노드와 엣지 그리기
for node in self.node_list:
if node.parent:
plt.plot([node.x, node.parent.x], [node.y, node.parent.y], "-g")
# 시작점과 목표점
plt.plot(self.start.x, self.start.y, "xr")
plt.plot(self.end.x, self.end.y, "xr")
# 무작위 노드
if rnd is not None:
plt.plot(rnd.x, rnd.y, "^k")
plt.axis("equal")
plt.grid(True)
plt.pause(0.01)
def main():
"""
메인 함수
"""
print("RRT 경로 계획 시작")
# 초기 설정
start = [0, 0]
goal = [30, 30]
# 장애물 리스트 [(x, y, radius), ...]
obstacle_list = [
(5, 5, 3),
(10, 10, 3),
(15, 15, 3),
(20, 20, 3),
(25, 25, 3),
]
# RRT 설정
rrt = RRTPlanner(
start=start,
goal=goal,
rand_area=[-2, 50],
obstacle_list=obstacle_list,
expand_dis=3.0,
goal_sample_rate=5,
max_iter=500
)
# 경로 계획 실행
path_x, path_y = rrt.planning(animation=True)
if path_x:
plt.plot(path_x, path_y, '-r')
plt.grid(True)
plt.pause(0.01)
plt.show()
else:
print("경로를 찾을 수 없습니다.")
if __name__ == '__main__':
main()
6. RRT (Rapidly-exploring Random Tree) - Python 예제 코드 실행 결과
7. Swarm NRRT*(Neural Rapidly-exploring Random Tree Star) - Python 예제 코드
import numpy as np
import matplotlib.pyplot as plt
from typing import List, Tuple, Optional, Dict
import random
import math
import sys
import os
from scipy.spatial import KDTree
class SwarmAgent:
"""개별 스웜 에이전트 클래스"""
def __init__(self, start: np.ndarray, goal: np.ndarray, id: int):
self.position = np.array(start)
self.goal = np.array(goal)
self.id = id
self.path = [self.position.copy()]
self.velocity = np.zeros(2)
self.best_position = self.position.copy()
self.best_cost = float('inf')
def update_position(self, new_position: np.ndarray):
"""에이전트 위치 업데이트"""
self.position = new_position
self.path.append(new_position.copy())
current_cost = np.linalg.norm(self.position - self.goal)
if current_cost < self.best_cost:
self.best_cost = current_cost
self.best_position = self.position.copy()
class SwarmRRTStar:
"""Swarm RRT* 알고리즘 클래스"""
def __init__(self,
num_agents: int,
start_positions: List[np.ndarray],
goal_positions: List[np.ndarray],
obstacle_list: List[Tuple[float, float, float]],
rand_area: List[float],
max_iter: int = 1000,
expand_dis: float = 3.0,
path_resolution: float = 0.5,
goal_sample_rate: int = 5,
max_agents_distance: float = 5.0):
self.num_agents = num_agents
self.agents = [SwarmAgent(start_positions[i], goal_positions[i], i)
for i in range(num_agents)]
self.obstacle_list = obstacle_list
self.min_rand, self.max_rand = rand_area
self.max_iter = max_iter
self.expand_dis = expand_dis
self.path_resolution = path_resolution
self.goal_sample_rate = goal_sample_rate
self.max_agents_distance = max_agents_distance
self.node_list = []
self.found_paths = set()
def plan_paths(self, animation: bool = True) -> Dict[int, List[np.ndarray]]:
"""전체 스웜에 대한 경로 계획"""
plt.ion() # 대화형 모드 활성화
paths = {i: [] for i in range(self.num_agents)}
for iteration in range(self.max_iter):
if len(self.found_paths) == self.num_agents:
break
for agent in self.agents:
if agent.id in self.found_paths:
continue
if random.randint(0, 100) > self.goal_sample_rate:
rnd_point = self._random_point()
else:
rnd_point = agent.goal
new_position = self._extend(agent.position, rnd_point)
if self._is_valid_position(new_position, agent.id):
neighbors = self._find_neighbors(new_position)
best_neighbor = self._choose_best_parent(new_position, neighbors)
if best_neighbor:
new_position = self._steer(best_neighbor.position, new_position)
agent.update_position(new_position)
self.node_list.append(new_position)
if animation and iteration % 5 == 0:
self._draw(iteration)
if np.linalg.norm(agent.position - agent.goal) < self.expand_dis:
paths[agent.id] = agent.path
self.found_paths.add(agent.id)
break
plt.pause(0.001)
plt.ioff()
return paths
def _random_point(self) -> np.ndarray:
return np.array([
random.uniform(self.min_rand, self.max_rand),
random.uniform(self.min_rand, self.max_rand)
])
def _extend(self, from_point: np.ndarray, to_point: np.ndarray) -> np.ndarray:
dir_vector = to_point - from_point
dist = np.linalg.norm(dir_vector)
if dist > self.expand_dis:
dir_vector = dir_vector / dist * self.expand_dis
return from_point + dir_vector
def _steer(self, from_point: np.ndarray, to_point: np.ndarray) -> np.ndarray:
dist = np.linalg.norm(to_point - from_point)
if dist > self.expand_dis:
return from_point + (to_point - from_point) * self.expand_dis / dist
return to_point
def _is_valid_position(self, position: np.ndarray, agent_id: int) -> bool:
for obs in self.obstacle_list:
if np.linalg.norm(position - np.array(obs[:2])) <= obs[2]:
return False
for agent in self.agents:
if agent.id != agent_id:
if np.linalg.norm(position - agent.position) < self.max_agents_distance:
return False
return True
def _find_neighbors(self, position: np.ndarray) -> List[SwarmAgent]:
neighbors = []
for agent in self.agents:
if np.linalg.norm(position - agent.position) < self.max_agents_distance:
neighbors.append(agent)
return neighbors
def _choose_best_parent(self, position: np.ndarray, neighbors: List[SwarmAgent]) -> Optional[SwarmAgent]:
min_cost = float('inf')
best_neighbor = None
for neighbor in neighbors:
cost = np.linalg.norm(position - neighbor.position) + neighbor.best_cost
if cost < min_cost:
min_cost = cost
best_neighbor = neighbor
return best_neighbor
def _draw(self, iteration: int):
plt.clf()
for obs in self.obstacle_list:
circle = plt.Circle((obs[0], obs[1]), obs[2], color='gray')
plt.gca().add_patch(circle)
colors = plt.cm.rainbow(np.linspace(0, 1, self.num_agents))
for agent, color in zip(self.agents, colors):
path = np.array(agent.path)
plt.plot(path[:, 0], path[:, 1], '-', color=color, alpha=0.5)
plt.plot(agent.goal[0], agent.goal[1], 'x', color=color)
plt.plot(agent.position[0], agent.position[1], 'o', color=color)
nodes = np.array(self.node_list)
if len(nodes) > 0:
plt.plot(nodes[:, 0], nodes[:, 1], '.k', alpha=0.1)
plt.grid(True)
plt.axis('equal')
plt.title(f'Iteration {iteration}')
def main():
print("Swarm RRT* 시작")
print("프로그램을 종료하려면 'q' 키를 누르세요.")
num_agents = 5
start_positions = [np.array([0, i*5]) for i in range(num_agents)]
goal_positions = [np.array([30, 30-i*5]) for i in range(num_agents)]
obstacle_list = [
(10, 10, 3),
(15, 15, 3),
(20, 20, 3),
(25, 15, 3),
(15, 25, 3),
]
planner = SwarmRRTStar(
num_agents=num_agents,
start_positions=start_positions,
goal_positions=goal_positions,
obstacle_list=obstacle_list,
rand_area=[-2, 50],
max_iter=200,
expand_dis=3.0,
path_resolution=0.5,
goal_sample_rate=5,
max_agents_distance=5.0
)
def on_key(event):
if event.key == 'q':
plt.close('all')
os._exit(0)
fig = plt.figure(figsize=(10, 10))
fig.canvas.mpl_connect('key_press_event', on_key)
paths = planner.plan_paths(animation=True)
plt.figure(figsize=(10, 10))
plt.title("Final Paths")
for obs in obstacle_list:
circle = plt.Circle((obs[0], obs[1]), obs[2], color='gray')
plt.gca().add_patch(circle)
colors = plt.cm.rainbow(np.linspace(0, 1, num_agents))
for agent_id, path in paths.items():
if path:
path = np.array(path)
plt.plot(path[:, 0], path[:, 1], '-',
color=colors[agent_id],
label=f'Agent {agent_id}')
plt.plot(path[0, 0], path[0, 1], 'o', color=colors[agent_id])
plt.plot(path[-1, 0], path[-1, 1], 'x', color=colors[agent_id])
plt.grid(True)
plt.axis('equal')
plt.legend()
def on_close(event):
os._exit(0)
plt.gcf().canvas.mpl_connect('close_event', on_close)
plt.gcf().canvas.mpl_connect('key_press_event', on_key)
try:
plt.show()
except KeyboardInterrupt:
os._exit(0)
if __name__ == '__main__':
try:
main()
except KeyboardInterrupt:
os._exit(0)
7. Swarm NRRT(Neural Rapidly-exploring Random Tree Star) - Python 예제 코드 실행 결과
728x90
반응형
'자율주행 모빌리티 (Autonomous Mobility)' 카테고리의 다른 글
| 비행체/주변환경 데이터 기반 무인기 의사결정 알고리즘 (0) | 2025.04.09 |
|---|---|
| Path Planning, Guidance, Navigation, Control 알고리즘 (3) | 2025.04.09 |
| Visual SLAM (0) | 2025.03.17 |
| Tesla Vision (0) | 2025.03.17 |
| UAV의 위치 및 자세 추정 기술 및 메커니즘 (0) | 2025.03.16 |