전체 과정 P2 로봇의 감각 — 오도메트리와 센서 · 약 200분 · 중급

CH04오도메트리 — 바퀴로 위치를 세다

바퀴 속도를 적분해 위치를 추정하는 오도메트리를 직접 구현하고, 오차가 왜 쌓이는지 시뮬레이터로 확인합니다.

선수 챕터: CH02 CH03
Python Playground시뮬레이터

학습 목표#

  • 로봇의 자세(pose) (x, y, θ) 를 2D 동차 변환 행렬로 나타내고, 로봇 기준 점 · 속도 (vx, vy, ω) 를 회전 행렬 R(θ) 로 월드 좌표로 바꿀 수 있습니다.
  • ch02 의 순기구학과 오일러 적분을 이어 바퀴 속도 → 위치 를 계산하는 오도메트리(odometry) 함수를 직접 만들 수 있습니다.
  • 샘플링 주기 Δt 가 곡선 경로의 적분 오차를 어떻게 바꾸는지 예측하고, 오일러 · 중점 적분의 오차를 숫자로 비교할 수 있습니다.
  • 바퀴 반지름 오차 · 회전 슬립 · 각속도 노이즈가 위치 오차에 주는 영향을 시뮬레이터로 분리하고, 각도 오차가 가장 위험한 이유 와 IMU 로 θ 를 보정하는 아이디어를 설명할 수 있습니다.
  • ROS 2 의 map → odom → base_footprint TF 구조와 nav_msgs/Odometry 메시지를 읽고, 각 변환을 누가 발행하는지 말할 수 있습니다.

이론#

ch03 에서 move_forward(0.5) 를 2 초 걸면 "대략" 1 m 간다는 것, 그리고 그 "대략" 이 매번 달라진다는 것을 봤습니다. 이번 차시에서는 바퀴가 실제로 돈 양(엔코더)을 읽어 로봇이 어디까지 갔는지 세는 방법, 즉 오도메트리(odometry) 를 만듭니다. 오도메트리는 자율주행의 가장 바닥에 있는 위치 정보입니다. 뒤 차시의 SLAM · AMCL · Nav2 모두 이 값 위에서 동작합니다.

기호뜻값출처
lx + ly회전 팔 길이0.3514 mch02 (URDF 바퀴 위치)
r바퀴 반지름0.05 mGazebo 설정의 지름 0.1 m — 실물은 확인 필요
순기구학바퀴 4개 → (vx, vy, ω)vx = (r/4)(w_FL + w_FR + w_RL + w_RR) 등ch02 3교시
이 차시의 시뮬레이터와 Playground 코드에 대해
  • 시뮬레이터 속 "실제 로봇" 은 교육용 모델입니다. 오차의 크기(반지름 오차 %, 슬립 %, 노이즈 크기)는 원리를 보이기 위한 값이며 myAGV Pro 실측값이 아닙니다.
  • Playground 코드는 표준 라이브러리 + numpy 만 씁니다. 실물 로봇 · ROS 2 가 필요한 부분은 ## 실습 (실물) 에 따로 두었습니다.

1교시 · 로봇 자세(pose)와 2D 변환#

자세 = 위치 + 방향#

평평한 창고 바닥 위의 로봇은 세 숫자로 정해집니다. 이것을 자세(pose) 라고 부릅니다.

  • x, y : 월드 좌표계에서 로봇 기준점(base_footprint, 차체 중심 바닥)의 위치 (m)
  • θ (theta, yaw) : 월드 x 축에서 로봇 앞(+x_r) 까지 반시계 방향 각도 (rad)
월드 좌표계(odom · map)와 로봇 좌표계(base_footprint) — 자세 (x, y, θ) X (월드) Y (월드) 원점 x y x_r (앞) y_r (왼쪽) θ 회전 행렬 R(θ) [ cosθ -sinθ ] [ sinθ cosθ ] 동차 변환 T(x, y, θ) [ cosθ -sinθ x ] [ sinθ cosθ y ] [ 0 0 1 ] 로봇 기준 점 p_r → 월드 점 p_w = T · [p_r; 1] 로봇 기준 속도 → 월드 속도 v_w = R(θ) · [vx; vy]
월드 좌표계와 로봇 좌표계. 로봇 좌표계는 ROS 관례대로 x_r = 앞, y_r = 왼쪽입니다 (ch01). 두 좌표계는 자세 (x, y, θ) 로 이어집니다.

"월드" 는 하나가 아닙니다. 이번 차시에서는 출발점을 원점으로 하는 **odom 좌표계를 쓰고, 4교시에서 지도 기준의 map** 좌표계와의 관계를 봅니다.

회전 행렬과 동차 변환#

로봇 기준으로 (px, py) 에 있는 점(예: 라이다가 본 팔레트 모서리)은 월드에서 어디일까요? 먼저 θ 만큼 돌리고, 그다음 로봇 위치만큼 옮깁니다.

text
p_w = R(θ) · p_r + [x, y]       R(θ) = [ cosθ  -sinθ ]
                                       [ sinθ   cosθ ]

이 "돌리고 옮기기" 를 행렬 하나로 묶은 것이 동차 변환(homogeneous transform) 입니다. 점 뒤에 1 을 붙여 3 칸 벡터로 만들면 곱셈 한 번이 됩니다.

text
          [ cosθ  -sinθ  x ]
T(x,y,θ) = [ sinθ   cosθ  y ]        [p_w; 1] = T · [p_r; 1]
          [  0      0    1 ]

동차 변환의 쓸모는 이어 붙이기 입니다. T_A_B 를 "A 좌표계에서 본 B 의 자세" 라고 쓰면

  • 이어 붙이기: T_A_C = T_A_B · T_B_C (월드 → 로봇 → 라이다 → 점)
  • 거꾸로: T_B_A = inv(T_A_B) — 회전 부분은 전치 Rᵀ, 이동 부분은 -Rᵀ · [x, y]

4교시의 TF 트리가 바로 이 곱셈들의 사슬입니다.

속도도 돌려야 합니다#

ch02 의 (vx, vy, ω) 는 로봇 기준 속도입니다. move_left_lateral 은 언제나 "로봇의 왼쪽" 으로 가지만, 그 왼쪽이 월드의 어느 쪽인지는 θ 에 달려 있습니다.

text
[ẋ]   [ cosθ  -sinθ ] [vx]            ẋ = vx·cosθ - vy·sinθ
[ẏ] = [ sinθ   cosθ ] [vy]     즉     ẏ = vx·sinθ + vy·cosθ
θ̇ = ω                                  θ̇ = ω  (회전 속도는 돌릴 필요 없음)

속도는 "방향" 이라서 이동(x, y)은 더하지 않고 회전 R(θ) 만 곱합니다. 차동 구동 로봇은 vy = 0 이라 vy 항이 사라지지만, 메카넘 휠의 myAGV Pro 는 vy 항이 살아 있어 부호 실수가 곧바로 경로 오류 가 됩니다.

각도는 늘 -π ~ π 로 감아 둡니다

θ 를 계속 더하면 3바퀴 돈 뒤 θ = 6π 처럼 커집니다. cos · sin 계산에는 문제가 없지만 "목표 각도와 차이" 를 비교할 때 틀립니다. θ = atan2(sin θ, cos θ) 로 -π ~ π 범위로 감아 두는 습관을 들이세요.

시뮬레이터 1 · 로봇 기준 → 월드 기준#

먼저 예측하세요. 로봇이 θ = 90° (월드 위쪽을 보고) 서 있을 때 move_left_lateral(0.5) 를 보내면, 월드 지도에서 로봇은 위 · 아래 · 왼쪽 · 오른쪽 중 어디로 갈까요? 공책에 적은 뒤 조작합니다.

시뮬레이터 1 · 자세 (x, y, θ) · 로봇 기준 점 · 이동 명령의 월드 방향 (로봇이나 보라색 점을 끌어 보세요)
· ·

해 볼 것

  1. θ = 90° 문제 세팅 을 누릅니다. 예측 모드라 화살표가 숨겨져 있습니다. 예측을 말한 뒤 정답 보기 를 누르세요. move_right_lateral · move_backward 로 바꿔 가며 같은 방식으로 예측 → 확인을 반복합니다.
  2. 예측 모드를 끄고 θ 슬라이더를 -180° → 180° 로 천천히 밉니다. move_left_lateral 의 주황 화살표는 항상 빨간 x_r 축과 어떤 각도를 이루나요? 월드 (ẋ, ẏ) 의 두 숫자는 어떤 함수 모양으로 바뀌나요?
  3. 보라색 점을 끌어 로봇 바로 오른쪽 옆(로봇 기준)에 놓아 보세요. 그때 p_r 의 py 부호는? 이번에는 로봇을 끌어 옮겨도 p_r 은 그대로인지, p_w 는 어떻게 되는지 봅니다.
  4. θ = 0° 로 두면 R(θ) 는 어떤 행렬이 되나요? 그때 p_w = p_r + (x, y) 가 되는지 숫자로 확인합니다.
  5. 명령을 "대각선 vx = vy = 0.35" 로 바꾸고 θ = 45° 로 둡니다. 월드 속도는 어느 쪽인가요? 크기 (0.35·√2 ≈ 0.495 m/s)는 θ 를 바꿔도 변하나요?

관찰 포인트 — R(θ) 는 벡터의 방향만 돌리고 길이는 바꾸지 않습니다. 로봇 기준 명령은 늘 같아도(왼쪽 0.5 m/s) 월드 속도는 θ 에 따라 (-0.5·sinθ, 0.5·cosθ) 로 돌아갑니다. 점 p 는 이동 + 회전 이 모두 필요하고(동차 변환 T), 속도는 회전만 필요합니다. 로봇을 끌어도 p_r 이 그대로인 것은 그 점이 "로봇에 붙어 있는" 좌표이기 때문입니다 — 라이다 점이 로봇 기준으로 들어오는 것과 같습니다.

확인 문제 1-1

시뮬레이터 1 에서 θ = 90° 로 두고 move_left_lateral(0.5) 를 보냈습니다. 월드 속도 (ẋ, ẏ) 로 알맞은 것은?

  1. (0, +0.5) — 지도 위쪽
  2. (-0.5, 0) — 지도 왼쪽
  3. (+0.5, 0) — 지도 오른쪽
  4. (0, -0.5) — 지도 아래쪽
정답 보기② — ẋ = vx·cosθ − vy·sinθ = 0 − 0.5·1 = −0.5, ẏ = vx·sinθ + vy·cosθ = 0 + 0.5·0 = 0 입니다. 로봇이 지도 위쪽(+Y)을 보고 있으니 로봇의 왼쪽은 지도의 왼쪽(−X)입니다. ① 은 θ = 0° 일 때의 답이고, 식에서 −sinθ 의 부호를 놓치면 ③ 이 나옵니다.
확인 문제 1-2

