전체 과정 P3 자율주행의 원리 · 약 200분 · 중급

CH06오도메트리와 센서 융합

바퀴 엔코더 · IMU 로 위치를 추정하는 오도메트리를 직접 구현하고, 오차가 쌓이는 원리와 칼만 필터(EKF) 로 센서를 합쳐 줄이는 방법을 발견합니다.

선수 챕터: CH02 CH03 CH04 CH05
URDF 뷰어Python Playground

학습 목표#

  • 바퀴 각속도 → 순기구학 → 지도 기준 회전 → Δt 적분 으로 이어지는 바퀴 오도메트리(odometry)를 50 ms 루프로 직접 구현하고, 적분 주기 Δt 가 길수록 회전 구간에서 오차가 커지는 이유를 설명할 수 있습니다.
  • 오차의 원천(바퀴 반지름 오차 · 롤러 미끄러짐 · 정수화 · 시간 지연)을 구분하고, 방향 오차 1° 가 10 m 뒤 약 17 cm 의 옆 오차(d · sin Δθ)가 되는 원리로 위치 오차가 쌓이는 과정을 계산할 수 있습니다.
  • 랜덤 오차는 √n, 계통(systematic) 오차는 n 에 비례해 커진다는 것을 선반 통로 왕복 시뮬레이션의 오차 타원으로 확인하고, 측면 이동이 많은 메카넘 로봇에서 오도메트리가 특히 약한 이유를 말할 수 있습니다.
  • IMU 자이로의 장단점(짧게는 정확 · 바이어스로 흐름)을 이해하고, 상보 필터 θ = α·(θ + ω_gyro·Δt) + (1−α)·θ_wheel 의 α 를 상황에 맞게 고를 수 있습니다.
  • 칼만 필터의 예측 → 측정 → 칼만 이득 K 로 섞기 과정을 1차원으로 구현하고, Q · R 이 추정에 주는 영향과 myagv_ros2 의 myagv_odometry + robot_localization ekf_node 구성을 읽을 수 있습니다.
이 차시를 공부하는 방법

이 페이지에는 브라우저에서 바로 동작하는 시뮬레이터 5개가 있습니다. 슬라이더를 움직이기 전에 결과를 먼저 예측해 적고, 조작해서 확인하세요. Playground 실습 코드는 ch04 · ch05 와 같이 pymycobot 대신 같은 메서드 이름을 흉내 내는 교육용 클래스를 코드 맨 위에 넣어 실행합니다. 실물 로봇은 보지 못하는 "실제 위치" 를 시뮬 클래스는 알고 있으므로, 추정이 얼마나 틀렸는지 바로 잴 수 있습니다.

이 차시의 수치 중 무엇이 공식값이고 무엇이 가정인가요?

공식 자료 기준: get_motor_velocity(id) 가 바퀴 각속도(rad/s)를 준다는 것, get_all_msg() 의 6번 항목이 자이로 원시 데이터(18바이트)라는 것(myagvPlus_docs 6.1.2 API). myagv_ros2 저장소의 myagv_odometry 노드 코드 · config/ekf.yaml · myagv_active.launch.py 내용(4교시에 원문 그대로 인용).

교육용 가정(실물 값은 확인 필요): 바퀴 반지름 0.05 m · lx + ly = 0.22 m(ch03 과 같음), 모터 번호와 바퀴 위치(M1 앞왼 · M2 앞오른 · M3 뒤왼 · M4 뒤오른, ch05 와 같음), 미끄러짐 · 출렁임 · 자이로 바이어스의 크기, 자이로 원시값 9개의 순서와 스케일, 위치 센서의 잡음. 시뮬레이터는 원리를 보여 주기 위한 것입니다. 또 myagv_ros2 는 myAGV(이전 모델)용으로 공개된 저장소라서, myAGV Plus 에 그대로 쓰이는지는 ch09 에서 확인합니다.

1교시 · 바퀴 오도메트리 구현#

오도메트리란#

오도메트리(odometry) 는 "바퀴가 얼마나 돌았는지" 로 "로봇이 어디까지 왔는지" 를 추정하는 방법입니다. 자동차 계기판의 주행거리계(odometer)와 같은 말에서 왔습니다. 바깥을 보지 않고 자기 몸의 움직임만 더해 가므로 추측 항법(dead reckoning) 이라고도 부릅니다.

① 바퀴 엔코더 ② 순기구학 (ch03) ③ 지도 기준으로 회전 ④ Δt 마다 적분 get_motor_velocity(1~4) w₁(FL) w₂(FR) w₃(RL) w₄(RR) [rad/s] 50 ms 마다 vx = r/4·(w₁+w₂+w₃+w₄) vy = r/4·(−w₁+w₂+w₃−w₄) ω = r/(4L)·(−w₁+w₂−w₃+w₄) 로봇 기준 [m/s, rad/s] ẋ = vx·cosθ − vy·sinθ ẏ = vx·sinθ + vy·cosθ 지도 기준 속도 x += ẋ·Δt y += ẏ·Δt θ += ω·Δt 추정 자세 (x, y, θ) 현재 θ 를 다음 회전에 다시 씀 → θ 가 틀리면 이후 모든 이동 방향이 틀어짐 r = 0.05 m, L = lx + ly = 0.22 m (ch03 교육용 가정) · 엔코더는 "바퀴가 얼마나 돌았나" 만 알고 "바닥 위에서 얼마나 갔나" 는 모릅니다
바퀴 오도메트리의 네 단계. ②는 ch03 3교시의 순기구학, ③ · ④는 ch03 의 "로봇 좌표 → 지도 좌표 적분" 을 그대로 다시 씁니다.
단계하는 일이 차시의 값
① 읽기get_motor_velocity(1~4) 로 네 바퀴 각속도 w₁~w₄ (rad/s)50 ms 마다
② 순기구학로봇 기준 (vx, vy, ω)r = 0.05 m, L = lx + ly = 0.22 m
③ 회전현재 방향 θ 로 지도 기준 속도 (ẋ, ẏ)ch02 의 회전 행렬
④ 적분x += ẋ·Δt, y += ẏ·Δt, θ += ω·ΔtΔt = 루프 주기

이 계산에는 큰 약점이 하나 있습니다. Δt 동안 속도가 일정했다고 가정합니다. 실제로는 그 사이에도 로봇이 돌고 있으므로, Δt 가 길어지면 "출발할 때의 방향" 으로만 직선을 긋는 셈이 됩니다. 1교시 시뮬레이터에서 이 차이를 직접 봅니다.

50 ms 오도메트리 루프#

실물 프로그램의 뼈대는 다음과 같습니다. 오도메트리는 한 번 계산하고 끝나는 것이 아니라, 같은 주기로 계속 더해 가는 루프입니다.

text
x = y = θ = 0
매 50 ms 마다:
    w = [get_motor_velocity(i) for i in 1..4]   ← ①
    vx, vy, ω = fk(w)                            ← ②
    x += (vx·cosθ − vy·sinθ)·Δt                  ← ③ · ④
    y += (vx·sinθ + vy·cosθ)·Δt
    θ += ω·Δt
  • Δt 는 "정해 둔 50 ms" 가 아니라 실제로 흐른 시간을 써야 합니다. get_motor_velocity() 를 네 번 부르는 데에도 시간이 걸리므로 루프가 50 ms 보다 늦어지기 쉽습니다. myagv_ros2 의 오도메트리 노드도 매번 dt = (현재 시각 − 이전 시각) 을 잽니다(4교시).
  • θ 는 라디안으로 누적합니다. 화면에 보일 때만 도(°)로 바꿉니다.
  • 위의 식은 Δt 동안 θ 를 "시작할 때의 값" 으로 고정합니다. 이를 오일러(Euler) 적분이라 합니다. 회전 중에는 Δt 의 중간 방향 θ + ω·Δt/2 를 쓰면 훨씬 정확해집니다(실습 과제 1-2).

시뮬레이터 1 · 연습장: 실제 위치와 오도메트리 추정#

빈 연습장 바닥(격자 0.5 m)에서 로봇이 정해진 경로를 달립니다. 초록은 실제 위치(1 ms 마다 정밀하게 계산), 주황은 Δt 마다 바퀴 속도를 한 번 읽어 적분한 오도메트리 추정입니다. 바닥 미끄러짐은 없다고 둡니다. 오차는 오직 적분 주기 때문에 생깁니다.

연습장 · 실제(초록) vs 오도메트리 추정(주황)
해 볼 것 (먼저 예측을 적고 확인하세요)
  1. 원 경로에서 Δt 를 50 ms → 300 ms 로 바꾸기 전에, 주황 궤적이 초록 원의 안쪽과 바깥쪽 중 어디로 벗어날지 예측합니다. "주황 궤적 가리기" 를 켠 채 Δt 를 300 ms 로 놓고, 예측을 적은 뒤 체크를 풉니다.
  2. 원 경로에서 Δt 를 50 · 100 · 200 · 300 · 500 ms 로 바꾸며 "최대 위치 오차" 와 표 맨 아래 "속력 × Δt" 를 비교합니다. 어떤 관계가 보이나요?
  3. 원 경로의 반복을 1 → 4 번으로 늘립니다. 오차가 바퀴 수만큼 커지나요? 이번엔 사각형 경로에서 같은 실험(Δt = 300 ms, 반복 1 → 4)을 합니다. 차이가 왜 생길까요?
  4. 옆걸음 사각형(회전 없음)에서 Δt 를 500 ms 까지 올려 봅니다. 사각형(회전 있음)과 비교해 오차가 어떤가요?
  5. Δt = 300 ms 그대로 중간각 체크를 켭니다. 원 · 돌면서 옆걸음 경로의 오차가 어떻게 되나요? 사각형 경로에서는 왜 완전히 사라지지 않을까요?
관찰 포인트
  • 직선 구간에서는 θ 가 변하지 않으므로 오일러 적분 오차가 없습니다. 옆걸음 사각형은 명령이 바뀌는 순간이 Δt 사이에 끼어 잠깐 어긋나지만(300 ms 에서 최대 약 8 cm), 다음 전환에서 되돌아와 끝 오차가 0 이고 반복해도 커지지 않습니다. θ 가 틀어지지 않았기 때문입니다.
  • 원 경로에서 오일러 적분은 매 Δt 마다 "시작 방향" 으로 직선을 긋습니다. 진짜 호보다 늘 회전 반대쪽(원의 바깥쪽) 으로 출발하므로, 추정 원 전체가 처음 진행 방향 쪽으로 약 v·Δt/2 만큼 밀립니다. 최대 오차는 약 v · Δt 입니다(0.4 m/s × 0.3 s = 12 cm).
  • 원처럼 ω 가 일정하면 θ 자체는 정확하므로 오차가 바퀴 수에 따라 늘지 않습니다. 사각형은 회전을 시작 · 끝내는 순간이 Δt 사이에 끼면 θ 가 틀린 채로 남고, 이후 직선이 모두 그 각도만큼 틀어지므로 반복할수록 커집니다. 이것이 2교시 "각도 오차가 위치 오차를 키운다" 의 시작입니다.
  • 중간각 보정은 "Δt 동안 속도 일정" 이 맞을 때(원 · 돌면서 옆걸음) 거의 완벽합니다. 명령이 Δt 중간에 바뀌는 경우(사각형의 회전 시작 · 끝)는 더 자주 읽는 것 말고는 고칠 수 없습니다.

실습 과제 1#

실습 과제 1-1 · 50 ms 오도메트리 루프 만들기

교육용 SimAGVPlus 는 원 궤적(vx 0.4 m/s · ω 0.5 rad/s, 한 바퀴 12.57 s)을 달리면서 get_motor_velocity(1~4) 로 네 바퀴 각속도를 줍니다. wait(dt) 는 실물의 time.sleep(dt) 자리이며 시뮬 시계를 dt 만큼 진행합니다. TODO 를 채워 2초마다 실제 위치와 추정 위치를 비교하세요. 예측 먼저: Δt = 50 ms 이면 위치 오차는 최대 몇 cm 쯤일까요? (시뮬레이터 1 의 "속력 × Δt" 를 떠올립니다.)

python시뮬 실행 가능
import math

class SimAGVPlus:
    """교육용 시뮬 shim: get_motor_velocity(id) 를 흉내 냄 (치수 · 경로는 교육용 가정)"""
    R, L = 0.05, 0.22                          # 바퀴 반지름, lx + ly (m)

    def __init__(self, path):
        self.path = path                       # [(지속 s, vx, vy, w), ...]
        self.t = 0.0
        self.x = self.y = self.th = 0.0        # 실제 자세 (시뮬만 앎)

    def _cmd(self, t):
        acc = 0.0
        for d, vx, vy, w in self.path:
            if t < acc + d:
                return vx, vy, w
            acc += d
        return 0.0, 0.0, 0.0

    def get_motor_velocity(self, mid):         # 1 FL, 2 FR, 3 RL, 4 RR  [rad/s]
        vx, vy, w = self._cmd(self.t)
        k = self.L * w
        wheel = [vx - vy - k, vx + vy + k, vx + vy - k, vx - vy + k]
        return round(wheel[mid - 1] / self.R, 2)

    def wait(self, dt):                        # 실물의 time.sleep(dt) 자리 - 시뮬 시계를 dt 만큼
        n = max(1, int(round(dt / 0.001)))
        h = dt / n
        for _ in range(n):
            vx, vy, w = self._cmd(self.t)
            m = self.th + w * h / 2
            self.x += (vx * math.cos(m) - vy * math.sin(m)) * h
            self.y += (vx * math.sin(m) + vy * math.cos(m)) * h
            self.th += w * h
            self.t += h

# 원 궤적: vx = 0.4 m/s, w = 0.5 rad/s 로 12.57 s (한 바퀴, 반지름 0.8 m)
agv = SimAGVPlus([(4 * math.pi, 0.4, 0.0, 0.5)])
R, L = SimAGVPlus.R, SimAGVPlus.L
DT = 0.05

def fk(w1, w2, w3, w4):
    # TODO 1: ch03 순기구학 - vx, vy, w 를 계산
    vx = vy = w = 0.0
    return vx, vy, w

x = y = th = 0.0
print("  t(s)   실제 x   y      추정 x   y      위치오차(m)")
for k in range(int(4 * math.pi / DT) + 1):
    w = [agv.get_motor_velocity(i) for i in range(1, 5)]
    vx, vy, om = fk(*w)
    # TODO 2: 지도 기준으로 돌려서 x, y 에 더하고, th 에 om * DT 를 더함
    agv.wait(DT)
    if k % 40 == 39:
        e = math.hypot(x - agv.x, y - agv.y)
        print("%6.2f  %6.3f %6.3f   %6.3f %6.3f   %.4f" % (agv.t, agv.x, agv.y, x, y, e))
풀이 코드 보기
python시뮬 실행 가능
import math

