Lecture 6. 경로 계획

출발점에서 목표점까지, 장애물을 피해 가는 길을 찾는 알고리즘. 전역 경로(global path)와 지역 경로(local path), 그리고 그래프 탐색과 샘플링 기반 기법까지.

1. 경로 계획이란?

모바일 로봇이 "어디로 갈지"를 정하는 문제입니다. 실제 시스템은 보통 세 층으로 나뉘어 돌아갑니다:

전역 경로 (Global path) — 전체 지도에서 출발점→목표점까지 큰 그림의 경로. 계산이 느려도 괜찮지만 장애물 정보가 정확해야 합니다. (예: A*)
지역 경로 (Local path) — 전역 경로 일부 구간에서 새로 발견된 장애물을 피하는 단기 재계획. 빠른 반응이 중요합니다. (예: DWA, TEB)
추종 제어 (Tracking control) — 경로를 따라가도록 바퀴·모터에 명령을 내리는 제어기. 다음 강의 L7·L8에서 다룹니다.

이 강의에서는 전역 경로 계획의 두 축인 그래프 탐색과 샘플링 기반 방법을 모두 살펴봅니다.

2. 그래프 표현과 탐색 전략

지도를 격자(grid)로 잘라 각 칸을 노드(node), 인접 칸 사이를 엣지(edge)로 보면 경로 찾기는 그래프 탐색 문제가 됩니다. 엣지에는 이동 비용 $w$가 붙고, 목표는 비용 합이 최소가 되는 노드 순서를 찾는 것입니다.

탐색 전략은 크게 두 가지로 나뉩니다:

무정보 탐색 (Uninformed / blind) — 목표의 위치를 전혀 사용하지 않고 기계적으로 확장. BFS, Dijkstra.
정보 기반 탐색 (Informed / heuristic) — "여기서 목표까지 얼마나 남았을지"의 추정값 $h(n)$을 활용. Greedy Best-First, A*, D*.

A*의 핵심 평가함수는 다음과 같습니다:

$$ f(n) = \underbrace{g(n)}_{\text{시작에서 } n \text{까지 실제 비용}} + \underbrace{h(n)}_{\text{} n \text{에서 목표까지 추정 비용}} $$

Admissibility: $h(n)$이 실제 최소 비용을 과대평가하지 않으면($h(n) \le h^*(n)$) A*는 최적해를 보장합니다. 유클리드·맨해튼 거리 모두 admissible 휴리스틱의 대표 예입니다.

$$ h_\text{euclid}(n) = \sqrt{(x_g-x_n)^2 + (y_g-y_n)^2} \qquad h_\text{manhattan}(n) = |x_g-x_n| + |y_g-y_n| $$

3. 네 알고리즘의 비교

알고리즘	우선순위	최단경로?	탐색 영역
BFS	깊이(step 수)	○ (가중치=1일 때)	원형으로 균일 확산
Dijkstra	$g(n)$	○	가까운 쪽 원형 확산
Greedy Best-First	$h(n)$	✗	목표 방향으로 직진 (장애물에 약함)
A*	$f(n) = g(n)+h(n)$	○ (admissible $h$)	목표 방향 집중 (좁고 효율적)

4. 인터랙티브: 네 알고리즘 동시 비교

🎮 알고리즘 선택 A* / Dijkstra / BFS / Greedy

같은 지도에서 네 알고리즘이 얼마나 많은 노드를 방문하는지를 직접 비교해 보세요. 격자를 클릭/드래그하여 장애물을 그리고, 모드를 바꾼 뒤 Run을 누르세요.

알고리즘 🟢 시작 🔴 목표 ⬛ 장애물

5. 샘플링 기반: PRM과 RRT

격자가 아닌 연속 공간에서 장애물이 복잡하면 그래프 탐색 대신 공간을 무작위로 샘플링해 그래프를 만드는 편이 훨씬 효율적입니다. 대표 기법은 두 가지입니다:

PRM (Probabilistic Roadmap) — 전체 공간에서 유효한 점들을 다수 뽑고, 가까운 점끼리 연결해 로드맵을 만든 뒤 그 위에서 A*로 경로를 찾습니다. 다회 질의에 유리(지도가 고정이면 로드맵 재사용).
RRT (Rapidly-exploring Random Tree) — 트리를 한 노드씩 빠르게 자라게 해서 한 번의 질의에 유리.

🎮 인터랙티브: PRM 로드맵

