CH06SLAM — 지도를 그리며 달리기
점유격자 지도가 스캔으로 채워지는 과정과, 스캔 매칭 · 루프 폐합이 오도메트리 오차를 바로잡는 원리를 시뮬레이터에서 관찰합니다.
학습 목표#
- 점유격자 지도(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 m | URDF (ch01) |
| 기본 지도 | agv_pro_navigation2/map/map.yaml — 해상도 0.05 m, mode: trinary | agv_pro_ros2 저장소 (humble 브랜치) |
| 2D SLAM | slam_gmapping (Gmapping) — 공식 문서 6.2.2 | 공식 문서 |
| 3D · 시각 SLAM | Point-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 |
- 시뮬레이터 속 창고 · 라이다 · 오도메트리 오차는 교육용 모델 입니다. 로그 오즈 증거값(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) |
data | int8 배열, 길이 width × height. 행(row) 우선, (0, 0) 부터 | −1 · 0 · 100 |
data 값은 세 가지만 기억하면 됩니다. −1 = 미지(unknown, 아직 못 본 곳), 0 = 빈 공간(free), 100 = 점유(occupied, 벽 · 물건). 그 사이 값(1 ~ 99)은 확률을 퍼센트로 적은 것인데, 저장된 지도를 다시 읽을 때는 보통 이 세 값으로 정리됩니다.
셀 (i, j) (i = 열, j = 행)의 중심이 세계의 어디인지는 다음과 같습니다.
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) 입니다.
레이 위는 빈 공간, 끝점은 점유 — 브레젠험 레이 추적#
라이다가 3.2 m 앞에서 벽을 봤다는 것은 두 가지를 알려 줍니다. ① 3.2 m 지점에 무언가가 있다, ② 거기까지는 아무것도 없다(빛이 막히지 않고 지나갔으니까). 그래서 스캔 한 장을 격자에 그릴 때는 로봇 셀에서 끝점 셀까지 직선이 지나가는 셀을 모두 찾아 "빈 공간" 증거를 주고, 끝 셀에만 "점유" 증거를 줍니다. 격자 위에서 직선이 지나가는 셀을 정수 덧셈만으로 빠르게 찾는 방법이 브레젠험(Bresenham) 직선 알고리즘 입니다. 최대 거리까지 아무것도 맞지 않은 광선(inf)은 끝까지 빈 공간만 주고, 점유 증거는 주지 않습니다.
로그 오즈 갱신 — 여러 번 본 셀일수록 확신이 커진다#
한 번의 측정은 틀릴 수 있습니다(ch05 의 먼지 · 노이즈). 그래서 셀 값을 한 번에 0 이나 100 으로 덮어쓰지 않고, 확률 p 를 로그 오즈 L = ln(p / (1 − p)) 로 바꿔 증거를 더합니다.
처음: 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 은 다음과 같습니다.
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 |
negate | 0 이면 검정 = 점유, 흰색 = 빈 공간 (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장을 찍어 지도에 더합니다. 해상도 · 증거값 · 갱신 방식을 바꾸면 지금까지 찍은 같은 스캔들 로 지도를 다시 그리므로 공정하게 비교할 수 있습니다.
해 볼 것
- 처음 상태(0.05 m, 스캔 1장)에서 오른쪽 지도의 벽을 봅니다. 벽은 진한 검정인가요, 회색인가요? 같은 자리 스캔 10장 을 누르기 전에 벽 색이 어떻게 바뀔지 예측한 뒤 눌러 봅니다. 출력의 "점유 판정까지 끝점 몇 번" 과 연결해 설명합니다.
- ▶ 방 8곳 돌며 스캔 을 눌러 방 전체를 채운 뒤, 해상도 버튼 0.02 → 0.05 → 0.10 을 차례로 누릅니다. 셀 수 · 50 m 창고 메모리는 몇 배씩 바뀌나요? 0.02 m 에서 벽이 왜 점선처럼 끊기고 두꺼워지는지(셀 하나가 받는 광선 수 · 거리 노이즈 σ 와 셀 크기 비교) 말해 봅니다.
- 해상도 0.10 m 에서 강조 광선 방향 슬라이더를 움직이며 주황 셀(빈 공간)과 빨간 끝 셀(점유)을 봅니다. 최대 거리를 2 m 로 줄여 벽에 닿지 않는 광선을 만들면 끝 셀은 어떻게 표시되나요?
- 거리 노이즈를 5 cm 로 올리고 갱신을 마지막 값 덮어쓰기 로 바꿉니다. 같은 자리에서 10장을 더 찍은 뒤 다시 로그 오즈 누적 으로 돌려 비교합니다. 어느 쪽 벽이 더 깨끗한가요?
- 표시를 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 에서 l_occ = 0.85, l_free = −0.4, 임곗값 occupied_thresh 0.65 · free_thresh 0.25 로 두었습니다. 어떤 셀을 광선이 2번 지나갔고(빈 공간 증거) 끝점으로 1번 맞았습니다. 이 셀을 지금 저장하면 값은?
- 100 — 한 번이라도 맞았으므로 점유
- 0 — 지나간 횟수가 더 많으므로 빈 공간
- −1 — L = 0.85 − 0.8 = 0.05, p ≈ 0.51 로 두 임곗값 사이라 미지
- 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 방식의 생각이고, 횟수만 비교(②)하면 증거의 세기를 무시하게 됩니다.같은 50 m × 50 m 창고 지도를 해상도 0.05 m 에서 0.02 m 로 바꾸면, 셀 수와 지도 품질에 대해 시뮬레이터 1 에서 관찰한 것과 가장 맞는 설명은?
- 셀 수 2.5 배, 벽이 항상 더 선명해진다
- 셀 수 6.25 배, 셀 하나에 떨어지는 광선이 줄어 같은 주행으로는 벽이 성기고 노이즈가 두드러질 수 있다
- 셀 수는 같고 그림만 커진다
- 셀 수 6.25 배, 라이다 노이즈가 셀 크기에 맞춰 함께 작아진다
정답 보기
② — 셀 수는 (0.05 / 0.02)² = 6.25 배(약 1 MB → 6.25 MB)입니다. 라이다 거리 노이즈(±3 cm 사양)는 지도 해상도와 상관없이 그대로이므로(④ 오답), 셀이 노이즈보다 작아지면 끝점이 여러 셀로 흩어지고, 셀마다 받는 증거가 줄어 같은 주행으로는 더 성긴 지도가 됩니다. 해상도는 "작을수록 좋다" 가 아니라 센서 정밀도 · 메모리 · 용도의 균형입니다.2교시 · 오도메트리만으로 지도를 그리면?#
지도는 "스캔 + 그 스캔을 찍은 위치" 입니다#
1교시에서는 로봇의 위치를 정확히 안다고 가정했습니다. 실제로 로봇이 아는 위치는 ch04 의 오도메트리 입니다. 바퀴 슬립 · 바닥 요철 · 회전 오차 때문에 오도메트리는 주행 거리에 따라 점점 틀어지고, 특히 방향(θ) 오차 는 그 뒤의 모든 위치를 부채꼴처럼 돌려 버립니다. 스캔은 정확해도 "어디서 찍었는지" 가 틀리면, 같은 벽이 다른 자리에 한 번 더 그려집니다. 이것이 이중 벽(ghost wall) 이고, 각도 오차가 서서히 쌓이면 곧은 복도가 휘어 보입니다.
예측하기: 같은 복도를 두 번 지나가면?#
시뮬레이터를 켜기 전에 먼저 예측해 봅니다. 로봇이 창고를 한 바퀴 돌고 처음 지나갔던 가운데 통로를 다시 지나가며 지도를 그립니다. 첫 번째 지나갈 때와 두 번째 지나갈 때 오도메트리 오차는 같을까요? 그러면 그 통로의 선반 벽은 지도에 어떻게 그려질까요? 공책에 "두 번째 통과 때의 벽은 첫 번째보다 ___ 쪽으로 ___ cm 쯤 어긋날 것이다" 라고 적어 두세요.
시뮬레이터 2 · 참 위치 지도 vs 오도메트리 지도#
14 m × 8 m 창고를 로봇이 정해진 경로로 돕니다. 왼쪽은 참 위치 로, 오른쪽은 오도메트리 위치 로 같은 스캔을 그린 지도입니다(해상도 0.1 m). 빨간 셀은 실제 벽에서 0.2 m 넘게 떨어진 "점유" 셀, 곧 이중 벽 입니다.
해 볼 것
- 노이즈 0 을 누릅니다. 두 지도가 똑같은지, 이중 벽 셀이 0 에 가까운지 확인합니다. 이것이 "위치만 정확하면 지도는 쉽다" 는 기준선입니다.
- 1단계 → 2단계 → 3단계 를 차례로 누르며 오른쪽 지도와 판정을 기록합니다. 어느 단계부터 선반이 두 겹으로 보이나요? 오차가 경로 앞부분 과 뒷부분 중 어디에서 더 크게 드러나나요?
- 슬라이더로 거리 슬립 3 % · 회전 오차 0 % · σθ 0 을 만들고 결과를 적습니다. 이번에는 거리 슬립 0 % · 회전 오차 3 % · σθ 0 으로 바꿉니다. 이중 벽을 더 많이 만드는 것은 어느 쪽인가요? 이유를 "θ 오차는 그 뒤의 모든 이동 방향을 돌린다" 로 설명해 봅니다.
- 앞에서 적은 예측을 확인합니다. 가운데 통로를 두 번 지나가기 를 켜고 2단계로 둡니다. 가운데 통로의 선반 벽이 몇 겹으로 보이나요? ▶ 주행 재생 으로 두 번째 통과 순간을 지켜봅니다.
- 다른 난수 를 3 ~ 4 번 눌러 같은 2단계에서도 결과가 얼마나 달라지는지 봅니다. "노이즈 단계" 만으로 지도 품질을 장담할 수 없는 이유를 말해 봅니다.
관찰 포인트 — 스캔 자체는 왼쪽과 오른쪽이 완전히 같습니다. 달라진 것은 "어디서 찍었다고 믿었나" 하나뿐입니다. 거리 배율 오차는 지도를 조금 늘이거나 줄이는 정도지만, 회전 오차는 모서리를 돌 때마다 그 뒤의 경로 전체를 돌려서 벽을 두 겹 · 세 겹으로 만듭니다. 같은 곳을 두 번째로 지날 때는 그동안 쌓인 오차만큼 벽이 어긋나 그려집니다. 사람이 보면 "아까 그 통로" 인데, 오도메트리만 믿는 로봇은 "새 통로" 라고 생각합니다. 이 어긋남을 알아차리고 바로잡는 것 이 3교시의 스캔 매칭과 루프 폐합입니다.
SLAM 문제 정의 — 위치와 지도를 동시에#
정리하면, 로봇에게 주어진 것은 오도메트리 u₁ … u_t 와 스캔 z₁ … z_t 뿐이고, 알고 싶은 것은 **경로 x₁ … x_t 와 지도 m** 두 가지입니다.
알고 싶은 것: 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 에서 거리 슬립 3 % · 회전 오차 0 % · σθ 0 으로 두었을 때와, 거리 슬립 0 % · 회전 오차 3 % · σθ 0 으로 두었을 때를 비교했습니다. 가장 그럴듯한 관찰은?
- 두 경우 이중 벽이 같다 — 둘 다 3 % 이므로
- 거리 슬립 쪽이 훨씬 나쁘다 — 직진 거리가 회전보다 길므로
- 회전 오차 쪽이 훨씬 나쁘다 — 모서리마다 생긴 방향 오차가 그 뒤 모든 직진의 방향을 돌려, 멀리 갈수록 위치 오차가 커진다
- 둘 다 이중 벽이 생기지 않는다 — 노이즈(σθ)가 0 이므로
정답 보기
③ — 90° 회전에서 3 % 오차는 약 2.7° 입니다. 그 뒤 10 m 를 직진하면 옆으로 약 10 × sin 2.7° ≈ 0.47 m 가 어긋나고, 모서리를 돌 때마다 쌓입니다. 거리 슬립 3 % 는 10 m 에 0.3 m 로, 주로 진행 방향으로만 늘어나 벽이 "길어질" 뿐 겹치는 정도는 작습니다. 노이즈가 0 이어도 배율 오차(계통 오차)만으로 이중 벽이 생깁니다(④ 오답)."가운데 통로를 두 번 지나가기" 를 켜고 2단계 노이즈로 돌렸더니, 가운데 통로의 선반이 두 겹으로 그려졌습니다. 이 현상을 가장 잘 설명한 것은?
- 라이다가 두 번째 통과 때 고장 났다
- 두 번째 통과 때의 오도메트리 위치에는 그동안 한 바퀴 돌며 쌓인 오차가 들어 있어, 같은 선반이 다른 자리에 한 번 더 그려졌다
- 로그 오즈가 두 번 더해져 벽이 두 개가 됐다
- 지도 해상도가 0.1 m 라서 생긴 반올림 오차다
정답 보기
② — 스캔은 두 번 다 정확합니다. 첫 번째 통과 때는 오차가 작았고, 두 번째 통과 때는 한 바퀴(수십 m) 동안 쌓인 오차가 들어 있어 같은 선반이 어긋난 자리에 그려집니다. 로그 오즈를 같은 자리에 더하면 벽이 더 진해질 뿐 두 개가 되지 않습니다(③). 해상도 반올림은 최대 0.05 m 수준입니다(④). 이 어긋남이 루프 폐합의 단서가 됩니다.3교시 · 스캔 매칭과 루프 폐합#
스캔 매칭 — 새 스캔을 지도에 맞춰 위치 고치기#
2교시의 이중 벽은 "새 스캔을 그릴 위치" 가 틀려서 생겼습니다. 그렇다면 새 스캔을 그리기 전에, 지금까지 만든 지도(또는 바로 앞 스캔)와 가장 잘 겹치는 위치를 찾으면 됩니다. 오도메트리 위치는 그 탐색의 출발점(초기 추정) 으로만 씁니다. 이것이 스캔 매칭(scan matching) 입니다.
가장 널리 알려진 방법이 ICP(Iterative Closest Point, 반복 최근접점) 입니다. 한 번의 반복은 세 단계입니다.
- 짝 찾기: 새 스캔의 점마다 기준 점군(지도)에서 가장 가까운 점을 짝으로 고릅니다. 너무 먼 짝(최대 짝 거리
d_max초과)은 버립니다. - 회전 · 이동 계산: 짝들의 무게중심
μ_src·μ_ref를 빼고, 남은 점들로 회전각을 구한 뒤 이동을 구합니다. - 옮기기: 구한 회전 · 이동을 현재 추정에 덧붙이고, 1 로 돌아갑니다.
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 로 좋은 초기 추정을 주는 것이 중요합니다.
시뮬레이터 3 · 두 점군 맞추기 (ICP)#
파란 점은 원점에서 찍은 기준 스캔(지도), 주황 점은 조금 움직인 뒤 찍은 새 스캔 을 지금의 추정 위치에 그린 것입니다. 초록 점선 삼각형이 새 스캔을 찍은 참 위치, 주황 삼각형이 추정 위치 입니다. 주황 점군을 끌어 옮기거나(이동), 손잡이 원을 끌어 돌려(회전) 손으로 맞춰 본 뒤, ICP 를 돌려 비교합니다.
해 볼 것
- 보통 (0.3 m, −0.2 m, 20°)에서 먼저 손으로 주황 점군을 끌고 돌려 파란 점군에 맞춰 봅니다. 출력의 오차가 몇 cm · 몇 ° 까지 줄었나요? 그다음 초기 추정으로 → ▶ 최대 반복까지 를 눌러 ICP 와 비교합니다.
- ICP 1회 를 천천히 여러 번 누르며 회색 짝 선을 봅니다. 처음 몇 번의 짝은 맞는 짝인가요? 반복할수록 짝이 어떻게 바뀌나요? 오른쪽 그래프에서 RMS 와 참 위치 오차가 함께 줄어드는지 봅니다.
- 큰 회전 (60°)으로 바꿔 최대 반복 20 회로 돌립니다. 수렴하나요? 최대 반복을 50 으로 늘리면 어떤가요? 이번에는 90° 틀림 을 50 회 돌립니다. 그래프의 RMS 가 어떤 값에서 멈추나요? 멈춘 자리의 주황 점군과 파란 점군을 비교해 봅니다.
- 수렴 경계 찾기 를 누르고, 최대 반복을 5 → 20 → 50 으로 바꿔 가며 다시 누릅니다. 반복을 늘리면 경계가 얼마나 넓어지나요? 끝까지 늘려도 넘을 수 없는 각도가 있나요?
- 최대 짝 거리
d_max를 0.3 m 와 3 m 로 바꿔 경계를 다시 찾습니다. 너무 작으면 왜 짝이 부족하고, 너무 크면 왜 틀린 짝이 끼어드는지 말해 봅니다.
관찰 포인트 — ICP 는 "가장 가까운 점이 짝일 것" 이라는 가정 위에서 움직입니다. 초기 오차가 작으면 그 가정이 대부분 맞아 몇 번 만에 cm 수준으로 맞춰집니다. 회전이 크면 틀린 짝이 많아 한 번에 조금씩만 나아가므로 반복이 많이 필요하고(60° 는 20 회로 실패, 50 회로 수렴), 어느 각도를 넘으면 반복을 아무리 늘려도 틀린 자리에 갇혀(국소 최솟값) RMS 가 큰 값에서 멈춥니다. 반복 횟수는 경계를 넓힐 뿐 없애지 못합니다. 매칭이 끝난 뒤에도 RMS 가 크게 남는 것은 "이 매칭은 믿을 수 없다" 는 신호입니다. 실제 SLAM 은 이런 결과를 버리고 오도메트리 값을 그대로 씁니다. 그래서 실제 SLAM 은 매 스캔(0.1 초)마다, 오도메트리로 초기 추정을 준 상태에서 작은 오차만 맞춥니다. 오차가 작을 때 자주 고치는 것이 핵심입니다.
시뮬레이터 3 에서 초기 θ 오차 90° 로 ICP 를 50 번 돌렸더니, 짝 거리 RMS 가 0.46 m 에서 더 이상 줄지 않았고 참 위치 오차는 약 1 m · 87° 였습니다. 가장 알맞은 해석과 대응은?
- 반복이 부족하다 — 500 번 돌리면 반드시 맞는다
- 틀린 짝들 때문에 다른 자세(국소 최솟값)에 갇혔다 — RMS 가 큰 채 멈춘 결과는 버리고, 더 좋은 초기 추정(오도메트리 · IMU)으로 다시 맞춘다
- 라이다 노이즈가 너무 커서 생긴 일이다 — 노이즈 σ 를 줄이면 해결된다
- 정답을 찾았다 — 반복이 멈췄으므로
정답 보기
② — 갱신량이 0 이 되어 멈췄으므로 반복을 늘려도 그 자리에 머뭅니다(①). 노이즈 1 cm 는 0.46 m 의 RMS 를 설명하지 못하고(③), "멈췄다" 는 것은 수렴했다는 뜻이지 정답이라는 뜻이 아닙니다(④). 실제 시스템은 매칭 뒤 남은 오차(적합도 점수)를 보고 결과를 받아들일지 정하고, 오도메트리로 초기 오차를 작게 유지해 이런 상황 자체를 피합니다.루프 폐합 — 출발점에 돌아왔을 때 한 번에 바로잡기#
스캔 매칭은 바로 앞과 비교하므로 오차를 작게 만들 뿐 0 으로 만들지는 못합니다. 남은 작은 오차는 긴 경로를 도는 동안 조금씩 쌓입니다(드리프트). 이것을 없애는 기회가 전에 와 본 곳에 돌아왔을 때 입니다.
- 알아보기: 지금 스캔이 예전 키프레임(일정 간격으로 저장한 위치 + 스캔)의 스캔과 잘 맞는지 확인합니다 (넓은 범위의 스캔 매칭, 카메라라면 이미지 특징 비교).
- 제약 만들기: "지금 위치는 예전 그 위치에서 (Δx, Δy, Δθ) 만큼 떨어져 있다" 는 루프 제약 을 얻습니다.
- 분산하기: 경로 전체를 노드(키프레임)와 간선(연속 이동 · 루프 제약)으로 이루어진 포즈 그래프(pose graph) 로 보고, 모든 제약을 가장 잘 만족하도록 키프레임 위치를 다시 계산합니다. 끝에서 드러난 오차가 경로를 따라 나눠 들어갑니다.
- 지도 다시 그리기: 고친 키프레임 위치로 저장해 둔 스캔을 다시 그립니다.
마지막 위치만 고치면 안 될까요? 그러면 출발점은 맞지만 중간 경로 전체에 쌓인 오차는 그대로 남아 지도는 여전히 이중 벽입니다. 시뮬레이터 4 에서 확인합니다.
시뮬레이터 4 · 루프 폐합 — 끝점만 고치기 vs 경로 전체에 나누기#
로봇이 12 m × 8 m 고리 모양 복도를 한 바퀴(약 32 m) 돌며 0.5 m 마다 키프레임을 남깁니다. 키프레임 스캔(점)을 추정 위치 에 그린 것이 지도입니다. 슬라이더로 주행을 진행시키고, 출발점에 돌아오면 루프 폐합 방법을 골라 봅니다.
해 볼 것
- ▶ 주행 재생 을 눌러 키프레임이 쌓이는 모습을 봅니다. 출발점에 돌아왔을 때 추정 경로(주황)의 끝은 출발점에서 얼마나 떨어져 있나요? 빨간 점(벽에서 10 cm 넘게 어긋난 스캔 점)은 경로의 앞쪽 과 뒤쪽 중 어디에 많나요?
- 마지막 위치만 고치기 를 누릅니다. 끝점은 출발점에 붙었는데 지도(점)는 얼마나 나아졌나요? 오른쪽 막대에서 "안 함" 과 비교합니다.
- 경로를 따라 고르게 나누기 를 누릅니다. 어긋남이 몇 cm 로 줄었나요? 왜 "끝만" 보다 훨씬 좋은지, 오차가 언제 생겨서 어디까지 퍼졌는지 로 설명해 봅니다.
- 각도 드리프트를 0 으로, 거리 오차를 5 % 로 바꿔 봅니다. 이번에는 "나누기" 가 얼마나 잘 고치나요? 반대로 각도 드리프트 1.5 °/m 에서는 어떤가요? 고르게 나누는 방식이 가정한 것이 무엇인지 생각해 봅니다.
- 다른 난수 로 여러 번 바꿔도 "나누기 < 끝만 < 안 함" 의 순서가 유지되는지 확인합니다.
관찰 포인트 — 드리프트는 출발부터 조금씩 쌓였으므로, 오차는 끝에 몰려 있지 않고 경로 전체에 퍼져 있습니다. 끝점 하나만 옮기면 그 키프레임의 스캔만 제자리로 가고 나머지는 그대로입니다. 오차를 경로 길이에 비례해 나누면 "조금씩 쌓인" 드리프트의 모양과 비슷해서 대부분의 벽이 제자리로 돌아옵니다. 그런데 각도 드리프트 0 · 거리 오차 5 % 에서는 "나누기" 도 거의 효과가 없습니다. 고리를 한 바퀴 돌면 거리 배율 오차는 오른쪽 · 왼쪽, 위 · 아래로 서로 상쇄되어 끝에 드러나지 않기 때문입니다. 루프 폐합은 "끝에서 드러난 오차" 만 고칠 수 있고, 지도 전체가 몇 % 늘어난 것 같은 오차는 스캔 매칭(벽과 벽 사이 거리를 직접 재는 것)이 막아야 합니다. 실제 SLAM(Cartographer · slam_toolbox 등)은 이렇게 단순히 비례 배분하지 않고, 모든 제약의 오차 제곱합이 최소가 되도록 포즈 그래프 최적화 를 풉니다. 시뮬레이터 4 의 "나누기" 는 그 결과를 아주 단순하게 흉내 낸 것입니다.
시뮬레이터 4 에서 출발점에 돌아왔을 때 추정 위치가 1.4 m 틀어져 있었습니다. "마지막 위치만 고치기" 를 했더니 평균 벽 어긋남이 거의 줄지 않았습니다. 그 이유로 가장 알맞은 것은?
- 루프 제약이 틀렸기 때문
- 오차는 경로 전체에서 조금씩 쌓였는데, 마지막 키프레임 하나만 옮겼기 때문에 나머지 키프레임의 스캔은 여전히 틀린 자리에 그려지기 때문
- 마지막 키프레임에는 스캔이 없기 때문
- 지도 해상도가 너무 낮기 때문
정답 보기
② — 1.4 m 는 끝에서 "드러난" 오차일 뿐, 실제로는 출발 직후부터 각 키프레임이 조금씩 틀어져 있었습니다. 지도는 모든 키프레임의 스캔으로 그리므로, 끝 하나를 고쳐도 전체 점의 극히 일부만 바뀝니다. 오차를 경로를 따라 나눠야(포즈 그래프 최적화) 중간 키프레임들도 제자리로 돌아옵니다.2D · 시각 SLAM 비교 — Gmapping · Cartographer · RTAB-Map#
| Gmapping | Cartographer | RTAB-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) |
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)에서 실행하고, 앞의 터미널은 끄지 않습니다.
# ① 로봇 기동: 모터 · 라이다 · 오도메트리 · 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 % 이하. 성공한 순간의 주행 거리가 기록이며, 짧을수록 좋습니다.
해 볼 것
- 먼저 ▶ 예시 경로 자동 주행 을 배속 ×6 으로 지켜봅니다. 루프 폐합 메시지가 언제 나오나요? 미션이 성공했다면 주행 거리는 몇 m 인가요? 이 거리를 이길 경로를 공책에 그려 봅니다.
- 처음부터 를 누르고 직접 몰아 미션에 도전합니다. 라이다 최대 거리(8 m)를 생각하면 모든 통로를 끝까지 들어가야 할까요? 복도와 선반 사이 통로 중 "한 번 지나가며 양쪽을 다 볼 수 있는" 곳은 어디인가요?
- 스캔 매칭 과 루프 폐합 을 모두 끄고 같은 예시 경로를 자동 주행합니다. 이중 벽 % 와 위치 오차를 기록합니다. 스캔 매칭만 켜면, 둘 다 켜면 어떻게 바뀌나요? (2교시의 "오도메트리만으로 그린 지도" 와 비교)
- 라이다 최대 거리를 3.5 m 로 줄이고, 스캔 매칭만 켠 채(루프 폐합 끔) 위쪽 긴 복도 를 동쪽 끝에서 서쪽 끝까지 W 로 지나갑니다. 복도에 들어가기 전과 나온 뒤의 위치 오차(특히 x 방향, 초록과 파랑 로봇의 앞뒤 차이)를 비교합니다. 왜 복도 안에서만 오차가 커질까요?
- 직진 속도 1.0 m/s, 회전 1.5 rad/s, 오도메트리 오차 3단계로 빠르게 몰아 봅니다(배속 ×1). 공식 문서의 권장값(0.25 m/s · 0.45 rad/s)이 왜 느린지 지도 결과로 설명해 봅니다.
관찰 포인트 — 스캔 매칭이 켜져 있으면 오도메트리 오차 2단계에서도 벽이 거의 한 줄로 유지됩니다. 매 스캔마다 작은 오차를 바로 고치기 때문입니다(3교시). 루프 폐합은 그래도 남는 드리프트를 출발점 · 지나온 통로에 돌아왔을 때 한 번에 나눠 고칩니다. 긴 복도에서는 양쪽 벽이 "복도의 어느 위치에서 봐도 똑같이" 보여 스캔 매칭이 길이 방향 을 고정하지 못합니다. 복도 끝이 라이다 최대 거리 밖이면 그 방향의 오차는 오도메트리 그대로 쌓입니다. 그래서 매핑할 때는 복도 끝 · 기둥 같은 특징이 보이게 지나가고, 지난 뒤 루프를 만듭니다.
시뮬레이터 5 에서 라이다 최대 거리 3.5 m, 스캔 매칭만 켜고 긴 복도를 지나갔더니, 복도를 나온 뒤 추정 위치가 참 위치보다 복도 방향(x)으로 30 cm 앞서 있었고 옆 방향(y)은 거의 맞았습니다. 가장 알맞은 설명은?
- 스캔 매칭이 꺼져 있었다
- 복도 양쪽 벽은 y 와 방향(θ)은 알려 주지만, 복도 끝이 보이지 않으면 x 를 알려 주는 특징이 없어 그 방향은 오도메트리 오차가 그대로 쌓였다
- 라이다 노이즈가 x 방향으로만 컸다
- 지도 해상도가 x 방향으로만 거칠다
정답 보기
② — 매끈한 평행 벽 사이에서는 로봇을 복도 방향으로 조금 옮겨도 스캔이 지도와 똑같이 겹칩니다. 스캔 매칭이 "어느 x 든 점수가 같다" 고 판단하므로 x 는 오도메트리 값(거리 슬립 포함)에 맡겨집니다. y · θ 는 벽까지 거리와 벽의 기울기로 잘 고쳐지므로 맞습니다. 이것을 퇴화(degeneracy) 라고 부르며, 최대 거리가 긴 라이다 · 바퀴 오도메트리 보정 · 카메라(시각 SLAM) · 복도 밖에서의 루프 폐합으로 보완합니다.다음 중 시뮬레이터 5 의 미션(커버리지 95 % · 이중 벽 2 % 이하 · 짧은 주행)에 가장 도움이 되는 매핑 주행 계획은?
- 가장 빠른 속도로 모든 통로를 한 번씩 왕복한다
- 권장 속도로 큰 고리를 따라 돌며 특징이 보이는 곳을 지나고, 지나온 통로나 출발점으로 돌아와 루프를 닫는다. 라이다 최대 거리 안에 양쪽이 다 보이는 통로는 끝까지 들어가지 않는다
- 제자리에서 여러 번 돌며 스캔을 많이 모은 뒤 한 번에 이동한다
- 루프를 만들지 않도록 같은 곳을 다시 지나가지 않는다
정답 보기
② — 커버리지는 "로봇이 간 곳" 이 아니라 "라이다가 본 곳" 이므로, 최대 거리 안에서 보이는 곳까지 굳이 들어갈 필요는 없습니다. 빠른 주행 · 빠른 회전(①, ③)은 스캔 사이 이동을 키워 스캔 매칭 오차를 늘립니다. 루프를 피하면(④) 드리프트를 고칠 기회가 사라집니다.3D SLAM — FAST-LIO · Point-LIO 와 2D 의 차이#
myAGV Pro 는 3D 라이다 옵션(Livox MID-360, Unitree L2)으로 3D 지도도 만들 수 있습니다. 공식 문서 6.2.6 · 6.2.7 의 흐름은 다음과 같습니다.
# 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 라이다를 함께 쓸 수 있습니다.
# 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 만 씁니다.
계획의 실습 "스캔 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 입니다.
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]}")해 볼 것
- 출력된 지도에서 상자 뒤쪽 은 왜 비어 있나요(
)? 로봇 위치px, py를 (1.5, −1.5) 로 옮기면 어느 면이 새로 보이나요? - "ROS values after 1 scan" 의 free 개수는 화면의
.개수보다 훨씬 적습니다. 로봇 근처 셀만 빈 공간(0)이 되는 이유를 광선 밀도로 설명해 봅니다. L_FREE를 −0.4 → −1.2 로 바꾸면 스캔 1장만으로 free 가 크게 늘어납니다. 대신 무엇이 위험할까요? (ch05 의 먼지 · 반사 실패 광선을 떠올려 보세요)L_CLAMP를 4.0 → 1.0 으로 줄이고 마지막 출력에서 p 가 몇에서 멈추는지 봅니다.
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에서 멈춥니다. 노이즈 때문에 끝점이 옆 셀로 떨어지는 스캔이 있으면 그 스캔에서는 오르지 않습니다.
계획의 시뮬레이터 활동 "해상도 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 는 가장 촘촘하니 벽도 가장 "빈틈없이" 그려질까요?
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} %")해 볼 것
- map 좌표 (0.02, 0.03) 은 PGM 그림에서 위쪽에 가깝나요, 아래쪽에 가깝나요?
cell_to_pgm을return j, i로 두었을 때와 비교합니다. - 50 m × 50 m 창고의 메모리를 0.02 m 와 0.1 m 로 비교하면 몇 배인가요? 해상도 비율(5 배)과 어떤 관계인가요?
- 0.02 m 에서 벽 커버리지가 100 % 가 되지 않는 이유는? 라이다 노이즈
sigma를 0.03 → 0.005 로 줄이면 어떻게 바뀌나요? - 0.1 m 의 벽 두께(약 18 cm)는 실제 벽 두께(10 cm)와 왜 다를까요? 내비게이션에서 좁은 통로를 지날 때 어떤 영향이 있을까요?
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 는 벽이 끊기지 않지만 두께가 셀 크기로 부풀어 보입니다.
계획의 실습 "노이즈 수준 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 보다 이중 벽이 많을까요, 적을까요?
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'}")해 볼 것
- 어느 단계부터 "NO" 가 나오나요? 그 단계의 마지막 위치 오차는 몇 cm 인가요? 이중 벽 판정 거리(0.2 m)와 비교해 봅니다.
drive(route, lev, seed=7)의seed를 1 ~ 5 로 바꿔 2단계 결과가 매번 같은지 봅니다. 판정이 흔들린다면 "쓸 수 있는 단계" 를 어떻게 정해야 할까요?LEVELS[2]의 세 값 중 하나만 남기고 나머지를 0 으로 만들어, 이중 벽에 가장 큰 영향을 주는 오차가 무엇인지 찾습니다.- R2 의 결과를 시뮬레이터 2 의 "가운데 통로를 두 번 지나가기" 화면과 비교합니다. 이중 벽은 지도의 어디에 생기나요?
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 의 이중 벽은 두 번째로 지나간 가운데 통로의 선반 면에 몰립니다.
계획의 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회만에 얼마나 맞을까요?
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")해 볼 것
- 반복마다 rms 가 줄어드는 폭을 봅니다. 처음 몇 번에 크게 줄고 나중에는 거의 그대로입니다. 시뮬레이터 3 의 오차 그래프와 모양이 같은가요?
- rms 가 0 이 되지 않고 약 3 cm 에서 멈추는 이유는?
room_scan의n을 240 → 720 으로 늘려 봅니다. (힌트: 두 스캔은 같은 벽의 다른 점 을 찍었습니다) guess의 각도를 3° → −20° 로 바꿔 봅니다. 최종 오차는 어떻게 되나요?best_fit에서 반사 보정(det(R) < 0) 줄을 지우고 점을 일직선(p[:, 1] = 0)으로 만든 경우를 상상해 봅니다. 왜 이 보정이 필요할까요?
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 이어야 합니다.
계획의 실습 "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° 도 맞출 수 있을까요?
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))해 볼 것
- 반복 5 회 표에서
O가 거의 없는 이유는? 수렴 기준을 3 cm → 10 cm 로 느슨하게 하면 어떻게 바뀌나요? - 반복 30 회에서 각도 몇 도부터 실패하나요? 이 경계는 위치 오차가 커지면 어떻게 움직이나요?
reject0.5 m 와 2.0 m 의 표를 비교합니다. 거부 거리를 키우면 왜 큰 초기 오차에서 유리할까요? 반대로 어떤 위험이 있을까요? (힌트: 사람 · 지게차처럼 지도에 없는 물체의 점)ANGLES를range(40, 71, 5)로 촘촘하게 바꿔 경계를 5° 단위로 찾습니다.
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 를 키우면 넓게 끌어오지만, 지도에 없는 물체의 점까지 짝지어 결과를 비틀 수 있습니다.
계획의 실습 "가장 짧은 주행 거리로 커버리지 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 % 를 넘을까요? 넘지 못한다면 어디가 비었을까요?
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'}")해 볼 것
- B 가 95 % 에 못 미치는 이유를 창고 배치(
B의 위쪽 벽[3.0, 6.5, 10.5, 6.65])에서 찾아봅니다. 어느 구역이 어느 위치에서도 보이지 않나요? - 경로 C 를 설계해 8 m 라이다에서 95 % 이상을 만듭니다. 몇 m 까지 줄일 수 있나요? 결과를 시뮬레이터 5 에서 직접 몰아 확인합니다.
- 라이다 최대 거리 3.5 m 에서는 같은 경로 C 의 커버리지가 어떻게 되나요? 최대 거리가 짧을 때 경로 설계는 어떻게 달라져야 할까요?
- "95% reached at" 은 경로 끝보다 앞에서 95 % 를 넘은 지점입니다. 그 뒤 출발점으로 돌아오는 구간은 낭비일까요? (3교시 루프 폐합을 생각해 보세요)
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 토픽이 그 역할). 가속 한계 · 미끄러짐 · 보호 영역 거리 등은 교육용 가정입니다.
작업장에서 제자리 회전하며 스캔 24 장을 모읍니다. 바퀴 미끄러짐 8 % 를 켜 두고, 같은 스캔을 (가) 참값 위치, (나) 오도메트리 위치로 20 cm 격자에 찍습니다 (점유 확률 없이 "맞은 칸" 만 표시하는 간단한 버전).
먼저 예측하세요. 제자리 회전만 했는데도 (나) 지도가 번질까요? 번진다면 어떤 모양일까요?
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 의 연결 방법 · 작업 공간 경로는 공식 문서와 로봇 이미지에 따라 다를 수 있으니 먼저 확인합니다 (확인 필요).
# 매핑이 도는 동안 다른 터미널에서 상태 확인
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)가 아닌 비율을 씁니다.
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()실물에서 해 볼 것:
- 매핑 시작 직후와 실습실을 한 바퀴 돈 뒤의
known %와width × height를 기록합니다. SLAM 노드가 지도를 넓혀 가면 크기와origin이 바뀌는지 확인합니다. - 같은 공간을 ⓐ 권장 속도로 한 바퀴 돌고 출발점으로 돌아오기, ⓑ 빠르게 제자리 회전을 섞어 돌기 두 번 매핑해 저장합니다(
-f이름을 다르게). 두.pgm을 이미지 뷰어로 열어 이중 벽 · 휜 벽을 비교합니다. - 저장된
.yaml을 열어resolution·origin을 읽고, 실습 1-2 의world_to_cell()·cell_to_pgm()으로 출발점(map 좌표 0, 0)이 그림의 어느 픽셀인지 계산한 뒤 그림에서 찾습니다. - 긴 복도가 있다면 줄자로 잰 길이와 지도에서 잰 길이(픽셀 수 × resolution)를 비교합니다. 차이가 크면 4교시의 "특징 없는 긴 복도" 와 연결해 원인을 적습니다.
자주 나는 오류와 해결#
1) map_saver 로 지도를 저장했는데 파일이 없음#
[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)#
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) 지도에 벽이 두 겹으로 겹치거나 휘어서 그려짐#
(지도 그림) 같은 벽이 20 ~ 40 cm 떨어진 두 줄로 보이고, 긴 벽이 활처럼 휨- 원인: 스캔 매칭이 오도메트리 오차를 따라잡지 못했습니다. 빠른 주행 · 급회전으로 스캔 사이 이동이 커지면 초기 추정이 수렴 경계 밖으로 나갑니다 (실습 3-2). 바퀴 슬립이 큰 바닥, 루프를 닫지 않은 긴 경로도 원인입니다 (실습 2-1 · 시뮬레이터 4).
- 해결: 권장 속도(0.25 m/s · 0.45 rad/s) 이하로 다시 매핑합니다. 경로에 루프를 만들고 출발점으로 돌아옵니다. ch04 의 오도메트리 보정(바퀴 반지름 · 회전 배율)을 먼저 해 두면 SLAM 이 고쳐야 할 오차가 줄어듭니다.
4) 저장한 지도를 Python 으로 불러왔더니 위아래가 뒤집힘#
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) 긴 복도를 지나고 나니 지도의 복도가 실제보다 짧거나 김#
measured corridor: 18.0 m / map corridor: 16.9 m- 원인: 양쪽이 매끈한 벽뿐인 복도에서는 스캔 매칭이 복도 길이 방향을 고정하지 못합니다 (퇴화, 확인 문제 4-1). 그 방향은 바퀴 오도메트리의 거리 오차가 그대로 쌓입니다.
- 해결: 복도 끝 · 문 · 기둥이 라이다 최대 거리 안에 보이도록 지나가고, 복도를 지난 뒤 넓은 곳에서 루프를 만듭니다. 필요하면 복도 벽에 상자 같은 임시 특징을 두고 매핑한 뒤 치웁니다. 오도메트리 거리 배율(ch04)도 다시 보정합니다.
6) ICP 결과가 오히려 더 틀어지거나 수렴하지 않음#
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-2 를 라이다 노이즈 σ = 0.5 · 1 · 3 · 5 cm 로 돌려 해상도(0.02 · 0.05 · 0.1 m)별 벽 커버리지 · 벽 두께 표를 만들고, myAGV Pro 라이다(±3 cm)에 맞는 해상도를 근거와 함께 고릅니다.
- 판정 기준 보고서: 실습 2-1 을 seed 10 개로 돌려 단계별 "쓸 수 있음" 비율을 구합니다. 이중 벽 기준(2 %)과 판정 거리(0.2 m)를 바꾸면 결론이 어떻게 달라지는지 한 문단으로 정리합니다.
- 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-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 뷰어