class SimAGVPlus:
    """교육용 시뮬 shim: get_motor_velocity(id) 를 흉내 냄 (치수 · 경로는 교육용 가정)"""
    R, L = 0.05, 0.22                          # 바퀴 반지름, lx + ly (m)

    def __init__(self, path):
        self.path = path                       # [(지속 s, vx, vy, w), ...]
        self.t = 0.0
        self.x = self.y = self.th = 0.0        # 실제 자세 (시뮬만 앎)

    def _cmd(self, t):
        acc = 0.0
        for d, vx, vy, w in self.path:
            if t < acc + d:
                return vx, vy, w
            acc += d
        return 0.0, 0.0, 0.0

    def get_motor_velocity(self, mid):         # 1 FL, 2 FR, 3 RL, 4 RR  [rad/s]
        vx, vy, w = self._cmd(self.t)
        k = self.L * w
        wheel = [vx - vy - k, vx + vy + k, vx + vy - k, vx - vy + k]
        return round(wheel[mid - 1] / self.R, 2)

    def wait(self, dt):                        # 실물의 time.sleep(dt) 자리 - 시뮬 시계를 dt 만큼
        n = max(1, int(round(dt / 0.001)))
        h = dt / n
        for _ in range(n):
            vx, vy, w = self._cmd(self.t)
            m = self.th + w * h / 2
            self.x += (vx * math.cos(m) - vy * math.sin(m)) * h
            self.y += (vx * math.sin(m) + vy * math.cos(m)) * h
            self.th += w * h
            self.t += h

# 원 궤적: vx = 0.4 m/s, w = 0.5 rad/s 로 12.57 s (한 바퀴, 반지름 0.8 m)
agv = SimAGVPlus([(4 * math.pi, 0.4, 0.0, 0.5)])
R, L = SimAGVPlus.R, SimAGVPlus.L
DT = 0.05

def fk(w1, w2, w3, w4):
    vx = R / 4 * (w1 + w2 + w3 + w4)
    vy = R / 4 * (-w1 + w2 + w3 - w4)
    w = R / (4 * L) * (-w1 + w2 - w3 + w4)
    return vx, vy, w

x = y = th = 0.0
print("  t(s)   실제 x   y      추정 x   y      위치오차(m)")
for k in range(int(4 * math.pi / DT) + 1):
    w = [agv.get_motor_velocity(i) for i in range(1, 5)]
    vx, vy, om = fk(*w)
    x += (vx * math.cos(th) - vy * math.sin(th)) * DT
    y += (vx * math.sin(th) + vy * math.cos(th)) * DT
    th += om * DT
    agv.wait(DT)
    if k % 40 == 39:
        e = math.hypot(x - agv.x, y - agv.y)
        print("%6.2f  %6.3f %6.3f   %6.3f %6.3f   %.4f" % (agv.t, agv.x, agv.y, x, y, e))

실행 결과:

text
  t(s)   실제 x   y      추정 x   y      위치오차(m)
  2.00   0.673  0.368    0.678  0.359   0.0096
  4.00   0.727  1.133    0.742  1.124   0.0168
  6.00   0.113  1.592    0.133  1.590   0.0200
  8.00  -0.605  1.323   -0.589  1.330   0.0182
 10.00  -0.767  0.573   -0.760  0.583   0.0120
 12.00  -0.224  0.032   -0.223  0.035   0.0028
  • 바퀴 값은 앞왼 5.8 · 앞오른 10.2 · 뒤왼 5.8 · 뒤오른 10.2 rad/s 입니다. 왼쪽 바퀴가 느리므로 왼쪽으로 돕니다. fk 는 이것을 (0.4, 0, 0.5) 로 정확히 되돌립니다.
  • 오차는 반 바퀴(약 6 s)에서 최대 2.0 cm = 0.4 m/s × 0.05 s 이고, 한 바퀴를 돌면 거의 0 으로 돌아옵니다. 추정 원이 통째로 약간 밀려 있을 뿐 θ 는 정확하기 때문입니다.
  • th += om * DT 를 x, y 계산 앞에 두면 "끝 방향" 을 쓰는 적분이 되어 원이 반대쪽으로 밀립니다. 순서도 결과를 바꿉니다.
실습 과제 1-2 · Δt 50 ms vs 300 ms, 그리고 중간각 보정

실습 과제 1-1 의 루프를 함수 run(DT, mid) 로 묶어 Δt 와 적분 방식을 바꿔 가며 최대 오차와 추정 원의 중심을 구합니다. 실제 원의 중심은 (0, 0.8) 입니다. 예측 먼저: Δt = 300 ms 이면 추정 원의 중심은 x 쪽(처음 진행 방향)과 y 쪽(원 안쪽) 중 어디로 옮겨질까요? 중간각을 쓰면 오차가 몇 분의 1 이 될까요?

python시뮬 실행 가능
import math

class SimAGVPlus:
    """교육용 시뮬 shim (실습 과제 1-1 과 같음)"""
    R, L = 0.05, 0.22
    def __init__(self, path):
        self.path, self.t = path, 0.0
        self.x = self.y = self.th = 0.0
    def _cmd(self, t):
        acc = 0.0
        for d, vx, vy, w in self.path:
            if t < acc + d: return vx, vy, w
            acc += d
        return 0.0, 0.0, 0.0
    def get_motor_velocity(self, mid):
        vx, vy, w = self._cmd(self.t); k = self.L * w
        wheel = [vx - vy - k, vx + vy + k, vx + vy - k, vx - vy + k]
        return round(wheel[mid - 1] / self.R, 2)
    def wait(self, dt):
        n = max(1, int(round(dt / 0.001))); h = dt / n
        for _ in range(n):
            vx, vy, w = self._cmd(self.t); m = self.th + w * h / 2
            self.x += (vx * math.cos(m) - vy * math.sin(m)) * h
            self.y += (vx * math.sin(m) + vy * math.cos(m)) * h
            self.th += w * h; self.t += h

R, L = SimAGVPlus.R, SimAGVPlus.L
def fk(w1, w2, w3, w4):
    return (R / 4 * (w1 + w2 + w3 + w4), R / 4 * (-w1 + w2 + w3 - w4),
            R / (4 * L) * (-w1 + w2 - w3 + w4))

def run(DT, mid=False, T=4 * math.pi):
    agv = SimAGVPlus([(T + 5, 0.4, 0.0, 0.5)])
    x = y = th = 0.0; emax = 0.0; xs = []; ys = []
    while agv.t < T - 1e-9:
        vx, vy, om = fk(*[agv.get_motor_velocity(i) for i in range(1, 5)])
        # TODO 1: mid 가 True 면 a = th + om * DT / 2, 아니면 a = th
        a = th
        x += (vx * math.cos(a) - vy * math.sin(a)) * DT
        y += (vx * math.sin(a) + vy * math.cos(a)) * DT
        th += om * DT
        agv.wait(DT)
        emax = max(emax, math.hypot(x - agv.x, y - agv.y)); xs.append(x); ys.append(y)
    # TODO 2: 추정 원의 중심 = (x 최댓값 + 최솟값) / 2, (y 최댓값 + 최솟값) / 2
    cx, cy = 0.0, 0.0
    return emax, cx, cy

print("실제 원 중심 (0.000, 0.800)")
print(" Dt(ms)  방식     최대오차(m)  v*Dt(m)  추정 원 중심")
for DT in (0.05, 0.3):
    for mid in (False, True):
        e, cx, cy = run(DT, mid)
        print("%6d   %-6s   %.4f       %.3f    (%.3f, %.3f)" % (DT * 1000, "중간각" if mid else "기본", e, 0.4 * DT, cx, cy))
풀이 코드 보기
python시뮬 실행 가능
import math

class SimAGVPlus:
    """교육용 시뮬 shim (실습 과제 1-1 과 같음)"""
    R, L = 0.05, 0.22
    def __init__(self, path):
        self.path, self.t = path, 0.0
        self.x = self.y = self.th = 0.0
    def _cmd(self, t):
        acc = 0.0
        for d, vx, vy, w in self.path:
            if t < acc + d: return vx, vy, w
            acc += d
        return 0.0, 0.0, 0.0
    def get_motor_velocity(self, mid):
        vx, vy, w = self._cmd(self.t); k = self.L * w
        wheel = [vx - vy - k, vx + vy + k, vx + vy - k, vx - vy + k]
        return round(wheel[mid - 1] / self.R, 2)
    def wait(self, dt):
        n = max(1, int(round(dt / 0.001))); h = dt / n
        for _ in range(n):
            vx, vy, w = self._cmd(self.t); m = self.th + w * h / 2
            self.x += (vx * math.cos(m) - vy * math.sin(m)) * h
            self.y += (vx * math.sin(m) + vy * math.cos(m)) * h
            self.th += w * h; self.t += h

R, L = SimAGVPlus.R, SimAGVPlus.L
def fk(w1, w2, w3, w4):
    return (R / 4 * (w1 + w2 + w3 + w4), R / 4 * (-w1 + w2 + w3 - w4),
            R / (4 * L) * (-w1 + w2 - w3 + w4))

def run(DT, mid=False, T=4 * math.pi):
    agv = SimAGVPlus([(T + 5, 0.4, 0.0, 0.5)])
    x = y = th = 0.0; emax = 0.0; xs = []; ys = []
    while agv.t < T - 1e-9:
        vx, vy, om = fk(*[agv.get_motor_velocity(i) for i in range(1, 5)])
        a = th + om * DT / 2 if mid else th
        x += (vx * math.cos(a) - vy * math.sin(a)) * DT
        y += (vx * math.sin(a) + vy * math.cos(a)) * DT
        th += om * DT
        agv.wait(DT)
        emax = max(emax, math.hypot(x - agv.x, y - agv.y)); xs.append(x); ys.append(y)
    cx, cy = (max(xs) + min(xs)) / 2, (max(ys) + min(ys)) / 2
    return emax, cx, cy

print("실제 원 중심 (0.000, 0.800)")
print(" Dt(ms)  방식     최대오차(m)  v*Dt(m)  추정 원 중심")
for DT in (0.05, 0.3):
    for mid in (False, True):
        e, cx, cy = run(DT, mid)
        print("%6d   %-6s   %.4f       %.3f    (%.3f, %.3f)" % (DT * 1000, "중간각" if mid else "기본", e, 0.4 * DT, cx, cy))

실행 결과:

text
실제 원 중심 (0.000, 0.800)
 Dt(ms)  방식     최대오차(m)  v*Dt(m)  추정 원 중심
    50   기본       0.0200       0.020    (0.010, 0.800)
    50   중간각      0.0000       0.020    (0.000, 0.800)
   300   기본       0.1200       0.120    (0.060, 0.798)
   300   중간각      0.0015       0.120    (-0.000, 0.801)
  • 기본(오일러) 적분의 최대 오차는 정확히 v · Δt 이고, 추정 원의 중심은 처음 진행 방향(+x)으로 v·Δt/2 만큼 옮겨집니다(0.3 s → 6 cm). 매 단계 "시작 방향" 으로 직선을 그어 늘 회전 바깥쪽으로 출발하기 때문입니다.
  • 중간각을 쓰면 300 ms 에서도 오차가 0.15 cm 로 약 80분의 1 이 됩니다. 이 경로처럼 Δt 동안 속도가 일정할 때만 이렇게 잘 맞습니다.
  • 실물에서는 get_motor_velocity() 네 번 + 계산 시간이 Δt 를 늘립니다. 주기를 짧게 유지하고, 회전 중에는 중간각을 쓰는 것이 값싼 개선입니다.
확인 문제 1-1

시뮬레이터 1 의 사각형 경로를 Δt = 300 ms 로 4번 반복했더니, 원 경로(같은 Δt, 4번 반복)보다 끝 위치 오차가 훨씬 컸습니다. 가장 알맞은 설명은?

  1. 사각형은 직선이 길어서 직선 구간마다 오일러 적분 오차가 쌓인다
  2. 회전이 Δt 중간에 시작 · 끝나면 그 Δt 의 ω 를 통째로 넣거나 빼게 되어 θ 가 틀린 채 남고, 이후 모든 직선이 그 각도만큼 틀어진다
  3. 사각형 경로에서는 get_motor_velocity() 가 반올림되기 때문이다
  4. 원 경로는 오차가 0 이기 때문이다
정답 보기② — 직선 구간은 θ 가 그대로이므로 오일러 적분 오차가 없습니다(① 틀림, 옆걸음 사각형으로 확인). 원은 ω 가 일정해 θ 가 정확하므로 오차가 v·Δt 수준에서 더 늘지 않지만 0 은 아닙니다(④ 틀림). 사각형은 회전 시작 · 끝이 표본 사이에 끼면 θ 에 남는 오차가 생기고, 이 각도 오차가 다음 직선 길이만큼 옆 오차로 커집니다(2교시).
확인 문제 1-2

v = 0.6 m/s 로 원을 도는 로봇의 오도메트리를 Δt = 0.2 s 로 오일러 적분합니다. 시뮬레이터 1 · 실습 과제 1-2 의 관계를 쓰면 최대 위치 오차는 대략?

  1. 0.6 cm
  2. 3 cm
  3. 12 cm
  4. 60 cm
정답 보기③ — 최대 오차 ≈ v · Δt = 0.6 × 0.2 = 0.12 m 입니다. Δt 를 50 ms 로 줄이면 3 cm, 중간각을 쓰면 거의 0 이 됩니다.

2교시 · 오차는 왜 쌓일까: 미끄러짐과 각도 오차#

오차의 원천#

1교시는 적분 주기만 다뤘습니다. 실물에서는 그보다 큰 오차들이 겹칩니다. 엔코더는 바퀴가 돈 양만 알고, 바퀴가 바닥에서 실제로 얼마나 갔는지는 모릅니다.

원천예오차 모양쌓이는 방식
바퀴 반지름 설정 오차코드에 r = 0.051 (실제 0.050)거리 · 회전각이 늘 2 % 크게 추정계통 오차 — 같은 방향으로 n 배
롤러 미끄러짐젖은 바닥, 먼지, 급가속, 옆걸음실제가 추정보다 덜 감 (특히 측면)바닥 · 동작마다 다름 (계통 + 랜덤)
좌우 바퀴 차이 · 마모한 바퀴 롤러가 닳음직진인데 조금씩 돎각도 오차 → 옆 오차로 커짐
정수화 · 분해능엔코더 눈금, 반올림 (round(…, 2))작은 속도가 0 또는 한 눈금으로작지만 늘 같은 쪽이면 누적
시간 지연 · 주기 흔들림Δt 를 50 ms 로 가정했는데 실제 62 ms이동 거리 과소 · 과대 추정루프가 늦을수록 누적
계통 오차와 랜덤 오차
  • 계통(systematic) 오차: 매번 같은 방향 · 같은 크기로 틀립니다(반지름 설정 오차). n 번 하면 n 배가 됩니다. 대신 측정해서 보정할 수 있습니다.
  • 랜덤(random) 오차: 매번 다르게 틀립니다(바닥 상태). 일부는 서로 상쇄되어 n 번 하면 약 √n 배가 됩니다. 보정할 수 없고, 범위(불확실성)로만 말할 수 있습니다.

