전체 과정 P3 스스로 길을 찾다 — 지도 · 위치 · 경로 · 약 200분 · 중급

CH06SLAM — 지도를 그리며 달리기

점유격자 지도가 스캔으로 채워지는 과정과, 스캔 매칭 · 루프 폐합이 오도메트리 오차를 바로잡는 원리를 시뮬레이터에서 관찰합니다.

선수 챕터: CH04 CH05
Python Playground시뮬레이터

학습 목표#

  • 점유격자 지도(Occupancy Grid)의 셀 값(미지 −1 · 빈 공간 0 · 점유 100), 해상도(m/셀), 원점(origin)을 설명하고, map.yaml 의 각 줄이 무엇을 뜻하는지 읽을 수 있습니다.
  • 라이다 스캔 한 장을 브레젠험(Bresenham) 레이 추적으로 격자에 그리고, 로그 오즈(log-odds) 갱신으로 "여러 번 본 셀일수록 확신이 커지는" 과정을 시뮬레이터와 numpy 코드로 확인할 수 있습니다.
  • 오도메트리 위치만으로 지도를 그리면 벽이 이중으로 겹치고 휘는 이유를 설명하고, 노이즈 수준에 따라 "쓸 수 있는 지도" 의 경계를 판정할 수 있습니다.
  • 스캔 매칭(ICP)의 한 반복(최근접 점 → 회전 · 이동 추정)을 구현하고, 초기 오차 · 반복 횟수에 따른 수렴 / 발산 경계를 찾을 수 있습니다. 루프 폐합이 누적 오차를 경로 전체에 나누는 원리를 설명할 수 있습니다.
  • myAGV Pro 의 실제 매핑 절차(bringup → Gmapping → 수동 주행 → map_saver)를 따라 하고, 좋은 지도를 만드는 주행 요령을 시뮬레이터 미션으로 검증할 수 있습니다.

이론#

ch04 에서는 바퀴로 위치를 세면 오차가 쌓인다는 것을, ch05 에서는 라이다가 "어느 방향으로 몇 m 앞에 무엇이 있는지" 를 알려 준다는 것을 확인했습니다. 이번 차시는 둘을 합칩니다. 라이다 스캔을 로봇이 있던 자리 에 차곡차곡 겹쳐 그리면 지도가 됩니다. 문제는 "로봇이 있던 자리" 를 정확히 모른다는 것입니다. 위치를 알아야 지도를 그리고, 지도가 있어야 위치를 아는 이 닭과 달걀 문제를 동시에 푸는 것이 SLAM(Simultaneous Localization And Mapping, 동시적 위치 추정 및 지도 작성) 입니다.

항목값출처
2D 라이다360° · 0 ~ 15 m · ±3 cm · 6 ~ 12 Hz공식 사양 (ch01 · ch05)
라이다 위치laser_link = base_link 앞 0.179 mURDF (ch01)
기본 지도agv_pro_navigation2/map/map.yaml — 해상도 0.05 m, mode: trinaryagv_pro_ros2 저장소 (humble 브랜치)
2D SLAMslam_gmapping (Gmapping) — 공식 문서 6.2.2공식 문서
3D · 시각 SLAMPoint-LIO (6.2.6) · FAST-LIO (6.2.7) · RTAB-Map (6.2.9)공식 문서
수동 주행 권장값직진 0.25 m/s · 회전 0.45 rad/s공식 문서 6.2.2
이 차시의 시뮬레이터와 Playground 코드에 대해
  • 시뮬레이터 속 창고 · 라이다 · 오도메트리 오차는 교육용 모델 입니다. 로그 오즈 증거값(l_occ · l_free), 오도메트리 오차 크기, "이중 벽" 판정 기준 같은 값은 원리를 보이기 위한 가정입니다.
  • 시뮬레이터의 스캔 매칭 · 루프 폐합은 실제 Gmapping · Cartographer 의 알고리즘을 그대로 옮긴 것이 아니라, 같은 원리를 가장 단순한 형태로 구현한 것입니다.
  • Playground 코드는 표준 라이브러리 + numpy 만 씁니다. 실물 로봇 · ROS 2 가 필요한 부분은 ## 실습 (실물) 에 따로 두었습니다.

1교시 · 점유격자 지도 (Occupancy Grid)#

세상을 모눈종이로 나누기#

점유격자 지도는 바닥을 같은 크기의 정사각형 칸(셀, cell)으로 나누고, 칸마다 "여기에 무엇이 있을 확률" 을 적어 둔 표입니다. ROS 2 에서는 nav_msgs/OccupancyGrid 메시지로 주고받습니다.

필드뜻예
info.resolution셀 한 변의 길이 (m/셀)0.05
info.width · info.height가로 · 세로 셀 수400 · 300 → 20 m × 15 m
info.origin셀 (0, 0) 의 왼쪽 아래 모서리 가 세계(map 좌표계)의 어디인지(−10, −24.4, 0)
dataint8 배열, 길이 width × height. 행(row) 우선, (0, 0) 부터−1 · 0 · 100

data 값은 세 가지만 기억하면 됩니다. −1 = 미지(unknown, 아직 못 본 곳), 0 = 빈 공간(free), 100 = 점유(occupied, 벽 · 물건). 그 사이 값(1 ~ 99)은 확률을 퍼센트로 적은 것인데, 저장된 지도를 다시 읽을 때는 보통 이 세 값으로 정리됩니다.

셀 (i, j) (i = 열, j = 행)의 중심이 세계의 어디인지는 다음과 같습니다.

text
x = origin_x + (i + 0.5) · resolution
y = origin_y + (j + 0.5) · resolution
data 인덱스 = j · width + i

거꾸로 세계 좌표 (x, y) 가 들어 있는 셀은 i = floor((x − origin_x) / resolution), j = floor((y − origin_y) / resolution) 입니다.

점유격자 지도 — 광선이 지나간 셀은 빈 공간, 끝 셀은 점유 로봇이 있는 셀 끝점 = 점유 (100) 회색 = 미지 (−1) 흰색 = 빈 공간 (0) origin = 왼쪽 아래 셀의 모서리 좌표 · 한 칸 = 해상도 r (m/셀) 로그 오즈(log-odds) 갱신 L = ln( p / (1 − p) ), 처음엔 L = 0 (p = 0.5) 끝점: L ← L + l_occ 지나감: L ← L + l_free 빈 공간 0 미지 −1 점유 100 L=0p=0.5 L=−1.10p=0.25 L=0.62p=0.65 같은 셀을 여러 번 볼수록 L 이 한쪽으로 쌓여 확신이 커지고, 한 번의 잘못된 측정은 다음 관측들이 되돌립니다. 0.65 / 0.25 = map.yaml 의 occupied_thresh / free_thresh
왼쪽: 로봇이 있는 셀에서 스캔 끝점까지 광선이 지나간 셀은 빈 공간, 끝점 셀은 점유. 오른쪽: 로그 오즈 L 이 쌓이며 미지 → 빈 공간 / 점유로 바뀝니다.

레이 위는 빈 공간, 끝점은 점유 — 브레젠험 레이 추적#

라이다가 3.2 m 앞에서 벽을 봤다는 것은 두 가지를 알려 줍니다. ① 3.2 m 지점에 무언가가 있다, ② 거기까지는 아무것도 없다(빛이 막히지 않고 지나갔으니까). 그래서 스캔 한 장을 격자에 그릴 때는 로봇 셀에서 끝점 셀까지 직선이 지나가는 셀을 모두 찾아 "빈 공간" 증거를 주고, 끝 셀에만 "점유" 증거를 줍니다. 격자 위에서 직선이 지나가는 셀을 정수 덧셈만으로 빠르게 찾는 방법이 브레젠험(Bresenham) 직선 알고리즘 입니다. 최대 거리까지 아무것도 맞지 않은 광선(inf)은 끝까지 빈 공간만 주고, 점유 증거는 주지 않습니다.

로그 오즈 갱신 — 여러 번 본 셀일수록 확신이 커진다#

한 번의 측정은 틀릴 수 있습니다(ch05 의 먼지 · 노이즈). 그래서 셀 값을 한 번에 0 이나 100 으로 덮어쓰지 않고, 확률 p 를 로그 오즈 L = ln(p / (1 − p)) 로 바꿔 증거를 더합니다.

text
처음:            L = 0          (p = 0.5, 모름)
끝점으로 맞음:    L ← L + l_occ   (예: l_occ = +0.85  ≈ p 0.7 의 증거)
광선이 지나감:    L ← L + l_free  (예: l_free = −0.4  ≈ p 0.4 의 증거)
확률로 되돌리기:  p = 1 / (1 + e^(−L))

곱셈(베이즈 갱신)이 덧셈으로 바뀌어 계산이 간단하고, 값이 0 과 1 에 딱 붙지 않습니다. 보통 L 이 끝없이 커지지 않게 ±4 정도에서 자릅니다(clamp). 그래야 나중에 물건이 치워졌을 때 다시 바뀔 수 있습니다.

map.yaml — 저장된 지도 읽기#

지도를 저장하면 그림 파일(.pgm, 회색조 이미지)과 설명 파일(.yaml) 두 개가 생깁니다. agv_pro_ros2 저장소의 agv_pro_navigation2/map/map.yaml 은 다음과 같습니다.

