CH07LiDAR 와 점유 격자 지도
360° LiDAR 스캔 데이터를 읽어 점으로 바꾸고, 점유 격자 지도를 직접 만들며 SLAM(Gmapping) 이 지도와 위치를 동시에 맞추는 원리를 발견합니다.
학습 목표#
sensor_msgs/LaserScan의angle_min·angle_increment·ranges[]·range_min/max를 읽고, i 번 광선을 θ = angle_min + i · angle_increment, x = r·cosθ, y = r·sinθ 로 점으로 바꾸며0.0·inf같은 무효값을 걸러낼 수 있습니다.- 광선 사이 간격 r · Δθ 와 물체 크기를 비교해 광선 수를 줄였을 때 좁은 기둥을 놓칠지 예측하고, 유리 · 검은 물체 · 사람 다리처럼 LiDAR 가 잘 못 보는 대상을 말할 수 있습니다.
- 점유 격자(occupancy grid)의 해상도와 세 상태(빈 칸 0 · 장애물 100 · 미확인 −1)를 설명하고, 브레젠험 직선 + 로그 오즈 누적으로 스캔 여러 장을 numpy 격자에 쌓아 지도를 만들 수 있습니다.
- 해상도(0.02~0.2 m)가 지도 크기 · 계산량 · 좁은 통로 표현에 주는 영향을 시뮬레이터로 확인하고 상황에 맞는 값을 고를 수 있습니다.
- SLAM 의 "닭과 달걀" 문제를 설명하고, 스캔 매칭(격자 탐색으로 Δx · Δy · Δθ 찾기)과 Gmapping 의 파티클 필터 직관, 그리고 지도를 망치는 운전 습관(빠른 주행 · 특징 없는 복도)을 시뮬레이터 결과로 설명할 수 있습니다.
이 페이지에는 브라우저에서 바로 동작하는 시뮬레이터 5개가 있습니다. 모두 같은 가상 창고("선반 통로" 맵, 12 m × 8 m)를 씁니다. 슬라이더를 움직이기 전에 결과를 먼저 예측해 적고, 조작해서 확인하세요. Playground 실습 코드는 맨 위에 같은 창고를 흉내 내는 sim_scan() 함수를 넣어 두었습니다. 실물 LiDAR 의 /scan 메시지와 같은 이름의 필드(angle_min, angle_increment, ranges …)를 돌려주므로, 나중에 실물 데이터로 바꿔도 처리 코드는 그대로 쓸 수 있습니다.
공식 자료 기준: myAGV Plus 의 LiDAR 측정 범위 0.12~8 m · 측정 각도 360°, 최고 속도 1.6 m/s (myagvPlus_docs 2. Product Feature). Gmapping 지도 작성 순서와 "키보드 조종은 기본 속도를 쓰라, 낮은 속도가 대체로 더 좋은 지도를 만든다" 는 권고, map_saver_cli 로 .pgm + .yaml 을 저장하는 방법(6.2.4 Real-time Mapping with Gmapping). 공식 문서에 없는 것: LiDAR 모델명 · 스캔 주파수 · 각 분해능은 Plus 문서에 나와 있지 않습니다. myagv_ros2 저장소(이전 모델 myAGV 용, galactic-JN 브랜치)의 ydlidar.yaml 에는 frequency: 10.0, frame_id: laser_frame, invalid_range_is_inf: false 등이 있지만 Plus 에 그대로 맞는지는 확인 필요입니다. 교육용 가정: 스캔 10 Hz, 광선 수, 거리 잡음 1 cm, 측정 실패율 1 %, 창고 치수, 오도메트리 오차 모델, 로그 오즈 값(−0.4 · +0.85), 검은 물체가 1.5 m 밖에서 안 보이는 것 등. 시뮬레이터는 원리를 보여 주기 위한 것이며 Gmapping 의 실제 내부 값과 다릅니다. "루프 닫기" 운전 요령은 공식 문서가 아니라 SLAM 일반에서 널리 쓰는 요령입니다.
1교시 · LiDAR 스캔 데이터 읽기#
LiDAR 는 무엇을 재나#
LiDAR(Light Detection And Ranging)는 레이저 빛을 쏘아 물체에 맞고 돌아오는 것을 보고 거리를 잽니다. myAGV Plus 위의 LiDAR 는 머리를 빙글빙글 돌리며 한 바퀴(360°)에 수백 번 거리를 재고, 한 바퀴 분량을 묶어 스캔(scan) 한 장으로 내보냅니다.
- 거리를 재는 방식은 빛이 돌아오는 시간(ToF)이나 반사 위치(삼각측량) 등 제품마다 다릅니다. 이 강좌에서는 "한 방향마다 거리 하나" 라는 결과만 씁니다.
- 공식 사양: 측정 범위 0.12~8 m, 각도 360°. 0.12 m 보다 가까우면 너무 가까워서, 8 m 보다 멀면 너무 멀어서 믿을 수 있는 값이 아닙니다.
- 2D LiDAR 는 바닥과 평행한 한 평면만 봅니다. 그 높이보다 낮거나 높은 물체(낮은 팔레트 · 테이블 상판 · 지게차 포크)는 아예 보이지 않습니다.
LaserScan 메시지 구조#
ROS 에서 스캔 한 장은 sensor_msgs/LaserScan 메시지로 오가며, myagv_ros2 에서는 scan 토픽 · laser_frame 좌표계를 씁니다(ROS 2 연결은 ch09 에서 다룹니다). 이 차시에서 필요한 필드는 다음과 같습니다.
| 필드 | 뜻 | 예 (광선 360개) |
|---|---|---|
angle_min | 0번 광선의 각도 (rad) | −π (= 로봇 바로 뒤) |
angle_max | 마지막 광선의 각도 | π − Δθ |
angle_increment | 광선 사이 각도 Δθ (rad) | 2π / 360 = 0.01745 rad = 1° |
range_min · range_max | 믿을 수 있는 거리 범위 (m) | 0.12 · 8.0 (공식 사양 기준) |
ranges[] | 광선마다 잰 거리 (m), 길이 = 광선 수 | [0.91, 0.90, …, inf, …, 0.0, …] |
header.frame_id · stamp | 어느 좌표계 · 언제 | laser_frame · 시각 |
각도는 ROS 관례(REP 103)대로 정면 0, 반시계(왼쪽)가 + 입니다. 그래서 θ = +90° 는 왼쪽, −90° 는 오른쪽, ±180° 는 뒤쪽입니다.
극좌표 → 직교좌표, 그리고 무효값 거르기#
ranges[i] 하나는 "각도 θᵢ 방향으로 r 만큼 떨어진 곳에 무언가 있다" 는 극좌표입니다. 지도에 그리려면 직교좌표로 바꿉니다.
θᵢ = angle_min + i · angle_increment
xᵢ = rᵢ · cos θᵢ (LiDAR 기준, 앞쪽 +)
yᵢ = rᵢ · sin θᵢ (LiDAR 기준, 왼쪽 +)문제는 ranges[] 에 거리가 아닌 값이 섞여 있다는 것입니다.
| 값 | 언제 나오나 | 처리 |
|---|---|---|
inf | 최대 거리 안에 아무것도 없음 · 빛이 돌아오지 않음 | 점으로 쓰지 않음 (지도에서는 "그 방향은 멀리까지 비어 있을 수 있다" 정도의 정보) |
0.0 | 측정 실패. 드라이버 설정에 따라 무효값을 inf 대신 0 으로 줌(invalid_range_is_inf: false) | 점으로 쓰지 않음 — **min() 을 그냥 쓰면 "0 m 앞에 장애물" 로 착각** |
nan | 일부 드라이버의 무효값 | 점으로 쓰지 않음 |
r < range_min 또는 r > range_max | 범위 밖 | 점으로 쓰지 않음 |
판단은 한 줄이면 됩니다: **range_min <= r <= range_max 일 때만 쓴다.** inf · nan · 0.0 이 모두 이 조건에서 걸러집니다(nan 은 어떤 비교도 False).
시뮬레이터 1 · 선반 통로에서 LiDAR 스캔 보기#
가상 창고 바닥에 선반 세 줄(A · B · C), 굵은 기둥 하나, 지름 6 cm 의 좁은 기둥 하나가 있습니다. 선반 B 와 C 사이는 폭 0.5 m 의 좁은 통로입니다. 지도를 클릭하거나 끌어서 로봇을 옮기고, 아래 그래프에서 ranges[] 를 "데이터 그대로" 봅니다.
- 통로 입구에서 아래 그래프를 봅니다. 가로축 0° 근처(정면)에 빨간 막대(inf)가 생기는 이유를 지도에서 찾아보세요. 최대 거리를 8 → 12 m 로 늘리면 그 막대는 어떻게 될까요? 예측하고 확인합니다.
- 좁은 기둥 3 m 앞에서 광선 수를 360 → 180 → 90 으로 줄이기 전에, 몇 개에서 기둥(빨간 점)이 사라질지 예측합니다. 표의 "그 거리의 광선 간격" 과 기둥 지름 6 cm 를 비교하세요. 로봇을 기둥 쪽으로 끌어 1 m 까지 다가가면 90개로도 보이나요?
- 로봇이 아는 것만 을 켜고 통로 입구 → 좁은 통로 입구로 옮겨 봅니다. 점만 보고 선반과 통로를 알아볼 수 있나요? 좁은 통로 안쪽 선반 벽은 왜 점이 드문드문할까요?
- 잡음 σ 를 0 → 5 cm 로 올리며 벽의 점이 어떻게 변하는지 봅니다. 가장 가까운 장애물의 "방향" 값이 왜 바뀔까요?
- 사람 다리 · 유리 벽 · 검은 상자 버튼을 차례로 누릅니다. 각각 몇 개의 광선이 맞는지, 맞지 않은 광선은 어디로 갔는지 확인합니다. 검은 상자 쪽으로 로봇을 끌어 1.5 m 안으로 들어가면 무엇이 달라지나요?
- 통로 입구에서 정면 광선은 선반 A · B 사이 통로를 따라 오른쪽 벽(약 10.3 m)까지 가므로 8 m 를 넘어 inf 가 됩니다. 최대 거리를 12 m 로 늘리면 점이 생깁니다. inf 는 "아무것도 없다" 가 아니라 "최대 거리 안에는 없다" 입니다.
- 광선 간격은 r · Δθ 입니다. 3 m 에서 90개(Δθ = 4°)는 약 21 cm, 360개(1°)는 약 5.2 cm 입니다. 간격이 물체보다 크면 맞을 수도, 안 맞을 수도 있습니다(광선과 기둥의 각도가 우연히 맞으면 보임). 확실히 보려면 간격이 물체 크기보다 작아야 합니다.
- 좁은 통로 안쪽 벽은 광선이 비스듬히 스쳐 맞으므로 점 간격이 r · Δθ / sin(스치는 각) 으로 크게 벌어집니다. 지도에서 긴 벽이 듬성듬성 보이는 이유입니다(2교시 지도에서 다시 봅니다).
- 0.0(보라 막대)은 무작위 측정 실패입니다. 필터 없이
min(ranges)를 하면 이 0.0 이 뽑혀 "바로 앞에 장애물" 로 착각합니다(실습 과제 1-1).
LiDAR 가 잘 못 보는 것#
시뮬레이터 1 의 특수 물체는 실제 창고에서 자주 문제가 되는 대상을 흉내 낸 것입니다.
| 대상 | 무엇이 일어나나 | 결과 |
|---|---|---|
| 유리 · 투명 아크릴 | 빛이 대부분 통과하고, 거의 수직으로 맞을 때만 반사 | 유리 벽 뒤의 물체가 보이거나, 유리 벽이 지도에서 빠짐 |
| 검은 · 무광 물체 | 빛을 흡수해 돌아오는 빛이 약함 | 가까이서만 보이고 멀면 inf (시뮬은 1.5 m 로 가정) |
| 거울 · 반짝이는 금속 | 빛이 다른 곳으로 튕김 | 벽 너머에 "가짜 공간" 이 생김 |
| 사람 다리 · 의자 다리 | 폭이 좁고 움직임 | 작은 점 두세 개, 매 스캔 위치가 바뀜 → 지도에 잡음 |
| LiDAR 높이 밖의 물체 | 2D 평면 위 · 아래를 지나감 | 아예 보이지 않음 → 지도만 믿으면 충돌 |
유리 벽에는 반투명 테이프 띠를 붙이고, 지도를 만들 때는 사람이 로봇 주변에 서 있지 않게 합니다. LiDAR 가 못 보는 높이의 장애물은 범퍼 · 초음파 · 카메라 같은 다른 센서나 "가지 마시오" 구역 설정으로 보완합니다(ch08 · ch10).
실습 과제 1#
sim_scan(x, y, θ) 는 창고의 그 자리에서 찍은 스캔 한 장을 LaserScan 과 같은 이름의 dict 로 돌려줍니다. 로봇 (1.0, 2.6) 에서 찍은 스캔으로 가장 가까운 장애물의 거리와 방향을 출력하세요. 예측 먼저: 필터 없이 min(ranges) 를 하면 무엇이 나올까요? 로봇 왼쪽 벽은 x = 0.1 에 있습니다. 가장 가까운 장애물의 방향은 몇 도쯤일까요?
import math
import numpy as np
# 선반 통로 맵 (교육용, 단위 m) - 사각형 (x0, y0, x1, y1)
BOXES = [(0, 0, 12, 0.1), (0, 7.9, 12, 8), (0, 0, 0.1, 8), (11.9, 0, 12, 8), # 바깥 벽
(2.5, 1.4, 9.5, 2.0), (2.5, 3.45, 9.5, 4.05), (2.5, 4.55, 9.5, 5.15), # 선반 A, B, C
(10.6, 3.8, 11.0, 4.2)] # 굵은 기둥
POLES = [(0.7, 4.8, 0.03)] # 좁은 기둥 (지름 6 cm)
def sim_scan(x, y, th, n=360, rmax=8.0, rmin=0.12, noise=0.01, drop=0.01, seed=0):
"""시뮬레이터 LiDAR 한 장 -> LaserScan 과 같은 이름의 dict (교육용)"""
rng = np.random.default_rng(seed)
a = -math.pi + np.arange(n) * 2 * math.pi / n + th
dx, dy = np.cos(a)[:, None], np.sin(a)[:, None]
S = []
for x0, y0, x1, y1 in BOXES:
S += [(x0, y0, x1, y0), (x1, y0, x1, y1), (x1, y1, x0, y1), (x0, y1, x0, y0)]
S = np.array(S, float)
sx, sy, qx, qy = S[:, 2] - S[:, 0], S[:, 3] - S[:, 1], S[:, 0] - x, S[:, 1] - y
with np.errstate(divide="ignore", invalid="ignore"):
den = dx * sy - dy * sx
t = (qx * sy - qy * sx) / den
u = (qx * dy - qy * dx) / den
t = np.where((t > 1e-9) & (u >= 0) & (u <= 1), t, np.inf).min(axis=1)
for cx, cy, cr in POLES:
b = (x - cx) * dx[:, 0] + (y - cy) * dy[:, 0]
disc = b * b - ((x - cx) ** 2 + (y - cy) ** 2 - cr * cr)
tc = -b - np.sqrt(np.maximum(disc, 0))
t = np.minimum(t, np.where((disc >= 0) & (tc > 1e-9), tc, np.inf))
r = t + rng.normal(0, noise, n)
r[t > rmax] = np.inf
r[(r < rmin) | (rng.random(n) < drop)] = 0.0
return {"angle_min": -math.pi, "angle_increment": 2 * math.pi / n,
"range_min": rmin, "range_max": rmax, "ranges": r.tolist()}
scan = sim_scan(1.0, 2.6, 0.0) # 로봇 (1.0, 2.6), 정면 = +x
ranges = scan["ranges"]
print("광선 수:", len(ranges), " angle_increment = %.4f rad" % scan["angle_increment"])
print("필터 없이 min(ranges) =", min(ranges))
best_r, best_i = None, None
for i, r in enumerate(ranges):
# TODO 1: r 이 range_min ~ range_max 밖이면(0.0, inf 포함) 건너뜀
# TODO 2: 가장 작은 r 과 그 번호 i 를 best_r, best_i 에 저장
pass
# TODO 3: i 번 광선의 각도 theta = angle_min + i * angle_increment, 도(deg)로 출력
# TODO 4: 그 점의 로봇 기준 좌표 x = r cos(theta), y = r sin(theta)풀이 코드 보기
import math
import numpy as np
# 선반 통로 맵 (교육용, 단위 m) - 사각형 (x0, y0, x1, y1)
BOXES = [(0, 0, 12, 0.1), (0, 7.9, 12, 8), (0, 0, 0.1, 8), (11.9, 0, 12, 8), # 바깥 벽
(2.5, 1.4, 9.5, 2.0), (2.5, 3.45, 9.5, 4.05), (2.5, 4.55, 9.5, 5.15), # 선반 A, B, C
(10.6, 3.8, 11.0, 4.2)] # 굵은 기둥
POLES = [(0.7, 4.8, 0.03)] # 좁은 기둥 (지름 6 cm)
def sim_scan(x, y, th, n=360, rmax=8.0, rmin=0.12, noise=0.01, drop=0.01, seed=0):
"""시뮬레이터 LiDAR 한 장 -> LaserScan 과 같은 이름의 dict (교육용)"""
rng = np.random.default_rng(seed)
a = -math.pi + np.arange(n) * 2 * math.pi / n + th
dx, dy = np.cos(a)[:, None], np.sin(a)[:, None]
S = []
for x0, y0, x1, y1 in BOXES:
S += [(x0, y0, x1, y0), (x1, y0, x1, y1), (x1, y1, x0, y1), (x0, y1, x0, y0)]
S = np.array(S, float)
sx, sy, qx, qy = S[:, 2] - S[:, 0], S[:, 3] - S[:, 1], S[:, 0] - x, S[:, 1] - y
with np.errstate(divide="ignore", invalid="ignore"):
den = dx * sy - dy * sx
t = (qx * sy - qy * sx) / den
u = (qx * dy - qy * dx) / den
t = np.where((t > 1e-9) & (u >= 0) & (u <= 1), t, np.inf).min(axis=1)
for cx, cy, cr in POLES:
b = (x - cx) * dx[:, 0] + (y - cy) * dy[:, 0]
disc = b * b - ((x - cx) ** 2 + (y - cy) ** 2 - cr * cr)
tc = -b - np.sqrt(np.maximum(disc, 0))
t = np.minimum(t, np.where((disc >= 0) & (tc > 1e-9), tc, np.inf))
r = t + rng.normal(0, noise, n)
r[t > rmax] = np.inf
r[(r < rmin) | (rng.random(n) < drop)] = 0.0
return {"angle_min": -math.pi, "angle_increment": 2 * math.pi / n,
"range_min": rmin, "range_max": rmax, "ranges": r.tolist()}
scan = sim_scan(1.0, 2.6, 0.0) # 로봇 (1.0, 2.6), 정면 = +x
ranges = scan["ranges"]
print("광선 수:", len(ranges), " angle_increment = %.4f rad" % scan["angle_increment"])
print("필터 없이 min(ranges) =", min(ranges))
best_r, best_i = None, None
n_zero = n_inf = 0
for i, r in enumerate(ranges):
if r == 0.0:
n_zero += 1
if math.isinf(r):
n_inf += 1
if not (scan["range_min"] <= r <= scan["range_max"]):
continue
if best_r is None or r < best_r:
best_r, best_i = r, i
theta = scan["angle_min"] + best_i * scan["angle_increment"]
x, y = best_r * math.cos(theta), best_r * math.sin(theta)
print("0.0 인 광선 %d개, inf 인 광선 %d개 -> 걸러냄" % (n_zero, n_inf))
print("가장 가까운 장애물: %.3f m, 방향 %.1f deg (i = %d)" % (best_r, math.degrees(theta), best_i))
print("로봇 기준 좌표: x = %.3f, y = %.3f" % (x, y))실행 결과:
광선 수: 360 angle_increment = 0.0175 rad
필터 없이 min(ranges) = 0.0
0.0 인 광선 4개, inf 인 광선 11개 -> 걸러냄
가장 가까운 장애물: 0.897 m, 방향 -176.0 deg (i = 4)
로봇 기준 좌표: x = -0.895, y = -0.063- 필터 없이
min()을 하면 측정 실패값 0.0 이 나옵니다. 실물에서 "로봇 앞에 0 m 장애물" 경고가 뜨는 흔한 원인입니다. - 가장 가까운 것은 로봇 뒤쪽(±180°) 의 왼쪽 벽, 약 0.9 m 입니다(1.0 − 0.1). 벽에 수직인 광선은 −180° 지만, 잡음 1 cm 때문에 그 옆 광선(−176°)이 아주 조금 더 짧게 나왔습니다. 가장 가까운 "한 점" 은 잡음에 흔들리므로, 실제 안전 판단은 앞쪽 ±30° 처럼 구역의 최솟값이나 몇 개의 중간값을 씁니다.
- inf 11개는 오른쪽으로 통로를 따라 8 m 를 넘는 광선들입니다.
지름 6 cm 의 좁은 기둥에서 1 · 2 · 3 · 4 m 떨어진 자리에서 광선 720 · 360 · 180 · 90 개로 스캔해, 기둥을 맞힌 광선 수를 표로 만듭니다. 예측 먼저: 표의 16칸 중 0 이 나올 칸에 표시해 보세요. 기준은 "그 거리의 광선 간격 r · Δθ 가 6 cm 보다 큰가" 입니다.
import math
import numpy as np
# 선반 통로 맵 (교육용, 단위 m) - 사각형 (x0, y0, x1, y1)
BOXES = [(0, 0, 12, 0.1), (0, 7.9, 12, 8), (0, 0, 0.1, 8), (11.9, 0, 12, 8), # 바깥 벽
(2.5, 1.4, 9.5, 2.0), (2.5, 3.45, 9.5, 4.05), (2.5, 4.55, 9.5, 5.15), # 선반 A, B, C
(10.6, 3.8, 11.0, 4.2)] # 굵은 기둥
POLES = [(0.7, 4.8, 0.03)] # 좁은 기둥 (지름 6 cm)
def sim_scan(x, y, th, n=360, rmax=8.0, rmin=0.12, noise=0.01, drop=0.01, seed=0):
"""시뮬레이터 LiDAR 한 장 -> LaserScan 과 같은 이름의 dict (교육용)"""
rng = np.random.default_rng(seed)
a = -math.pi + np.arange(n) * 2 * math.pi / n + th
dx, dy = np.cos(a)[:, None], np.sin(a)[:, None]
S = []
for x0, y0, x1, y1 in BOXES:
S += [(x0, y0, x1, y0), (x1, y0, x1, y1), (x1, y1, x0, y1), (x0, y1, x0, y0)]
S = np.array(S, float)
sx, sy, qx, qy = S[:, 2] - S[:, 0], S[:, 3] - S[:, 1], S[:, 0] - x, S[:, 1] - y
with np.errstate(divide="ignore", invalid="ignore"):
den = dx * sy - dy * sx
t = (qx * sy - qy * sx) / den
u = (qx * dy - qy * dx) / den
t = np.where((t > 1e-9) & (u >= 0) & (u <= 1), t, np.inf).min(axis=1)
for cx, cy, cr in POLES:
b = (x - cx) * dx[:, 0] + (y - cy) * dy[:, 0]
disc = b * b - ((x - cx) ** 2 + (y - cy) ** 2 - cr * cr)
tc = -b - np.sqrt(np.maximum(disc, 0))
t = np.minimum(t, np.where((disc >= 0) & (tc > 1e-9), tc, np.inf))
r = t + rng.normal(0, noise, n)
r[t > rmax] = np.inf
r[(r < rmin) | (rng.random(n) < drop)] = 0.0
return {"angle_min": -math.pi, "angle_increment": 2 * math.pi / n,
"range_min": rmin, "range_max": rmax, "ranges": r.tolist()}
PX, PY, PR = POLES[0] # 좁은 기둥: 중심 (0.7, 4.8), 반지름 0.03 m
def pole_hits(scan, x, y):
"""로봇 (x, y, 방향 0) 의 스캔에서 기둥을 맞힌 광선 수"""
hits = 0
for i, r in enumerate(scan["ranges"]):
if not (scan["range_min"] <= r <= scan["range_max"]):
continue
th = scan["angle_min"] + i * scan["angle_increment"]
# TODO 1: 점의 지도 좌표 (x + r cos th, y + r sin th) 가 기둥 중심에서 PR + 0.01 m 안이면 hits += 1
return hits
print("예측을 먼저 적으세요: 기둥까지 3 m 일 때 90개 광선이 기둥을 맞힐까요?")
print(" 거리 ", " ".join("%5d개" % n for n in (720, 360, 180, 90)))
for d in (1.0, 2.0, 3.0, 4.0):
x, y = PX - 0.023 * d, PY - d # 기둥이 왼쪽 약 91.3 도 방향, d m 거리
row = []
for n in (720, 360, 180, 90):
scan = sim_scan(x, y, 0.0, n=n, noise=0.0, drop=0.0)
row.append(pole_hits(scan, x, y))
# TODO 2: 그 거리에서 광선 사이 간격 d * angle_increment (cm) 도 함께 출력해 기둥 지름 6 cm 와 비교
print("%4.1f m " % d, " ".join("%6d" % h for h in row))풀이 코드 보기
import math
import numpy as np
# 선반 통로 맵 (교육용, 단위 m) - 사각형 (x0, y0, x1, y1)
BOXES = [(0, 0, 12, 0.1), (0, 7.9, 12, 8), (0, 0, 0.1, 8), (11.9, 0, 12, 8), # 바깥 벽
(2.5, 1.4, 9.5, 2.0), (2.5, 3.45, 9.5, 4.05), (2.5, 4.55, 9.5, 5.15), # 선반 A, B, C
(10.6, 3.8, 11.0, 4.2)] # 굵은 기둥
POLES = [(0.7, 4.8, 0.03)] # 좁은 기둥 (지름 6 cm)
def sim_scan(x, y, th, n=360, rmax=8.0, rmin=0.12, noise=0.01, drop=0.01, seed=0):
"""시뮬레이터 LiDAR 한 장 -> LaserScan 과 같은 이름의 dict (교육용)"""
rng = np.random.default_rng(seed)
a = -math.pi + np.arange(n) * 2 * math.pi / n + th
dx, dy = np.cos(a)[:, None], np.sin(a)[:, None]
S = []
for x0, y0, x1, y1 in BOXES:
S += [(x0, y0, x1, y0), (x1, y0, x1, y1), (x1, y1, x0, y1), (x0, y1, x0, y0)]
S = np.array(S, float)
sx, sy, qx, qy = S[:, 2] - S[:, 0], S[:, 3] - S[:, 1], S[:, 0] - x, S[:, 1] - y
with np.errstate(divide="ignore", invalid="ignore"):
den = dx * sy - dy * sx
t = (qx * sy - qy * sx) / den
u = (qx * dy - qy * dx) / den
t = np.where((t > 1e-9) & (u >= 0) & (u <= 1), t, np.inf).min(axis=1)
for cx, cy, cr in POLES:
b = (x - cx) * dx[:, 0] + (y - cy) * dy[:, 0]
disc = b * b - ((x - cx) ** 2 + (y - cy) ** 2 - cr * cr)
tc = -b - np.sqrt(np.maximum(disc, 0))
t = np.minimum(t, np.where((disc >= 0) & (tc > 1e-9), tc, np.inf))
r = t + rng.normal(0, noise, n)
r[t > rmax] = np.inf
r[(r < rmin) | (rng.random(n) < drop)] = 0.0
return {"angle_min": -math.pi, "angle_increment": 2 * math.pi / n,
"range_min": rmin, "range_max": rmax, "ranges": r.tolist()}
PX, PY, PR = POLES[0] # 좁은 기둥: 중심 (0.7, 4.8), 반지름 0.03 m
def pole_hits(scan, x, y):
"""로봇 (x, y, 방향 0) 의 스캔에서 기둥을 맞힌 광선 수"""
hits = 0
for i, r in enumerate(scan["ranges"]):
if not (scan["range_min"] <= r <= scan["range_max"]):
continue
th = scan["angle_min"] + i * scan["angle_increment"]
if math.hypot(x + r * math.cos(th) - PX, y + r * math.sin(th) - PY) < PR + 0.01:
hits += 1
return hits
print("기둥을 맞힌 광선 수 (괄호 = 그 거리에서 광선 간격 cm)")
print(" 거리 ", " ".join("%11d개" % n for n in (720, 360, 180, 90)))
for d in (1.0, 2.0, 3.0, 4.0):
x, y = PX - 0.023 * d, PY - d # 기둥이 왼쪽 약 91.3 도 방향, d m 거리
row = []
for n in (720, 360, 180, 90):
scan = sim_scan(x, y, 0.0, n=n, noise=0.0, drop=0.0)
gap = d * scan["angle_increment"] * 100
row.append("%3d (%4.1f)" % (pole_hits(scan, x, y), gap))
print("%4.1f m " % d, " ".join("%12s" % s for s in row))실행 결과:
기둥을 맞힌 광선 수 (괄호 = 그 거리에서 광선 간격 cm)
거리 720개 360개 180개 90개
1.0 m 7 ( 0.9) 4 ( 1.7) 2 ( 3.5) 1 ( 7.0)
2.0 m 4 ( 1.7) 2 ( 3.5) 1 ( 7.0) 1 (14.0)
3.0 m 2 ( 2.6) 1 ( 5.2) 0 (10.5) 0 (20.9)
4.0 m 2 ( 3.5) 1 ( 7.0) 0 (14.0) 0 (27.9)- 간격이 6 cm 보다 작은 칸은 반드시 1개 이상 맞습니다. 맞는 개수 ≈ 6 cm ÷ 간격 입니다(1 m · 720개: 6 / 0.9 ≈ 7).
- 간격이 6 cm 보다 큰 칸은 운입니다. 2 m · 90개(간격 14 cm)는 우연히 맞았고, 3 m · 180개(10.5 cm)는 놓쳤습니다. 로봇이 조금만 움직여도 결과가 바뀌므로 지도에서 기둥이 "깜빡" 입니다.
- 결론: "이 크기의 물체를 이 거리에서 확실히 보려면 r · Δθ < 물체 폭". 광선 수를 줄이면 계산은 가벼워지지만 멀리 있는 좁은 물체부터 사라집니다.
angle_min = −3.1416, angle_increment = 0.0175 (rad) 인 스캔에서 ranges[270] = 2.0 입니다. 이 점의 LiDAR 기준 좌표에 가장 가까운 것은?
- (2.0, 0.0) — 정면
- (0.0, 2.0) — 왼쪽
- (0.0, −2.0) — 오른쪽
- (−2.0, 0.0) — 뒤쪽
정답 보기
② — θ = −3.1416 + 270 × 0.0175 ≈ 1.58 rad ≈ 90.6° 이므로 왼쪽입니다. x = 2.0 · cos(1.58) ≈ −0.02, y = 2.0 · sin(1.58) ≈ 2.0. 0번 광선이 뒤쪽(−180°)에서 시작한다는 점이 함정입니다. 270 번째를 "270°" 로 읽으면 ③ 이나 ④ 를 고르게 됩니다.시뮬레이터 1 에서 광선 360개로 4 m 떨어진 좁은 기둥(지름 6 cm)이 보였다 안 보였다 했습니다. 기둥을 항상 보이게 하는 방법으로 가장 알맞은 것은?
- 최대 거리를 8 m 에서 12 m 로 늘린다
- 잡음 σ 를 0 으로 줄인다
- 광선을 720개로 늘리거나 기둥에 2 m 안쪽으로 다가간다
- 로봇 방향을 기둥 쪽으로 돌린다
정답 보기
③ — 4 m · 360개의 간격은 4 × 0.01745 ≈ 7 cm 로 기둥(6 cm)보다 큽니다. 720개(3.5 cm)로 늘리거나 2 m(3.5 cm)로 다가가면 간격이 기둥보다 작아집니다. 최대 거리(①)와 잡음(②)은 간격과 상관없고, 360° LiDAR 는 로봇 방향(④)과 관계없이 모든 방향을 같은 간격으로 봅니다(우연히 맞는 각도가 바뀔 뿐 확실해지지 않음).2교시 · 점유 격자 지도 만들기#
점유 격자란#
스캔 한 장은 "지금 이 순간 보이는 점" 일 뿐입니다. 로봇이 경로를 계획하려면 "이 창고의 어디가 비어 있고 어디가 막혀 있는지" 를 한 장에 모은 지도가 필요합니다. 가장 널리 쓰이는 형태가 점유 격자(occupancy grid) 입니다.
- 바닥을 같은 크기의 정사각형 셀(cell) 로 나눕니다. 셀 한 변의 길이가 해상도(resolution) 이며 ROS 지도에서는 0.05 m(5 cm)를 많이 씁니다.
- 셀마다 "장애물이 있을 확률 p" 를 기억하고, 사용할 때는 세 상태로 나눕니다.
| 상태 | 뜻 | ROS nav_msgs/OccupancyGrid 값 | 저장 이미지(pgm) 색 |
|---|---|---|---|
| 빈 칸 (free) | 광선이 지나갔다 = 비어 있다 | 0 | 흰색 (254) |
| 장애물 (occupied) | 광선이 여기서 멈췄다 = 무언가 있다 | 100 | 검정 (0) |
| 미확인 (unknown) | 한 번도 보지 못했다 | −1 | 회색 (205) |
ROS 의 OccupancyGrid 는 셀 값을 int8 하나(−1 ~ 100)로 저장하므로 셀 하나에 1 바이트가 듭니다. 0 과 100 사이 값(예: 65)은 "점유 확률 65 %" 를 뜻하지만, 지도 저장 · 경로 계획에서는 보통 임계값으로 세 상태로 나눕니다.
OccupancyGrid 값 0 은 빈 칸이고, pgm 이미지 픽셀값 0 은 검정 = 장애물입니다. 숫자 0 의 뜻이 정반대입니다(4교시 지도 저장 형식에서 다시 확인).
광선 한 줄로 셀 갱신하기: 브레젠험 + 로그 오즈#
광선 하나가 알려 주는 것은 두 가지입니다. 지나간 셀은 비어 있고, 끝 셀에는 무언가 있다. 지나간 셀을 고르는 데에는 컴퓨터 그래픽의 브레젠험(Bresenham) 직선 알고리즘을 씁니다. 정수 덧셈만으로 출발 셀에서 끝 셀까지 계단처럼 이어지는 셀들을 빠르게 고릅니다.
한 번 보고 "비어 있음" 으로 정해 버리면, 잡음이나 지나가는 사람 하나로 지도가 망가집니다. 그래서 여러 번 본 증거를 더해 갑니다. 확률을 직접 곱하는 대신 로그 오즈(log-odds) 로 바꾸면 덧셈만으로 누적됩니다.
l = ln( p / (1 − p) ) p = 0.5 (모름) -> l = 0
지나간 셀: l ← l + l_free (예: −0.4)
끝 셀: l ← l + l_occ (예: +0.85)
쓸 때: p = 1 − 1 / (1 + eˡ)
p > 0.65 -> 장애물 100, p < 0.25 -> 빈 칸 0, 나머지 -> 미확인 −1- l = 0 에서 출발하므로 처음엔 모든 셀이 미확인입니다.
- l_occ 가 l_free 보다 큰 이유: 광선이 끝난 곳은 확실한 증거지만, 지나간 곳은 "그 높이에서 그 순간 비었다" 는 약한 증거이기 때문입니다.
- 값이 끝없이 커지지 않게 l 을 ±5 정도로 자릅니다(clamp). 그래야 나중에 치운 물건이 지도에서 다시 지워질 수 있습니다.
inf광선은 이 시뮬레이터에서는 쓰지 않습니다(끝이 없으므로). 실제 SLAM 패키지는 "최대 거리까지는 비어 있다" 로 쓰는 옵션이 있습니다.
시뮬레이터 2 · 점유 격자 만들기 (해상도 · 로그 오즈)#
로봇이 파란 점 자리에서 스캔을 찍으며 창고 왼쪽 → 위쪽 통로 → 오른쪽으로 돕니다. 위치는 정확하다고 둡니다(위치 오차는 3교시). 셀을 클릭하면 그 셀의 l · p · ROS 값이 표에 나옵니다.
- 스캔 수 N = 1 로 두고 지도를 봅니다. 미확인(회색)이 몇 % 일까요? N 을 1 → 5 → 20 으로 늘리며 세 상태 비율이 어떻게 바뀌는지 기록합니다.
- 보기를 "확률 p" 로 바꿉니다. N = 3 일 때와 N = 30 일 때, 벽 셀의 색이 얼마나 진한지 비교합니다. 벽 셀을 클릭해 l 값을 읽어 보세요. 몇 번 맞으면 p > 0.65 를 넘을까요? (l_occ = 0.85, 경계 l = 0.62)
- 해상도를 0.05 → 0.02 → 0.2 m 로 바꾸기 전에 셀 수 · 메모리 · 셀 갱신 횟수가 각각 몇 배가 될지 예측합니다. 0.02 m 에서 같은 N 인데 지도가 왜 더 "듬성듬성" 할까요?
- 좁은 통로 확대를 켜고 해상도를 0.05 → 0.1 → 0.2 m 로 바꾸며 표의 "x = 3.5 m 단면" 을 봅니다. 폭 0.5 m 통로가 빈 칸 몇 개로 보이나요? 0.2 m 에서 선반 벽은 어떻게 보이나요?
- l_free 를 −0.4 → −1.5 로 바꿉니다. 선반 B · C 의 벽이 어떻게 되나요? 반대로 l_occ 를 0.2 까지 낮추면요?
- N = 1 이면 로봇 주변에서 광선이 닿은 부채꼴만 빈 칸이 되고, 선반 뒤 · 좁은 통로 안은 미확인으로 남습니다. 지도는 본 만큼만 생깁니다.
- 벽 셀은 한 번 맞으면 l = 0.85 로 곧바로 경계(0.62)를 넘지만, 다른 광선이 스치며 −0.4 씩 빼면 다시 내려갑니다. 여러 장을 쌓을수록 진짜 벽은 진해지고 우연한 점은 지워집니다.
- 셀 크기를 반으로 줄이면 셀 수 · 메모리는 4배, 광선이 지나가는 셀 수(계산량)는 약 2배가 됩니다(광선 길이 ÷ 셀 크기). 0.02 m 에서는 셀이 작아 광선이 지나가지 않은 틈이 많이 남으므로 같은 N 으로는 지도가 덜 채워집니다. 더 많이, 더 촘촘한 광선으로 찍어야 합니다.
- 0.2 m 에서는 0.5 m 통로가 셀 경계에 따라 빈 칸 1~2개(0.2~0.4 m) 로 줄어 보이고, 선반 벽은 20 cm 두께로 뭉툭해집니다. 지도에서 실제보다 좁아 보이는 통로는 경로 계획이 "지나갈 수 없음" 으로 판단할 수 있습니다(ch08).
- l_free 가 너무 크면(−1.5) 스치는 광선 몇 번에 벽이 지워지고, l_occ 가 너무 작으면 벽이 경계를 넘지 못해 미확인으로 남습니다. 두 값의 비율이 지도 품질을 정합니다.
해상도의 균형#
| 해상도 | 12 m × 8 m 창고의 셀 수 | int8 지도 | 좋은 점 | 나쁜 점 |
|---|---|---|---|---|
| 0.02 m | 600 × 400 = 240,000 | 약 234 kB | 좁은 틈 · 얇은 기둥 표현 | 계산 · 메모리 큼, 같은 스캔으로 덜 채워짐, 잡음이 그대로 보임 |
| 0.05 m | 240 × 160 = 38,400 | 약 38 kB | 실내 이동 로봇의 흔한 기본값 | — |
| 0.1 m | 120 × 80 = 9,600 | 약 9 kB | 빠름, 넓은 공간 | 0.5 m 통로가 4~5칸 |
| 0.2 m | 60 × 40 = 2,400 | 약 2 kB | 아주 빠름 | 벽이 두꺼워지고 좁은 통로가 1~2칸으로 줄거나 끊김 |
넓이가 100 m × 100 m 인 실제 물류 창고라면 0.05 m 에서 2000 × 2000 = 400만 셀(약 4 MB)입니다. 해상도는 로봇이 지나가야 하는 가장 좁은 틈의 1/5~1/10 정도를 기준으로 고르는 것이 무난합니다.
실습 과제 2#
로봇이 (1.6, 2.7) → (1.6, 5.8) → (10.25, 5.8) 로 0.5 m 마다 스캔을 찍습니다. 해상도 0.2 m 의 60 × 40 격자에 로그 오즈를 쌓고, ROS 값(0 · 100 · −1)으로 바꿔 #(장애물) · .(빈 칸) · ?(미확인) 텍스트 지도를 출력하세요. 브레젠험 함수는 완성되어 있습니다. 예측 먼저: 로봇이 한 번도 들어가지 않은 선반 B 와 C 의 안쪽(선반 속)과 창고 오른쪽 아래 구역은 어떤 글자로 나올까요?
import math
import numpy as np
# 선반 통로 맵 (교육용, 단위 m) - 사각형 (x0, y0, x1, y1)
BOXES = [(0, 0, 12, 0.1), (0, 7.9, 12, 8), (0, 0, 0.1, 8), (11.9, 0, 12, 8), # 바깥 벽
(2.5, 1.4, 9.5, 2.0), (2.5, 3.45, 9.5, 4.05), (2.5, 4.55, 9.5, 5.15), # 선반 A, B, C
(10.6, 3.8, 11.0, 4.2)] # 굵은 기둥
POLES = [(0.7, 4.8, 0.03)] # 좁은 기둥 (지름 6 cm)
def sim_scan(x, y, th, n=360, rmax=8.0, rmin=0.12, noise=0.01, drop=0.01, seed=0):
"""시뮬레이터 LiDAR 한 장 -> LaserScan 과 같은 이름의 dict (교육용)"""
rng = np.random.default_rng(seed)
a = -math.pi + np.arange(n) * 2 * math.pi / n + th
dx, dy = np.cos(a)[:, None], np.sin(a)[:, None]
S = []
for x0, y0, x1, y1 in BOXES:
S += [(x0, y0, x1, y0), (x1, y0, x1, y1), (x1, y1, x0, y1), (x0, y1, x0, y0)]
S = np.array(S, float)
sx, sy, qx, qy = S[:, 2] - S[:, 0], S[:, 3] - S[:, 1], S[:, 0] - x, S[:, 1] - y
with np.errstate(divide="ignore", invalid="ignore"):
den = dx * sy - dy * sx
t = (qx * sy - qy * sx) / den
u = (qx * dy - qy * dx) / den
t = np.where((t > 1e-9) & (u >= 0) & (u <= 1), t, np.inf).min(axis=1)
for cx, cy, cr in POLES:
b = (x - cx) * dx[:, 0] + (y - cy) * dy[:, 0]
disc = b * b - ((x - cx) ** 2 + (y - cy) ** 2 - cr * cr)
tc = -b - np.sqrt(np.maximum(disc, 0))
t = np.minimum(t, np.where((disc >= 0) & (tc > 1e-9), tc, np.inf))
r = t + rng.normal(0, noise, n)
r[t > rmax] = np.inf
r[(r < rmin) | (rng.random(n) < drop)] = 0.0
return {"angle_min": -math.pi, "angle_increment": 2 * math.pi / n,
"range_min": rmin, "range_max": rmax, "ranges": r.tolist()}
RES = 0.2 # 셀 크기 (m)
NX, NY = int(12 / RES), int(8 / RES) # 60 x 40 셀
L = np.zeros((NY, NX)) # 로그 오즈, 행 = y, 열 = x
L_FREE, L_OCC, L_MAX = -0.4, 0.85, 5.0
def bresenham(i0, j0, i1, j1):
"""셀 (i0, j0) 에서 (i1, j1) 까지 지나가는 셀 목록 (끝 셀 포함)"""
cells = []
di, dj = abs(i1 - i0), abs(j1 - j0)
si, sj = (1 if i1 > i0 else -1), (1 if j1 > j0 else -1)
e = di - dj
while True:
cells.append((i0, j0))
if i0 == i1 and j0 == j1:
return cells
e2 = 2 * e
if e2 > -dj:
e -= dj
i0 += si
if e2 < di:
e += di
j0 += sj
def add_scan(x, y, th, scan):
ci, cj = int(x / RES), int(y / RES)
for k, r in enumerate(scan["ranges"]):
if not (scan["range_min"] <= r <= scan["range_max"]):
continue
a = th + scan["angle_min"] + k * scan["angle_increment"]
ei, ej = int((x + r * math.cos(a)) / RES), int((y + r * math.sin(a)) / RES)
cells = bresenham(ci, cj, ei, ej)
# TODO 1: 마지막 셀을 뺀 나머지(지나간 셀)에 L_FREE 를 더함 (격자 안일 때만, -L_MAX 아래로는 안 내려감)
# TODO 2: 끝 셀 (ei, ej) 에 L_OCC 를 더함 (L_MAX 위로는 안 올라감)
# 로봇 경로: (1.6, 2.7) -> (1.6, 5.8) -> (10.25, 5.8), 0.5 m 마다 스캔
poses = [(1.6, 2.7 + 0.5 * k, math.pi / 2) for k in range(7)]
poses += [(1.6 + 0.5 * k, 5.8, 0.0) for k in range(18)]
for s, (x, y, th) in enumerate(poses):
add_scan(x, y, th, sim_scan(x, y, th, n=180, seed=s))
# TODO 3: p > 0.65 이면 100, p < 0.25 이면 0, 나머지는 -1 (로그 오즈 경계값을 계산해 비교)
ros = np.full((NY, NX), -1)
print("스캔 %d장, 격자 %d x %d, 빈 칸 %d, 장애물 %d, 미확인 %d"
% (len(poses), NX, NY, (ros == 0).sum(), (ros == 100).sum(), (ros == -1).sum()))
for j in range(NY - 1, -1, -1): # 위쪽(y 큰 쪽)부터 출력
print("".join({100: "#", 0: ".", -1: "?"}[v] for v in ros[j]))풀이 코드 보기
import math
import numpy as np
# 선반 통로 맵 (교육용, 단위 m) - 사각형 (x0, y0, x1, y1)
BOXES = [(0, 0, 12, 0.1), (0, 7.9, 12, 8), (0, 0, 0.1, 8), (11.9, 0, 12, 8), # 바깥 벽
(2.5, 1.4, 9.5, 2.0), (2.5, 3.45, 9.5, 4.05), (2.5, 4.55, 9.5, 5.15), # 선반 A, B, C
(10.6, 3.8, 11.0, 4.2)] # 굵은 기둥
POLES = [(0.7, 4.8, 0.03)] # 좁은 기둥 (지름 6 cm)
def sim_scan(x, y, th, n=360, rmax=8.0, rmin=0.12, noise=0.01, drop=0.01, seed=0):
"""시뮬레이터 LiDAR 한 장 -> LaserScan 과 같은 이름의 dict (교육용)"""
rng = np.random.default_rng(seed)
a = -math.pi + np.arange(n) * 2 * math.pi / n + th
dx, dy = np.cos(a)[:, None], np.sin(a)[:, None]
S = []
for x0, y0, x1, y1 in BOXES:
S += [(x0, y0, x1, y0), (x1, y0, x1, y1), (x1, y1, x0, y1), (x0, y1, x0, y0)]
S = np.array(S, float)
sx, sy, qx, qy = S[:, 2] - S[:, 0], S[:, 3] - S[:, 1], S[:, 0] - x, S[:, 1] - y
with np.errstate(divide="ignore", invalid="ignore"):
den = dx * sy - dy * sx
t = (qx * sy - qy * sx) / den
u = (qx * dy - qy * dx) / den
t = np.where((t > 1e-9) & (u >= 0) & (u <= 1), t, np.inf).min(axis=1)
for cx, cy, cr in POLES:
b = (x - cx) * dx[:, 0] + (y - cy) * dy[:, 0]
disc = b * b - ((x - cx) ** 2 + (y - cy) ** 2 - cr * cr)
tc = -b - np.sqrt(np.maximum(disc, 0))
t = np.minimum(t, np.where((disc >= 0) & (tc > 1e-9), tc, np.inf))
r = t + rng.normal(0, noise, n)
r[t > rmax] = np.inf
r[(r < rmin) | (rng.random(n) < drop)] = 0.0
return {"angle_min": -math.pi, "angle_increment": 2 * math.pi / n,
"range_min": rmin, "range_max": rmax, "ranges": r.tolist()}
RES = 0.2 # 셀 크기 (m)
NX, NY = int(12 / RES), int(8 / RES) # 60 x 40 셀
L = np.zeros((NY, NX)) # 로그 오즈, 행 = y, 열 = x
L_FREE, L_OCC, L_MAX = -0.4, 0.85, 5.0
def bresenham(i0, j0, i1, j1):
"""셀 (i0, j0) 에서 (i1, j1) 까지 지나가는 셀 목록 (끝 셀 포함)"""
cells = []
di, dj = abs(i1 - i0), abs(j1 - j0)
si, sj = (1 if i1 > i0 else -1), (1 if j1 > j0 else -1)
e = di - dj
while True:
cells.append((i0, j0))
if i0 == i1 and j0 == j1:
return cells
e2 = 2 * e
if e2 > -dj:
e -= dj
i0 += si
if e2 < di:
e += di
j0 += sj
def add_scan(x, y, th, scan):
ci, cj = int(x / RES), int(y / RES)
for k, r in enumerate(scan["ranges"]):
if not (scan["range_min"] <= r <= scan["range_max"]):
continue
a = th + scan["angle_min"] + k * scan["angle_increment"]
ei, ej = int((x + r * math.cos(a)) / RES), int((y + r * math.sin(a)) / RES)
cells = bresenham(ci, cj, ei, ej)
for i, j in cells[:-1]: # 지나간 셀 -> 빈 칸 쪽
if 0 <= i < NX and 0 <= j < NY:
L[j, i] = max(-L_MAX, L[j, i] + L_FREE)
if 0 <= ei < NX and 0 <= ej < NY: # 끝 셀 -> 장애물 쪽
L[ej, ei] = min(L_MAX, L[ej, ei] + L_OCC)
# 로봇 경로: (1.6, 2.7) -> (1.6, 5.8) -> (10.25, 5.8), 0.5 m 마다 스캔
poses = [(1.6, 2.7 + 0.5 * k, math.pi / 2) for k in range(7)]
poses += [(1.6 + 0.5 * k, 5.8, 0.0) for k in range(18)]
for s, (x, y, th) in enumerate(poses):
add_scan(x, y, th, sim_scan(x, y, th, n=180, seed=s))
occ = L > math.log(0.65 / 0.35) # p > 0.65
free = L < math.log(0.25 / 0.75) # p < 0.25
ros = np.where(occ, 100, np.where(free, 0, -1)) # ROS OccupancyGrid 값
print("스캔 %d장, 격자 %d x %d, 빈 칸 %d, 장애물 %d, 미확인 %d"
% (len(poses), NX, NY, (ros == 0).sum(), (ros == 100).sum(), (ros == -1).sum()))
for j in range(NY - 1, -1, -1): # 위쪽(y 큰 쪽)부터 출력
print("".join({100: "#", 0: ".", -1: "?"}[v] for v in ros[j]))실행 결과 (위쪽 일부):
스캔 25장, 격자 60 x 40, 빈 칸 1299, 장애물 251, 미확인 850
############################################################
#..........................................................#
#..........................................................#
...
#...........####################################...........#
#...........#??????????????????????????????????#...........#
#...........#???????????????????????????????????...........#
#...........####?????????????????#?????????????............#
#....................??????????????????????????......##....#
#...........#####?#??????????#????????????????#?....###....#
...- 위쪽 통로(y = 5.8 m 근처)는 로봇이 지나갔으므로 넓게
.이고, 선반 C 의 윗면은#한 줄로 또렷합니다. - 선반 속은 광선이 들어갈 수 없으므로
?입니다. 미확인은 "모른다" 이지 "막혔다" 가 아닙니다. 경로 계획에서 미확인을 어떻게 다룰지는 따로 정해야 합니다(ch08). - 좁은 통로 줄(
#....................???)은 입구에서 1.5 m 쯤(x ≈ 4 m)까지만.이고 안쪽은?입니다. 입구 앞을 0.5 m 간격으로 지나가며 찍었기 때문에 통로 방향으로 정확히 겨눈 광선이 적었습니다. - 오른쪽 아래(선반 A 아래 통로 · 출발점 오른쪽 아래)는 대부분
?입니다. 지도는 로봇이 본 만큼만 생깁니다.
좁은 통로(y = 4.05 ~ 4.55, 폭 0.5 m) 입구 앞에서 로봇이 위아래로 움직이며 8장을 찍습니다. 해상도 0.02 · 0.05 · 0.1 · 0.2 m 로 각각 지도를 만들고, x = 3.5 m 에서 통로를 가로지르는 단면을 출력해 빈 칸 수를 셉니다. 예측 먼저: 0.2 m 에서 0.5 m 통로는 빈 칸 몇 개로 보일까요? 0.02 m 에서는 25칸이 모두 . 일까요?
import math
import numpy as np
# 선반 통로 맵 (교육용, 단위 m) - 사각형 (x0, y0, x1, y1)
BOXES = [(0, 0, 12, 0.1), (0, 7.9, 12, 8), (0, 0, 0.1, 8), (11.9, 0, 12, 8), # 바깥 벽
(2.5, 1.4, 9.5, 2.0), (2.5, 3.45, 9.5, 4.05), (2.5, 4.55, 9.5, 5.15), # 선반 A, B, C
(10.6, 3.8, 11.0, 4.2)] # 굵은 기둥
POLES = [(0.7, 4.8, 0.03)] # 좁은 기둥 (지름 6 cm)
def sim_scan(x, y, th, n=360, rmax=8.0, rmin=0.12, noise=0.01, drop=0.01, seed=0):
"""시뮬레이터 LiDAR 한 장 -> LaserScan 과 같은 이름의 dict (교육용)"""
rng = np.random.default_rng(seed)
a = -math.pi + np.arange(n) * 2 * math.pi / n + th
dx, dy = np.cos(a)[:, None], np.sin(a)[:, None]
S = []
for x0, y0, x1, y1 in BOXES:
S += [(x0, y0, x1, y0), (x1, y0, x1, y1), (x1, y1, x0, y1), (x0, y1, x0, y0)]
S = np.array(S, float)
sx, sy, qx, qy = S[:, 2] - S[:, 0], S[:, 3] - S[:, 1], S[:, 0] - x, S[:, 1] - y
with np.errstate(divide="ignore", invalid="ignore"):
den = dx * sy - dy * sx
t = (qx * sy - qy * sx) / den
u = (qx * dy - qy * dx) / den
t = np.where((t > 1e-9) & (u >= 0) & (u <= 1), t, np.inf).min(axis=1)
for cx, cy, cr in POLES:
b = (x - cx) * dx[:, 0] + (y - cy) * dy[:, 0]
disc = b * b - ((x - cx) ** 2 + (y - cy) ** 2 - cr * cr)
tc = -b - np.sqrt(np.maximum(disc, 0))
t = np.minimum(t, np.where((disc >= 0) & (tc > 1e-9), tc, np.inf))
r = t + rng.normal(0, noise, n)
r[t > rmax] = np.inf
r[(r < rmin) | (rng.random(n) < drop)] = 0.0
return {"angle_min": -math.pi, "angle_increment": 2 * math.pi / n,
"range_min": rmin, "range_max": rmax, "ranges": r.tolist()}
def bresenham(i0, j0, i1, j1):
cells = []
di, dj = abs(i1 - i0), abs(j1 - j0)
si, sj = (1 if i1 > i0 else -1), (1 if j1 > j0 else -1)
e = di - dj
while True:
cells.append((i0, j0))
if i0 == i1 and j0 == j1:
return cells
e2 = 2 * e
if e2 > -dj:
e -= dj
i0 += si
if e2 < di:
e += di
j0 += sj
def build(res, poses):
nx, ny = int(round(12 / res)), int(round(8 / res))
L = np.zeros((ny, nx))
ops = 0
for s, (x, y, th) in enumerate(poses):
scan = sim_scan(x, y, th, n=360, rmax=5.0, seed=s)
ci, cj = int(x / res), int(y / res)
for k, r in enumerate(scan["ranges"]):
if not (scan["range_min"] <= r <= scan["range_max"]):
continue
a = th + scan["angle_min"] + k * scan["angle_increment"]
ei, ej = int((x + r * math.cos(a)) / res), int((y + r * math.sin(a)) / res)
cells = bresenham(ci, cj, ei, ej)
ops += len(cells)
for i, j in cells[:-1]:
L[j, i] = max(-5.0, L[j, i] - 0.4)
L[ej, ei] = min(5.0, L[ej, ei] + 0.85)
ros = np.where(L > 0.619, 100, np.where(L < -1.099, 0, -1))
return ros, ops
# 좁은 통로(선반 B 와 C 사이, y = 4.05 ~ 4.55, 폭 0.5 m) 입구 앞에서 위아래로 움직이며 8장
poses = [(1.6, 3.9 + 0.1 * k, 0.0) for k in range(8)]
print(" 해상도 격자 셀 수 메모리(int8) 셀 갱신 수 통로 단면 (x = 3.5 m, 아래 -> 위)")
for res in (0.02, 0.05, 0.1, 0.2):
ros, ops = build(res, poses)
i = int(3.5 / res)
js = [j for j in range(ros.shape[0]) if 3.7 <= (j + 0.5) * res <= 4.9]
col = "".join({100: "#", 0: ".", -1: "?"}[ros[j, i]] for j in js)
nfree = 0 # TODO: js 중 셀 중심 (j + 0.5) * res 가 4.05 ~ 4.55 사이이고 값이 0(빈 칸)인 셀 수
print("%5.2f m %3d x %3d %7d %9.1f kB %9d %s (빈 칸 %d개 = %.2f m)"
% (res, ros.shape[1], ros.shape[0], ros.size, ros.size / 1024, ops, col, nfree, nfree * res))풀이 코드 보기
import math
import numpy as np
# 선반 통로 맵 (교육용, 단위 m) - 사각형 (x0, y0, x1, y1)
BOXES = [(0, 0, 12, 0.1), (0, 7.9, 12, 8), (0, 0, 0.1, 8), (11.9, 0, 12, 8), # 바깥 벽
(2.5, 1.4, 9.5, 2.0), (2.5, 3.45, 9.5, 4.05), (2.5, 4.55, 9.5, 5.15), # 선반 A, B, C
(10.6, 3.8, 11.0, 4.2)] # 굵은 기둥
POLES = [(0.7, 4.8, 0.03)] # 좁은 기둥 (지름 6 cm)
def sim_scan(x, y, th, n=360, rmax=8.0, rmin=0.12, noise=0.01, drop=0.01, seed=0):
"""시뮬레이터 LiDAR 한 장 -> LaserScan 과 같은 이름의 dict (교육용)"""
rng = np.random.default_rng(seed)
a = -math.pi + np.arange(n) * 2 * math.pi / n + th
dx, dy = np.cos(a)[:, None], np.sin(a)[:, None]
S = []
for x0, y0, x1, y1 in BOXES:
S += [(x0, y0, x1, y0), (x1, y0, x1, y1), (x1, y1, x0, y1), (x0, y1, x0, y0)]
S = np.array(S, float)
sx, sy, qx, qy = S[:, 2] - S[:, 0], S[:, 3] - S[:, 1], S[:, 0] - x, S[:, 1] - y
with np.errstate(divide="ignore", invalid="ignore"):
den = dx * sy - dy * sx
t = (qx * sy - qy * sx) / den
u = (qx * dy - qy * dx) / den
t = np.where((t > 1e-9) & (u >= 0) & (u <= 1), t, np.inf).min(axis=1)
for cx, cy, cr in POLES:
b = (x - cx) * dx[:, 0] + (y - cy) * dy[:, 0]
disc = b * b - ((x - cx) ** 2 + (y - cy) ** 2 - cr * cr)
tc = -b - np.sqrt(np.maximum(disc, 0))
t = np.minimum(t, np.where((disc >= 0) & (tc > 1e-9), tc, np.inf))
r = t + rng.normal(0, noise, n)
r[t > rmax] = np.inf
r[(r < rmin) | (rng.random(n) < drop)] = 0.0
return {"angle_min": -math.pi, "angle_increment": 2 * math.pi / n,
"range_min": rmin, "range_max": rmax, "ranges": r.tolist()}
def bresenham(i0, j0, i1, j1):
cells = []
di, dj = abs(i1 - i0), abs(j1 - j0)
si, sj = (1 if i1 > i0 else -1), (1 if j1 > j0 else -1)
e = di - dj
while True:
cells.append((i0, j0))
if i0 == i1 and j0 == j1:
return cells
e2 = 2 * e
if e2 > -dj:
e -= dj
i0 += si
if e2 < di:
e += di
j0 += sj
def build(res, poses):
nx, ny = int(round(12 / res)), int(round(8 / res))
L = np.zeros((ny, nx))
ops = 0
for s, (x, y, th) in enumerate(poses):
scan = sim_scan(x, y, th, n=360, rmax=5.0, seed=s)
ci, cj = int(x / res), int(y / res)
for k, r in enumerate(scan["ranges"]):
if not (scan["range_min"] <= r <= scan["range_max"]):
continue
a = th + scan["angle_min"] + k * scan["angle_increment"]
ei, ej = int((x + r * math.cos(a)) / res), int((y + r * math.sin(a)) / res)
cells = bresenham(ci, cj, ei, ej)
ops += len(cells)
for i, j in cells[:-1]:
L[j, i] = max(-5.0, L[j, i] - 0.4)
L[ej, ei] = min(5.0, L[ej, ei] + 0.85)
ros = np.where(L > 0.619, 100, np.where(L < -1.099, 0, -1))
return ros, ops
# 좁은 통로(선반 B 와 C 사이, y = 4.05 ~ 4.55, 폭 0.5 m) 입구 앞에서 위아래로 움직이며 8장
poses = [(1.6, 3.9 + 0.1 * k, 0.0) for k in range(8)]
print(" 해상도 격자 셀 수 메모리(int8) 셀 갱신 수 통로 단면 (x = 3.5 m, 아래 -> 위)")
for res in (0.02, 0.05, 0.1, 0.2):
ros, ops = build(res, poses)
i = int(3.5 / res)
js = [j for j in range(ros.shape[0]) if 3.7 <= (j + 0.5) * res <= 4.9]
col = "".join({100: "#", 0: ".", -1: "?"}[ros[j, i]] for j in js)
nfree = sum(1 for j in js if ros[j, i] == 0 and 4.05 < (j + 0.5) * res < 4.55)
print("%5.2f m %3d x %3d %7d %9.1f kB %9d %s (빈 칸 %d개 = %.2f m)"
% (res, ros.shape[1], ros.shape[0], ros.size, ros.size / 1024, ops, col, nfree, nfree * res))실행 결과:
해상도 격자 셀 수 메모리(int8) 셀 갱신 수 통로 단면 (x = 3.5 m, 아래 -> 위)
0.02 m 600 x 400 240000 234.4 kB 325017 ??????????????????.?..?.?..?.?.?..?..?.?..?????????????????? (빈 칸 15개 = 0.30 m)
0.05 m 240 x 160 38400 37.5 kB 131681 ??????.?.........??????? (빈 칸 9개 = 0.45 m)
0.10 m 120 x 80 9600 9.4 kB 67243 ????....???? (빈 칸 4개 = 0.40 m)
0.20 m 60 x 40 2400 2.3 kB 35082 ??..??? (빈 칸 2개 = 0.40 m)- 셀 크기가 1/10 이 되면(0.2 → 0.02) 셀 수 · 메모리는 100배, 셀 갱신 횟수(계산량)는 약 9배입니다.
- 0.2 m 에서 0.5 m 통로는 빈 칸 2개(0.4 m) 로 보입니다. 통로 양쪽 벽이 셀 한가운데를 지나므로, 셀 경계 위치에 따라 1칸(0.2 m)까지 줄 수도 있습니다.
- 0.02 m 는 가장 정밀해야 하지만 실제로는
.와?가 섞여 빈 칸이 15개뿐입니다. 광선 사이에 빠진 셀이 많기 때문입니다. 촘촘한 격자는 촘촘한 데이터(더 많은 스캔 · 더 많은 광선)가 있어야 제값을 합니다. - 통로 벽은 x = 3.5 m 에서 모두
?입니다. 입구에서 보면 통로 벽은 광선이 거의 평행하게 스치므로 점이 거의 찍히지 않습니다(1교시 관찰 포인트).
ROS OccupancyGrid 를 받아 보니 어떤 셀 값이 -1, 다른 셀은 0, 또 다른 셀은 100 이었습니다. 바르게 짝지은 것은?
- −1 = 장애물, 0 = 빈 칸, 100 = 미확인
- −1 = 미확인, 0 = 빈 칸, 100 = 장애물
- −1 = 빈 칸, 0 = 미확인, 100 = 장애물
- −1 = 미확인, 0 = 장애물, 100 = 빈 칸
정답 보기
② — OccupancyGrid 값은 "점유 확률 %" 이므로 0 이 빈 칸, 100 이 장애물, −1 이 한 번도 보지 못한 셀입니다. ④ 는 pgm 이미지(0 = 검정 = 장애물)와 헷갈린 경우입니다.시뮬레이터 2 에서 해상도를 0.05 m → 0.2 m 로 바꿨더니 폭 0.5 m 좁은 통로가 빈 칸 1~2개로 보이고, 선반 벽은 두껍게 보였습니다. 이 지도로 폭 0.35 m 로봇의 경로를 계획할 때 가장 걱정되는 것은?
- 셀 수가 늘어 계산이 느려진다
- 통로가 셀 단위로 좁아지거나 군데군데 끊겨 "지나갈 수 없음" 으로 판단될 수 있다
- 미확인 셀이 모두 빈 칸으로 바뀐다
- 로그 오즈 값이 음수가 된다
정답 보기
② — 0.2 m 셀은 벽이 걸친 셀 전체를 장애물로 만들어 통로가 0.2 m 단위로 줄고, 몇 열은 빈 칸이 끊깁니다. 경로 계획은 여기에 로봇 반지름만큼 여유(inflation)를 더 두므로 실제로는 지나갈 수 있는 통로를 막힌 것으로 볼 수 있습니다(ch08). 셀 수는 오히려 1/16 로 줄고(①), 미확인은 그대로입니다(③).3교시 · SLAM 의 원리: 지도와 위치를 동시에#
닭과 달걀 문제#
2교시 시뮬레이터는 로봇 위치를 정확히 안다고 두고 지도를 그렸습니다. 실제로는 그렇지 않습니다.
- 스캔을 지도에 붙이려면 그 순간 로봇이 어디서 어느 방향을 보고 있었는지 알아야 합니다. 위치가 10 cm 틀리면 벽도 10 cm 틀린 자리에 그려집니다.
- 그런데 ch06 에서 본 것처럼 오도메트리는 시간이 지날수록 흐릅니다. 바깥 기준(지도)이 있어야 위치를 바로잡을 수 있습니다.
- 지도가 있어야 위치를 알고, 위치를 알아야 지도를 그립니다. 이 둘을 동시에 풀어 가는 것이 SLAM(Simultaneous Localization And Mapping, 동시적 위치 추정 및 지도 작성)입니다.
스캔 매칭: 새 스캔을 지도에 겹쳐 보기#
스캔 매칭(scan matching) 은 "새 스캔을 어디에 놓아야 지금까지의 지도와 가장 잘 겹치는가" 를 찾는 일입니다.
- 오도메트리가 말하는 자세 (x, y, θ) 에서 출발합니다.
- 후보 보정 (Δx, Δy, Δθ) 를 하나 골라 스캔 점들을 그 자세로 옮깁니다(ch02 의 회전 + 평행이동).
- 옮긴 점이 지도의 장애물 셀에 얼마나 많이 떨어지는지를 점수로 매깁니다.
- 정해진 범위(예: ±0.3 m, ±10°)를 격자처럼 모두 시험해 점수가 가장 높은 보정을 고릅니다. 이것이 가장 단순한 격자 탐색(brute-force search) 입니다.
실제 SLAM 패키지는 점수를 부드럽게 만들거나(장애물에서 멀어질수록 천천히 줄어드는 점수), 굵은 격자로 대충 찾은 뒤 가는 격자로 다듬는 방식(coarse-to-fine)으로 같은 일을 빠르게 합니다.
시뮬레이터 3 · 스캔 매칭 손으로 해 보기#
회색 지도는 로봇이 통로를 따라 찍은 스캔으로 이미 만든 것입니다. 로봇이 조금 더 가서 새 스캔(주황 점)을 찍었는데, 오도메트리가 말하는 자세가 조금 틀렸습니다. 슬라이더로 주황 점이 회색 지도의 벽에 딱 겹치게 맞춰 보세요. 아래 왼쪽 그림은 지금 Δθ 에서 Δx · Δy 를 바꿨을 때의 점수 지형도입니다.
- 선반 통로에서 Δθ 부터 맞춥니다. 점들이 벽과 나란해질 때까지 Δθ 만 움직이고, 그다음 Δx · Δy 를 맞춥니다. 점수가 몇까지 오르나요? "실제 자세 보기" 를 켜서 얼마나 가까운지 확인합니다.
- Δθ 가 틀린 상태(예: 0°)에서 지형도를 보고, Δθ 를 맞춘 뒤 지형도를 다시 봅니다. 밝은 봉우리가 어떻게 바뀌나요? 왜 각도를 먼저 맞추는 것이 쉬울까요?
- 🔎 격자 탐색으로 찾기를 누릅니다. 손으로 찾은 값과 비교하세요. 🎲 로 오차를 3번 바꿔 가며 격자 탐색이 매번 맞히는지 봅니다.
- 장소를 긴 복도 (특징 없음) 로 바꾸기 전에, 지형도가 어떤 모양일지 예측합니다(점 하나? 띠?). 바꾼 뒤 격자 탐색 결과와 "실제 자세와의 차이" 를 확인합니다.
- 회전이 틀리면 먼 점일수록 많이 어긋나(먼 점 이동 = 거리 × Δθ) 어떤 Δx · Δy 로도 잘 겹치지 않습니다. 지형도의 봉우리가 낮고 흐립니다. Δθ 를 맞추면 봉우리가 하나로 또렷해집니다.
- 선반 통로에서는 선반 끝 · 기둥 · 벽 모서리 같은 특징이 x · y 를 모두 붙잡아 주므로 격자 탐색이 실제 자세에서 1~2 cm 안쪽을 찾습니다.
- 긴 복도에서는 좌우 벽이 y 와 θ 는 붙잡지만, x 방향으로 밀어도 벽은 계속 벽이므로 지형도에 x 방향 띠(능선) 가 생깁니다. 격자 탐색은 그 띠 위에서 조금이라도 높은 곳을 고르므로 x 가 수십 cm 틀린 답을 냅니다. 이것이 4교시 "특징 없는 환경" 문제의 핵심입니다.
시뮬레이터 4 · 지도 작성 모드: 오도메트리 vs 스캔 매칭#
이번에는 로봇이 창고를 한 바퀴(약 23.5 m, 0.3 m/s) 돌며 처음부터 지도를 그립니다. 오도메트리에는 ch06 에서 본 회전 바이어스 · 거리 오차 · 미끄러짐을 넣었습니다. 스캔 매칭을 끄면 오도메트리 자세 그대로 지도를 그리고, 켜면 매 스캔마다 ±0.1 m · ±3° 범위를 격자 탐색해 자세를 고친 뒤 그립니다.
- 스캔 매칭을 끈 채 ▶ 처음부터 재생을 누릅니다. 로봇이 첫 모퉁이를 돌기 전과 후에 지도의 벽이 어떻게 달라지나요? 한 바퀴를 돌아 출발점 근처에 왔을 때 무엇이 보이나요?
- 회전 오차를 5 % → 0 % 로 바꾸고, 거리 오차만 남깁니다. 벽이 휘는 모습과 밀리는 모습 중 어느 것이 사라지나요?
- 스캔 매칭을 켜기 전에 예측합니다: 회전 오차 5 % · 거리 오차 2 % 그대로일 때 "벽 제자리 비율" 은 몇 % 가 될까요? 켜고 확인한 뒤, 보라 점선(오도메트리만)과 주황(추정)을 비교합니다.
- 스캔 매칭을 켠 채 회전 오차를 10 %, 미끄러짐 잡음을 3 으로 최대로 올립니다. 그래도 지도가 유지되나요? 🎲 로 몇 번 바꿔 봅니다.
- 스캔 매칭 없이 회전 바이어스가 있으면 모퉁이를 돌 때마다 방향이 몇 도씩 틀어지고, 그 뒤의 벽이 비스듬히 그려집니다. 출발점에 돌아오면 처음 그린 벽과 나중에 그린 벽이 두 겹으로 겹칩니다. 기본값에서 벽 제자리 비율은 약 50 %, 위치 오차는 1 m 를 넘습니다.
- 회전 오차가 0 이면 벽이 휘지 않고, 거리 오차만큼 길이가 늘거나 밀립니다. ch06 에서 본 "각도 오차가 위치 오차를 키운다" 를 지도에서 직접 보는 셈입니다.
- 스캔 매칭을 켜면 보라 점선(오도메트리)은 그대로 흘러가지만 주황(추정)은 초록(실제)을 따라가고, 벽 제자리 비율이 거의 100 % 가 됩니다. 매 스캔의 작은 오차를 지도가 바로잡아 주므로 오차가 쌓이지 않습니다.
- 매칭이 고칠 수 있는 범위(여기서는 ±0.1 m · ±3°)보다 한 걸음의 오차가 커지면 틀린 자리에 맞추고, 그 틀린 지도에 다음 스캔을 맞추므로 한번 틀어지면 회복하기 어렵습니다(4교시 빠른 주행).
Gmapping 의 직관: 여러 가설 로봇을 동시에 굴리기#
myAGV Plus 공식 문서가 안내하는 Gmapping 은 스캔 매칭에 파티클 필터(particle filter) 를 더한 SLAM 입니다. 직관은 다음과 같습니다.
- 가설 로봇(파티클) 을 여러 대(ROS gmapping 기본 30개) 둡니다. 각 파티클은 "로봇이 지금까지 이 경로로 왔다" 는 가설 하나와 그 가설로 그린 자기만의 지도를 가집니다.
- 오도메트리로 모든 파티클을 움직이되, 파티클마다 조금씩 다른 잡음을 넣습니다. 그래야 "바퀴가 미끄러졌을지도 모른다" 는 여러 가능성을 함께 따라갑니다.
- 새 스캔이 오면 각 파티클이 스캔 매칭으로 자세를 다듬고, 자기 지도와 얼마나 잘 맞는지로 가중치를 받습니다.
- 가중치가 한쪽으로 쏠리면 리샘플링: 그럴듯한 파티클은 복제하고 엉터리 파티클은 지웁니다.
- 출력하는 지도는 가중치가 가장 큰 파티클의 지도입니다.
루프 클로저(loop closure) 는 이 구조에서 자연스럽게 일어납니다. 한 바퀴 돌아 처음 본 곳에 다시 오면, 경로가 정확했던 파티클의 지도는 새 스캔과 잘 맞고 경로가 흐른 파티클의 지도는 벽이 두 겹이 되어 점수가 낮습니다. 리샘플링으로 좋은 파티클만 남으면 지금 위치뿐 아니라 지나온 지도 전체가 일관된 쪽으로 정리됩니다. 파티클 수가 너무 적으면 "정답에 가까운 가설" 이 이미 사라진 뒤라 루프를 닫지 못합니다.
ROS 2 에는 Gmapping 외에 그래프 기반 SLAM(예: slam_toolbox, Cartographer)도 있습니다. 이들은 파티클 대신 "자세들 사이의 제약" 을 그래프로 모아 두었다가 루프를 찾으면 그래프 전체를 다시 최적화합니다. 실행 방법과 비교는 ch10 에서 다룹니다.
실습 과제 3#
기준 지도는 정확한 자세에서 찍은 스캔 3장의 끝점(장애물 셀 = 1, 이웃 셀 = 0.5)입니다. 새 스캔의 실제 자세는 (5.4, 2.75, 3°) 인데 오도메트리는 (5.55, 2.62, 9°) 라고 말합니다. Δx · Δy 를 ±0.3 m (0.05 m 간격), Δθ 를 ±10° (1° 간격)로 모두 시험해 가장 잘 맞는 보정을 찾으세요. 예측 먼저: 정답 보정은 대략 (Δx, Δy, Δθ) = (?, ?, ?) 일까요? 오도메트리 그대로의 점수는 높을까요, 낮을까요?
import math
import numpy as np
# 선반 통로 맵 (교육용, 단위 m) - 사각형 (x0, y0, x1, y1)
BOXES = [(0, 0, 12, 0.1), (0, 7.9, 12, 8), (0, 0, 0.1, 8), (11.9, 0, 12, 8), # 바깥 벽
(2.5, 1.4, 9.5, 2.0), (2.5, 3.45, 9.5, 4.05), (2.5, 4.55, 9.5, 5.15), # 선반 A, B, C
(10.6, 3.8, 11.0, 4.2)] # 굵은 기둥
POLES = [(0.7, 4.8, 0.03)] # 좁은 기둥 (지름 6 cm)
def sim_scan(x, y, th, n=360, rmax=8.0, rmin=0.12, noise=0.01, drop=0.01, seed=0):
"""시뮬레이터 LiDAR 한 장 -> LaserScan 과 같은 이름의 dict (교육용)"""
rng = np.random.default_rng(seed)
a = -math.pi + np.arange(n) * 2 * math.pi / n + th
dx, dy = np.cos(a)[:, None], np.sin(a)[:, None]
S = []
for x0, y0, x1, y1 in BOXES:
S += [(x0, y0, x1, y0), (x1, y0, x1, y1), (x1, y1, x0, y1), (x0, y1, x0, y0)]
S = np.array(S, float)
sx, sy, qx, qy = S[:, 2] - S[:, 0], S[:, 3] - S[:, 1], S[:, 0] - x, S[:, 1] - y
with np.errstate(divide="ignore", invalid="ignore"):
den = dx * sy - dy * sx
t = (qx * sy - qy * sx) / den
u = (qx * dy - qy * dx) / den
t = np.where((t > 1e-9) & (u >= 0) & (u <= 1), t, np.inf).min(axis=1)
for cx, cy, cr in POLES:
b = (x - cx) * dx[:, 0] + (y - cy) * dy[:, 0]
disc = b * b - ((x - cx) ** 2 + (y - cy) ** 2 - cr * cr)
tc = -b - np.sqrt(np.maximum(disc, 0))
t = np.minimum(t, np.where((disc >= 0) & (tc > 1e-9), tc, np.inf))
r = t + rng.normal(0, noise, n)
r[t > rmax] = np.inf
r[(r < rmin) | (rng.random(n) < drop)] = 0.0
return {"angle_min": -math.pi, "angle_increment": 2 * math.pi / n,
"range_min": rmin, "range_max": rmax, "ranges": r.tolist()}
RES = 0.05
NX, NY = int(12 / RES), int(8 / RES)
def endpoints(x, y, th, scan):
"""스캔 -> 지도 좌표의 점들 (N x 2)"""
r = np.array(scan["ranges"])
a = th + scan["angle_min"] + np.arange(len(r)) * scan["angle_increment"]
ok = (r >= scan["range_min"]) & (r <= scan["range_max"])
return np.stack([x + r[ok] * np.cos(a[ok]), y + r[ok] * np.sin(a[ok])], axis=1)
# 1) 기준 지도: 정확한 자세에서 찍은 스캔 3장의 끝점을 '장애물' 로 표시하고 이웃 셀은 0.5 점
like = np.zeros((NY, NX))
for s, (x, y, th) in enumerate([(4.4, 2.7, 0.0), (4.7, 2.7, 0.0), (5.0, 2.7, 0.0)]):
P = endpoints(x, y, th, sim_scan(x, y, th, seed=s))
i, j = (P[:, 0] / RES).astype(int), (P[:, 1] / RES).astype(int)
for di in (-1, 0, 1):
for dj in (-1, 0, 1):
like[np.clip(j + dj, 0, NY - 1), np.clip(i + di, 0, NX - 1)] = np.maximum(
like[np.clip(j + dj, 0, NY - 1), np.clip(i + di, 0, NX - 1)], 0.5)
like[j, i] = 1.0
# 2) 새 스캔: 실제 자세는 (5.4, 2.75, 3 deg) 인데 오도메트리는 (5.55, 2.62, 9 deg) 라고 말함
true = (5.4, 2.75, math.radians(3))
odom = (5.55, 2.62, math.radians(9))
scan = sim_scan(*true, seed=9)
r = np.array(scan["ranges"])
a = scan["angle_min"] + np.arange(len(r)) * scan["angle_increment"]
ok = (r >= scan["range_min"]) & (r <= scan["range_max"])
local = np.stack([r[ok] * np.cos(a[ok]), r[ok] * np.sin(a[ok])], axis=1) # 로봇 기준 점
def score(x, y, th):
"""로봇 기준 점들을 자세 (x, y, th) 로 옮겨 놓았을 때 기준 지도와 겹치는 정도 (0 ~ 1)"""
# TODO 1: local 점들을 th 만큼 돌리고 (x, y) 만큼 옮겨 지도 좌표 gx, gy 를 만듦 (ch02 의 회전)
# TODO 2: 셀 번호 i = gx / RES, j = gy / RES (정수, 격자 밖은 clip) 로 like[j, i] 의 평균을 돌려줌
return 0.0
best = (-1.0, (0.0, 0.0, 0.0))
for dth in np.radians(np.arange(-10, 10.5, 1.0)):
for dx in np.arange(-0.3, 0.301, RES):
for dy in np.arange(-0.3, 0.301, RES):
pass # TODO 3: score(odom 에 dx, dy, dth 를 더한 자세) 가 best[0] 보다 크면 best 를 바꿈
s, (dx, dy, dth) = best
est = (odom[0] + dx, odom[1] + dy, odom[2] + dth)
print("오도메트리 그대로 점수: %.3f" % score(*odom))
print("가장 잘 맞는 보정: dx = %+.2f m, dy = %+.2f m, dth = %+.0f deg (점수 %.3f)"
% (dx, dy, math.degrees(dth), s))
print("보정한 자세 : (%.2f, %.2f, %.1f deg)" % (est[0], est[1], math.degrees(est[2])))
print("실제 자세 : (%.2f, %.2f, %.1f deg)" % (true[0], true[1], math.degrees(true[2])))
print("남은 오차 : %.1f cm, %.1f deg" % (100 * math.hypot(est[0] - true[0], est[1] - true[1]),
math.degrees(est[2] - true[2])))풀이 코드 보기
import math
import numpy as np
# 선반 통로 맵 (교육용, 단위 m) - 사각형 (x0, y0, x1, y1)
BOXES = [(0, 0, 12, 0.1), (0, 7.9, 12, 8), (0, 0, 0.1, 8), (11.9, 0, 12, 8), # 바깥 벽
(2.5, 1.4, 9.5, 2.0), (2.5, 3.45, 9.5, 4.05), (2.5, 4.55, 9.5, 5.15), # 선반 A, B, C
(10.6, 3.8, 11.0, 4.2)] # 굵은 기둥
POLES = [(0.7, 4.8, 0.03)] # 좁은 기둥 (지름 6 cm)
def sim_scan(x, y, th, n=360, rmax=8.0, rmin=0.12, noise=0.01, drop=0.01, seed=0):
"""시뮬레이터 LiDAR 한 장 -> LaserScan 과 같은 이름의 dict (교육용)"""
rng = np.random.default_rng(seed)
a = -math.pi + np.arange(n) * 2 * math.pi / n + th
dx, dy = np.cos(a)[:, None], np.sin(a)[:, None]
S = []
for x0, y0, x1, y1 in BOXES:
S += [(x0, y0, x1, y0), (x1, y0, x1, y1), (x1, y1, x0, y1), (x0, y1, x0, y0)]
S = np.array(S, float)
sx, sy, qx, qy = S[:, 2] - S[:, 0], S[:, 3] - S[:, 1], S[:, 0] - x, S[:, 1] - y
with np.errstate(divide="ignore", invalid="ignore"):
den = dx * sy - dy * sx
t = (qx * sy - qy * sx) / den
u = (qx * dy - qy * dx) / den
t = np.where((t > 1e-9) & (u >= 0) & (u <= 1), t, np.inf).min(axis=1)
for cx, cy, cr in POLES:
b = (x - cx) * dx[:, 0] + (y - cy) * dy[:, 0]
disc = b * b - ((x - cx) ** 2 + (y - cy) ** 2 - cr * cr)
tc = -b - np.sqrt(np.maximum(disc, 0))
t = np.minimum(t, np.where((disc >= 0) & (tc > 1e-9), tc, np.inf))
r = t + rng.normal(0, noise, n)
r[t > rmax] = np.inf
r[(r < rmin) | (rng.random(n) < drop)] = 0.0
return {"angle_min": -math.pi, "angle_increment": 2 * math.pi / n,
"range_min": rmin, "range_max": rmax, "ranges": r.tolist()}
RES = 0.05
NX, NY = int(12 / RES), int(8 / RES)
def endpoints(x, y, th, scan):
"""스캔 -> 지도 좌표의 점들 (N x 2)"""
r = np.array(scan["ranges"])
a = th + scan["angle_min"] + np.arange(len(r)) * scan["angle_increment"]
ok = (r >= scan["range_min"]) & (r <= scan["range_max"])
return np.stack([x + r[ok] * np.cos(a[ok]), y + r[ok] * np.sin(a[ok])], axis=1)
# 1) 기준 지도: 정확한 자세에서 찍은 스캔 3장의 끝점을 '장애물' 로 표시하고 이웃 셀은 0.5 점
like = np.zeros((NY, NX))
for s, (x, y, th) in enumerate([(4.4, 2.7, 0.0), (4.7, 2.7, 0.0), (5.0, 2.7, 0.0)]):
P = endpoints(x, y, th, sim_scan(x, y, th, seed=s))
i, j = (P[:, 0] / RES).astype(int), (P[:, 1] / RES).astype(int)
for di in (-1, 0, 1):
for dj in (-1, 0, 1):
like[np.clip(j + dj, 0, NY - 1), np.clip(i + di, 0, NX - 1)] = np.maximum(
like[np.clip(j + dj, 0, NY - 1), np.clip(i + di, 0, NX - 1)], 0.5)
like[j, i] = 1.0
# 2) 새 스캔: 실제 자세는 (5.4, 2.75, 3 deg) 인데 오도메트리는 (5.55, 2.62, 9 deg) 라고 말함
true = (5.4, 2.75, math.radians(3))
odom = (5.55, 2.62, math.radians(9))
scan = sim_scan(*true, seed=9)
r = np.array(scan["ranges"])
a = scan["angle_min"] + np.arange(len(r)) * scan["angle_increment"]
ok = (r >= scan["range_min"]) & (r <= scan["range_max"])
local = np.stack([r[ok] * np.cos(a[ok]), r[ok] * np.sin(a[ok])], axis=1) # 로봇 기준 점
def score(x, y, th):
"""로봇 기준 점들을 자세 (x, y, th) 로 옮겨 놓았을 때 기준 지도와 겹치는 정도 (0 ~ 1)"""
c, s = math.cos(th), math.sin(th)
gx = x + local[:, 0] * c - local[:, 1] * s
gy = y + local[:, 0] * s + local[:, 1] * c
i = np.clip((gx / RES).astype(int), 0, NX - 1)
j = np.clip((gy / RES).astype(int), 0, NY - 1)
return like[j, i].mean()
best = (-1.0, (0.0, 0.0, 0.0))
for dth in np.radians(np.arange(-10, 10.5, 1.0)):
for dx in np.arange(-0.3, 0.301, RES):
for dy in np.arange(-0.3, 0.301, RES):
s = score(odom[0] + dx, odom[1] + dy, odom[2] + dth)
if s > best[0]:
best = (s, (dx, dy, dth))
s, (dx, dy, dth) = best
est = (odom[0] + dx, odom[1] + dy, odom[2] + dth)
print("오도메트리 그대로 점수: %.3f" % score(*odom))
print("가장 잘 맞는 보정: dx = %+.2f m, dy = %+.2f m, dth = %+.0f deg (점수 %.3f)"
% (dx, dy, math.degrees(dth), s))
print("보정한 자세 : (%.2f, %.2f, %.1f deg)" % (est[0], est[1], math.degrees(est[2])))
print("실제 자세 : (%.2f, %.2f, %.1f deg)" % (true[0], true[1], math.degrees(true[2])))
print("남은 오차 : %.1f cm, %.1f deg" % (100 * math.hypot(est[0] - true[0], est[1] - true[1]),
math.degrees(est[2] - true[2])))실행 결과:
오도메트리 그대로 점수: 0.190
가장 잘 맞는 보정: dx = -0.15 m, dy = +0.15 m, dth = -6 deg (점수 0.909)
보정한 자세 : (5.40, 2.77, 3.0 deg)
실제 자세 : (5.40, 2.75, 3.0 deg)
남은 오차 : 2.0 cm, -0.0 deg- 오도메트리 그대로는 점의 19 % 만 지도와 겹치고, 보정 후에는 91 % 가 겹칩니다.
- 남은 오차 2 cm 는 탐색 간격(0.05 m)보다 작습니다. 격자 탐색의 정밀도는 탐색 간격에, 찾을 수 있는 오차의 크기는 탐색 범위에 달려 있습니다.
- 시험한 후보는 13 × 13 × 21 = 3,549 개입니다. 범위를 2배로 넓히면 Δx · Δy 각각 2배라 약 4배(각도까지 넓히면 8배)가 됩니다. 실제 SLAM 이 오도메트리 예측을 먼저 쓰고 그 근처만 찾는 이유입니다.
지도는 이미 있다고 두고, 로봇이 선반 통로를 0.6 m 씩 10걸음 갑니다. 바퀴는 매번 6 % 더 갔고 1° 더 돌았다고 거짓말을 합니다. 가설 로봇 50개를 오도메트리 + 잡음으로 움직이고, 스캔이 지도와 맞는 정도로 가중치를 주고, 리샘플링합니다. 예측 먼저: 10걸음 뒤 오도메트리의 위치 오차는 몇 cm 쯤일까요? 파티클 중 최고 가설은요? (6 % × 6 m = ?, 방향 10° 틀어짐도 생각합니다.)
import math
import numpy as np
# 선반 통로 맵 (교육용, 단위 m) - 사각형 (x0, y0, x1, y1)
BOXES = [(0, 0, 12, 0.1), (0, 7.9, 12, 8), (0, 0, 0.1, 8), (11.9, 0, 12, 8), # 바깥 벽
(2.5, 1.4, 9.5, 2.0), (2.5, 3.45, 9.5, 4.05), (2.5, 4.55, 9.5, 5.15), # 선반 A, B, C
(10.6, 3.8, 11.0, 4.2)] # 굵은 기둥
POLES = [(0.7, 4.8, 0.03)] # 좁은 기둥 (지름 6 cm)
def sim_scan(x, y, th, n=360, rmax=8.0, rmin=0.12, noise=0.01, drop=0.01, seed=0):
"""시뮬레이터 LiDAR 한 장 -> LaserScan 과 같은 이름의 dict (교육용)"""
rng = np.random.default_rng(seed)
a = -math.pi + np.arange(n) * 2 * math.pi / n + th
dx, dy = np.cos(a)[:, None], np.sin(a)[:, None]
S = []
for x0, y0, x1, y1 in BOXES:
S += [(x0, y0, x1, y0), (x1, y0, x1, y1), (x1, y1, x0, y1), (x0, y1, x0, y0)]
S = np.array(S, float)
sx, sy, qx, qy = S[:, 2] - S[:, 0], S[:, 3] - S[:, 1], S[:, 0] - x, S[:, 1] - y
with np.errstate(divide="ignore", invalid="ignore"):
den = dx * sy - dy * sx
t = (qx * sy - qy * sx) / den
u = (qx * dy - qy * dx) / den
t = np.where((t > 1e-9) & (u >= 0) & (u <= 1), t, np.inf).min(axis=1)
for cx, cy, cr in POLES:
b = (x - cx) * dx[:, 0] + (y - cy) * dy[:, 0]
disc = b * b - ((x - cx) ** 2 + (y - cy) ** 2 - cr * cr)
tc = -b - np.sqrt(np.maximum(disc, 0))
t = np.minimum(t, np.where((disc >= 0) & (tc > 1e-9), tc, np.inf))
r = t + rng.normal(0, noise, n)
r[t > rmax] = np.inf
r[(r < rmin) | (rng.random(n) < drop)] = 0.0
return {"angle_min": -math.pi, "angle_increment": 2 * math.pi / n,
"range_min": rmin, "range_max": rmax, "ranges": r.tolist()}
RES = 0.05
NX, NY = int(12 / RES), int(8 / RES)
rng = np.random.default_rng(1)
# 지도는 이미 있다고 둡니다 (설계도의 선반 · 벽 셀 = 1, 바로 옆 셀 = 0.5)
like = np.zeros((NY, NX))
for x0, y0, x1, y1 in BOXES:
i0, i1 = int(x0 / RES), int(math.ceil(x1 / RES))
j0, j1 = int(y0 / RES), int(math.ceil(y1 / RES))
like[max(0, j0 - 1):j1 + 1, max(0, i0 - 1):i1 + 1] = np.maximum(like[max(0, j0 - 1):j1 + 1, max(0, i0 - 1):i1 + 1], 0.5)
like[j0:j1, i0:i1] = 1.0
def local_points(scan):
r = np.array(scan["ranges"])
a = scan["angle_min"] + np.arange(len(r)) * scan["angle_increment"]
ok = (r >= scan["range_min"]) & (r <= scan["range_max"])
return r[ok] * np.cos(a[ok]), r[ok] * np.sin(a[ok])
def score(p, lx, ly):
c, s = math.cos(p[2]), math.sin(p[2])
i = np.clip(((p[0] + lx * c - ly * s) / RES).astype(int), 0, NX - 1)
j = np.clip(((p[1] + lx * s + ly * c) / RES).astype(int), 0, NY - 1)
return like[j, i].mean()
N = 50
true = np.array([2.0, 2.7, 0.0])
odom = true.copy()
parts = np.tile(true, (N, 1)) # 가설 로봇 50대, 처음엔 모두 같은 자리
print("걸음 실제 (x, y) 오도메트리 위치 오차 파티클 최고 위치 오차 유효 파티클 수")
for step in range(1, 11):
d, dth = 0.6, math.radians(0.5) # 실제로는 0.6 m 전진 + 0.5 도 회전
true += [d * math.cos(true[2]), d * math.sin(true[2]), dth]
od, oth = d * 1.06, dth + math.radians(1.0) # 바퀴는 6 % 더 갔고 1 도 더 돌았다고 말함
odom += [od * math.cos(odom[2]), od * math.sin(odom[2]), oth]
# 1) 예측: 가설마다 오도메트리 이동 + 서로 다른 잡음
nd = od + rng.normal(0, 0.05, N)
nth = oth + rng.normal(0, math.radians(1.5), N)
parts[:, 0] += nd * np.cos(parts[:, 2])
parts[:, 1] += nd * np.sin(parts[:, 2])
parts[:, 2] += nth
# 2) 가중치: 실제 스캔을 각 가설 자리에 놓아 보고 지도와 얼마나 맞는지
lx, ly = local_points(sim_scan(*true, n=180, seed=step))
sc = np.array([score(p, lx, ly) for p in parts])
w = np.ones(N) / N # TODO 1: 점수가 높을수록 큰 가중치 w = exp(30 * (sc - sc.max())), 합이 1 이 되게 나눔
best = parts[np.argmax(w)].copy()
n_eff = 1.0 / np.sum(w ** 2)
# 3) 리샘플링: 가중치에 비례해 가설을 다시 뽑음 (그럴듯한 가설은 복제, 나머지는 사라짐)
# TODO 2: rng.choice(N, size=N, p=w) 로 번호를 뽑아 parts 를 다시 만듦
e_odom = math.hypot(odom[0] - true[0], odom[1] - true[1])
e_best = math.hypot(best[0] - true[0], best[1] - true[1])
print("%3d (%5.2f, %4.2f) %6.1f cm %6.1f cm %5.1f"
% (step, true[0], true[1], 100 * e_odom, 100 * e_best, n_eff))
print("마지막 방향 오차: 오도메트리 %+.1f deg, 파티클 최고 %+.1f deg"
% (math.degrees(odom[2] - true[2]), math.degrees(best[2] - true[2])))풀이 코드 보기
import math
import numpy as np
# 선반 통로 맵 (교육용, 단위 m) - 사각형 (x0, y0, x1, y1)
BOXES = [(0, 0, 12, 0.1), (0, 7.9, 12, 8), (0, 0, 0.1, 8), (11.9, 0, 12, 8), # 바깥 벽
(2.5, 1.4, 9.5, 2.0), (2.5, 3.45, 9.5, 4.05), (2.5, 4.55, 9.5, 5.15), # 선반 A, B, C
(10.6, 3.8, 11.0, 4.2)] # 굵은 기둥
POLES = [(0.7, 4.8, 0.03)] # 좁은 기둥 (지름 6 cm)
def sim_scan(x, y, th, n=360, rmax=8.0, rmin=0.12, noise=0.01, drop=0.01, seed=0):
"""시뮬레이터 LiDAR 한 장 -> LaserScan 과 같은 이름의 dict (교육용)"""
rng = np.random.default_rng(seed)
a = -math.pi + np.arange(n) * 2 * math.pi / n + th
dx, dy = np.cos(a)[:, None], np.sin(a)[:, None]
S = []
for x0, y0, x1, y1 in BOXES:
S += [(x0, y0, x1, y0), (x1, y0, x1, y1), (x1, y1, x0, y1), (x0, y1, x0, y0)]
S = np.array(S, float)
sx, sy, qx, qy = S[:, 2] - S[:, 0], S[:, 3] - S[:, 1], S[:, 0] - x, S[:, 1] - y
with np.errstate(divide="ignore", invalid="ignore"):
den = dx * sy - dy * sx
t = (qx * sy - qy * sx) / den
u = (qx * dy - qy * dx) / den
t = np.where((t > 1e-9) & (u >= 0) & (u <= 1), t, np.inf).min(axis=1)
for cx, cy, cr in POLES:
b = (x - cx) * dx[:, 0] + (y - cy) * dy[:, 0]
disc = b * b - ((x - cx) ** 2 + (y - cy) ** 2 - cr * cr)
tc = -b - np.sqrt(np.maximum(disc, 0))
t = np.minimum(t, np.where((disc >= 0) & (tc > 1e-9), tc, np.inf))
r = t + rng.normal(0, noise, n)
r[t > rmax] = np.inf
r[(r < rmin) | (rng.random(n) < drop)] = 0.0
return {"angle_min": -math.pi, "angle_increment": 2 * math.pi / n,
"range_min": rmin, "range_max": rmax, "ranges": r.tolist()}
RES = 0.05
NX, NY = int(12 / RES), int(8 / RES)
rng = np.random.default_rng(1)
# 지도는 이미 있다고 둡니다 (설계도의 선반 · 벽 셀 = 1, 바로 옆 셀 = 0.5)
like = np.zeros((NY, NX))
for x0, y0, x1, y1 in BOXES:
i0, i1 = int(x0 / RES), int(math.ceil(x1 / RES))
j0, j1 = int(y0 / RES), int(math.ceil(y1 / RES))
like[max(0, j0 - 1):j1 + 1, max(0, i0 - 1):i1 + 1] = np.maximum(like[max(0, j0 - 1):j1 + 1, max(0, i0 - 1):i1 + 1], 0.5)
like[j0:j1, i0:i1] = 1.0
def local_points(scan):
r = np.array(scan["ranges"])
a = scan["angle_min"] + np.arange(len(r)) * scan["angle_increment"]
ok = (r >= scan["range_min"]) & (r <= scan["range_max"])
return r[ok] * np.cos(a[ok]), r[ok] * np.sin(a[ok])
def score(p, lx, ly):
c, s = math.cos(p[2]), math.sin(p[2])
i = np.clip(((p[0] + lx * c - ly * s) / RES).astype(int), 0, NX - 1)
j = np.clip(((p[1] + lx * s + ly * c) / RES).astype(int), 0, NY - 1)
return like[j, i].mean()
N = 50
true = np.array([2.0, 2.7, 0.0])
odom = true.copy()
parts = np.tile(true, (N, 1)) # 가설 로봇 50대, 처음엔 모두 같은 자리
print("걸음 실제 (x, y) 오도메트리 위치 오차 파티클 최고 위치 오차 유효 파티클 수")
for step in range(1, 11):
d, dth = 0.6, math.radians(0.5) # 실제로는 0.6 m 전진 + 0.5 도 회전
true += [d * math.cos(true[2]), d * math.sin(true[2]), dth]
od, oth = d * 1.06, dth + math.radians(1.0) # 바퀴는 6 % 더 갔고 1 도 더 돌았다고 말함
odom += [od * math.cos(odom[2]), od * math.sin(odom[2]), oth]
# 1) 예측: 가설마다 오도메트리 이동 + 서로 다른 잡음
nd = od + rng.normal(0, 0.05, N)
nth = oth + rng.normal(0, math.radians(1.5), N)
parts[:, 0] += nd * np.cos(parts[:, 2])
parts[:, 1] += nd * np.sin(parts[:, 2])
parts[:, 2] += nth
# 2) 가중치: 실제 스캔을 각 가설 자리에 놓아 보고 지도와 얼마나 맞는지
lx, ly = local_points(sim_scan(*true, n=180, seed=step))
sc = np.array([score(p, lx, ly) for p in parts])
w = np.exp(30 * (sc - sc.max()))
w /= w.sum()
best = parts[np.argmax(w)].copy()
n_eff = 1.0 / np.sum(w ** 2)
# 3) 리샘플링: 가중치에 비례해 가설을 다시 뽑음 (그럴듯한 가설은 복제, 나머지는 사라짐)
parts = parts[rng.choice(N, size=N, p=w)]
e_odom = math.hypot(odom[0] - true[0], odom[1] - true[1])
e_best = math.hypot(best[0] - true[0], best[1] - true[1])
print("%3d (%5.2f, %4.2f) %6.1f cm %6.1f cm %5.1f"
% (step, true[0], true[1], 100 * e_odom, 100 * e_best, n_eff))
print("마지막 방향 오차: 오도메트리 %+.1f deg, 파티클 최고 %+.1f deg"
% (math.degrees(odom[2] - true[2]), math.degrees(best[2] - true[2])))실행 결과:
걸음 실제 (x, y) 오도메트리 위치 오차 파티클 최고 위치 오차 유효 파티클 수
1 ( 2.60, 2.70) 3.6 cm 2.9 cm 6.1
2 ( 3.20, 2.71) 7.3 cm 6.4 cm 14.8
3 ( 3.80, 2.72) 11.2 cm 3.2 cm 18.4
...
9 ( 7.40, 2.89) 49.8 cm 6.4 cm 20.8
10 ( 7.99, 2.94) 59.4 cm 15.2 cm 23.5
마지막 방향 오차: 오도메트리 +10.0 deg, 파티클 최고 +0.4 deg- 오도메트리는 거리 오차(6 %)와 방향 오차(매번 1°)가 쌓여 60 cm 가까이 흐릅니다. 파티클 최고 가설은 대부분 3~6 cm 에 머물고, 방향 오차도 0.4° 입니다.
- 유효 파티클 수(1 / Σw²)는 가중치가 고르게 퍼져 있으면 50, 한 가설에 몰리면 1 에 가깝습니다. 첫 걸음에서 6 이라는 것은 "50개 중 몇 개만 그럴듯하다" 는 뜻이고, 리샘플링으로 그 몇 개가 복제됩니다.
- 마지막 걸음에서 15 cm 로 커진 것은 통로 방향(x)의 특징이 적어지는 구간(선반 끝 근처)이기 때문입니다. 시뮬레이터 3 의 긴 복도와 같은 원리입니다.
- 실제 Gmapping 은 여기에 더해 파티클마다 자기 지도를 가지고 함께 고쳐 나갑니다. 이 실습은 그중 "가설을 여러 개 굴리고 맞는 것만 남긴다" 부분만 떼어 본 것입니다.
시뮬레이터 4 에서 스캔 매칭을 끄고 회전 오차 5 %, 거리 오차 0 % 로 한 바퀴 돌았더니, 출발점 근처에서 벽이 두 겹으로 겹쳐 보였습니다. 가장 알맞은 설명은?
- LiDAR 잡음이 커서 벽이 두껍게 그려졌다
- 모퉁이마다 방향이 조금씩 틀어져, 나중에 그린 벽이 처음 그린 벽과 다른 각도 · 위치에 놓였다
- 해상도가 너무 작아서 셀이 두 줄이 되었다
- 로봇이 출발점을 지나가지 않아서 미확인 셀이 남았다
정답 보기
② — 지도의 벽은 "그 순간 추정한 자세" 에 붙습니다. 회전 바이어스로 방향이 몇 도 틀어지면 그 뒤에 그린 모든 벽이 같이 돌아가 있고, 한 바퀴 뒤 같은 벽을 다시 볼 때 처음 것과 어긋나 두 겹이 됩니다. 스캔 매칭을 켜면 매번 지도에 맞춰 방향을 고치므로 겹침이 사라집니다.시뮬레이터 3 의 긴 복도에서 격자 탐색이 실제 자세와 x 로 수십 cm 다른 답을 냈습니다. 지형도에는 가로로 긴 밝은 띠가 있었습니다. 가장 알맞은 해석은?
- 탐색 간격이 너무 커서 정답을 건너뛰었다
- 좌우 벽만 보이는 복도에서는 x 로 밀어도 점수가 거의 같아서, 스캔만으로는 x 를 정할 수 없다
- 복도에서는 LiDAR 의 최대 거리가 짧아진다
- Δθ 가 틀려서 봉우리가 흐려졌다
정답 보기
② — 지형도의 띠는 "x 를 바꿔도 점수가 비슷하다" 는 뜻입니다. 이럴 때 스캔 매칭은 x 정보를 주지 못하고, 오도메트리에 기대거나(오차가 그대로 남음) 띠 위의 엉뚱한 곳을 고릅니다. 벽에 기둥 · 문틀 같은 특징이 있으면 띠가 봉우리로 바뀝니다(시뮬레이터 5).4교시 · 지도 작성 도전: 좋은 지도를 만드는 운전법#
공식 문서의 지도 작성 순서와 권고#
myAGV Plus 공식 문서(6.2.4 Real-time Mapping with Gmapping)는 지도를 다음 순서로 만듭니다. ROS 2 로 직접 실행하는 것은 ch09 · ch10 에서 하고, 여기서는 흐름과 운전 요령만 봅니다.
# (미리 보기 - 실행은 ch10) 공식 문서 6.2.4 의 순서
ros2 launch myagv_plus_bringup myagv_plus_bringup.launch.py # 1) 로봇 · LiDAR 켜기
ros2 launch slam_gmapping slam_gmapping.launch.py # 2) Gmapping 시작
ros2 run teleop_twist_keyboard teleop_twist_keyboard # 3) 키보드로 천천히 운전
ros2 run nav2_map_server map_saver_cli -f ~/myagv_plus_ros2/src/myagv_plus_navigation2/map/map_test # 4) 저장공식 문서의 운전 권고는 "키보드 조종은 기본 속도를 쓰라 — 낮은 속도가 대체로 더 좋은 지도를 만든다" 입니다. 여기에 SLAM 에서 널리 쓰는 요령을 더하면 좋은 지도의 운전법은 다음과 같습니다.
| 요령 | 이유 |
|---|---|
| 기본 속도 이하로 천천히 직진 | 스캔 사이 이동이 작아 스캔 매칭이 쉽고, 한 장 찍는 동안의 왜곡 · 바퀴 미끄러짐이 작음 |
| 회전도 천천히, 제자리 빙빙 돌기 피하기 | 한 장 찍는 동안 돌면 먼 벽이 크게 휨(5 m 앞 점은 0.6 rad/s 에서 30 cm 밀림, 실습 과제 4-2) |
| 같은 곳을 다시 지나가 루프 닫기 | 출발점 · 교차로를 다시 보며 쌓인 흐름을 바로잡을 기회를 줌 (SLAM 일반 요령, 공식 문서에는 없음) |
| 벽 · 선반을 골고루 보게 운전 | 지도는 본 만큼만 생김. 좁은 통로 입구는 정면으로 들여다보기 |
| 사람 · 지게차가 없을 때 작성 | 움직이는 물체가 지도에 유령 장애물로 남음 |
시뮬레이터 5 · 운전법 비교: 속도 · 회전 · 루프 닫기#
스캔 매칭은 켜 둔 채 운전법만 바꿉니다(오도메트리 오차: 회전 2 % · 거리 2 % · 미끄러짐 기본). 이 시뮬레이터는 실제 현상을 단순하게 흉내 냅니다: 빠를수록 스캔 사이 이동이 크고, 한 장 찍는 동안의 왜곡이 크고, 바퀴 미끄러짐이 속도의 제곱으로 커집니다. 결과는 표에 쌓이므로 조건을 바꿔 가며 비교하세요.
- 🐢 0.3 m/s · 0.6 rad/s 와 🐇 1.0 m/s · 2.0 rad/s 중 어느 쪽 지도가 깨끗할지 예측합니다. 각각 🎲 로 3번씩 뽑아 "벽 제자리 %" 와 "유령 벽" 의 평균을 비교하세요.
- 둘 중 무엇이 더 해로운지 나눠 봅니다: 1.0 m/s · 0.6 rad/s 와 0.3 m/s · 2.0 rad/s 를 각각 2~3번 돌립니다.
- 🐇 조건에서 루프 닫기를 켭니다. "최대 위치 오차" 와 "끝 오차" 중 무엇이 줄어드나요? 유령 벽도 사라지나요? 지도에 분홍 원(루프 닫힘)이 어디에 생기는지 봅니다.
- 장소를 긴 복도 (특징 없음) 로 바꾸고 🐢 로 돌립니다. 벽 제자리 % 는 높은데 최대 위치 오차는 어떤가요? 긴 복도 + 기둥 · 문틀 로 바꾸면요?
- 선반 통로에서 🐢 는 벽 제자리 비율이 거의 100 %, 최대 위치 오차 10 cm 안쪽입니다. 🐇 는 뽑을 때마다 70~98 % 로 들쭉날쭉하고 유령 벽이 수백 개 생깁니다. 빠른 운전은 평균만 나쁜 것이 아니라 결과를 운에 맡깁니다.
- 이 시뮬레이터의 모델에서는 직진 속도를 올린 쪽(1.0 m/s · 0.6 rad/s)이 회전만 빠른 쪽(0.3 m/s · 2.0 rad/s)보다 지도를 더 망칩니다. 실제 로봇에서는 빠른 회전도 스캔 왜곡과 메카넘 롤러 미끄러짐을 크게 만드므로 둘 다 낮추는 것이 원칙입니다.
- 루프 닫기는 출발점에서 "처음 그린 지도" 와 맞춰 본 어긋남을 지나온 경로 전체에 나눠 고칩니다. 끝 오차와 전체 어긋남은 줄지만(최대 위치 오차는 루프를 닫기 전 지나가며 기록된 값이라 그대로입니다), 빠르게 지나가며 이미 번진 벽(유령 벽)까지 깨끗해지지는 않습니다. 천천히 운전하는 것을 대신하지 못합니다.
- 특징 없는 긴 복도에서는 좌우 벽이 반듯하게 그려져 벽 제자리 % 가 높아 보이지만, 로봇이 복도 어디쯤 있는지(x)를 잃어 최대 위치 오차가 수 m 까지 커집니다. 복도 길이가 틀린 지도입니다. 벽에 기둥 · 문틀 같은 특징이 몇 개만 있어도 대부분 10~15 cm 안쪽으로 돌아옵니다.
특징 없는 환경에서 SLAM 이 헷갈리는 이유#
시뮬레이터 3 · 5 에서 본 것을 정리합니다.
- 긴 복도: 좌우 벽은 옆 방향(y)과 방향(θ)만 알려 줍니다. 앞뒤(x)는 "어디나 똑같은 벽" 이므로 스캔 매칭 점수에 띠가 생기고, x 는 오도메트리에 기대거나 엉뚱한 곳으로 미끄러집니다. 복도가 실제보다 짧거나 길게 그려집니다.
- 반복 선반: 물류 창고의 선반 통로는 서로 똑같이 생겼습니다. 1번 통로와 3번 통로의 스캔이 거의 같으면, 루프 닫기가 엉뚱한 통로를 "같은 곳" 으로 착각해 지도를 접어 버릴 수 있습니다.
- 넓은 빈 공간: 최대 거리(8 m) 안에 벽이 없으면 스캔이 거의 inf 이고, 매칭할 점 자체가 없습니다.
대책은 특징을 보게 하는 것입니다. 통로 끝 · 기둥 · 교차로를 시야에 넣고 운전하거나, 처음 지도 작성 때는 일부러 벽에 가깝게 다니고, 필요하면 반사 테이프 · 표지판 같은 인공 특징을 둡니다.
지도 저장 형식: map.pgm + map.yaml#
map_saver_cli 는 지도를 두 파일로 저장합니다. 이미지(.pgm)는 셀 하나 = 픽셀 하나이고, 그 이미지를 어떻게 읽을지는 .yaml 이 알려 줍니다.
| map.yaml 필드 | 뜻 |
|---|---|
image | 이미지 파일 이름 |
mode | trinary(빈 칸 · 장애물 · 미확인 세 값) 등 |
resolution | 픽셀 한 변 = 몇 m |
origin | 이미지 왼쪽 아래 픽셀 모서리의 지도 좌표 [x, y, yaw] |
negate | 0 이면 어두울수록 점유 확률이 큼 |
occupied_thresh · free_thresh | 픽셀을 확률로 바꾼 뒤 장애물 · 빈 칸을 나누는 경계 |
- pgm 이미지는 맨 위 행부터 저장되므로 행 번호가 커질수록 지도의 y 는 작아집니다. numpy 배열(행 0 = y 가 작은 쪽)을 그대로 이미지로 저장하면 지도가 위아래로 뒤집힙니다(오류 6).
- 픽셀 (열 c, 행 r) 의 중심: x = origin_x + (c + 0.5) · res, y = origin_y + (높이 − 1 − r + 0.5) · res.
픽셀값 205 를 확률로 바꾸면 (255 − 205) / 255 ≈ 0.196 입니다. 지도를 확률 경계로 다시 읽을 때 free_thresh 가 이 값보다 크면 미확인이 빈 칸으로 읽힐 수 있습니다. 직접 만든 yaml 을 쓰기보다 저장 도구가 만든 yaml 을 그대로 쓰고, 값을 바꿨다면 지도를 다시 불러 회색 영역이 그대로인지 확인하세요(구체적인 동작은 사용하는 map_server 버전 확인 필요).
실습 과제 4#
작은 텍스트 지도를 pgm(P2, 숫자를 글자로 쓰는 형식) 과 yaml 로 바꾸고, 픽셀 ↔ 지도 좌표 변환 함수를 완성해 몇 점의 상태를 읽습니다. 해상도 0.1 m, origin = (−1.0, −0.5). 예측 먼저: 픽셀 (0, 0) — 이미지 맨 위 왼쪽 — 의 중심은 지도 좌표로 어디일까요? 지도 (0.95, 0.45) 는 어떤 상태일까요?
import math
# 작은 지도 (위쪽 줄이 y 가 큰 쪽): '#' 장애물, '.' 빈 칸, '?' 미확인
TEXT = """\
????????????????????
?##################?
?#................#?
?#..######........#?
?#..######....##..#?
?#............##..#?
?#................#?
?##################?
????????????????????
????????????????????"""
RES = 0.1
ORIGIN = (-1.0, -0.5, 0.0) # 이미지 왼쪽 아래 픽셀의 모서리가 지도의 어디인가 (x, y, yaw)
rows = TEXT.splitlines()
H, W = len(rows), len(rows[0])
PIX = {"#": 0, ".": 254, "?": 205} # map_saver 의 trinary 저장값
# 1) map.pgm (P2 = 숫자를 글자로 쓰는 PGM) - 이미지는 맨 위 줄부터 저장
pgm = ["P2", "%d %d" % (W, H), "255"]
for line in rows:
pgm.append(" ".join(str(PIX[ch]) for ch in line))
# 2) map.yaml
yaml = "\n".join([
"image: map.pgm", "mode: trinary", "resolution: %.3f" % RES,
"origin: [%.3f, %.3f, %.1f]" % ORIGIN, "negate: 0",
"occupied_thresh: 0.65", "free_thresh: 0.25"])
print("\n".join(pgm[:5]), "\n...")
print(yaml)
def pixel_to_world(c, r):
"""픽셀 (열 c, 행 r) 의 중심 -> 지도 좌표 (x, y)"""
# TODO 1: 열 c 는 x 와 같은 방향, 행 r 은 위에서부터 세므로 y 와 반대 방향
return (0.0, 0.0)
def world_to_pixel(x, y):
# TODO 2: pixel_to_world 의 반대 계산 (음수에서도 맞게 int 대신 math.floor)
return 0, 0
def state(x, y):
c, r = world_to_pixel(x, y)
if not (0 <= c < W and 0 <= r < H):
return "지도 밖"
v = PIX[rows[r][c]]
p = (255 - v) / 255.0 # negate: 0 -> 어두울수록 점유 확률이 큼
name = {0: "장애물(100)", 254: "빈 칸(0)"}.get(v, "미확인(-1)")
return "%s 픽셀값 %d, p = %.3f" % (name, v, p)
print()
print("픽셀 (0, 0) 중심 =", tuple(round(v, 2) for v in pixel_to_world(0, 0)))
print("픽셀 (0, %d) 중심 =" % (H - 1), tuple(round(v, 2) for v in pixel_to_world(0, H - 1)))
for x, y in [(0.0, 0.0), (-0.5, 0.1), (0.4, -0.1), (0.95, 0.45), (1.5, 0.0), (-1.03, 0.0)]:
print("지도 (%5.2f, %5.2f) -> 픽셀 %s : %s" % (x, y, world_to_pixel(x, y), state(x, y)))풀이 코드 보기
import math
# 작은 지도 (위쪽 줄이 y 가 큰 쪽): '#' 장애물, '.' 빈 칸, '?' 미확인
TEXT = """\
????????????????????
?##################?
?#................#?
?#..######........#?
?#..######....##..#?
?#............##..#?
?#................#?
?##################?
????????????????????
????????????????????"""
RES = 0.1
ORIGIN = (-1.0, -0.5, 0.0) # 이미지 왼쪽 아래 픽셀의 모서리가 지도의 어디인가 (x, y, yaw)
rows = TEXT.splitlines()
H, W = len(rows), len(rows[0])
PIX = {"#": 0, ".": 254, "?": 205} # map_saver 의 trinary 저장값
# 1) map.pgm (P2 = 숫자를 글자로 쓰는 PGM) - 이미지는 맨 위 줄부터 저장
pgm = ["P2", "%d %d" % (W, H), "255"]
for line in rows:
pgm.append(" ".join(str(PIX[ch]) for ch in line))
# 2) map.yaml
yaml = "\n".join([
"image: map.pgm", "mode: trinary", "resolution: %.3f" % RES,
"origin: [%.3f, %.3f, %.1f]" % ORIGIN, "negate: 0",
"occupied_thresh: 0.65", "free_thresh: 0.25"])
print("\n".join(pgm[:5]), "\n...")
print(yaml)
def pixel_to_world(c, r):
"""픽셀 (열 c, 행 r) 의 중심 -> 지도 좌표 (x, y)"""
return (ORIGIN[0] + (c + 0.5) * RES, ORIGIN[1] + (H - 1 - r + 0.5) * RES)
def world_to_pixel(x, y):
c = math.floor((x - ORIGIN[0]) / RES)
r = H - 1 - math.floor((y - ORIGIN[1]) / RES)
return c, r
def state(x, y):
c, r = world_to_pixel(x, y)
if not (0 <= c < W and 0 <= r < H):
return "지도 밖"
v = PIX[rows[r][c]]
p = (255 - v) / 255.0 # negate: 0 -> 어두울수록 점유 확률이 큼
name = {0: "장애물(100)", 254: "빈 칸(0)"}.get(v, "미확인(-1)")
return "%s 픽셀값 %d, p = %.3f" % (name, v, p)
print()
print("픽셀 (0, 0) 중심 =", tuple(round(v, 2) for v in pixel_to_world(0, 0)))
print("픽셀 (0, %d) 중심 =" % (H - 1), tuple(round(v, 2) for v in pixel_to_world(0, H - 1)))
for x, y in [(0.0, 0.0), (-0.5, 0.1), (0.4, -0.1), (0.95, 0.45), (1.5, 0.0), (-1.03, 0.0)]:
print("지도 (%5.2f, %5.2f) -> 픽셀 %s : %s" % (x, y, world_to_pixel(x, y), state(x, y)))실행 결과:
P2
20 10
255
205 205 205 205 205 205 205 205 205 205 205 205 205 205 205 205 205 205 205 205
205 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 205
...
image: map.pgm
mode: trinary
resolution: 0.100
origin: [-1.000, -0.500, 0.0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25
픽셀 (0, 0) 중심 = (-0.95, 0.45)
픽셀 (0, 9) 중심 = (-0.95, -0.45)
지도 ( 0.00, 0.00) -> 픽셀 (10, 4) : 빈 칸(0) 픽셀값 254, p = 0.004
지도 (-0.50, 0.10) -> 픽셀 (5, 4) : 장애물(100) 픽셀값 0, p = 1.000
지도 ( 0.40, -0.10) -> 픽셀 (13, 5) : 빈 칸(0) 픽셀값 254, p = 0.004
지도 ( 0.95, 0.45) -> 픽셀 (19, 0) : 미확인(-1) 픽셀값 205, p = 0.196
지도 ( 1.50, 0.00) -> 픽셀 (25, 4) : 지도 밖
지도 (-1.03, 0.00) -> 픽셀 (-1, 4) : 지도 밖- 픽셀 (0, 0) 은 이미지 맨 위 왼쪽이므로 지도에서는 왼쪽 위 (−0.95, 0.45) 입니다. origin 이 가리키는 것은 맨 아래 행(행 9)의 모서리입니다.
- 지도 (0.95, 0.45) 는 오른쪽 위 구석의
?셀입니다. p = 0.196 으로, free_thresh 0.25 보다 작지만 trinary 저장값 205 이므로 미확인입니다(위 tip). - 마지막 점 (−1.03, 0.0) 은 origin 보다 왼쪽이라 지도 밖입니다.
int(-0.3)은 0 쪽으로 잘라 0 이 되므로int()를 쓰면 지도 안(열 0)으로 잘못 판정합니다.math.floor(-0.3)은 −1 입니다(오류 7).
스캔이 1초에 10장(교육용 가정) 들어올 때, 직진 속도와 회전 속도에 따라 스캔 사이 이동 · 1 m 당 스캔 수 · 한 장 찍는 동안 5 m 앞 점이 밀리는 거리를 표로 만듭니다. 예측 먼저: 0.6 rad/s 로 돌면 5 m 앞 벽의 점은 몇 cm 밀릴까요? 해상도 0.05 m 지도로 몇 칸일까요?
import math
HZ = 10.0 # 스캔 주기 (교육용 가정: 1초에 10장)
print("직진 속도 스캔 1장 사이 이동 1 m 동안 스캔 수 한 장 찍는 동안 이동(왜곡)")
for v in (0.3, 0.6, 1.0, 1.6):
move = 0.0 # TODO 1: 스캔 1장 사이 이동 (m) = 속도 / 스캔 주기
per_m = 0.0 # TODO 2: 1 m 동안 찍히는 스캔 수
print("%5.1f m/s %8.1f cm %10.0f장 %12.1f cm" % (v, 100 * move, per_m, 100 * move))
print()
print("회전 속도 한 장 동안 회전 5 m 앞 점이 밀리는 거리 격자 0.05 m 몇 칸")
for w in (0.3, 0.6, 1.0, 2.0):
a = 0.0 # TODO 3: 한 장 찍는 동안 회전한 각도 (rad)
shift = 0.0 # TODO 4: 5 m 앞 점이 옆으로 밀리는 거리 = 반지름 x 각도
print("%5.1f rad/s %8.1f deg %14.1f cm %12.1f칸" % (w, math.degrees(a), 100 * shift, shift / 0.05))풀이 코드 보기
import math
HZ = 10.0 # 스캔 주기 (교육용 가정: 1초에 10장)
print("직진 속도 스캔 1장 사이 이동 1 m 동안 스캔 수 한 장 찍는 동안 이동(왜곡)")
for v in (0.3, 0.6, 1.0, 1.6):
print("%5.1f m/s %8.1f cm %10.0f장 %12.1f cm" % (v, 100 * v / HZ, HZ / v, 100 * v / HZ))
print()
print("회전 속도 한 장 동안 회전 5 m 앞 점이 밀리는 거리 격자 0.05 m 몇 칸")
for w in (0.3, 0.6, 1.0, 2.0):
a = w / HZ
shift = 5.0 * a
print("%5.1f rad/s %8.1f deg %14.1f cm %12.1f칸" % (w, math.degrees(a), 100 * shift, shift / 0.05))실행 결과:
직진 속도 스캔 1장 사이 이동 1 m 동안 스캔 수 한 장 찍는 동안 이동(왜곡)
0.3 m/s 3.0 cm 33장 3.0 cm
0.6 m/s 6.0 cm 17장 6.0 cm
1.0 m/s 10.0 cm 10장 10.0 cm
1.6 m/s 16.0 cm 6장 16.0 cm
회전 속도 한 장 동안 회전 5 m 앞 점이 밀리는 거리 격자 0.05 m 몇 칸
0.3 rad/s 1.7 deg 15.0 cm 3.0칸
0.6 rad/s 3.4 deg 30.0 cm 6.0칸
1.0 rad/s 5.7 deg 50.0 cm 10.0칸
2.0 rad/s 11.5 deg 100.0 cm 20.0칸- 0.3 m/s 이면 스캔 사이 이동 3 cm 로 지도 한 칸(5 cm)보다 작습니다. 1.0 m/s 는 10 cm(2칸), 공식 최고 속도 1.6 m/s 는 16 cm(3칸 이상)입니다.
- 회전 왜곡은 거리에 비례합니다. 0.6 rad/s 에서도 5 m 앞 점은 30 cm(6칸) 밀립니다. 그래서 실제 SLAM 드라이버는 스캔 안의 시간차를 오도메트리로 보정(de-skew)하기도 하고, 사람은 회전을 천천히 합니다.
- 이 숫자들이 시뮬레이터 5 의 🐢 · 🐇 차이를 만든 원인입니다.
다음 중 지도 품질을 해치는 운전 습관이 아닌 것은?
- 시간을 아끼려고 최고 속도에 가깝게 직진한다
- 넓은 공간 한가운데에서 제자리 회전을 여러 번 빠르게 한다
- 출발점 근처 교차로를 마지막에 한 번 더 지나간다
- 사람들이 오가는 작업 시간에 지도를 만든다
정답 보기
③ — 이미 본 곳을 다시 지나가면 루프를 닫아 쌓인 흐름을 고칠 기회가 생깁니다. ① 은 스캔 사이 이동 · 미끄러짐을 키우고(시뮬레이터 5 의 🐇), ② 는 한 장 안의 회전 왜곡과 바퀴 미끄러짐을 키우며 넓은 공간이라 매칭할 특징도 적습니다. ④ 는 움직이는 사람이 유령 장애물로 남습니다.resolution: 0.05, origin: [-2.0, -1.0, 0.0] 이고 이미지 높이가 200 픽셀인 지도에서, 지도 좌표 (0.0, 0.0) 은 어느 픽셀(열 c, 행 r)에 있을까요?
- (0, 0)
- (40, 20)
- (40, 179)
- (20, 40)
정답 보기
③ — 열 c = (0.0 − (−2.0)) / 0.05 = 40, 아래에서 센 행 = (0.0 − (−1.0)) / 0.05 = 20 이므로 위에서 센 행 r = 200 − 1 − 20 = 179 입니다. ② 는 이미지 행이 위에서부터 세어진다는 것을 잊은 경우입니다.모델 확인 (3D 뷰어)#
강좌용 myAGV Plus 모델(assets/robot/robot.json)이 아직 없다면 뷰어에 "모델 없음" 안내가 나옵니다. 모델이 준비되면 다음을 확인합니다.
- LiDAR 링크(myagv_ros2 에서는
laser_frame)가base_footprint기준 어디에 · 어느 방향으로 붙어 있는지. 스캔 점은 LiDAR 기준이므로, 로봇 중심 기준 거리로 쓰려면 이 위치만큼 옮겨야 합니다(ch02 의 TF). - LiDAR 의 높이. 이 높이의 평면만 보이므로 그보다 낮은 팔레트 · 높은 선반 판은 지도에 없습니다.
- 장착 방향이 뒤집혀 있지 않은지. myagv_ros2 의
ydlidar.yaml에는reversion: true·inverted: true같은 설정이 있는데, 이것이 실제 장착과 맞지 않으면 지도가 좌우로 뒤집히거나 앞뒤가 바뀝니다(오류 2). Plus 의 실제 값은 확인 필요입니다.
모델이 없을 때는 아래 계산으로 장착 위치 · 방향이 점에 주는 영향을 확인할 수 있습니다.
import math
# 교육용 가정: laser_frame 은 base_footprint 기준 앞으로 0.10 m, 옆 0, 회전(yaw) 0
# 실제 장착 위치 · 방향은 myAGV Plus 의 URDF (3D 뷰어) 에서 확인해 고칩니다.
def laser_to_base(r, th, mount):
"""LiDAR 기준 (r, th) -> base_footprint 기준 (x, y)"""
mx, my, myaw = mount
xl, yl = r * math.cos(th), r * math.sin(th)
c, s = math.cos(myaw), math.sin(myaw)
return mx + xl * c - yl * s, my + xl * s + yl * c
for mount in [(0.10, 0.0, 0.0), (0.10, 0.0, math.pi)]:
print("장착 (x, y, yaw) =", tuple(round(v, 2) for v in mount))
for name, th in [("LiDAR 정면", 0.0), ("LiDAR 왼쪽", math.pi / 2), ("LiDAR 뒤", math.pi)]:
x, y = laser_to_base(1.0, th, mount)
print(" %s 1.0 m -> 로봇 기준 (%5.2f, %5.2f), 로봇 중심에서 %.2f m" % (name, x, y, math.hypot(x, y)))- LiDAR 가 로봇 중심보다 0.10 m 앞에 있으면, LiDAR 가 정면 1.0 m 라고 한 장애물은 로봇 중심에서 1.10 m, 뒤 1.0 m 라고 한 것은 0.90 m 입니다. 뒤로 후진할 때 이 차이를 무시하면 10 cm 덜 남은 것을 모릅니다.
- 장착 yaw 를 π 로 잘못 적으면 정면 장애물이 뒤에, 왼쪽 장애물이 오른쪽에 나타납니다. 실물에서는 로봇 왼쪽에 상자를 두고 스캔 점이 y > 0 에 나오는지 꼭 확인합니다.
자주 나는 오류와 해결#
1) 앞에 아무것도 없는데 "가장 가까운 장애물 0.0 m"#
nearest = 0.000 m at -12.0 deg -> 비상 정지!- 원인: 측정 실패 광선이
0.0으로 들어옵니다(드라이버 설정invalid_range_is_inf: false등). 필터 없이min(ranges)를 썼습니다. - 해결:
range_min <= r <= range_max인 값만 씁니다(실습 과제 1-1). 안전 판단은 한 점이 아니라 구역의 최솟값을 2~3 스캔 연속으로 확인합니다.
2) 지도가 좌우로 뒤집힘 · 왼쪽으로 돌았는데 지도는 오른쪽으로 돎#
(rviz2) 로봇 왼쪽에 둔 상자가 스캔에서는 오른쪽(y < 0)에 보임- 원인: LiDAR 장착 방향(TF 의 yaw)이나 드라이버의 각도 방향 설정(
inverted·reversion류)이 실제와 다릅니다. 각도가 시계 방향으로 증가하는 스캔을 반시계로 해석해도 같은 증상입니다. - 해결: 로봇 왼쪽 1 m 에 상자를 두고 그 점이 θ ≈ +90°, y > 0 에 나오는지 확인합니다. 아니면 드라이버 각도 설정과 URDF · TF 의 LiDAR 방향을 고칩니다(3D 뷰어 절의 계산).
3) 한 바퀴 돌고 오니 벽이 두 겹 · 통로가 휘어짐#
(지도) 출발점 근처 벽이 30 cm 간격으로 두 줄, 선반이 비스듬히 그려짐- 원인: 스캔 매칭이 쌓인 오도메트리 오차를 따라잡지 못했습니다. 너무 빨리 운전했거나, 오도메트리 회전 오차가 크거나(ch06 의 바퀴 반지름 · IMU 부호), 특징이 적은 구간이 길었습니다.
- 해결: 기본 속도 이하로 천천히, 회전도 천천히 다시 작성합니다. 오도메트리부터 점검하고(ch06 실물 ①), 출발점을 다시 지나 루프를 닫습니다(시뮬레이터 4 · 5).
4) 회전할 때만 지도가 부채꼴로 번짐 · SLAM 이 메시지를 버린다는 경고#
[WARN] Message Filter dropping message: frame 'laser_frame' at time 1700000000.123 for reason 'the timestamp on the message is earlier than all the data in the transform cache'- 원인: 스캔 시각과 TF(odom → base → laser) 시각이 맞지 않아, 회전 중인 로봇의 다른 순간 자세로 스캔을 붙이거나 아예 버립니다. 컴퓨터 시계 차이 · 지연 · 빠른 회전이 겹치면 심해집니다(경고 문구는 SLAM 노드 · 버전에 따라 다름).
- 해결: 회전 속도를 낮추고, 스캔 · 오도메트리 · TF 를 같은 컴퓨터의 같은 시계로 발행하는지 확인합니다.
ros2 run tf2_ros tf2_echo odom laser_frame으로 TF 가 끊김 없이 나오는지 봅니다(ROS 2 도구는 ch09).
5) 유리 벽 · 검은 선반이 지도에서 빠짐 · 벽 너머에 가짜 방#
(지도) 유리 칸막이 자리가 흰색(빈 칸), 그 너머 사무실이 지도에 그려짐- 원인: LiDAR 빛이 유리를 통과하거나 검은 물체에 흡수되어 돌아오지 않습니다(1교시 표). 거울 · 반짝이는 금속은 가짜 공간을 만듭니다.
- 해결: 유리에는 반투명 띠 테이프를 붙이고, 검은 물체는 가까이 지나가며 작성합니다. 그래도 빠진 곳은 지도 이미지를 편집하거나 "가지 마시오" 구역으로 막습니다(ch10).
6) numpy 로 만든 지도를 이미지로 저장했더니 위아래가 뒤집힘#
(이미지) 창고 위쪽 통로가 아래에, 출발점이 왼쪽 위에 보임- 원인: numpy 배열은 행 0 이 y 가 작은 쪽인데, pgm 이미지는 행 0 이 맨 위입니다.
- 해결: 저장할 때
np.flipud(grid)로 뒤집고, origin 은 "이미지 왼쪽 아래 모서리" 좌표로 적습니다(실습 과제 4-1). 텍스트로 출력할 때도range(NY - 1, -1, -1)로 위에서부터 찍습니다(실습 과제 2-1).
7) 격자에 점을 찍다가 IndexError · 음수 좌표가 0번 셀에 들어감#
IndexError: index 60 is out of bounds for axis 1 with size 60- 원인: 광선 끝점이 격자 밖(최대 거리 8 m 가 지도보다 큼)인데 범위 검사 없이
L[j, i]를 썼습니다. 또int(-0.03 / 0.2)는 0 이 되어(0 쪽으로 자름) 격자 밖 점이 0번 셀에 잘못 들어갑니다. - 해결:
0 <= i < NX and 0 <= j < NY를 확인한 뒤에 씁니다(실습 과제 2-1 의add_scan). 음수가 나올 수 있으면int()대신math.floor()를 씁니다.
8) 긴 복도 지도가 실제보다 짧음#
(지도) 18 m 복도가 12 m 로 그려지고, 복도 끝 문이 엉뚱한 자리에- 원인: 특징 없는 복도에서는 스캔 매칭이 앞뒤 위치를 정하지 못합니다(시뮬레이터 3 의 띠 · 시뮬레이터 5).
- 해결: 복도는 오도메트리가 정확한 상태(천천히 · 직진)로 지나가고, 문틀 · 소화기 · 기둥이 보이도록 벽 쪽에 가깝게 다닙니다. 필요하면 일정 간격으로 표지(반사판 · 상자)를 두고 작성한 뒤 치웁니다.
과제#
- 스캔 한 장 분석기: 실습 과제 1-1 을 고쳐 앞쪽 ±30° · 왼쪽 · 오른쪽 · 뒤쪽 네 구역의 최소 거리를 출력하는 함수
sectors(scan)을 만드세요.sim_scan의 자리를 바꿔 가며, 통로 안 · 좁은 통로 입구 · 모퉁이에서 결과를 표로 비교하세요. - 해상도 보고서: 시뮬레이터 2 와 실습 과제 2-2 를 이용해, 100 m × 60 m 창고를 0.02 · 0.05 · 0.1 m 로 만들 때의 셀 수 · 메모리 · 0.6 m 통로의 표현(빈 칸 수)을 계산하고 어떤 해상도를 고를지 이유와 함께 5줄로 쓰세요.
- 운전법 실험 노트: 시뮬레이터 5 에서 (0.3 · 0.6), (0.6 · 1.0), (1.0 · 2.0) 세 조건을 각각 🎲 로 5번씩 돌려 벽 제자리 % 의 평균 · 최솟값을 표로 만들고, 루프 닫기를 켰을 때와 비교하세요. "우리 팀의 지도 작성 운전 규칙" 3가지를 정리합니다.
참고자료#
- myAGV Plus 공식 문서 (영문): https://github.com/elephantrobotics/myagvPlus_docs/tree/main/MYAGV_PLUS_EN
- 제품 사양 (LiDAR 0.12~8 m · 360°, 최고 속도): https://github.com/elephantrobotics/myagvPlus_docs/blob/main/MYAGV_PLUS_EN/2-ProductFeature/2-ProductFeature.md
- 6.2.4 Real-time Mapping with Gmapping (지도 작성 · 저장 순서, 속도 권고): https://github.com/elephantrobotics/myagvPlus_docs/blob/main/MYAGV_PLUS_EN/6-Software_Development_Guide/6.2-ApplicationBaseROS2/6.2.4-Real-time_Mapping_with_Gmapping.md
- myagv_ros2 저장소 (myAGV 용,
galactic-JN브랜치 — ydlidar_ros2_driver · slam_gmapping 포함): https://github.com/elephantrobotics/myagv_ros2 - myagv_ros2 의 LiDAR 드라이버 설정
ydlidar.yaml: https://github.com/elephantrobotics/myagv_ros2/blob/galactic-JN/ydlidar_ros2_driver/params/ydlidar.yaml - ROS 메시지 정의
sensor_msgs/LaserScan: https://docs.ros2.org/latest/api/sensor_msgs/msg/LaserScan.html - ROS 메시지 정의
nav_msgs/OccupancyGrid: https://docs.ros2.org/latest/api/nav_msgs/msg/OccupancyGrid.html - Nav2 map_server (map.yaml 형식 · map_saver): https://github.com/ros-navigation/navigation2/tree/main/nav2_map_server
- G. Grisetti, C. Stachniss, W. Burgard, Improved Techniques for Grid Mapping with Rao-Blackwellized Particle Filters (Gmapping 논문), IEEE Transactions on Robotics, 2007
- S. Thrun, W. Burgard, D. Fox, Probabilistic Robotics (점유 격자 · 로그 오즈 · 파티클 필터), MIT Press, 2005
- myAGV Plus 제품 사진 · 소개 (hyenc.net): https://www.hyenc.net/products/35
- 이 사이트의 도구: URDF 뷰어 · Python Playground
- 이전 차시: ch06 오도메트리와 센서 융합 · 다음 차시: ch08 경로 계획과 장애물 회피