각도 오차는 위치 오차를 키운다#

선반 선반 출발 e Δθ = 1° 실제 이동 (방향 θ, 거리 d = 10 m) 추정 이동 (방향 θ + Δθ) 옆 오차 e = d · sin Δθ ≈ d · Δθ[rad] → 10 m × sin 1° = 10 × 0.01745 ≈ 0.17 m 방향이 1° 틀리면 가는 거리 d 에 비례해 옆으로 벌어집니다 (그림의 기울기는 과장)
방향이 Δθ 틀리면 d 만큼 가는 동안 옆으로 d · sin Δθ 벌어집니다.
text
옆 오차 e = d · sin Δθ  ≈  d · Δθ(rad)          (Δθ 가 작을 때)
1° = π/180 = 0.01745 rad  →  10 m 이동 후 e ≈ 10 × 0.01745 = 0.17 m
  • 거리 오차(반지름 2 % → 10 m 에서 20 cm)는 진행 방향으로만 생기지만, 각도 오차는 옆 방향으로 생겨서 선반 · 벽에 닿는 사고로 이어집니다. 폭 1.2 m 통로에서 로봇 반폭이 0.15 m 이면 옆으로 0.45 m 만 틀려도 선반에 닿습니다.
  • 방향은 θ 하나에 모든 과거 오차가 쌓이고, 그 θ 가 이후의 모든 이동 방향을 돌려 버립니다(1교시 그림 ③의 되먹임). 그래서 "방향을 정확히 아는 것" 이 위치 추정의 핵심입니다. 3교시의 자이로가 바로 이 문제를 겨냥합니다.

시뮬레이터 2 · 오차 계산기: 거리 오차 + 각도 오차#

길이 d 를 가는 동안 반지름 설정 오차(진행 방향)와 방향 오차(옆 방향)가 만드는 끝 위치 오차를 그립니다. 폭 1.2 m 의 선반 통로 기준입니다.

거리 d 이동 후 끝 위치 오차 · 진행 방향 + 옆 방향
해 볼 것
  1. d = 10 m, Δθ = 1°, 반지름 오차 0 % 에서 옆 오차가 그림의 17 cm 와 맞는지 확인합니다.
  2. Δθ = 0°, 반지름 오차를 +2 %(코드의 반지름이 실제보다 2 % 큼)로 놓기 전에, 10 m 뒤 오차가 어느 방향으로 몇 cm 일지 예측합니다. 로봇은 "오도메트리가 10 m" 라고 할 때 멈춥니다.
  3. Δθ 를 1° 로 두고 d 를 늘려 선반에 닿기 시작하는 거리를 찾습니다. Δθ = 2° 이면 몇 m 일까요?
  4. 반지름 오차를 −2 % 로 바꾸면 진행 방향 오차의 부호가 어떻게 되나요?
관찰 포인트
  • 반지름을 2 % 크게 설정하면 오도메트리는 바퀴 한 바퀴를 실제보다 2 % 멀리 간 것으로 셉니다. 그래서 "10 m 갔다" 고 멈췄을 때 실제로는 10 / 1.02 = 9.80 m, 즉 약 20 cm 덜 간 곳에 있습니다.
  • 옆 오차는 거리에 정비례합니다. Δθ = 1° 이면 약 26 m, 2° 이면 약 13 m 에서 0.45 m 를 넘습니다. 긴 통로일수록 작은 각도 오차가 치명적입니다.

오차 타원: 불확실성이 자라는 모습#

같은 명령을 여러 번 실행하면, 매번 바닥 상태가 조금씩 달라 실제 도착점이 흩어집니다. 이 흩어짐을 타원으로 그린 것이 오차 타원(error ellipse) 입니다. 타원의 크기가 "오도메트리를 믿을 때 실제로 있을 수 있는 범위", 즉 불확실성입니다. 4교시의 칼만 필터는 이 불확실성을 공분산 행렬 P 라는 숫자로 들고 다닙니다.

시뮬레이터 3 · 선반 통로 왕복 10회#

선반 통로(폭 1.2 m)를 왕복합니다. 로봇은 오도메트리를 믿고 "통로 끝에 왔다" 고 판단하면 돌아섭니다. 그래서 주황(로봇이 믿는 경로)은 늘 통로 한가운데 직선이고, 초록(같은 명령을 60번 실행한 실제 경로)이 흩어집니다. 귀환할 때마다 출발점에서 실제 위치의 2σ 오차 타원을 그립니다.

text
전진 한 번: 실제 거리 = d · (r_실제 / r_설정) · (1 + N(0, σ)),  방향 흔들림 N(0, 0.05·σ·d) rad
180° 제자리 회전: 실제 각 = π · (r_실제 / r_설정) · (1 + N(0, 0.3·σ))
옆걸음: 실제 거리 = d · (r_실제 / r_설정) · (0.55 + 0.40·μ) · (1 + N(0, 3σ)),  방향 흔들림 3배   (ch03 미끄러짐 모델)
선반 통로 왕복 · 실제 경로 60개와 2σ 오차 타원
해 볼 것 (예측 → 확인)
  1. 기본값(회전 방식 · 반지름 오차 0 · σ 1 %)에서 왕복 1회와 10회의 "2σ 타원 긴 반지름" 을 비교합니다. 10배가 되나요, 약 √10 ≈ 3.2배가 되나요?
  2. σ 를 0 으로, 반지름 설정 오차를 +2 % 로 놓습니다. 통로 길이 10 m, 왕복 1회에서 "첫 편도 끝의 평균 위치 오차" 를 먼저 예측해 적고 확인합니다(시뮬레이터 2 해 볼 것 2 와 같은 상황).
  3. 그대로 통로를 8 m 로 줄이고 왕복 1회로 둡니다. 회전 방식에서 돌아오는 길에 무슨 일이 생기나요? 왕복 방식을 전진 → 후진으로 바꾸고 왕복을 10회로 늘리면 어떻게 되나요? 왜 다를까요?
  4. 반지름 오차 0, σ 1 % 로 되돌리고 옆걸음 방식으로 바꿉니다. μ = 0.6 에서 첫 편도 끝의 오차와 타원 크기를 회전 방식과 비교한 뒤, μ 를 0.3 으로 낮춥니다.
  5. "🎲 다시 뽑기" 를 여러 번 누릅니다. 초록 선 모양은 바뀌는데 타원 크기는 거의 그대로입니다. 왜 그럴까요?
관찰 포인트
  • 랜덤 오차는 √n 으로 자랍니다. σ 1 % 의 회전 방식에서 타원 긴 반지름은 1회 약 23 cm 에서 10회 약 75 cm 안팎(뽑을 때마다 조금씩 다름)으로 약 3배가 됩니다. 0.45 m 를 넘는 실제 경로가 생기기 시작하면 선반 접촉(×)이 나옵니다.
  • 계통 오차는 n 으로 자랍니다. 반지름 +2 % 는 직선에서 2 % 덜 가게 하고(10 m 에서 약 20 cm), 회전에서도 2 % 덜 돌게 합니다(180° → 약 176.5°). 회전 방식에서는 돌아오는 8 m 동안 8 × sin 3.5° ≈ 0.49 m 옆으로 벗어나 첫 귀환에서 모든 경로가 선반에 닿습니다. 선반이 없다면 이 오차가 왕복마다 약 0.5 m 씩 쌓입니다(실습 과제 2-2). 전진 → 후진 방식은 회전이 없어서 갈 때 모자란 만큼 올 때도 모자라 거리 오차가 상쇄되고 귀환점 오차가 0 입니다.
  • 옆걸음은 오도메트리의 약점입니다. 롤러가 미끄러져 실제로는 μ = 0.6 에서 약 79 %, μ = 0.3 에서 약 67 % 만 갑니다. 엔코더는 이것을 전혀 모르므로 통로 끝에서 1.5 m 이상 모자라고, 랜덤 성분도 3배라 타원이 훨씬 큽니다. 메카넘 로봇이 측면 이동을 많이 할수록 바퀴 말고 다른 센서가 필요해집니다.
  • 이 시뮬레이터의 미끄러짐 크기는 교육용 가정입니다. 실물의 σ 는 같은 경로를 여러 번 달려 줄자로 재어 구합니다(실물 실습 ①).

실습 과제 2#

실습 과제 2-1 · 반지름 2 % 오차와 각도 1° 오차 계산

코드의 바퀴 반지름을 실제보다 2 % 크게 설정한 로봇이 "오도메트리 10 m" 에서 멈춥니다. 또 방향 오차 1° 로 10 m 를 갑니다. 각각의 오차를 계산하고, 둘이 함께 있을 때 끝 위치 오차를 구하세요. 예측 먼저: 반지름 오차로 인한 10 m 후 오차는 약 몇 cm 이고, 실제 위치는 목표보다 앞일까요 뒤일까요?

python시뮬 실행 가능
import math

D = 10.0              # 오도메트리가 10 m 라고 할 때 멈춤
R_TRUE = 0.050        # 실제 바퀴 반지름 (m)
R_SET = R_TRUE * 1.02 # 코드에 잘못 넣은 반지름 (2 % 큼)
DTH_DEG = 1.0         # 방향 오차 (도)

# TODO 1: 바퀴가 돈 각도(rad) = D / R_SET, 실제 이동 거리 = 바퀴 각도 x R_TRUE
real = 0.0
print("반지름 오차만: 실제 이동 %.3f m, 진행 방향 오차 %.1f cm" % (real, (D - real) * 100))

# TODO 2: 방향 오차만: 옆 오차 = D * sin(Δθ)   (Δθ 는 라디안으로)
lat = 0.0
print("각도 오차만 : 옆 오차 %.1f cm (근사 D*각도 = %.1f cm)" % (lat * 100, D * math.radians(DTH_DEG) * 100))

# TODO 3: 둘 다: 실제 끝점 = (real*cos, real*sin), 목표 (D, 0) 와의 거리
both = 0.0
print("둘 다      : 끝 위치 오차 %.1f cm" % (both * 100))

# 덤: 옆 오차가 0.45 m 를 넘는 거리
for deg in (0.5, 1.0, 2.0):
    print("각도오차 %.1f 도 -> 통로 반폭 0.45 m 에 닿는 거리 %.1f m" % (deg, 0.45 / math.sin(math.radians(deg))))
풀이 코드 보기
python시뮬 실행 가능
import math

D = 10.0              # 오도메트리가 10 m 라고 할 때 멈춤
R_TRUE = 0.050        # 실제 바퀴 반지름 (m)
R_SET = R_TRUE * 1.02 # 코드에 잘못 넣은 반지름 (2 % 큼)
DTH_DEG = 1.0         # 방향 오차 (도)

turn = D / R_SET                  # 오도메트리가 10 m 라고 믿는 바퀴 각도
real = turn * R_TRUE              # 실제로 간 거리
print("반지름 오차만: 실제 이동 %.3f m, 진행 방향 오차 %.1f cm" % (real, (D - real) * 100))

lat = D * math.sin(math.radians(DTH_DEG))
print("각도 오차만 : 옆 오차 %.1f cm (근사 D*각도 = %.1f cm)" % (lat * 100, D * math.radians(DTH_DEG) * 100))

ex = real * math.cos(math.radians(DTH_DEG))
ey = real * math.sin(math.radians(DTH_DEG))
both = math.hypot(D - ex, ey)
print("둘 다      : 끝 위치 오차 %.1f cm" % (both * 100))

for deg in (0.5, 1.0, 2.0):
    print("각도오차 %.1f 도 -> 통로 반폭 0.45 m 에 닿는 거리 %.1f m" % (deg, 0.45 / math.sin(math.radians(deg))))

실행 결과:

text
반지름 오차만: 실제 이동 9.804 m, 진행 방향 오차 19.6 cm
각도 오차만 : 옆 오차 17.5 cm (근사 D*각도 = 17.5 cm)
둘 다      : 끝 위치 오차 26.1 cm
각도오차 0.5 도 -> 통로 반폭 0.45 m 에 닿는 거리 51.6 m
각도오차 1.0 도 -> 통로 반폭 0.45 m 에 닿는 거리 25.8 m
각도오차 2.0 도 -> 통로 반폭 0.45 m 에 닿는 거리 12.9 m
  • 반지름을 크게 잡으면 바퀴가 덜 돌아도 10 m 라고 믿으므로 실제로는 약 20 cm 앞에서(목표에 못 미쳐) 멈춥니다. 오차는 d × 0.02 / 1.02 ≈ 19.6 cm 입니다.
  • 두 오차는 서로 거의 수직이라 피타고라스로 더해집니다: 실제 거리 9.80 m 기준 옆 오차 9.80 × sin 1° ≈ 17.1 cm, 진행 방향 오차 10 − 9.80 × cos 1° ≈ 19.7 cm → √(19.7² + 17.1²) ≈ 26.1 cm.
  • 각도 오차 1° 만으로도 약 26 m 를 가면 폭 1.2 m 통로의 선반에 닿습니다. 긴 통로를 오도메트리만으로 다니면 안 되는 이유입니다.
실습 과제 2-2 · 몬테카를로로 오차 타원 키우기 (numpy)

시뮬레이터 3 의 회전 방식 모델을 numpy 로 옮겼습니다. 같은 명령을 200번 실행했다고 상상하고, 왕복마다 귀환점 200개의 공분산 행렬과 2σ 타원 반지름(고윳값의 제곱근 × 2)을 구하세요. 그다음 R_ERR = 0.02 로 바꿔 다시 실행합니다. 예측 먼저: R_ERR = 0 일 때 10회 타원은 1회의 몇 배일까요? R_ERR = 0.02 이면 "평균 위치 오차" 열은 어떻게 자랄까요?

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

D = 8.0          # 통로 길이 (m) - 로봇은 오도메트리가 8 m 라고 할 때 멈춤
TRIPS = 10       # 왕복 횟수
RUNS = 200       # 몬테카를로 반복 (같은 명령을 200 번 실행했다고 상상)
SIG = 0.01       # 미끄러짐 랜덤 크기 1 %
R_ERR = 0.0      # 반지름 설정 오차 (0.02 = 2 % 크게 설정)

rng = np.random.default_rng(6)