text
image: map.pgm
mode: trinary
resolution: 0.05
origin: [-10, -24.4, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25
줄뜻
image같은 폴더의 그림 파일 이름
mode: trinary픽셀을 점유 / 빈 공간 / 미지 세 값으로만 읽음
resolution픽셀 하나 = 0.05 m (5 cm)
origin그림의 왼쪽 아래 픽셀 이 map 좌표 (−10 m, −24.4 m) 에 있고 회전은 0
negate0 이면 검정 = 점유, 흰색 = 빈 공간 (1 이면 반대)
occupied_thresh · free_thresh픽셀 밝기로 구한 점유 확률 p 가 0.65 초과면 점유, 0.25 미만이면 빈 공간, 그 사이는 미지
그림의 위아래는 격자와 반대입니다

이미지 파일은 맨 윗줄부터 저장하지만 격자의 행 0 은 맨 아래(origin 쪽) 입니다. 그래서 PGM 의 픽셀 (row, col) 은 격자의 j = height − 1 − row, i = col 입니다. 이것을 놓치면 지도가 위아래로 뒤집힙니다 (자주 나는 오류 4).

해상도를 고르는 기준#

해상도를 절반으로 줄이면(더 촘촘하게) 셀 수는 4 배 가 됩니다. 50 m × 50 m 창고라면 0.1 m 는 25만 셀(약 0.25 MB), 0.05 m 는 100만 셀(약 1 MB), 0.02 m 는 625만 셀(약 6.25 MB, int8 1 바이트/셀 기준)입니다. 메모리만의 문제가 아닙니다. 셀이 작을수록 한 셀이 받는 광선 수가 줄어 증거가 덜 쌓이고, 라이다 거리 노이즈(±3 cm)보다 셀이 작아지면 벽이 한 줄로 모이지 않고 흩어집니다. myAGV Pro 의 기본 지도가 0.05 m 인 것도 이런 균형 때문입니다.

시뮬레이터 1 · 스캔이 점유격자를 채우는 과정#

왼쪽은 6 m × 4 m 의 실제 방(위에서 본 모습), 오른쪽은 로봇이 만든 지도입니다. 로봇을 끌어 옮기면 그 자리에서 스캔 1장을 찍어 지도에 더합니다. 해상도 · 증거값 · 갱신 방식을 바꾸면 지금까지 찍은 같은 스캔들 로 지도를 다시 그리므로 공정하게 비교할 수 있습니다.

시뮬레이터 1 · 점유격자 만들기 (왼쪽: 실제 방, 파란 로봇을 끌어 놓으면 스캔 1장 / 오른쪽: 지도, 마우스를 올리면 셀 정보)
해상도: · 갱신 · 표시

해 볼 것

  1. 처음 상태(0.05 m, 스캔 1장)에서 오른쪽 지도의 벽을 봅니다. 벽은 진한 검정인가요, 회색인가요? 같은 자리 스캔 10장 을 누르기 전에 벽 색이 어떻게 바뀔지 예측한 뒤 눌러 봅니다. 출력의 "점유 판정까지 끝점 몇 번" 과 연결해 설명합니다.
  2. ▶ 방 8곳 돌며 스캔 을 눌러 방 전체를 채운 뒤, 해상도 버튼 0.02 → 0.05 → 0.10 을 차례로 누릅니다. 셀 수 · 50 m 창고 메모리는 몇 배씩 바뀌나요? 0.02 m 에서 벽이 왜 점선처럼 끊기고 두꺼워지는지(셀 하나가 받는 광선 수 · 거리 노이즈 σ 와 셀 크기 비교) 말해 봅니다.
  3. 해상도 0.10 m 에서 강조 광선 방향 슬라이더를 움직이며 주황 셀(빈 공간)과 빨간 끝 셀(점유)을 봅니다. 최대 거리를 2 m 로 줄여 벽에 닿지 않는 광선을 만들면 끝 셀은 어떻게 표시되나요?
  4. 거리 노이즈를 5 cm 로 올리고 갱신을 마지막 값 덮어쓰기 로 바꿉니다. 같은 자리에서 10장을 더 찍은 뒤 다시 로그 오즈 누적 으로 돌려 비교합니다. 어느 쪽 벽이 더 깨끗한가요?
  5. 표시를 trinary (저장되는 모양) 으로 바꾸고, 벽 근처 셀에 마우스를 올려 L · p · 저장 값(−1 / 0 / 100)을 읽습니다. p = 0.6 인 셀은 저장하면 무엇이 되나요?

관찰 포인트 — 한 장의 스캔으로는 벽 셀이 "조금 점유 같다" 수준에 머뭅니다. l_occ = 0.85 이면 L 이 0.619 를 넘는 데 끝점 1번이면 되지만, l_free = −0.4 로 빈 공간이 되려면 3번을 지나가야 합니다. 같은 셀을 여러 번 볼수록 L 이 쌓여 노이즈 하나가 결과를 뒤집지 못합니다. 해상도를 절반으로 줄이면 셀 수는 4 배, 그런데 셀 하나가 받는 광선은 줄어듭니다. 셀 크기(2 cm)가 거리 노이즈(1.5 cm)와 비슷해지면 끝점이 이웃 셀로 흩어져 벽이 두껍고 성기게 보입니다.

확인 문제 1-1

시뮬레이터 1 에서 l_occ = 0.85, l_free = −0.4, 임곗값 occupied_thresh 0.65 · free_thresh 0.25 로 두었습니다. 어떤 셀을 광선이 2번 지나갔고(빈 공간 증거) 끝점으로 1번 맞았습니다. 이 셀을 지금 저장하면 값은?

  1. 100 — 한 번이라도 맞았으므로 점유
  2. 0 — 지나간 횟수가 더 많으므로 빈 공간
  3. −1 — L = 0.85 − 0.8 = 0.05, p ≈ 0.51 로 두 임곗값 사이라 미지
  4. 50 — 확률 50 % 로 저장
정답 보기③ — 로그 오즈는 더하기입니다. L = 0.85 + 2 × (−0.4) = 0.05 → p = 1 / (1 + e^−0.05) ≈ 0.512. 0.25 ≤ p ≤ 0.65 이므로 trinary 로 저장하면 미지(−1, PGM 밝기 205)입니다. "한 번이라도 맞으면 점유"(①)는 덮어쓰기나 OR 방식의 생각이고, 횟수만 비교(②)하면 증거의 세기를 무시하게 됩니다.
확인 문제 1-2

같은 50 m × 50 m 창고 지도를 해상도 0.05 m 에서 0.02 m 로 바꾸면, 셀 수와 지도 품질에 대해 시뮬레이터 1 에서 관찰한 것과 가장 맞는 설명은?

  1. 셀 수 2.5 배, 벽이 항상 더 선명해진다
  2. 셀 수 6.25 배, 셀 하나에 떨어지는 광선이 줄어 같은 주행으로는 벽이 성기고 노이즈가 두드러질 수 있다
  3. 셀 수는 같고 그림만 커진다
  4. 셀 수 6.25 배, 라이다 노이즈가 셀 크기에 맞춰 함께 작아진다
정답 보기② — 셀 수는 (0.05 / 0.02)² = 6.25 배(약 1 MB → 6.25 MB)입니다. 라이다 거리 노이즈(±3 cm 사양)는 지도 해상도와 상관없이 그대로이므로(④ 오답), 셀이 노이즈보다 작아지면 끝점이 여러 셀로 흩어지고, 셀마다 받는 증거가 줄어 같은 주행으로는 더 성긴 지도가 됩니다. 해상도는 "작을수록 좋다" 가 아니라 센서 정밀도 · 메모리 · 용도의 균형입니다.

2교시 · 오도메트리만으로 지도를 그리면?#

지도는 "스캔 + 그 스캔을 찍은 위치" 입니다#

1교시에서는 로봇의 위치를 정확히 안다고 가정했습니다. 실제로 로봇이 아는 위치는 ch04 의 오도메트리 입니다. 바퀴 슬립 · 바닥 요철 · 회전 오차 때문에 오도메트리는 주행 거리에 따라 점점 틀어지고, 특히 방향(θ) 오차 는 그 뒤의 모든 위치를 부채꼴처럼 돌려 버립니다. 스캔은 정확해도 "어디서 찍었는지" 가 틀리면, 같은 벽이 다른 자리에 한 번 더 그려집니다. 이것이 이중 벽(ghost wall) 이고, 각도 오차가 서서히 쌓이면 곧은 복도가 휘어 보입니다.

같은 스캔, 다른 위치 — 지도를 망가뜨리는 것은 "어디서 찍었다고 믿었나" 입니다 ① 참 위치로 그림 벽이 한 줄 · 모서리가 직각 ② 오도메트리 위치로 그림 이중 벽 · 휜 벽 (각도 오차 누적) ③ SLAM: 위치와 지도를 함께 스캔 매칭 + 루프 폐합으로 위치를 고침
같은 스캔이라도 ① 참 위치로 그리면 벽이 한 줄, ② 오도메트리 위치로 그리면 이중 벽 · 휜 벽, ③ SLAM 은 스캔을 이용해 위치도 함께 고칩니다.

예측하기: 같은 복도를 두 번 지나가면?#

시뮬레이터를 켜기 전에 먼저 예측해 봅니다. 로봇이 창고를 한 바퀴 돌고 처음 지나갔던 가운데 통로를 다시 지나가며 지도를 그립니다. 첫 번째 지나갈 때와 두 번째 지나갈 때 오도메트리 오차는 같을까요? 그러면 그 통로의 선반 벽은 지도에 어떻게 그려질까요? 공책에 "두 번째 통과 때의 벽은 첫 번째보다 ___ 쪽으로 ___ cm 쯤 어긋날 것이다" 라고 적어 두세요.

시뮬레이터 2 · 참 위치 지도 vs 오도메트리 지도#

14 m × 8 m 창고를 로봇이 정해진 경로로 돕니다. 왼쪽은 참 위치 로, 오른쪽은 오도메트리 위치 로 같은 스캔을 그린 지도입니다(해상도 0.1 m). 빨간 셀은 실제 벽에서 0.2 m 넘게 떨어진 "점유" 셀, 곧 이중 벽 입니다.

시뮬레이터 2 · 같은 스캔, 다른 위치 (왼쪽: 참 위치로 그린 지도 / 오른쪽: 오도메트리 위치로 그린 지도, 빨강 = 이중 벽 셀)
노이즈 단계: · ·

해 볼 것

  1. 노이즈 0 을 누릅니다. 두 지도가 똑같은지, 이중 벽 셀이 0 에 가까운지 확인합니다. 이것이 "위치만 정확하면 지도는 쉽다" 는 기준선입니다.
  2. 1단계 → 2단계 → 3단계 를 차례로 누르며 오른쪽 지도와 판정을 기록합니다. 어느 단계부터 선반이 두 겹으로 보이나요? 오차가 경로 앞부분 과 뒷부분 중 어디에서 더 크게 드러나나요?
  3. 슬라이더로 거리 슬립 3 % · 회전 오차 0 % · σθ 0 을 만들고 결과를 적습니다. 이번에는 거리 슬립 0 % · 회전 오차 3 % · σθ 0 으로 바꿉니다. 이중 벽을 더 많이 만드는 것은 어느 쪽인가요? 이유를 "θ 오차는 그 뒤의 모든 이동 방향을 돌린다" 로 설명해 봅니다.
  4. 앞에서 적은 예측을 확인합니다. 가운데 통로를 두 번 지나가기 를 켜고 2단계로 둡니다. 가운데 통로의 선반 벽이 몇 겹으로 보이나요? ▶ 주행 재생 으로 두 번째 통과 순간을 지켜봅니다.
  5. 다른 난수 를 3 ~ 4 번 눌러 같은 2단계에서도 결과가 얼마나 달라지는지 봅니다. "노이즈 단계" 만으로 지도 품질을 장담할 수 없는 이유를 말해 봅니다.

관찰 포인트 — 스캔 자체는 왼쪽과 오른쪽이 완전히 같습니다. 달라진 것은 "어디서 찍었다고 믿었나" 하나뿐입니다. 거리 배율 오차는 지도를 조금 늘이거나 줄이는 정도지만, 회전 오차는 모서리를 돌 때마다 그 뒤의 경로 전체를 돌려서 벽을 두 겹 · 세 겹으로 만듭니다. 같은 곳을 두 번째로 지날 때는 그동안 쌓인 오차만큼 벽이 어긋나 그려집니다. 사람이 보면 "아까 그 통로" 인데, 오도메트리만 믿는 로봇은 "새 통로" 라고 생각합니다. 이 어긋남을 알아차리고 바로잡는 것 이 3교시의 스캔 매칭과 루프 폐합입니다.

SLAM 문제 정의 — 위치와 지도를 동시에#

정리하면, 로봇에게 주어진 것은 오도메트리 u₁ … u_t 와 스캔 z₁ … z_t 뿐이고, 알고 싶은 것은 **경로 x₁ … x_t 와 지도 m** 두 가지입니다.

text
알고 싶은 것:  p( x₁…x_t , m  |  z₁…z_t , u₁…u_t )
               (경로)  (지도)    (스캔)    (오도메트리)
  • 위치를 정확히 알면 지도는 1교시 방법으로 바로 그려집니다 (시뮬레이터 2 의 왼쪽).
  • 지도를 정확히 알면 스캔을 지도에 맞춰 위치를 찾을 수 있습니다 (뒤 차시의 위치 추정 주제).
  • 둘 다 모르므로, 새 스캔을 지금까지 만든 지도에 맞춰 위치를 고치고(스캔 매칭), 고친 위치로 지도를 갱신 하는 일을 번갈아 합니다. 이 부분을 SLAM 의 프런트엔드(front-end) 라고 부릅니다.
  • 그래도 조금씩 남는 오차는 "전에 와 본 곳" 을 알아봤을 때 경로 전체를 한꺼번에 고쳐 없앱니다. 이것이 루프 폐합(loop closure) 이고, 경로 전체를 최적화하는 부분을 백엔드(back-end) 라고 부릅니다.

ROS 2 에서 SLAM 노드는 지도(map 토픽)와 함께 TF map → odom 을 발행합니다(ch04 의 TF 트리). 오도메트리는 계속 부드럽게 odom → base_footprint 를 내고, SLAM 은 "오도메트리가 얼마나 틀렸는지" 를 map → odom 으로 보정합니다.

확인 문제 2-1

시뮬레이터 2 에서 거리 슬립 3 % · 회전 오차 0 % · σθ 0 으로 두었을 때와, 거리 슬립 0 % · 회전 오차 3 % · σθ 0 으로 두었을 때를 비교했습니다. 가장 그럴듯한 관찰은?

  1. 두 경우 이중 벽이 같다 — 둘 다 3 % 이므로
  2. 거리 슬립 쪽이 훨씬 나쁘다 — 직진 거리가 회전보다 길므로
  3. 회전 오차 쪽이 훨씬 나쁘다 — 모서리마다 생긴 방향 오차가 그 뒤 모든 직진의 방향을 돌려, 멀리 갈수록 위치 오차가 커진다
  4. 둘 다 이중 벽이 생기지 않는다 — 노이즈(σθ)가 0 이므로
정답 보기③ — 90° 회전에서 3 % 오차는 약 2.7° 입니다. 그 뒤 10 m 를 직진하면 옆으로 약 10 × sin 2.7° ≈ 0.47 m 가 어긋나고, 모서리를 돌 때마다 쌓입니다. 거리 슬립 3 % 는 10 m 에 0.3 m 로, 주로 진행 방향으로만 늘어나 벽이 "길어질" 뿐 겹치는 정도는 작습니다. 노이즈가 0 이어도 배율 오차(계통 오차)만으로 이중 벽이 생깁니다(④ 오답).
확인 문제 2-2

"가운데 통로를 두 번 지나가기" 를 켜고 2단계 노이즈로 돌렸더니, 가운데 통로의 선반이 두 겹으로 그려졌습니다. 이 현상을 가장 잘 설명한 것은?

  1. 라이다가 두 번째 통과 때 고장 났다
  2. 두 번째 통과 때의 오도메트리 위치에는 그동안 한 바퀴 돌며 쌓인 오차가 들어 있어, 같은 선반이 다른 자리에 한 번 더 그려졌다
  3. 로그 오즈가 두 번 더해져 벽이 두 개가 됐다
  4. 지도 해상도가 0.1 m 라서 생긴 반올림 오차다
정답 보기② — 스캔은 두 번 다 정확합니다. 첫 번째 통과 때는 오차가 작았고, 두 번째 통과 때는 한 바퀴(수십 m) 동안 쌓인 오차가 들어 있어 같은 선반이 어긋난 자리에 그려집니다. 로그 오즈를 같은 자리에 더하면 벽이 더 진해질 뿐 두 개가 되지 않습니다(③). 해상도 반올림은 최대 0.05 m 수준입니다(④). 이 어긋남이 루프 폐합의 단서가 됩니다.

3교시 · 스캔 매칭과 루프 폐합#

스캔 매칭 — 새 스캔을 지도에 맞춰 위치 고치기#

2교시의 이중 벽은 "새 스캔을 그릴 위치" 가 틀려서 생겼습니다. 그렇다면 새 스캔을 그리기 전에, 지금까지 만든 지도(또는 바로 앞 스캔)와 가장 잘 겹치는 위치를 찾으면 됩니다. 오도메트리 위치는 그 탐색의 출발점(초기 추정) 으로만 씁니다. 이것이 스캔 매칭(scan matching) 입니다.

가장 널리 알려진 방법이 ICP(Iterative Closest Point, 반복 최근접점) 입니다. 한 번의 반복은 세 단계입니다.

  1. 짝 찾기: 새 스캔의 점마다 기준 점군(지도)에서 가장 가까운 점을 짝으로 고릅니다. 너무 먼 짝(최대 짝 거리 d_max 초과)은 버립니다.
  2. 회전 · 이동 계산: 짝들의 무게중심 μ_src · μ_ref 를 빼고, 남은 점들로 회전각을 구한 뒤 이동을 구합니다.
  3. 옮기기: 구한 회전 · 이동을 현재 추정에 덧붙이고, 1 로 돌아갑니다.
text
a_k = p_k − μ_src ,  b_k = q_k − μ_ref           (짝 k 의 무게중심 뺀 좌표)
Δθ  = atan2( Σ (a_x b_y − a_y b_x) ,  Σ (a_x b_x + a_y b_y) )
t   = μ_ref − R(Δθ) · μ_src

이 식은 "짝 사이 거리 제곱의 합을 가장 작게 하는 회전 · 이동" 을 2D 에서 바로 푼 것입니다(3D 에서는 SVD 를 씁니다). 짝이 맞다면 한 번에 정답이 나오지만, 처음에는 짝이 틀리기 때문에 반복합니다. 초기 오차가 크면 틀린 짝으로 계산한 답이 엉뚱한 방향을 가리켜 잘못된 자리에 수렴(국소 최솟값) 합니다. 그래서 오도메트리 · IMU 로 좋은 초기 추정을 주는 것이 중요합니다.

스캔 매칭(ICP)은 짧은 구간을, 루프 폐합은 긴 구간을 바로잡습니다 ICP 한 번 = ① 짝 찾기 → ② 회전 · 이동 계산 → ③ 옮기기 ① 새 점마다 가장 가까운 지도 점 ② 무게중심 μ_src, μ_ref 를 빼고 Δθ = atan2(Σ a×b, Σ a·b) t = μ_ref − R(Δθ)·μ_src ③ 옮긴 뒤 ①부터 반복 파랑 = 지도(기준 스캔), 주황 = 새 스캔, 점선 = 가장 가까운 짝 a, b = 무게중심을 뺀 짝 점 · a×b = a_x b_y − a_y b_x 초기 오차가 크면 ① 의 짝이 틀려 엉뚱한 곳으로 수렴(발산) 루프 폐합 (포즈 그래프) "여기는 출발점이다" (루프 제약) 끝에서 드러난 오차를 경로 전체에 나눠 줍니다
왼쪽: ICP 한 번의 세 단계. 오른쪽: 루프 폐합 — 출발점으로 돌아와 "여기는 출발점" 이라는 제약이 생기면, 끝에서 드러난 오차를 경로 전체에 나눠 줍니다.

시뮬레이터 3 · 두 점군 맞추기 (ICP)#

파란 점은 원점에서 찍은 기준 스캔(지도), 주황 점은 조금 움직인 뒤 찍은 새 스캔 을 지금의 추정 위치에 그린 것입니다. 초록 점선 삼각형이 새 스캔을 찍은 참 위치, 주황 삼각형이 추정 위치 입니다. 주황 점군을 끌어 옮기거나(이동), 손잡이 원을 끌어 돌려(회전) 손으로 맞춰 본 뒤, ICP 를 돌려 비교합니다.

시뮬레이터 3 · ICP 스캔 매칭 (주황 점군 끌기 = 이동, 손잡이 ○ 끌기 = 회전 / 오른쪽: 반복마다 오차 그래프)
· 예:

해 볼 것

  1. 보통 (0.3 m, −0.2 m, 20°)에서 먼저 손으로 주황 점군을 끌고 돌려 파란 점군에 맞춰 봅니다. 출력의 오차가 몇 cm · 몇 ° 까지 줄었나요? 그다음 초기 추정으로 → ▶ 최대 반복까지 를 눌러 ICP 와 비교합니다.
  2. ICP 1회 를 천천히 여러 번 누르며 회색 짝 선을 봅니다. 처음 몇 번의 짝은 맞는 짝인가요? 반복할수록 짝이 어떻게 바뀌나요? 오른쪽 그래프에서 RMS 와 참 위치 오차가 함께 줄어드는지 봅니다.
  3. 큰 회전 (60°)으로 바꿔 최대 반복 20 회로 돌립니다. 수렴하나요? 최대 반복을 50 으로 늘리면 어떤가요? 이번에는 90° 틀림 을 50 회 돌립니다. 그래프의 RMS 가 어떤 값에서 멈추나요? 멈춘 자리의 주황 점군과 파란 점군을 비교해 봅니다.
  4. 수렴 경계 찾기 를 누르고, 최대 반복을 5 → 20 → 50 으로 바꿔 가며 다시 누릅니다. 반복을 늘리면 경계가 얼마나 넓어지나요? 끝까지 늘려도 넘을 수 없는 각도가 있나요?
  5. 최대 짝 거리 d_max 를 0.3 m 와 3 m 로 바꿔 경계를 다시 찾습니다. 너무 작으면 왜 짝이 부족하고, 너무 크면 왜 틀린 짝이 끼어드는지 말해 봅니다.

관찰 포인트 — ICP 는 "가장 가까운 점이 짝일 것" 이라는 가정 위에서 움직입니다. 초기 오차가 작으면 그 가정이 대부분 맞아 몇 번 만에 cm 수준으로 맞춰집니다. 회전이 크면 틀린 짝이 많아 한 번에 조금씩만 나아가므로 반복이 많이 필요하고(60° 는 20 회로 실패, 50 회로 수렴), 어느 각도를 넘으면 반복을 아무리 늘려도 틀린 자리에 갇혀(국소 최솟값) RMS 가 큰 값에서 멈춥니다. 반복 횟수는 경계를 넓힐 뿐 없애지 못합니다. 매칭이 끝난 뒤에도 RMS 가 크게 남는 것은 "이 매칭은 믿을 수 없다" 는 신호입니다. 실제 SLAM 은 이런 결과를 버리고 오도메트리 값을 그대로 씁니다. 그래서 실제 SLAM 은 매 스캔(0.1 초)마다, 오도메트리로 초기 추정을 준 상태에서 작은 오차만 맞춥니다. 오차가 작을 때 자주 고치는 것이 핵심입니다.

확인 문제 3-1

시뮬레이터 3 에서 초기 θ 오차 90° 로 ICP 를 50 번 돌렸더니, 짝 거리 RMS 가 0.46 m 에서 더 이상 줄지 않았고 참 위치 오차는 약 1 m · 87° 였습니다. 가장 알맞은 해석과 대응은?

  1. 반복이 부족하다 — 500 번 돌리면 반드시 맞는다
  2. 틀린 짝들 때문에 다른 자세(국소 최솟값)에 갇혔다 — RMS 가 큰 채 멈춘 결과는 버리고, 더 좋은 초기 추정(오도메트리 · IMU)으로 다시 맞춘다
  3. 라이다 노이즈가 너무 커서 생긴 일이다 — 노이즈 σ 를 줄이면 해결된다
  4. 정답을 찾았다 — 반복이 멈췄으므로
정답 보기② — 갱신량이 0 이 되어 멈췄으므로 반복을 늘려도 그 자리에 머뭅니다(①). 노이즈 1 cm 는 0.46 m 의 RMS 를 설명하지 못하고(③), "멈췄다" 는 것은 수렴했다는 뜻이지 정답이라는 뜻이 아닙니다(④). 실제 시스템은 매칭 뒤 남은 오차(적합도 점수)를 보고 결과를 받아들일지 정하고, 오도메트리로 초기 오차를 작게 유지해 이런 상황 자체를 피합니다.

루프 폐합 — 출발점에 돌아왔을 때 한 번에 바로잡기#

스캔 매칭은 바로 앞과 비교하므로 오차를 작게 만들 뿐 0 으로 만들지는 못합니다. 남은 작은 오차는 긴 경로를 도는 동안 조금씩 쌓입니다(드리프트). 이것을 없애는 기회가 전에 와 본 곳에 돌아왔을 때 입니다.

  1. 알아보기: 지금 스캔이 예전 키프레임(일정 간격으로 저장한 위치 + 스캔)의 스캔과 잘 맞는지 확인합니다 (넓은 범위의 스캔 매칭, 카메라라면 이미지 특징 비교).
  2. 제약 만들기: "지금 위치는 예전 그 위치에서 (Δx, Δy, Δθ) 만큼 떨어져 있다" 는 루프 제약 을 얻습니다.
  3. 분산하기: 경로 전체를 노드(키프레임)와 간선(연속 이동 · 루프 제약)으로 이루어진 포즈 그래프(pose graph) 로 보고, 모든 제약을 가장 잘 만족하도록 키프레임 위치를 다시 계산합니다. 끝에서 드러난 오차가 경로를 따라 나눠 들어갑니다.
  4. 지도 다시 그리기: 고친 키프레임 위치로 저장해 둔 스캔을 다시 그립니다.

마지막 위치만 고치면 안 될까요? 그러면 출발점은 맞지만 중간 경로 전체에 쌓인 오차는 그대로 남아 지도는 여전히 이중 벽입니다. 시뮬레이터 4 에서 확인합니다.

시뮬레이터 4 · 루프 폐합 — 끝점만 고치기 vs 경로 전체에 나누기#

로봇이 12 m × 8 m 고리 모양 복도를 한 바퀴(약 32 m) 돌며 0.5 m 마다 키프레임을 남깁니다. 키프레임 스캔(점)을 추정 위치 에 그린 것이 지도입니다. 슬라이더로 주행을 진행시키고, 출발점에 돌아오면 루프 폐합 방법을 골라 봅니다.

시뮬레이터 4 · 루프 폐합 (점 = 키프레임 스캔, 주황 = 추정 경로, 초록 점선 = 참 경로 / 오른쪽: 방법별 벽 어긋남)
루프 폐합: ·

해 볼 것

  1. ▶ 주행 재생 을 눌러 키프레임이 쌓이는 모습을 봅니다. 출발점에 돌아왔을 때 추정 경로(주황)의 끝은 출발점에서 얼마나 떨어져 있나요? 빨간 점(벽에서 10 cm 넘게 어긋난 스캔 점)은 경로의 앞쪽 과 뒤쪽 중 어디에 많나요?
  2. 마지막 위치만 고치기 를 누릅니다. 끝점은 출발점에 붙었는데 지도(점)는 얼마나 나아졌나요? 오른쪽 막대에서 "안 함" 과 비교합니다.
  3. 경로를 따라 고르게 나누기 를 누릅니다. 어긋남이 몇 cm 로 줄었나요? 왜 "끝만" 보다 훨씬 좋은지, 오차가 언제 생겨서 어디까지 퍼졌는지 로 설명해 봅니다.
  4. 각도 드리프트를 0 으로, 거리 오차를 5 % 로 바꿔 봅니다. 이번에는 "나누기" 가 얼마나 잘 고치나요? 반대로 각도 드리프트 1.5 °/m 에서는 어떤가요? 고르게 나누는 방식이 가정한 것이 무엇인지 생각해 봅니다.
  5. 다른 난수 로 여러 번 바꿔도 "나누기 < 끝만 < 안 함" 의 순서가 유지되는지 확인합니다.

관찰 포인트 — 드리프트는 출발부터 조금씩 쌓였으므로, 오차는 끝에 몰려 있지 않고 경로 전체에 퍼져 있습니다. 끝점 하나만 옮기면 그 키프레임의 스캔만 제자리로 가고 나머지는 그대로입니다. 오차를 경로 길이에 비례해 나누면 "조금씩 쌓인" 드리프트의 모양과 비슷해서 대부분의 벽이 제자리로 돌아옵니다. 그런데 각도 드리프트 0 · 거리 오차 5 % 에서는 "나누기" 도 거의 효과가 없습니다. 고리를 한 바퀴 돌면 거리 배율 오차는 오른쪽 · 왼쪽, 위 · 아래로 서로 상쇄되어 끝에 드러나지 않기 때문입니다. 루프 폐합은 "끝에서 드러난 오차" 만 고칠 수 있고, 지도 전체가 몇 % 늘어난 것 같은 오차는 스캔 매칭(벽과 벽 사이 거리를 직접 재는 것)이 막아야 합니다. 실제 SLAM(Cartographer · slam_toolbox 등)은 이렇게 단순히 비례 배분하지 않고, 모든 제약의 오차 제곱합이 최소가 되도록 포즈 그래프 최적화 를 풉니다. 시뮬레이터 4 의 "나누기" 는 그 결과를 아주 단순하게 흉내 낸 것입니다.

확인 문제 3-2

시뮬레이터 4 에서 출발점에 돌아왔을 때 추정 위치가 1.4 m 틀어져 있었습니다. "마지막 위치만 고치기" 를 했더니 평균 벽 어긋남이 거의 줄지 않았습니다. 그 이유로 가장 알맞은 것은?

  1. 루프 제약이 틀렸기 때문
  2. 오차는 경로 전체에서 조금씩 쌓였는데, 마지막 키프레임 하나만 옮겼기 때문에 나머지 키프레임의 스캔은 여전히 틀린 자리에 그려지기 때문
  3. 마지막 키프레임에는 스캔이 없기 때문
  4. 지도 해상도가 너무 낮기 때문
정답 보기② — 1.4 m 는 끝에서 "드러난" 오차일 뿐, 실제로는 출발 직후부터 각 키프레임이 조금씩 틀어져 있었습니다. 지도는 모든 키프레임의 스캔으로 그리므로, 끝 하나를 고쳐도 전체 점의 극히 일부만 바뀝니다. 오차를 경로를 따라 나눠야(포즈 그래프 최적화) 중간 키프레임들도 제자리로 돌아옵니다.

2D · 시각 SLAM 비교 — Gmapping · Cartographer · RTAB-Map#

GmappingCartographerRTAB-Map
방식파티클 필터 (Rao-Blackwellized PF) — 파티클마다 자기 경로 · 지도를 가짐그래프 기반 — 작은 서브맵(submap)을 만들고 서브맵 사이 제약으로 포즈 그래프 최적화그래프 기반 + 시각 — RGB-D 카메라 영상의 특징으로 장소를 알아봄 (2D 라이다도 함께 쓸 수 있음)
스캔 매칭파티클마다 스캔 매칭으로 자세를 다듬음서브맵에 대한 스캔 매칭 + 넓은 범위 탐색(branch-and-bound)으로 루프 탐지영상 특징 · 깊이 · 라이다 스캔 정합
루프 폐합맞는 경로를 가진 파티클이 살아남는 방식으로 간접적으로명시적 루프 제약 + 전역 최적화외형 기반(bag-of-words) 장소 인식 + 그래프 최적화
장점작은 실내 공간에서 간단 · 가벼움넓은 공간 · 긴 루프에 강함, 실시간 루프 폐합라이다로 구분하기 어려운 곳(긴 복도)에서 시각 정보 활용, 3D 지도
약점파티클 수만큼 메모리 · 계산, 큰 루프에서 맞는 파티클이 사라질 수 있음설정 항목이 많음조명 · 질감 없는 벽에 약함, 계산량 큼
myAGV Pro공식 문서 6.2.2 (slam_gmapping)저장소에 cartographer · cartographer_ros 폴더가 있으나 공식 매핑 문서는 없음 (확인 필요)공식 문서 6.2.9 (Orbbec Gemini 2 카메라, rtabmap_demos)
Gmapping 의 매개변수

ROS 1 의 gmapping 문서에는 파티클 수 particles 기본값 30, 지도 해상도 delta 기본값 0.05 m 로 적혀 있습니다. myAGV Pro 의 ROS 2 포트(slam_gmapping/slam_gmapping/launch/slam_gmapping.launch.py)는 launch 파일에서 매개변수를 넘기지 않으므로, 실제 값은 패키지 소스에서 확인해야 합니다 (확인 필요).

4교시 · 실제 매핑 절차와 좋은 지도 만들기#

myAGV Pro 매핑 흐름 (공식 문서 6.2.2)#

공식 문서의 Gmapping 매핑은 터미널 4 개로 진행합니다. 각 명령은 새 터미널(Ctrl+Alt+T)에서 실행하고, 앞의 터미널은 끄지 않습니다.

myAGV Pro 매핑 흐름 (공식 문서 6.2.2 기준) — 터미널 4 개 ① 로봇 기동 agv_pro_bringup 라이다 스캔 · 오도메트리 odom → base_footprint ② SLAM (Gmapping) slam_gmapping + RViz 구독 scan → 발행 map map → odom TF ③ 수동 주행 teleop_twist_keyboard 권장 0.25 m/s · 0.45 rad/s k = 정지 ④ 지도 저장 nav2_map_server map_saver_cli -f 경로/이름 → 이름.yaml + 이름.pgm ①②③ 을 켜 둔 채 ④ 를 새 터미널에서 실행합니다. 저장한 지도는 뒤 차시의 위치 추정 · 내비게이션에서 다시 씁니다. 3D 라이다(FAST-LIO · Point-LIO)는 ② 자리에 다른 패키지가 들어가고, 결과가 점군(PCD)이라 pcd2pgm 으로 2D 지도로 바꿉니다.
로봇 기동 → SLAM 노드 → 수동 주행 → 지도 저장. ①②③ 을 켜 둔 상태에서 ④ 를 실행합니다.
bash
# ① 로봇 기동: 모터 · 라이다 · 오도메트리 · TF
ros2 launch agv_pro_bringup agv_pro_bringup.launch.py

# ② Gmapping SLAM + RViz (scan 구독 -> map 발행, map -> odom TF)
ros2 launch slam_gmapping slam_gmapping.launch.py

# ③ 키보드로 수동 주행 (k = 정지). 문서 권장: 직진 0.25 m/s, 회전 0.45 rad/s
ros2 run teleop_twist_keyboard teleop_twist_keyboard

# ④ 지도 저장: -f 뒤의 경로/이름으로 .yaml 과 .pgm 이 생김
ros2 run nav2_map_server map_saver_cli -f ~/agv_pro_ros2/src/agv_pro_navigation2/map/map_test

④ 를 실행하면 map_test.yaml 과 map_test.pgm 이 생깁니다. 공식 문서는 -f 뒤의 폴더 경로가 정확해야 하고, 틀리면 저장이 실패한다고 안내합니다(자주 나는 오류 1). 저장한 지도는 뒤 차시의 위치 추정 · 내비게이션에서 불러 씁니다.

터미널마다 환경 불러오기

새 터미널을 열 때마다 작업 공간의 환경을 먼저 불러와야 패키지를 찾습니다. 공식 문서의 3D SLAM 절차는 ~/agv_pro_ros2 에서 source ./install/setup.bash 를 먼저 실행합니다. 로봇에 미리 설정되어 있을 수 있으니, Package 'agv_pro_bringup' not found 가 나오면 이것부터 확인합니다.

매핑 주행 요령#

요령이유 (앞 교시와 연결)
천천히 — 직진 0.25 m/s, 회전 0.45 rad/s 정도스캔 사이 이동이 작아야 스캔 매칭의 초기 오차가 작습니다 (시뮬레이터 3 의 수렴 경계). 특히 빠른 제자리 회전 은 각도 오차를 키웁니다
루프를 만들기 — 출발점이나 지나온 곳으로 한 번씩 돌아오기루프 폐합이 일어날 기회를 줍니다 (시뮬레이터 4). 한쪽 끝까지 갔다가 다른 길로 돌아오는 경로가 좋습니다
특징 없는 긴 복도 주의양쪽이 매끈한 벽뿐이면 스캔이 복도 방향으로 어디에 있든 똑같이 보여, 스캔 매칭이 복도 길이 방향 을 고정하지 못합니다. 복도 끝 · 문 · 기둥이 보이게 지나가고, 지나간 뒤 넓은 곳에서 루프를 만듭니다
움직이는 물체 피하기사람 · 지게차가 지도에 벽처럼 남습니다. 매핑할 때는 통로를 비웁니다
유리 · 검은 물체 확인라이다가 못 보는 물체(ch05)는 지도에 구멍으로 남습니다. 저장한 뒤 이미지 편집으로 보완하기도 합니다
끝나면 출발점으로마지막 루프 폐합 기회이자, 지도 원점과 실제 위치를 맞추기 쉽게 합니다

시뮬레이터 5 · 매핑 미션 — 직접 몰아서 창고 지도 완성하기#

14 m × 8 m 창고(선반 두 줄, 기둥, 팔레트, 그리고 위쪽의 긴 복도)를 키보드로 몰며 지도를 만듭니다. 로봇은 0.1 초마다 스캔을 찍고, 스캔 매칭 으로 위치를 고친 뒤 지도에 그립니다. 0.3 m 마다 키프레임을 남기고, 전에 지나간 곳(6 m 이상 전에 지난 키프레임 1.2 m 안)에 오면 루프 폐합 을 시도합니다.

  • 조작: 지도를 한 번 클릭한 뒤 W/S 앞뒤, A/D 왼쪽 · 오른쪽 옆걸음(전방향 바퀴), Q/E 제자리 회전, Space 정지. 화살표 키도 됩니다. 아래 버튼을 눌러도 됩니다.
  • 미션 성공 조건: 커버리지(로봇이 갈 수 있는 빈 공간 중 지도에 "빈 공간 / 점유" 로 확정된 비율) 95 % 이상, 이중 벽 셀(실제 벽에서 0.2 m 넘게 떨어진 점유 셀) 2 % 이하. 성공한 순간의 주행 거리가 기록이며, 짧을수록 좋습니다.
시뮬레이터 5 · 매핑 미션 (지도를 클릭한 뒤 W/A/S/D 이동 · Q/E 회전 · Space 정지 / 파랑 = 추정 위치, 초록 점선 = 참 위치)
·
오도메트리 오차 · 배속 ·

해 볼 것

  1. 먼저 ▶ 예시 경로 자동 주행 을 배속 ×6 으로 지켜봅니다. 루프 폐합 메시지가 언제 나오나요? 미션이 성공했다면 주행 거리는 몇 m 인가요? 이 거리를 이길 경로를 공책에 그려 봅니다.
  2. 처음부터 를 누르고 직접 몰아 미션에 도전합니다. 라이다 최대 거리(8 m)를 생각하면 모든 통로를 끝까지 들어가야 할까요? 복도와 선반 사이 통로 중 "한 번 지나가며 양쪽을 다 볼 수 있는" 곳은 어디인가요?
  3. 스캔 매칭 과 루프 폐합 을 모두 끄고 같은 예시 경로를 자동 주행합니다. 이중 벽 % 와 위치 오차를 기록합니다. 스캔 매칭만 켜면, 둘 다 켜면 어떻게 바뀌나요? (2교시의 "오도메트리만으로 그린 지도" 와 비교)
  4. 라이다 최대 거리를 3.5 m 로 줄이고, 스캔 매칭만 켠 채(루프 폐합 끔) 위쪽 긴 복도 를 동쪽 끝에서 서쪽 끝까지 W 로 지나갑니다. 복도에 들어가기 전과 나온 뒤의 위치 오차(특히 x 방향, 초록과 파랑 로봇의 앞뒤 차이)를 비교합니다. 왜 복도 안에서만 오차가 커질까요?
  5. 직진 속도 1.0 m/s, 회전 1.5 rad/s, 오도메트리 오차 3단계로 빠르게 몰아 봅니다(배속 ×1). 공식 문서의 권장값(0.25 m/s · 0.45 rad/s)이 왜 느린지 지도 결과로 설명해 봅니다.

관찰 포인트 — 스캔 매칭이 켜져 있으면 오도메트리 오차 2단계에서도 벽이 거의 한 줄로 유지됩니다. 매 스캔마다 작은 오차를 바로 고치기 때문입니다(3교시). 루프 폐합은 그래도 남는 드리프트를 출발점 · 지나온 통로에 돌아왔을 때 한 번에 나눠 고칩니다. 긴 복도에서는 양쪽 벽이 "복도의 어느 위치에서 봐도 똑같이" 보여 스캔 매칭이 길이 방향 을 고정하지 못합니다. 복도 끝이 라이다 최대 거리 밖이면 그 방향의 오차는 오도메트리 그대로 쌓입니다. 그래서 매핑할 때는 복도 끝 · 기둥 같은 특징이 보이게 지나가고, 지난 뒤 루프를 만듭니다.

확인 문제 4-1

시뮬레이터 5 에서 라이다 최대 거리 3.5 m, 스캔 매칭만 켜고 긴 복도를 지나갔더니, 복도를 나온 뒤 추정 위치가 참 위치보다 복도 방향(x)으로 30 cm 앞서 있었고 옆 방향(y)은 거의 맞았습니다. 가장 알맞은 설명은?

  1. 스캔 매칭이 꺼져 있었다
  2. 복도 양쪽 벽은 y 와 방향(θ)은 알려 주지만, 복도 끝이 보이지 않으면 x 를 알려 주는 특징이 없어 그 방향은 오도메트리 오차가 그대로 쌓였다
  3. 라이다 노이즈가 x 방향으로만 컸다
  4. 지도 해상도가 x 방향으로만 거칠다
정답 보기② — 매끈한 평행 벽 사이에서는 로봇을 복도 방향으로 조금 옮겨도 스캔이 지도와 똑같이 겹칩니다. 스캔 매칭이 "어느 x 든 점수가 같다" 고 판단하므로 x 는 오도메트리 값(거리 슬립 포함)에 맡겨집니다. y · θ 는 벽까지 거리와 벽의 기울기로 잘 고쳐지므로 맞습니다. 이것을 퇴화(degeneracy) 라고 부르며, 최대 거리가 긴 라이다 · 바퀴 오도메트리 보정 · 카메라(시각 SLAM) · 복도 밖에서의 루프 폐합으로 보완합니다.
확인 문제 4-2

다음 중 시뮬레이터 5 의 미션(커버리지 95 % · 이중 벽 2 % 이하 · 짧은 주행)에 가장 도움이 되는 매핑 주행 계획은?

  1. 가장 빠른 속도로 모든 통로를 한 번씩 왕복한다
  2. 권장 속도로 큰 고리를 따라 돌며 특징이 보이는 곳을 지나고, 지나온 통로나 출발점으로 돌아와 루프를 닫는다. 라이다 최대 거리 안에 양쪽이 다 보이는 통로는 끝까지 들어가지 않는다
  3. 제자리에서 여러 번 돌며 스캔을 많이 모은 뒤 한 번에 이동한다
  4. 루프를 만들지 않도록 같은 곳을 다시 지나가지 않는다
정답 보기② — 커버리지는 "로봇이 간 곳" 이 아니라 "라이다가 본 곳" 이므로, 최대 거리 안에서 보이는 곳까지 굳이 들어갈 필요는 없습니다. 빠른 주행 · 빠른 회전(①, ③)은 스캔 사이 이동을 키워 스캔 매칭 오차를 늘립니다. 루프를 피하면(④) 드리프트를 고칠 기회가 사라집니다.

3D SLAM — FAST-LIO · Point-LIO 와 2D 의 차이#

myAGV Pro 는 3D 라이다 옵션(Livox MID-360, Unitree L2)으로 3D 지도도 만들 수 있습니다. 공식 문서 6.2.6 · 6.2.7 의 흐름은 다음과 같습니다.

bash
# FAST-LIO (Livox MID-360) - 공식 문서 6.2.7
ros2 launch agv_pro_bringup agv_pro_bringup.launch.py
ros2 launch livox_ros_driver2 msg_MID360_launch.py
ros2 launch fast_lio mapping.launch.py
ros2 run teleop_twist_keyboard teleop_twist_keyboard     # 문서에는 teletop 으로 오타가 있음

# Point-LIO (Unitree L2) - 공식 문서 6.2.6
ros2 launch unitree_lidar_ros2 launch.py
ros2 launch point_lio mapping_unilidar_l2.launch.py

# Ctrl+C 로 끝내면 점군이 PCD 로 저장됨 -> 2D 지도로 변환
ros2 launch pcd2pgm pcd2pgm.launch.py
ros2 run nav2_map_server map_saver_cli -f <지도 이름>
2D 라이다 SLAM (Gmapping 등)3D 라이다-관성 SLAM (FAST-LIO · Point-LIO)
입력한 높이의 수평 스캔 + 바퀴 오도메트리3D 점군 + IMU (라이다에 내장된 IMU 등)
위치x, y, θ (3 자유도)x, y, z, 롤, 피치, 요 (6 자유도)
핵심스캔 매칭 + 파티클 / 그래프IMU 로 움직임을 예측하고 점군을 지도에 정합하는 반복 칼만 필터(iterated EKF). Point-LIO 는 점 하나하나를 받을 때마다 갱신
결과점유격자 (.pgm + .yaml)3D 점군 (.pcd) → 필요하면 2D 로 투영 (pcd2pgm)
강점가볍고 내비게이션에 바로 사용낮은 장애물 · 돌출물까지 지도에 담김(ch05), 바퀴 슬립 · 경사로에 덜 민감
약점한 높이만 봄, 바퀴 오도메트리에 의존계산량 · 저장 용량이 큼, 2D 내비게이션에 쓰려면 변환 필요

RTAB-Map(6.2.9)은 깊이 카메라 영상으로 장소를 알아보는 시각 SLAM 이며, 끝내면 ~/.ros/rtabmap.db 에 데이터베이스로 저장됩니다(공식 문서).

RTAB-Map 으로 매핑 · 내비게이션 함께 하기 (공식 문서 6.2.9)#

RTAB-Map 은 새 영상이 전에 본 장소인지를 단어 가방(bag-of-words) 방식으로 판단해 루프 폐합을 찾는 그래프 SLAM 입니다 (3교시의 루프 폐합이 영상으로 일어남). 비주얼 내비게이션형은 깊이 카메라(Orbbec Gemini 2)와 2D 라이다를 함께 쓸 수 있습니다.

bash
# 1) 매핑만
ros2 launch agv_pro_bringup agv_pro_bringup.launch.py          # 오도메트리 · 라이다
ros2 launch orbbec_camera gemini2.launch.py                    # 깊이 카메라
ros2 launch rtabmap_demos robot_mapping_demo.launch.py rviz:=true rtabmap_viz:=false
ros2 run teleop_twist_keyboard teleop_twist_keyboard           # 문서에는 teletop 오타

# 2) 매핑 + Nav2 동시 (RTAB-Map 이 AMCL 자리를 대신함)
ros2 launch rtabmap_demos agvpro_rgbd_scan.launch.py           # 카메라 + 라이다
#    카메라만: agvpro_rgbd.launch.py   ·   라이다만: agvpro_scan.launch.py

# 3) 다 그린 뒤 위치 추정 모드 (지도를 더 늘리지 않음)
ros2 launch rtabmap_demos agvpro_rgbd_scan.launch.py localization:=true
항목내용 (공식 문서)
저장Ctrl+C 로 끝내면 ~/.ros/rtabmap.db 에 자동 저장 (~/.ros 는 숨김 폴더 — 파일 탐색기에서 Ctrl+H)
코스트맵카메라의 /camera/obstacles · /camera/ground 점군과 /scan 으로 복셀 레이어를 채워, 2D 라이다만 쓸 때보다 낮은 · 튀어나온 장애물에 강함 (ch05 · ch08)
주행 요령일정한 속도로, 급가속 · 급회전 · 오래 제자리 회전 금지. teleop 기본값 speed 0.25 · turn 0.5 권장
위치 추정 모드다시 켠 뒤 로봇을 움직여 루프 폐합이 검출되어야 2D 지도가 다시 나타나고 내비게이션을 이어 갈 수 있음

agvpro_*.launch.py 파일은 저장소 rtabmap_ros/rtabmap_demos/launch/agvpro/ 에 들어 있습니다.

실습 (시뮬레이터)#

교시마다 Playground 에서 코드를 직접 돌립니다. starter 의 TODO 를 채워 실행한 뒤 풀이 와 비교하세요. 모든 코드는 표준 라이브러리 + numpy 만 씁니다.

Playground 열기

실습 1-1 · 스캔 1장을 50 × 50 격자에 그리기 (1교시)

계획의 실습 "스캔 1개를 50×50 격자에 그려 보는 numpy 코드 완성" 입니다. 5 m × 5 m 방을 0.1 m 셀 50 × 50 으로 나누고, (−1.0, −0.5) 에 선 로봇의 스캔 360 광선을 브레젠험 레이 추적과 로그 오즈로 그립니다.

  • TODO 1 world_to_cell() — 세계 좌표를 셀 (i, j) 로 바꿉니다. 원점은 셀 (0, 0) 의 왼쪽 아래 모서리 (−2.5, −2.5) 입니다.
  • TODO 2 bresenham() — 오차 err 를 보며 x 또는 y 로 한 칸씩 나아갑니다.
  • TODO 3 integrate() — 끝 셀 앞까지는 L_FREE, 끝 셀은 맞았으면 L_OCC 를 더합니다. 최대 거리까지 맞지 않은 광선(inf)은 끝 셀도 빈 공간입니다.

예측 먼저 — 스캔 1장만 그렸을 때 ROS 값(to_ros)으로 "빈 공간(0)" 이 되는 셀은 많을까요, 적을까요? L_FREE = −0.4 는 p ≈ 0.40 이고, 빈 공간 판정은 p < 0.25 입니다.

python시뮬 실행 가능
import numpy as np

# 5 m x 5 m 를 0.1 m 셀 50 x 50 으로. 셀 (0, 0) 의 왼쪽 아래 모서리 = (-2.5, -2.5)
N, RES, OX, OY = 50, 0.1, -2.5, -2.5
L_OCC, L_FREE, L_CLAMP = 0.85, -0.4, 4.0
# 방: 벽 4개 + 상자 1개 (x0, y0, x1, y1)
BOXES = np.array([[-2.2, -2.2, 2.2, -2.1], [-2.2, 2.1, 2.2, 2.2], [-2.2, -2.2, -2.1, 2.2],
                  [2.1, -2.2, 2.2, 2.2], [0.6, 0.4, 1.2, 1.0]])

def lidar(px, py, n=360, rmax=3.0, sigma=0.0, seed=0):
    """(px, py) 에서 n 개 광선의 거리. 최대 거리 밖이면 inf"""
    rng = np.random.default_rng(seed)
    ang = np.linspace(-np.pi, np.pi, n, endpoint=False)
    c, s = np.cos(ang)[:, None], np.sin(ang)[:, None]
    x0, y0, x1, y1 = (BOXES[:, k] - (px, py, px, py)[k] for k in range(4))
    with np.errstate(divide="ignore", invalid="ignore"):
        tmin = np.maximum(np.minimum(x0 / c, x1 / c), np.minimum(y0 / s, y1 / s))
        tmax = np.minimum(np.maximum(x0 / c, x1 / c), np.maximum(y0 / s, y1 / s))
    t = np.where((tmax >= tmin) & (tmin > 0), tmin, np.inf).min(axis=1)
    t = t + rng.normal(0, sigma, n)
    return ang, np.where(t <= rmax, t, np.inf)

def world_to_cell(x, y):
    # TODO 1: 세계 좌표 -> 셀 (i, j). i = floor((x - OX) / RES)
    return 0, 0

def bresenham(i0, j0, i1, j1):
    """(i0, j0) 에서 (i1, j1) 까지 직선이 지나는 셀 목록 (양 끝 포함)"""
    cells = []
    dx, dy = abs(i1 - i0), -abs(j1 - j0)
    sx, sy = (1 if i0 < i1 else -1), (1 if j0 < j1 else -1)
    err, i, j = dx + dy, i0, j0
    while True:
        cells.append((i, j))
        if i == i1 and j == j1:
            break
        # TODO 2: e2 = 2 * err. e2 >= dy 이면 x 로 한 칸, e2 <= dx 이면 y 로 한 칸
        break                               # TODO 2 를 채우면 이 줄을 지웁니다
    return cells

def integrate(L, px, py, ang, rng_, rmax=3.0):
    i0, j0 = world_to_cell(px, py)
    for a, r in zip(ang, rng_):
        hit = np.isfinite(r)
        d = r if hit else rmax
        i1, j1 = world_to_cell(px + d * np.cos(a), py + d * np.sin(a))
        cells = bresenham(i0, j0, i1, j1)
        # TODO 3: 끝 셀 앞까지는 L_FREE, 끝 셀은 맞았으면(hit) L_OCC / 아니면 L_FREE
        pass
    np.clip(L, -L_CLAMP, L_CLAMP, out=L)

def to_ros(L):
    """로그 오즈 -> ROS 값: 한 번도 안 본 셀 -1, p > 0.65 -> 100, p < 0.25 -> 0, 그 사이 -1"""
    p = 1 / (1 + np.exp(-L))
    out = np.full(L.shape, -1, dtype=np.int8)
    out[(L != 0) & (p < 0.25)] = 0
    out[(L != 0) & (p > 0.65)] = 100
    return out

def show(L, robot):
    """L > 0 은 '#', L < 0 은 '.', 아직 0 이면 ' ' (위쪽 = 큰 y 부터 출력)"""
    for j in range(N - 1, -1, -1):
        print("".join("R" if (i, j) == robot else "#" if L[j, i] > 0 else "." if L[j, i] < 0 else " "
                      for i in range(N)))

L = np.zeros((N, N))
px, py = -1.0, -0.5
ang, r = lidar(px, py)
integrate(L, px, py, ang, r)
show(L, world_to_cell(px, py))
g = to_ros(L)
print("ROS values after 1 scan: unknown", int((g == -1).sum()), " free", int((g == 0).sum()), " occupied", int((g == 100).sum()))
print("bresenham (0,0)->(5,2):", bresenham(0, 0, 5, 2))

# 같은 자리에서 노이즈 있는 스캔을 여러 번: 상자 앞면 셀의 확신이 어떻게 자라나?
L2 = np.zeros((N, N))
ci, cj = world_to_cell(0.62, 0.75)           # 상자 왼쪽 면이 들어 있는 셀
for k in range(1, 11):
    ang, r = lidar(px, py, sigma=0.03, seed=k)
    integrate(L2, px, py, ang, r)
    if k in (1, 2, 3, 5, 10):
        p = 1 / (1 + np.exp(-L2[cj, ci]))
        print(f"scans {k:2d}: L = {L2[cj, ci]:+.2f}  p = {p:.2f}  ros = {to_ros(L2)[cj, ci]}")

해 볼 것

  1. 출력된 지도에서 상자 뒤쪽 은 왜 비어 있나요( )? 로봇 위치 px, py 를 (1.5, −1.5) 로 옮기면 어느 면이 새로 보이나요?
  2. "ROS values after 1 scan" 의 free 개수는 화면의 . 개수보다 훨씬 적습니다. 로봇 근처 셀만 빈 공간(0)이 되는 이유를 광선 밀도로 설명해 봅니다.
  3. L_FREE 를 −0.4 → −1.2 로 바꾸면 스캔 1장만으로 free 가 크게 늘어납니다. 대신 무엇이 위험할까요? (ch05 의 먼지 · 반사 실패 광선을 떠올려 보세요)
  4. L_CLAMP 를 4.0 → 1.0 으로 줄이고 마지막 출력에서 p 가 몇에서 멈추는지 봅니다.
풀이
python시뮬 실행 가능
import numpy as np

# 5 m x 5 m 를 0.1 m 셀 50 x 50 으로. 셀 (0, 0) 의 왼쪽 아래 모서리 = (-2.5, -2.5)
N, RES, OX, OY = 50, 0.1, -2.5, -2.5
L_OCC, L_FREE, L_CLAMP = 0.85, -0.4, 4.0
# 방: 벽 4개 + 상자 1개 (x0, y0, x1, y1)
BOXES = np.array([[-2.2, -2.2, 2.2, -2.1], [-2.2, 2.1, 2.2, 2.2], [-2.2, -2.2, -2.1, 2.2],
                  [2.1, -2.2, 2.2, 2.2], [0.6, 0.4, 1.2, 1.0]])

def lidar(px, py, n=360, rmax=3.0, sigma=0.0, seed=0):
    """(px, py) 에서 n 개 광선의 거리. 최대 거리 밖이면 inf"""
    rng = np.random.default_rng(seed)
    ang = np.linspace(-np.pi, np.pi, n, endpoint=False)
    c, s = np.cos(ang)[:, None], np.sin(ang)[:, None]
    x0, y0, x1, y1 = (BOXES[:, k] - (px, py, px, py)[k] for k in range(4))
    with np.errstate(divide="ignore", invalid="ignore"):
        tmin = np.maximum(np.minimum(x0 / c, x1 / c), np.minimum(y0 / s, y1 / s))
        tmax = np.minimum(np.maximum(x0 / c, x1 / c), np.maximum(y0 / s, y1 / s))
    t = np.where((tmax >= tmin) & (tmin > 0), tmin, np.inf).min(axis=1)
    t = t + rng.normal(0, sigma, n)
    return ang, np.where(t <= rmax, t, np.inf)

def world_to_cell(x, y):
    # TODO 1: 세계 좌표 -> 셀 (i, j). i = floor((x - OX) / RES)
    return int(np.floor((x - OX) / RES)), int(np.floor((y - OY) / RES))

def bresenham(i0, j0, i1, j1):
    """(i0, j0) 에서 (i1, j1) 까지 직선이 지나는 셀 목록 (양 끝 포함)"""
    cells = []
    dx, dy = abs(i1 - i0), -abs(j1 - j0)
    sx, sy = (1 if i0 < i1 else -1), (1 if j0 < j1 else -1)
    err, i, j = dx + dy, i0, j0
    while True:
        cells.append((i, j))
        if i == i1 and j == j1:
            break
        # TODO 2: e2 = 2 * err. e2 >= dy 이면 x 로 한 칸, e2 <= dx 이면 y 로 한 칸
        e2 = 2 * err
        if e2 >= dy:
            err += dy
            i += sx
        if e2 <= dx:
            err += dx
            j += sy
    return cells

def integrate(L, px, py, ang, rng_, rmax=3.0):
    i0, j0 = world_to_cell(px, py)
    for a, r in zip(ang, rng_):
        hit = np.isfinite(r)
        d = r if hit else rmax
        i1, j1 = world_to_cell(px + d * np.cos(a), py + d * np.sin(a))
        cells = bresenham(i0, j0, i1, j1)
        # TODO 3: 끝 셀 앞까지는 L_FREE, 끝 셀은 맞았으면(hit) L_OCC / 아니면 L_FREE
        for (i, j) in cells[:-1]:
            if 0 <= i < N and 0 <= j < N:
                L[j, i] += L_FREE
        i, j = cells[-1]
        if 0 <= i < N and 0 <= j < N:
            L[j, i] += L_OCC if hit else L_FREE
    np.clip(L, -L_CLAMP, L_CLAMP, out=L)

def to_ros(L):
    """로그 오즈 -> ROS 값: 한 번도 안 본 셀 -1, p > 0.65 -> 100, p < 0.25 -> 0, 그 사이 -1"""
    p = 1 / (1 + np.exp(-L))
    out = np.full(L.shape, -1, dtype=np.int8)
    out[(L != 0) & (p < 0.25)] = 0
    out[(L != 0) & (p > 0.65)] = 100
    return out

def show(L, robot):
    """L > 0 은 '#', L < 0 은 '.', 아직 0 이면 ' ' (위쪽 = 큰 y 부터 출력)"""
    for j in range(N - 1, -1, -1):
        print("".join("R" if (i, j) == robot else "#" if L[j, i] > 0 else "." if L[j, i] < 0 else " "
                      for i in range(N)))

L = np.zeros((N, N))
px, py = -1.0, -0.5
ang, r = lidar(px, py)
integrate(L, px, py, ang, r)
show(L, world_to_cell(px, py))
g = to_ros(L)
print("ROS values after 1 scan: unknown", int((g == -1).sum()), " free", int((g == 0).sum()), " occupied", int((g == 100).sum()))
print("bresenham (0,0)->(5,2):", bresenham(0, 0, 5, 2))

# 같은 자리에서 노이즈 있는 스캔을 여러 번: 상자 앞면 셀의 확신이 어떻게 자라나?
L2 = np.zeros((N, N))
ci, cj = world_to_cell(0.62, 0.75)           # 상자 왼쪽 면이 들어 있는 셀
for k in range(1, 11):
    ang, r = lidar(px, py, sigma=0.03, seed=k)
    integrate(L2, px, py, ang, r)
    if k in (1, 2, 3, 5, 10):
        p = 1 / (1 + np.exp(-L2[cj, ci]))
        print(f"scans {k:2d}: L = {L2[cj, ci]:+.2f}  p = {p:.2f}  ros = {to_ros(L2)[cj, ci]}")
  • 스캔 1장 뒤 ROS 값은 대략 unknown 1465 · free 925 · occupied 110 입니다. 로봇 가까이는 여러 광선이 같은 셀을 지나가 L 이 −0.4 를 여러 번 받지만, 먼 셀은 광선 하나만 지나가 L = −0.4 (p 0.40) 에 머물러 아직 "미지" 로 남습니다.
  • 상자 면 셀의 p 는 0.70 → 0.93 → 0.98 로 커지다가 L_CLAMP = 4 에서 멈춥니다. 노이즈 때문에 끝점이 옆 셀로 떨어지는 스캔이 있으면 그 스캔에서는 오르지 않습니다.
실습 1-2 · map.yaml 로 좌표 읽기와 해상도 비교 (1교시)

계획의 시뮬레이터 활동 "해상도 0.02 / 0.05 / 0.1 m 비교, map.yaml 구조 읽기" 를 코드로 확인합니다. agv_pro_ros2 의 map.yaml 값을 그대로 쓰고, 6 m × 4 m 방을 같은 스캔 8장으로 세 해상도에 그립니다.

  • TODO 1 world_to_cell() — origin · resolution 으로 셀 (i, j) 계산.
  • TODO 2 cell_to_pgm() — 격자 행 j(아래부터)를 PGM 행(위부터)으로.
  • TODO 3 벽 두께 = 점유 셀 넓이 합 ÷ 표면 길이, 벽 커버리지 = 벽 표면 점 중 근처(±1 셀)에 점유 셀이 있는 비율.

예측 먼저 — 0.02 m 는 가장 촘촘하니 벽도 가장 "빈틈없이" 그려질까요?

python시뮬 실행 가능
import numpy as np

MAP_YAML = """image: map.pgm
mode: trinary
resolution: 0.05
origin: [-10, -24.4, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25"""

def parse_yaml(text):
    """한 줄에 '키: 값' 인 간단한 yaml 만 읽음. [a, b, c] 는 float 목록으로"""
    info = {}
    for line in text.splitlines():
        key, val = [s.strip() for s in line.split(":", 1)]
        if val.startswith("["):
            info[key] = [float(v) for v in val.strip("[]").split(",")]
        else:
            try:
                info[key] = float(val)
            except ValueError:
                info[key] = val
    return info

def world_to_cell(info, x, y):
    # TODO 1: origin 과 resolution 으로 셀 (i, j) 계산
    return 0, 0

def cell_to_pgm(info, i, j, height):
    # TODO 2: 격자 행 j 는 아래부터, PGM 행은 위부터 -> (row, col)
    return j, i

info = parse_yaml(MAP_YAML)
H = 600                                              # 예: 그림 높이 600 픽셀이라고 가정
for x, y in [(0.02, 0.03), (-10.0, -24.4), (2.5, -20.0)]:
    i, j = world_to_cell(info, x, y)
    print(f"map ({x:6.2f}, {y:6.2f}) m -> cell (i={i}, j={j}) -> pgm (row={cell_to_pgm(info, i, j, H)[0]}, col={i})")

# --- 해상도 비교: 6 m x 4 m 방을 같은 스캔 8장으로 ---
BOXES = np.array([[0, 0, 6, 0.1], [0, 3.9, 6, 4], [0, 0, 0.1, 4], [5.9, 0, 6, 4],
                  [2.0, 1.2, 2.6, 2.0], [4.2, 2.6, 4.5, 2.9]])
POSES = [(0.8, 0.8), (1.5, 3.0), (3.2, 0.7), (3.4, 2.4), (5.0, 1.0), (5.2, 3.3), (1.0, 2.0), (3.5, 3.4)]
L_OCC, L_FREE = 0.85, -0.4

def lidar(px, py, n=360, rmax=8.0, sigma=0.03, rng=None):
    ang = np.linspace(-np.pi, np.pi, n, endpoint=False)
    c, s = np.cos(ang)[:, None], np.sin(ang)[:, None]
    x0, y0, x1, y1 = (BOXES[:, k] - (px, py, px, py)[k] for k in range(4))
    with np.errstate(divide="ignore", invalid="ignore"):
        tmin = np.maximum(np.minimum(x0 / c, x1 / c), np.minimum(y0 / s, y1 / s))
        tmax = np.minimum(np.maximum(x0 / c, x1 / c), np.maximum(y0 / s, y1 / s))
    t = np.where((tmax >= tmin) & (tmin > 0), tmin, np.inf).min(axis=1)
    return ang, t + rng.normal(0, sigma, n)

def build(res, seed=1):
    rng = np.random.default_rng(seed)
    nx, ny = int(round(6 / res)), int(round(4 / res))
    L = np.zeros((ny, nx))
    for px, py in POSES:
        ang, r = lidar(px, py, rng=rng)
        # 광선 위를 반 셀 간격으로 샘플링해 지나간 셀(빈 공간)을 한꺼번에 찾음
        k = np.arange(0, 8.0, res / 2)[None, :]
        xs, ys = px + np.cos(ang)[:, None] * k, py + np.sin(ang)[:, None] * k
        before = k < (r[:, None] - res / 2)
        fi, fj = np.floor(xs[before] / res).astype(int), np.floor(ys[before] / res).astype(int)
        ok = (fi >= 0) & (fi < nx) & (fj >= 0) & (fj < ny)
        free = np.unique(fj[ok] * nx + fi[ok])
        ei = np.floor((px + r * np.cos(ang)) / res).astype(int)
        ej = np.floor((py + r * np.sin(ang)) / res).astype(int)
        ok = (ei >= 0) & (ei < nx) & (ej >= 0) & (ej < ny)
        occ = np.unique(ej[ok] * nx + ei[ok])
        free = np.setdiff1d(free, occ)
        L.flat[free] += L_FREE
        L.flat[occ] += L_OCC
    return L

def face_points(step=0.01):
    """방 안쪽에서 보이는 벽 · 상자 표면 위의 점들"""
    pts = []
    for x in np.arange(0.1, 5.9, step):
        pts += [(x, 0.1), (x, 3.9)]
    for y in np.arange(0.1, 3.9, step):
        pts += [(0.1, y), (5.9, y)]
    for x0, y0, x1, y1 in BOXES[4:]:
        for x in np.arange(x0, x1, step):
            pts += [(x, y0), (x, y1)]
        for y in np.arange(y0, y1, step):
            pts += [(x0, y), (x1, y)]
    return np.array(pts)

FACE = face_points()
print()
print("res   | 50mx50m warehouse     | this room: occupied cells  wall thickness  wall coverage")
for res in (0.02, 0.05, 0.1):
    cells = int(round(50 / res)) ** 2
    L = build(res)
    occ = L > 0.5
    # TODO 3: 벽 두께 = 점유 셀 넓이 합 / 표면 길이, 벽 커버리지 = 표면 점 중 '그 점 근처 셀(±1)' 이 점유인 비율
    thick = 0.0
    near = np.zeros(len(FACE), bool)
    print(f"{res:.2f} m | {cells:>9,d} cells {cells / 1e6:5.2f} MB | {int(occ.sum()):6d}             "
          f"{100 * thick:5.1f} cm         {100 * near.mean():5.1f} %")

해 볼 것

  1. map 좌표 (0.02, 0.03) 은 PGM 그림에서 위쪽에 가깝나요, 아래쪽에 가깝나요? cell_to_pgm 을 return j, i 로 두었을 때와 비교합니다.
  2. 50 m × 50 m 창고의 메모리를 0.02 m 와 0.1 m 로 비교하면 몇 배인가요? 해상도 비율(5 배)과 어떤 관계인가요?
  3. 0.02 m 에서 벽 커버리지가 100 % 가 되지 않는 이유는? 라이다 노이즈 sigma 를 0.03 → 0.005 로 줄이면 어떻게 바뀌나요?
  4. 0.1 m 의 벽 두께(약 18 cm)는 실제 벽 두께(10 cm)와 왜 다를까요? 내비게이션에서 좁은 통로를 지날 때 어떤 영향이 있을까요?
풀이
python시뮬 실행 가능
import numpy as np

MAP_YAML = """image: map.pgm
mode: trinary
resolution: 0.05
origin: [-10, -24.4, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25"""

def parse_yaml(text):
    """한 줄에 '키: 값' 인 간단한 yaml 만 읽음. [a, b, c] 는 float 목록으로"""
    info = {}
    for line in text.splitlines():
        key, val = [s.strip() for s in line.split(":", 1)]
        if val.startswith("["):
            info[key] = [float(v) for v in val.strip("[]").split(",")]
        else:
            try:
                info[key] = float(val)
            except ValueError:
                info[key] = val
    return info

def world_to_cell(info, x, y):
    # TODO 1: origin 과 resolution 으로 셀 (i, j) 계산
    ox, oy = info["origin"][0], info["origin"][1]
    res = info["resolution"]
    return int(np.floor((x - ox) / res)), int(np.floor((y - oy) / res))

def cell_to_pgm(info, i, j, height):
    # TODO 2: 격자 행 j 는 아래부터, PGM 행은 위부터 -> (row, col)
    return height - 1 - j, i

info = parse_yaml(MAP_YAML)
H = 600                                              # 예: 그림 높이 600 픽셀이라고 가정
for x, y in [(0.02, 0.03), (-10.0, -24.4), (2.5, -20.0)]:
    i, j = world_to_cell(info, x, y)
    print(f"map ({x:6.2f}, {y:6.2f}) m -> cell (i={i}, j={j}) -> pgm (row={cell_to_pgm(info, i, j, H)[0]}, col={i})")

# --- 해상도 비교: 6 m x 4 m 방을 같은 스캔 8장으로 ---
BOXES = np.array([[0, 0, 6, 0.1], [0, 3.9, 6, 4], [0, 0, 0.1, 4], [5.9, 0, 6, 4],
                  [2.0, 1.2, 2.6, 2.0], [4.2, 2.6, 4.5, 2.9]])
POSES = [(0.8, 0.8), (1.5, 3.0), (3.2, 0.7), (3.4, 2.4), (5.0, 1.0), (5.2, 3.3), (1.0, 2.0), (3.5, 3.4)]
L_OCC, L_FREE = 0.85, -0.4

def lidar(px, py, n=360, rmax=8.0, sigma=0.03, rng=None):
    ang = np.linspace(-np.pi, np.pi, n, endpoint=False)
    c, s = np.cos(ang)[:, None], np.sin(ang)[:, None]
    x0, y0, x1, y1 = (BOXES[:, k] - (px, py, px, py)[k] for k in range(4))
    with np.errstate(divide="ignore", invalid="ignore"):
        tmin = np.maximum(np.minimum(x0 / c, x1 / c), np.minimum(y0 / s, y1 / s))
        tmax = np.minimum(np.maximum(x0 / c, x1 / c), np.maximum(y0 / s, y1 / s))
    t = np.where((tmax >= tmin) & (tmin > 0), tmin, np.inf).min(axis=1)
    return ang, t + rng.normal(0, sigma, n)

def build(res, seed=1):
    rng = np.random.default_rng(seed)
    nx, ny = int(round(6 / res)), int(round(4 / res))
    L = np.zeros((ny, nx))
    for px, py in POSES:
        ang, r = lidar(px, py, rng=rng)
        # 광선 위를 반 셀 간격으로 샘플링해 지나간 셀(빈 공간)을 한꺼번에 찾음
        k = np.arange(0, 8.0, res / 2)[None, :]
        xs, ys = px + np.cos(ang)[:, None] * k, py + np.sin(ang)[:, None] * k
        before = k < (r[:, None] - res / 2)
        fi, fj = np.floor(xs[before] / res).astype(int), np.floor(ys[before] / res).astype(int)
        ok = (fi >= 0) & (fi < nx) & (fj >= 0) & (fj < ny)
        free = np.unique(fj[ok] * nx + fi[ok])
        ei = np.floor((px + r * np.cos(ang)) / res).astype(int)
        ej = np.floor((py + r * np.sin(ang)) / res).astype(int)
        ok = (ei >= 0) & (ei < nx) & (ej >= 0) & (ej < ny)
        occ = np.unique(ej[ok] * nx + ei[ok])
        free = np.setdiff1d(free, occ)
        L.flat[free] += L_FREE
        L.flat[occ] += L_OCC
    return L

def face_points(step=0.01):
    """방 안쪽에서 보이는 벽 · 상자 표면 위의 점들"""
    pts = []
    for x in np.arange(0.1, 5.9, step):
        pts += [(x, 0.1), (x, 3.9)]
    for y in np.arange(0.1, 3.9, step):
        pts += [(0.1, y), (5.9, y)]
    for x0, y0, x1, y1 in BOXES[4:]:
        for x in np.arange(x0, x1, step):
            pts += [(x, y0), (x, y1)]
        for y in np.arange(y0, y1, step):
            pts += [(x0, y), (x1, y)]
    return np.array(pts)

FACE = face_points()
print()
print("res   | 50mx50m warehouse     | this room: occupied cells  wall thickness  wall coverage")
for res in (0.02, 0.05, 0.1):
    cells = int(round(50 / res)) ** 2
    L = build(res)
    occ = L > 0.5
    # TODO 3: 벽 두께 = 점유 셀 넓이 합 / 표면 길이, 벽 커버리지 = 표면 점 중 '그 점 근처 셀(±1)' 이 점유인 비율
    thick = occ.sum() * res * res / (len(FACE) * 0.01)
    ny, nx = L.shape
    i = np.clip(np.floor(FACE[:, 0] / res).astype(int), 1, nx - 2)
    j = np.clip(np.floor(FACE[:, 1] / res).astype(int), 1, ny - 2)
    near = np.zeros(len(FACE), bool)
    for di in (-1, 0, 1):
        for dj in (-1, 0, 1):
            near |= occ[j + dj, i + di]
    print(f"{res:.2f} m | {cells:>9,d} cells {cells / 1e6:5.2f} MB | {int(occ.sum()):6d}             "
          f"{100 * thick:5.1f} cm         {100 * near.mean():5.1f} %")
  • map (0.02, 0.03) → 셀 (200, 488) → PGM 행 111 입니다. 높이 600 픽셀 그림에서 행 111 은 그림의 위쪽 입니다. 행 변환을 빼먹으면 위아래가 뒤집힌 지도가 됩니다(자주 나는 오류 4). (0.02, 0.03) 대신 정확히 셀 경계 위의 점(예: y = 0.0)을 넣으면 부동소수점 때문에 488 이 아니라 487 이 나올 수 있습니다. 경계 위의 점은 어느 셀이든 맞는 답입니다.
  • 메모리는 해상도의 제곱 에 반비례해 25 배(6.25 MB : 0.25 MB)입니다. 0.02 m 는 셀이 라이다 노이즈(σ 3 cm)보다 작아 끝점이 여러 셀로 흩어지고, 한 셀이 받는 증거가 적어 벽에 빈틈이 생깁니다(커버리지 약 91 %). 0.1 m 는 벽이 끊기지 않지만 두께가 셀 크기로 부풀어 보입니다.
실습 2-1 · 노이즈 3단계로 창고 지도 그리고 "쓸 수 있는가" 판정 (2교시)

계획의 실습 "노이즈 수준 3단계로 창고 지도를 그려 어느 단계부터 쓸 수 없는지 판정" 입니다. 시뮬레이터 2 와 같은 창고 · 경로 · 노이즈 단계를 numpy 로 다시 돌립니다. 판정 기준은 시뮬레이터 5 미션과 같이 이중 벽 셀 2 % 이하 입니다.

  • TODO 1 제자리 회전 — 오도메트리 각도에 da × (1 + er) 를 더합니다 (회전 배율 오차).
  • TODO 2 직진 — 각도에 N(0, 1) × st × √ds 노이즈를 더하고, 거리는 ds × (1 + es) 로 적분합니다 (ch04 의 오도메트리 적분).
  • TODO 3 ghost_ratio() — 점유 셀 중 실제 벽에서 2 셀(0.2 m)보다 먼 셀의 비율.

예측 먼저 — 가운데 통로를 두 번 지나가는 R2 는 R1 보다 이중 벽이 많을까요, 적을까요?

python시뮬 실행 가능
import numpy as np

# 시뮬레이터 2 와 같은 14 m x 8 m 창고 (상자 = x0, y0, x1, y1)
B = np.array([[0, 0, 14, 0.15], [0, 7.85, 14, 8], [0, 0, 0.15, 8], [13.85, 0, 14, 8],
              [2.5, 2.0, 11.5, 2.8], [2.5, 4.4, 11.5, 5.2], [12.9, 0.15, 13.85, 0.9],
              [0.15, 7.0, 0.9, 7.85], [7.0, 6.0, 7.3, 6.3], [5.0, 0.15, 5.3, 0.45]])
R1 = [(1.2, 1.0), (12.4, 1.0), (12.4, 3.6), (1.5, 3.6), (1.5, 6.8), (12.4, 6.8)]
R2 = R1 + [(12.4, 3.6), (1.2, 3.6), (1.2, 1.0)]          # 가운데 통로를 두 번 지나감
# 단계: (거리 슬립 %, 회전 배율 오차 %, 각도 노이즈 deg/sqrt(m))
LEVELS = {0: (0, 0, 0), 1: (0.5, 0.5, 0.1), 2: (1, 1, 0.2), 3: (3, 3, 0.8)}
RES, NX, NY, NR, RMAX = 0.1, 140, 80, 180, 8.0
L_OCC, L_FREE, L_MAX, L_TH = 0.85, -0.4, 4.0, np.log(0.65 / 0.35)

def lidar(x, y, th, rng):
    ang = th + np.linspace(0, 2 * np.pi, NR, endpoint=False)
    c, s = np.cos(ang)[:, None], np.sin(ang)[:, None]
    x0, y0, x1, y1 = B[:, 0] - x, B[:, 1] - y, B[:, 2] - x, B[:, 3] - y
    with np.errstate(divide="ignore", invalid="ignore"):
        tmin = np.maximum(np.minimum(x0 / c, x1 / c), np.minimum(y0 / s, y1 / s))
        tmax = np.minimum(np.maximum(x0 / c, x1 / c), np.maximum(y0 / s, y1 / s))
    t = np.where((tmax >= tmin) & (tmin > 0), tmin, np.inf).min(axis=1)
    t = t + rng.normal(0, 0.015, NR)
    return ang - th, np.where(t <= RMAX, t, np.inf)     # 로봇 기준 각도, 거리

def drive(route, level, seed=7):
    """참 위치와 오도메트리 위치를 함께 적분하고, 0.25 m 마다 스캔을 찍음"""
    es, er, st = LEVELS[level]
    es, er, st = es / 100, er / 100, np.radians(st)
    rng = np.random.default_rng(seed)
    tx, ty = route[0]
    tth = np.arctan2(route[1][1] - ty, route[1][0] - tx)
    ox, oy, oth = tx, ty, tth
    frames = [((tx, ty, tth), (ox, oy, oth), lidar(tx, ty, tth, rng))]
    for gx, gy in route[1:]:
        ga = np.arctan2(gy - ty, gx - tx)
        da = np.arctan2(np.sin(ga - tth), np.cos(ga - tth))
        # TODO 1: 제자리 회전. 참 각도는 ga, 오도메트리 각도는 da 를 (1 + er) 배 만큼 더함
        tth = ga
        oth = ga
        n = int(np.ceil(np.hypot(gx - tx, gy - ty) / 0.05))
        ds = np.hypot(gx - tx, gy - ty) / n
        for k in range(1, n + 1):
            tx, ty = tx + ds * np.cos(ga), ty + ds * np.sin(ga)
            # TODO 2: 오도메트리 직진. 각도에 노이즈 st*sqrt(ds) 를 더하고, 거리는 ds*(1+es) 로 적분
            ox, oy = tx, ty
            if k % 5 == 0 or k == n:
                frames.append(((tx, ty, tth), (ox, oy, oth), lidar(tx, ty, tth, rng)))
    return frames

def integrate(L, pose, scan):
    x, y, th = pose
    ang, r = scan
    a = th + ang
    k = np.arange(0, RMAX, RES / 2)[None, :]
    rr = np.where(np.isfinite(r), r, RMAX)[:, None]
    xs, ys = x + np.cos(a)[:, None] * k, y + np.sin(a)[:, None] * k
    m = k < rr - RES / 2
    fi, fj = np.floor(xs[m] / RES).astype(int), np.floor(ys[m] / RES).astype(int)
    ok = (fi >= 0) & (fi < NX) & (fj >= 0) & (fj < NY)
    free = np.unique(fj[ok] * NX + fi[ok])
    hit = np.isfinite(r)
    ei = np.floor((x + r[hit] * np.cos(a[hit])) / RES).astype(int)
    ej = np.floor((y + r[hit] * np.sin(a[hit])) / RES).astype(int)
    ok = (ei >= 0) & (ei < NX) & (ej >= 0) & (ej < NY)
    occ = np.unique(ej[ok] * NX + ei[ok])
    L.flat[np.setdiff1d(free, occ)] += L_FREE
    L.flat[occ] += L_OCC
    np.clip(L, -L_MAX, L_MAX, out=L)

# 실제 벽 셀에서 2 셀(0.2 m) 안에 있는 셀 = '벽 근처'
cx = (np.arange(NX) + 0.5) * RES
cy = (np.arange(NY) + 0.5) * RES
X, Y = np.meshgrid(cx, cy)
wall = np.zeros((NY, NX), bool)
for x0, y0, x1, y1 in B:
    wall |= (X + RES / 2 > x0) & (X - RES / 2 < x1) & (Y + RES / 2 > y0) & (Y - RES / 2 < y1)
near = np.zeros_like(wall)
for di in range(-2, 3):
    for dj in range(-2, 3):
        near |= np.roll(np.roll(wall, dj, 0), di, 1)

def ghost_ratio(L):
    # TODO 3: 점유(L > L_TH) 셀 중 '벽 근처' 가 아닌 셀의 비율 (%)
    return 0.0

LIMIT = 2.0          # 판정 기준: 이중 벽 셀 2 % 이하면 '쓸 수 있음' (시뮬레이터 5 미션과 같은 기준)
print("route | level | final odom error | ghost wall cells | usable?")
for name, route in (("R1", R1), ("R2", R2)):
    for lev in (0, 1, 2, 3):
        fr = drive(route, lev)
        L = np.zeros((NY, NX))
        for true_p, odom_p, scan in fr:
            integrate(L, odom_p, scan)
        (tx, ty, _), (ox, oy, _), _ = fr[-1]
        g = ghost_ratio(L)
        print(f" {name}   |   {lev}   |     {np.hypot(ox - tx, oy - ty):5.2f} m      |     {g:5.1f} %      | "
              f"{'yes' if g <= LIMIT else 'NO'}")

해 볼 것

  1. 어느 단계부터 "NO" 가 나오나요? 그 단계의 마지막 위치 오차는 몇 cm 인가요? 이중 벽 판정 거리(0.2 m)와 비교해 봅니다.
  2. drive(route, lev, seed=7) 의 seed 를 1 ~ 5 로 바꿔 2단계 결과가 매번 같은지 봅니다. 판정이 흔들린다면 "쓸 수 있는 단계" 를 어떻게 정해야 할까요?
  3. LEVELS[2] 의 세 값 중 하나만 남기고 나머지를 0 으로 만들어, 이중 벽에 가장 큰 영향을 주는 오차가 무엇인지 찾습니다.
  4. R2 의 결과를 시뮬레이터 2 의 "가운데 통로를 두 번 지나가기" 화면과 비교합니다. 이중 벽은 지도의 어디에 생기나요?
풀이
python시뮬 실행 가능
import numpy as np

# 시뮬레이터 2 와 같은 14 m x 8 m 창고 (상자 = x0, y0, x1, y1)
B = np.array([[0, 0, 14, 0.15], [0, 7.85, 14, 8], [0, 0, 0.15, 8], [13.85, 0, 14, 8],
              [2.5, 2.0, 11.5, 2.8], [2.5, 4.4, 11.5, 5.2], [12.9, 0.15, 13.85, 0.9],
              [0.15, 7.0, 0.9, 7.85], [7.0, 6.0, 7.3, 6.3], [5.0, 0.15, 5.3, 0.45]])
R1 = [(1.2, 1.0), (12.4, 1.0), (12.4, 3.6), (1.5, 3.6), (1.5, 6.8), (12.4, 6.8)]
R2 = R1 + [(12.4, 3.6), (1.2, 3.6), (1.2, 1.0)]          # 가운데 통로를 두 번 지나감
# 단계: (거리 슬립 %, 회전 배율 오차 %, 각도 노이즈 deg/sqrt(m))
LEVELS = {0: (0, 0, 0), 1: (0.5, 0.5, 0.1), 2: (1, 1, 0.2), 3: (3, 3, 0.8)}
RES, NX, NY, NR, RMAX = 0.1, 140, 80, 180, 8.0
L_OCC, L_FREE, L_MAX, L_TH = 0.85, -0.4, 4.0, np.log(0.65 / 0.35)

def lidar(x, y, th, rng):
    ang = th + np.linspace(0, 2 * np.pi, NR, endpoint=False)
    c, s = np.cos(ang)[:, None], np.sin(ang)[:, None]
    x0, y0, x1, y1 = B[:, 0] - x, B[:, 1] - y, B[:, 2] - x, B[:, 3] - y
    with np.errstate(divide="ignore", invalid="ignore"):
        tmin = np.maximum(np.minimum(x0 / c, x1 / c), np.minimum(y0 / s, y1 / s))
        tmax = np.minimum(np.maximum(x0 / c, x1 / c), np.maximum(y0 / s, y1 / s))
    t = np.where((tmax >= tmin) & (tmin > 0), tmin, np.inf).min(axis=1)
    t = t + rng.normal(0, 0.015, NR)
    return ang - th, np.where(t <= RMAX, t, np.inf)     # 로봇 기준 각도, 거리

def drive(route, level, seed=7):
    """참 위치와 오도메트리 위치를 함께 적분하고, 0.25 m 마다 스캔을 찍음"""
    es, er, st = LEVELS[level]
    es, er, st = es / 100, er / 100, np.radians(st)
    rng = np.random.default_rng(seed)
    tx, ty = route[0]
    tth = np.arctan2(route[1][1] - ty, route[1][0] - tx)
    ox, oy, oth = tx, ty, tth
    frames = [((tx, ty, tth), (ox, oy, oth), lidar(tx, ty, tth, rng))]
    for gx, gy in route[1:]:
        ga = np.arctan2(gy - ty, gx - tx)
        da = np.arctan2(np.sin(ga - tth), np.cos(ga - tth))
        # TODO 1: 제자리 회전. 참 각도는 ga, 오도메트리 각도는 da 를 (1 + er) 배 만큼 더함
        tth = ga
        oth += da * (1 + er)
        n = int(np.ceil(np.hypot(gx - tx, gy - ty) / 0.05))
        ds = np.hypot(gx - tx, gy - ty) / n
        for k in range(1, n + 1):
            tx, ty = tx + ds * np.cos(ga), ty + ds * np.sin(ga)
            # TODO 2: 오도메트리 직진. 각도에 노이즈 st*sqrt(ds) 를 더하고, 거리는 ds*(1+es) 로 적분
            oth += rng.normal() * st * np.sqrt(ds)
            ox += ds * (1 + es) * np.cos(oth)
            oy += ds * (1 + es) * np.sin(oth)
            if k % 5 == 0 or k == n:
                frames.append(((tx, ty, tth), (ox, oy, oth), lidar(tx, ty, tth, rng)))
    return frames

def integrate(L, pose, scan):
    x, y, th = pose
    ang, r = scan
    a = th + ang
    k = np.arange(0, RMAX, RES / 2)[None, :]
    rr = np.where(np.isfinite(r), r, RMAX)[:, None]
    xs, ys = x + np.cos(a)[:, None] * k, y + np.sin(a)[:, None] * k
    m = k < rr - RES / 2
    fi, fj = np.floor(xs[m] / RES).astype(int), np.floor(ys[m] / RES).astype(int)
    ok = (fi >= 0) & (fi < NX) & (fj >= 0) & (fj < NY)
    free = np.unique(fj[ok] * NX + fi[ok])
    hit = np.isfinite(r)
    ei = np.floor((x + r[hit] * np.cos(a[hit])) / RES).astype(int)
    ej = np.floor((y + r[hit] * np.sin(a[hit])) / RES).astype(int)
    ok = (ei >= 0) & (ei < NX) & (ej >= 0) & (ej < NY)
    occ = np.unique(ej[ok] * NX + ei[ok])
    L.flat[np.setdiff1d(free, occ)] += L_FREE
    L.flat[occ] += L_OCC
    np.clip(L, -L_MAX, L_MAX, out=L)

# 실제 벽 셀에서 2 셀(0.2 m) 안에 있는 셀 = '벽 근처'
cx = (np.arange(NX) + 0.5) * RES
cy = (np.arange(NY) + 0.5) * RES
X, Y = np.meshgrid(cx, cy)
wall = np.zeros((NY, NX), bool)
for x0, y0, x1, y1 in B:
    wall |= (X + RES / 2 > x0) & (X - RES / 2 < x1) & (Y + RES / 2 > y0) & (Y - RES / 2 < y1)
near = np.zeros_like(wall)
for di in range(-2, 3):
    for dj in range(-2, 3):
        near |= np.roll(np.roll(wall, dj, 0), di, 1)

def ghost_ratio(L):
    # TODO 3: 점유(L > L_TH) 셀 중 '벽 근처' 가 아닌 셀의 비율 (%)
    occ = L > L_TH
    return 100 * (occ & ~near).sum() / max(occ.sum(), 1)

LIMIT = 2.0          # 판정 기준: 이중 벽 셀 2 % 이하면 '쓸 수 있음' (시뮬레이터 5 미션과 같은 기준)
print("route | level | final odom error | ghost wall cells | usable?")
for name, route in (("R1", R1), ("R2", R2)):
    for lev in (0, 1, 2, 3):
        fr = drive(route, lev)
        L = np.zeros((NY, NX))
        for true_p, odom_p, scan in fr:
            integrate(L, odom_p, scan)
        (tx, ty, _), (ox, oy, _), _ = fr[-1]
        g = ghost_ratio(L)
        print(f" {name}   |   {lev}   |     {np.hypot(ox - tx, oy - ty):5.2f} m      |     {g:5.1f} %      | "
              f"{'yes' if g <= LIMIT else 'NO'}")
  • seed = 7 에서 1단계는 마지막 오차 약 0.18 m, 이중 벽 0 % 로 쓸 수 있고, 2단계부터 이중 벽 약 8 ~ 10 % 로 쓸 수 없습니다. 3단계는 오차 1 m 이상, 이중 벽 30 % 안팎입니다.
  • 1단계도 오차가 0 은 아닙니다. 판정 기준(0.2 m)보다 작아서 "벽 근처" 로 인정될 뿐이므로, 더 긴 경로에서는 결국 기준을 넘습니다. 그래서 오도메트리만으로는 넓은 창고의 지도를 만들 수 없고 스캔으로 위치를 고치는 SLAM 이 필요합니다.
  • 2단계는 seed 에 따라 결과가 흔들립니다(예: seed 3 은 이중 벽 약 1 % 로 통과). 한 번의 실행으로 판정하지 말고 여러 seed 에서 "대부분 통과" 하는 단계를 기준으로 삼아야 합니다.
  • 2단계의 세 값을 하나씩만 남겨 보면, 거리 슬립 1 % 만은 이중 벽 0 %, 각도 노이즈만도 0 % 인데 회전 배율 오차 1 % 만으로 약 17 % 가 나옵니다. 모퉁이마다 90° 를 돌 때 0.9° 씩 틀어지고, 각도 오차는 그 뒤 달린 거리만큼 위치 오차로 커지기 때문입니다(ch04). R2 의 이중 벽은 두 번째로 지나간 가운데 통로의 선반 면에 몰립니다.
실습 3-1 · numpy 로 2D ICP 한 반복 구현 (3교시)

계획의 Playground 활동 "최근접 점 → 회전 · 이동 추정" 입니다. 기준 스캔(원점)과 새 스캔(참 위치 0.30, 0.10, 8°)을 만들고, 오도메트리가 말한 위치(0.25, 0.0, 3°)에서 출발해 ICP 로 위치를 고칩니다.

  • TODO 1 nearest() — (N, 1, 2) − (1, M, 2) 브로드캐스팅으로 모든 쌍의 거리를 구하고 argmin 합니다.
  • TODO 2 best_fit() — 무게중심을 뺀 뒤 H = Pᵀ Q, U, S, Vᵀ = svd(H), R = V Uᵀ, t = μq − R μp. det(R) < 0 이면 반사이므로 바로잡습니다.

예측 먼저 — 오도메트리 추정은 참 위치에서 약 11 cm · 5° 틀렸습니다. ICP 1회만에 얼마나 맞을까요?

python시뮬 실행 가능
import numpy as np

def room_scan(x, y, th, n=240, sigma=0.01, seed=0):
    """L 자 벽 + 상자가 있는 방을 (x, y, th) 에서 본 점군 (로봇 좌표계, N x 2)"""
    B = np.array([[-1, -1, 4, -0.9], [-1, -1, -0.9, 3], [1.5, 1.0, 2.0, 1.6], [3.0, 2.0, 3.3, 2.3]])
    rng = np.random.default_rng(seed)
    ang = th + np.linspace(-np.pi, np.pi, n, endpoint=False)
    c, s = np.cos(ang)[:, None], np.sin(ang)[:, None]
    x0, y0, x1, y1 = B[:, 0] - x, B[:, 1] - y, B[:, 2] - x, B[:, 3] - y
    with np.errstate(divide="ignore", invalid="ignore"):
        tmin = np.maximum(np.minimum(x0 / c, x1 / c), np.minimum(y0 / s, y1 / s))
        tmax = np.minimum(np.maximum(x0 / c, x1 / c), np.maximum(y0 / s, y1 / s))
    t = np.where((tmax >= tmin) & (tmin > 0), tmin, np.inf).min(axis=1)
    ok = t < 6.0
    t = t[ok] + rng.normal(0, sigma, ok.sum())
    a = ang[ok] - th
    return np.c_[t * np.cos(a), t * np.sin(a)]

def rot(th):
    return np.array([[np.cos(th), -np.sin(th)], [np.sin(th), np.cos(th)]])

def nearest(src, dst):
    """src 의 각 점에서 가장 가까운 dst 점의 인덱스와 거리"""
    # TODO 1: (N, 1, 2) - (1, M, 2) 로 모든 쌍의 거리를 구하고 argmin
    idx = np.zeros(len(src), dtype=int)
    return idx, np.linalg.norm(src - dst[idx], axis=1)

def best_fit(p, q):
    """짝지은 점 p[k] -> q[k] 를 가장 잘 겹치는 회전 R, 이동 t (q ~ R p + t)"""
    mp, mq = p.mean(axis=0), q.mean(axis=0)
    # TODO 2: 무게중심을 뺀 뒤 H = P^T Q, SVD 로 R = V U^T, t = mq - R mp
    R = np.eye(2)
    t = mq - mp
    return R, t

def icp(src, dst, iters=20, reject=0.5, verbose=True):
    """src 를 dst 에 맞추는 누적 R, t. reject 보다 먼 짝은 버림"""
    R_acc, t_acc, cur = np.eye(2), np.zeros(2), src.copy()
    for k in range(iters):
        idx, d = nearest(cur, dst)
        keep = d < reject
        if keep.sum() < 3:
            break
        R, t = best_fit(cur[keep], dst[idx[keep]])
        cur = cur @ R.T + t
        R_acc, t_acc = R @ R_acc, R @ t_acc + t
        if verbose:
            print(f"iter {k + 1:2d}: pairs {keep.sum():3d}  rms {np.sqrt(np.mean(d[keep] ** 2)) * 100:6.2f} cm  "
                  f"pose x {t_acc[0]:+.3f} y {t_acc[1]:+.3f} th {np.degrees(np.arctan2(R_acc[1, 0], R_acc[0, 0])):+6.2f} deg")
    return R_acc, t_acc

# 기준 스캔: (0, 0, 0) 에서. 새 스캔: 로봇이 (0.30, 0.10) 로 가고 8 도 돌았을 때
ref = room_scan(0, 0, 0, seed=1)
true = (0.30, 0.10, np.radians(8))
new = room_scan(*true, seed=2)
# 새 스캔을 '오도메트리가 말한 위치' 로 옮겨 놓고 시작 (오도메트리는 0.25, 0.0, 3 도 라고 함)
guess = (0.25, 0.0, np.radians(3))
src = new @ rot(guess[2]).T + guess[:2]
print("true pose :", true[0], true[1], round(np.degrees(true[2]), 1))
print("odom guess:", guess[0], guess[1], round(np.degrees(guess[2]), 1))
R, t = icp(src, ref, iters=10)
# 최종 위치 = ICP 보정 (R, t) 를 오도메트리 추정 위에 합성
th = np.arctan2(R[1, 0], R[0, 0]) + guess[2]
xy = R @ np.array(guess[:2]) + t
print(f"ICP pose  : {xy[0]:.3f} {xy[1]:.3f} {np.degrees(th):.2f}   "
      f"error {np.hypot(*(xy - true[:2])) * 100:.1f} cm, {np.degrees(th - true[2]):+.2f} deg")

해 볼 것

  1. 반복마다 rms 가 줄어드는 폭을 봅니다. 처음 몇 번에 크게 줄고 나중에는 거의 그대로입니다. 시뮬레이터 3 의 오차 그래프와 모양이 같은가요?
  2. rms 가 0 이 되지 않고 약 3 cm 에서 멈추는 이유는? room_scan 의 n 을 240 → 720 으로 늘려 봅니다. (힌트: 두 스캔은 같은 벽의 다른 점 을 찍었습니다)
  3. guess 의 각도를 3° → −20° 로 바꿔 봅니다. 최종 오차는 어떻게 되나요?
  4. best_fit 에서 반사 보정(det(R) < 0) 줄을 지우고 점을 일직선(p[:, 1] = 0)으로 만든 경우를 상상해 봅니다. 왜 이 보정이 필요할까요?
풀이
python시뮬 실행 가능
import numpy as np

def room_scan(x, y, th, n=240, sigma=0.01, seed=0):
    """L 자 벽 + 상자가 있는 방을 (x, y, th) 에서 본 점군 (로봇 좌표계, N x 2)"""
    B = np.array([[-1, -1, 4, -0.9], [-1, -1, -0.9, 3], [1.5, 1.0, 2.0, 1.6], [3.0, 2.0, 3.3, 2.3]])
    rng = np.random.default_rng(seed)
    ang = th + np.linspace(-np.pi, np.pi, n, endpoint=False)
    c, s = np.cos(ang)[:, None], np.sin(ang)[:, None]
    x0, y0, x1, y1 = B[:, 0] - x, B[:, 1] - y, B[:, 2] - x, B[:, 3] - y
    with np.errstate(divide="ignore", invalid="ignore"):
        tmin = np.maximum(np.minimum(x0 / c, x1 / c), np.minimum(y0 / s, y1 / s))
        tmax = np.minimum(np.maximum(x0 / c, x1 / c), np.maximum(y0 / s, y1 / s))
    t = np.where((tmax >= tmin) & (tmin > 0), tmin, np.inf).min(axis=1)
    ok = t < 6.0
    t = t[ok] + rng.normal(0, sigma, ok.sum())
    a = ang[ok] - th
    return np.c_[t * np.cos(a), t * np.sin(a)]

def rot(th):
    return np.array([[np.cos(th), -np.sin(th)], [np.sin(th), np.cos(th)]])

def nearest(src, dst):
    """src 의 각 점에서 가장 가까운 dst 점의 인덱스와 거리"""
    # TODO 1: (N, 1, 2) - (1, M, 2) 로 모든 쌍의 거리를 구하고 argmin
    d = np.linalg.norm(src[:, None, :] - dst[None, :, :], axis=2)
    idx = d.argmin(axis=1)
    return idx, d[np.arange(len(src)), idx]

def best_fit(p, q):
    """짝지은 점 p[k] -> q[k] 를 가장 잘 겹치는 회전 R, 이동 t (q ~ R p + t)"""
    mp, mq = p.mean(axis=0), q.mean(axis=0)
    # TODO 2: 무게중심을 뺀 뒤 H = P^T Q, SVD 로 R = V U^T, t = mq - R mp
    H = (p - mp).T @ (q - mq)
    U, S, Vt = np.linalg.svd(H)
    R = Vt.T @ U.T
    if np.linalg.det(R) < 0:              # 거울상(반사)이 나오면 바로잡기
        Vt[1] *= -1
        R = Vt.T @ U.T
    t = mq - R @ mp
    return R, t

def icp(src, dst, iters=20, reject=0.5, verbose=True):
    """src 를 dst 에 맞추는 누적 R, t. reject 보다 먼 짝은 버림"""
    R_acc, t_acc, cur = np.eye(2), np.zeros(2), src.copy()
    for k in range(iters):
        idx, d = nearest(cur, dst)
        keep = d < reject
        if keep.sum() < 3:
            break
        R, t = best_fit(cur[keep], dst[idx[keep]])
        cur = cur @ R.T + t
        R_acc, t_acc = R @ R_acc, R @ t_acc + t
        if verbose:
            print(f"iter {k + 1:2d}: pairs {keep.sum():3d}  rms {np.sqrt(np.mean(d[keep] ** 2)) * 100:6.2f} cm  "
                  f"pose x {t_acc[0]:+.3f} y {t_acc[1]:+.3f} th {np.degrees(np.arctan2(R_acc[1, 0], R_acc[0, 0])):+6.2f} deg")
    return R_acc, t_acc

# 기준 스캔: (0, 0, 0) 에서. 새 스캔: 로봇이 (0.30, 0.10) 로 가고 8 도 돌았을 때
ref = room_scan(0, 0, 0, seed=1)
true = (0.30, 0.10, np.radians(8))
new = room_scan(*true, seed=2)
# 새 스캔을 '오도메트리가 말한 위치' 로 옮겨 놓고 시작 (오도메트리는 0.25, 0.0, 3 도 라고 함)
guess = (0.25, 0.0, np.radians(3))
src = new @ rot(guess[2]).T + guess[:2]
print("true pose :", true[0], true[1], round(np.degrees(true[2]), 1))
print("odom guess:", guess[0], guess[1], round(np.degrees(guess[2]), 1))
R, t = icp(src, ref, iters=10)
# 최종 위치 = ICP 보정 (R, t) 를 오도메트리 추정 위에 합성
th = np.arctan2(R[1, 0], R[0, 0]) + guess[2]
xy = R @ np.array(guess[:2]) + t
print(f"ICP pose  : {xy[0]:.3f} {xy[1]:.3f} {np.degrees(th):.2f}   "
      f"error {np.hypot(*(xy - true[:2])) * 100:.1f} cm, {np.degrees(th - true[2]):+.2f} deg")
  • 1회 반복 뒤 rms 13 cm → 10회 뒤 3 cm, 최종 위치 오차 약 0.4 cm · 0.1° 입니다.
  • 남는 rms 는 위치가 틀려서가 아니라, 새 스캔의 점이 기준 스캔의 점과 점 사이 에 떨어지기 때문입니다(점 간격 = 거리 × 각도 간격). 그래서 실제 SLAM 은 점-대-점 대신 점-대-선(point-to-line) 거리나 격자 지도와의 매칭(시뮬레이터 5)을 씁니다.
  • 반사 보정이 없으면 점들이 거의 일직선일 때 SVD 가 "뒤집힌" 회전(det = −1)을 내놓을 수 있습니다. 로봇은 뒤집히지 않으므로 R 은 항상 det = +1 이어야 합니다.
실습 3-2 · ICP 수렴 / 발산 경계 찾기 (3교시)

계획의 실습 "ICP 반복 횟수와 초기 오차를 바꾸며 수렴/발산 경계 찾기" 입니다. 실습 3-1 의 ICP 를 그대로 쓰고, 초기 위치 오차(0.1 ~ 1.0 m) × 초기 각도 오차(0 ~ 90°) 표를 반복 횟수 · 거부 거리(reject)별로 만듭니다.

  • TODO 1 trial() — 최종 위치 오차(m)와 각도 오차(°, −180 ~ 180 으로 감기)를 돌려줍니다.
  • TODO 2 converged() — 위치 3 cm 이내, 각도 1° 이내면 수렴.
  • TODO 3 표의 칸마다 trial(off, off / 2, a, iters, reject) 를 돌려 수렴이면 O, 아니면 ..

예측 먼저 — 반복을 5 → 30 회로 늘리면 표의 O 는 어느 쪽(오른쪽 = 큰 각도, 아래 = 큰 위치)으로 넓어질까요? 반복을 무한히 늘리면 90° 도 맞출 수 있을까요?

python시뮬 실행 가능
import numpy as np

def room_scan(x, y, th, n=240, sigma=0.01, seed=0):
    """L 자 벽 + 상자가 있는 방을 (x, y, th) 에서 본 점군 (로봇 좌표계, N x 2)"""
    B = np.array([[-1, -1, 4, -0.9], [-1, -1, -0.9, 3], [1.5, 1.0, 2.0, 1.6], [3.0, 2.0, 3.3, 2.3]])
    rng = np.random.default_rng(seed)
    ang = th + np.linspace(-np.pi, np.pi, n, endpoint=False)
    c, s = np.cos(ang)[:, None], np.sin(ang)[:, None]
    x0, y0, x1, y1 = B[:, 0] - x, B[:, 1] - y, B[:, 2] - x, B[:, 3] - y
    with np.errstate(divide="ignore", invalid="ignore"):
        tmin = np.maximum(np.minimum(x0 / c, x1 / c), np.minimum(y0 / s, y1 / s))
        tmax = np.minimum(np.maximum(x0 / c, x1 / c), np.maximum(y0 / s, y1 / s))
    t = np.where((tmax >= tmin) & (tmin > 0), tmin, np.inf).min(axis=1)
    ok = t < 6.0
    t = t[ok] + rng.normal(0, sigma, ok.sum())
    a = ang[ok] - th
    return np.c_[t * np.cos(a), t * np.sin(a)]

def rot(th):
    return np.array([[np.cos(th), -np.sin(th)], [np.sin(th), np.cos(th)]])

def nearest(src, dst):
    """src 의 각 점에서 가장 가까운 dst 점의 인덱스와 거리"""
    # TODO 1: (N, 1, 2) - (1, M, 2) 로 모든 쌍의 거리를 구하고 argmin
    d = np.linalg.norm(src[:, None, :] - dst[None, :, :], axis=2)
    idx = d.argmin(axis=1)
    return idx, d[np.arange(len(src)), idx]

def best_fit(p, q):
    """짝지은 점 p[k] -> q[k] 를 가장 잘 겹치는 회전 R, 이동 t (q ~ R p + t)"""
    mp, mq = p.mean(axis=0), q.mean(axis=0)
    # TODO 2: 무게중심을 뺀 뒤 H = P^T Q, SVD 로 R = V U^T, t = mq - R mp
    H = (p - mp).T @ (q - mq)
    U, S, Vt = np.linalg.svd(H)
    R = Vt.T @ U.T
    if np.linalg.det(R) < 0:              # 거울상(반사)이 나오면 바로잡기
        Vt[1] *= -1
        R = Vt.T @ U.T
    t = mq - R @ mp
    return R, t

def icp(src, dst, iters=20, reject=0.5, verbose=True):
    """src 를 dst 에 맞추는 누적 R, t. reject 보다 먼 짝은 버림"""
    R_acc, t_acc, cur = np.eye(2), np.zeros(2), src.copy()
    for k in range(iters):
        idx, d = nearest(cur, dst)
        keep = d < reject
        if keep.sum() < 3:
            break
        R, t = best_fit(cur[keep], dst[idx[keep]])
        cur = cur @ R.T + t
        R_acc, t_acc = R @ R_acc, R @ t_acc + t
        if verbose:
            print(f"iter {k + 1:2d}: pairs {keep.sum():3d}  rms {np.sqrt(np.mean(d[keep] ** 2)) * 100:6.2f} cm  "
                  f"pose x {t_acc[0]:+.3f} y {t_acc[1]:+.3f} th {np.degrees(np.arctan2(R_acc[1, 0], R_acc[0, 0])):+6.2f} deg")
    return R_acc, t_acc

def trial(dx, dy, dth_deg, iters, reject=0.5):
    """오도메트리 추정이 참 위치에서 (dx, dy, dth) 만큼 틀렸을 때 ICP 가 바로잡는가?"""
    true = np.array([0.30, 0.10, np.radians(8)])
    guess = true + np.array([dx, dy, np.radians(dth_deg)])
    src = NEW @ rot(guess[2]).T + guess[:2]
    R, t = icp(src, REF, iters=iters, reject=reject, verbose=False)
    th = np.arctan2(R[1, 0], R[0, 0]) + guess[2]
    xy = R @ guess[:2] + t
    # TODO 1: 위치 오차(m)와 각도 오차(deg, -180 ~ 180 으로 감기)를 돌려줌
    return 0.0, 0.0

def converged(e_xy, e_th):
    # TODO 2: 수렴 기준 - 위치 3 cm 이내, 각도 1 도 이내
    return False

REF = room_scan(0, 0, 0, seed=1)
NEW = room_scan(0.30, 0.10, np.radians(8), seed=2)

ANGLES = range(0, 91, 10)
for iters in (5, 30):
    for reject in (0.5, 2.0):
        print(f"== iters {iters}, reject {reject} m   (O = converged, . = failed)")
        print("  offset | dtheta  " + " ".join(f"{a:3d}" for a in ANGLES))
        for off in (0.1, 0.3, 0.6, 1.0):
            row = []
            for a in ANGLES:
                # TODO 3: 위치 오차를 (off, off/2) 로 주고 각도 오차 a 도로 trial, 수렴하면 'O'
                row.append("  ?")
            print(f"  {off:4.1f} m          " + " ".join(row))

해 볼 것

  1. 반복 5 회 표에서 O 가 거의 없는 이유는? 수렴 기준을 3 cm → 10 cm 로 느슨하게 하면 어떻게 바뀌나요?
  2. 반복 30 회에서 각도 몇 도부터 실패하나요? 이 경계는 위치 오차가 커지면 어떻게 움직이나요?
  3. reject 0.5 m 와 2.0 m 의 표를 비교합니다. 거부 거리를 키우면 왜 큰 초기 오차에서 유리할까요? 반대로 어떤 위험이 있을까요? (힌트: 사람 · 지게차처럼 지도에 없는 물체의 점)
  4. ANGLES 를 range(40, 71, 5) 로 촘촘하게 바꿔 경계를 5° 단위로 찾습니다.
풀이
python시뮬 실행 가능
import numpy as np

def room_scan(x, y, th, n=240, sigma=0.01, seed=0):
    """L 자 벽 + 상자가 있는 방을 (x, y, th) 에서 본 점군 (로봇 좌표계, N x 2)"""
    B = np.array([[-1, -1, 4, -0.9], [-1, -1, -0.9, 3], [1.5, 1.0, 2.0, 1.6], [3.0, 2.0, 3.3, 2.3]])
    rng = np.random.default_rng(seed)
    ang = th + np.linspace(-np.pi, np.pi, n, endpoint=False)
    c, s = np.cos(ang)[:, None], np.sin(ang)[:, None]
    x0, y0, x1, y1 = B[:, 0] - x, B[:, 1] - y, B[:, 2] - x, B[:, 3] - y
    with np.errstate(divide="ignore", invalid="ignore"):
        tmin = np.maximum(np.minimum(x0 / c, x1 / c), np.minimum(y0 / s, y1 / s))
        tmax = np.minimum(np.maximum(x0 / c, x1 / c), np.maximum(y0 / s, y1 / s))
    t = np.where((tmax >= tmin) & (tmin > 0), tmin, np.inf).min(axis=1)
    ok = t < 6.0
    t = t[ok] + rng.normal(0, sigma, ok.sum())
    a = ang[ok] - th
    return np.c_[t * np.cos(a), t * np.sin(a)]

def rot(th):
    return np.array([[np.cos(th), -np.sin(th)], [np.sin(th), np.cos(th)]])

def nearest(src, dst):
    """src 의 각 점에서 가장 가까운 dst 점의 인덱스와 거리"""
    # TODO 1: (N, 1, 2) - (1, M, 2) 로 모든 쌍의 거리를 구하고 argmin
    d = np.linalg.norm(src[:, None, :] - dst[None, :, :], axis=2)
    idx = d.argmin(axis=1)
    return idx, d[np.arange(len(src)), idx]

def best_fit(p, q):
    """짝지은 점 p[k] -> q[k] 를 가장 잘 겹치는 회전 R, 이동 t (q ~ R p + t)"""
    mp, mq = p.mean(axis=0), q.mean(axis=0)
    # TODO 2: 무게중심을 뺀 뒤 H = P^T Q, SVD 로 R = V U^T, t = mq - R mp
    H = (p - mp).T @ (q - mq)
    U, S, Vt = np.linalg.svd(H)
    R = Vt.T @ U.T
    if np.linalg.det(R) < 0:              # 거울상(반사)이 나오면 바로잡기
        Vt[1] *= -1
        R = Vt.T @ U.T
    t = mq - R @ mp
    return R, t

def icp(src, dst, iters=20, reject=0.5, verbose=True):
    """src 를 dst 에 맞추는 누적 R, t. reject 보다 먼 짝은 버림"""
    R_acc, t_acc, cur = np.eye(2), np.zeros(2), src.copy()
    for k in range(iters):
        idx, d = nearest(cur, dst)
        keep = d < reject
        if keep.sum() < 3:
            break
        R, t = best_fit(cur[keep], dst[idx[keep]])
        cur = cur @ R.T + t
        R_acc, t_acc = R @ R_acc, R @ t_acc + t
        if verbose:
            print(f"iter {k + 1:2d}: pairs {keep.sum():3d}  rms {np.sqrt(np.mean(d[keep] ** 2)) * 100:6.2f} cm  "
                  f"pose x {t_acc[0]:+.3f} y {t_acc[1]:+.3f} th {np.degrees(np.arctan2(R_acc[1, 0], R_acc[0, 0])):+6.2f} deg")
    return R_acc, t_acc

def trial(dx, dy, dth_deg, iters, reject=0.5):
    """오도메트리 추정이 참 위치에서 (dx, dy, dth) 만큼 틀렸을 때 ICP 가 바로잡는가?"""
    true = np.array([0.30, 0.10, np.radians(8)])
    guess = true + np.array([dx, dy, np.radians(dth_deg)])
    src = NEW @ rot(guess[2]).T + guess[:2]
    R, t = icp(src, REF, iters=iters, reject=reject, verbose=False)
    th = np.arctan2(R[1, 0], R[0, 0]) + guess[2]
    xy = R @ guess[:2] + t
    # TODO 1: 위치 오차(m)와 각도 오차(deg, -180 ~ 180 으로 감기)를 돌려줌
    e_xy = np.hypot(*(xy - true[:2]))
    e_th = np.degrees(np.arctan2(np.sin(th - true[2]), np.cos(th - true[2])))
    return e_xy, e_th

def converged(e_xy, e_th):
    # TODO 2: 수렴 기준 - 위치 3 cm 이내, 각도 1 도 이내
    return e_xy < 0.03 and abs(e_th) < 1.0

REF = room_scan(0, 0, 0, seed=1)
NEW = room_scan(0.30, 0.10, np.radians(8), seed=2)

ANGLES = range(0, 91, 10)
for iters in (5, 30):
    for reject in (0.5, 2.0):
        print(f"== iters {iters}, reject {reject} m   (O = converged, . = failed)")
        print("  offset | dtheta  " + " ".join(f"{a:3d}" for a in ANGLES))
        for off in (0.1, 0.3, 0.6, 1.0):
            row = []
            for a in ANGLES:
                # TODO 3: 위치 오차를 (off, off/2) 로 주고 각도 오차 a 도로 trial, 수렴하면 'O'
                row.append("  O" if converged(*trial(off, off / 2, a, iters, reject)) else "  .")
            print(f"  {off:4.1f} m          " + " ".join(row))
  • 반복 5 회는 각도 오차가 0 일 때만 수렴합니다. 한 반복이 오차를 "일부" 만 줄이므로 횟수가 모자라면 기준(3 cm · 1°)에 닿지 못합니다.
  • 반복 30 회 · reject 0.5 m 에서는 각도 약 40° 까지 수렴하고 50° 부터 실패합니다. reject 2.0 m 에서는 경계가 50 ~ 60° 로 조금 넓어집니다. 그 이상은 반복을 늘려도 안 됩니다. 최근접 점 짝이 처음부터 다른 벽 과 이어져 엉뚱한 자세(지역 최솟값)에 갇히기 때문입니다.
  • 그래서 실제 SLAM 은 ① 오도메트리로 초기 추정을 충분히 가깝게 주고(천천히 주행, 4교시), ② 여러 후보 자세를 먼저 훑는 탐색(시뮬레이터 5 의 격자 탐색 · Cartographer 의 분기 한정 탐색)으로 출발점을 잡습니다. reject 를 키우면 넓게 끌어오지만, 지도에 없는 물체의 점까지 짝지어 결과를 비틀 수 있습니다.
실습 4-1 · 커버리지 95 % 를 가장 짧은 경로로 (4교시)

계획의 실습 "가장 짧은 주행 거리로 커버리지 95 % 이상, 이중 벽 없는 지도" 의 경로 설계 부분입니다. 시뮬레이터 5 와 같은 창고에서 경유점 목록(경로)마다 주행 거리와 커버리지를 계산합니다. 이중 벽을 없애는 부분은 시뮬레이터 5 의 스캔 매칭 · 루프 폐합으로 확인하므로, 여기서는 참 위치 로 그려 경로 자체의 커버리지만 봅니다.

  • TODO 1 path_length() — 이웃한 경유점 사이 거리의 합.
  • TODO 2 coverage() — 갈 수 있는 셀 중 "빈 공간으로 확정"(L < T_FREE, p < 0.25) 된 셀의 비율(%).
  • TODO 3 경로 C — 커버리지 95 % 이상이면서 예시 경로 A 보다 훨씬 짧고, 출발점으로 돌아오는(루프) 경로를 설계합니다. 경유점은 상자와 겹치지 않게 둡니다.

예측 먼저 — 선반 사이 통로를 한 번씩만 지나는 B(27 m)는 95 % 를 넘을까요? 넘지 못한다면 어디가 비었을까요?

python시뮬 실행 가능
import numpy as np

# 시뮬레이터 5 와 같은 14 m x 8 m 창고 (상자 = x0, y0, x1, y1), 출발점 (1.4, 1.1)
B = np.array([[0, 0, 14, 0.15], [0, 7.85, 14, 8], [0, 0, 0.15, 8], [13.85, 0, 14, 8],
              [3.0, 6.5, 10.5, 6.65], [2.5, 1.9, 9.5, 2.6], [2.5, 4.0, 9.5, 4.7],
              [12.6, 0.15, 13.85, 1.2], [11.5, 3.2, 11.8, 3.5], [0.15, 4.9, 0.8, 5.6]])
START = (1.4, 1.1)
RES, NX, NY, NR = 0.1, 140, 80, 180
L_OCC, L_FREE, L_MAX = 0.9, -0.7, 4.0
T_OCC, T_FREE = np.log(0.65 / 0.35), np.log(0.25 / 0.75)

def lidar(x, y, rmax):
    ang = np.linspace(0, 2 * np.pi, NR, endpoint=False)
    c, s = np.cos(ang)[:, None], np.sin(ang)[:, None]
    x0, y0, x1, y1 = B[:, 0] - x, B[:, 1] - y, B[:, 2] - x, B[:, 3] - y
    with np.errstate(divide="ignore", invalid="ignore"):
        tmin = np.maximum(np.minimum(x0 / c, x1 / c), np.minimum(y0 / s, y1 / s))
        tmax = np.minimum(np.maximum(x0 / c, x1 / c), np.maximum(y0 / s, y1 / s))
    t = np.where((tmax >= tmin) & (tmin > 0), tmin, np.inf).min(axis=1)
    return ang, np.where(t <= rmax, t, np.inf)

def integrate(L, x, y, rmax):
    ang, r = lidar(x, y, rmax)
    k = np.arange(0, rmax, RES / 2)[None, :]
    rr = np.where(np.isfinite(r), r, rmax)[:, None]
    xs, ys = x + np.cos(ang)[:, None] * k, y + np.sin(ang)[:, None] * k
    m = k < rr - RES / 2
    fi, fj = np.floor(xs[m] / RES).astype(int), np.floor(ys[m] / RES).astype(int)
    ok = (fi >= 0) & (fi < NX) & (fj >= 0) & (fj < NY)
    free = np.unique(fj[ok] * NX + fi[ok])
    hit = np.isfinite(r)
    ei = np.floor((x + r[hit] * np.cos(ang[hit])) / RES).astype(int)
    ej = np.floor((y + r[hit] * np.sin(ang[hit])) / RES).astype(int)
    ok = (ei >= 0) & (ei < NX) & (ej >= 0) & (ej < NY)
    occ = np.unique(ej[ok] * NX + ei[ok])
    L.flat[np.setdiff1d(free, occ)] += L_FREE
    L.flat[occ] += L_OCC
    np.clip(L, -L_MAX, L_MAX, out=L)

# 로봇이 갈 수 있는 빈 셀 = 출발점에서 상하좌우로 이어진, 상자와 겹치지 않는 셀
cx, cy = np.meshgrid((np.arange(NX) + 0.5) * RES, (np.arange(NY) + 0.5) * RES)
solid = np.zeros((NY, NX), bool)
for x0, y0, x1, y1 in B:
    solid |= (cx + RES / 2 > x0) & (cx - RES / 2 < x1) & (cy + RES / 2 > y0) & (cy - RES / 2 < y1)
reach = np.zeros_like(solid)
stack = [(int(START[1] / RES), int(START[0] / RES))]
while stack:
    j, i = stack.pop()
    if 0 <= j < NY and 0 <= i < NX and not reach[j, i] and not solid[j, i]:
        reach[j, i] = True
        stack += [(j + 1, i), (j - 1, i), (j, i + 1), (j, i - 1)]

def path_length(route):
    # TODO 1: 이웃한 경유점 사이 거리의 합
    return 0.0

def coverage(L):
    # TODO 2: 갈 수 있는 셀(reach) 중 '빈 공간으로 확정'(L < T_FREE) 된 셀의 비율 (%)
    return 0.0

def run(route, rmax=8.0, step=0.3):
    """경로를 따라 step 마다 스캔. 커버리지가 95 % 를 처음 넘은 지점의 주행 거리도 기록"""
    L, dist, hit95 = np.zeros((NY, NX)), 0.0, None
    integrate(L, *route[0], rmax)
    for (x0, y0), (x1, y1) in zip(route[:-1], route[1:]):
        seg = np.hypot(x1 - x0, y1 - y0)
        n = max(1, int(np.ceil(seg / step)))
        for k in range(1, n + 1):
            integrate(L, x0 + (x1 - x0) * k / n, y0 + (y1 - y0) * k / n, rmax)
            dist += seg / n
            if hit95 is None and coverage(L) >= 95:
                hit95 = dist
    return coverage(L), hit95

ROUTES = {
    # 시뮬레이터 5 의 '예시 경로'
    "A example": [(1.4, 1.1), (11.0, 1.1), (11.0, 3.3), (1.6, 3.3), (1.6, 5.6), (12.5, 5.6),
                  (12.5, 7.25), (1.6, 7.25), (1.6, 5.6), (1.4, 1.1)],
    # 선반 사이 통로를 한 번씩만 지나고 출발점으로 돌아오는 고리
    "B one loop": [(1.4, 1.1), (10.5, 1.1), (10.5, 5.6), (1.6, 5.6), (1.4, 1.1)],
    # TODO 3: 커버리지 95 % 이상이면서 B 보다 짧거나, 같은 길이로 더 높은 커버리지를 내는 경로
    "C mine": [(1.4, 1.1), (10.5, 1.1), (10.5, 5.6), (1.6, 5.6), (1.4, 1.1)],
}

print(f"reachable free cells: {int(reach.sum())}")
for rmax in (8.0, 3.5):
    print(f"== lidar max range {rmax} m")
    for name, route in ROUTES.items():
        cov, d95 = run(route, rmax)
        closed = np.hypot(route[-1][0] - route[0][0], route[-1][1] - route[0][1]) < 0.5
        print(f"  {name:11s} length {path_length(route):5.1f} m  coverage {cov:5.1f} %  "
              f"95% reached at {('%.1f m' % d95) if d95 else '  -   '}  loop {'yes' if closed else 'no'}")

해 볼 것

  1. B 가 95 % 에 못 미치는 이유를 창고 배치(B 의 위쪽 벽 [3.0, 6.5, 10.5, 6.65])에서 찾아봅니다. 어느 구역이 어느 위치에서도 보이지 않나요?
  2. 경로 C 를 설계해 8 m 라이다에서 95 % 이상을 만듭니다. 몇 m 까지 줄일 수 있나요? 결과를 시뮬레이터 5 에서 직접 몰아 확인합니다.
  3. 라이다 최대 거리 3.5 m 에서는 같은 경로 C 의 커버리지가 어떻게 되나요? 최대 거리가 짧을 때 경로 설계는 어떻게 달라져야 할까요?
  4. "95% reached at" 은 경로 끝보다 앞에서 95 % 를 넘은 지점입니다. 그 뒤 출발점으로 돌아오는 구간은 낭비일까요? (3교시 루프 폐합을 생각해 보세요)
풀이
python시뮬 실행 가능
import numpy as np

# 시뮬레이터 5 와 같은 14 m x 8 m 창고 (상자 = x0, y0, x1, y1), 출발점 (1.4, 1.1)
B = np.array([[0, 0, 14, 0.15], [0, 7.85, 14, 8], [0, 0, 0.15, 8], [13.85, 0, 14, 8],
              [3.0, 6.5, 10.5, 6.65], [2.5, 1.9, 9.5, 2.6], [2.5, 4.0, 9.5, 4.7],
              [12.6, 0.15, 13.85, 1.2], [11.5, 3.2, 11.8, 3.5], [0.15, 4.9, 0.8, 5.6]])
START = (1.4, 1.1)
RES, NX, NY, NR = 0.1, 140, 80, 180
L_OCC, L_FREE, L_MAX = 0.9, -0.7, 4.0
T_OCC, T_FREE = np.log(0.65 / 0.35), np.log(0.25 / 0.75)

def lidar(x, y, rmax):
    ang = np.linspace(0, 2 * np.pi, NR, endpoint=False)
    c, s = np.cos(ang)[:, None], np.sin(ang)[:, None]
    x0, y0, x1, y1 = B[:, 0] - x, B[:, 1] - y, B[:, 2] - x, B[:, 3] - y
    with np.errstate(divide="ignore", invalid="ignore"):
        tmin = np.maximum(np.minimum(x0 / c, x1 / c), np.minimum(y0 / s, y1 / s))
        tmax = np.minimum(np.maximum(x0 / c, x1 / c), np.maximum(y0 / s, y1 / s))
    t = np.where((tmax >= tmin) & (tmin > 0), tmin, np.inf).min(axis=1)
    return ang, np.where(t <= rmax, t, np.inf)

def integrate(L, x, y, rmax):
    ang, r = lidar(x, y, rmax)
    k = np.arange(0, rmax, RES / 2)[None, :]
    rr = np.where(np.isfinite(r), r, rmax)[:, None]
    xs, ys = x + np.cos(ang)[:, None] * k, y + np.sin(ang)[:, None] * k
    m = k < rr - RES / 2
    fi, fj = np.floor(xs[m] / RES).astype(int), np.floor(ys[m] / RES).astype(int)
    ok = (fi >= 0) & (fi < NX) & (fj >= 0) & (fj < NY)
    free = np.unique(fj[ok] * NX + fi[ok])
    hit = np.isfinite(r)
    ei = np.floor((x + r[hit] * np.cos(ang[hit])) / RES).astype(int)
    ej = np.floor((y + r[hit] * np.sin(ang[hit])) / RES).astype(int)
    ok = (ei >= 0) & (ei < NX) & (ej >= 0) & (ej < NY)
    occ = np.unique(ej[ok] * NX + ei[ok])
    L.flat[np.setdiff1d(free, occ)] += L_FREE
    L.flat[occ] += L_OCC
    np.clip(L, -L_MAX, L_MAX, out=L)

# 로봇이 갈 수 있는 빈 셀 = 출발점에서 상하좌우로 이어진, 상자와 겹치지 않는 셀
cx, cy = np.meshgrid((np.arange(NX) + 0.5) * RES, (np.arange(NY) + 0.5) * RES)
solid = np.zeros((NY, NX), bool)
for x0, y0, x1, y1 in B:
    solid |= (cx + RES / 2 > x0) & (cx - RES / 2 < x1) & (cy + RES / 2 > y0) & (cy - RES / 2 < y1)
reach = np.zeros_like(solid)
stack = [(int(START[1] / RES), int(START[0] / RES))]
while stack:
    j, i = stack.pop()
    if 0 <= j < NY and 0 <= i < NX and not reach[j, i] and not solid[j, i]:
        reach[j, i] = True
        stack += [(j + 1, i), (j - 1, i), (j, i + 1), (j, i - 1)]

def path_length(route):
    # TODO 1: 이웃한 경유점 사이 거리의 합
    p = np.array(route)
    return float(np.hypot(*np.diff(p, axis=0).T).sum())

def coverage(L):
    # TODO 2: 갈 수 있는 셀(reach) 중 '빈 공간으로 확정'(L < T_FREE) 된 셀의 비율 (%)
    return 100 * (reach & (L < T_FREE)).sum() / reach.sum()

def run(route, rmax=8.0, step=0.3):
    """경로를 따라 step 마다 스캔. 커버리지가 95 % 를 처음 넘은 지점의 주행 거리도 기록"""
    L, dist, hit95 = np.zeros((NY, NX)), 0.0, None
    integrate(L, *route[0], rmax)
    for (x0, y0), (x1, y1) in zip(route[:-1], route[1:]):
        seg = np.hypot(x1 - x0, y1 - y0)
        n = max(1, int(np.ceil(seg / step)))
        for k in range(1, n + 1):
            integrate(L, x0 + (x1 - x0) * k / n, y0 + (y1 - y0) * k / n, rmax)
            dist += seg / n
            if hit95 is None and coverage(L) >= 95:
                hit95 = dist
    return coverage(L), hit95

ROUTES = {
    # 시뮬레이터 5 의 '예시 경로'
    "A example": [(1.4, 1.1), (11.0, 1.1), (11.0, 3.3), (1.6, 3.3), (1.6, 5.6), (12.5, 5.6),
                  (12.5, 7.25), (1.6, 7.25), (1.6, 5.6), (1.4, 1.1)],
    # 선반 사이 통로를 한 번씩만 지나고 출발점으로 돌아오는 고리
    "B one loop": [(1.4, 1.1), (10.5, 1.1), (10.5, 5.6), (1.6, 5.6), (1.4, 1.1)],
    # TODO 3: 커버리지 95 % 이상이면서 B 보다 짧거나, 같은 길이로 더 높은 커버리지를 내는 경로
    "C mine": [(1.4, 1.1), (12.0, 1.1), (12.0, 7.2), (1.6, 7.2), (1.4, 1.1)],
}

print(f"reachable free cells: {int(reach.sum())}")
for rmax in (8.0, 3.5):
    print(f"== lidar max range {rmax} m")
    for name, route in ROUTES.items():
        cov, d95 = run(route, rmax)
        closed = np.hypot(route[-1][0] - route[0][0], route[-1][1] - route[0][1]) < 0.5
        print(f"  {name:11s} length {path_length(route):5.1f} m  coverage {cov:5.1f} %  "
              f"95% reached at {('%.1f m' % d95) if d95 else '  -   '}  loop {'yes' if closed else 'no'}")
  • B 는 위쪽 긴 복도(y 6.65 ~ 7.85)가 벽 [3.0, 6.5, 10.5, 6.65] 에 가려 보이지 않아 약 91 % 에 머뭅니다. 풀이의 C 는 창고 오른쪽 끝으로 올라가 위쪽 복도를 한 번 지나고 왼쪽 통로로 내려오는 고리(33.2 m, 약 99 %)입니다. 선반 사이 통로는 양 끝에서 들여다보는 것만으로 8 m 라이다가 채웁니다.
  • 3.5 m 라이다에서는 같은 C 가 약 86 % 입니다. 통로 끝에서 반대편 끝까지 닿지 않으므로, 예시 경로 A 처럼 통로마다 들어가야 합니다(A 는 3.5 m 에서도 98 %).
  • 95 % 를 넘은 뒤 출발점으로 돌아오는 구간은 커버리지에는 도움이 없지만, 루프를 닫아 누적 오차를 바로잡는 기회가 됩니다(시뮬레이터 4 · 5). 실제 매핑에서는 이 구간을 빼지 않습니다.

창고 시뮬레이터로 확인하기#

Playground 의 타깃을 Simulator 로 두면 공식 URDF 로 만든 3D 창고 시뮬레이터에 from pymycobot import MyAGVPro 가 그대로 연결됩니다. agvsim 은 참값 위치 · 스캔 · 지도처럼 실물에는 없는 정보를 읽는 시뮬레이터 전용 모듈입니다 (실물에서는 ROS 2 토픽이 그 역할). 가속 한계 · 미끄러짐 · 보호 영역 거리 등은 교육용 가정입니다.

Playground (시뮬레이터 · 방) 열기

실습 6-1 · 같은 스캔, 다른 위치로 그린 지도 (1 · 2교시 복습)

작업장에서 제자리 회전하며 스캔 24 장을 모읍니다. 바퀴 미끄러짐 8 % 를 켜 두고, 같은 스캔을 (가) 참값 위치, (나) 오도메트리 위치로 20 cm 격자에 찍습니다 (점유 확률 없이 "맞은 칸" 만 표시하는 간단한 버전).

먼저 예측하세요. 제자리 회전만 했는데도 (나) 지도가 번질까요? 번진다면 어떤 모양일까요?

python시뮬 실행 가능
from pymycobot import MyAGVPro
import agvsim, time, math
import numpy as np

agv = MyAGVPro("/dev/agvpro_controller", baudrate=1000000)
if agv.is_power_on() != 1:
    agv.power_on()

RES, X0, Y0, W, H = 0.2, -1.2, -2.8, 42, 34      # 20 cm 격자, 작업장 전체
LIDAR_X = 0.179                                   # base_link -> laser_link (URDF)

def add_scan(grid, pose, scan):
    x, y, th = pose
    sx, sy = x + LIDAR_X * math.cos(th), y + LIDAR_X * math.sin(th)
    for i, r in enumerate(scan["ranges"]):
        if r is None:
            continue
        a = th + scan["angle_min"] + i * scan["angle_increment"]
        cx = int((sx + r * math.cos(a) - X0) / RES)
        cy = int((sy + r * math.sin(a) - Y0) / RES)
        if 0 <= cx < W and 0 <= cy < H:
            grid[cy, cx] += 1

def show(grid, title):
    print(title, "- occupied cells:", int((grid > 0).sum()))
    for row in grid[::-1]:
        print("".join("#" if v > 0 else "." for v in row))

agvsim.set_pose(0, 0, 0)
agvsim.set_slip(0.08)                     # 바퀴 미끄러짐 8 %
g_true = np.zeros((H, W)); g_odom = np.zeros((H, W))
agv.turn_left(0.6)                        # 제자리에서 한 바퀴 넘게 돌며 스캔 모으기
for k in range(24):
    time.sleep(0.5)
    scan = agvsim.get_scan()
    add_scan(g_true, agvsim.get_pose(), scan)     # 정답 위치로 그린 지도
    add_scan(g_odom, agvsim.get_odom(), scan)     # 오도메트리로 그린 지도
agv.stop()
agvsim.set_slip(0.0)
show(g_true, "TRUE pose")
show(g_odom, "ODOM pose")
관찰 · 해설(가) 는 벽 · 기둥 · 선반이 한 줄로 또렷하고, (나) 는 같은 벽이 **부채꼴로 번져** 점유 칸 수가 거의 두 배가 됩니다. 미끄러짐 때문에 오도메트리의 각도가 실제보다 더 돌았다고 믿기 때문입니다 — 위치는 거의 그대로인데 θ 가 틀리면 먼 벽일수록 크게 어긋납니다(거리 × 각도 오차). 2교시의 "오도메트리만으로 지도를 그리면?" 이 이것이고, 3교시의 스캔 매칭은 바로 이 θ 를 스캔끼리 맞춰 고치는 과정입니다.

실습 (실물)#

실물 매핑 주행 전 안전 확인

4교시의 명령 ① ~ ④ (공식 문서 6.2.2) 로 실습실 · 복도를 매핑합니다. 로봇과 PC 의 연결 방법 · 작업 공간 경로는 공식 문서와 로봇 이미지에 따라 다를 수 있으니 먼저 확인합니다 (확인 필요).

bash
# 매핑이 도는 동안 다른 터미널에서 상태 확인
ros2 topic list | grep -E "map|scan|odom"     # /map, /scan, /odom 이 있는가
ros2 topic hz /scan                           # 스캔 주기 (6 ~ 12 Hz)
ros2 run tf2_ros tf2_echo map odom            # SLAM 이 내는 보정 (map -> odom)
ros2 topic echo /map --once --field info      # 해상도 · 크기 · 원점

아래는 매핑 중 /map 을 받아 알려진 셀 비율 · 점유 셀 수 를 주기적으로 출력하는 실물 전용 예시입니다. 시뮬레이터 5 의 커버리지 표시처럼, 주행하면서 지도가 얼마나 채워지는지 숫자로 봅니다. 여기서는 "갈 수 있는 셀" 을 모르므로 전체 셀 중 미지(−1)가 아닌 비율을 씁니다.

python실물 전용
import rclpy
from rclpy.node import Node
from nav_msgs.msg import OccupancyGrid

class MapWatcher(Node):
    def __init__(self):
        super().__init__("map_watcher")
        # 기본 QoS(RELIABLE, VOLATILE) 는 SLAM 노드의 /map 발행과 대부분 호환됨 (확인 필요)
        self.create_subscription(OccupancyGrid, "/map", self.on_map, 10)

    def on_map(self, msg):
        d = msg.data
        n = len(d)
        unknown = sum(1 for v in d if v < 0)
        occ = sum(1 for v in d if v >= 65)
        free = sum(1 for v in d if 0 <= v <= 25)
        info = msg.info
        self.get_logger().info(
            f"{info.width}x{info.height} @ {info.resolution:.3f} m, origin "
            f"({info.origin.position.x:.2f}, {info.origin.position.y:.2f})  "
            f"known {100 * (n - unknown) / n:.1f} %  free {free}  occupied {occ}")

def main():
    rclpy.init()
    node = MapWatcher()
    try:
        rclpy.spin(node)
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == "__main__":
    main()

실물에서 해 볼 것:

  1. 매핑 시작 직후와 실습실을 한 바퀴 돈 뒤의 known % 와 width × height 를 기록합니다. SLAM 노드가 지도를 넓혀 가면 크기와 origin 이 바뀌는지 확인합니다.
  2. 같은 공간을 ⓐ 권장 속도로 한 바퀴 돌고 출발점으로 돌아오기, ⓑ 빠르게 제자리 회전을 섞어 돌기 두 번 매핑해 저장합니다(-f 이름을 다르게). 두 .pgm 을 이미지 뷰어로 열어 이중 벽 · 휜 벽을 비교합니다.
  3. 저장된 .yaml 을 열어 resolution · origin 을 읽고, 실습 1-2 의 world_to_cell() · cell_to_pgm() 으로 출발점(map 좌표 0, 0)이 그림의 어느 픽셀인지 계산한 뒤 그림에서 찾습니다.
  4. 긴 복도가 있다면 줄자로 잰 길이와 지도에서 잰 길이(픽셀 수 × resolution)를 비교합니다. 차이가 크면 4교시의 "특징 없는 긴 복도" 와 연결해 원인을 적습니다.

자주 나는 오류와 해결#

1) map_saver 로 지도를 저장했는데 파일이 없음#