로봇이 월드 (3.0, 1.0) 에서 θ = 180° 로 서 있고, 라이다가 로봇 기준 (1.0, 0.5) (앞 1 m, 왼쪽 0.5 m)에서 팔레트를 봤습니다. 팔레트의 월드 좌표는?

  1. (4.0, 1.5)
  2. (2.0, 1.5)
  3. (2.0, 0.5)
  4. (4.0, 0.5)
정답 보기③ — R(180°) 는 두 성분의 부호를 모두 뒤집습니다: (1.0, 0.5) → (−1.0, −0.5). 여기에 로봇 위치 (3.0, 1.0) 을 더하면 (2.0, 0.5) 입니다. 로봇이 왼쪽(−X)을 보고 있으므로 "앞" 은 −X, "왼쪽" 은 −Y 입니다. 회전 없이 더하기만 하면 ① 이 됩니다. 시뮬레이터 1 에서 x = 3, y = 1, θ = 180°, px = 1, py = 0.5 로 맞춰 확인해 보세요.

2교시 · 추측 항법(dead reckoning) 구현#

바퀴 엔코더에서 위치까지#

오도메트리는 네 단계를 Δt 마다 되풀이합니다. 바다 위의 배가 "속도 × 시간" 으로 지도에 자기 위치를 찍어 나가던 추측 항법(dead reckoning) 과 같은 생각입니다.

text
① 엔코더 읽기      바퀴 4개의 각속도 w_FL, w_FR, w_RL, w_RR (rad/s)
② 순기구학 (ch02)  → 로봇 기준 속도 (vx, vy, ω)
③ 회전 (1교시)     → 월드 속도 (ẋ, ẏ, θ̇) = (vx·cosθ - vy·sinθ, vx·sinθ + vy·cosθ, ω)
④ 적분            → x += ẋ·Δt,  y += ẏ·Δt,  θ += ω·Δt

④ 처럼 "지금 속도 × Δt 를 더하기" 를 오일러 적분(Euler integration) 이라 합니다. 직선에서는 정확하지만, 곡선에서는 한 걸음 동안 방향이 바뀌는 것을 무시하므로 매 걸음 작은 오차가 생깁니다.

추측 항법의 한 걸음 — 실제는 호(arc), 오일러 적분은 시작 방향의 직선 실제 경로 (원호) 오일러: θₖ 방향으로 직진 중점: θₖ + ωΔt/2 방향으로 직진 (xₖ, yₖ, θₖ) 한 걸음 오차 ≈ v·Δt × ωΔt/2 오일러 적분 (Δt 마다 반복) x += (vx·cosθ - vy·sinθ)·Δt y += (vx·sinθ + vy·cosθ)·Δt θ += ω·Δt 중점 적분 (2차) a = θ + ω·Δt/2 x += (vx·cos a - vy·sin a)·Δt ... 오일러 오차 ∝ Δt (1차) 중점 오차 ∝ Δt² (2차)
오일러 적분은 한 걸음 동안 시작 방향 θₖ 로 직진한다고 봅니다. 걸음 중간의 방향 θₖ + ωΔt/2 를 쓰면(중점 적분) 실제 호에 훨씬 가까워집니다.
  • 한 걸음의 오차는 v·Δt × ωΔt/2 정도 — Δt² 에 비례합니다.
  • 일정 시간 T 동안 걸음 수는 T/Δt 개이므로 쌓인 오차는 Δt 에 비례합니다 (1차 방법).
  • 중점 적분은 걸음 오차가 Δt³, 쌓인 오차가 Δt² 에 비례합니다 (2차 방법). 계산량은 거의 같습니다.
  • 속도가 한 걸음 동안 일정하다면 원호를 그대로 계산하는 정확 적분 도 가능합니다 (실습 2-2 풀이 참고).
오도메트리의 Δt 는 누가 정하나

ROS 2 드라이버나 Gazebo 구동 플러그인은 정해진 주기(publish_rate)로 오도메트리를 계산해 발행합니다. 4교시에서 볼 Gazebo 설정의 publish_rate 50 은 Δt = 1/50 = 0.02 s 를 뜻합니다. 직접 루프를 짤 때는 time.sleep(0.02) 를 믿지 말고 실제로 잰 경과 시간 을 Δt 로 씁니다 (자주 나는 오류 4).

시뮬레이터 2 · 실제 경로 vs 오도메트리 경로#

검은 선은 실제 경로(정확한 원호), 파란 점은 오도메트리가 Δt 마다 찍은 위치입니다. 바퀴 측정은 완벽하다고 두었으니, 두 선의 차이는 순수하게 적분 방법과 Δt 때문 입니다.

먼저 예측하세요. 반지름 1 m 원을 0.5 m/s 로 돌 때, Δt 를 0.2 s 에서 0.5 s 로 2.5 배 늘리면 최대 오차는 몇 배가 될까요? 그리고 한 바퀴를 다 돈 끝 위치 의 오차는 클까요, 작을까요?

시뮬레이터 2 · 원 궤적 추측 항법 — 실제(검정) vs 오도메트리(파랑) · 오른쪽: 시간에 따른 위치 오차

해 볼 것

  1. Δt = 0.02 / 0.2 / 0.5 버튼을 차례로 누르고 최대 위치 오차를 표에 적습니다. Δt 가 10 배 · 2.5 배 될 때 오차도 같은 배수로 커지나요? (참고 줄의 v·Δt 와 비교)
  2. Δt = 0.5 에서 ▶ 애니메이션 을 봅니다. 파란 상자(오도메트리)는 매끄럽게 움직이나요, 0.5 s 마다 뚝뚝 끊기나요? 오차가 가장 커지는 순간은 원의 어디쯤인가요?
  3. 1 바퀴의 "마지막 샘플 오차" 와 "최대 오차" 를 비교합니다. 시간을 반 바퀴로 바꾸면? 끝 위치만 재서 오도메트리를 평가하면 왜 속을 수 있는지 말해 봅니다.
  4. 적분 방법을 중점 으로 바꿔 같은 Δt 에서 오차를 비교합니다. Δt = 0.2 → 0.4 로 2 배 늘리면 중점 적분의 오차는 몇 배가 되나요?
  5. 운동을 직선 (ω = 0) 으로 바꾸고 Δt 를 1.0 까지 올려 봅니다. 오차는? 또 옆으로 가며 원 으로 바꿔도 오일러 오차가 v·Δt 와 맞는지 확인합니다.

관찰 포인트 — 오일러 적분의 오차는 Δt 에 비례 합니다 (원 운동에서 최대 오차 ≈ v·Δt: 0.5 m/s × 0.2 s = 10 cm, 0.5 s 면 25 cm). 중점 적분은 Δt² 에 비례 해서 같은 Δt 에서 수십 배 작습니다. 직선(ω = 0)에서는 Δt 가 얼마든 오차가 0 입니다 — 적분 오차는 방향이 바뀌는 동안 에만 생깁니다. 또 오일러 경로는 "앞으로 v·Δt/2 만큼 밀린 원" 이 되어 한 바퀴를 돌면 출발점 근처로 돌아옵니다. 끝 위치만 보면 오차가 작아 보이는 함정이 있습니다.

확인 문제 2-1

시뮬레이터 2 의 기본값(원, v = 0.5 m/s, R = 1 m, 오일러, 1 바퀴)에서 Δt 를 0.2 s → 0.5 s 로 바꿨습니다. 최대 위치 오차의 변화로 알맞은 것은?

  1. 약 10 cm → 약 25 cm (2.5 배)
  2. 약 10 cm → 약 63 cm (6.25 배)
  3. 약 10 cm → 약 10 cm (변화 없음 — 바퀴 측정이 완벽하므로)
  4. 약 1 cm → 약 2.5 cm
정답 보기① — 오일러 적분의 쌓인 오차는 Δt 에 비례하는 1차 방법이라 2.5 배가 됩니다 (원 운동에서 최대 오차 ≈ v·Δt). ② 의 Δt² 비례는 중점 적분의 성질입니다. 측정이 완벽해도 "한 걸음 동안 방향이 그대로" 라는 적분 가정 때문에 오차가 생기므로 ③ 은 틀립니다. 실제 드라이버는 Δt = 0.02 s(50 Hz) 정도로 돌려 이 오차를 1 cm 수준으로 줄입니다.
확인 문제 2-2

Δt = 0.5 s 오일러에서 "1 바퀴 끝의 오차" 는 1 cm 도 안 되는데 "최대 오차" 는 약 25 cm 였습니다. 이 결과를 가장 잘 설명한 것은?

  1. 오차가 한 바퀴 동안 우연히 상쇄되었고, 다음 바퀴에는 크게 쌓인다
  2. 오일러 경로는 실제 원과 크기가 같고 위치만 밀린 원이라, 출발점 근처에서는 맞고 반대편에서 가장 크게 벌어진다
  3. 마지막 샘플에서 θ 오차가 0 이 되도록 프로그램이 자동 보정한다
  4. 최대 오차는 화면 표시 오류이고 실제 오차는 1 cm 이다
정답 보기② — 오일러 경로의 점들은 한 걸음씩 원을 따라가지만 중심이 진행 방향으로 v·Δt/2 만큼 밀린 원(정다각형) 위에 놓입니다. 출발점에서 두 원이 만나므로 한 바퀴 뒤에는 다시 가까워지고, 반대편(반 바퀴)에서 v·Δt 만큼 벌어집니다. 2 바퀴로 바꿔 보면 오차 그래프가 같은 모양으로 반복될 뿐 커지지 않으므로 ① 도 아닙니다. 그래서 오도메트리를 평가할 때는 끝점 한 곳이 아니라 경로 전체의 오차를 봐야 합니다.

3교시 · 오차는 왜 쌓일까 — 노이즈와 드리프트#

두 종류의 오차#

2교시는 측정이 완벽하다고 가정했습니다. 실제 바퀴 측정에는 두 종류의 오차가 섞입니다.

종류예성질줄이는 법
계통 오차 (systematic)바퀴 반지름이 설정값과 다름 (마모 · 적재 · 타이어 눌림), 회전할 때마다 미끄러짐매번 같은 방향 으로 쌓임 → 거리 · 회전 수에 비례보정(calibration), 다른 센서
랜덤 오차 (random)엔코더 눈금 양자화, 바닥 요철, 순간적인 미끄러짐방향이 매번 다름 → 일부 상쇄되며 √시간 꼴로 퍼짐평균, 필터, 다른 센서