def one_run():
    x = y = th = 0.0
    homes = []
    s = 1.0 / (1.0 + R_ERR)                  # 실제 이동 = 오도메트리 값 x r_true / r_set
    for leg in range(2 * TRIPS):
        d = D * s * (1 + rng.normal(0, SIG))
        dth = rng.normal(0, 0.05 * SIG * D)  # 좌우 바퀴 차이로 생기는 방향 흔들림
        x += d * math.cos(th + dth / 2)
        y += d * math.sin(th + dth / 2)
        th += dth
        th += math.pi * s * (1 + rng.normal(0, 0.3 * SIG))   # 끝에서 180 도 제자리 회전
        if leg % 2 == 1:
            homes.append((x, y))
    return homes

data = np.array([one_run() for _ in range(RUNS)])   # (RUNS, TRIPS, 2)

print("왕복  평균 위치오차(m)  2시그마 타원 긴축(m)  짧은축(m)")
for k in range(TRIPS):
    pts = data[:, k, :]
    mean = pts.mean(axis=0)
    # TODO 1: 2x2 공분산 행렬 cov = np.cov(pts.T)
    # TODO 2: 고윳값 ev = np.linalg.eigvalsh(cov)  (작은 값, 큰 값 순서)
    ev = [0.0, 0.0]
    print("%3d    %8.3f          %8.3f           %8.3f" %
          (k + 1, math.hypot(*mean), 2 * math.sqrt(ev[1]), 2 * math.sqrt(ev[0])))
풀이 코드 보기
python시뮬 실행 가능
import math
import numpy as np

D = 8.0          # 통로 길이 (m) - 로봇은 오도메트리가 8 m 라고 할 때 멈춤
TRIPS = 10       # 왕복 횟수
RUNS = 200       # 몬테카를로 반복 (같은 명령을 200 번 실행했다고 상상)
SIG = 0.01       # 미끄러짐 랜덤 크기 1 %
R_ERR = 0.0      # 반지름 설정 오차 (0.02 = 2 % 크게 설정)

rng = np.random.default_rng(6)

def one_run():
    x = y = th = 0.0
    homes = []
    s = 1.0 / (1.0 + R_ERR)                  # 실제 이동 = 오도메트리 값 x r_true / r_set
    for leg in range(2 * TRIPS):
        d = D * s * (1 + rng.normal(0, SIG))
        dth = rng.normal(0, 0.05 * SIG * D)  # 좌우 바퀴 차이로 생기는 방향 흔들림
        x += d * math.cos(th + dth / 2)
        y += d * math.sin(th + dth / 2)
        th += dth
        th += math.pi * s * (1 + rng.normal(0, 0.3 * SIG))   # 끝에서 180 도 제자리 회전
        if leg % 2 == 1:
            homes.append((x, y))
    return homes

data = np.array([one_run() for _ in range(RUNS)])   # (RUNS, TRIPS, 2)

print("왕복  평균 위치오차(m)  2시그마 타원 긴축(m)  짧은축(m)")
for k in range(TRIPS):
    pts = data[:, k, :]
    mean = pts.mean(axis=0)
    cov = np.cov(pts.T)
    ev = np.linalg.eigvalsh(cov)             # 작은 값, 큰 값
    print("%3d    %8.3f          %8.3f           %8.3f" %
          (k + 1, math.hypot(*mean), 2 * math.sqrt(ev[1]), 2 * math.sqrt(ev[0])))

R_ERR = 0 과 R_ERR = 0.02 의 결과(줄 일부):

text
R_ERR = 0.0
왕복  평균 위치오차(m)  2시그마 타원 긴축(m)  짧은축(m)
  1       0.019             0.226              0.147
  5       0.018             0.506              0.360
 10       0.035             0.737              0.515
R_ERR = 0.02
  1       0.486             0.221              0.141
  5       2.391             0.495              0.343
 10       4.557             0.707              0.495
  • 랜덤 오차만 있을 때 평균은 거의 0 이고 타원만 0.23 → 0.74 m 로 약 √10 배 커집니다.
  • 반지름 +2 % 는 타원 크기는 거의 그대로 두고 평균 오차를 왕복당 약 0.48 m 씩 일정하게 늘립니다. 계통 오차는 n 배로 자라지만, 같은 방향으로 틀리므로 측정해서 반지름을 고치면 사라집니다. 실물에서 오도메트리 보정(calibration)을 먼저 하는 이유입니다.
  • 타원의 긴 축은 통로를 가로지르는 방향(y)입니다. 각도 오차가 옆 오차로 커지기 때문입니다.
확인 문제 2-1 · 오차 원인과 결과 짝짓기

시뮬레이터 2 · 3 의 결과를 떠올려, 원인과 가장 잘 맞는 결과를 짝지은 것을 고르세요.

(가) 코드의 바퀴 반지름이 2 % 큼 (나) 젖은 바닥에서 옆걸음 (다) 좌우 바퀴 중 하나의 롤러 마모 (라) 루프가 실제로는 60 ms 인데 Δt = 50 ms 로 계산

  • ⓐ 직진 명령인데 실제 경로가 한쪽으로 휘며 옆 오차가 거리에 비례해 커짐
  • ⓑ 거리 · 회전각 모두 같은 비율로 틀리고, 회전 방식 왕복에서 오차가 왕복마다 일정하게 쌓임
  • ⓒ 오도메트리가 이동 거리를 약 17 % 작게 추정 (실제가 더 감)
  • ⓓ 엔코더는 다 갔다고 하는데 실제로는 20~30 % 덜 가고, 흩어짐도 큼
  1. 가-ⓑ, 나-ⓓ, 다-ⓐ, 라-ⓒ
  2. 가-ⓒ, 나-ⓓ, 다-ⓐ, 라-ⓑ
  3. 가-ⓑ, 나-ⓐ, 다-ⓓ, 라-ⓒ
  4. 가-ⓓ, 나-ⓑ, 다-ⓒ, 라-ⓐ
정답 보기① — (가) 반지름 오차는 계통 오차로 거리와 회전 모두 같은 비율로 틀립니다(시뮬레이터 3 해 볼 것 3). (나) 옆걸음은 롤러 미끄러짐이 커서 μ 가 낮을수록 실제 이동이 크게 줄고 랜덤 성분도 큽니다. (다) 한쪽 바퀴만 덜 밀면 차체가 돌아 각도 오차 → 옆 오차(d·sinΔθ). (라) 실제 60 ms 동안 간 거리를 50 ms 분량으로 계산하니 50/60 ≈ 0.83, 즉 약 17 % 작게 추정합니다.
확인 문제 2-2

실습 과제 2-2(반지름 오차 0, 랜덤 미끄러짐만)에서 왕복 4회의 2σ 타원 긴 반지름이 약 46 cm 였습니다. 같은 조건으로 왕복 16회라면 대략?

  1. 약 46 cm (그대로)
  2. 약 92 cm
  3. 약 184 cm
  4. 약 740 cm
정답 보기② — 랜덤 오차는 √n 으로 자랍니다. 왕복 수가 4배이면 타원은 √4 = 2배, 약 92 cm 입니다. 통로 반폭 0.45 m 를 이미 훌쩍 넘으므로, 오도메트리만으로는 이 통로를 16번 왕복할 수 없습니다. 반지름 오차 같은 계통 오차였다면 4배(③)로 자랐을 것입니다.

3교시 · IMU 자이로로 방향 보정하기#

자이로가 주는 것#

IMU(관성 측정 장치, Inertial Measurement Unit) 는 가속도계와 자이로(gyroscope) 를 묶은 센서입니다. 자이로는 로봇이 도는 각속도 ω (rad/s 또는 °/s) 를 바닥과 상관없이 잽니다. 바퀴가 헛돌아도, 옆걸음 중 롤러가 미끄러져도 차체가 실제로 돈 만큼만 읽힙니다.

바퀴로 구한 방향 θ_wheel자이로를 적분한 방향 θ_gyro
짧은 시간롤러 접촉 · 속도 제어 때문에 출렁임, 미끄러지면 틀림매끈하고 정확
긴 시간출렁임은 평균이 맞음 (미끄러짐은 쌓임)바이어스(bias) 로 천천히 흐름(drift)
오차 모양빠르게 흔들림시간에 비례해 커짐: 오차 = b · t

바이어스는 가만히 있어도 자이로가 읽는 작은 값입니다. 예를 들어 b = 0.2 °/s 이면 1분 뒤 방향이 12° 틀어집니다. 온도에 따라서도 변합니다.

get_all_msg() 의 자이로 원시 데이터#

ch05 에서 본 것처럼 get_all_msg() 의 6번 항목이 자이로 원시 데이터(18바이트를 해석한 값)입니다. 18바이트는 2바이트 정수 9개로 볼 수 있지만, 공식 문서에는 순서 · 단위 · 스케일이 적혀 있지 않습니다.

text
원시 정수 (예: 52)  ×  스케일 (예: 0.1 °/s 한 눈금)  →  5.2 °/s  →  × π/180  →  0.0908 rad/s
  • 이 차시의 시뮬 클래스는 "9개 값 = 가속도 3 · 각속도 3 · 각도 3, z축 각속도는 5번, 1눈금 = 0.1 °/s" 라고 가정합니다. 실물에서는 로봇을 천천히 돌리며 어느 번호가 변하는지, 360° 돌았을 때 적분값이 얼마인지로 확인해야 합니다(실물 실습 ②).
  • 참고로 myagv_ros2 의 myAGV.cpp 는 (myAGV 의 하위 보드 패킷에서) 각속도를 (원시값 − 10000) × 0.1 로 바꿉니다. myAGV Plus 의 ESP32 데이터 형식과 같은지는 확인 필요입니다.

상보 필터#

자이로 ω_gyro빠르고 매끈 · 바이어스 바퀴 θ_wheel출렁임 · 미끄러짐 θ + ω_gyro·Δt× α θ_wheel× (1 − α) + θ (상보 필터 출력)다음 주기의 θ 로 되돌아감 α 가 1 에 가까울수록 빠른 변화는 자이로, 느린 변화는 바퀴를 따름 · 시간 상수 τ = α·Δt / (1 − α) (Δt = 50 ms, α = 0.98 → τ ≈ 2.45 s)
상보 필터: 빠른 변화는 자이로에서, 느린 변화는 바퀴에서 가져옵니다.
text
θ = α · (θ + ω_gyro · Δt) + (1 − α) · θ_wheel          (0 ≤ α ≤ 1)
  • α = 0: 바퀴만 · α = 1: 자이로만. 그 사이 값은 둘을 섞습니다.
  • 매 주기 자이로 쪽에 α 를 곱해 "자이로의 오래된 기억" 을 조금씩 잊고, 그 자리를 바퀴 값이 채웁니다. 잊는 속도는 시간 상수 τ = α·Δt / (1 − α) 입니다. Δt = 50 ms 에서 α = 0.98 이면 τ ≈ 2.45 s, α = 0.995 이면 τ ≈ 10 s 입니다.
  • τ 보다 빠른 바퀴 출렁임은 걸러지고, τ 보다 느린 자이로 드리프트는 바퀴 값이 붙잡아 둡니다. 자이로 바이어스 b 가 남기는 방향 오차는 약 b · τ 로 묶입니다.
  • 이름이 "상보(complementary)" 인 이유: 자이로 쪽 가중치 α 와 바퀴 쪽 1 − α 가 서로를 채워 합이 1 입니다.

시뮬레이터 4 · 미끄러운 바닥의 세 추정값#

물류 피킹 경로(8 s 직진 0.4 m/s → 좌/우 90° 회전을 번갈아 6번, 약 57 s)를 달립니다. 왼쪽은 각 방향 추정으로 그린 지도, 오른쪽은 시간에 따른 방향 오차입니다. 보라는 바퀴만, 파랑은 자이로만, 주황은 상보 필터입니다(Δt = 50 ms).

text
바퀴 ω  = 실제 ω × (1 + 회전 미끄러짐) + d/dt[출렁임 A·(sin 2π·0.9t + 0.5·sin(2π·2.3t + 1))]
자이로 ω = 실제 ω + 바이어스 b + 잡음 N(0, 0.3 °/s)          (모두 교육용 가정)
상보 필터 · 바퀴만(보라) · 자이로만(파랑) · 상보(주황)
해 볼 것 (예측 → 확인)
  1. 미끄러운 바닥에서 α 를 0 · 0.5 · 0.98 로 바꾸기 전에, 셋 중 어느 값의 RMS 방향 오차가 가장 작을지 예측해 적습니다. 버튼으로 확인합니다.
  2. 같은 상황에서 α = 1(자이로만)은 어떤가요? 오른쪽 그래프에서 파랑 선의 모양(직선으로 기울어짐)과 보라 선의 모양(빠르게 흔들림)을 비교합니다.
  3. 자이로 드리프트 큼(b = 1.0 °/s)을 고르고 α 를 0.9 · 0.95 · 0.98 · 0.99 · 0.995 · 1 로 바꿉니다. α 를 1 에 가깝게 할수록 계속 좋아지나요? 표 위의 b·τ 와 비교합니다.
  4. 미끄러운 바닥에서 회전 미끄러짐을 5 % 로 올립니다. 이 오차는 어느 추정에 남고, α 를 아무리 바꿔도 사라지지 않는 이유는 무엇일까요?
  5. 마른 바닥에서는 α = 0(바퀴만)도 꽤 정확합니다. 바닥이 좋으면 상보 필터가 필요 없을까요? 바이어스를 0.5 °/s 로 올려 자이로만과 비교합니다.
관찰 포인트
  • 바퀴 방향은 빠르게 흔들리지만(보라) 평균은 맞고, 자이로 방향은 매끈하지만 b · t 로 기울어집니다(파랑). 상보 필터는 빠른 것은 자이로, 느린 것은 바퀴 에서 가져와 둘의 단점을 함께 줄입니다.
  • α = 0.5 는 τ = 0.05 s 라 사실상 바퀴만과 같습니다. 출렁임(약 0.9 Hz, 주기 1.1 s)을 걸러내려면 τ 가 그보다 길어야 하므로 α 는 0.9 이상이어야 효과가 있습니다.
  • 반대로 α 가 너무 1 에 가까우면 τ 가 길어져 바이어스 오차 b·τ 가 커집니다. 드리프트 1 °/s 에서 α = 0.999 이면 τ ≈ 50 s 로 사실상 자이로만과 같습니다. 이 상황의 좋은 α 는 대략 0.95 ~ 0.99 입니다(b·τ ≈ 1 ~ 5°).
  • 회전 미끄러짐처럼 바퀴 값 자체에 쌓이는 오차는 상보 필터가 결국 따라갑니다. 바퀴가 "느린 기준" 역할을 하기 때문입니다. 바퀴도 자이로도 아닌 바깥 기준이 필요하다는 신호입니다(4교시 · ch07).
  • 숫자는 브라우저 난수를 써서 실습 과제 3-1 의 Python 결과와 소수점 아래가 조금 다를 수 있습니다.

실습 과제 3#