text
[ERROR] [map_saver]: Failed to save the map
  • 원인: -f 뒤의 폴더 경로가 없거나 틀렸습니다 (예: agv_pro_navigation2/map 폴더 이름 오타, 작업 공간이 ~/agv_pro_ros2 가 아님). 공식 문서도 경로가 정확해야 한다고 안내합니다. /map 토픽이 아직 하나도 발행되지 않았을 때도 저장이 실패합니다. 위의 에러 문구는 nav2 버전에 따라 다를 수 있습니다 (확인 필요).
  • 해결: ls ~/agv_pro_ros2/src/agv_pro_navigation2/map 으로 폴더가 있는지 먼저 확인하고, -f 에는 확장자 없이 이름까지만 적습니다. SLAM 노드가 살아 있는지 ros2 topic echo /map --once --field info 로 확인한 뒤 저장합니다.

2) RViz 에 지도가 보이지 않음 (No map received)#

text
Map  Status: Warn   No map received
Global Status: Error   Fixed Frame [map] does not exist
  • 원인: ① SLAM 노드가 아직 첫 스캔을 처리하지 못해 map 프레임이 없거나, ② RViz 의 Map 표시가 다른 토픽 · 맞지 않는 QoS(Durability)로 구독하고 있습니다.
  • 해결: ros2 topic hz /scan 으로 스캔이 들어오는지, ros2 run tf2_ros tf2_echo map odom 으로 map → odom 이 나오는지 확인합니다. RViz 의 Fixed Frame 을 map 으로, Map 표시의 Topic 을 /map 으로 두고, 지도가 한 번만 발행되는 경우(map_server)는 Durability 를 Transient Local 로 바꿉니다.