메카넘 휠은 바닥과 롤러가 비스듬히 닿아 움직이므로 일반 바퀴보다 미끄러짐이 많고, 특히 제자리 회전 에서 바퀴가 돈 만큼 차체가 돌지 않는 일이 흔합니다 (ch02 4교시의 미끄러짐 실습).

각도 오차가 위치 오차보다 위험한 이유#

같은 "1 %" 라도 어디에 생기느냐에 따라 결과가 크게 다릅니다.

text
거리 오차 1 %      → 10 m 가면 10 cm 틀림 (진행 방향으로만)
각도 오차 1° (0.017 rad) → 10 m 가면 옆으로 10 × sin(1°) ≈ 17 cm 틀림
                          → 그 뒤로 가는 모든 거리에서 계속 벌어짐
  • 거리 오차는 그 구간에서 끝나지만, 각도 오차는 이후 모든 이동의 방향을 돌려 놓습니다. 횡방향 오차 ≈ 이후 이동 거리 × δθ.
  • 그래서 회전이 많은 경로(창고 순회 = 코너 4 개 × 여러 바퀴)에서 회전 슬립은 오차의 주범이 됩니다.

불확실성 타원(공분산)#

랜덤 오차가 있으면 "로봇은 정확히 여기" 가 아니라 "대략 이 범위 안" 입니다. 같은 경로를 여러 번 돌려 오도메트리 끝점을 찍으면 점들이 타원 모양으로 퍼지는데, 이 퍼짐을 숫자로 나타낸 것이 공분산(covariance) 행렬이고 그림으로 그린 것이 불확실성 타원 입니다. 타원은 시간이 지날수록 커집니다. 이것이 드리프트(drift) 입니다. 4교시의 nav_msgs/Odometry 메시지에는 이 공분산을 담는 칸이 있습니다.

IMU 로 θ 를 보정하는 아이디어#

각도가 가장 위험하다면, 각도만큼은 바퀴 말고 다른 센서로 재면 됩니다. IMU(관성 측정 장치) 의 자이로스코프는 차체의 회전 속도 ω 를 직접 잽니다. 바퀴가 미끄러져도 차체가 실제로 돈 만큼만 잽니다.

  • 대신 자이로는 바이어스(bias) 가 있어 가만히 있어도 아주 조금씩 돈다고 읽습니다 → θ 가 천천히 흘러갑니다 (예: 0.0002 rad/s 면 1 시간에 약 41°).
  • 그래서 실제 시스템은 "바퀴로 거리, 자이로로 회전" 처럼 나눠 맡기거나 둘을 확률적으로 섞습니다(EKF 등, 뒤 차시).
  • myAGV Pro 의 URDF 에는 imu_link 가 있고(ch01), 옵션 센서 가운데 3D 라이다 Livox MID-360 · Unitree L2 와 깊이 카메라 Orbbec Gemini 2 도 IMU 를 내장합니다. 어느 IMU 가 어떤 토픽으로 나오는지는 구성에 따라 다르니 실물에서 확인합니다 (확인 필요).

시뮬레이터 3 · 창고 순회 3 바퀴 — 무엇이 오차를 쌓나#

6 × 3 m 직사각형 통로를 0.5 m/s 로 달리고 코너에서 0.5 rad/s 로 제자리 90° 회전합니다 (1 바퀴 = 18 m, 약 48.6 s). 오도메트리는 Δt = 0.02 s, 중간 과정은 2교시 그대로입니다. 검정 = 실제 경로, 파랑 = 오도메트리 경로, 주황 타원 = 같은 설정으로 40 번 돌렸을 때 코너마다 오도메트리 위치의 퍼짐(2σ).

이 시뮬레이터의 오차 모델 (교육용 가정)
  • 바퀴 반지름 오차 e: 실제 반지름 = 설정 × (1 + e). 실제 이동 · 회전이 모두 (1 + e) 배가 되지만 오도메트리는 설정값으로 계산합니다.
  • 회전 슬립 s: 회전하는 동안 실제 차체 회전 = 바퀴로 계산한 회전 × (1 − s).
  • 노이즈 σ_v · σ_ω: 오도메트리가 읽는 vx · ω 에 50 Hz 마다 정규분포 잡음을 더합니다.
  • IMU 로 θ: 오도메트리의 ω 를 바퀴 대신 자이로 값(실제 ω + 바이어스 + 잡음 0.005 rad/s)으로 씁니다.

먼저 예측하세요. 반지름 오차 +2 %, 회전 슬립 5 %, ω 노이즈 0.05 rad/s 중 3 바퀴 뒤 위치 오차가 가장 클 것은? 오차는 바퀴 수에 비례해 커질까요, √ 로 커질까요?

시뮬레이터 3 · 창고 순회 경로 — 실제(검정) vs 오도메트리(파랑) · 불확실성 타원(주황, 2σ)
· ·
하나만 켜기:

해 볼 것

  1. 하나만 켜기 버튼으로 반지름 +2 % · 회전 슬립 5 % · ω 노이즈 0.05 · v 노이즈 0.05 를 차례로 켜고, 3 바퀴 뒤 끝 위치 오차와 최대 오차를 표로 적습니다. 예측과 순위가 같나요?
  2. 회전 슬립 5 % 에서 바퀴별 끝 각도 오차를 봅니다. 한 바퀴에 몇 도씩 늘어나나요? 4 코너 × 90° × 5 % 와 비교합니다. 위치 오차는 바퀴 수에 비례하나요?
  3. ω 노이즈 0.05 와 v 노이즈 0.05 를 번갈아 켜고 다른 난수 를 여러 번 눌러 최대 오차를 비교합니다. 같은 크기의 잡음인데 어느 쪽이 더 크고 자주 벗어나나요? 주황 타원은 코너를 지날수록 어떻게 변하나요?
  4. 슬립 5 % + IMU 를 누릅니다. 오차가 얼마로 줄었나요? 이번에는 반지름 오차 +2 % 를 더해 보세요. IMU 로 고쳐지는 오차와 고쳐지지 않는 오차를 구분합니다.
  5. IMU 를 켠 채로 자이로 바이어스를 0.0002 → 0.002 rad/s 로 올립니다. 각도 오차는 시간에 비례해 커지나요? 바이어스 0.002 rad/s 는 1 시간에 몇 도인지 계산해 봅니다.

관찰 포인트 — 계통 오차(반지름 · 슬립)는 바퀴마다 같은 양씩 쌓여 오차가 바퀴 수에 거의 비례합니다. 회전 슬립 5 % 는 바퀴마다 각도를 18° 씩 틀어 3 바퀴 뒤 54°, 위치 오차는 약 3 m 로 가장 큽니다. 같은 크기의 잡음이라도 ω 쪽이 위치를 훨씬 많이 흔듭니다 — 각도 오차는 그 뒤의 모든 이동에 곱해지기 때문입니다. 불확실성 타원은 시간이 갈수록 커지지만, 계통 오차는 타원에 나타나지 않습니다. 슬립이 있으면 실제 위치가 타원 밖으로 나갑니다. IMU 는 회전 쪽 오차(슬립 · 반지름의 회전 성분)를 크게 줄이지만, 거리 쪽 오차(반지름 +2 % → 6 m 마다 12 cm)와 자이로 바이어스에 의한 느린 드리프트는 남습니다. 그래서 결국 외부 기준(지도 · 라이다) 으로 고쳐 줘야 하고, 그 자리가 4교시의 map → odom 입니다.

확인 문제 3-1

시뮬레이터 3 에서 회전 슬립 5 % 만 켜고 3 바퀴를 돌렸더니 끝 각도 오차가 약 +54° 였습니다. 이 숫자가 나온 이유로 알맞은 것은?

  1. 직선 구간 18 m × 3 바퀴 동안 바퀴가 미끄러져 각도가 돌아갔다
  2. 코너 12 번 × 90° × 5 % = 54° — 오도메트리는 매번 90° 돌았다고 계산하지만 실제로는 85.5° 만 돌았다
  3. 자이로 바이어스가 시간에 비례해 쌓였다
  4. 오일러 적분의 Δt 오차가 코너에서 쌓였다
정답 보기② — 슬립은 회전 중에만 넣었으므로 코너마다 4.5° 씩, 12 번이면 54° 입니다. 오도메트리는 바퀴가 돈 양으로 계산하므로 슬립을 알 수 없고, 그만큼 계속 같은 방향으로 쌓입니다(계통 오차). 이 시뮬레이터는 IMU 를 끈 상태라 ③ 은 해당하지 않고, Δt = 0.02 s · 일정한 ω 라 각도 적분 오차는 없습니다(2교시).
확인 문제 3-2

"슬립 5 % + IMU" 에 반지름 오차 +2 % 를 더했습니다. IMU 를 켜도 남는 오차에 대한 설명으로 알맞은 것은?

  1. 남는 오차가 없다 — IMU 가 위치까지 직접 재기 때문
  2. 각도는 자이로가 실제 회전을 재므로 거의 맞지만, 이동 거리는 여전히 바퀴로 계산하므로 반지름 오차 2 % 만큼(직선 6 m 마다 약 12 cm) 거리 오차가 남는다
  3. 각도 오차가 오히려 54° 보다 커진다
  4. 반지름 오차는 각도에만 영향을 주므로 IMU 로 모두 사라진다
정답 보기② — 자이로는 회전 속도만 잽니다. 위치를 IMU 가속도로 두 번 적분하는 방법은 잡음이 훨씬 빨리 쌓여 바퀴 오도메트리 대신 쓰기 어렵습니다. 그래서 "θ 는 자이로, 거리는 바퀴" 로 나눠 맡기고, 남는 거리 오차와 자이로 바이어스 드리프트는 라이다 · 지도로 고칩니다(ch06 · ch07).

4교시 · ROS 2 의 odom 과 TF#

ROS 2 에서 오도메트리는 nav_msgs/Odometry 메시지(보통 /odom 토픽)와 TF 변환 odom → base_footprint 두 가지로 동시에 나옵니다. 메시지에는 2 · 3교시에서 계산한 것이 모두 들어갑니다.

필드담는 것이번 차시의 대응
header.frame_id자세를 잰 기준 좌표계: odom오도메트리 원점 (출발점)
child_frame_id움직이는 좌표계: base_footprint로봇
pose.pose위치 (x, y, z) + 방향 쿼터니언 (qx, qy, qz, qw)2교시의 (x, y, θ) — θ 는 qz = sin(θ/2), qw = cos(θ/2)
pose.covariance6 × 6 = 36 칸 (x, y, z, roll, pitch, yaw 순서)3교시의 불확실성 타원
twist.twist선속도 (vx, vy) · 각속도 ω — 로봇 기준(child frame)ch02 순기구학 결과
twist.covariance속도의 불확실성 6 × 6엔코더 잡음