실습 과제 3-1 · 자이로 원시값 변환과 α 비교

교육용 SimIMU 는 시뮬레이터 4 의 "미끄러운 바닥" 과 같은 모델입니다. get_all_msg()[6] 의 5번 값(가정)을 rad/s 로 바꾸는 gyro_rad_s() 와 상보 필터 한 줄을 채우고, α = 0 · 0.5 · 0.98 의 RMS 방향 오차를 비교하세요. 예측 먼저: 시뮬레이터 4 에서 본 것처럼 0.98 이 가장 작을까요? 자이로만(α 와 무관한 가운데 열)은 바퀴만보다 좋을까요?

python시뮬 실행 가능
import math, random

class SimIMU:
    """교육용: 바퀴 각속도(rad/s)와 get_all_msg()[6] 자이로 원시값을 흉내 냄 (모든 수치는 가정)"""
    GYRO_SCALE = 0.1          # 원시값 1 = 0.1 deg/s 로 가정 (실물 단위 확인 필요)
    def __init__(self, bias_dps=0.2, wobble_deg=6.0, seed=1):
        self.t, self.th = 0.0, 0.0
        self.b, self.A = bias_dps, wobble_deg
        self.rnd = random.Random(seed)
        self.seg = []
        for i in range(6):     # 피킹 경로: 8 s 직진 - 좌/우 90 도 회전 번갈아
            self.seg += [(8.0, 0.0), (math.pi / 2, 1.0 if i % 2 == 0 else -1.0)]
        self.T = sum(d for d, _ in self.seg)
    def _w(self, t):
        acc = 0.0
        for d, w in self.seg:
            if t < acc + d: return w
            acc += d
        return 0.0
    def _wob(self, t):         # 바퀴 각도의 출렁임 (속도 제어기가 평균은 맞춤)
        return math.radians(self.A) * (math.sin(2 * math.pi * 0.9 * t) + 0.5 * math.sin(2 * math.pi * 2.3 * t + 1))
    def wheel_omega(self, dt): # 바퀴 순기구학으로 얻은 w (rad/s)
        return self._w(self.t) + (self._wob(self.t + dt) - self._wob(self.t)) / dt
    def get_all_msg(self):     # [6] 은 9개 값: 가속도 3, 각속도 3, 각도 3 이라고 가정
        gz = math.degrees(self._w(self.t)) + self.b + self.rnd.gauss(0, 0.3)
        raw = [0, 0, 98, 0, 0, round(gz / self.GYRO_SCALE), 0, 0, 0]
        return [0, 0, 0, "00", 23.1, 22.9, raw, 23.1, 22.9]
    def wait(self, dt):
        self.th += self._w(self.t) * dt
        self.t += dt

DT = 0.05

def gyro_rad_s(msg):
    # TODO 1: msg[6][5] x GYRO_SCALE 로 deg/s, 다시 rad/s 로
    return 0.0

def run(alpha, bias=0.2, wobble=6.0):
    imu = SimIMU(bias, wobble)
    th_w = th_g = th_c = 0.0
    sq = {"w": 0.0, "g": 0.0, "c": 0.0}; n = 0
    while imu.t < imu.T - 1e-9:
        ww = imu.wheel_omega(DT)
        wg = gyro_rad_s(imu.get_all_msg())
        th_w += ww * DT
        th_g += wg * DT
        # TODO 2: 상보 필터 th_c = alpha * (th_c + wg * DT) + (1 - alpha) * th_w
        imu.wait(DT)
        for k, v in (("w", th_w), ("g", th_g), ("c", th_c)):
            sq[k] += math.degrees(v - imu.th) ** 2
        n += 1
    return {k: math.sqrt(v / n) for k, v in sq.items()}

print("미끄러운 바닥 (바이어스 0.2 deg/s, 출렁임 6 deg)")
print(" alpha   바퀴만   자이로만   상보필터   (RMS 방향 오차, deg)")
for a in (0.0, 0.5, 0.98):
    r = run(a)
    print(" %.2f    %5.2f    %5.2f      %5.2f" % (a, r["w"], r["g"], r["c"]))
풀이 코드 보기
python시뮬 실행 가능
import math, random

class SimIMU:
    """교육용: 바퀴 각속도(rad/s)와 get_all_msg()[6] 자이로 원시값을 흉내 냄 (모든 수치는 가정)"""
    GYRO_SCALE = 0.1          # 원시값 1 = 0.1 deg/s 로 가정 (실물 단위 확인 필요)
    def __init__(self, bias_dps=0.2, wobble_deg=6.0, seed=1):
        self.t, self.th = 0.0, 0.0
        self.b, self.A = bias_dps, wobble_deg
        self.rnd = random.Random(seed)
        self.seg = []
        for i in range(6):     # 피킹 경로: 8 s 직진 - 좌/우 90 도 회전 번갈아
            self.seg += [(8.0, 0.0), (math.pi / 2, 1.0 if i % 2 == 0 else -1.0)]
        self.T = sum(d for d, _ in self.seg)
    def _w(self, t):
        acc = 0.0
        for d, w in self.seg:
            if t < acc + d: return w
            acc += d
        return 0.0
    def _wob(self, t):         # 바퀴 각도의 출렁임 (속도 제어기가 평균은 맞춤)
        return math.radians(self.A) * (math.sin(2 * math.pi * 0.9 * t) + 0.5 * math.sin(2 * math.pi * 2.3 * t + 1))
    def wheel_omega(self, dt): # 바퀴 순기구학으로 얻은 w (rad/s)
        return self._w(self.t) + (self._wob(self.t + dt) - self._wob(self.t)) / dt
    def get_all_msg(self):     # [6] 은 9개 값: 가속도 3, 각속도 3, 각도 3 이라고 가정
        gz = math.degrees(self._w(self.t)) + self.b + self.rnd.gauss(0, 0.3)
        raw = [0, 0, 98, 0, 0, round(gz / self.GYRO_SCALE), 0, 0, 0]
        return [0, 0, 0, "00", 23.1, 22.9, raw, 23.1, 22.9]
    def wait(self, dt):
        self.th += self._w(self.t) * dt
        self.t += dt

DT = 0.05

def gyro_rad_s(msg):
    return math.radians(msg[6][5] * SimIMU.GYRO_SCALE)

def run(alpha, bias=0.2, wobble=6.0):
    imu = SimIMU(bias, wobble)
    th_w = th_g = th_c = 0.0
    sq = {"w": 0.0, "g": 0.0, "c": 0.0}; n = 0
    while imu.t < imu.T - 1e-9:
        ww = imu.wheel_omega(DT)
        wg = gyro_rad_s(imu.get_all_msg())
        th_w += ww * DT
        th_g += wg * DT
        th_c = alpha * (th_c + wg * DT) + (1 - alpha) * th_w
        imu.wait(DT)
        for k, v in (("w", th_w), ("g", th_g), ("c", th_c)):
            sq[k] += math.degrees(v - imu.th) ** 2
        n += 1
    return {k: math.sqrt(v / n) for k, v in sq.items()}

print("미끄러운 바닥 (바이어스 0.2 deg/s, 출렁임 6 deg)")
print(" alpha   바퀴만   자이로만   상보필터   (RMS 방향 오차, deg)")
for a in (0.0, 0.5, 0.98):
    r = run(a)
    print(" %.2f    %5.2f    %5.2f      %5.2f" % (a, r["w"], r["g"], r["c"]))

print()
print("자이로 드리프트 큼 (바이어스 1.0 deg/s): alpha 를 더 촘촘히")
for a in (0.9, 0.95, 0.98, 0.99, 0.995, 0.999):
    tau = a * DT / (1 - a)
    print(" alpha %.3f  tau %5.2f s  b*tau %5.2f deg  RMS %5.2f deg" % (a, tau, tau * 1.0, run(a, bias=1.0)["c"]))

실행 결과:

text
미끄러운 바닥 (바이어스 0.2 deg/s, 출렁임 6 deg)
 alpha   바퀴만   자이로만   상보필터   (RMS 방향 오차, deg)
 0.00     5.36     6.71       5.36
 0.50     5.36     6.71       4.89
 0.98     5.36     6.71       1.99

자이로 드리프트 큼 (바이어스 1.0 deg/s): alpha 를 더 촘촘히
 alpha 0.900  tau  0.45 s  b*tau  0.45 deg  RMS  2.55 deg
 alpha 0.950  tau  0.95 s  b*tau  0.95 deg  RMS  1.73 deg
 alpha 0.980  tau  2.45 s  b*tau  2.45 deg  RMS  0.34 deg
 alpha 0.990  tau  4.95 s  b*tau  4.95 deg  RMS  2.27 deg
 alpha 0.995  tau  9.95 s  b*tau  9.95 deg  RMS  6.37 deg
 alpha 0.999  tau 49.95 s  b*tau 49.95 deg  RMS 21.36 deg
  • 미끄러운 바닥에서는 α = 0.98 이 바퀴만(5.4°) · 자이로만(6.7°)보다 훨씬 좋은 약 2.0° 입니다. α = 0.5 는 τ = 0.05 s 라 바퀴만과 거의 같습니다.
  • 드리프트가 크면 α 를 1 쪽으로 너무 밀 때 b·τ 가 커져 다시 나빠집니다. 이 조건에서는 0.95 ~ 0.99 가 좋은 범위입니다. 0.98 의 0.34° 는 바이어스 쪽 오차와 출렁임이 남긴 오차가 우연히 서로 상쇄된 값이라, 범위로 기억하는 편이 안전합니다.
  • gyro_rad_s 에서 math.radians 를 빠뜨리면 자이로 값이 57배 커져 자이로만 · 상보 필터가 엉뚱한 방향을 냅니다(오류 2).
실습 과제 3-2 · 출발 전 자이로 바이어스 재기

바이어스는 "가만히 있을 때의 평균" 이므로, 출발 전에 몇 초 동안 자이로를 읽어 평균을 빼면 대부분 없앨 수 있습니다. 교육용 SimGyro 는 처음 3초 동안 정지해 있다가 출발합니다(바이어스 0.8 °/s 가정). 예측 먼저: 보정 없이 57초를 달리면 방향이 몇 도 틀어질까요? 보정 후에는 몇 분의 1 이 될까요?

python시뮬 실행 가능
import math, random

class SimGyro:
    """교육용: 처음 3 s 정지 후 출발. gyro_z() 는 deg/s (바이어스 + 잡음 포함, 수치는 가정)"""
    def __init__(self, bias_dps=0.8, seed=3):
        self.t, self.th, self.b = 0.0, 0.0, bias_dps
        self.rnd = random.Random(seed)
        self.seg = [(3.0, 0.0)] + [(8.0, 0.0), (math.pi / 2, 1.0), (8.0, 0.0), (math.pi / 2, -1.0)] * 3
        self.T = sum(d for d, _ in self.seg)
    def _w(self, t):
        acc = 0.0
        for d, w in self.seg:
            if t < acc + d: return w
            acc += d
        return 0.0
    def gyro_z(self):
        return math.degrees(self._w(self.t)) + self.b + self.rnd.gauss(0, 0.3)
    def wait(self, dt):
        self.th += self._w(self.t) * dt
        self.t += dt

DT = 0.05
g = SimGyro()

# 1) 정지 3 초 동안 표본을 모음
samples = []
while g.t < 3.0 - 1e-9:
    samples.append(g.gyro_z())
    g.wait(DT)
# TODO 1: bias = 표본 평균
bias = 0.0
print("정지 %d 개 표본 평균 = %.3f deg/s (실제 바이어스 %.1f)" % (len(samples), bias, g.b))

# 2) 출발 후: 보정 없는 적분 vs 보정한 적분
th_raw = th_cal = 0.0
th0 = g.th
while g.t < g.T - 1e-9:
    z = g.gyro_z()
    th_raw += math.radians(z) * DT
    # TODO 2: th_cal 에는 (z - bias) 를 적분
    g.wait(DT)
truth = g.th - th0
print("주행 %.0f s 후 방향 오차: 보정 없음 %.2f deg, 바이어스 보정 %.2f deg"
      % (g.T - 3.0, math.degrees(th_raw - truth), math.degrees(th_cal - truth)))
풀이 코드 보기
python시뮬 실행 가능
import math, random

class SimGyro:
    """교육용: 처음 3 s 정지 후 출발. gyro_z() 는 deg/s (바이어스 + 잡음 포함, 수치는 가정)"""
    def __init__(self, bias_dps=0.8, seed=3):
        self.t, self.th, self.b = 0.0, 0.0, bias_dps
        self.rnd = random.Random(seed)
        self.seg = [(3.0, 0.0)] + [(8.0, 0.0), (math.pi / 2, 1.0), (8.0, 0.0), (math.pi / 2, -1.0)] * 3
        self.T = sum(d for d, _ in self.seg)
    def _w(self, t):
        acc = 0.0
        for d, w in self.seg:
            if t < acc + d: return w
            acc += d
        return 0.0
    def gyro_z(self):
        return math.degrees(self._w(self.t)) + self.b + self.rnd.gauss(0, 0.3)
    def wait(self, dt):
        self.th += self._w(self.t) * dt
        self.t += dt

DT = 0.05
g = SimGyro()

samples = []
while g.t < 3.0 - 1e-9:
    samples.append(g.gyro_z())
    g.wait(DT)
bias = sum(samples) / len(samples)
print("정지 %d 개 표본 평균 = %.3f deg/s (실제 바이어스 %.1f)" % (len(samples), bias, g.b))

th_raw = th_cal = 0.0
th0 = g.th
while g.t < g.T - 1e-9:
    z = g.gyro_z()
    th_raw += math.radians(z) * DT
    th_cal += math.radians(z - bias) * DT
    g.wait(DT)
truth = g.th - th0
print("주행 %.0f s 후 방향 오차: 보정 없음 %.2f deg, 바이어스 보정 %.2f deg"
      % (g.T - 3.0, math.degrees(th_raw - truth), math.degrees(th_cal - truth)))

실행 결과:

text
정지 60 개 표본 평균 = 0.714 deg/s (실제 바이어스 0.8)
주행 57 s 후 방향 오차: 보정 없음 45.65 deg, 바이어스 보정 4.62 deg
  • 0.8 °/s × 57 s ≈ 46° 가 그대로 쌓였고, 3초 평균을 빼니 약 10분의 1 로 줄었습니다.
  • 남은 4.6° 는 3초 표본 60개의 평균에도 잡음이 섞여 0.086 °/s 만큼 틀렸기 때문입니다(0.086 × 57 ≈ 4.9°). 정지 시간을 늘리면 줄어듭니다.
  • 실물에서는 바이어스가 온도에 따라 바뀌므로, 출발할 때마다(또는 충전대에 서 있을 때마다) 다시 잽니다. 정지 판단은 "바퀴 속도가 모두 0" 일 때로 합니다.