3) 지도에 벽이 두 겹으로 겹치거나 휘어서 그려짐#

text
(지도 그림) 같은 벽이 20 ~ 40 cm 떨어진 두 줄로 보이고, 긴 벽이 활처럼 휨
  • 원인: 스캔 매칭이 오도메트리 오차를 따라잡지 못했습니다. 빠른 주행 · 급회전으로 스캔 사이 이동이 커지면 초기 추정이 수렴 경계 밖으로 나갑니다 (실습 3-2). 바퀴 슬립이 큰 바닥, 루프를 닫지 않은 긴 경로도 원인입니다 (실습 2-1 · 시뮬레이터 4).
  • 해결: 권장 속도(0.25 m/s · 0.45 rad/s) 이하로 다시 매핑합니다. 경로에 루프를 만들고 출발점으로 돌아옵니다. ch04 의 오도메트리 보정(바퀴 반지름 · 회전 배율)을 먼저 해 두면 SLAM 이 고쳐야 할 오차가 줄어듭니다.

4) 저장한 지도를 Python 으로 불러왔더니 위아래가 뒤집힘#

text
cell (i=200, j=488) is wall in RViz, but my numpy grid says free
  • 원인: PGM 그림은 맨 윗줄부터 저장되지만 격자의 행 0 은 맨 아래(origin 쪽)입니다. 그림을 그대로 grid[row, col] 로 읽으면 j 와 row 가 뒤집힙니다 (1교시 주의 상자).
  • 해결: 그림 배열을 np.flipud(img) 로 뒤집은 뒤 격자로 쓰거나, 실습 1-2 의 j = height − 1 − row 를 적용합니다. negate: 0 이면 어두운 픽셀이 점유라는 것도 함께 확인합니다.