평면 로봇이라 z · roll · pitch 는 쓰지 않습니다. 각도는 쿼터니언으로 바꿔 담는다는 것, 그리고 pose 는 월드(odom) 기준, twist 는 로봇 기준 이라는 점을 기억하세요.

Gazebo 설정 읽어 보기#

공식 ROS 2 저장소 agv_pro_ros2 의 Gazebo 시뮬레이션 설정에는 전방향 구동 플러그인(omni drive controller)이 오도메트리를 계산하는 데 쓰는 값이 적혀 있습니다. 이번 차시와 관련된 값만 추리면 다음과 같습니다 (파일 위치 · 태그 이름은 저장소에서 확인).

설정값뜻 · 이번 차시와의 연결
wheel_separation0.36 m좌우 바퀴 간격 = 2 × ly (0.1799 × 2 ≈ 0.36, ch02)
wheel_diameter0.1 m반지름 r = 0.05 m — 3교시의 "반지름 오차" 가 바로 이 값과 실물의 차이
publish_rate50 Hz오도메트리 계산 · 발행 주기 → Δt = 0.02 s (2교시)
직접 찾아 읽기

저장소에서 omni, wheel_separation, publish_rate 로 검색해 보세요. 같은 블록 안에서 오도메트리 좌표계 이름(odom · base_footprint)과 TF 발행 여부를 정하는 설정도 찾을 수 있는지 확인합니다. 메카넘 순기구학에는 앞뒤 거리 lx 도 필요한데, 플러그인이 이 값을 따로 받는지(아니면 다른 가정을 쓰는지)도 함께 확인해 볼 만합니다.

map → odom → base_footprint — 왜 세 단계일까#

map → odom → base_footprint → base_link — 변환마다 발행하는 노드가 다릅니다 map odom base_footprint base_link map → odom 발행: SLAM(ch06) · AMCL(ch07) 뜻: 오도메트리 드리프트 보정값 성질: 정확하지만 가끔 점프 지도에 맞춰 위치를 고칠 때 이 변환만 바뀝니다 odom → base_footprint 발행: 바퀴 오도메트리(드라이버) 또는 EKF(IMU 융합) — 한 곳만 성질: 매끄럽고 연속, 그러나 드리프트 nav_msgs/Odometry 의 pose 와 같은 값 Gazebo: 구동 플러그인이 발행 base_footprint → base_link 발행: robot_state_publisher 뜻: URDF 의 fixed joint 값: (0, 0, 0.020) 고정 (ch01) laser_link 등 센서 링크도 이 아래에 고정으로 붙습니다
myAGV Pro 의 위치 관련 TF 사슬. 변환마다 발행하는 노드가 정해져 있고, 한 좌표계의 부모는 하나뿐입니다.

ROS 의 좌표계 관례(REP 105)는 두 종류의 "월드" 를 둡니다.

  • **odom: 오도메트리 기준. 위치가 연속적이고 매끄럽게 변하지만 시간이 지날수록 틀어집니다(드리프트)**. 짧은 시간의 움직임 · 속도 제어에 씁니다.
  • **map: 지도 기준. 라이다를 지도에 맞춰 구하므로 오래 지나도 틀어지지 않지만, 고칠 때마다 위치가 뚝 점프** 할 수 있습니다.

SLAM · AMCL 은 "지도에서 로봇은 여기" (map → base_footprint)를 계산합니다. 그런데 이 값을 그대로 발행하면 base_footprint 에 부모가 둘(odom, map) 생기고 TF 트리 규칙이 깨집니다. 그래서 차이만 계산해 **map → odom** 으로 발행합니다.

text
T_map_odom = T_map_base · inv(T_odom_base)     ← SLAM / AMCL 이 계산해 발행
T_map_base = T_map_odom · T_odom_base           ← 다른 노드가 TF 로 조회할 때

결과적으로 드리프트 보정은 모두 map → odom 에 들어가고, odom → base_footprint 는 끝까지 매끄럽게 유지됩니다.

시뮬레이터 4 · TF 패널 — map 과 odom 이 벌어지는 모습#

로봇이 5 × 2.5 m 경로를 계속 돕니다. 오도메트리에는 각도 드리프트와 거리 오차가 있고, 위치 추정(SLAM · AMCL 역할)은 정해진 주기마다 실제 위치를 약간의 잡음과 함께 알려 줍니다. 지도 위: 회색 점선 상자 = 실제 로봇, 파란 상자 = TF 로 합성한 로봇 위치(map → odom → base_footprint), **주황 축 = odom 좌표계의 원점과 방향**.

먼저 예측하세요. 위치 추정을 끄고 2 분을 돌리면 map → odom 값은 어떻게 변할까요? 위치 추정을 켜면 odom → base_footprint 값은 점프할까요?

시뮬레이터 4 · map → odom → base_footprint TF 패널 · 아래 그래프: 최근 40 s
· 배속 ·

해 볼 것

  1. 위치 추정을 끈 채 ×10 배속으로 2 분(t ≈ 120 s)쯤 돌립니다. 주황 odom 축은 움직이나요? 파란 상자(TF 위치)와 회색 점선(실제)의 차이는 어떻게 커지나요? 아래 첫 그래프(odom → base_footprint 의 x)는 매끄러운가요?
  2. 위치 추정 켜기 를 체크합니다. 파란 상자가 실제 위치로 돌아오는 순간, TF 트리에서 바뀐 변환은 어느 것 인가요? 둘째 그래프(map → odom 의 x)의 모양은?
  3. 위치 추정 주기를 1 s 와 10 s 로 바꿔 봅니다. 주기가 길면 "TF 위치와 실제의 차이" 가 어떤 톱니 모양으로 바뀌나요?
  4. 보정을 넣는 곳을 odom → base_footprint 덮어쓰기 (잘못된 설계) 로 바꿉니다. 셋째 그래프(odom 으로 잰 속도)에 무엇이 생기나요? 이 속도를 보고 속도를 조절하는 제어기가 있다면 로봇은 어떻게 반응할까요?
  5. 드리프트를 0 °/분 · 거리 오차 0 % 로 두면 위치 추정을 켜도 map → odom 은 거의 변하지 않는지 확인합니다. 남는 작은 흔들림은 무엇 때문인가요?

관찰 포인트 — 위치 추정이 꺼져 있으면 map → odom 은 처음 값(출발 위치) 그대로이고, 오도메트리 드리프트가 그대로 지도 위 위치 오차가 됩니다. 켜면 **map → odom 만 계단처럼 점프** 하고 odom → base_footprint 는 끝까지 매끄럽습니다. odom 의 원점이 지도 위에서 조금씩 옮겨 다니는 것은 "오도메트리가 틀어진 만큼 odom 좌표계 자체를 옮겨 맞추는" 것입니다. 보정을 odom → base_footprint 에 직접 넣으면 위치가 점프해 속도가 순간적으로 수 m/s 로 튀는 가짜 신호가 생깁니다 — 그래서 매끄러움이 필요한 제어는 odom 을, 정확함이 필요한 목적지 계산은 map 을 씁니다.

확인 문제 4-1

시뮬레이터 4 에서 위치 추정을 켜고 "map → odom (REP 105 방식)" 으로 두었습니다. 3 s 마다 보정이 일어날 때 아래 그래프에 보이는 모습으로 알맞은 것은?

  1. 첫째 그래프(odom → base_footprint 의 x)가 3 s 마다 계단처럼 점프하고, 둘째(map → odom)는 0 에 머문다
  2. 첫째는 매끄럽고, 둘째(map → odom 의 x)가 3 s 마다 작은 계단처럼 바뀐다. 셋째(속도)에는 가시가 없다
  3. 세 그래프 모두 매끄럽다 — 보정은 TF 에 나타나지 않는다
  4. 셋째 그래프에 3 s 마다 가시가 생긴다
정답 보기② — 보정값은 T_map_odom = T_map_base · inv(T_odom_base) 로 계산되어 map → odom 에만 들어갑니다. odom → base_footprint 는 바퀴 적분 그대로라 매끄럽고, 그 위치 변화로 잰 속도도 0.5 m/s 근처에 머뭅니다. ① · ④ 는 "덮어쓰기(잘못된 설계)" 를 골랐을 때의 모습입니다.
확인 문제 4-2

myAGV Pro 를 ROS 2 로 운용할 때 각 변환을 발행하는 노드의 짝으로 알맞은 것은? (SLAM · AMCL 은 뒤 차시에서 다룹니다)

  1. map → odom: 바퀴 오도메트리 드라이버 / odom → base_footprint: AMCL
  2. map → odom: robot_state_publisher / base_footprint → base_link: SLAM
  3. map → odom: SLAM 또는 AMCL (한 번에 하나) / odom → base_footprint: 바퀴 오도메트리 드라이버(또는 IMU 를 섞는 EKF 중 하나) / base_footprint → base_link: robot_state_publisher
  4. 세 변환 모두 바퀴 오도메트리 드라이버
정답 보기③ — 지도와 맞춰 보는 노드(SLAM · AMCL)가 map → odom, 바퀴 · IMU 를 적분하는 노드가 odom → base_footprint, URDF 의 fixed joint 는 robot_state_publisher 가 발행합니다. 같은 변환을 두 노드가 동시에 발행하면(예: 드라이버와 EKF 가 모두 odom → base_footprint) 로봇 위치가 두 값 사이를 오가며 떨립니다 (자주 나는 오류 5).

실습 (시뮬레이터)#

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

Playground 열기

실습 1-1 · 회전 행렬과 동차 변환 만들기 (1교시)

R(θ) 와 T(x, y, θ) 를 numpy 로 만들고, 로봇 기준 점을 월드 좌표로 바꿉니다. 확인 문제 1-2 의 답 (2.0, 0.5) 이 나와야 합니다.

python시뮬 실행 가능
import numpy as np

def R(th):
    # TODO 1: 2x2 회전 행렬 [[cos, -sin], [sin, cos]]
    return np.eye(2)

def T(x, y, th):
    # TODO 2: 3x3 동차 변환 - 왼쪽 위 2x2 는 R(th), 오른쪽 열은 (x, y)
    return np.eye(3)

def to_world(pose, p_r):
    # TODO 3: [p_r; 1] 을 만들어 T 를 곱하고 앞 두 칸만 돌려주기
    return np.array(p_r, dtype=float)

