CH08경로 계획과 장애물 회피
지도 위에서 A* 로 전역 경로를 찾고, 코스트맵 팽창과 DWA 지역 계획으로 움직이는 장애물을 피하며 목표까지 가는 자율주행 루프를 완성합니다.
학습 목표#
- 점유 격자를 그래프로 보고 BFS · 다익스트라 · A\* 가 셀을 꺼내는 순서를 비교하며, 휴리스틱(맨해튼 · 유클리드)과 가중치 w 가 탐색 셀 수와 경로 길이를 어떻게 바꾸는지 예측하고 시뮬레이터로 확인할 수 있습니다.
- Python
heapq로 A\* 를 직접 구현해 텍스트 지도에 경로(*)를 찍고, 4방향 · 8방향 이웃과 "모서리 자르기 금지" 규칙을 전방향(메카넘) 로봇에 맞게 고를 수 있습니다. - 로봇을 점으로 보지 않도록 장애물을 팽창(inflation) 시키는 이유를 설명하고, Nav2 비용 값(254 · 253 · 1~252 · 0)과
inflation_radius·cost_scaling_factor의 역할, 발자국(footprint)의 내접 · 외접 반지름으로 알맞은 팽창 반경을 고를 수 있습니다. - Pure Pursuit 의 lookahead 거리와 DWA 의 점수 가중치(목표 · 장애물 거리 · 속도)가 궤적 모양 · 도착 시간 · 안전 여유에 주는 영향을 시뮬레이터 결과로 설명하고, 메카넘 로봇에서 측면 속도 vy 후보가 주는 이점을 말할 수 있습니다.
- 감지 → 위치 추정 → 계획 → 제어 루프에 재계획 조건과 복구 행동을 넣고, 성공률 · 소요 시간 · 최소 장애물 거리로 성능을 비교할 수 있습니다.
이 페이지에는 브라우저에서 바로 동작하는 시뮬레이터 5개가 있습니다. 슬라이더를 움직이기 전에 결과를 먼저 예측해 적고, 조작해서 확인하세요. 예측이 틀린 곳이 이 차시에서 가장 많이 배우는 곳입니다. Playground 실습 코드는 표준 라이브러리 + numpy 만 씁니다. 실습 1-1 · 1-2 · 2-1 은 시뮬레이터와 같은 지도 · 같은 규칙이라 같은 숫자가 나옵니다. 서로 맞춰 보며 코드와 화면을 함께 이해하세요.
공식 자료 기준: myAGV Plus 최고 속도 1.6 m/s · LiDAR 0.12~8 m(myagvPlus_docs 2. Product Feature). Plus 문서의 6.2.5 Map Navigation 은 myagv_plus_navigation2 패키지로 내비게이션을 실행하는 방법만 설명하고, 코스트맵 · 컨트롤러 파라미터 값은 적혀 있지 않습니다. 참고한 공개 설정: myagv_ros2 저장소(이전 모델 myAGV 용, galactic-JN 브랜치)의 myagv_navigation2/param/myagv.yaml — 전역 robot_radius: 0.22, 지역 footprint 0.34 × 0.24 m, inflation_radius: 0.25, cost_scaling_factor: 5.0, 해상도 0.05 m, 플래너 NavfnPlanner(use_astar: false), 컨트롤러 DWBLocalPlanner(max_vel_x: 0.26, max_vel_y: 0.0), 복구 spin · backup · wait. Plus 에 그대로 맞는지는 확인 필요입니다. 교육용 가정: 시뮬레이터의 지도 치수, 로봇 반경 0.2 m, 최고 속도 0.5 m/s, 가속 한계, 사람의 걷는 방식, 상자가 나타나는 규칙, DWA 점수의 정규화 방식, 비용을 경로 길이에 더하는 비율(1 + 비용/63) 등. 시뮬레이터는 원리를 보여 주기 위한 것이며 Nav2 내부 구현과 같지 않습니다.
1교시 · 격자 지도 위 A* 경로 탐색#
지도를 그래프로 바꾸기#
ch07 에서 만든 점유 격자는 칸마다 "빈 칸 · 장애물 · 미확인" 이 적힌 표입니다. 경로 계획(path planning)은 이 표를 그래프로 보고 출발 칸에서 목표 칸까지 가는 칸의 줄을 찾는 문제입니다.
- 노드(node) = 빈 칸 하나. 셀 크기(해상도)가 0.05 m 면 12 m × 8 m 창고에 노드가 38 400 개입니다.
- 간선(edge) = 이웃한 두 빈 칸을 잇는 선. 가중치는 칸 사이 거리(가로 · 세로 1칸, 대각선 √2 칸)입니다.
- 경로 = 출발에서 목표까지 간선을 따라 이은 칸의 줄. 칸 수가 아니라 거리(m) 의 합이 짧을수록 좋은 경로입니다.
BFS · 다익스트라 · A* 비교#
세 방법은 모두 "아직 확정하지 않은 후보 칸들" 에서 하나를 꺼내(확정하고) 그 이웃을 후보로 올리는 일을 반복합니다. 차이는 어떤 칸을 먼저 꺼내느냐 하나뿐입니다.
| 방법 | 먼저 꺼내는 칸 | 자료구조 | 최단 보장 | 탐색 범위 |
|---|---|---|---|---|
| BFS (너비 우선) | 먼저 들어온 칸 (걸음 수 순) | 큐(deque) | 걸음 수 최소만. 대각선 √2 를 1로 셈 | 사방으로 고르게 |
| 다익스트라 | 지금까지 온 거리 g 가 가장 작은 칸 | 우선순위 큐(heapq) | 거리 최소 보장 | 사방으로 고르게 |
| A* | f = g + h 가 가장 작은 칸. h = 목표까지 남은 거리 추정 | 우선순위 큐 | h 가 실제 거리보다 크지 않으면 보장 | 목표 쪽으로 좁게 |
| 가중 A* | f = g + w·h (w > 1) | 우선순위 큐 | 보장 안 됨 (최대 w 배까지 길어질 수 있음) | 더 좁게 |
휴리스틱(heuristic) h 는 "벽을 무시하고 목표까지 직선으로 가면 얼마나 남았나" 의 추정입니다.
| 휴리스틱 | 식 (Δx, Δy = 칸 차이 × 해상도) | 4방향 이웃 | 8방향 이웃 |
|---|---|---|---|
| 맨해튼(Manhattan) | |Δx| + |Δy| | 실제 거리를 넘지 않음 → 최단 보장 | 대각선으로 가면 실제보다 크게 추정 → 최단 보장 안 됨, 대신 탐색이 줄어듦 |
| 유클리드(Euclidean) | √(Δx² + Δy²) | 너무 작게 추정 → 최단이지만 탐색이 조금 넓음 | 실제 거리를 넘지 않음 → 최단 보장 |
w = 0 이면 h 를 아예 안 보므로 다익스트라와 같아지고, w 를 키울수록 "목표 쪽으로만 서두르는" 탐색이 됩니다.
4방향 vs 8방향 이웃#
- 4방향: 상하좌우만. 대각선으로 가려면 계단 모양이 되어 실제보다 길어집니다(같은 지점까지 √2 배 손해).
- 8방향: 대각선 이웃도 씀. 메카넘 휠 로봇은 몸을 돌리지 않고도 대각선으로 갈 수 있으므로(ch03 전방향 운동학) 8방향 경로를 그대로 따라가기 좋습니다.
- 모서리 자르기 금지: 대각선 칸이 비어 있어도 그 양옆 두 칸 중 하나가 벽이면 대각선으로 가지 않습니다. 허용하면 경로가 벽 모서리를 스치는 "틈"을 지나가게 됩니다.
바퀴 두 개로 도는 차동 구동 로봇은 대각선 칸으로 가려면 먼저 몸을 돌려야 합니다. 그래서 차동 구동은 방향 전환 비용을 따로 더하거나, 계획 뒤에 경로를 부드럽게 다듬습니다. 메카넘 로봇은 이 부담이 적습니다.
시뮬레이터 1 · 좁은 통로 미로에서 탐색 비교#
셀 한 칸이 0.25 m 인 24 × 16 칸 지도입니다. 지도를 클릭 · 드래그해서 벽을 놓거나 지우고, 시작 · 목표를 옮겨 보세요. ▶ 탐색 과정 재생 을 누르면 셀이 꺼내지는 순서가 보입니다. 이 단계에서는 로봇을 크기 없는 점으로 봅니다(2교시에서 고칩니다).
- 좁은 통로 미로에서 알고리즘을 A* (w = 1) → 다익스트라 → BFS 로 바꾸기 전에, 탐색 셀 수가 어떤 순서로 커질지 예측합니다. 📊 네 가지 비교 로 확인하세요.
- 가중치 w 를 0 → 1 → 3 으로 바꾸면 탐색 셀 수와 경로 길이가 각각 어떻게 될까요? 예측을 적은 뒤 w 버튼으로 확인하고, "최단 대비" 칸을 봅니다.
- 빈 방 에서 다익스트라와 A* 의 탐색 셀 수를 비교합니다. 몇 배 차이가 날까요? 탐색 과정 재생으로 파란 영역의 모양(원 vs 직선)을 비교하세요.
- U자 함정 에서 이웃을 4방향 → 8방향으로 바꿉니다. 경로 길이가 얼마나 줄까요? 8방향에서 휴리스틱을 유클리드 → 맨해튼으로 바꾸면 무엇이 달라지나요?
- 외길 미로 에서 w 를 0 → 3 으로 바꿔 봅니다. 빈 방에서보다 차이가 큰가요, 작은가요? 벽 몇 개를 지워 지름길을 만들면 어떻게 될까요?
- 좁은 통로 미로(4방향 · 맨해튼): BFS 162 · 다익스트라 162 · A* w=1 131 · A* w=3 38 셀. 경로 길이는 w = 3 만 8.50 m 로, 나머지(8.00 m)보다 6 % 깁니다. 휴리스틱을 부풀리면 목표 쪽 통로에 먼저 뛰어들었다가 그 길을 그대로 써 버립니다.
- 4방향에서 모든 간선 길이가 같으면 BFS 와 다익스트라는 같은 수의 셀을 꺼냅니다. 8방향에서는 BFS 가 대각선(√2)을 1걸음으로 세므로 걸음 수는 최소지만 거리는 최단이 아닐 수 있습니다.
- 빈 방: 다익스트라 232 셀 vs A* 20 셀(4방향). 장애물이 적을수록 휴리스틱이 정확해져 A* 의 이득이 큽니다. 외길 미로에서는 어차피 갈 길이 하나라 휴리스틱이 거의 도움이 되지 않습니다(147 → 145).
- U자 함정: 4방향 7.25 m → 8방향 6.08 m (16 % 짧음). 8방향 + 맨해튼은 탐색 셀이 210 → 152 로 줄지만 이 지도에서는 길이가 같았습니다. 맨해튼이 8방향에서 "과대평가" 라는 것은 항상 길어진다는 뜻이 아니라 최단을 보장하지 못한다는 뜻입니다.
실습 과제 1#
시뮬레이터 1 의 "좁은 통로 미로" 를 문자열 지도로 옮겼습니다. TODO 를 채워 A* 를 완성하고 경로를 * 로 찍으세요. 이어서 w = 0 · 1 · 3 을 비교합니다. 예측 먼저: 시뮬레이터에서 본 숫자(131 셀, 8.00 m)가 그대로 나올까요? heapq 에 (f, count, 칸) 처럼 count 를 넣는 이유는 무엇일까요?
import heapq
# 좁은 통로 미로 (시뮬레이터 1 과 같은 지도) - 셀 한 칸 = 0.25 m, '#' 벽, S 시작, G 목표
MAP = [
"########################",
"#S.....................#",
"#.###.####.####.####.#.#",
"#.###.####.####.####.#.#",
"#......................#",
"#.####.#########.#####.#",
"#.####.#########.#####.#",
"#......................#",
"#.#####.####.#########.#",
"#.#####.####.#########.#",
"#......................#",
"#.##########.#########.#",
"#.##########.#########.#",
"#....................G.#",
"#......................#",
"########################",
]
RES = 0.25
grid = [list(row) for row in MAP]
NR, NC = len(grid), len(grid[0])
start = next((r, c) for r in range(NR) for c in range(NC) if grid[r][c] == "S")
goal = next((r, c) for r in range(NR) for c in range(NC) if grid[r][c] == "G")
# 이웃 순서: 오른쪽, 왼쪽, 위, 아래 (시뮬레이터와 같은 순서라야 같은 숫자가 나옵니다)
N4 = [(0, 1, 1.0), (0, -1, 1.0), (-1, 0, 1.0), (1, 0, 1.0)]
def free(r, c):
return 0 <= r < NR and 0 <= c < NC and grid[r][c] != "#"
def heuristic(a, b):
# TODO 1: 맨해튼 거리 (|dr| + |dc|) * RES 를 돌려줍니다
return 0.0
def astar(start, goal, w=1.0):
g = {start: 0.0}
parent = {start: None}
closed = set()
count = 0 # f 가 같으면 먼저 넣은 것부터 꺼냄
heap = [(w * heuristic(start, goal), count, start)]
while heap:
f, _, cur = heapq.heappop(heap)
if cur in closed:
continue # 더 좋은 값으로 이미 꺼낸 셀
closed.add(cur)
if cur == goal:
break
for dr, dc, step in N4:
nxt = (cur[0] + dr, cur[1] + dc)
if not free(*nxt) or nxt in closed:
continue
# TODO 2: ng = g[cur] + step * RES 가 g 에 적힌 값(없으면 무한대)보다 작으면
# g[nxt], parent[nxt] 를 고치고, count 를 1 늘린 뒤
# (ng + w * heuristic(nxt, goal), count, nxt) 를 heap 에 넣습니다
pass
if goal not in parent:
return None, len(closed)
path = [goal]
while parent[path[-1]] is not None:
path.append(parent[path[-1]])
return path[::-1], len(closed)
path, expanded = astar(start, goal)
if path is None:
print("경로 없음 - TODO 를 채우세요. 꺼낸 셀:", expanded)
else:
for r, c in path[1:-1]:
grid[r][c] = "*"
print("\n".join("".join(row) for row in grid))
print("탐색(꺼낸) 셀 %d개, 경로 %d칸 = %.2f m" % (expanded, len(path) - 1, (len(path) - 1) * RES))
# TODO 3: w = 0, 1, 3 으로 astar 를 불러 탐색 셀 수와 경로 길이를 비교해 출력합니다풀이 코드 보기
import heapq
# 좁은 통로 미로 (시뮬레이터 1 과 같은 지도) - 셀 한 칸 = 0.25 m, '#' 벽, S 시작, G 목표
MAP = [
"########################",
"#S.....................#",
"#.###.####.####.####.#.#",
"#.###.####.####.####.#.#",
"#......................#",
"#.####.#########.#####.#",
"#.####.#########.#####.#",
"#......................#",
"#.#####.####.#########.#",
"#.#####.####.#########.#",
"#......................#",
"#.##########.#########.#",
"#.##########.#########.#",
"#....................G.#",
"#......................#",
"########################",
]
RES = 0.25
grid = [list(row) for row in MAP]
NR, NC = len(grid), len(grid[0])
start = next((r, c) for r in range(NR) for c in range(NC) if grid[r][c] == "S")
goal = next((r, c) for r in range(NR) for c in range(NC) if grid[r][c] == "G")
# 이웃 순서: 오른쪽, 왼쪽, 위, 아래 (시뮬레이터와 같은 순서라야 같은 숫자가 나옵니다)
N4 = [(0, 1, 1.0), (0, -1, 1.0), (-1, 0, 1.0), (1, 0, 1.0)]
def free(r, c):
return 0 <= r < NR and 0 <= c < NC and grid[r][c] != "#"
def heuristic(a, b):
return (abs(a[0] - b[0]) + abs(a[1] - b[1])) * RES
def astar(start, goal, w=1.0):
g = {start: 0.0}
parent = {start: None}
closed = set()
count = 0 # f 가 같으면 먼저 넣은 것부터 꺼냄
heap = [(w * heuristic(start, goal), count, start)]
while heap:
f, _, cur = heapq.heappop(heap)
if cur in closed:
continue # 더 좋은 값으로 이미 꺼낸 셀
closed.add(cur)
if cur == goal:
break
for dr, dc, step in N4:
nxt = (cur[0] + dr, cur[1] + dc)
if not free(*nxt) or nxt in closed:
continue
ng = g[cur] + step * RES
if ng < g.get(nxt, float("inf")):
g[nxt] = ng
parent[nxt] = cur
count += 1
heapq.heappush(heap, (ng + w * heuristic(nxt, goal), count, nxt))
if goal not in parent:
return None, len(closed)
path = [goal]
while parent[path[-1]] is not None:
path.append(parent[path[-1]])
return path[::-1], len(closed)
path, expanded = astar(start, goal)
if path is None:
print("경로 없음 - TODO 를 채우세요. 꺼낸 셀:", expanded)
else:
for r, c in path[1:-1]:
grid[r][c] = "*"
print("\n".join("".join(row) for row in grid))
print("탐색(꺼낸) 셀 %d개, 경로 %d칸 = %.2f m" % (expanded, len(path) - 1, (len(path) - 1) * RES))
print()
for w in [0, 1, 3]:
p, e = astar(start, goal, w)
print("w = %d : 탐색 셀 %3d개, 경로 %.2f m" % (w, e, (len(p) - 1) * RES))실행 결과:
########################
#S****.................#
#.###*####.####.####.#.#
#.###*####.####.####.#.#
#....**................#
#.####*#########.#####.#
#.####*#########.#####.#
#.....*******..........#
#.#####.####*#########.#
#.#####.####*#########.#
#...........*..........#
#.##########*#########.#
#.##########*#########.#
#...........*********G.#
#......................#
########################
탐색(꺼낸) 셀 131개, 경로 32칸 = 8.00 m
w = 0 : 탐색 셀 162개, 경로 8.00 m
w = 1 : 탐색 셀 131개, 경로 8.00 m
w = 3 : 탐색 셀 38개, 경로 8.50 m- 시뮬레이터와 같은 숫자(131 · 162 · 38 셀)가 나옵니다. 이웃을 보는 순서와 "f 가 같으면 먼저 넣은 칸부터" 라는 규칙까지 같기 때문입니다. count 를 빼면 f 가 같을 때 튜플의 다음 값(칸 좌표)으로 비교해 꺼내는 순서가 달라지고, 탐색 셀 수도 달라집니다.
if cur in closed: continue는 같은 칸이 더 나쁜 f 로 힙에 남아 있는 경우(지연 삭제)를 건너뜁니다. 이 줄이 없으면 한 칸을 여러 번 확장해 탐색 셀 수가 부풀어 오릅니다.- w = 3 의 경로(8.50 m)는 최단(8.00 m)보다 2칸 깁니다. 실시간으로 자주 다시 계획해야 할 때는 "조금 긴 경로를 빨리" 가 나을 수도 있습니다.
"U자 함정" 지도에서 9가지 조합의 탐색 셀 수 · 경로 길이 · 칸 수를 표로 만듭니다. TODO 두 곳(유클리드 휴리스틱, 대각선 모서리 규칙)을 채우세요. 예측 먼저: 8방향으로 바꾸면 경로 길이는 몇 % 줄까요? "칸 수" 와 "경로(m)" 중 어느 쪽이 더 많이 줄까요?
import heapq
import math
from collections import deque
# U자 함정 지도 (시뮬레이터 1 의 'U자 함정' 과 같음) - 셀 한 칸 = 0.25 m
MAP = [
"########################",
"#......................#",
"#......................#",
"#..........#######.....#",
"#................#.....#",
"#................#.....#",
"#................#.....#",
"#.S..............#...G.#",
"#................#.....#",
"#................#.....#",
"#................#.....#",
"#..........#######.....#",
"#......................#",
"#......................#",
"#......................#",
"########################",
]
RES = 0.25
NR, NC = len(MAP), len(MAP[0])
find = lambda ch: next((r, c) for r in range(NR) for c in range(NC) if MAP[r][c] == ch)
START, GOAL = find("S"), find("G")
D = math.sqrt(2)
N4 = [(0, 1, 1.0), (0, -1, 1.0), (-1, 0, 1.0), (1, 0, 1.0)]
N8 = N4 + [(-1, 1, D), (-1, -1, D), (1, 1, D), (1, -1, D)]
def free(r, c):
return 0 <= r < NR and 0 <= c < NC and MAP[r][c] != "#"
def h(a, b, kind):
dr, dc = abs(a[0] - b[0]), abs(a[1] - b[1])
# TODO 1: kind 가 "euc" 이면 유클리드 거리 math.hypot(dr, dc) * RES 를 돌려줍니다
return (dr + dc) * RES
def neighbors(cur, nb):
for dr, dc, step in (N8 if nb == 8 else N4):
r, c = cur[0] + dr, cur[1] + dc
if not free(r, c):
continue
# TODO 2: 대각선(dr, dc 가 둘 다 0 이 아님)이면 양옆 두 칸 (cur[0]+dr, cur[1]) 과
# (cur[0], cur[1]+dc) 가 모두 비어 있을 때만 갑니다 (벽 모서리를 스치지 않게)
yield (r, c), step * RES
def search(alg, nb=4, kind="man", w=1.0):
"""alg: 'bfs' 또는 'astar' (w=0 이면 다익스트라). (탐색 셀 수, 경로 길이 m, 칸 수) 반환"""
g, parent, closed = {START: 0.0}, {START: None}, []
if alg == "bfs":
q, seen = deque([START]), {START}
while q:
cur = q.popleft()
closed.append(cur)
if cur == GOAL:
break
for nxt, st in neighbors(cur, nb):
if nxt not in seen:
seen.add(nxt)
g[nxt], parent[nxt] = g[cur] + st, cur
q.append(nxt)
else:
done, count = set(), 0
heap = [(w * h(START, GOAL, kind), 0, START)]
while heap:
_, _, cur = heapq.heappop(heap)
if cur in done:
continue
done.add(cur)
closed.append(cur)
if cur == GOAL:
break
for nxt, st in neighbors(cur, nb):
ng = g[cur] + st
if nxt not in done and ng < g.get(nxt, float("inf")):
g[nxt], parent[nxt] = ng, cur
count += 1
heapq.heappush(heap, (ng + w * h(nxt, GOAL, kind), count, nxt))
n, cur = 0, GOAL
while parent[cur] is not None:
cur, n = parent[cur], n + 1
return len(closed), g[GOAL], n
print("%7s %8s %6s %s" % ("탐색셀", "경로(m)", "칸수", "방법"))
for name, alg, nb, kind, w in [
("BFS 4방향", "bfs", 4, "man", 0), ("다익스트라 4방향", "astar", 4, "man", 0),
("A* 4방향 맨해튼", "astar", 4, "man", 1), ("A* 4방향 맨해튼 w=3", "astar", 4, "man", 3),
("BFS 8방향", "bfs", 8, "euc", 0), ("다익스트라 8방향", "astar", 8, "euc", 0),
("A* 8방향 유클리드", "astar", 8, "euc", 1), ("A* 8방향 맨해튼", "astar", 8, "man", 1),
("A* 8방향 유클리드 w=3", "astar", 8, "euc", 3)]:
e, L, n = search(alg, nb, kind, w)
print("%7d %8.2f %6d %s" % (e, L, n, name))풀이 코드 보기
import heapq
import math
from collections import deque
# U자 함정 지도 (시뮬레이터 1 의 'U자 함정' 과 같음) - 셀 한 칸 = 0.25 m
MAP = [
"########################",
"#......................#",
"#......................#",
"#..........#######.....#",
"#................#.....#",
"#................#.....#",
"#................#.....#",
"#.S..............#...G.#",
"#................#.....#",
"#................#.....#",
"#................#.....#",
"#..........#######.....#",
"#......................#",
"#......................#",
"#......................#",
"########################",
]
RES = 0.25
NR, NC = len(MAP), len(MAP[0])
find = lambda ch: next((r, c) for r in range(NR) for c in range(NC) if MAP[r][c] == ch)
START, GOAL = find("S"), find("G")
D = math.sqrt(2)
N4 = [(0, 1, 1.0), (0, -1, 1.0), (-1, 0, 1.0), (1, 0, 1.0)]
N8 = N4 + [(-1, 1, D), (-1, -1, D), (1, 1, D), (1, -1, D)]
def free(r, c):
return 0 <= r < NR and 0 <= c < NC and MAP[r][c] != "#"
def h(a, b, kind):
dr, dc = abs(a[0] - b[0]), abs(a[1] - b[1])
return (math.hypot(dr, dc) if kind == "euc" else dr + dc) * RES
def neighbors(cur, nb):
for dr, dc, step in (N8 if nb == 8 else N4):
r, c = cur[0] + dr, cur[1] + dc
if not free(r, c):
continue
if dr and dc and not (free(cur[0] + dr, cur[1]) and free(cur[0], cur[1] + dc)):
continue # 대각선은 양옆 두 칸이 모두 비어 있을 때만 (모서리 자르기 금지)
yield (r, c), step * RES
def search(alg, nb=4, kind="man", w=1.0):
"""alg: 'bfs' 또는 'astar' (w=0 이면 다익스트라). (탐색 셀 수, 경로 길이 m, 칸 수) 반환"""
g, parent, closed = {START: 0.0}, {START: None}, []
if alg == "bfs":
q, seen = deque([START]), {START}
while q:
cur = q.popleft()
closed.append(cur)
if cur == GOAL:
break
for nxt, st in neighbors(cur, nb):
if nxt not in seen:
seen.add(nxt)
g[nxt], parent[nxt] = g[cur] + st, cur
q.append(nxt)
else:
done, count = set(), 0
heap = [(w * h(START, GOAL, kind), 0, START)]
while heap:
_, _, cur = heapq.heappop(heap)
if cur in done:
continue
done.add(cur)
closed.append(cur)
if cur == GOAL:
break
for nxt, st in neighbors(cur, nb):
ng = g[cur] + st
if nxt not in done and ng < g.get(nxt, float("inf")):
g[nxt], parent[nxt] = ng, cur
count += 1
heapq.heappush(heap, (ng + w * h(nxt, GOAL, kind), count, nxt))
n, cur = 0, GOAL
while parent[cur] is not None:
cur, n = parent[cur], n + 1
return len(closed), g[GOAL], n
print("%7s %8s %6s %s" % ("탐색셀", "경로(m)", "칸수", "방법"))
for name, alg, nb, kind, w in [
("BFS 4방향", "bfs", 4, "man", 0), ("다익스트라 4방향", "astar", 4, "man", 0),
("A* 4방향 맨해튼", "astar", 4, "man", 1), ("A* 4방향 맨해튼 w=3", "astar", 4, "man", 3),
("BFS 8방향", "bfs", 8, "euc", 0), ("다익스트라 8방향", "astar", 8, "euc", 0),
("A* 8방향 유클리드", "astar", 8, "euc", 1), ("A* 8방향 맨해튼", "astar", 8, "man", 1),
("A* 8방향 유클리드 w=3", "astar", 8, "euc", 3)]:
e, L, n = search(alg, nb, kind, w)
print("%7d %8.2f %6d %s" % (e, L, n, name))실행 결과:
탐색셀 경로(m) 칸수 방법
285 7.25 29 BFS 4방향
285 7.25 29 다익스트라 4방향
208 7.25 29 A* 4방향 맨해튼
117 7.25 29 A* 4방향 맨해튼 w=3
286 6.08 21 BFS 8방향
286 6.08 21 다익스트라 8방향
210 6.08 21 A* 8방향 유클리드
152 6.08 21 A* 8방향 맨해튼
85 6.52 24 A* 8방향 유클리드 w=3- 8방향은 칸 수(29 → 21)가 거리(7.25 → 6.08 m)보다 더 많이 줄어듭니다. 대각선 한 칸이 √2 칸 거리이기 때문입니다.
- TODO 2(모서리 규칙)를 빼고 돌리면 경로가 U자 벽의 끝 모서리를 대각선으로 스쳐 지나갑니다. 실제 로봇은 크기가 있으므로 이 규칙만으로도 부족하고, 2교시의 팽창이 필요합니다.
- 8방향 유클리드 w = 3 은 탐색 셀이 85 개로 가장 적지만 경로가 6.52 m 로 7 % 깁니다.
좁은 통로 미로에서 A* 의 가중치 w 를 1 에서 3 으로 올렸더니 탐색 셀은 131 → 38 개로 줄었지만 경로는 8.00 → 8.50 m 로 길어졌습니다. 가장 알맞은 설명은?
- w 를 키우면 휴리스틱이 작아져 다익스트라처럼 된다
- w·h 가 실제 남은 거리보다 커져, 목표 쪽으로 서둘러 찾은 첫 경로가 최단이 아닐 수 있다
- 4방향 이웃에서는 w 와 상관없이 항상 최단이 나오므로 시뮬레이터 오류다
- w 를 키우면 대각선 이동이 허용되어 경로가 바뀐다
정답 보기
② — f = g + w·h 에서 w > 1 이면 남은 거리를 부풀려 추정(과대평가)하므로 "목표에 가까워 보이는" 칸을 먼저 꺼냅니다. 탐색은 크게 줄지만 최단 보장이 사라집니다. w = 0 이 다익스트라입니다(①은 반대). 이웃 방식(④)은 w 와 무관합니다.myAGV Plus(메카넘)로 장애물이 듬성듬성한 넓은 창고에서 최단 거리를 보장하며 탐색 셀도 줄이고 싶습니다. 가장 알맞은 조합은?
- 4방향 이웃 + 휴리스틱 없음(BFS)
- 8방향 이웃 + 유클리드 휴리스틱, w = 1
- 8방향 이웃 + 맨해튼 휴리스틱, w = 3
- 4방향 이웃 + 유클리드 휴리스틱, w = 3
정답 보기
② — 메카넘은 대각선으로 바로 갈 수 있으니 8방향이 맞고, 8방향에서 실제 거리를 넘지 않는(과대평가하지 않는) 휴리스틱은 유클리드입니다. ③ · ④ 는 휴리스틱을 부풀려 최단을 보장하지 못하고, ① 은 계단식 경로에 탐색도 넓습니다.2교시 · 코스트맵과 안전 여유#
로봇은 점이 아니다: 장애물 팽창#
1교시의 경로는 벽 바로 옆 칸을 스치듯 지나갑니다. 점이라면 괜찮지만 myAGV Plus 는 폭이 있는 몸체입니다. 경로를 따라 로봇 중심을 옮기면 옆면이 선반에 닿습니다.
해결은 간단합니다. 장애물을 로봇 반경만큼 부풀려(inflate) 놓고, 로봇은 다시 점으로 본다. 장애물 표면에서 거리 r 안의 칸을 모두 "통과 불가" 로 칠하면, 그 바깥으로 지나가는 점 경로는 반경 r 인 원형 로봇의 경로가 됩니다. 이것을 구성 공간(configuration space) 으로 바꾼다고 합니다.
Nav2 코스트맵의 비용 값#
ROS 2 의 Nav2 는 칸마다 0~255 의 비용(cost) 을 적은 코스트맵(costmap)을 씁니다. 팽창 레이어(inflation layer)가 장애물 주변의 비용을 정합니다.
| 비용 | 이름 | 뜻 | 플래너의 판단 |
|---|---|---|---|
| 254 | LETHAL | 장애물이 있는 칸 | 절대 못 감 |
| 253 | INSCRIBED | 로봇 중심이 여기 오면 몸이 반드시 장애물에 닿음 (거리 < 내접 반지름) | 못 감 |
| 1~252 | (팽창) | 닿을 수도 있는 띠. 장애물에 가까울수록 비쌈 | 갈 수 있지만 비용을 치름 → 가운데로 다니게 됨 |
| 0 | FREE | inflation_radius 바깥 | 비용 없음 |
| 255 | NO_INFORMATION | 미확인 칸 | 설정(allow_unknown)에 따라 다름 |
팽창 띠의 비용은 252 · exp(−cost_scaling_factor · (d − 내접 반지름)) 입니다(d = 장애물 표면까지 거리).
| 파라미터 | 뜻 | 키우면 | myagv.yaml (myAGV 용, Plus 는 확인 필요) |
|---|---|---|---|
robot_radius · footprint | 로봇 크기 → 내접 반지름(통과 불가 띠) | 좁은 길이 먼저 막힘 | 전역 robot_radius: 0.22, 지역 footprint 0.34 × 0.24 m |
inflation_radius | 비용 띠의 바깥 끝 | 더 멀리서부터 장애물을 피함 | 0.25 m |
cost_scaling_factor | 비용이 줄어드는 빠르기 | 비용이 빨리 줄어 장애물 가까이도 싸짐 | 5.0 |
inflation_radius 는 "통과 불가 거리" 가 아니라 비용이 0 이 되는 거리입니다. 경로가 아예 막히는 것은 로봇 반경(내접 반지름) 때문입니다. 반대로 cost_scaling_factor 는 키울수록 덜 조심합니다. 이름만 보고 거꾸로 조절하는 일이 흔합니다.
전역 코스트맵 vs 지역 코스트맵#
| 전역(global) 코스트맵 | 지역(local) 코스트맵 | |
|---|---|---|
| 범위 | 지도 전체 (ch07 에서 저장한 지도) | 로봇 주변 몇 m 창(window)이 로봇과 함께 움직임 (myagv.yaml: 3 m × 3 m rolling_window) |
| 주로 담는 것 | 정적 지도 + 센서로 본 장애물 + 팽창 | 지금 LiDAR 로 본 장애물 + 팽창 |
| 쓰는 곳 | 전역 플래너: 출발 → 목표 경로 (A* 등) | 지역 컨트롤러: 다음 순간의 속도 (DWA 등, 3교시) |
| 갱신 | 드물게 | 자주 |
전역 경로는 "어느 통로로 갈지" 를 정하고, 지역 컨트롤러는 "지금 걸어오는 사람을 어떻게 비킬지" 를 정합니다. 둘이 같은 팽창 규칙을 써야 전역 경로를 지역 컨트롤러가 따라갈 수 있습니다.
시뮬레이터 2 · 팽창 반경과 0.6 m 통로#
출발(S) 에서 목표(G) 까지 가장 짧은 길은 가운데 폭 0.6 m 통로입니다. 위 · 아래로 돌아가는 길은 길지만 더 넓습니다(가장 좁은 곳: 아래 0.8 m, 위 0.9 m). 보라색은 통과 불가 띠, 경로 위의 사각형은 그 자리에 놓인 로봇 발자국입니다.
- 팽창 반경 r = 0.10 m 에서 빈 로봇이 선반에 닿는 곳(빨간 점)이 있나요? r 을 0.01 씩 올리며 스침이 사라지는 r 을 찾고, 아래 그래프의 내접(초록) · 외접(주황) 선과 비교합니다.
- 예측: 폭 0.6 m 통로가 막혀 경로가 돌아가기 시작하는 r 은 몇 m 일까요? 이론값(0.30 m)과 시뮬레이터 값이 다르다면 이유는 무엇일까요?
- r 을 계속 올려 경로가 완전히 사라지는 r 을 찾습니다. 어느 길이 마지막까지 남았나요?
- 발자국을 상자 적재 0.50 × 0.40 으로 바꾸고 1 을 다시 합니다. 스침이 사라지는 r 은 통로가 닫히는 r 보다 큰가요, 작은가요? 그러면 이 짐을 싣고는 어느 길로 가야 하나요?
- myagv.yaml 값 버튼을 누른 뒤 비용 띠를 끄고 켜며 경로와 최소 여유를 비교합니다. cost_scaling_factor 를 1 → 20 으로 바꾸면 경로가 어떻게 바뀌나요?
- 빈 로봇(0.34 × 0.24 m)은 r = 0.18 m 부터 스침이 사라집니다. 내접 반지름(0.12)보다는 크고 외접 반지름(0.208)보다는 조금 작습니다. 직선 구간은 내접 반지름이면 충분하지만 모서리를 돌 때 몸의 모서리가 바깥으로 쓸고 지나가기 때문입니다.
- 0.6 m 통로는 r = 0.28 m 에서 막힙니다. 이론상 가운데 점까지 거리는 0.30 m 지만, 해상도 0.05 m 격자에는 정확히 가운데에 놓인 셀이 없어 가장 가운데 셀도 0.275 m 입니다. 해상도가 실제 통과 폭을 깎는다는 점을 기억하세요.
- r ≥ 0.43 m 이면 위쪽(0.9 m)도 막혀 경로 없음. 0.8 m 아래 길은 0.38 m 부근에서 먼저 막힙니다.
- 상자 적재(외접 0.32 m)는 r ≈ 0.28 m 가 되어야 스침이 사라지는데, 그 r 에서는 0.6 m 통로가 닫힙니다. 즉 이 짐을 싣고는 0.6 m 통로를 안전하게 못 지나갑니다 — 경로가 돌아가는 것이 맞는 결과입니다.
- 비용 띠를 켜면 경로가 선반에서 떨어져 통로 가운데로 다니고, 비용이 큰 0.6 m 통로를 피해 돌아가기도 합니다. cost_scaling_factor 를 키우면 비용이 빨리 줄어 다시 장애물 가까이를 지나갑니다.
발자국(footprint)과 적재물#
Nav2 에서 로봇 크기는 둘 중 하나로 적습니다.
robot_radius: 로봇을 원으로 봄. 계산이 간단하지만 길쭉한 로봇은 손해(외접원으로 잡으면 좁은 길이 막히고, 내접원으로 잡으면 모서리가 닿음).footprint: 로봇을 다각형으로 봄. 꼭짓점을[[x, y], ...]로 적습니다(로봇 중심 기준, x 앞 · y 왼쪽). 지역 컨트롤러가 후보 궤적마다 다각형을 놓아 보고 충돌을 검사합니다.
| 발자국 | 내접 반지름 | 외접 반지름 | 0.6 m 통로 |
|---|---|---|---|
| 빈 로봇 0.34 × 0.24 m | 0.12 m | 0.21 m | 원형(외접)으로 잡아도 통과 |
| 상자 0.50 × 0.40 m 를 실음 | 0.20 m | 0.32 m | 직진은 되지만 원형으로 잡으면 막힘 |
| 긴 판재 0.70 × 0.30 m | 0.15 m | 0.38 m | 회전에 0.76 m 원이 필요 |
짐이 차체보다 크면 발자국도 짐에 맞춰 늘려야 합니다. 차체 기준 발자국으로 계획하면 경로는 "로봇은 지나가는" 길을 고르고, 튀어나온 짐이 선반 모서리를 칩니다. 반대로 빈 로봇으로 돌아오면 발자국을 다시 줄여야 좁은 길을 씁니다(ch11 의 물류 시나리오에서 다시 다룹니다).
실습 과제 2#
시뮬레이터 2 의 창고를 numpy 격자로 만들고, 각 셀에서 장애물 표면까지 거리를 계산해 r 을 0.00 ~ 0.50 m 로 바꿔 가며 통로 · 경로가 막히는 r 을 찾습니다. 아래쪽에서는 Nav2 비용 곡선 표도 찍습니다. 예측 먼저: 통로 단면(x = 4.0 m) 12개 셀의 표면 거리 중 가장 큰 값은 0.30 m 일까요? 통로가 닫히는 r 과 경로가 사라지는 r 을 적어 두세요.
import math
from collections import deque
import numpy as np
# 시뮬레이터 2 의 창고 (8 m x 5 m, 해상도 0.05 m). 사각형 (x0, y0, x1, y1)
RES, W, H = 0.05, 8.0, 5.0
BOXES = [(0, 0, 8, 0.1), (0, 4.9, 8, 5), (0, 0, 0.1, 5), (7.9, 0, 8, 5), # 바깥 벽
(2.0, 2.8, 6.0, 3.6), (2.0, 1.4, 6.0, 2.2), # 선반 두 줄 -> 사이 0.6 m 통로
(3.8, 4.5, 4.2, 4.9), (3.8, 0.1, 4.2, 0.6)] # 위 · 아래 길을 좁히는 기둥
START, GOAL = (0.75, 1.0), (7.25, 4.0)
NX, NY = round(W / RES), round(H / RES)
occ = np.zeros((NY, NX), bool) # occ[j, i]: j = y 방향, i = x 방향
for x0, y0, x1, y1 in BOXES:
occ[math.floor(y0 / RES + 1e-6):math.ceil(y1 / RES - 1e-6),
math.floor(x0 / RES + 1e-6):math.ceil(x1 / RES - 1e-6)] = True
def distance_map(occ, max_d=0.7):
"""각 셀 중심에서 가장 가까운 장애물 '표면' 까지 거리 (m). max_d 보다 먼 곳은 max_d"""
n = int(max_d / RES) + 1
best = np.full(occ.shape, np.inf)
pad = np.pad(occ, n, constant_values=True) # 지도 밖은 벽으로 봄
for dj in range(-n, n + 1):
for di in range(-n, n + 1):
d = math.hypot(di, dj) * RES
if d > max_d + RES:
continue
shifted = pad[n + dj:n + dj + occ.shape[0], n + di:n + di + occ.shape[1]]
best = np.where(shifted & (d < best), d, best)
return np.minimum(np.maximum(best - RES / 2, 0), max_d) # 중심 거리 -> 표면 거리
def reachable(blocked, s, g):
"""4방향 BFS 로 s 에서 g 까지 갈 수 있는지"""
si, sj = int(s[0] / RES), int(s[1] / RES)
gi, gj = int(g[0] / RES), int(g[1] / RES)
if blocked[sj, si] or blocked[gj, gi]:
return False
seen = np.zeros_like(blocked)
seen[sj, si] = True
q = deque([(sj, si)])
while q:
j, i = q.popleft()
if (j, i) == (gj, gi):
return True
for dj, di in ((0, 1), (0, -1), (1, 0), (-1, 0)):
a, b = j + dj, i + di
if 0 <= a < NY and 0 <= b < NX and not blocked[a, b] and not seen[a, b]:
seen[a, b] = True
q.append((a, b))
return False
def corridor_open(blocked):
"""선반 사이 0.6 m 통로(x 2.0~6.0) 의 가운데 단면에 막히지 않은 셀이 있는지"""
i = round(4.0 / RES)
return not blocked[round(2.2 / RES):round(2.8 / RES), i].all()
D = distance_map(occ)
print("통로 단면 (x = 4.0 m) 셀별 표면 거리:", np.round(D[round(2.2 / RES):round(2.8 / RES), round(4.0 / RES)], 3))
print()
prev = None
for r100 in range(0, 51):
r = r100 / 100
blocked = occ.copy() # TODO 1: 표면 거리 D 가 r 보다 작은 셀도 막기 (D < r)
state = (corridor_open(blocked), reachable(blocked, START, GOAL))
if state != prev:
print("r = %.2f m : 0.6 m 통로 %s, 시작 -> 목표 경로 %s" %
(r, "열림" if state[0] else "막힘", "있음" if state[1] else "없음"))
prev = state
# Nav2 inflation_layer 의 비용 곡선 (r_ins = 막힘 띠, inflation_radius, cost_scaling_factor)
def nav2_cost(d, r_ins=0.22, r_inf=0.25, csf=5.0):
if d <= 0:
return 254 # LETHAL: 장애물 셀 자체
if d < r_ins:
return 253 # INSCRIBED: 로봇 중심이 오면 반드시 닿음
# TODO 2: d 가 r_inf 보다 크면 0, 아니면 int(252 * exp(-csf * (d - r_ins)))
return 0
print()
for r_inf, csf in [(0.25, 5.0), (0.55, 5.0), (0.55, 2.0)]:
row = ["%3d" % nav2_cost(d / 100, 0.22, r_inf, csf) for d in range(0, 61, 5)]
print("inflation_radius %.2f, csf %.1f :" % (r_inf, csf), " ".join(row))
print("(거리 0.00, 0.05, ..., 0.60 m)")풀이 코드 보기
import math
from collections import deque
import numpy as np
# 시뮬레이터 2 의 창고 (8 m x 5 m, 해상도 0.05 m). 사각형 (x0, y0, x1, y1)
RES, W, H = 0.05, 8.0, 5.0
BOXES = [(0, 0, 8, 0.1), (0, 4.9, 8, 5), (0, 0, 0.1, 5), (7.9, 0, 8, 5), # 바깥 벽
(2.0, 2.8, 6.0, 3.6), (2.0, 1.4, 6.0, 2.2), # 선반 두 줄 -> 사이 0.6 m 통로
(3.8, 4.5, 4.2, 4.9), (3.8, 0.1, 4.2, 0.6)] # 위 · 아래 길을 좁히는 기둥
START, GOAL = (0.75, 1.0), (7.25, 4.0)
NX, NY = round(W / RES), round(H / RES)
occ = np.zeros((NY, NX), bool) # occ[j, i]: j = y 방향, i = x 방향
for x0, y0, x1, y1 in BOXES:
occ[math.floor(y0 / RES + 1e-6):math.ceil(y1 / RES - 1e-6),
math.floor(x0 / RES + 1e-6):math.ceil(x1 / RES - 1e-6)] = True
def distance_map(occ, max_d=0.7):
"""각 셀 중심에서 가장 가까운 장애물 '표면' 까지 거리 (m). max_d 보다 먼 곳은 max_d"""
n = int(max_d / RES) + 1
best = np.full(occ.shape, np.inf)
pad = np.pad(occ, n, constant_values=True) # 지도 밖은 벽으로 봄
for dj in range(-n, n + 1):
for di in range(-n, n + 1):
d = math.hypot(di, dj) * RES
if d > max_d + RES:
continue
shifted = pad[n + dj:n + dj + occ.shape[0], n + di:n + di + occ.shape[1]]
best = np.where(shifted & (d < best), d, best)
return np.minimum(np.maximum(best - RES / 2, 0), max_d) # 중심 거리 -> 표면 거리
def reachable(blocked, s, g):
"""4방향 BFS 로 s 에서 g 까지 갈 수 있는지"""
si, sj = int(s[0] / RES), int(s[1] / RES)
gi, gj = int(g[0] / RES), int(g[1] / RES)
if blocked[sj, si] or blocked[gj, gi]:
return False
seen = np.zeros_like(blocked)
seen[sj, si] = True
q = deque([(sj, si)])
while q:
j, i = q.popleft()
if (j, i) == (gj, gi):
return True
for dj, di in ((0, 1), (0, -1), (1, 0), (-1, 0)):
a, b = j + dj, i + di
if 0 <= a < NY and 0 <= b < NX and not blocked[a, b] and not seen[a, b]:
seen[a, b] = True
q.append((a, b))
return False
def corridor_open(blocked):
"""선반 사이 0.6 m 통로(x 2.0~6.0) 의 가운데 단면에 막히지 않은 셀이 있는지"""
i = round(4.0 / RES)
return not blocked[round(2.2 / RES):round(2.8 / RES), i].all()
D = distance_map(occ)
print("통로 단면 (x = 4.0 m) 셀별 표면 거리:", np.round(D[round(2.2 / RES):round(2.8 / RES), round(4.0 / RES)], 3))
print()
prev = None
for r100 in range(0, 51):
r = r100 / 100
blocked = D < r # 팽창: 표면 거리가 r 보다 가까운 셀은 통과 불가
state = (corridor_open(blocked), reachable(blocked, START, GOAL))
if state != prev:
print("r = %.2f m : 0.6 m 통로 %s, 시작 -> 목표 경로 %s" %
(r, "열림" if state[0] else "막힘", "있음" if state[1] else "없음"))
prev = state
# Nav2 inflation_layer 의 비용 곡선 (r_ins = 막힘 띠, inflation_radius, cost_scaling_factor)
def nav2_cost(d, r_ins=0.22, r_inf=0.25, csf=5.0):
if d <= 0:
return 254 # LETHAL: 장애물 셀 자체
if d < r_ins:
return 253 # INSCRIBED: 로봇 중심이 오면 반드시 닿음
if d > r_inf:
return 0
return int(252 * math.exp(-csf * (d - r_ins)))
print()
for r_inf, csf in [(0.25, 5.0), (0.55, 5.0), (0.55, 2.0)]:
row = ["%3d" % nav2_cost(d / 100, 0.22, r_inf, csf) for d in range(0, 61, 5)]
print("inflation_radius %.2f, csf %.1f :" % (r_inf, csf), " ".join(row))
print("(거리 0.00, 0.05, ..., 0.60 m)")실행 결과:
통로 단면 (x = 4.0 m) 셀별 표면 거리: [0.025 0.075 0.125 0.175 0.225 0.275 0.275 0.225 0.175 0.125 0.075 0.025]
r = 0.00 m : 0.6 m 통로 열림, 시작 -> 목표 경로 있음
r = 0.28 m : 0.6 m 통로 막힘, 시작 -> 목표 경로 있음
r = 0.43 m : 0.6 m 통로 막힘, 시작 -> 목표 경로 없음
inflation_radius 0.25, csf 5.0 : 254 253 253 253 253 216 0 0 0 0 0 0 0
inflation_radius 0.55, csf 5.0 : 254 253 253 253 253 216 168 131 102 79 62 48 0
inflation_radius 0.55, csf 2.0 : 254 253 253 253 253 237 214 194 175 159 143 130 0
(거리 0.00, 0.05, ..., 0.60 m)- 통로 단면의 가장 큰 표면 거리는 0.275 m 입니다(셀 중심이 통로 가운데 0.30 m 에 오지 않음). 그래서 r = 0.28 m 에서 닫혀 시뮬레이터 2 와 같은 값이 나옵니다.
- 해상도를 0.025 m 로 바꾸면 가장 가운데 셀이 0.2875 m 가 되어 더 큰 r 까지 버팁니다. 지도 해상도와 팽창 반경은 함께 정해야 합니다.
- 비용 표: inflation_radius 를 0.25 → 0.55 m 로 늘리면 비용 띠가 넓어지고, csf 를 5 → 2 로 줄이면 같은 거리에서 비용이 더 큽니다(더 조심함).
발자국 다각형 네 가지의 내접 · 외접 반지름을 계산하고, 0.6 m 통로에서 어떤 r 을 쓸지 판단합니다. 예측 먼저: "앞으로 튀어나온 짐" 은 빈 로봇과 폭이 같습니다. 직진 여유는 같을까요? 외접 반지름은 몇 배가 될까요?
import math
# 발자국(footprint): 로봇 중심 기준 꼭짓점 (x 앞, y 왼쪽), 단위 m
FOOTPRINTS = {
"빈 로봇 0.34 x 0.24": [(0.17, 0.12), (0.17, -0.12), (-0.17, -0.12), (-0.17, 0.12)],
"상자 적재 0.50 x 0.40": [(0.25, 0.20), (0.25, -0.20), (-0.25, -0.20), (-0.25, 0.20)],
"긴 판재 0.70 x 0.30": [(0.35, 0.15), (0.35, -0.15), (-0.35, -0.15), (-0.35, 0.15)],
"앞으로 튀어나온 짐": [(0.40, 0.12), (0.40, -0.12), (-0.17, -0.12), (-0.17, 0.12)],
}
def point_seg(px, py, a, b):
dx, dy = b[0] - a[0], b[1] - a[1]
t = max(0.0, min(1.0, ((px - a[0]) * dx + (py - a[1]) * dy) / (dx * dx + dy * dy)))
return math.hypot(px - a[0] - t * dx, py - a[1] - t * dy)
def radii(poly):
"""내접 반지름(중심에서 가장 가까운 변) · 외접 반지름(가장 먼 꼭짓점)"""
edges = list(zip(poly, poly[1:] + poly[:1])) # (꼭짓점, 다음 꼭짓점) 짝 = 변
# TODO 1: 내접 반지름 = 원점 (0, 0) 에서 각 변까지 거리(point_seg) 중 가장 작은 값
# TODO 2: 외접 반지름 = 원점에서 각 꼭짓점까지 거리 중 가장 큰 값
r_in, r_out = 0.0, 0.0
return r_in, r_out
CORRIDOR = 0.60
CLOSE_R = 0.28 # 실습 2-1 · 시뮬레이터 2 에서 0.6 m 통로가 막히기 시작한 팽창 반경
for name, poly in FOOTPRINTS.items():
r_in, r_out = radii(poly)
width = max(y for _, y in poly) - min(y for _, y in poly)
margin = (CORRIDOR - width) / 2 # 통로 가운데로 똑바로 갈 때 한쪽 여유
if r_out < CLOSE_R:
verdict = "r = 외접 %.2f 로 잡아도 0.6 m 통로가 열려 있음 -> 원형으로 단순하게" % r_out
elif width < CORRIDOR:
verdict = "r = 외접이면 통로가 닫힘 -> r 은 내접 근처로, 충돌은 발자국 다각형으로 검사"
else:
verdict = "폭이 통로보다 넓음 -> 이 통로는 지나갈 수 없음"
print("[%s] 내접 %.3f m, 외접 %.3f m, 폭 %.2f m, 직진 시 한쪽 여유 %.3f m" % (name, r_in, r_out, width, margin))
print(" ", verdict)풀이 코드 보기
import math
# 발자국(footprint): 로봇 중심 기준 꼭짓점 (x 앞, y 왼쪽), 단위 m
FOOTPRINTS = {
"빈 로봇 0.34 x 0.24": [(0.17, 0.12), (0.17, -0.12), (-0.17, -0.12), (-0.17, 0.12)],
"상자 적재 0.50 x 0.40": [(0.25, 0.20), (0.25, -0.20), (-0.25, -0.20), (-0.25, 0.20)],
"긴 판재 0.70 x 0.30": [(0.35, 0.15), (0.35, -0.15), (-0.35, -0.15), (-0.35, 0.15)],
"앞으로 튀어나온 짐": [(0.40, 0.12), (0.40, -0.12), (-0.17, -0.12), (-0.17, 0.12)],
}
def point_seg(px, py, a, b):
dx, dy = b[0] - a[0], b[1] - a[1]
t = max(0.0, min(1.0, ((px - a[0]) * dx + (py - a[1]) * dy) / (dx * dx + dy * dy)))
return math.hypot(px - a[0] - t * dx, py - a[1] - t * dy)
def radii(poly):
"""내접 반지름(중심에서 가장 가까운 변) · 외접 반지름(가장 먼 꼭짓점)"""
edges = zip(poly, poly[1:] + poly[:1])
r_in = min(point_seg(0, 0, a, b) for a, b in edges)
r_out = max(math.hypot(x, y) for x, y in poly)
return r_in, r_out
CORRIDOR = 0.60
CLOSE_R = 0.28 # 실습 2-1 · 시뮬레이터 2 에서 0.6 m 통로가 막히기 시작한 팽창 반경
for name, poly in FOOTPRINTS.items():
r_in, r_out = radii(poly)
width = max(y for _, y in poly) - min(y for _, y in poly)
margin = (CORRIDOR - width) / 2 # 통로 가운데로 똑바로 갈 때 한쪽 여유
if r_out < CLOSE_R:
verdict = "r = 외접 %.2f 로 잡아도 0.6 m 통로가 열려 있음 -> 원형으로 단순하게" % r_out
elif width < CORRIDOR:
verdict = "r = 외접이면 통로가 닫힘 -> r 은 내접 근처로, 충돌은 발자국 다각형으로 검사"
else:
verdict = "폭이 통로보다 넓음 -> 이 통로는 지나갈 수 없음"
print("[%s] 내접 %.3f m, 외접 %.3f m, 폭 %.2f m, 직진 시 한쪽 여유 %.3f m" % (name, r_in, r_out, width, margin))
print(" ", verdict)실행 결과:
[빈 로봇 0.34 x 0.24] 내접 0.120 m, 외접 0.208 m, 폭 0.24 m, 직진 시 한쪽 여유 0.180 m
r = 외접 0.21 로 잡아도 0.6 m 통로가 열려 있음 -> 원형으로 단순하게
[상자 적재 0.50 x 0.40] 내접 0.200 m, 외접 0.320 m, 폭 0.40 m, 직진 시 한쪽 여유 0.100 m
r = 외접이면 통로가 닫힘 -> r 은 내접 근처로, 충돌은 발자국 다각형으로 검사
[긴 판재 0.70 x 0.30] 내접 0.150 m, 외접 0.381 m, 폭 0.30 m, 직진 시 한쪽 여유 0.150 m
r = 외접이면 통로가 닫힘 -> r 은 내접 근처로, 충돌은 발자국 다각형으로 검사
[앞으로 튀어나온 짐] 내접 0.120 m, 외접 0.418 m, 폭 0.24 m, 직진 시 한쪽 여유 0.180 m
r = 외접이면 통로가 닫힘 -> r 은 내접 근처로, 충돌은 발자국 다각형으로 검사- "앞으로 튀어나온 짐" 은 폭이 같아 직진 여유는 빈 로봇과 같지만, 외접 반지름이 0.21 → 0.42 m 로 두 배가 됩니다. 회전 중심이 짐의 가운데가 아니라 로봇 중심이기 때문에, 제자리 회전 · 코너에서 짐 끝이 크게 휘둘립니다.
- 길쭉하거나 비대칭인 발자국은 원 하나로 나타내기 어렵습니다. 그래서 Nav2 는 지역 컨트롤러에서
footprint다각형으로 충돌을 검사합니다.
빈 myAGV Plus(교육용 발자국 0.34 × 0.24 m)가 폭 0.6 m 통로가 있는 창고를 다닙니다(해상도 0.05 m). 전역 코스트맵의 통과 불가 반경(로봇 반경)으로 가장 알맞은 값은?
- 0.05 m — 경로가 가장 짧아지도록
- 0.22 m — 외접 반지름(0.21 m)보다 조금 크게
- 0.30 m — 통로 폭의 절반
- 0.45 m — 넉넉하게
정답 보기
② — 외접 반지름보다 조금 크면 어느 방향으로 돌아도 닿지 않으면서, 0.6 m 통로(이 해상도에서 0.28 m 부터 닫힘)는 열려 있습니다. myagv.yaml 의 `robot_radius: 0.22` 도 같은 생각입니다. ① 은 모서리에서 스치고, ③ 은 통로가 닫히며, ④ 는 위 · 아래 길까지 모두 막혀 경로가 없습니다.시뮬레이터 2 에서 상자 적재(0.50 × 0.40 m) 발자국으로 r 을 0.22 m 로 두었더니 경로는 0.6 m 통로를 지나지만 빨간 점(스침)이 생겼습니다. 가장 알맞은 조치는?
- inflation_radius 를 0.25 → 1.0 m 로 늘린다
- cost_scaling_factor 를 20 으로 올린다
- 짐에 맞게 발자국 · 반경을 늘려, 0.6 m 통로 대신 넓은 길로 돌아가게 한다
- 해상도를 0.2 m 로 거칠게 바꾼다
정답 보기
③ — 짐의 외접 반지름(0.32 m)에 맞춰 반경을 늘리면 0.6 m 통로가 닫히고 넓은 길로 돌아가며, 이것이 안전한 결과입니다. ① 은 비용 띠만 넓힐 뿐 통과 불가 영역은 그대로라 비용을 감수하고 좁은 통로를 지나는 경로가 남을 수 있고, ② 는 비용이 빨리 줄어 오히려 덜 조심합니다. ④ 는 지도가 거칠어져 통로 폭을 더 부정확하게 만듭니다.3교시 · 경로 추종과 지역 회피: Pure Pursuit 와 DWA#
계획한 경로를 "따라가는" 문제#
A* 경로는 칸 중심을 이은 꺾은선입니다. 로봇은 이것을 그대로 따라갈 수 없습니다.
- 꺾인 점에서 순간적으로 방향을 바꿀 수 없습니다(가속 한계 · 관성).
- 위치 추정(ch06)에 오차가 있어 로봇이 경로 옆에 있을 때가 많습니다.
- 계획한 뒤에 사람이 걸어 들어오는 등 세상이 바뀝니다.
그래서 내비게이션은 전역 계획(어느 길로) 과 지역 제어(지금 어떤 속도로) 를 나눕니다. 지역 제어의 대표 두 가지가 Pure Pursuit(경로 추종) 와 DWA(지역 회피) 입니다.
Pure Pursuit: 앞쪽 점을 쫓아가기#
lookahead 점: 경로 위에서 로봇과 거리 L 인 점
α = 로봇 정면과 lookahead 점 방향 사이 각
κ = 2 · sin α / L (그 점을 지나는 원호의 곡률, 1/m)
ω = v · κ (v 는 정한 직선 속도, ω 는 최대값으로 자름)- L 이 짧으면: 경로에 바짝 붙어 따라가지만 α 가 조금만 변해도 ω 가 크게 변합니다. 제어 지연(명령이 늦게 적용됨)이 있으면 좌우로 흔들립니다.
- L 이 길면: 부드럽지만 코너 훨씬 앞에서 꺾기 시작해 코너 안쪽을 잘라 갑니다. 선반 모서리 쪽으로 붙는다는 뜻입니다.
- 보통 L 을 속도에 비례하게(예: L = 1~2 초 동안 가는 거리) 잡고 최소 · 최대값으로 자릅니다. Nav2 에도 이 방식의 컨트롤러(Regulated Pure Pursuit)가 있습니다.
메카넘 로봇은 몸을 돌리지 않고 lookahead 점 방향으로 속도 벡터를 바로 낼 수도 있습니다(4교시 시뮬레이터가 이 방식). 여기서는 회전으로 방향을 바꾸는 고전 방식으로 lookahead 의 효과를 봅니다.
시뮬레이터 3 · lookahead 거리와 궤적 모양#
꺾인 점이 4개 있는 경로를 로봇이 따라갑니다. 로봇은 경로보다 0.3 m 옆에서 출발하고, 명령은 한 주기(100 ms) 늦게 적용됩니다. 아래 그래프는 각속도 명령 ω 입니다.
- L = 0.1 을 누르기 전에 궤적과 ω 그래프가 어떻게 생길지 그려 보세요. 확인한 뒤 ▶ 재생 으로 lookahead 원이 움직이는 모습을 봅니다.
- L = 0.1 → 0.3 → 0.8 → 1.4 로 바꾸며 표의 "흔들림" 과 "코너 자르기" 를 기록합니다. 두 값이 모두 작은 L 은 어디쯤인가요?
- L = 0.1 에서 제어 지연 을 1 → 0 으로 줄여 봅니다. 흔들림이 줄어든다면, 흔들림의 원인은 L 하나뿐일까요?
- L = 0.3 에서 속도 v 를 0.3 → 1.0 m/s 로 올립니다. 같은 L 이 빠를 때도 괜찮은가요? 속도에 맞춰 L 을 바꿔야 하는 이유를 적어 보세요.
- L = 1.4 에서 코너 거리(분홍 글자)가 0.45 m 를 넘습니다. 실제 창고에서 이 경로 옆이 선반이라면 무슨 일이 생길까요?
- v = 0.6 m/s · 지연 1주기 기준: L = 0.1 은 ω 가 좌우로 37번 바뀌며 최대 29 cm 벗어나고 도착도 30 s 로 늦습니다. L = 0.3 은 흔들림 · 이탈(6 cm)이 작고, L = 0.8 · 1.4 는 흔들림이 없지만 코너에서 27 · 46 cm 떨어진 곳으로 잘라 갑니다.
- 지연을 0 으로 줄이면 L = 0.1 의 흔들림이 크게 줄어듭니다. 흔들림은 짧은 L(큰 이득) + 지연 이 함께 만듭니다. 실물은 통신 · 모터 응답 때문에 지연이 항상 있습니다.
- 같은 L = 0.3 이라도 v = 1.0 m/s 에서는 다시 흔들립니다(최대 이탈 26 cm). 빠를수록 한 주기에 가는 거리가 길어 같은 L 이 상대적으로 짧아지기 때문입니다. 그래서 L 을 속도에 비례하게 잡습니다.
- L 이 길면 시간이 조금 줄지만(코너를 잘라 경로가 짧아짐), 그 대가는 안전 여유입니다. 2교시의 팽창 띠 안으로 궤적이 들어갈 수 있습니다.
DWA: 짧게 굴려 보고 점수로 고르기#
Pure Pursuit 는 경로만 보고 장애물은 보지 않습니다. 계획에 없던 사람이 나타나면 DWA(Dynamic Window Approach) 처럼 장애물을 직접 보는 지역 계획이 필요합니다.
- 동적 창(dynamic window): 가속 한계 때문에 0.1 s 뒤에 낼 수 있는 속도는 지금 속도 ± 가속도 × 0.1 s 안뿐입니다. 이 창 안에서 (vx, vy, ω) 후보를 몇 개씩 뽑습니다.
- 굴려 보기(rollout): 후보 속도를 1.5 s 동안 유지한다고 보고 궤적을 계산합니다(myagv.yaml 의
sim_time: 1.7). - 거르기: 궤적이 장애물에 닿거나, 장애물 앞에서 제때 설 수 없는(속도 > √(2 · 감속도 · 여유 거리)) 후보는 버립니다.
- 점수: 남은 후보마다
α · 목표 진행 + β · 장애물 여유 + γ · 속도를 계산해 가장 높은 것을 고릅니다. - 0.1 s 만 실행하고 다시 1 로 돌아갑니다.
Nav2 의 DWB 컨트롤러도 같은 틀이며, 점수 항목을 critic 이라고 부릅니다(myagv.yaml: BaseObstacle · PathAlign · PathDist · GoalAlign · GoalDist · RotateToGoal · Oscillation). 실제 DWB 는 점수가 낮을수록 좋은 "비용" 으로 계산합니다.
메카넘의 장점: 측면 속도 vy 후보#
차동 구동 로봇의 후보는 (v, ω) 2차원이라, 앞에 사람이 서 있으면 몸을 돌려야 비킬 수 있습니다. 메카넘 로봇은 (vx, vy, ω) 3차원이라 앞을 보고 선 채로 옆으로 미끄러지듯 비킬 수 있습니다.
- 후보가 많아집니다(이 시뮬레이터에서 35개 → 175개). 계산은 늘지만 "부딪히지 않는 후보" 가 훨씬 많아집니다.
- 비키는 동안에도 정면이 목표를 향하므로, 비킨 뒤 다시 방향을 돌릴 필요가 없습니다.
- 그런데 myagv.yaml 은
max_vel_y: 0.0으로 측면 이동을 쓰지 않습니다(AMCL 도differential). 메카넘의 장점을 쓰려면 이런 값을 바꿔야 한다는 뜻이며, 실제 설정은 ch10 에서 다룹니다.
시뮬레이터 4 · 사람 혼잡 구역에서 DWA#
걷는 사람 6명(주황)과 서 있는 작업자 2명(보라)이 있는 10 m × 6 m 구역을 왼쪽에서 오른쪽 목표(G)까지 지나갑니다. 사람은 로봇이 아주 가까워지면 조금 비켜서거나 멈춰 기다립니다. 매 순간 그려지는 선들이 DWA 후보 궤적입니다.
- 예측: 장애물 거리 가중치 β 를 0.5 → 2.0 으로 키우면 도착 시간은 늘까요, 줄까요? 사람과의 최소 간격은요? 적어 둔 뒤 각각 📊 배치 1~5 평균 으로 확인합니다.
- β = 0.5 에서 ▶ 재생을 누르고 빨간 후보(버려진 것)가 많아지는 순간을 찾습니다. 그때 로봇은 무엇을 하나요?
- 측면 속도 vy 후보 넣기 를 켜고 1 의 비교를 다시 합니다. 서 있는 작업자를 지날 때 로봇의 방향(빨간 삼각형)이 어떻게 다른가요?
- α(목표)를 0 으로 내리면 어떻게 될까요? γ(속도)를 0 으로 내리면요?
- 사람 이동 예측 을 켜면 후보를 거를 때 사람이 1.5 s 동안 걸어갈 자리까지 봅니다(주황 점선). 0.3 m 안으로 가까웠던 시간이 어떻게 바뀌나요?
- β 를 0.5 → 2.0 으로 키우면 배치 1~5 평균 도착 시간이 약 21 s → 39 s 로 늘어납니다. 사람이 없는 빈 곳을 선호하느라 목표 쪽 진행을 미루고, 사람이 지나갈 때까지 기다리거나 크게 돌아가기 때문입니다.
- 그런데 최소 간격은 늘지 않습니다(배치 1~5 평균 약 7 cm → 4 cm 로 오히려 조금 줄었습니다). 혼잡 구역에 오래 머물수록 사람과 마주칠 기회도 늘기 때문입니다. "더 조심하게" 만든 설정이 꼭 "더 안전한" 결과를 내지는 않습니다.
- vy 후보를 켜면 평균 도착 시간이 줄고(약 19 s) 정면을 유지한 채 옆으로 비킵니다. vy + 이동 예측을 함께 켜면 0.3 m 안으로 다가간 시간이 가장 짧았습니다.
- α = 0 이면 목표로 가려는 이유가 없어 대부분 60 s 안에 도착하지 못합니다. γ = 0 은 이 시뮬레이터에서 큰 차이가 없습니다. 목표 진행 점수가 이미 "빨리 다가가는" 후보를 좋아하기 때문입니다 — 점수 항목끼리 겹치는 역할이 있다는 뜻입니다.
- 사람 쪽에서 로봇에게 걸어오는 경우는 정적 장애물로 보는 DWA 가 늦게 반응합니다. 이동 예측이 이 문제를 줄이지만, 예측이 틀리면(사람이 갑자기 방향을 바꾸면) 다시 늦습니다.
실습 과제 3#
시뮬레이터 3 과 같은 경로에서 L 을 0.1 ~ 1.2 m 로 바꿔 도착 시간 · 최대 이탈 · 좌우 바뀜 · 코너 거리를 표로 만듭니다. TODO 두 곳(lookahead 점 찾기, 각속도 계산)을 채우세요. 예측 먼저: 최대 이탈이 가장 작은 L 은 0.1 일까요? 그래프를 그리면 어떤 모양일까요?
import math
# 시뮬레이터 3 과 같은 경로 (꺾은선) 를 0.05 m 간격 점으로
CORNERS = [(0.5, 0.8), (5.5, 0.8), (5.5, 3.2), (2.0, 3.2), (2.0, 2.0), (4.0, 2.0)]
PATH = [CORNERS[0]]
for (ax, ay), (bx, by) in zip(CORNERS, CORNERS[1:]):
n = math.ceil(math.hypot(bx - ax, by - ay) / 0.05)
PATH += [(ax + (bx - ax) * k / n, ay + (by - ay) * k / n) for k in range(1, n + 1)]
def wrap(a):
return (a + math.pi) % (2 * math.pi) - math.pi
def seg_dist(px, py, a, b):
dx, dy = b[0] - a[0], b[1] - a[1]
L2 = dx * dx + dy * dy
t = max(0.0, min(1.0, ((px - a[0]) * dx + (py - a[1]) * dy) / L2)) if L2 else 0.0
return math.hypot(px - a[0] - t * dx, py - a[1] - t * dy)
def pure_pursuit(L, v=0.6, w_max=2.5, dt=0.1, delay=1, x=0.5, y=1.1, th=0.0, T=80):
"""차동 구동 방식 Pure Pursuit. delay: 명령이 몇 주기 늦게 적용되는지"""
queue, traj, idx, t, flips, last = [0.0] * delay, [(x, y)], 0, 0.0, 0, 0
end = PATH[-1]
while t < T:
d_end = math.hypot(end[0] - x, end[1] - y)
if d_end < 0.05 and idx >= len(PATH) - 2:
break
# 1) 경로에서 지금 가장 가까운 구간 (앞쪽 40개만 봄)
best = float("inf")
for i in range(idx, min(len(PATH) - 1, idx + 40)):
d = seg_dist(x, y, PATH[i], PATH[i + 1])
if d < best - 1e-9:
best, idx = d, i
# 2) 그 뒤에서 처음으로 L 이상 떨어진 점 = lookahead 점
lx, ly = end
# TODO 1: PATH[idx + 1:] 를 차례로 보며 로봇에서 L 이상 떨어진 첫 점을 (lx, ly) 로
# 3) 곡률 = 2 sin(alpha) / Ld, 각속도 = v * 곡률
alpha = wrap(math.atan2(ly - y, lx - x) - th)
Ld = max(1e-3, math.hypot(lx - x, ly - y))
vv = min(v, max(0.05, d_end))
w = 0.0 # TODO 2: v * 2 sin(alpha) / Ld 를 +-w_max 로 자르기
queue.append(w)
w = queue.pop(0) # 제어 지연
if abs(w) > 0.05:
s = 1 if w > 0 else -1
flips += (last != 0 and s != last)
last = s
th += w * dt
x += vv * math.cos(th) * dt
y += vv * math.sin(th) * dt
t += dt
traj.append((x, y))
err = [min(seg_dist(px, py, a, b) for a, b in zip(PATH, PATH[1:])) for px, py in traj]
late = err[int(3 / dt):] # 출발 3 s 뒤부터 (처음 0.3 m 어긋남 제외)
cut = [min(math.hypot(px - cx, py - cy) for px, py in traj) for cx, cy in CORNERS[1:-1]]
return t, max(late), flips, sum(cut) / len(cut)
print("%6s %8s %12s %10s %12s" % ("L(m)", "시간(s)", "최대이탈(m)", "좌우바뀜", "코너거리(m)"))
for L in [0.1, 0.2, 0.3, 0.5, 0.8, 1.2]:
t, emax, flips, cut = pure_pursuit(L)
print("%6.1f %8.1f %12.3f %10d %12.3f" % (L, t, emax, flips, cut))풀이 코드 보기
import math
# 시뮬레이터 3 과 같은 경로 (꺾은선) 를 0.05 m 간격 점으로
CORNERS = [(0.5, 0.8), (5.5, 0.8), (5.5, 3.2), (2.0, 3.2), (2.0, 2.0), (4.0, 2.0)]
PATH = [CORNERS[0]]
for (ax, ay), (bx, by) in zip(CORNERS, CORNERS[1:]):
n = math.ceil(math.hypot(bx - ax, by - ay) / 0.05)
PATH += [(ax + (bx - ax) * k / n, ay + (by - ay) * k / n) for k in range(1, n + 1)]
def wrap(a):
return (a + math.pi) % (2 * math.pi) - math.pi
def seg_dist(px, py, a, b):
dx, dy = b[0] - a[0], b[1] - a[1]
L2 = dx * dx + dy * dy
t = max(0.0, min(1.0, ((px - a[0]) * dx + (py - a[1]) * dy) / L2)) if L2 else 0.0
return math.hypot(px - a[0] - t * dx, py - a[1] - t * dy)
def pure_pursuit(L, v=0.6, w_max=2.5, dt=0.1, delay=1, x=0.5, y=1.1, th=0.0, T=80):
"""차동 구동 방식 Pure Pursuit. delay: 명령이 몇 주기 늦게 적용되는지"""
queue, traj, idx, t, flips, last = [0.0] * delay, [(x, y)], 0, 0.0, 0, 0
end = PATH[-1]
while t < T:
d_end = math.hypot(end[0] - x, end[1] - y)
if d_end < 0.05 and idx >= len(PATH) - 2:
break
# 1) 경로에서 지금 가장 가까운 구간 (앞쪽 40개만 봄)
best = float("inf")
for i in range(idx, min(len(PATH) - 1, idx + 40)):
d = seg_dist(x, y, PATH[i], PATH[i + 1])
if d < best - 1e-9:
best, idx = d, i
# 2) 그 뒤에서 처음으로 L 이상 떨어진 점 = lookahead 점
lx, ly = end
for px, py in PATH[idx + 1:]:
if math.hypot(px - x, py - y) >= L:
lx, ly = px, py
break
# 3) 곡률 = 2 sin(alpha) / Ld, 각속도 = v * 곡률
alpha = wrap(math.atan2(ly - y, lx - x) - th)
Ld = max(1e-3, math.hypot(lx - x, ly - y))
vv = min(v, max(0.05, d_end))
w = max(-w_max, min(w_max, vv * 2 * math.sin(alpha) / Ld))
queue.append(w)
w = queue.pop(0) # 제어 지연
if abs(w) > 0.05:
s = 1 if w > 0 else -1
flips += (last != 0 and s != last)
last = s
th += w * dt
x += vv * math.cos(th) * dt
y += vv * math.sin(th) * dt
t += dt
traj.append((x, y))
err = [min(seg_dist(px, py, a, b) for a, b in zip(PATH, PATH[1:])) for px, py in traj]
late = err[int(3 / dt):] # 출발 3 s 뒤부터 (처음 0.3 m 어긋남 제외)
cut = [min(math.hypot(px - cx, py - cy) for px, py in traj) for cx, cy in CORNERS[1:-1]]
return t, max(late), flips, sum(cut) / len(cut)
print("%6s %8s %12s %10s %12s" % ("L(m)", "시간(s)", "최대이탈(m)", "좌우바뀜", "코너거리(m)"))
for L in [0.1, 0.2, 0.3, 0.5, 0.8, 1.2]:
t, emax, flips, cut = pure_pursuit(L)
print("%6.1f %8.1f %12.3f %10d %12.3f" % (L, t, emax, flips, cut))실행 결과:
L(m) 시간(s) 최대이탈(m) 좌우바뀜 코너거리(m)
0.1 29.5 0.349 42 0.039
0.2 25.6 0.125 32 0.040
0.3 24.6 0.065 10 0.090
0.5 24.0 0.118 1 0.173
0.8 23.3 0.197 1 0.272
1.2 22.4 0.310 1 0.400- 최대 이탈은 L = 0.3 m 에서 가장 작고 양쪽으로 커지는 U자 모양입니다. 짧은 쪽은 흔들림(좌우 바뀜 42회) 때문에, 긴 쪽은 코너 자르기 때문에 커집니다.
- lookahead 점을 경로 점 중에서 고르는 단순 방식이라 시뮬레이터 3(원과 경로의 교점을 계산)과 숫자가 조금 다르지만 경향은 같습니다.
정면 1.0 m(조금 오른쪽)에 사람이 서 있는 상황에서 DWA 후보를 만들고 점수를 매겨 하나를 고릅니다. vy 후보를 켜고 끄며, β 를 0 · 0.5 · 2.0 으로 바꿉니다. TODO 의 점수 세 개를 채우세요. 예측 먼저: vy 후보를 켜면 통과하는 후보 수는 몇 배가 될까요? β 를 키우면 고른 vx 는 커질까요, 작아질까요?
import math
import numpy as np
# 상황: 로봇 (0, 0), 정면 +x, 지금 속도 vx 0.3 m/s. 목표는 4 m 앞.
# 정면 1.0 m (조금 오른쪽) 에 사람이 서 있음, 오른쪽 1.0 m 에 선반 벽 (y = -1.0)
R_ROBOT, R_PERSON = 0.2, 0.25
GOAL = (4.0, 0.0)
PEOPLE = [(1.0, -0.1)]
WALL_Y = -1.0
V_MAX, VY_MAX, W_MAX = 0.5, 0.35, 1.5
ACC, ACC_W, DT, T = 1.0, 3.0, 0.1, 1.5
v_now = (0.3, 0.0, 0.0) # (vx, vy, w) 지금 속도
def rollout(vx, vy, w):
"""(vx, vy, w) 를 T 초 유지했을 때 궤적의 끝점과 최소 여유 (m)"""
x = y = th = 0.0
clear = float("inf")
for _ in range(round(T / DT)):
x += (vx * math.cos(th) - vy * math.sin(th)) * DT
y += (vx * math.sin(th) + vy * math.cos(th)) * DT
th += w * DT
clear = min(clear, y - WALL_Y - R_ROBOT)
for px, py in PEOPLE:
clear = min(clear, math.hypot(px - x, py - y) - R_ROBOT - R_PERSON)
return x, y, clear
def window(c, a, lo, hi, n):
"""동적 창: 지금 값 c 에서 한 주기(DT) 동안 바꿀 수 있는 범위를 n 개로 나눔"""
return np.linspace(max(lo, c - a * DT), min(hi, c + a * DT), n)
def dwa(alpha, beta, gamma, use_vy):
d0 = math.dist((0, 0), GOAL)
vys = window(v_now[1], ACC, -VY_MAX, VY_MAX, 5) if use_vy else [0.0]
cands = []
for vx in window(v_now[0], ACC, -0.1, V_MAX, 5):
for vy in vys:
for w in window(v_now[2], ACC_W, -W_MAX, W_MAX, 7):
x, y, clear = rollout(vx, vy, w)
speed = math.hypot(vx, vy)
if clear <= 0 or speed > math.sqrt(2 * ACC * clear):
continue # 부딪히거나 제때 못 서는 후보는 버림
# TODO: 세 점수를 계산합니다
# G = (d0 - 끝점과 GOAL 사이 거리) / (V_MAX * T) 목표 쪽으로 얼마나 다가가나
# C = min(clear, 1.0) 장애물 여유 (1 m 에서 자름)
# S = speed / V_MAX 빠를수록
G = C = S = 0.0
cands.append((alpha * G + beta * C + gamma * S, vx, vy, w, clear))
cands.sort(reverse=True)
return cands
for use_vy in [False, True]:
for beta in [0.0, 0.5, 2.0]:
c = dwa(1.0, beta, 0.3, use_vy)
s, vx, vy, w, clear = c[0]
print("vy 후보 %-3s beta %.1f : 후보 %3d개 통과 -> vx %.2f vy %+.2f w %+.2f (여유 %.2f m, 점수 %.2f)"
% ("on" if use_vy else "off", beta, len(c), vx, vy, w, clear, s))풀이 코드 보기
import math
import numpy as np
# 상황: 로봇 (0, 0), 정면 +x, 지금 속도 vx 0.3 m/s. 목표는 4 m 앞.
# 정면 1.0 m (조금 오른쪽) 에 사람이 서 있음, 오른쪽 1.0 m 에 선반 벽 (y = -1.0)
R_ROBOT, R_PERSON = 0.2, 0.25
GOAL = (4.0, 0.0)
PEOPLE = [(1.0, -0.1)]
WALL_Y = -1.0
V_MAX, VY_MAX, W_MAX = 0.5, 0.35, 1.5
ACC, ACC_W, DT, T = 1.0, 3.0, 0.1, 1.5
v_now = (0.3, 0.0, 0.0) # (vx, vy, w) 지금 속도
def rollout(vx, vy, w):
"""(vx, vy, w) 를 T 초 유지했을 때 궤적의 끝점과 최소 여유 (m)"""
x = y = th = 0.0
clear = float("inf")
for _ in range(round(T / DT)):
x += (vx * math.cos(th) - vy * math.sin(th)) * DT
y += (vx * math.sin(th) + vy * math.cos(th)) * DT
th += w * DT
clear = min(clear, y - WALL_Y - R_ROBOT)
for px, py in PEOPLE:
clear = min(clear, math.hypot(px - x, py - y) - R_ROBOT - R_PERSON)
return x, y, clear
def window(c, a, lo, hi, n):
"""동적 창: 지금 값 c 에서 한 주기(DT) 동안 바꿀 수 있는 범위를 n 개로 나눔"""
return np.linspace(max(lo, c - a * DT), min(hi, c + a * DT), n)
def dwa(alpha, beta, gamma, use_vy):
d0 = math.dist((0, 0), GOAL)
vys = window(v_now[1], ACC, -VY_MAX, VY_MAX, 5) if use_vy else [0.0]
cands = []
for vx in window(v_now[0], ACC, -0.1, V_MAX, 5):
for vy in vys:
for w in window(v_now[2], ACC_W, -W_MAX, W_MAX, 7):
x, y, clear = rollout(vx, vy, w)
speed = math.hypot(vx, vy)
if clear <= 0 or speed > math.sqrt(2 * ACC * clear):
continue # 부딪히거나 제때 못 서는 후보는 버림
G = (d0 - math.dist((x, y), GOAL)) / (V_MAX * T) # 목표 쪽으로 얼마나 다가가나
C = min(clear, 1.0) # 장애물 여유 (1 m 에서 자름)
S = speed / V_MAX # 빠를수록
cands.append((alpha * G + beta * C + gamma * S, vx, vy, w, clear))
cands.sort(reverse=True)
return cands
for use_vy in [False, True]:
for beta in [0.0, 0.5, 2.0]:
c = dwa(1.0, beta, 0.3, use_vy)
s, vx, vy, w, clear = c[0]
print("vy 후보 %-3s beta %.1f : 후보 %3d개 통과 -> vx %.2f vy %+.2f w %+.2f (여유 %.2f m, 점수 %.2f)"
% ("on" if use_vy else "off", beta, len(c), vx, vy, w, clear, s))실행 결과:
vy 후보 off beta 0.0 : 후보 23개 통과 -> vx 0.35 vy +0.00 w +0.20 (여유 0.06 m, 점수 0.90)
vy 후보 off beta 0.5 : 후보 23개 통과 -> vx 0.35 vy +0.00 w +0.20 (여유 0.06 m, 점수 0.93)
vy 후보 off beta 2.0 : 후보 23개 통과 -> vx 0.20 vy +0.00 w +0.30 (여유 0.28 m, 점수 1.06)
vy 후보 on beta 0.0 : 후보 119개 통과 -> vx 0.40 vy +0.10 w +0.20 (여유 0.09 m, 점수 1.00)
vy 후보 on beta 0.5 : 후보 119개 통과 -> vx 0.40 vy +0.10 w +0.20 (여유 0.09 m, 점수 1.04)
vy 후보 on beta 2.0 : 후보 119개 통과 -> vx 0.40 vy +0.10 w +0.30 (여유 0.13 m, 점수 1.23)- vy 를 넣으면 통과 후보가 23 → 119 개로 약 5배가 됩니다. "부딪히지 않는 선택지" 가 많을수록 혼잡한 곳에서 멈추는 일이 줄어듭니다.
- vy off 에서 β 를 2.0 으로 키우면 vx 를 0.35 → 0.20 m/s 로 줄이고 더 크게 돌아(ω +0.30) 여유를 0.06 → 0.28 m 로 넓힙니다. 이것이 시뮬레이터 4 에서 β 가 클 때 도착이 늦어지는 이유입니다.
- vy on 은 더 빠른 vx(0.40) 로 가면서 왼쪽으로 비킵니다(vy +0.10).
시뮬레이터 3 에서 세 설정의 궤적을 봤습니다. (가) 경로를 따라 좌우로 잘게 흔들리며 감, (나) 경로에 거의 겹쳐 부드럽게 감, (다) 코너마다 안쪽으로 크게 질러 감. lookahead 거리 L 을 바르게 짝지은 것은?
- (가) 1.4 m · (나) 0.3 m · (다) 0.1 m
- (가) 0.1 m · (나) 0.3 m · (다) 1.4 m
- (가) 0.3 m · (나) 0.1 m · (다) 1.4 m
- (가) 0.1 m · (나) 1.4 m · (다) 0.3 m
정답 보기
② — L 이 짧으면 곡률 κ = 2 sin α / L 이 작은 α 에도 크게 변해 지연과 함께 흔들리고, L 이 길면 코너 훨씬 앞의 점을 보고 미리 꺾어 안쪽을 자릅니다. 중간 값(0.3 m)이 둘의 균형입니다(v = 0.6 m/s 기준).사람 혼잡 구역에서 장애물 거리 가중치 β 를 0.5 → 2.0 으로 올렸더니 평균 도착 시간이 약 21 s → 39 s 로 늘었지만, 사람과의 최소 간격은 늘지 않았습니다(오히려 조금 줄었습니다). 가장 알맞은 해석은?
- β 를 키우면 DWA 가 장애물을 무시하게 된다
- 장애물에서 멀어지려는 점수가 목표 진행을 눌러 머뭇거리고 돌아가며, 오래 머무는 만큼 사람과 마주칠 기회도 늘었다
- β 는 속도 한계를 낮추는 값이라 시간이 늘었다
- 시뮬레이터 오류이므로 β 를 다시 0.5 로 되돌려야 한다
정답 보기
② — 점수는 α · 진행 + β · 여유 + γ · 속도의 합이라 β 가 커지면 "넓은 곳에 머무는" 후보가 이깁니다. 하지만 사람은 계속 움직이므로 오래 머무르면 다시 가까워질 수 있습니다. 혼잡한 곳에서는 β 만 키우기보다 vy 후보 · 이동 예측처럼 **선택지와 정보**를 늘리는 것이 효과적이었습니다.4교시 · 도전: 나만의 자율주행 루프#
감지 → 위치 추정 → 계획 → 제어#
| 단계 | 하는 일 | 이 강좌에서 | 실패하면 |
|---|---|---|---|
| 감지 | 주변 장애물 보기 | LiDAR 스캔 (ch07) | 못 본 상자에 막힘 |
| 위치 추정 | 지도 위 내 위치 | 오도메트리 · 센서 융합 (ch06), 스캔 매칭 (ch07) | 경로를 엉뚱한 곳에서 따라감 |
| 전역 계획 | 목표까지 길 | 코스트맵 + A* (1 · 2교시) | 경로 없음 |
| 추종 · 회피 | 다음 순간 속도 | Pure Pursuit · DWA (3교시) | 흔들림 · 코너 스침 |
| 감시 | 루프가 잘 도는지 | 재계획 · 복구 (이번 교시) | 같은 자리에서 영원히 멈춤 |
재계획 조건과 복구 행동#
한 번 계획한 경로는 시간이 지나면 틀린 경로가 됩니다. 언제 다시 계획할지 정하는 것이 재계획 조건입니다.
- 경로가 막혔을 때: 새로 본 장애물 때문에 남은 경로의 칸이 통과 불가가 됨 → 다시 계획.
- 목표가 바뀌었을 때: 다음 배송지로 넘어감 · 작업자가 목표를 바꿈 → 다시 계획.
- 일정 시간 진전이 없을 때: 3 s 동안 0.1 m 도 못 감 → 계획이 아니라 상황이 문제일 수 있음 → 복구 행동.
복구 행동(recovery behavior) 은 "지금 자리에서는 계획 자체가 안 되는" 상황을 벗어나기 위한 정해진 동작입니다.
| 상황 | 계획이 실패하는 이유 | 복구 행동 |
|---|---|---|
| 상자가 로봇 바로 앞에 갑자기 놓임 | 로봇 중심이 통과 불가 띠 안 → 출발점 자체가 막힘 | 후진 해서 띠 밖으로 나온 뒤 다시 계획 |
| 지도에 유령 장애물(지나간 사람의 흔적)이 남음 | 지도의 장애물이 실제로는 없음 | 제자리 회전으로 주변을 새로 스캔해 지도 갱신 |
| 사람이 길을 막고 서 있음 | 잠시 뒤 비켜 줄 수 있음 | 대기 후 다시 시도 |
myagv.yaml 의 복구 서버도 spin · backup · wait 세 가지를 씁니다. 복구도 실패하면 "도와주세요" 신호(ch05 의 LED · 소리)를 보내고 사람에게 넘기는 것이 실제 물류 현장의 마지막 단계입니다.
시뮬레이터 5 · 선반 통로에서 목표 3곳 순회#
ch07 의 "선반 통로" 맵(12 m × 8 m)에서 출발 → A → B → C 를 돕니다. 각 구간마다 경로 위 어딘가에 0.5 m 상자가 한 개씩 나타나고, 30 % 확률로는 로봇 바로 앞에 갑자기 놓입니다. 로봇은 3 m 안의 상자만 봅니다. 통과 불가 반경은 0.25 m, 로봇은 메카넘이라 lookahead 점을 향해 바로 속도 벡터를 냅니다.
- 무작위 상자 나타남 을 끄고 ▶ 재생합니다. 재계획 없이도 성공하나요? 걸린 시간과 최소 간격을 적어 두세요(기준값).
- 상자를 다시 켭니다. 예측: 재계획을 끈 채 10회 실행하면 성공률은 몇 % 일까요? 재계획을 켜면요? 적어 둔 뒤 📊 10회 실행 으로 확인합니다.
- 재계획만 켠 상태에서 실패한 실행 번호를 골라 ▶ 재생합니다. 아래 기록에 어떤 문구가 반복되나요? 로봇은 어디에 서 있나요?
- 복구 행동 을 켜고 3 의 실행 번호를 다시 재생합니다. 후진(분홍 로봇)이 일어나는 순간과 그 뒤의 기록을 확인하세요.
- 재계획 + 복구에서 10회 실행의 평균 시간을 1 의 기준값과 비교합니다. 몇 초가 늘었고, 무엇 때문인가요?
- 상자가 없으면 재계획 없이도 약 45 s 에 성공합니다. 상자를 켜고 재계획을 끄면 0 / 10 — 계획한 경로 위에 상자가 놓이면 로봇은 안전 정지만 할 뿐 다른 길을 찾지 않습니다.
- 재계획만 켜면 3 / 10. 실패는 모두 "출발점이 막힘 띠 안" 입니다. 상자가 바로 앞에 놓이면 로봇 중심이 통과 불가 띠(0.25 m) 안에 들어가 A* 가 출발할 수조차 없고, 아무리 다시 계획해도 결과가 같습니다.
- 복구 행동(후진)을 켜면 10 / 10. 띠 밖으로 물러난 뒤 다시 계획하면 옆으로 돌아가는 길이 나옵니다. 평균 시간은 약 58 s 로 기준보다 10 s 넘게 늘었습니다 — 돌아가는 길 · 멈춤 · 후진의 대가입니다.
- 최소 간격은 상자가 바로 앞에 나타난 순간에 정해집니다(몇 cm). 로봇이 잘못한 것이 아니라 상황이 만든 값이므로, 평가할 때는 "최소 간격" 과 함께 그 순간이 언제였는지를 같이 봅니다.
성능 평가: 무엇을 잴까#
| 지표 | 뜻 | 왜 필요한가 |
|---|---|---|
| 성공률 | N 번 중 모든 목표에 도착한 비율 | 한 번 성공은 운일 수 있음. 무작위 상황에서 여러 번 돌려야 함 |
| 소요 시간 | 성공한 실행의 평균 · 최대 시간 | 물류 처리량. 최대값이 "가끔 매우 늦음" 을 보여 줌 |
| 최소 장애물 거리 | 로봇 몸과 장애물 사이 가장 가까웠던 거리 | 안전 여유. 평균이 아니라 최솟값이 사고를 만듦 |
| 재계획 · 복구 횟수 | 루프가 얼마나 자주 "고쳐서" 갔나 | 많으면 지도 · 파라미터가 현장과 안 맞는다는 신호 |
같은 실행 번호(난수 씨앗)로 설정만 바꿔 비교해야 공정한 비교가 됩니다. 시뮬레이터 5 의 10회 실행이 항상 번호 1~10 을 쓰는 이유입니다.
- 이 차시의 루프를 실물에 올리기 전에 최고 속도를 0.2~0.3 m/s 로 제한하고, 처음에는 사람 없는 빈 구역에서만 돌립니다.
- 보조자 한 명이 비상 정지(전원 스위치 · 원격 정지 명령)를 언제든 누를 수 있는 자리에 섭니다. 정지 방법은 ch04 · ch05 에서 확인한 것을 씁니다.
- 복구 행동의 후진은 뒤쪽을 보지 않고 움직일 수 있습니다. 로봇 뒤 1 m 를 비우고, 후진 거리를 0.3 m 이하로 제한합니다.
- 상자가 "갑자기 나타나는" 시험은 사람이 아니라 빈 종이 상자로 합니다. 사람이 로봇 앞에 뛰어드는 시험은 하지 않습니다.
실습 과제 4#
선반 통로 맵을 0.25 m 격자로 단순화했습니다. 로봇은 0.5 s 마다 한 칸(0.25 m) 움직이고, 3 m 안의 상자만 지도에 넣습니다. TODO 두 곳(재계획 조건, 복구 행동)을 채운 뒤 세 설정을 10번씩 돌려 성공률을 비교하세요. 예측 먼저: 시뮬레이터 5 와 같은 경향(0 → 일부 → 전부)이 나올까요? "재계획만" 의 성공률은 시뮬레이터와 같을까요?
import heapq
import math
import random
# 선반 통로 맵 (12 m x 8 m) 을 0.25 m 격자로. 사각형 (x0, y0, x1, y1)
RES, NX, NY = 0.25, 48, 32
SHELVES = [(0, 0, 12, 0.25), (0, 7.75, 12, 8), (0, 0, 0.25, 8), (11.75, 0, 12, 8),
(2.5, 1.5, 9.5, 2.0), (2.5, 3.5, 9.5, 4.0), (2.5, 4.5, 9.5, 5.0), (10.5, 3.75, 11.0, 4.25)]
START = (4, 3) # 셀 (i, j) = (1.0 m, 0.75 m)
GOALS = [(32, 10), (41, 26), (6, 25)] # A (8.0, 2.6) · B (10.3, 6.6) · C (1.6, 6.3)
SENSE = 3.0 # 이 거리 안의 상자만 보임 (m)
def cells_of(box):
x0, y0, x1, y1 = box
return {(i, j) for i in range(int(x0 / RES), math.ceil(x1 / RES))
for j in range(int(y0 / RES), math.ceil(y1 / RES))}
STATIC = set().union(*[cells_of(b) for b in SHELVES])
def inflate(occ):
"""장애물 둘레 1칸(0.25 m) 을 통과 불가로 (로봇 반경 0.2 m 를 칸 단위로 올림)"""
return {(i + di, j + dj) for i, j in occ for di in (-1, 0, 1) for dj in (-1, 0, 1)}
def astar(blocked, s, g):
if s in blocked or g in blocked:
return None
G, parent, heap, done, n = {s: 0}, {s: None}, [(0, 0, s)], set(), 0
while heap:
_, _, cur = heapq.heappop(heap)
if cur in done:
continue
done.add(cur)
if cur == g:
path = [cur]
while parent[path[-1]]:
path.append(parent[path[-1]])
return path[::-1]
for di, dj in ((1, 0), (-1, 0), (0, 1), (0, -1)):
nb = (cur[0] + di, cur[1] + dj)
if nb in blocked or not (0 <= nb[0] < NX and 0 <= nb[1] < NY):
continue
if G[cur] + 1 < G.get(nb, 1e9):
G[nb], parent[nb] = G[cur] + 1, cur
n += 1
heapq.heappush(heap, (G[nb] + abs(nb[0] - g[0]) + abs(nb[1] - g[1]), n, nb))
return None
def trial(seed, replan, recover, max_steps=360):
"""한 번 실행: 0.5 s 마다 한 칸(0.25 m) 이동 -> 0.5 m/s. 결과 (성공, 시간 s, 재계획 수, 복구 수)"""
rnd = random.Random(seed)
pos, known, boxes = START, set(STATIC), []
replans = recovers = stuck = 0
for leg, goal in enumerate(GOALS):
path = astar(inflate(known), pos, goal) # 새 목표 -> 항상 계획
k = 1
spawn_at = int(len(path) * rnd.uniform(0.35, 0.65)) if path else None
close = rnd.random() < 0.3 # 30 %: 로봇 바로 앞에 갑자기 나타남
while pos != goal:
max_steps -= 1
if max_steps < 0 or stuck > 40:
return False, None, replans, recovers
# 1) 상자 등장 (경로 중간 지점에 가까워지면)
if spawn_at is not None and path and k >= spawn_at - (1 if close else 8):
a = path[min(k if close else spawn_at, len(path) - 1)]
b = path[min(k + 1 if close else spawn_at + 1, len(path) - 1)]
di, dj = b[0] - a[0], b[1] - a[1] # 진행 방향
off = rnd.choice([-1, 0]) if close else rnd.choice([-2, -1, 0, 1])
box = {(a[0] + m * di - (n + off) * dj, a[1] + m * dj + (n + off) * di)
for m in (0, 1) for n in (0, 1)} # 0.5 m x 0.5 m 상자, 옆으로 off 칸 비켜 놓음
if goal not in box and pos not in box:
boxes.append(box)
spawn_at = None
# 2) 감지: SENSE 안의 상자를 지도에 추가
new = False
for box in boxes:
if not box <= known and min(math.dist(c, pos) for c in box) * RES < SENSE:
known |= box
new = True
blocked = inflate(known)
# 3) 재계획 조건
if replan:
# TODO 1: 재계획 조건 - 경로가 없거나, 새 상자를 봤는데 남은 경로(path[k:]) 중
# 막힌 칸이 있거나, 3 s(6 걸음)마다 진전이 없을 때 (stuck in (6, 12, ...))
if path is None:
# TODO 2: 복구 - recover 가 켜져 있고 지금 칸(pos)이 blocked 안이면
# blocked 가 아닌 상하좌우 옆칸으로 한 칸 물러나고 recovers += 1
path, k = astar(blocked, pos, goal), 1
replans += 1
# 4) 한 칸 이동 (다음 칸에 실제 장애물이 있으면 안전 정지)
if path is None or k >= len(path):
stuck += 1
continue
nxt = path[k]
if any(nxt in b for b in boxes) or nxt in STATIC:
stuck += 1
continue
pos, k, stuck = nxt, k + 1, 0
return True, (360 - max_steps) * 0.5, replans, recovers
for replan, recover in [(False, False), (True, False), (True, True)]:
res = [trial(s, replan, recover) for s in range(10)]
ok = [r for r in res if r[0]]
print("재계획 %-3s 복구 %-3s : 성공 %2d/10, 평균 시간 %s, 재계획 %d회, 복구 %d회" % (
"on" if replan else "off", "on" if recover else "off", len(ok),
"%.0f s" % (sum(r[1] for r in ok) / len(ok)) if ok else "-",
sum(r[2] for r in res), sum(r[3] for r in res)))풀이 코드 보기
import heapq
import math
import random
# 선반 통로 맵 (12 m x 8 m) 을 0.25 m 격자로. 사각형 (x0, y0, x1, y1)
RES, NX, NY = 0.25, 48, 32
SHELVES = [(0, 0, 12, 0.25), (0, 7.75, 12, 8), (0, 0, 0.25, 8), (11.75, 0, 12, 8),
(2.5, 1.5, 9.5, 2.0), (2.5, 3.5, 9.5, 4.0), (2.5, 4.5, 9.5, 5.0), (10.5, 3.75, 11.0, 4.25)]
START = (4, 3) # 셀 (i, j) = (1.0 m, 0.75 m)
GOALS = [(32, 10), (41, 26), (6, 25)] # A (8.0, 2.6) · B (10.3, 6.6) · C (1.6, 6.3)
SENSE = 3.0 # 이 거리 안의 상자만 보임 (m)
def cells_of(box):
x0, y0, x1, y1 = box
return {(i, j) for i in range(int(x0 / RES), math.ceil(x1 / RES))
for j in range(int(y0 / RES), math.ceil(y1 / RES))}
STATIC = set().union(*[cells_of(b) for b in SHELVES])
def inflate(occ):
"""장애물 둘레 1칸(0.25 m) 을 통과 불가로 (로봇 반경 0.2 m 를 칸 단위로 올림)"""
return {(i + di, j + dj) for i, j in occ for di in (-1, 0, 1) for dj in (-1, 0, 1)}
def astar(blocked, s, g):
if s in blocked or g in blocked:
return None
G, parent, heap, done, n = {s: 0}, {s: None}, [(0, 0, s)], set(), 0
while heap:
_, _, cur = heapq.heappop(heap)
if cur in done:
continue
done.add(cur)
if cur == g:
path = [cur]
while parent[path[-1]]:
path.append(parent[path[-1]])
return path[::-1]
for di, dj in ((1, 0), (-1, 0), (0, 1), (0, -1)):
nb = (cur[0] + di, cur[1] + dj)
if nb in blocked or not (0 <= nb[0] < NX and 0 <= nb[1] < NY):
continue
if G[cur] + 1 < G.get(nb, 1e9):
G[nb], parent[nb] = G[cur] + 1, cur
n += 1
heapq.heappush(heap, (G[nb] + abs(nb[0] - g[0]) + abs(nb[1] - g[1]), n, nb))
return None
def trial(seed, replan, recover, max_steps=360):
"""한 번 실행: 0.5 s 마다 한 칸(0.25 m) 이동 -> 0.5 m/s. 결과 (성공, 시간 s, 재계획 수, 복구 수)"""
rnd = random.Random(seed)
pos, known, boxes = START, set(STATIC), []
replans = recovers = stuck = 0
for leg, goal in enumerate(GOALS):
path = astar(inflate(known), pos, goal) # 새 목표 -> 항상 계획
k = 1
spawn_at = int(len(path) * rnd.uniform(0.35, 0.65)) if path else None
close = rnd.random() < 0.3 # 30 %: 로봇 바로 앞에 갑자기 나타남
while pos != goal:
max_steps -= 1
if max_steps < 0 or stuck > 40:
return False, None, replans, recovers
# 1) 상자 등장 (경로 중간 지점에 가까워지면)
if spawn_at is not None and path and k >= spawn_at - (1 if close else 8):
a = path[min(k if close else spawn_at, len(path) - 1)]
b = path[min(k + 1 if close else spawn_at + 1, len(path) - 1)]
di, dj = b[0] - a[0], b[1] - a[1] # 진행 방향
off = rnd.choice([-1, 0]) if close else rnd.choice([-2, -1, 0, 1])
box = {(a[0] + m * di - (n + off) * dj, a[1] + m * dj + (n + off) * di)
for m in (0, 1) for n in (0, 1)} # 0.5 m x 0.5 m 상자, 옆으로 off 칸 비켜 놓음
if goal not in box and pos not in box:
boxes.append(box)
spawn_at = None
# 2) 감지: SENSE 안의 상자를 지도에 추가
new = False
for box in boxes:
if not box <= known and min(math.dist(c, pos) for c in box) * RES < SENSE:
known |= box
new = True
blocked = inflate(known)
# 3) 재계획 조건
if replan:
ahead_blocked = path is not None and any(c in blocked for c in path[k:])
if path is None or (new and ahead_blocked) or stuck in (6, 12, 18, 24, 30, 36):
if pos in blocked and recover: # 출발 칸이 막힘 띠 안 -> 복구: 막히지 않은 옆칸으로 후진
free = [(pos[0] + a, pos[1] + b) for a, b in ((1, 0), (-1, 0), (0, 1), (0, -1))
if (pos[0] + a, pos[1] + b) not in blocked]
if free:
pos = free[0]
recovers += 1
path, k = astar(blocked, pos, goal), 1
replans += 1
# 4) 한 칸 이동 (다음 칸에 실제 장애물이 있으면 안전 정지)
if path is None or k >= len(path):
stuck += 1
continue
nxt = path[k]
if any(nxt in b for b in boxes) or nxt in STATIC:
stuck += 1
continue
pos, k, stuck = nxt, k + 1, 0
return True, (360 - max_steps) * 0.5, replans, recovers
for replan, recover in [(False, False), (True, False), (True, True)]:
res = [trial(s, replan, recover) for s in range(10)]
ok = [r for r in res if r[0]]
print("재계획 %-3s 복구 %-3s : 성공 %2d/10, 평균 시간 %s, 재계획 %d회, 복구 %d회" % (
"on" if replan else "off", "on" if recover else "off", len(ok),
"%.0f s" % (sum(r[1] for r in ok) / len(ok)) if ok else "-",
sum(r[2] for r in res), sum(r[3] for r in res)))실행 결과:
재계획 off 복구 off : 성공 0/10, 평균 시간 -, 재계획 0회, 복구 0회
재계획 on 복구 off : 성공 6/10, 평균 시간 56 s, 재계획 184회, 복구 0회
재계획 on 복구 on : 성공 10/10, 평균 시간 57 s, 재계획 30회, 복구 5회- 재계획 off 는 0/10, 재계획만 6/10, 재계획 + 복구 10/10 으로 시뮬레이터 5 와 경향이 같습니다. 격자 · 이동 방식 · 상자 위치 규칙이 달라 "재계획만" 의 숫자는 다릅니다.
- "재계획만" 의 재계획 횟수(184회)가 유난히 많은 것은 출발점이 막힌 채로 매 걸음 다시 계획했다가 실패하기 때문입니다. 같은 실패를 반복하는 재계획은 복구가 아닙니다. 몇 번 실패하면 다른 행동(후진 · 회전 · 대기)으로 넘어가야 합니다.
- 실물이라면
pos = free[0](한 칸 후진) 대신 ch04 의 속도 명령으로 천천히 물러나고, 물러나는 방향의 LiDAR 거리부터 확인해야 합니다.
다음 중 "다시 계획하기" 만으로는 해결되지 않고 복구 행동이 필요한 상황은?
- 다음 배송지가 A 에서 B 로 바뀌었다
- 가는 길 2 m 앞 통로에 상자가 놓였지만 옆 통로는 비어 있다
- 상자가 로봇 바로 앞 0.2 m 에 놓여 로봇 중심이 통과 불가 띠 안에 들어갔다
- 경로가 조금 길어도 괜찮으니 더 넓은 통로로 가고 싶다
정답 보기
③ — 출발점이 통과 불가 띠 안이면 A* 는 출발조차 못 하므로(시뮬레이터 5 의 "출발점이 막힘 띠 안") 몇 번을 다시 계획해도 실패합니다. 후진으로 띠 밖에 나온 뒤 계획해야 합니다. ① · ② 는 재계획으로 해결되고, ④ 는 비용 띠(2교시)를 조절할 문제입니다.시뮬레이터 5 에서 10회 실행 결과가 "재계획 off: 0 %, 재계획만: 30 %, 재계획 + 복구: 100 %" 였습니다. 이 결과로 말할 수 있는 것으로 가장 알맞은 것은?
- 재계획은 쓸모없고 복구 행동만 있으면 된다
- 경로 위 장애물은 재계획이, 출발점이 막히는 상황은 복구가 맡아 둘이 함께 있어야 성공한다
- 10회 모두 성공했으니 실물에서도 100 % 성공한다
- 복구 행동을 켜면 평균 시간이 짧아진다
정답 보기
② — 재계획 off 는 경로 위 상자 하나로 실패하고, 재계획만으로는 바로 앞 상자를 못 벗어납니다. 둘이 서로 다른 실패를 맡습니다. ③ 10회는 표본이 작고 시뮬레이터는 실제 센서 잡음 · 위치 오차가 없습니다. ④ 복구는 실패를 성공으로 바꾸지만 시간은 오히려 늘었습니다(약 45 s → 58 s).모델 확인 (3D 뷰어)#
강좌용 myAGV Plus 모델(assets/robot/robot.json)이 아직 없다면 뷰어에 "모델 없음" 안내가 나옵니다. 모델이 준비되면 다음을 확인합니다.
- 차체의 앞뒤 · 좌우 끝이
base_footprint원점에서 각각 몇 m 인지 → 이 값으로footprint다각형을 적습니다. 바퀴 · 범퍼 · LiDAR 처럼 가장 튀어나온 부분까지 포함해야 합니다. - 원점이 차체의 한가운데인지. 원점이 앞쪽으로 치우쳐 있으면 발자국 다각형도 비대칭이 되고, 외접 반지름이 커집니다(실습 2-2 의 "앞으로 튀어나온 짐" 과 같은 효과).
- 위에 짐 · 로봇팔을 올릴 때 그 끝이 차체 밖으로 얼마나 나오는지.
모델이 없을 때는 아래 계산으로 원점 위치가 발자국 반지름에 주는 영향을 확인할 수 있습니다.
import math
# 교육용 가정: 차체 0.34 x 0.24 m (myagv.yaml 의 footprint 와 같은 크기)
# 실제 치수 · 원점 위치는 myAGV Plus 모델(3D 뷰어)이나 공식 도면에서 확인해 고칩니다.
L, W = 0.34, 0.24
for shift in [0.0, 0.03, 0.06]: # 원점이 차체 중심보다 앞으로 shift m 치우침
corners = [(L / 2 - shift, W / 2), (L / 2 - shift, -W / 2), (-L / 2 - shift, -W / 2), (-L / 2 - shift, W / 2)]
r_out = max(math.hypot(x, y) for x, y in corners)
front, back = L / 2 - shift, L / 2 + shift
print("원점 치우침 %.2f m: 앞 %.2f m, 뒤 %.2f m, 외접 반지름 %.3f m, 제자리 회전에 필요한 원 지름 %.2f m"
% (shift, front, back, r_out, 2 * r_out))- 원점이 6 cm 만 치우쳐도 외접 반지름이 0.208 → 0.259 m 로 커져, 제자리 회전에 필요한 폭이 0.42 → 0.52 m 가 됩니다. 실습 2-1 의 0.6 m 통로 안에서 제자리 회전이 아슬아슬해지는 크기입니다.
- 그래서 URDF(ch02)의
base_footprint위치와 Nav2 의footprint를 같은 기준으로 맞추는 것이 중요합니다.
자주 나는 오류와 해결#
1) 경로가 선반 모서리를 스치듯 지나가고, 실물이 코너에서 선반을 긁음#
(rviz2) 전역 경로가 선반 모서리에 붙어 있음, 로봇 옆면 · 짐이 모서리에 닿음- 원인: 로봇 반경(
robot_radius) 이 발자국의 외접 반지름보다 작거나, 짐을 실었는데 발자국을 그대로 두었습니다. A* 는 비용 띠가 없으면 장애물에 바짝 붙는 경로를 고릅니다(시뮬레이터 2). - 해결: 발자국을 실제(짐 포함) 크기로 적고, 비용 띠(
inflation_radius> 로봇 반경)를 켜서 경로가 통로 가운데로 다니게 합니다. 코너에서는 lookahead 를 줄이거나 속도를 낮춥니다(시뮬레이터 3).
2) 분명히 지나갈 수 있는 통로인데 "경로 없음"#
[planner_server]: GridBased: failed to create plan, no valid path found.- 원인: 팽창 반경이 통로 폭의 절반보다 크거나, 지도 해상도 때문에 실제보다 좁게 잡혔습니다(0.6 m 통로가 0.28 m 에서 닫힘). 지도에 지나간 사람의 흔적(유령 장애물)이 남아 있을 수도 있습니다. 메시지 문구는 Nav2 버전 · 플래너에 따라 다릅니다.
- 해결: 실습 2-1 처럼 통로 단면의 표면 거리를 확인하고 반경 · 해상도를 함께 조절합니다. 유령 장애물이면 제자리 회전으로 지도를 새로 스캔하거나 코스트맵을 지웁니다(clear costmap — ch10).
3) "Starting point in lethal space" — 로봇이 출발조차 못 함#
[planner_server]: GridBased: failed to create plan with tolerance 2.00.
[WARN] Starting point in lethal space! Cannot create feasible plan.- 원인: 로봇 중심 칸이 통과 불가 띠(253 · 254) 안입니다. 벽에 너무 붙어 멈췄거나, 바로 옆에 상자가 놓였습니다(시뮬레이터 5 의 "출발점이 막힘 띠 안"). 경고 문구는 버전에 따라 다름(확인 필요).
- 해결: 복구 행동(후진 · 회전)으로 띠 밖으로 나온 뒤 다시 계획합니다. 사람이 로봇을 손으로 옮기지 말고 원격으로 천천히 후진시킵니다.
4) 직선 통로에서 로봇이 좌우로 흔들리며 감#
(rviz2) 로봇 궤적이 경로를 중심으로 지그재그, cmd_vel 의 angular.z 부호가 계속 바뀜- 원인: lookahead 가 속도에 비해 너무 짧거나, 제어 지연(통신 · 모터 응답)이 큽니다. 위치 추정이 튀어도 같은 증상입니다(ch06).
- 해결: lookahead 를 늘리고 속도에 비례하게 잡습니다(시뮬레이터 3). 제어 주기 · 오도메트리 발행 주기가 일정한지 확인합니다.
5) 사람 앞에서 로봇이 멈춰 버리고 움직이지 않음#
[controller_server]: No valid trajectories out of 175!
[controller_server]: Failed to make progress- 원인: DWA 후보가 모두 버려졌습니다. 장애물 거리 가중치 · 안전 거리가 너무 크거나, vy 후보가 없어(
max_vel_y: 0.0) 비킬 방법이 회전뿐입니다. 문구는 DWB · 버전에 따라 다름. - 해결: 측면 속도를 허용해 후보를 늘리고(메카넘),
sim_time을 줄여 가까운 미래만 보게 하거나, 대기 복구 후 다시 시도합니다(시뮬레이터 4 · 5).
6) A* 가 엄청 느리거나 메모리가 부족함#
(Playground) 실행 시간 초과 · 브라우저가 멈춤- 원인: 닫힌 집합(closed)을 확인하지 않아 같은 칸을 여러 번 확장하거나, 힙에 넣을 때 g 가 더 작아지는지 확인하지 않았습니다. 리스트에서
min()으로 꺼내도 셀 수의 제곱만큼 느려집니다. - 해결:
heapq+if cur in closed: continue+ng < g.get(nxt, inf)세 가지를 모두 씁니다(실습 1-1). 해상도를 필요 이상으로 높이지 않습니다.
7) 같은 지도 · 같은 코드인데 탐색 셀 수가 시뮬레이터와 다름#
탐색(꺼낸) 셀 129개 (시뮬레이터는 131개)- 원인: 이웃을 보는 순서, f 가 같을 때 꺼내는 순서(count 유무), 시작 · 목표 좌표(행 · 열 순서, y 축 방향)가 다릅니다.
- 해결: 경로 길이(최단 여부)가 같다면 정상입니다. 숫자까지 맞추려면 이웃 순서와
(f, count, 칸)규칙을 맞춥니다.
과제 / 팀 미션#
- 휴리스틱 실험 노트: 시뮬레이터 1 에서 직접 지도를 하나 만들어(벽 20개 이상), 8방향 · 유클리드로 w = 0 · 1 · 1.5 · 3 · 5 의 탐색 셀 수와 경로 길이를 표로 만들고, "우리 지도에서 쓸 w" 를 이유와 함께 3줄로 쓰세요.
- 발자국 카드: 우리 로봇이 실을 짐 세 가지(크기 직접 정함)의 발자국 다각형 · 내접 · 외접 반지름을 실습 2-2 로 계산하고, 각각 지나갈 수 있는 가장 좁은 통로 폭을 적으세요.
- DWA 튜닝 보고서: 시뮬레이터 4 에서 vy on 상태로 (α, β, γ) 조합 세 가지를 정해 배치 1~5 평균 도착 시간 · 0.3 m 안 시간을 비교하고, 혼잡 구역에 맞는 조합을 고르세요.
Part 3(오도메트리 · LiDAR 지도 · 경로 계획)을 하나로 묶습니다. 팀별로 실습 과제 4-1 의 코드를 고쳐 다음 규칙으로 겨룹니다.
- 과제: 출발 → A → B → C 순회, 무작위 상자 모드, 실행 번호 0~19 (20회).
- 바꿀 수 있는 것: 재계획 조건, 복구 행동의 종류(후진 거리 · 대기 · 다른 방향), 팽창 칸 수, 목표 순서는 그대로.
- 점수: 성공률(%) × 10 − 평균 시간(s) − 장애물 옆 칸을 지난 횟수 × 0.5.
- 발표: 감지 → 위치 추정 → 계획 → 제어 루프 그림(@fig 와 같은 모양)에 우리 팀이 고친 곳을 표시하고, 가장 효과가 컸던 변경 하나와 그 근거(바꾸기 전 · 후 숫자)를 3분 안에 설명합니다.
- 역할 나누기: 계획 담당(A* · 팽창), 제어 담당(이동 · 안전 정지), 감시 담당(재계획 · 복구), 평가 담당(20회 실행 · 표 작성).
참고자료#
- myAGV Plus 공식 문서 (영문): https://github.com/elephantrobotics/myagvPlus_docs/tree/main/MYAGV_PLUS_EN
- 제품 사양 (최고 속도 1.6 m/s, LiDAR 0.12~8 m): https://github.com/elephantrobotics/myagvPlus_docs/blob/main/MYAGV_PLUS_EN/2-ProductFeature/2-ProductFeature.md
- 6.2.5 Map Navigation (Plus 의 내비게이션 실행 순서 —
myagv_plus_navigation2): https://github.com/elephantrobotics/myagvPlus_docs/blob/main/MYAGV_PLUS_EN/6-Software_Development_Guide/6.2-ApplicationBaseROS2/6.2.5-Navigation-Map_Navigation.md - myagv_ros2 저장소 (이전 모델 myAGV 용,
galactic-JN브랜치): https://github.com/elephantrobotics/myagv_ros2 - myagv_ros2 의 Nav2 파라미터
myagv.yaml(코스트맵 · DWB · 복구 설정): https://github.com/elephantrobotics/myagv_ros2/blob/galactic-JN/myagv_navigation2/param/myagv.yaml - Nav2 문서 — Inflation Layer 파라미터: https://docs.nav2.org/configuration/packages/costmap-plugins/inflation.html
- Nav2 문서 — Costmap 2D 설정: https://docs.nav2.org/configuration/packages/configuring-costmaps.html
- Nav2 문서 — DWB Controller: https://docs.nav2.org/configuration/packages/configuring-dwb-controller.html
- Nav2 문서 — Regulated Pure Pursuit: https://docs.nav2.org/configuration/packages/configuring-regulated-pp.html
- P. E. Hart, N. J. Nilsson, B. Raphael, A Formal Basis for the Heuristic Determination of Minimum Cost Paths (A* 원 논문), IEEE Transactions on Systems Science and Cybernetics, 1968
- D. Fox, W. Burgard, S. Thrun, The Dynamic Window Approach to Collision Avoidance, IEEE Robotics & Automation Magazine, 1997
- R. C. Coulter, Implementation of the Pure Pursuit Path Tracking Algorithm, CMU-RI-TR-92-01, 1992
- myAGV Plus 제품 사진 · 소개 (hyenc.net): https://www.hyenc.net/products/35
- 이 사이트의 도구: URDF 뷰어 · Python Playground
- 이전 차시: ch07 LiDAR 와 점유 격자 지도 · 다음 차시: ch09 ROS 2 Humble 과 myAGV Plus 패키지