5) 긴 복도를 지나고 나니 지도의 복도가 실제보다 짧거나 김#

text
measured corridor: 18.0 m   /   map corridor: 16.9 m
  • 원인: 양쪽이 매끈한 벽뿐인 복도에서는 스캔 매칭이 복도 길이 방향을 고정하지 못합니다 (퇴화, 확인 문제 4-1). 그 방향은 바퀴 오도메트리의 거리 오차가 그대로 쌓입니다.
  • 해결: 복도 끝 · 문 · 기둥이 라이다 최대 거리 안에 보이도록 지나가고, 복도를 지난 뒤 넓은 곳에서 루프를 만듭니다. 필요하면 복도 벽에 상자 같은 임시 특징을 두고 매핑한 뒤 치웁니다. 오도메트리 거리 배율(ch04)도 다시 보정합니다.

6) ICP 결과가 오히려 더 틀어지거나 수렴하지 않음#

text
iter 30: pairs  12  rms  41.20 cm  pose x +0.912 y -0.388 th +47.10 deg
  • 원인: 초기 각도 · 위치 오차가 수렴 경계 밖이라 최근접 점 짝이 다른 벽과 이어졌습니다 (실습 3-2). 거부 거리(reject)가 너무 작아 짝이 몇 개 남지 않은 경우도 있습니다. best_fit 에서 반사 보정을 빼면 거울상 회전이 나올 수 있습니다.
  • 해결: 오도메트리 추정을 초기값으로 쓰고, 큰 오차가 예상되면 여러 각도 후보에서 ICP 를 돌려 rms 가 가장 작은 것을 고릅니다. 남은 짝 수가 너무 적으면 결과를 버리고 오도메트리 값을 유지합니다. det(R) < 0 보정을 꼭 둡니다.