pose = (3.0, 1.0, np.radians(180))
print("pallet  :", np.round(to_world(pose, (1.0, 0.5)), 3))      # 기대: [2.  0.5]
print("lidar   :", np.round(to_world(pose, (0.179, 0.0)), 3))    # 라이다 위치 (ch01)
for deg in (0, 90, 180, -90):
    print(f"theta={deg:4d}  front 1 m ->", np.round(to_world((0, 0, np.radians(deg)), (1, 0)), 3))
풀이
python시뮬 실행 가능
import numpy as np

def R(th):
    c, s = np.cos(th), np.sin(th)
    return np.array([[c, -s], [s, c]])

def T(x, y, th):
    M = np.eye(3)
    M[:2, :2] = R(th)
    M[:2, 2] = (x, y)
    return M

def to_world(pose, p_r):
    return (T(*pose) @ np.array([p_r[0], p_r[1], 1.0]))[:2]

pose = (3.0, 1.0, np.radians(180))
print("pallet  :", np.round(to_world(pose, (1.0, 0.5)), 3))
print("lidar   :", np.round(to_world(pose, (0.179, 0.0)), 3))
for deg in (0, 90, 180, -90):
    print(f"theta={deg:4d}  front 1 m ->", np.round(to_world((0, 0, np.radians(deg)), (1, 0)), 3))

# 성질 확인: R 은 길이를 바꾸지 않고, R 의 역행렬은 전치
th = np.radians(37)
print("det R =", round(np.linalg.det(R(th)), 6), "| R R^T = I ?", np.allclose(R(th) @ R(th).T, np.eye(2)))

np.round(..., 3) 으로 반올림하지 않으면 cos(90°) 가 6.1e-17 처럼 0 이 아닌 아주 작은 수로 보입니다. 부동소수점 오차라서 정상입니다.

실습 1-2 · 명령의 월드 방향 · 변환 이어 붙이기 · 거꾸로 (1교시)

(1) θ = 90° 에서 네 이동 명령의 월드 속도를 계산해 계획의 실습 질문("move_left_lateral 은 월드에서 어느 방향?")에 답합니다. (2) T_world_robot · T_robot_lidar 로 라이다의 월드 자세를 구하고, (3) inv(T) 로 월드의 목표점을 로봇 기준으로 되돌려 "목표가 로봇의 앞/옆 어디에 있는지" 를 알아냅니다.

python시뮬 실행 가능
import numpy as np

def T(x, y, th):
    c, s = np.cos(th), np.sin(th)
    return np.array([[c, -s, x], [s, c, y], [0, 0, 1.0]])

def T_inv(M):
    # TODO 1: 회전 부분은 R^T, 이동 부분은 -R^T @ t
    return np.eye(3)

def world_vel(th, vx, vy):
    # TODO 2: R(th) @ (vx, vy)
    return np.array([vx, vy])

CMDS = {"move_forward": (0.5, 0), "move_backward": (-0.5, 0),
        "move_left_lateral": (0, 0.5), "move_right_lateral": (0, -0.5)}
for name, (vx, vy) in CMDS.items():
    print(f"theta=90  {name:19s} ->", np.round(world_vel(np.radians(90), vx, vy), 3))

T_w_r = T(3.0, 2.0, np.radians(90))          # 로봇 자세
T_r_l = T(0.179, 0.0, 0.0)                    # base -> laser (ch01, 높이 제외)
print("lidar in world:", np.round(T_w_r @ T_r_l @ [0, 0, 1], 3))

goal_w = np.array([2.0, 2.5, 1.0])            # 월드의 목표점
goal_r = T_inv(T_w_r) @ goal_w
print("goal in robot frame:", np.round(goal_r[:2], 3), "(+x 앞, +y 왼쪽)")
풀이
python시뮬 실행 가능
import numpy as np

def T(x, y, th):
    c, s = np.cos(th), np.sin(th)
    return np.array([[c, -s, x], [s, c, y], [0, 0, 1.0]])

def T_inv(M):
    Rt = M[:2, :2].T
    out = np.eye(3)
    out[:2, :2] = Rt
    out[:2, 2] = -Rt @ M[:2, 2]
    return out

def world_vel(th, vx, vy):
    c, s = np.cos(th), np.sin(th)
    return np.array([vx * c - vy * s, vx * s + vy * c])

CMDS = {"move_forward": (0.5, 0), "move_backward": (-0.5, 0),
        "move_left_lateral": (0, 0.5), "move_right_lateral": (0, -0.5)}
for name, (vx, vy) in CMDS.items():
    print(f"theta=90  {name:19s} ->", np.round(world_vel(np.radians(90), vx, vy), 3))

T_w_r = T(3.0, 2.0, np.radians(90))
T_r_l = T(0.179, 0.0, 0.0)
print("lidar in world:", np.round(T_w_r @ T_r_l @ [0, 0, 1], 3))

goal_w = np.array([2.0, 2.5, 1.0])
goal_r = T_inv(T_w_r) @ goal_w
print("goal in robot frame:", np.round(goal_r[:2], 3), "(+x 앞, +y 왼쪽)")
print("check inv:", np.allclose(T_inv(T_w_r) @ T_w_r, np.eye(3)))

θ = 90° 에서 move_left_lateral 은 (-0.5, 0), 즉 월드 -X 방향(지도 왼쪽) 입니다. 목표점은 로봇 기준 (0.5, 1.0) — 앞으로 0.5 m, 왼쪽으로 1.0 m 에 있으니 메카넘 로봇은 회전 없이 대각선으로 갈 수 있습니다.

실습 2-1 · 바퀴 속도 → 오도메트리 함수 완성 (2교시)

ch02 의 순기구학으로 바퀴 4개의 각속도를 (vx, vy, ω) 로 바꾸고, 2교시의 오일러 적분으로 자세를 갱신하는 Odometry 클래스를 완성합니다. 테스트용 바퀴 속도는 역기구학으로 만들어 두었습니다. "앞 1 m → 왼쪽 0.5 m → 90° 회전" 뒤 (1.00, 0.50, 90°) 가 나와야 합니다 (ch03 실습 1-1 과 같은 경로).

python시뮬 실행 가능
import math

R_W, K = 0.05, 0.3514            # 바퀴 반지름, lx + ly (ch02)

def ik(vx, vy, om):              # 테스트 데이터 생성용 (ch02 역기구학)
    return [(vx - vy - K * om) / R_W, (vx + vy + K * om) / R_W,
            (vx + vy - K * om) / R_W, (vx - vy + K * om) / R_W]   # FL, FR, RL, RR

def fk(w):
    fl, fr, rl, rr = w
    # TODO 1: ch02 순기구학 (vx, vy, om)
    return 0.0, 0.0, 0.0

class Odometry:
    def __init__(self):
        self.x = self.y = self.th = 0.0
    def update(self, wheels, dt):
        vx, vy, om = fk(wheels)
        # TODO 2: 월드 속도로 돌려서 x, y 적분 (1교시 식)
        # TODO 3: th 적분 후 -pi ~ pi 로 감기 (atan2)
        pass
    def pose(self):
        return round(self.x, 3), round(self.y, 3), round(math.degrees(self.th), 1)

odo, DT = Odometry(), 0.02
plan = [((0.5, 0, 0), 2.0), ((0, 0.25, 0), 2.0), ((0, 0, 0.5), math.pi)]
for (vx, vy, om), sec in plan:
    for _ in range(round(sec / DT)):
        odo.update(ik(vx, vy, om), DT)
    print(f"after cmd {vx, vy, om} for {sec:.2f}s ->", odo.pose())
풀이
python시뮬 실행 가능
import math

R_W, K = 0.05, 0.3514

def ik(vx, vy, om):
    return [(vx - vy - K * om) / R_W, (vx + vy + K * om) / R_W,
            (vx + vy - K * om) / R_W, (vx - vy + K * om) / R_W]

def fk(w):
    fl, fr, rl, rr = w
    vx = R_W / 4 * (fl + fr + rl + rr)
    vy = R_W / 4 * (-fl + fr + rl - rr)
    om = R_W / (4 * K) * (-fl + fr - rl + rr)
    return vx, vy, om

class Odometry:
    def __init__(self):
        self.x = self.y = self.th = 0.0
    def update(self, wheels, dt):
        vx, vy, om = fk(wheels)
        c, s = math.cos(self.th), math.sin(self.th)
        self.x += (vx * c - vy * s) * dt
        self.y += (vx * s + vy * c) * dt
        self.th = math.atan2(math.sin(self.th + om * dt), math.cos(self.th + om * dt))
    def pose(self):
        return round(self.x, 3), round(self.y, 3), round(math.degrees(self.th), 1)

odo, DT = Odometry(), 0.02
plan = [((0.5, 0, 0), 2.0), ((0, 0.25, 0), 2.0), ((0, 0, 0.5), math.pi)]
for (vx, vy, om), sec in plan:
    for _ in range(round(sec / DT)):
        odo.update(ik(vx, vy, om), DT)
    print(f"after cmd {vx, vy, om} for {sec:.2f}s ->", odo.pose())

# 같은 경로를 순서만 바꿔서: 먼저 90 도 돌고 '왼쪽' 으로 가면?
odo2 = Odometry()
for (vx, vy, om), sec in [((0, 0, 0.5), math.pi), ((0, 0.25, 0), 2.0)]:
    for _ in range(round(sec / DT)):
        odo2.update(ik(vx, vy, om), DT)
print("turn first, then left 0.5 m ->", odo2.pose())

마지막 줄은 θ = 90° 에서 move_left_lateral 이 월드 -X 로 간다는 1교시 결과를 오도메트리로 다시 확인합니다 ((-0.5, 0, 90°)). 회전 단계는 π / 0.02 = 157.08 걸음인데 round 로 157 걸음만 돌므로 실제로는 89.95° 입니다 (소수 한 자리로 반올림해 90.0° 로 보임). 이것도 "Δt 로 끊어 세는" 오도메트리의 성질입니다.

실습 2-2 · 반지름 1 m 원을 Δt = 0.02 / 0.2 / 0.5 s 로 적분 (2교시)

계획의 실습 그대로입니다. 오일러 · 중점 · 정확(원호) 적분으로 한 바퀴를 돌며 매 샘플의 위치 오차를 재고, 최대 오차 와 마지막 오차 를 표로 비교합니다. 시뮬레이터 2 의 숫자와 같은지 확인하세요.

python시뮬 실행 가능
import math

V, RAD = 0.5, 1.0
OM = V / RAD
T_LAP = 2 * math.pi / OM

def truth(t):
    a = OM * t
    return RAD * math.sin(a), RAD * (1 - math.cos(a))