확인 문제 3-1 · 드리프트 상황의 α 고르기

시뮬레이터 4 의 "자이로 드리프트 큼"(b = 1.0 °/s, Δt = 50 ms, 바퀴 출렁임 주기 약 1.1 s)에서 상보 필터의 α 로 가장 알맞은 범위는?

  1. 0 ~ 0.5 — 자이로가 흐르므로 바퀴를 주로 믿는다
  2. 0.6 ~ 0.85 — τ 를 0.1 ~ 0.3 s 로 짧게 둔다
  3. 0.95 ~ 0.99 — τ 를 출렁임 주기보다 길게(약 1 ~ 5 s) 두되, b·τ 가 몇 도 이내가 되게 한다
  4. 0.999 ~ 1 — 자이로가 가장 정확하므로 거의 자이로만 쓴다
정답 보기③ — τ = α·Δt/(1−α) 가 출렁임 주기(1.1 s)보다 짧으면 출렁임이 그대로 들어오고(①, ②), 너무 길면 바이어스 오차 b·τ 가 커집니다(④: α = 0.999 → τ ≈ 50 s → 수십 도). 시뮬레이터 4 와 실습 과제 3-1 에서 0.95 ~ 0.99 구간의 RMS 가 가장 작았습니다.
확인 문제 3-2

시뮬레이터 4 에서 "회전 미끄러짐" 을 5 % 로 올렸더니, α 를 0.9 ~ 0.995 어디로 두어도 상보 필터의 끝 방향 오차가 바퀴만과 비슷하게 남았습니다. 가장 알맞은 해석은?

  1. α 를 정확히 0.97 로 맞추면 사라진다
  2. 상보 필터는 느린 성분을 바퀴에서 가져오므로, 바퀴 값에 쌓인 계통 오차는 결국 따라간다 — 바깥 기준(지도 · LiDAR 등)이나 바퀴 보정이 필요하다
  3. 자이로 바이어스가 원인이다
  4. Δt 를 500 ms 로 늘리면 해결된다
정답 보기② — 회전할 때마다 바퀴가 5 % 더 돈 것으로 세는 오차는 θ_wheel 에 남아 쌓입니다. 상보 필터에서 바퀴는 "느린 기준" 이므로 τ 몇 배의 시간이 지나면 그 오차를 그대로 따라갑니다. 바퀴 값도 결국 누적값이라 진짜 기준이 아니라는 뜻이며, 4교시의 EKF 와 ch07 의 LiDAR 지도가 이 문제를 다룹니다.

4교시 · EKF 와 robot_localization#

칼만 필터 직관#

상보 필터는 α 를 사람이 정했습니다. 칼만 필터(Kalman filter) 는 각 정보가 얼마나 불확실한지를 숫자(분산)로 들고 다니며, 섞는 비율을 매번 스스로 계산합니다.

위치 x (m) ① 예측 x⁻ (넓음: P⁻) ② 측정 z (R) ③ 합친 추정 x (가장 좁음) x⁻zx 예측: x⁻ = x + u·Δt, P⁻ = P + Q 칼만 이득: K = P⁻ / (P⁻ + R) (0 ≤ K ≤ 1) 측정 반영: x = x⁻ + K·(z − x⁻), P = (1 − K)·P⁻ → K 가 1 에 가까우면 센서를, 0 에 가까우면 모델을 따름
예측(넓은 종)과 측정(좁은 종)을 합치면 둘보다 좁은 종이 됩니다. 합친 위치는 더 확실한 쪽에 가깝습니다.
기호뜻커지면
x, P추정값과 그 분산 (불확실성)P 가 크면 "잘 모름"
u모델 입력 (여기서는 오도메트리 속도)
Q공정 잡음: 예측 한 번마다 모델이 틀릴 수 있는 정도모델을 덜 믿음 → 센서를 더 따름
R측정 잡음: 센서 한 번이 틀릴 수 있는 정도센서를 덜 믿음 → 모델을 더 따름
K칼만 이득 = P⁻ / (P⁻ + R), 0 ~ 11 이면 측정값으로 바꿈, 0 이면 무시
text
① 예측:  x⁻ = x + u·Δt          P⁻ = P + Q          (움직일수록 불확실성이 커짐)
② 측정:  K = P⁻ / (P⁻ + R)
         x = x⁻ + K · (z − x⁻)   P = (1 − K) · P⁻    (측정하면 불확실성이 줄어듦)
  • 1차원에서는 이것이 전부입니다. 상보 필터의 (1 − α) 자리에 K 가 들어가는데, K 는 P 와 R 로부터 자동으로 정해집니다.
  • EKF(확장 칼만 필터, Extended Kalman Filter) 는 같은 일을 (x, y, θ, vx, vy, ω …) 여러 변수로, 그리고 cos θ · sin θ 처럼 비선형인 운동 모델을 현재 추정점에서 직선으로 근사(선형화)해서 합니다. 이때 P 는 행렬이 되고, 2교시의 오차 타원이 바로 P 의 (x, y) 부분입니다.

시뮬레이터 5 · 1차원 칼만 필터: Q 와 R#

로봇이 20 m 선반 통로를 0.5 m/s 로 16초 가고 4초 쉬기를 두 번 합니다. 모델은 오도메트리 속도(실제로는 3 % 미끄러짐 + 흔들림이 있음), 측정은 0.2초마다 들어오는 잡음 섞인 위치 센서입니다. 위 그래프는 각 추정의 오차(추정 − 실제), 아래 그래프는 칼만 이득 K 입니다.

어떤 위치 센서일까요?

물류 현장에서는 바닥 표식(QR · 마그네틱 테이프), UWB 같은 무선 측위, LiDAR 와 지도 맞추기 등이 "바깥 기준" 위치를 줍니다. 여기서는 종류와 상관없이 잡음 σ_z 를 가진 위치 측정으로만 다룹니다. LiDAR 는 ch07 에서 다룹니다.

1차원 칼만 필터 · 오도메트리(파랑) · 측정(회색 점) · 칼만(주황, 띠 = ±2√P)
해 볼 것 (예측 → 확인)
  1. 기본값(Q = 1e-4, R ≈ 0.09 = 0.3²)에서 칼만의 RMS 오차가 오도메트리만 · 센서만보다 작은지 확인합니다. 파랑 선은 왜 시간이 갈수록 아래로 내려가나요?
  2. R ×10 을 누르기 전에, 주황 선이 회색 점(센서)을 더 따를지 파랑 선(모델)을 더 따를지 예측해 적습니다. K 그래프는 어떻게 바뀔까요?
  3. R 을 되돌리고 Q ×10 을 누릅니다. 결과가 2번과 반대 방향인가요? 이번엔 Q ×10 과 R ×10 을 함께 눌러 봅니다. K 는 어떻게 되나요?
  4. Q 를 가장 작게(1e-6) 내립니다. 처음엔 괜찮다가 시간이 지날수록 주황 선이 파랑 선처럼 내려가는 이유는 무엇일까요?
  5. 20~30 s 센서 끊김을 켭니다. 그 10초 동안 주황 띠(±2√P)와 오차는 어떻게 되고, 센서가 돌아온 순간 K 는 어떻게 되나요?
관찰 포인트
  • 오도메트리만(파랑)은 3 % 미끄러짐이 계통 오차라 계속 한쪽으로 쌓입니다. 센서는 쌓이지 않지만 매번 ±0.3 m 씩 튑니다. 칼만은 "센서의 장기 기준 + 모델의 매끈함" 을 가져옵니다. 3교시 상보 필터와 같은 생각입니다.
  • R 을 키우면 K 가 작아져 모델을 더 따르고(매끈하지만 계통 오차에 끌려감), Q 를 키우면 K 가 커져 센서를 더 따릅니다(빠르지만 잡음이 섞임). K 는 Q 와 R 의 비율로 정해지므로 Q · R 을 같은 배수로 바꾸면 K 는 그대로입니다.
  • Q 가 너무 작으면 P 가 계속 줄어 K 가 0 에 가까워집니다. 필터가 "나는 이미 정확하다" 고 믿고 센서를 무시하다가 모델 오차에 끌려갑니다. Q 를 너무 작게 두는 것이 가장 흔한 튜닝 실수입니다.
  • 센서가 끊기면 예측만 하므로 P 가 Q 씩 커져 띠가 넓어지고, 돌아온 첫 측정에서 K 가 커져 크게 바로잡습니다. EKF 도 바깥 기준이 없으면 결국 오도메트리와 같아진다는 뜻입니다.

myagv_ros2 의 구성: myagv_odometry + robot_localization#

하위 보드vx · vy · vθ가속도 · 각속도 · yaw myagv_odometry_node myagv_odometry/src/myAGV.cpp θ = IMU yaw 누적 (0.1° 미만 변화는 버림) x += (vx·cosθ − vy·sinθ)·dt y += (vx·sinθ + vy·cosθ)·dt 자체 odom TF 방송은 주석 처리 ekf_node robot_localization config/ekf.yaml 30 Hz · two_d_mode 직렬 /odom /imu /odometry/filtered TF odom →base_footprint odom0_config: vx · vy · vyaw 만 true (odom 의 위치 · 각도는 쓰지 않음) imu0_config: vyaw 만 true → EKF 가 "속도들" 을 합쳐 스스로 적분합니다 myagv_ros2 의 myagv_active.launch.py 구성 (myAGV 용 저장소 — Plus 적용은 ch09 에서 확인)
myagv_ros2 의 myagv_active.launch.py 가 띄우는 오도메트리 · EKF 부분.

myagv_ros2 저장소의 myagv_odometry/launch/myagv_active.launch.py 는 myagv_odometry_node, robot_state_publisher, IMU · 카메라의 정적 TF, **robot_localization 의 ekf_node**, LiDAR 드라이버를 함께 띄웁니다. 오도메트리와 관련된 부분만 보면 다음과 같습니다(ROS 2 의 실행 · 설정은 ch09 에서 자세히 합니다).

구성 요소하는 일 (저장소 코드 기준)
myagv_odometry_node (src/myAGV.cpp)하위 보드에서 vx · vy · vθ (1바이트, (값 − 128) × 0.01) 와 IMU 값을 받아 /odom (nav_msgs/Odometry) · /imu (sensor_msgs/Imu) 를 발행
그 안의 오도메트리θ 는 IMU yaw 를 누적(변화가 0.1 미만이면 0 으로 버림), x · y 는 1교시와 같은 x += (vx·cosθ − vy·sinθ)·dt, dt 는 매번 잰 시간
자체 TFodomBroadcaster->sendTransform 줄이 주석 처리되어 있음 — "robot_localization 을 대신 쓴다" 는 주석
ekf_node (config/ekf.yaml)/odom 과 /imu 를 융합해 /odometry/filtered 와 TF odom → base_footprint 를 발행

config/ekf.yaml 의 핵심 줄(원문 그대로):

yaml
ekf_node:
    ros__parameters:
        frequency: 30.0
        two_d_mode: true
        publish_tf: true
        odom_frame: odom
        base_link_frame: base_footprint
        world_frame: odom
        odom0: odom
        odom0_config: [false, false, false,
                      false, false, false,
                      true, true, false,
                      false, false, true,
                      false, false, false]
        imu0: imu
        imu0_config: [false, false, false,
                      false, false, false,
                      false, false, false,
                      false, false, true,
                      false, false, false]
  • *_config 의 15칸은 [x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az] 순서입니다. /odom 에서는 vx · vy · vyaw, /imu 에서는 vyaw 만 씁니다. 즉 EKF 는 위치를 받지 않고 속도들을 합쳐 스스로 적분합니다.
  • two_d_mode: true 는 z · roll · pitch 를 0 으로 둡니다(평평한 바닥의 AGV). world_frame: odom 이므로 이 EKF 의 결과도 여전히 "오도메트리 좌표계" 이며, 시간이 지나면 흐릅니다.
  • 같은 vyaw 를 두 센서가 주므로, 어느 쪽을 더 믿을지는 각 메시지의 공분산(R 에 해당)으로 정해집니다. myAGV.cpp 는 IMU 각속도 z 의 공분산을 1e-6, odom twist 의 yaw 성분을 1e-9 로 고정해 보냅니다. 값이 작을수록 "매우 정확하다" 는 뜻이라, 두 값의 상대 크기가 실제 센서 품질과 맞는지는 검토가 필요합니다(확인 필요).
ROS 의 좌표계 이름 (REP 105)

ch02 에서 본 것처럼 base_footprint 는 로봇 바닥 중심, odom 은 "출발점 기준, 연속적이지만 흐르는" 좌표계, map 은 "흐르지 않지만 가끔 점프할 수 있는" 좌표계입니다. 이 차시의 오도메트리 · EKF 는 모두 odom → base_footprint 를 만듭니다. map → odom 은 바깥 기준(LiDAR + 지도)이 있어야 만들 수 있으며 ch07 · ch10 에서 다룹니다.

EKF 로도 지울 수 없는 오차#

오차바퀴+ 자이로 (상보 · EKF)+ 바깥 기준 (LiDAR · 지도 · 바닥 표식)
바퀴 출렁임 · 짧은 미끄러짐의 방향 오차큼작아짐작음
자이로 바이어스–정지 보정 후 작음, 남은 것은 쌓임작음
옆걸음 · 미끄러짐으로 인한 거리 오차큼그대로 (자이로는 거리를 모름)작음
시간에 따라 쌓이는 위치 오차쌓임느리게 쌓임묶임 (bounded)
  • 바퀴와 자이로는 모두 자기 몸의 움직임만 재는 센서입니다. 둘을 아무리 잘 섞어도 "지금 지도 위 어디에 있는가" 를 직접 재지 않으므로 오차는 결국 쌓입니다.
  • 시뮬레이터 5 에서 위치 센서가 있을 때만 오차가 묶였던 것처럼, 바깥 세상을 보는 센서가 있어야 오차가 일정 범위 안에 머뭅니다. ch07 에서 LiDAR 로 주변을 재고 점유 격자 지도를 만드는 이유입니다.

실습 과제 4#

실습 과제 4-1 · 1차원 칼만 필터와 Q · R

시뮬레이터 5 와 같은 모델을 Python 으로 옮겼습니다. TODO 를 채워 예측 · 측정 단계를 완성하고, Q · R 을 바꿔 RMS 오차와 K 를 비교하세요. 예측 먼저: R 을 10배(0.09 → 0.9)로 키우면 K 는 커질까요, 작아질까요? 추정은 센서와 모델 중 어느 쪽을 더 따를까요? Q 를 10배 키우는 것과 R 을 10분의 1 로 줄이는 것은 결과가 같을까요?

python시뮬 실행 가능
import math, random