과제#

  1. 해상도 · 노이즈 표: 실습 1-2 를 라이다 노이즈 σ = 0.5 · 1 · 3 · 5 cm 로 돌려 해상도(0.02 · 0.05 · 0.1 m)별 벽 커버리지 · 벽 두께 표를 만들고, myAGV Pro 라이다(±3 cm)에 맞는 해상도를 근거와 함께 고릅니다.
  2. 판정 기준 보고서: 실습 2-1 을 seed 10 개로 돌려 단계별 "쓸 수 있음" 비율을 구합니다. 이중 벽 기준(2 %)과 판정 거리(0.2 m)를 바꾸면 결론이 어떻게 달라지는지 한 문단으로 정리합니다.
  3. ICP 수렴 지도: 실습 3-2 의 표를 반복 10 · 20 · 50 회와 reject 0.3 · 0.5 · 1.0 · 2.0 m 로 만들고, "스캔 사이 이동이 이 정도 이하면 안전" 하다는 기준을 각도 · 거리로 제안합니다. 그 기준을 0.25 m/s · 0.45 rad/s, 스캔 10 Hz 와 비교합니다.
  4. 매핑 경로 설계: 실습 4-1 의 경로 C 를 라이다 최대 거리 3.5 m · 8 m 각각에 대해 가장 짧게 설계하고, 시뮬레이터 5 에서 직접 몰아 이중 벽 2 % 이하로 성공한 기록(주행 거리 · 화면)을 첨부합니다.