def integrate(dt, method):
    x = y = th = 0.0
    n = int(T_LAP / dt + 1e-9)
    max_err = 0.0
    for k in range(1, n + 1):
        if method == "euler":
            # TODO 1: 오일러 - 시작 방향 th 로 V*dt 직진
            pass
        elif method == "mid":
            # TODO 2: 중점 - 방향 th + OM*dt/2 로 V*dt 직진
            pass
        th += OM * dt
        tx, ty = truth(k * dt)
        err = math.hypot(x - tx, y - ty)
        max_err = max(max_err, err)
    return max_err, err

print("dt     method   max_err(cm)  last_err(cm)")
for dt in (0.02, 0.2, 0.5):
    for m in ("euler", "mid"):
        mx, last = integrate(dt, m)
        print(f"{dt:<6} {m:8} {mx * 100:10.3f}  {last * 100:10.3f}")
풀이
python시뮬 실행 가능
import math

V, RAD = 0.5, 1.0
OM = V / RAD
T_LAP = 2 * math.pi / OM

def truth(t):
    a = OM * t
    return RAD * math.sin(a), RAD * (1 - math.cos(a))

def integrate(dt, method):
    x = y = th = 0.0
    n = int(T_LAP / dt + 1e-9)
    max_err = 0.0
    for k in range(1, n + 1):
        if method == "euler":
            x += V * math.cos(th) * dt
            y += V * math.sin(th) * dt
        elif method == "mid":
            a = th + OM * dt / 2
            x += V * math.cos(a) * dt
            y += V * math.sin(a) * dt
        else:  # exact: 한 걸음 동안 속도가 일정하면 원호를 그대로 계산
            x += V / OM * (math.sin(th + OM * dt) - math.sin(th))
            y += -V / OM * (math.cos(th + OM * dt) - math.cos(th))
        th += OM * dt
        tx, ty = truth(k * dt)
        err = math.hypot(x - tx, y - ty)
        max_err = max(max_err, err)
    return max_err, err

print("dt     method   max_err(cm)  last_err(cm)")
for dt in (0.02, 0.2, 0.5):
    for m in ("euler", "mid", "exact"):
        mx, last = integrate(dt, m)
        print(f"{dt:<6} {m:8} {mx * 100:10.3f}  {last * 100:10.3f}")
print("euler 이론값 v*dt (cm):", [round(V * d * 100, 1) for d in (0.02, 0.2, 0.5)])

오일러의 최대 오차는 1.0 / 10.0 / 25.0 cm 로 v·Δt 와 같고(Δt 에 비례), 중점은 0.0 / 0.08 / 0.52 cm 로 Δt² 에 비례합니다. "exact" 는 속도가 일정한 이 문제에서 반올림 오차 수준(0)입니다. 마지막 오차는 셋 다 작습니다 — 끝점 하나만 보면 오일러의 25 cm 오차를 놓칩니다.

실습 3-1 · 창고 3 바퀴 — 오차의 주범 찾기 (3교시)

계획의 실습입니다. 시뮬레이터 3 과 같은 모델로 오차 원인을 하나씩만 켜고 3 바퀴를 돌린 뒤, 바퀴마다 끝 위치 오차와 전체 최대 오차를 비교해 가장 큰 원인을 찾습니다. 랜덤 오차는 난수 30 개의 평균을 냅니다.

python시뮬 실행 가능
import math
import numpy as np

DT = 0.02

def route(laps):
    u = []
    for _ in range(laps):
        for L in (6.0, 3.0, 6.0, 3.0):
            u += [(0.5, 0.0)] * round(L / 0.5 / DT)
            u += [(0.0, 0.5)] * round(math.pi / 2 / 0.5 / DT)
    return u

def run(laps=3, er=0.0, slip=0.0, sw=0.0, sv=0.0, imu=False, bias=0.0, seed=0):
    rng = np.random.default_rng(seed)
    U = route(laps)
    xt, yt, tt = 1.0, 1.0, 0.0       # 실제
    xo, yo, to = 1.0, 1.0, 0.0       # 오도메트리
    lap_n, lap_err, mx = len(U) // laps, [], 0.0
    for k, (v, w) in enumerate(U):
        vr = v * (1 + er)
        wr = w * (1 + er) * (1 - slip)
        # TODO 1: 오도메트리가 읽는 속도 vo, wo 만들기
        #   vo = v + sv * 정규난수,  wo = (IMU 면) wr + bias + 0.005*정규난수, (아니면) w + sw*정규난수
        vo, wo = v, w
        xt += vr * math.cos(tt) * DT; yt += vr * math.sin(tt) * DT; tt += wr * DT
        xo += vo * math.cos(to) * DT; yo += vo * math.sin(to) * DT; to += wo * DT
        e = math.hypot(xo - xt, yo - yt)
        mx = max(mx, e)
        if (k + 1) % lap_n == 0:
            lap_err.append(e)
    return lap_err, mx

cases = {
    "radius +2%": dict(er=0.02),
    "turn slip 5%": dict(slip=0.05),
    "omega noise 0.05": dict(sw=0.05),
    "v noise 0.05": dict(sv=0.05),
}
for name, kw in cases.items():
    # TODO 2: 랜덤 오차(sw, sv)가 있으면 seed 0~29 로 30 번 돌려 평균, 아니면 1 번
    laps, mx = run(**kw)
    print(f"{name:18s} lap ends (m): {np.round(laps, 3)}  max: {mx:.3f}")
풀이
python시뮬 실행 가능
import math
import numpy as np

DT = 0.02

def route(laps):
    u = []
    for _ in range(laps):
        for L in (6.0, 3.0, 6.0, 3.0):
            u += [(0.5, 0.0)] * round(L / 0.5 / DT)
            u += [(0.0, 0.5)] * round(math.pi / 2 / 0.5 / DT)
    return u

def run(laps=3, er=0.0, slip=0.0, sw=0.0, sv=0.0, imu=False, bias=0.0, seed=0):
    rng = np.random.default_rng(seed)
    U = route(laps)
    xt, yt, tt = 1.0, 1.0, 0.0
    xo, yo, to = 1.0, 1.0, 0.0
    lap_n, lap_err, mx = len(U) // laps, [], 0.0
    for k, (v, w) in enumerate(U):
        vr = v * (1 + er)
        wr = w * (1 + er) * (1 - slip)
        vo = v + sv * rng.standard_normal()
        wo = (wr + bias + 0.005 * rng.standard_normal()) if imu else (w + sw * rng.standard_normal())
        xt += vr * math.cos(tt) * DT; yt += vr * math.sin(tt) * DT; tt += wr * DT
        xo += vo * math.cos(to) * DT; yo += vo * math.sin(to) * DT; to += wo * DT
        e = math.hypot(xo - xt, yo - yt)
        mx = max(mx, e)
        if (k + 1) % lap_n == 0:
            lap_err.append(e)
    return lap_err, mx

def avg(n=30, **kw):
    rs = [run(seed=s, **kw) for s in range(n)]
    return np.mean([r[0] for r in rs], axis=0), np.mean([r[1] for r in rs])

cases = {
    "radius +2%": dict(er=0.02),
    "turn slip 5%": dict(slip=0.05),
    "omega noise 0.05": dict(sw=0.05),
    "v noise 0.05": dict(sv=0.05),
    "slip 5% + IMU": dict(slip=0.05, imu=True, bias=0.0002),
    "radius +2% + IMU": dict(er=0.02, imu=True, bias=0.0002),
}
result = {}
for name, kw in cases.items():
    noisy = kw.get("sw", 0) or kw.get("sv", 0) or kw.get("imu", False)
    laps, mx = avg(**kw) if noisy else run(**kw)
    result[name] = mx
    print(f"{name:18s} lap ends (m): {np.round(laps, 3)}  max: {mx:.3f}")
worst = max(list(result)[:4], key=result.get)
print("biggest source (without IMU):", worst)

회전 슬립 5 % 가 3 바퀴 뒤 약 3.1 m 로 압도적입니다. 계통 오차라서 바퀴마다 약 1 m 씩 일정하게 늘어납니다. ω 노이즈(최대 약 0.4 m)는 같은 크기의 v 노이즈(약 0.1 m)보다 4 배쯤 큽니다. IMU 를 켜면 슬립 오차가 약 0.1 m 로 줄지만, 반지름 오차의 거리 성분은 남습니다.

실습 3-2 · 각도 오차 vs 거리 오차 — 직선 20 m 에서 퍼짐 재기 (3교시)

(1) 각도 1° 오차와 거리 1 % 오차가 5 · 10 · 20 m 뒤 만드는 위치 오차를 계산합니다. (2) 같은 크기의 랜덤 잡음(σ = 0.05)을 v 또는 ω 에 넣고 직선 20 m 를 500 번 돌려, 거리별 옆 방향(y) 퍼짐의 표준편차 를 비교합니다. 퍼짐이 거리에 대해 어떤 꼴로 커지는지 보세요.

python시뮬 실행 가능
import numpy as np

# (1) 계통 오차
for d in (5, 10, 20):
    lat = 0.0      # TODO 1: 각도 1 도 오차 -> 옆 방향 오차 d * sin(1 deg)
    lon = 0.0      # TODO 2: 거리 1 % 오차 -> d * 0.01
    print(f"d={d:2d} m  heading 1deg -> {lat * 100:5.1f} cm   distance 1% -> {lon * 100:5.1f} cm")

# (2) 랜덤 오차의 퍼짐 (몬테카를로)
DT, V, N = 0.02, 0.5, 500
steps = int(20 / V / DT)
rng = np.random.default_rng(0)

def spread(sv, sw):
    x = np.zeros(N); y = np.zeros(N); th = np.zeros(N)
    out = {}
    for k in range(1, steps + 1):
        v = V + sv * rng.standard_normal(N)
        w = 0 + sw * rng.standard_normal(N)
        # TODO 3: N 개 로봇을 한꺼번에 오일러 적분 (x, y, th 배열)
        d = k * V * DT
        if abs(d - round(d)) < 1e-9 and round(d) in (5, 10, 20):
            out[round(d)] = y.std()
    return out

print("v noise 0.05    :", {d: round(float(s) * 100, 1) for d, s in spread(0.05, 0).items()}, "cm (y std)")
print("omega noise 0.05:", {d: round(float(s) * 100, 1) for d, s in spread(0, 0.05).items()}, "cm (y std)")
풀이
python시뮬 실행 가능
import numpy as np

for d in (5, 10, 20):
    lat = d * np.sin(np.radians(1))
    lon = d * 0.01
    print(f"d={d:2d} m  heading 1deg -> {lat * 100:5.1f} cm   distance 1% -> {lon * 100:5.1f} cm")