def simulate(Q, R, seed=4, sig_z=0.3, drop=False):
    """통로 20 m 를 0.5 m/s 로 이동. 모델(오도메트리 속도) + 0.2 s 마다 위치 측정 z"""
    rnd = random.Random(seed)
    dt = 0.05
    x_true = 0.0
    x, P = 0.0, 0.01          # 칼만 추정과 분산
    x_odo = 0.0               # 오도메트리만
    rows = []
    K = 0.0
    t = 0.0
    k = 0
    while t < 40.0 - 1e-9:
        u = 0.5 if (t % 20) < 16 else 0.0                     # 16 s 가고 4 s 쉼
        v_true = u * 0.97 + rnd.gauss(0, 0.03) * (u > 0)      # 실제: 3 % 미끄러짐 + 흔들림
        x_true += v_true * dt
        x_odo += u * dt
        # TODO 1: 예측 - x 에 u*dt 를 더하고, P 에 Q 를 더함
        k += 1
        z = None
        if k % 4 == 0 and not (drop and 20 <= t < 30):
            z = x_true + rnd.gauss(0, sig_z)
            # TODO 2: K = P / (P + R),  x = x + K*(z - x),  P = (1 - K)*P
        t += dt
        rows.append((t, x_true, x_odo, z, x, P, K))
    return rows

def report(Q, R, drop=False):
    rows = simulate(Q, R, drop=drop)
    e_odo = math.sqrt(sum((r[2] - r[1]) ** 2 for r in rows) / len(rows))
    zs = [(r[3] - r[1]) for r in rows if r[3] is not None]
    e_z = math.sqrt(sum(e * e for e in zs) / len(zs))
    e_kf = math.sqrt(sum((r[4] - r[1]) ** 2 for r in rows) / len(rows))
    K = rows[-1][6]
    print("Q=%.0e R=%.3f | RMS 오도메트리 %.3f  센서 %.3f  칼만 %.3f m | K=%.3f"
          % (Q, R, e_odo, e_z, e_kf, K))
    return rows

rows = report(1e-4, 0.09)
print("  t(s)  실제x   오도메트리   측정z    칼만x   2*sqrt(P)")
for r in rows[39::80]:
    print("%5.1f  %6.2f   %6.2f     %6s   %6.2f   %.3f" % (r[0], r[1], r[2], "-" if r[3] is None else "%.2f" % r[3], r[4], 2 * math.sqrt(r[5])))
report(1e-4, 0.9)
report(1e-3, 0.09)
report(1e-6, 0.09)
report(1e-4, 0.009)
풀이 코드 보기
python시뮬 실행 가능
import math, random

def simulate(Q, R, seed=4, sig_z=0.3, drop=False):
    """통로 20 m 를 0.5 m/s 로 이동. 모델(오도메트리 속도) + 0.2 s 마다 위치 측정 z"""
    rnd = random.Random(seed)
    dt = 0.05
    x_true = 0.0
    x, P = 0.0, 0.01          # 칼만 추정과 분산
    x_odo = 0.0               # 오도메트리만
    rows = []
    K = 0.0
    t = 0.0
    k = 0
    while t < 40.0 - 1e-9:
        u = 0.5 if (t % 20) < 16 else 0.0                     # 16 s 가고 4 s 쉼
        v_true = u * 0.97 + rnd.gauss(0, 0.03) * (u > 0)      # 실제: 3 % 미끄러짐 + 흔들림
        x_true += v_true * dt
        x_odo += u * dt
        # ① 예측
        x += u * dt
        P += Q
        # ② 측정 (0.2 s 마다, drop 이면 20~30 s 끊김)
        k += 1
        z = None
        if k % 4 == 0 and not (drop and 20 <= t < 30):
            z = x_true + rnd.gauss(0, sig_z)
            K = P / (P + R)
            x = x + K * (z - x)
            P = (1 - K) * P
        t += dt
        rows.append((t, x_true, x_odo, z, x, P, K))
    return rows

def report(Q, R, drop=False):
    rows = simulate(Q, R, drop=drop)
    e_odo = math.sqrt(sum((r[2] - r[1]) ** 2 for r in rows) / len(rows))
    zs = [(r[3] - r[1]) for r in rows if r[3] is not None]
    e_z = math.sqrt(sum(e * e for e in zs) / len(zs))
    e_kf = math.sqrt(sum((r[4] - r[1]) ** 2 for r in rows) / len(rows))
    K = rows[-1][6]
    print("Q=%.0e R=%.3f | RMS 오도메트리 %.3f  센서 %.3f  칼만 %.3f m | K=%.3f"
          % (Q, R, e_odo, e_z, e_kf, K))
    return rows

rows = report(1e-4, 0.09)
print("  t(s)  실제x   오도메트리   측정z    칼만x   2*sqrt(P)")
for r in rows[39::80]:
    print("%5.1f  %6.2f   %6.2f     %6s   %6.2f   %.3f" % (r[0], r[1], r[2], "-" if r[3] is None else "%.2f" % r[3], r[4], 2 * math.sqrt(r[5])))
report(1e-4, 0.9)
report(1e-3, 0.09)
report(1e-6, 0.09)
report(1e-4, 0.009)
print("센서 끊김 20~30 s:")
rows = report(1e-4, 0.09, drop=True)
for r in rows[379:640:80]:
    print("%5.1f  2*sqrt(P) = %.3f" % (r[0], 2 * math.sqrt(r[5])))

실행 결과(일부):

text
Q=1e-04 R=0.090 | RMS 오도메트리 0.254  센서 0.313  칼만 0.060 m | K=0.064
Q=1e-04 R=0.900 | RMS 오도메트리 0.254  센서 0.313  칼만 0.099 m | K=0.021
Q=1e-03 R=0.090 | RMS 오도메트리 0.254  센서 0.313  칼만 0.085 m | K=0.190
Q=1e-06 R=0.090 | RMS 오도메트리 0.254  센서 0.313  칼만 0.127 m | K=0.008
Q=1e-04 R=0.009 | RMS 오도메트리 0.254  센서 0.313  칼만 0.085 m | K=0.190
센서 끊김 20~30 s:
Q=1e-04 R=0.090 | RMS 오도메트리 0.244  센서 0.316  칼만 0.071 m | K=0.065
 19.0  2*sqrt(P) = 0.152
 23.0  2*sqrt(P) = 0.217
 27.0  2*sqrt(P) = 0.281
 31.0  2*sqrt(P) = 0.214
  • 칼만(6.0 cm)은 오도메트리(25.4 cm) · 센서(31.3 cm) 둘 다보다 정확합니다.
  • R ×10 → K 가 0.064 에서 0.021 로 작아져 모델을 더 따르고, 3 % 미끄러짐에 끌려가 오차가 9.9 cm 로 커졌습니다. Q ×10 과 R ÷10 은 K 와 결과가 똑같습니다(0.190, 8.5 cm). 비율 Q/R 만 중요합니다.
  • Q = 1e-6 이면 K ≈ 0.008 로 센서를 거의 무시해 12.7 cm 가 됩니다. 센서가 끊기면 2√P 가 0.15 → 0.28 m 로 커졌다가, 센서가 돌아오자 다시 줄어듭니다.
실습 과제 4-2 · ekf.yaml 읽기: 무엇을 융합하나?

odom0_config · imu0_config 의 15칸 true/false 목록을 사람이 읽기 쉬운 이름으로 바꾸는 함수를 만듭니다. 설정 파일을 바꿨을 때 "무엇을 융합하는지" 를 바로 확인하는 작은 도구입니다. 예측 먼저: myagv_ros2 의 설정에서 /odom 의 위치 x, y 는 융합될까요?

python시뮬 실행 가능
# myagv_ros2/myagv_odometry/config/ekf.yaml 에서 옮겨 온 줄
EKF_YAML = """
ekf_node:
    ros__parameters:
        frequency: 30.0
        two_d_mode: true
        odom_frame: odom
        base_link_frame: base_footprint
        world_frame: odom
        odom0: odom
        odom0_config: [false, false, false,
                      false, false, false,
                      true, true, false,
                      false, false, true,
                      false, false, false]
        imu0: imu
        imu0_config: [false, false, false,
                      false, false, false,
                      false, false, false,
                      false, false, true,
                      false, false, false]
"""

NAMES = ["x", "y", "z", "roll", "pitch", "yaw",
         "vx", "vy", "vz", "vroll", "vpitch", "vyaw",
         "ax", "ay", "az"]

def read_configs(text):
    """'이름_config: [ ... ]' 여러 줄 목록을 {이름: [True/False x 15]} 로"""
    out, key, buf = {}, None, ""
    for line in text.splitlines():
        s = line.strip()
        if key is None and "_config:" in s:
            key = s.split("_config:")[0]
            buf = s.split(":", 1)[1]
        elif key is not None:
            buf += s
        if key is not None and "]" in buf:
            # TODO 1: buf 의 [ ] 를 벗기고 쉼표로 나눠, "true" 이면 True 인 목록을 out[key] 에 저장
            key, buf = None, ""
    return out

cfg = read_configs(EKF_YAML)
for src, flags in cfg.items():
    # TODO 2: flags 가 True 인 칸의 NAMES 를 모아 used 목록 만들기
    used = []
    print("%-5s (%d 개) 융합 항목: %s" % (src, len(flags), ", ".join(used)))
풀이 코드 보기
python시뮬 실행 가능
# myagv_ros2/myagv_odometry/config/ekf.yaml 에서 옮겨 온 줄
EKF_YAML = """
ekf_node:
    ros__parameters:
        frequency: 30.0
        two_d_mode: true
        odom_frame: odom
        base_link_frame: base_footprint
        world_frame: odom
        odom0: odom
        odom0_config: [false, false, false,
                      false, false, false,
                      true, true, false,
                      false, false, true,
                      false, false, false]
        imu0: imu
        imu0_config: [false, false, false,
                      false, false, false,
                      false, false, false,
                      false, false, true,
                      false, false, false]
"""

NAMES = ["x", "y", "z", "roll", "pitch", "yaw",
         "vx", "vy", "vz", "vroll", "vpitch", "vyaw",
         "ax", "ay", "az"]

def read_configs(text):
    """'이름_config: [ ... ]' 여러 줄 목록을 {이름: [True/False x 15]} 로"""
    out, key, buf = {}, None, ""
    for line in text.splitlines():
        s = line.strip()
        if key is None and "_config:" in s:
            key = s.split("_config:")[0]
            buf = s.split(":", 1)[1]
        elif key is not None:
            buf += s
        if key is not None and "]" in buf:
            items = buf.strip().strip("[]").split(",")
            out[key] = [it.strip() == "true" for it in items if it.strip()]
            key, buf = None, ""
    return out

cfg = read_configs(EKF_YAML)
for src, flags in cfg.items():
    used = [NAMES[i] for i, f in enumerate(flags) if f]
    print("%-5s (%d 개) 융합 항목: %s" % (src, len(flags), ", ".join(used)))

실행 결과:

text
odom0 (15 개) 융합 항목: vx, vy, vyaw
imu0  (15 개) 융합 항목: vyaw
  • /odom 의 위치 x, y 와 yaw 는 융합하지 않습니다. 노드가 이미 적분한 위치를 다시 넣으면 같은 정보를 두 번 쓰는 셈이고, 위치가 흐를수록 EKF 도 같이 흐르기 때문입니다. 속도만 넣고 EKF 가 적분하게 하는 것이 robot_localization 에서 흔히 권하는 방식입니다.
  • 칸이 15개가 아니면(쉼표 하나 빠짐) robot_localization 이 설정을 거부하거나 엉뚱한 변수에 매핑합니다. 설정을 고친 뒤 이 도구로 개수와 이름을 확인합니다(오류 8).
확인 문제 4-1 · R 을 10배 키우면

시뮬레이터 5 에서 R 을 0.09 에서 0.9 로 10배 키웠습니다. 일어나는 일로 맞는 것은?

  1. K 가 커지고, 추정이 센서 값을 더 따라 회색 점처럼 튄다
  2. K 가 작아지고, 추정이 모델(오도메트리)을 더 따라 매끈해지지만 3 % 미끄러짐 쪽으로 끌려간다
  3. K 는 그대로이고 ±2√P 띠만 넓어진다
  4. 센서 값을 완전히 무시하므로 오도메트리와 똑같아진다
정답 보기② — K = P⁻/(P⁻+R) 이므로 R 이 커지면 K 가 작아집니다(0.064 → 0.021). 센서를 덜 믿으니 매끈하지만, 모델의 계통 오차를 늦게 바로잡아 오차가 커졌습니다. K 가 0 은 아니므로 오도메트리와 똑같아지지는 않습니다(④ 틀림).
확인 문제 4-2 · Q · R 조절 결과

다음 중 시뮬레이터 5 · 실습 과제 4-1 의 결과로 틀린 것은?

  1. Q 를 10배 키우는 것과 R 을 10분의 1 로 줄이는 것은 K 와 추정 결과가 같다
  2. Q 를 너무 작게 하면 P 가 계속 줄어 센서를 거의 무시하게 된다
  3. 센서가 끊기면 P 가 커지고, 다시 들어온 첫 측정의 K 가 커진다
  4. Q · R 을 둘 다 10배 키우면 K 도 10배가 된다
정답 보기④ — K 는 Q 와 R 의 비율로 정해지므로 둘을 같은 배수로 바꾸면 K 는 (정상 상태에서) 그대로입니다. 시뮬레이터 5 해 볼 것 3 에서 확인한 내용입니다. ①~③ 은 모두 관찰한 그대로입니다.
확인 문제 4-3

myagv_ros2 의 ekf.yaml 에서 world_frame: odom, odom0_config 는 vx · vy · vyaw, imu0_config 는 vyaw 만 true 입니다. 이 EKF 의 출력 /odometry/filtered 에 대한 설명으로 맞는 것은?

  1. 지도 기준 절대 위치이므로 오래 달려도 흐르지 않는다
  2. 바퀴 · 자이로의 속도를 합쳐 적분한 odom 좌표계 위치로, 매끈하지만 시간이 지나면 흐른다
  3. /odom 의 x, y 를 그대로 복사한 것이다
  4. IMU 의 yaw 각도를 직접 융합한다
정답 보기② — 바깥 기준이 없으므로 결과는 여전히 odom 좌표계이며 오차가 쌓입니다(① 틀림). /odom 의 위치는 융합하지 않고 속도만 씁니다(③ 틀림). IMU 쪽도 각속도 vyaw 만 true 입니다(④ 틀림). 흐르지 않는 map 기준 위치는 ch07 · ch10 의 LiDAR · 지도가 필요합니다.

모델 확인 (3D 뷰어)#

3D 뷰어 열기

모델이 준비되면