참고자료#

  • myAGV Pro 공식 문서 (6.2 SLAM · 매핑 절차): https://docs.elephantrobotics.com/docs/myAGV_Pro_en/
  • myAGV Pro ROS 2 패키지 (agv_pro_bringup · agv_pro_navigation2/map/map.yaml): https://github.com/elephantrobotics/agv_pro_ros2
  • 제품 페이지: https://www.elephantrobotics.com/en/myagv-pro/
  • 제품 소개 · 이미지 (현영이앤씨): https://www.hyenc.net/products/36
  • nav_msgs/OccupancyGrid 메시지 정의: https://docs.ros.org/en/humble/p/nav_msgs/interfaces/msg/OccupancyGrid.html
  • Nav2 Map Server · map_saver (map.yaml 형식): https://docs.nav2.org/configuration/packages/configuring-map-server.html
  • slam_gmapping (ROS 2 포트): https://github.com/Project-MANAS/slam_gmapping
  • Cartographer ROS: https://google-cartographer-ros.readthedocs.io/
  • RTAB-Map: http://introlab.github.io/rtabmap/
  • FAST-LIO: https://github.com/hku-mars/FAST_LIO
  • Point-LIO: https://github.com/hku-mars/Point-LIO
  • S. Thrun, W. Burgard, D. Fox, Probabilistic Robotics (MIT Press, 2005) — 9장 점유격자 지도, 13장 FastSLAM
  • P. J. Besl, N. D. McKay, "A Method for Registration of 3-D Shapes", IEEE TPAMI, 1992 — ICP 원 논문
  • 이 사이트의 도구: Playground · 물류창고 시뮬레이터 · 3D 뷰어