DT, V, N = 0.02, 0.5, 500
steps = int(20 / V / DT)
rng = np.random.default_rng(0)

def spread(sv, sw):
    x = np.zeros(N); y = np.zeros(N); th = np.zeros(N)
    out = {}
    for k in range(1, steps + 1):
        v = V + sv * rng.standard_normal(N)
        w = 0 + sw * rng.standard_normal(N)
        x += v * np.cos(th) * DT
        y += v * np.sin(th) * DT
        th += w * DT
        d = k * V * DT
        if abs(d - round(d)) < 1e-9 and round(d) in (5, 10, 20):
            out[round(d)] = y.std()
    return out

sv = spread(0.05, 0)
sw = spread(0, 0.05)
print("v noise 0.05    :", {d: round(float(s) * 100, 1) for d, s in sv.items()}, "cm (y std)")
print("omega noise 0.05:", {d: round(float(s) * 100, 1) for d, s in sw.items()}, "cm (y std)")
print("omega: 10m->20m ratio =", round(sw[20] / sw[10], 2), "(2^1.5 = 2.83)")

v 잡음은 진행 방향(x)으로만 퍼지므로 옆(y) 퍼짐이 0 입니다. ω 잡음은 각도가 √시간 꼴로 흔들리고, 그 각도가 남은 거리 전체에 곱해져 옆 퍼짐이 거리의 약 1.5 제곱으로 커집니다 (10 m → 20 m 에서 약 2.8 배). 이것이 불확실성 타원이 진행 방향보다 옆으로 길쭉해지는 이유입니다.

실습 4-1 · map → odom 보정값 계산 (4교시)

위치 추정(AMCL 역할)이 "지도에서 로봇은 (4.10, 2.05, 92°)" 라고 알려 왔고, 같은 순간 오도메트리는 odom → base_footprint = (2.70, 0.85, 97°) 입니다. 처음 map → odom 은 출발 위치 (1.5, 1.0, 0°) 였습니다. T_map_odom = T_map_base · inv(T_odom_base) 로 새 보정값을 구하고, 보정 전후로 TF 합성 위치가 어떻게 바뀌는지 출력합니다.

python시뮬 실행 가능
import numpy as np

def T(x, y, deg):
    th = np.radians(deg); c, s = np.cos(th), np.sin(th)
    return np.array([[c, -s, x], [s, c, y], [0, 0, 1.0]])

def pose(M):
    return np.round([M[0, 2], M[1, 2], np.degrees(np.arctan2(M[1, 0], M[0, 0]))], 3)

T_map_odom_old = T(1.5, 1.0, 0)
T_odom_base = T(2.70, 0.85, 97)
T_map_base_est = T(4.10, 2.05, 92)          # 위치 추정 결과

print("before: map->base (TF) =", pose(T_map_odom_old @ T_odom_base))
# TODO 1: 새 map->odom 계산
T_map_odom_new = T_map_odom_old
print("new map->odom          =", pose(T_map_odom_new))
# TODO 2: 새 map->odom 과 그대로인 odom->base 로 합성한 map->base 가 추정값과 같은지 확인
print("after : map->base (TF) =", pose(T_map_odom_new @ T_odom_base))
print("odom->base unchanged   =", pose(T_odom_base))
풀이
python시뮬 실행 가능
import numpy as np

def T(x, y, deg):
    th = np.radians(deg); c, s = np.cos(th), np.sin(th)
    return np.array([[c, -s, x], [s, c, y], [0, 0, 1.0]])

def pose(M):
    return np.round([M[0, 2], M[1, 2], np.degrees(np.arctan2(M[1, 0], M[0, 0]))], 3)

T_map_odom_old = T(1.5, 1.0, 0)
T_odom_base = T(2.70, 0.85, 97)
T_map_base_est = T(4.10, 2.05, 92)

print("before: map->base (TF) =", pose(T_map_odom_old @ T_odom_base))
T_map_odom_new = T_map_base_est @ np.linalg.inv(T_odom_base)
print("new map->odom          =", pose(T_map_odom_new))
print("after : map->base (TF) =", pose(T_map_odom_new @ T_odom_base))
print("odom->base unchanged   =", pose(T_odom_base))

# 보정 뒤 로봇이 오도메트리로 앞으로 1 m 더 가면?
T_odom_base2 = T_odom_base @ T(1.0, 0, 0)
print("1 m later map->base    =", pose(T_map_odom_new @ T_odom_base2))

보정 전 TF 위치는 (4.2, 1.85, 97°) 로 추정값과 약 22 cm · 5° 어긋나 있습니다. 새 map → odom 은 약 (1.34, 1.44, -5°) — 오도메트리가 5° 더 돈 것으로 착각한 만큼 odom 좌표계 자체를 -5° 돌리고 옮겨 맞춘 값입니다. 보정 뒤 합성 위치는 추정값 (4.10, 2.05, 92°) 와 정확히 같아집니다. odom → base_footprint 는 손대지 않았으므로 오도메트리를 쓰는 제어는 점프를 겪지 않습니다.

실습 4-2 · Odometry 메시지와 TF 트리를 직접 만들기 (4교시)

계획의 실습("TF 트리를 그리고 각 변환을 누가 발행하는지 적기")을 코드로 합니다. (1) (x, y, θ, vx, vy, ω) 와 분산으로 nav_msgs/Odometry 모양의 딕셔너리를 만들고, (2) 변환 목록에서 부모가 둘인 좌표계 를 찾아내고 map 에서 laser_link 까지의 사슬과 발행 노드를 출력합니다.

python시뮬 실행 가능
import math

def make_odom(x, y, th, vx, vy, om, var_xy=0.01, var_th=0.003):
    cov = [0.0] * 36
    # TODO 1: 대각선 칸 (0,0) (1,1) 에 var_xy, (5,5) 에 var_th  -> 인덱스 = 행*6 + 열
    return {
        "header": {"frame_id": "odom"},
        "child_frame_id": "base_footprint",
        "pose": {"position": (x, y, 0.0),
                 "orientation": (0.0, 0.0, 0.0, 1.0),   # TODO 2: yaw -> (0, 0, sin(th/2), cos(th/2))
                 "covariance": cov},
        "twist": {"linear": (vx, vy, 0.0), "angular": (0.0, 0.0, om)},
    }

msg = make_odom(2.7, 0.85, math.radians(97), 0.5, 0.0, 0.0)
print("orientation:", [round(q, 4) for q in msg["pose"]["orientation"]])
print("cov diag   :", [msg["pose"]["covariance"][i * 7] for i in range(6)])

# (child, parent, publisher)
TF = [
    ("odom", "map", "slam_toolbox or AMCL"),
    ("base_footprint", "odom", "wheel odometry driver"),
    ("base_footprint", "odom", "EKF (robot_localization)"),
    ("base_link", "base_footprint", "robot_state_publisher"),
    ("laser_link", "base_link", "robot_state_publisher"),
    ("imu_link", "base_link", "robot_state_publisher"),
]
# TODO 3: child 별로 발행자를 모아 2 개 이상이면 경고 출력
# TODO 4: laser_link 에서 parent 를 따라 map 까지 올라가며 사슬 출력
풀이
python시뮬 실행 가능
import math

def make_odom(x, y, th, vx, vy, om, var_xy=0.01, var_th=0.003):
    cov = [0.0] * 36
    cov[0 * 6 + 0] = var_xy
    cov[1 * 6 + 1] = var_xy
    cov[5 * 6 + 5] = var_th
    return {
        "header": {"frame_id": "odom"},
        "child_frame_id": "base_footprint",
        "pose": {"position": (x, y, 0.0),
                 "orientation": (0.0, 0.0, math.sin(th / 2), math.cos(th / 2)),
                 "covariance": cov},
        "twist": {"linear": (vx, vy, 0.0), "angular": (0.0, 0.0, om)},
    }

msg = make_odom(2.7, 0.85, math.radians(97), 0.5, 0.0, 0.0)
q = msg["pose"]["orientation"]
print("orientation:", [round(v, 4) for v in q])
print("yaw back   :", round(math.degrees(2 * math.atan2(q[2], q[3])), 1), "deg")
print("cov diag   :", [msg["pose"]["covariance"][i * 7] for i in range(6)])
print("1-sigma xy :", round(math.sqrt(0.01) * 100, 1), "cm,  yaw:", round(math.degrees(math.sqrt(0.003)), 1), "deg")

TF = [
    ("odom", "map", "slam_toolbox or AMCL"),
    ("base_footprint", "odom", "wheel odometry driver"),
    ("base_footprint", "odom", "EKF (robot_localization)"),
    ("base_link", "base_footprint", "robot_state_publisher"),
    ("laser_link", "base_link", "robot_state_publisher"),
    ("imu_link", "base_link", "robot_state_publisher"),
]
pubs, parent = {}, {}
for child, par, who in TF:
    pubs.setdefault(child, []).append(who)
    parent[child] = par
for child, who in pubs.items():
    if len(who) > 1:
        print(f"WARNING: {child} has {len(who)} publishers -> {who}  (keep only one)")

frame, chain = "laser_link", []
while frame in parent:
    who = pubs[frame][0]
    chain.append(f"{parent[frame]} -> {frame}  [{who}]")
    frame = parent[frame]
for line in reversed(chain):
    print(line)

base_footprint 의 발행자가 둘(드라이버 · EKF)이라는 경고가 나옵니다. EKF 를 쓰면 드라이버의 TF 발행을 끄고 /odom 메시지만 EKF 입력으로 넘깁니다. 공분산 0.01 m² 는 표준편차 10 cm, 0.003 rad² 는 약 3.1° 입니다.

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

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

Playground (시뮬레이터 · 빈 바닥) 열기

실습 5-1 · 오도메트리로 1 m 사각형을 닫으면 (2 · 3교시 복습)

오도메트리 값을 되먹임으로 써서 "1 m 전진 → 왼쪽 90°" 를 네 번 합니다. 미끄러짐 0 % 와 10 % 에서 마지막 위치를 비교합니다.

먼저 예측하세요. 미끄러짐 10 % 에서 로봇은 스스로 사각형을 잘 닫았다고 생각할까요? 실제 위치는 어디쯤일까요?

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

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

A_LIN, A_ROT = 1.0, 2.0          # 시뮬레이터 가감속 한계 (교육용 가정)

def forward(dist, v=0.4):
    x0, y0, _ = agvsim.get_odom()
    agv.move_forward(v)
    while True:                   # 오도메트리로 거리를 재며 감속 거리만큼 일찍 멈춤
        x, y, _ = agvsim.get_odom()
        if math.hypot(x - x0, y - y0) >= dist - v * v / (2 * A_LIN):
            break
        time.sleep(0.02)
    agv.stop(); time.sleep(0.6)