강좌용 myAGV Plus 모델(assets/robot/robot.json)이 아직 없다면 뷰어에 "모델 없음" 안내가 나옵니다. 모델이 준비되면 다음을 확인합니다.

  • base_footprint 와 IMU 링크(imu_link)의 위치 · 방향. myagv_ros2 의 launch 파일은 base_footprint → imu_link 정적 TF 를 0 0 0 0 3.14159 3.14159 로 줍니다. 마지막 두 값이 pitch · roll 로 π 씩 돌린 것이라면 IMU 가 뒤집혀 장착된 것이고, z축 각속도의 부호가 반대가 됩니다(오류 1). 실제 장착 방향은 확인 필요입니다.
  • 바퀴 링크의 반지름과 바퀴 사이 거리. 1교시의 r = 0.05 m, L = 0.22 m 를 모델의 값과 비교합니다.

모델이 없을 때는 아래 계산으로 IMU 가 뒤집혔을 때의 부호를 확인할 수 있습니다.

python시뮬 실행 가능
import numpy as np

def rot_x(a):
    c, s = np.cos(a), np.sin(a)
    return np.array([[1, 0, 0], [0, c, -s], [0, s, c]])

def rot_y(a):
    c, s = np.cos(a), np.sin(a)
    return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]])

# static_transform_publisher 인수: x y z yaw pitch roll = 0 0 0 0 3.14159 3.14159
R_base_imu = rot_y(3.14159) @ rot_x(3.14159)       # yaw 0 -> pitch pi -> roll pi (ZYX 순서)
w_base = np.array([0.0, 0.0, 0.5])                 # 로봇이 왼쪽으로 0.5 rad/s 회전
w_imu = R_base_imu.T @ w_base                      # IMU 좌표계에서 본 각속도
print("base 기준 w =", np.round(w_base, 3))
print("IMU 기준 w  =", np.round(w_imu, 3))
print("IMU z 축 부호:", "같음" if w_imu[2] > 0 else "반대")
  • pitch π 와 roll π 를 함께 돌리면 결과는 yaw π 회전과 같아서 z축은 그대로입니다(실행해서 "같음" 을 확인합니다). 숫자 몇 개가 회전 순서 · 축에 따라 전혀 다른 장착을 뜻할 수 있으므로, 실물에서는 로봇을 왼쪽으로 돌렸을 때 자이로 z 가 + 인지 직접 봅니다(실물 실습 ②).

실습 (실물)#

실물 실행 전 확인

① 1 m 직진 오도메트리 비교 — 바퀴 속도를 50 ms 마다 읽어 오도메트리를 계산하며 0.15 m/s 로 직진하다가, 오도메트리가 1.0 m 라고 하면 멈춥니다. 실제 이동 거리를 줄자로 재어 비교합니다.

python실물 전용
import math, time
from pymycobot import MyAGVPlus

R, L = 0.05, 0.22             # 교육용 가정값 - 실물 측정으로 고칠 값
agv = MyAGVPlus("/dev/myagvplus_controller", baudrate=921600,
                esp32_port="/dev/myagvplus_esp32", esp32_baud=115200)

def fk(w):
    w1, w2, w3, w4 = w
    return (R / 4 * (w1 + w2 + w3 + w4), R / 4 * (-w1 + w2 + w3 - w4),
            R / (4 * L) * (-w1 + w2 - w3 + w4))

x = y = th = 0.0
log = []
try:
    print("power_on ->", agv.power_on())
    print("move_forward ->", agv.move_forward(0.15))
    t_prev = time.monotonic()
    while x < 1.0:
        w = [agv.get_motor_velocity(i) for i in range(1, 5)]
        now = time.monotonic()
        dt = now - t_prev                      # 실제로 흐른 시간
        t_prev = now
        if None in w or -1 in w:
            print("읽기 실패:", w); break
        vx, vy, om = fk(w)
        x += (vx * math.cos(th) - vy * math.sin(th)) * dt
        y += (vx * math.sin(th) + vy * math.cos(th)) * dt
        th += om * dt
        log.append((round(dt * 1000), w))
        time.sleep(max(0.0, 0.05 - (time.monotonic() - now)))
finally:
    agv.stop()

print("오도메트리: x=%.3f y=%.3f th=%.2f deg" % (x, y, math.degrees(th)))
print("루프 주기 ms (처음 10개):", [d for d, _ in log[:10]])
print("바퀴 값 예:", log[len(log) // 2][1])

② 자이로 원시값 확인과 바이어스 — 정지 5초 동안 get_all_msg()[6] 의 9개 값 평균을 내고, 이어서 turn_left(0.3) 으로 3초 돌며 어느 번호가 크게 변하는지 봅니다.

python실물 전용
import time
from pymycobot import MyAGVPlus

agv = MyAGVPlus("/dev/myagvplus_controller", baudrate=921600,
                esp32_port="/dev/myagvplus_esp32", esp32_baud=115200)

def mean_raw(sec):
    acc, n = None, 0
    t_end = time.monotonic() + sec
    while time.monotonic() < t_end:
        msg = agv.get_all_msg()
        if msg and len(msg) > 6 and isinstance(msg[6], (list, tuple)):
            v = list(msg[6])
            acc = v if acc is None else [a + b for a, b in zip(acc, v)]
            n += 1
        time.sleep(0.05)
    return [round(a / n, 2) for a in acc] if n else None

try:
    still = mean_raw(5.0)
    print("정지 평균 :", still)
    agv.power_on()
    agv.turn_left(0.3)
    turning = mean_raw(3.0)
    print("회전 평균 :", turning)
finally:
    agv.stop()

if still and turning:
    diff = [round(b - a, 2) for a, b in zip(still, turning)]
    print("차이      :", diff, "<- 가장 크게 변한 번호가 z 축 각속도 후보")
실물 실습 순서
  1. ① 을 3번 실행하고, 매번 줄자로 실제 이동 거리를 잽니다. 평균 실제 거리가 0.97 m 라면 오도메트리가 3 % 크게 센다는 뜻이므로 R 을 0.05 × 0.97 로 고친 뒤 다시 3번 잽니다(반지름 보정).
  2. ① 의 "루프 주기 ms" 가 50 근처인지 봅니다. 60 ms 를 넘게 자주 나오면 dt 를 고정값으로 쓰는 코드가 얼마나 틀릴지 계산합니다(확인 문제 2-1 의 (라)).
  3. ② 를 실행해 z축 각속도 후보 번호를 찾고, 정지 평균값(바이어스)을 기록합니다. 0.3 rad/s ≈ 17.2 °/s 와 "차이" 값을 비교해 스케일(한 눈금이 몇 °/s 인지)을 추정합니다(확인 필요 항목 정리).
  4. 1 m × 1 m 사각형(전진 + 제자리 90° 회전)을 오도메트리로 돌고 돌아와, 출발점 테이프와의 차이를 잽니다. 바퀴만 · 자이로 보정 후를 비교하면 이 차시의 결론을 실물로 확인할 수 있습니다.

자주 나는 오류와 해결#

1) 왼쪽으로 돌았는데 추정 방향이 오른쪽으로 돈다#

text
turn_left(0.3) 후 오도메트리 th = -15.2 deg   (기대: +)
  • 원인: 바퀴 번호와 위치의 대응(M1~M4)이 실물과 다르거나, 오른쪽 바퀴 부호가 반대로 보고됩니다. 자이로라면 IMU 가 뒤집혀 장착되어 z축 부호가 반대입니다.
  • 해결: 바퀴를 띄우고 turn_left 를 저속으로 보내 네 바퀴 값의 부호를 기록해 ch03 의 (−, +, −, +) 패턴과 비교합니다. 자이로는 3D 뷰어 절의 계산과 실물 실습 ② 로 부호를 확인하고, 필요하면 -1 을 곱합니다.

2) 직진만 했는데 추정 궤적이 크게 휘거나 원을 그림#

text
x=0.21 y=0.63 th=4170.3 deg
  • 원인: θ 를 도(°) 로 누적해 놓고 math.cos(th) 에 넣었습니다. 또는 자이로 원시값을 rad/s 로 바꾸지 않았습니다(°/s 를 rad/s 로 쓰면 57배).
  • 해결: 내부 계산은 모두 라디안으로 하고, 출력할 때만 math.degrees() 를 씁니다. 원시값 → °/s → rad/s 변환을 함수 하나(gyro_rad_s)로 모읍니다.

3) 오도메트리 거리가 실제의 20배 또는 20분의 1#

text
오도메트리 x = 20.1 m  (실제 1 m 이동)
  • 원인: 바퀴 각속도(rad/s)에 반지름 r 을 곱하지 않았거나, 이미 m/s 인 값에 r 을 또 곱했습니다(1 / 0.05 = 20).
  • 해결: 단위를 변수 이름에 넣습니다(w_rad_s, v_m_s). 바퀴를 띄우고 move_forward(0.1) 일 때 바퀴 값이 약 0.1 / 0.05 = 2 rad/s 인지 확인합니다.

4) 늘 몇 % 씩 덜 간 것으로 나옴#

text
오도메트리 1.000 m, 줄자 1.12 m  (루프 주기 로그: 56, 61, 58, 63 ...)
  • 원인: dt = 0.05 로 고정했는데 실제 루프는 get_motor_velocity 네 번 호출 때문에 더 느립니다.
  • 해결: time.monotonic() 으로 매번 실제 dt 를 잽니다(실물 ①). 반지름 보정은 dt 를 고친 다음에 합니다. 순서를 바꾸면 시간 오차를 반지름에 숨기게 됩니다.

5) 가만히 있는데 자이로 방향이 계속 돈다#

text
정지 60 s 후 th_gyro = 12.4 deg
  • 원인: 자이로 바이어스입니다(0.2 °/s × 60 s = 12°).
  • 해결: 출발 전 정지 상태에서 몇 초 평균을 빼고(실습 과제 3-2), 상보 필터나 EKF 로 바퀴 정보와 섞습니다. 바이어스는 온도에 따라 변하므로 정지할 때마다 다시 잽니다.

6) 상보 필터 방향이 ±180° 근처에서 갑자기 0° 로 튐#

text
th_wheel = 179.0 deg, th_gyro = -179.0 deg  ->  th = 0.0 deg ?
  • 원인: 각도를 −180°~180° 로 감싸(wrap) 저장하면, 179° 와 −179° 는 2° 차이인데 산술 평균은 0° 가 됩니다.
  • 해결: 필터 안에서는 각도를 감싸지 않고 계속 누적하거나, 섞을 때 차이를 감싸서 더합니다: d = (th_wheel - th_pred + math.pi) % (2*math.pi) - math.pi, th = th_pred + (1 - alpha) * d.

7) 칼만 필터가 센서를 전혀 따르지 않거나, 센서처럼 튐#

text
K = 0.002  (추정이 오도메트리와 같이 흐름)
K = 0.95   (추정이 측정값처럼 들쭉날쭉)
  • 원인: Q 가 너무 작거나(P → 0) R 이 너무 큼 / 그 반대입니다. 단위도 자주 틀립니다(R 은 σ² 이므로 σ = 0.3 m 이면 0.09 m², 0.3 이 아님).
  • 해결: R 은 센서 데이터의 분산으로 먼저 정하고(정지 상태에서 측정값의 분산), Q 는 오도메트리가 한 주기에 틀릴 만한 크기에서 시작해 시뮬레이터 5 처럼 조정합니다. Q 를 0 에 가깝게 두지 않습니다.

8) ROS 2 에서 로봇 위치가 두 곳을 오가며 깜빡임#

text
(rviz2) 로봇 모델이 두 위치 사이를 번갈아 깜빡임
Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame base_footprint ...   (함께 나올 수 있음)
  • 원인: odom → base_footprint TF 를 두 노드가 동시에 발행합니다. 예를 들어 myAGV.cpp 의 주석 처리된 sendTransform 을 되살렸는데 ekf.yaml 의 publish_tf: true 도 그대로입니다. 또는 *_config 칸 수가 15가 아니어서 EKF 가 이상한 값을 냅니다.
  • 해결: odom TF 를 내는 노드를 하나만 둡니다. ros2 run tf2_ros tf2_echo odom base_footprint 로 값이 안정적인지 보고, 설정을 고쳤다면 실습 과제 4-2 의 도구로 15칸을 확인합니다(ROS 2 실행 방법은 ch09).

과제#

  1. 오도메트리 보정 보고서: 실물 ① 을 반지름 보정 전 · 후 각 5회 해서 실제 거리의 평균 · 표준편차를 표로 만들고, 남은 오차 중 계통 · 랜덤 부분을 구분해 3줄로 쓰세요.
  2. 상보 필터 + 각도 감싸기: 실습 과제 3-1 을 고쳐 각도를 −180°~180° 로 감싸 저장하면서도 오류 6 이 생기지 않는 상보 필터 함수를 만들고, 경로를 왼쪽 회전만 8번(720°)으로 바꿔 시험하세요.
  3. 2차원 칼만 맛보기: 실습 과제 4-1 을 (x, y) 두 변수로 늘려, 시뮬레이터 3 의 통로를 따라가며 8 m 마다 바닥 표식(σ = 0.05 m)으로 위치를 측정한다고 할 때 왕복 10회 후 오차가 어떻게 되는지 오도메트리만과 비교하세요(P 는 2×2 대각 행렬로 단순화해도 됩니다).

참고자료#

  • myAGV Plus 공식 문서 (영문): https://github.com/elephantrobotics/myagvPlus_docs/tree/main/MYAGV_PLUS_EN
  • 공식 Python API (6.1.2 — get_motor_velocity, get_all_msg 등): https://github.com/elephantrobotics/myagvPlus_docs/blob/main/MYAGV_PLUS_EN/6-Software_Development_Guide/6.1-ApplicationBasePython/6.1.2-API.md
  • myagv_ros2 저장소: https://github.com/elephantrobotics/myagv_ros2
  • myagv_odometry 노드 코드 (myAGV.cpp): https://github.com/elephantrobotics/myagv_ros2/blob/main/myagv_odometry/src/myAGV.cpp
  • EKF 설정 (config/ekf.yaml): https://github.com/elephantrobotics/myagv_ros2/blob/main/myagv_odometry/config/ekf.yaml
  • 실행 파일 (myagv_active.launch.py): https://github.com/elephantrobotics/myagv_ros2/blob/main/myagv_odometry/launch/myagv_active.launch.py
  • robot_localization 저장소 · 문서: https://github.com/cra-ros-pkg/robot_localization
  • REP 105 — 이동 로봇의 좌표계 (map · odom · base_link): https://www.ros.org/reps/rep-0105.html
  • G. Welch, G. Bishop, An Introduction to the Kalman Filter: https://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf
  • myAGV Plus 제품 사진 · 소개 (hyenc.net): https://www.hyenc.net/products/35
  • 이 사이트의 도구: URDF 뷰어 · Python Playground
  • 이전 차시: ch05 상태 진단 · LED · IO · 원격 제어 · 다음 차시: ch07 LiDAR 와 점유 격자 지도