점을 뽑고(샘플링), 반경 $r$ 안의 점들과 장애물을 피해 연결합니다. 그 후 시작→목표 최단 경로를 Dijkstra로 찾습니다.

샘플 수 80 연결 반경 r 90

5.1 RRT — 트리 성장형

RRT는 트리를 점점 자라게 해서 빠르게 장애물 공간을 탐색합니다.

공간에서 랜덤 점 $q_\text{rand}$를 뽑습니다.
현재 트리에서 가장 가까운 노드 $q_\text{near}$를 찾습니다.
$q_\text{near}$에서 $q_\text{rand}$ 방향으로 작은 거리만큼 전진해 새 노드 $q_\text{new}$를 만듭니다.
장애물에 부딪히지 않으면 트리에 추가.
목표에 충분히 가까워지면 종료.

RRT*는 새 노드를 추가할 때 주변 노드들을 다시 연결하여 최적성을 보장합니다.

🎮 인터랙티브: RRT 트리 자라기

PythonRobotics RRT 알고리즘의 시각화입니다. Step을 누르면 트리가 한 노드씩 자랍니다.

6. 인공 포텐셜 필드 (Artificial Potential Field)

로봇을 전기를 띤 입자로 가정해, 목표점에서 끌어당기는 힘과 장애물에서 미는 힘을 합성한 방향으로 움직입니다. PythonRobotics의 Potential Field 구현을 참고했습니다.

$$ U(x) = \underbrace{\tfrac12 k_a \|x - x_g\|^2}_{\text{인력}} + \underbrace{\sum_o \tfrac12 k_r \left(\tfrac{1}{d(x,o)} - \tfrac{1}{d_0}\right)^2}_{\text{척력}} $$

지역 최솟값(local minimum)에 빠질 수 있다는 단점이 있지만 구현이 매우 단순합니다.

🎮 인터랙티브: 포텐셜 필드 내비게이션

마우스를 끌어 목표점(녹색)을 옮기면, 로봇(파랑)이 인력+척력 합력 방향으로 움직입니다. 장애물 주위의 등고선도 보입니다.

인력 ka 1 척력 kr 800

7. Dubins 곡선 — 운동학적 제약

자동차나 비행기는 제자리 회전을 못합니다. 최소 회전 반경 $R$이 정해져 있죠. 두 자세(위치+방향) 사이의 가장 짧은 경로는 항상 직선과 원호의 조합 6가지 중 하나입니다 (Dubins, 1957). PythonRobotics의 DubinsPath 모듈이 표준 구현입니다.

🎮 인터랙티브: Dubins LSL 경로

출발과 도착의 자세, 회전 반경을 조절해 보세요. 차가 갈 수 있는 가장 짧은 "좌-직선-좌" 경로를 그립니다.

출발 방향 (도) 30 도착 방향 (도) 90 회전 반경 R 60

8. 코드 예제 — A* (Python)

import heapq, math

def astar(grid, start, goal):
    H, W = len(grid), len(grid[0])
    h = lambda a,b: math.hypot(a[0]-b[0], a[1]-b[1])
    open_, came, g = [(h(start,goal), 0, start)], {start: None}, {start: 0}
    while open_:
        _, gc, cur = heapq.heappop(open_)
        if cur == goal:
            path = []
            while cur: path.append(cur); cur = came[cur]
            return path[::-1]
        for dx,dy in [(1,0),(-1,0),(0,1),(0,-1),(1,1),(-1,1),(1,-1),(-1,-1)]:
            nx, ny = cur[0]+dx, cur[1]+dy
            if 0<=nx



  📖 더 깊이 공부하기
  
    Red Blob Games — A* Introduction — 시각화가 매우 친절해 처음 공부하는 사람에게 최적.
    PythonRobotics PathPlanning — A*, Dijkstra, RRT, RRT*, PRM, Dubins 등 모든 알고리즘의 짧은 파이썬 참조 구현.
    Planning Algorithms (S. LaValle, Cambridge 2006) — 샘플링 기반 경로 계획의 고전 교과서. 온라인 공개.
    Karaman & Frazzoli, "Sampling-based algorithms for optimal motion planning", IJRR 2011 — RRT*의 원논문.
    Dubins, "On curves of minimal length with a constraint on average curvature", Amer. J. Math. 1957 — Dubins 곡선의 원논문.
  

  
    ← 이전L5. 칼만 2D
    다음 →L7. 피드백 제어

🌊 Study Page