def turn_left(angle, w=0.6):
    _, _, th0 = agvsim.get_odom()
    agv.turn_left(w)
    while True:
        _, _, th = agvsim.get_odom()
        turned = (th - th0) % (2 * math.pi)
        if turned >= angle - w * w / (2 * A_ROT) and turned < math.pi:
            break
        time.sleep(0.02)
    agv.stop(); time.sleep(0.6)

def square(slip):
    agvsim.set_slip(slip)
    agvsim.set_pose(0, 0, 0)               # 참값 · 오도메트리를 원점에 맞춤
    for _ in range(4):
        forward(1.0)
        turn_left(math.pi / 2)
    tx, ty, tth = agvsim.get_pose()        # 참값 (실물에는 없음 - 줄자로 잼)
    ox, oy, oth = agvsim.get_odom()        # 로봇이 믿는 위치
    print(f"slip {slip:4.2f}  odom ({ox:+.2f},{oy:+.2f},{math.degrees(oth):+6.1f}deg)"
          f"  truth ({tx:+.2f},{ty:+.2f},{math.degrees(tth):+6.1f}deg)")

for slip in (0.0, 0.1):
    square(slip)
agvsim.set_slip(0.0)
관찰 · 해설미끄러짐 0 % 에서는 odom 과 truth 가 같습니다 (원점에서 몇 cm · 몇 도 벗어나는 것은 명령 전달 지연으로 생긴 실제 오차이고, 오도메트리는 그것까지 정확히 셉니다). 미끄러짐 10 % 에서는 odom 이 여전히 "거의 원점" 인데 truth 는 수십 cm · 수십 도 벗어납니다. 엔코더는 바퀴가 돈 양만 알고 바닥이 실제로 얼마나 밀렸는지는 모르기 때문입니다 — 3교시의 **드리프트**가 바로 이것이고, ch07 에서 라이다로 `map → odom` 을 고쳐야 하는 이유입니다. 회전이 들어간 사각형일수록 오차가 큰 것도 ch02 실습 5-3 과 같습니다.
실습 5-2 · 엔코더 잡음과 미끄러짐은 다르게 쌓입니다 (3교시 복습)

시뮬레이터를 새 탭으로 열고 패널에서 (가) 미끄러짐 0 % · 엔코더 잡음 5 %, (나) 미끄러짐 5 % · 잡음 0 % 로 각각 키보드로 3 m 왕복을 세 번 합니다. HUD 의 오차 를 매번 적고, 어느 쪽이 매번 같은 방향으로 쌓이는지(계통 오차) · 어느 쪽이 들쭉날쭉 한지(우연 오차) 표로 정리하세요.

실습 (실물)#

실물 오도메트리 측정 전 안전 확인

ROS 2 드라이버를 띄운 뒤(방법은 공식 문서 · ch09) 오도메트리 토픽과 TF 를 확인합니다. 토픽 · 좌표계 이름은 드라이버 버전에 따라 다를 수 있으니 먼저 목록에서 찾습니다 (확인 필요).

bash
ros2 topic list | grep -i odom                 # 오도메트리 토픽 이름 찾기 (예: /odom)
ros2 topic echo /odom --once                   # pose · twist · covariance 한 번 보기
ros2 topic hz /odom                            # 발행 주기 (Δt) 확인
ros2 run tf2_ros tf2_echo odom base_footprint  # 변환 값을 계속 출력
ros2 run tf2_tools view_frames                 # TF 트리를 frames.pdf 로 저장

실물에서 해 볼 것:

  1. 바닥에 테이프로 출발점과 방향을 표시하고 직진 2 m 를 3 번 합니다. 줄자로 잰 거리와 /odom 의 x 를 비교해 거리 오차(%)를 구합니다 → 반지름 보정 계수.
  2. 제자리 360° 회전 을 3 번 합니다. 바닥 표시로 실제 각도를 맞추고 /odom 의 yaw 와 비교합니다 → 회전 슬립(%).
  3. 2 × 2 m 사각형을 3 바퀴 돈 뒤 출발점과의 차이를 잽니다. 1 · 2 의 결과로 시뮬레이터 3 에 반지름 오차 · 슬립을 넣었을 때의 예측과 비교합니다.
  4. view_frames 결과에서 odom → base_footprint 의 발행 노드 이름을 확인하고, 실습 4-2 의 TF 목록을 실제 이름으로 고쳐 씁니다.

자주 나는 오류와 해결#

1) 옆으로 가면 오도메트리 경로가 반대쪽으로 그려짐#

text
cmd: move_left_lateral at theta=90deg   ->   odom moves toward +X (expected -X)
  • 원인: x += (vx·cosθ + vy·sinθ)·Δt 처럼 vy 항의 부호를 틀렸습니다. 차동 구동에서는 vy = 0 이라 드러나지 않다가 메카넘 휠에서 처음 나타납니다.
  • 해결: ẋ = vx·cosθ - vy·sinθ, ẏ = vx·sinθ + vy·cosθ 를 확인합니다. 실습 2-1 풀이의 "먼저 90° 돌고 왼쪽" 테스트(기대 (-0.5, 0, 90°))를 단위 테스트로 남겨 둡니다.

2) 각도를 도(°)로 넣어 경로가 엉뚱하게 꺾임#

text
>>> math.cos(90)
-0.4480736161291701
  • 원인: math.cos · np.cos 는 라디안을 받습니다. 90 을 넣으면 90 rad(약 5157°)로 계산됩니다.
  • 해결: 입력에서 math.radians(deg) 로 바꾸고, 내부 계산은 모두 rad 로 통일합니다. 출력할 때만 math.degrees 를 씁니다.

3) 각도 비교가 어느 순간부터 틀림#

text
target 0.10 rad, odom theta 6.38 rad -> error 6.28 rad -> robot spins a full turn
  • 원인: θ 를 계속 더하기만 해서 한 바퀴(2π)를 넘었습니다. 6.38 rad 와 0.10 rad 는 사실 같은 방향입니다.
  • 해결: 적분할 때마다, 그리고 두 각도의 차를 구할 때 math.atan2(math.sin(a), math.cos(a)) 로 -π ~ π 로 감습니다.

4) 직접 짠 오도메트리가 실제보다 늘 조금 짧게 나옴#

text
measured 2.00 m, odom 1.93 m   (loop: time.sleep(0.02), dt = 0.02 fixed)
  • 원인: time.sleep(0.02) 에 통신 · 계산 시간이 더해져 실제 주기는 0.0207 s 처럼 깁니다. 고정 Δt 를 쓰면 매 걸음 시간을 덜 세어 거리가 짧아집니다.
  • 해결: t = time.monotonic() 으로 실제 경과 시간을 재서 Δt 로 씁니다. 엔코더 누적 카운트 를 받을 수 있다면 속도 × Δt 보다 카운트 차이로 각도를 직접 구하는 편이 더 정확합니다.

5) RViz 에서 로봇이 두 위치 사이를 오가며 떨림#

text
[WARN] [rviz2]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame base_footprint (parent odom) at time ...
  • 원인: 같은 변환 odom → base_footprint 를 두 노드(예: 바퀴 드라이버와 EKF)가 동시에 발행하고 있습니다. 값이 조금씩 달라 TF 가 둘 사이를 오갑니다.
  • 해결: ros2 run tf2_tools view_frames 로 발행 노드를 확인하고 한쪽의 TF 발행을 끕니다 (드라이버의 TF 발행 옵션 또는 EKF 의 publish_tf). 실습 4-2 의 "부모가 둘" 검사를 떠올리세요.

6) map 기준 좌표를 조회하면 오류#

text
[ERROR] Could not transform base_footprint to map: "map" passed to lookupTransform argument target_frame does not exist.
  • 원인: map → odom 을 발행하는 노드(SLAM · AMCL)가 아직 실행되지 않았습니다. 오도메트리만 있는 상태에서는 odom 이 트리의 뿌리입니다.
  • 해결: 오도메트리만 쓰는 실습에서는 기준 좌표계를 odom 으로 둡니다. 지도 기준이 필요하면 SLAM(ch06) 또는 AMCL(ch07)을 먼저 띄웁니다.

과제#

  1. 오차 예산표: 시뮬레이터 3 에서 반지름 오차 · 회전 슬립 · ω 노이즈 · v 노이즈를 하나씩 켠 결과(1 · 3 바퀴 끝 오차, 최대 오차)를 표로 만들고, "계통 / 랜덤" 과 "IMU 로 줄어드나" 열을 채웁니다.
  2. 적분 방법 보고서: 실습 2-2 를 확장해 Δt = 0.02 ~ 1.0 s 의 오일러 · 중점 최대 오차를 구하고, log-log 로 보았을 때 기울기가 1 과 2 에 가까운지 숫자로 확인합니다 (그래프는 손으로 그려도 됩니다).
  3. TF 트리 그림: 실물 · Gazebo 에서 view_frames 로 얻은 트리(없으면 이번 차시 그림)를 공책에 그리고, 각 화살표 옆에 발행 노드 · 갱신 주기 · "연속 / 점프" 성질을 적습니다. SLAM 과 AMCL 을 동시에 켜면 어떤 문제가 생길지 한 문장으로 씁니다.
  4. 보정 계수 실측(실물이 있으면): 실물 실습 1 · 2 의 결과로 반지름 보정 계수와 회전 슬립을 구하고, 그 값을 실습 3-1 의 run() 에 넣어 3 바퀴 오차를 예측한 뒤 실측과 비교합니다.

참고자료#

  • myAGV Pro 공식 문서: https://docs.elephantrobotics.com/docs/myAGV_Pro_en/
  • myAGV Pro ROS 2 패키지 (Gazebo 설정 · 드라이버): https://github.com/elephantrobotics/agv_pro_ros2
  • 제품 페이지: https://www.elephantrobotics.com/en/myagv-pro/
  • 제품 소개 · 이미지 (현영이앤씨): https://www.hyenc.net/products/36
  • REP 105 — 모바일 로봇의 좌표계 (map · odom · base_link): https://www.ros.org/reps/rep-0105.html
  • nav_msgs/Odometry 메시지 정의: https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html
  • ROS 2 tf2 튜토리얼: https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Tf2-Main.html
  • robot_localization (EKF 로 바퀴 + IMU 융합): https://docs.ros.org/en/humble/p/robot_localization/
  • Livox MID-360 (IMU 내장 3D 라이다): https://www.livoxtech.com/mid-360
  • 이 사이트의 도구: Playground · 물류창고 시뮬레이터 · 3D 뷰어