Encoder와 Odometry

1. 엔코더는 거리를 재지 않는다

앞에서 본 센서들은 모두 바깥세상을 봤습니다. 카메라는 방을 보고 LiDAR 는 벽을 보고 IMU 는 중력을 봅니다. 엔코더는 다릅니다. 엔코더는 로봇 자신의 몸만 봅니다.

엔코더가 아는 것은 딱 하나입니다. 바퀴 축이 얼마나 돌았는가.

그리고 이 화면 전체의 주제는 한 문장으로 줄일 수 있습니다.

바퀴가 돌았다는 것과 로봇이 갔다는 것은 다른 말입니다.

바퀴는 헛돌 수 있고, 미끄러질 수 있고, 문턱에서 튈 수 있습니다. 엔코더는 그중 어느 것도 알아채지 못합니다. 바퀴가 공중에서 돌고 있어도 엔코더는 "로봇이 잘 가고 있다"고 보고합니다.

그런데도 엔코더는 로봇의 기본 감각입니다. 이유가 있습니다.

① 아주 빠릅니다. 초당 1000번도 읽을 수 있습니다. LiDAR 10 Hz, 카메라 30 Hz 와는 차원이 다릅니다.

② 잡음이 거의 없습니다. 카운트는 정수이고 세다가 틀리는 일은 잘 없습니다.

③ 환경에 흔들리지 않습니다. 어둡든 밝든, 유리가 있든 없든, 자기장이 어지럽든 말든 똑같이 동작합니다.

즉 엔코더는 짧은 시간 동안은 거의 완벽하고, 긴 시간에 걸쳐서는 완전히 틀립니다. 이 성질을 정확히 이해하는 것이 뒤에 나올 센서 융합의 출발점입니다.

A 상과 B 상이 4분의 1 주기만큼 어긋나 있습니다. 어느 쪽이 먼저 바뀌는지를 보면 회전 방향까지 알 수 있습니다.

주의 PPR 과 CPR 을 혼동하는 실수가 매우 흔합니다. PPR 은 한 바퀴에 나오는 펄스 수이고, 쿼드러처로 네 모서리를 다 세면 카운트는 그 네 배가 됩니다. 이것을 잘못 알면 오도메트리가 정확히 네 배 또는 4분의 1로 틀립니다. 바퀴를 손으로 정확히 한 바퀴 돌리고 카운트를 읽어 보면 5초 만에 확인됩니다.

2. 카운트를 미터로 바꾸기

엔코더가 주는 것은 정수 카운트입니다. 이것을 미터로 바꾸는 식은 간단합니다.

한 바퀴에 필요한 카운트 = PPR × 4 × 기어비

이동 거리 = (카운트 ÷ 한 바퀴 카운트) × 2 × 원주율 × 바퀴 반지름

실제 숫자를 넣어 봅니다. PPR 1000 짜리 엔코더, 쿼드러처 4체배, 기어비 30:1, 바퀴 반지름 0.05 m 라면

한 바퀴 = 1000 × 4 × 30 = 120,000 카운트

바퀴 둘레 = 2 × 3.14159 × 0.05 = 0.3142 m

카운트 하나 = 0.3142 ÷ 120,000 = 0.0026 mm

약 2.6 마이크로미터입니다. 머리카락 굵기의 30분의 1입니다.

여기서 아주 중요한 감각 하나를 얻어야 합니다. 분해능은 거의 언제나 넘치도록 충분합니다. 오도메트리가 틀리는 이유는 한 번도 분해능이었던 적이 없습니다.

진짜 문제는 저 식의 마지막 항, 바퀴 반지름입니다. 그 숫자는 센서가 준 것이 아니라 여러분이 손으로 적어 넣은 상수입니다.

설정 파일에 0.05 라고 적었는데 실제 바퀴가 공기압이 빠져 0.0495 라면 1 % 오차입니다. 10 m 를 가면 10 cm 가 틀립니다.

소수점 여섯 자리를 주는 정밀한 센서에 부정확한 상수를 곱하면 결과는 부정확합니다. 이 화면의 마지막 절이 그 상수를 재는 방법을 다룹니다.

속도를 계산할 때 흔한 함정도 하나 짚어 둡니다. "카운트 차이를 0.02 로 나눈다"처럼 주기를 상수로 박아 넣지 마십시오. 실제 호출 간격은 늘 흔들립니다. 앞 번 읽은 시각과 이번 시각의 차이를 그때그때 써야 속도가 떨리지 않습니다.

분해능은 2.6 마이크로미터인데 반지름을 1 % 잘못 알면 10 m 에 10 cm 가 틀립니다. 오도메트리의 정확도는 센서가 아니라 상수가 결정합니다.

"""엔코더 상수가 결과에 얼마나 영향을 주는지 직접 계산해 본다."""
import math

PPR = 1000        # 엔코더 한 바퀴 펄스 수
QUAD = 4          # 쿼드러처 4체배
GEAR = 30         # 모터 30바퀴에 바퀴 1바퀴
RADIUS = 0.05     # 설정 파일에 적은 바퀴 반지름 [m]

counts_per_rev = PPR * QUAD * GEAR
circumference = 2 * math.pi * RADIUS

print(f"한 바퀴 카운트 : {counts_per_rev:,}")
print(f"바퀴 둘레      : {circumference:.4f} m")
print(f"카운트 하나    : {circumference / counts_per_rev * 1000:.4f} mm")
print()

def distance(counts, radius):
    return counts / counts_per_rev * 2 * math.pi * radius

# 10 m 를 갈 만큼의 카운트
counts = counts_per_rev * 10.0 / circumference

print("설정한 반지름 대 실제 반지름의 차이가 만드는 오차")
for real in (0.0500, 0.0495, 0.0490, 0.0475):
    reported = distance(counts, RADIUS)     # 로봇이 보고하는 값
    actual = distance(counts, real)         # 실제로 간 거리
    err = actual - reported
    print(f"  실제 {real:.4f} m → 보고 {reported:.2f} m, "
          f"실제 {actual:.2f} m, 오차 {err * 100:+.1f} cm")

# 실행 결과
# 한 바퀴 카운트 : 120,000
# 바퀴 둘레      : 0.3142 m
# 카운트 하나    : 0.0026 mm
#
# 설정한 반지름 대 실제 반지름의 차이가 만드는 오차
#   실제 0.0500 m → 보고 10.00 m, 실제 10.00 m, 오차 +0.0 cm
#   실제 0.0495 m → 보고 10.00 m, 실제 9.90 m, 오차 -10.0 cm
#   실제 0.0490 m → 보고 10.00 m, 실제 9.80 m, 오차 -20.0 cm
#   실제 0.0475 m → 보고 10.00 m, 실제 9.50 m, 오차 -50.0 cm

3. 두 바퀴에서 위치를 만들어 낸다

이제 왼쪽과 오른쪽 바퀴가 각각 얼마나 갔는지 압니다. 여기서 로봇이 어디에 있는지를 계산해 내는 것이 오도메트리입니다.

짧은 시간 동안 왼쪽 바퀴가 왼거리만큼, 오른쪽 바퀴가 오른거리만큼 갔다고 하면

로봇 중심이 간 거리 = (왼거리 + 오른거리) ÷ 2

로봇이 돈 각도 = (오른거리 − 왼거리) ÷ 축간거리

두 번째 식이 왜 저렇게 생겼는지가 핵심입니다. 두 바퀴가 똑같이 가면 차이가 0 이라 로봇은 안 돕니다. 오른쪽이 더 가면 로봇은 왼쪽으로 돕니다. 같은 차이라도 두 바퀴가 멀리 떨어져 있으면 덜 돌고, 가까이 붙어 있으면 많이 돕니다. 그래서 차이를 축간거리로 나눕니다.

이제 이 두 값을 지도 위의 좌표로 옮깁니다.

x 에 더할 값 = 간 거리 × cos(현재각 + 돈각 ÷ 2)

y 에 더할 값 = 간 거리 × sin(현재각 + 돈각 ÷ 2)

돈각을 2로 나눠 더하는 것을 빼먹지 마십시오. 이 구간에서 로봇의 방향은 시작 방향에서 끝 방향으로 계속 바뀌었습니다. 그 중간 방향을 쓰는 것이 시작 방향을 쓰는 것보다 훨씬 정확합니다. 이것을 빠뜨리면 곡선 주행에서 궤적이 조금씩 안쪽으로 휩니다.

이렇게 자기 움직임만으로 위치를 이어 붙이는 것을 추측 항법이라고 부릅니다. 바깥세상을 한 번도 안 보고 "내가 이만큼 갔으니 여기쯤 있겠지"만으로 가는 방식입니다.

그리고 위치를 이어 붙인다는 말은 오차도 이어 붙인다는 뜻입니다. 한 번 틀린 값은 지우개로 지울 수 없습니다. 다음 절의 주제가 이것입니다.

좌우 속도 차이가 겨우 0.1 m/s 인데 10초 만에 191도를 돌았습니다. 축간거리로 나누는 항이 결과를 얼마나 크게 좌우하는지 보여 줍니다.

"""차동 구동 오도메트리를 직접 적분해 본다. ROS 없이 그냥 실행된다."""
import math

WHEEL_BASE = 0.30        # 좌우 바퀴 사이 거리 [m]

def integrate(x, y, th, d_left, d_right):
    """한 주기 동안의 좌우 이동 거리로 자세를 갱신한다."""
    ds = (d_right + d_left) / 2.0
    dth = (d_right - d_left) / WHEEL_BASE
    # 구간의 중간 방향을 쓴다. th 만 쓰면 곡선에서 안쪽으로 휜다.
    mid = th + dth / 2.0
    return x + ds * math.cos(mid), y + ds * math.sin(mid), th + dth

# 왼쪽 0.5 m/s, 오른쪽 0.6 m/s 로 10초 — 큰 원을 그리며 돈다
dt = 0.1
x = y = th = 0.0
print(" t[s]     x[m]     y[m]   heading[deg]")
for i in range(1, 101):
    x, y, th = integrate(x, y, th, 0.5 * dt, 0.6 * dt)
    if i % 20 == 0:
        print(f"{i * dt:5.1f} {x:8.3f} {y:8.3f} {math.degrees(th):10.1f}")

v = (0.5 + 0.6) / 2
w = (0.6 - 0.5) / WHEEL_BASE
print(f"\n선속도 {v:.2f} m/s, 각속도 {w:.3f} rad/s, 회전 반지름 {v / w:.2f} m")

# 실행 결과
#  t[s]     x[m]     y[m]   heading[deg]
#   2.0    1.020    0.353       38.2
#   4.0    1.604    1.262       76.4
#   6.0    1.500    2.337      114.6
#   8.0    0.755    3.118      152.8
#  10.0   -0.314    3.270      191.0
#
# 선속도 0.55 m/s, 각속도 0.333 rad/s, 회전 반지름 1.65 m

좌우 엔코더 카운트 읽기 → 바퀴별 이동 거리 변환 → 중심 이동량과 회전량 계산 → 중간 방향으로 자세 적분 → Odometry 메시지와 odom → base_link TF 발행

카운트에서 좌표까지 가는 길입니다. 어느 단계든 상수가 틀리면 그 아래는 전부 틀립니다.

4. 오도메트리는 반드시 틀린다 — 두 가지 방식으로

오도메트리 오차는 성격이 완전히 다른 두 무리로 나뉩니다. 이 둘을 구분하지 못하면 고칠 수 있는 것을 못 고치고, 못 고칠 것을 붙들고 시간을 씁니다.

① 계통 오차 — 상수가 틀린 것.

항상 같은 방향으로 같은 비율만큼 쌓입니다. 몇 번을 다시 해도 똑같이 재현됩니다.

그래서 잴 수 있고, 잴 수 있으면 고칠 수 있습니다.

• 바퀴 반지름이 틀림 → 직진 거리가 비례해서 틀림
• 좌우 바퀴 반지름이 서로 다름 → 직진 명령인데 한쪽으로 휨
• 축간거리가 틀림 → 회전 각도가 비례해서 틀림

② 비계통 오차 — 그때그때 다른 것.

재현되지 않습니다. 같은 경로를 두 번 가도 다르게 틀립니다.

고칠 수 없고 줄일 수만 있습니다.

• 미끄러짐 — 제자리 회전할 때 가장 심하고, 카펫과 급가속에서 심합니다
• 문턱과 틈새 — 바퀴가 순간적으로 튀거나 뜹니다
• 짐과 공기압 — 무거운 짐을 실으면 바퀴가 눌려 반지름이 줄어듭니다
• 바닥의 굴곡 — 한쪽 바퀴가 잠깐 뜨면 그 사이 거리는 통째로 사라집니다

그리고 반드시 알아야 할 것이 하나 더 있습니다.

각도 오차가 거리 오차보다 훨씬 위험합니다.

방향이 1도 틀린 채 10 m 를 직진하면 옆으로 17.5 cm 벗어납니다. 3도면 52 cm, 5도면 87 cm 입니다. 거리를 1 % 틀리는 것보다 각도를 1도 틀리는 쪽이 훨씬 큰 사고를 냅니다.

여기서 오도메트리의 성격이 결정됩니다. 짧은 시간에는 거의 완벽하고 긴 시간에는 쓸모없습니다. 그래서 나중에 배울 위치 추정은 오도메트리를 "지금 있는 곳"으로 쓰지 않고 "조금 전에서 얼마나 움직였는가"로만 씁니다. 움직인 양은 믿고, 쌓인 위치는 안 믿는 것입니다.

두 오차는 대응 방법이 정반대입니다. 어느 쪽인지 먼저 가르는 것이 진단의 첫 단계입니다.

주의 제자리 회전은 오도메트리에게 가장 가혹한 동작입니다. 두 바퀴가 서로 반대로 돌면서 바닥을 심하게 비비기 때문에 미끄러짐이 최대가 됩니다. 가능하면 제자리 회전 대신 완만한 곡선으로 방향을 바꾸도록 경로를 짜는 것만으로도 오도메트리 품질이 눈에 띄게 좋아집니다.

5. nav_msgs/Odometry 와 odom 프레임의 계약

계산한 결과는 두 가지 형태로 내보냅니다. 메시지 하나와 좌표 변환 하나입니다. 둘 다 있어야 합니다.

nav_msgs/Odometry 의 구조

• header.frame_id — odom. 이 위치가 어느 좌표계 기준인지
• header.stamp — 계산에 쓴 엔코더를 읽은 시각
• child_frame_id — base_link. 무엇의 위치인지
• pose — odom 기준 위치와 방향, 그리고 6×6 분산
• twist — 속도와 그 분산

여기서 가장 자주 틀리는 곳이 twist 입니다.

pose 는 odom 기준이지만 twist 는 base_link 기준, 즉 로봇 자신 기준입니다. 앞으로 0.5 m/s 로 가고 있으면 로봇이 어느 방향을 보고 있든 twist.linear.x 는 0.5 입니다. 여기에 전역 기준 속도를 넣으면 에러도 경고도 없이 조용히 틀립니다.

그리고 odom → base_link 변환도 함께 발행해야 합니다. 메시지만 내보내고 TF 를 빠뜨리면 다른 노드가 센서 데이터를 로봇 위치와 맞출 수 없습니다.

odom 프레임에는 지켜야 할 계약이 두 개 있습니다.

① 절대 튀지 않는다. odom 안에서 로봇의 위치는 언제나 연속적으로 변합니다. 순간이동은 없습니다.

② 시간이 지나면 드리프트한다. 위에서 본 이유로 반드시 실제와 벌어집니다.

map 프레임은 정확히 반대입니다. 위치 추정이 보정될 때 툭 튈 수 있지만, 대신 드리프트하지 않습니다.

이 두 계약이 실무에서 무엇을 결정하는가.

속도 제어와 장애물 회피는 odom 을 씁니다. 튀면 로봇이 급격히 반응하니까요. 목적지와 경로는 map 을 씁니다. 드리프트하면 엉뚱한 데로 가니까요. 이 구분을 지키지 않은 코드는 위치가 보정될 때마다 로봇이 움찔합니다.

분산은 어떻게 채우는가. 참값을 알 필요가 없습니다. "내 로봇은 1 m 를 가면 몇 cm 틀리는가" 를 몇 번 재서 그 값을 넣으면 됩니다. 평면을 다니는 로봇이면 z, roll, pitch 는 쓰지 않으므로 아주 큰 값을 넣어 "이 값은 믿지 말라"고 알려 줍니다.

메시지와 TF 를 같은 header 로 함께 내보내는 것이 핵심입니다. 둘 중 하나만 있으면 다른 노드에서 센서 데이터와 위치를 맞출 수 없습니다.

"""오도메트리를 계산해 메시지와 TF 로 함께 내보내는 노드."""
import math
import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from geometry_msgs.msg import TransformStamped, Quaternion
from tf2_ros import TransformBroadcaster


def yaw_to_quaternion(yaw):
    return Quaternion(x=0.0, y=0.0,
                      z=math.sin(yaw / 2.0), w=math.cos(yaw / 2.0))


class WheelOdometry(Node):
    def __init__(self):
        super().__init__('wheel_odometry')
        self.declare_parameter('wheel_radius', 0.05)
        self.declare_parameter('wheel_base', 0.30)
        self.radius = self.get_parameter('wheel_radius').value
        self.base = self.get_parameter('wheel_base').value

        self.x = self.y = self.th = 0.0
        self.prev = None
        self.pub = self.create_publisher(Odometry, 'odom', 10)
        self.tf = TransformBroadcaster(self)
        self.create_timer(0.02, self.tick)          # 50 Hz

    def read_wheels(self):
        """드라이버에서 좌우 바퀴 회전각[rad]을 읽어 온다. 실제 하드웨어로 교체."""
        raise NotImplementedError

    def tick(self):
        now = self.get_clock().now()
        left, right = self.read_wheels()
        if self.prev is None:
            self.prev = (now, left, right)
            return

        prev_time, prev_left, prev_right = self.prev
        # 주기를 상수로 박지 않고 실제 시각 차이를 쓴다
        dt = (now - prev_time).nanoseconds * 1e-9
        if dt <= 0.0:
            return

        dl = (left - prev_left) * self.radius
        dr = (right - prev_right) * self.radius
        ds = (dr + dl) / 2.0
        dth = (dr - dl) / self.base

        self.x += ds * math.cos(self.th + dth / 2.0)
        self.y += ds * math.sin(self.th + dth / 2.0)
        self.th += dth
        self.prev = (now, left, right)

        msg = Odometry()
        msg.header.stamp = now.to_msg()
        msg.header.frame_id = 'odom'
        msg.child_frame_id = 'base_link'
        msg.pose.pose.position.x = self.x
        msg.pose.pose.position.y = self.y
        msg.pose.pose.orientation = yaw_to_quaternion(self.th)
        # twist 는 로봇 자신 기준이다. 전역 속도를 넣으면 안 된다.
        msg.twist.twist.linear.x = ds / dt
        msg.twist.twist.angular.z = dth / dt
        # 직접 재서 얻은 값. 안 쓰는 축에는 큰 값을 넣어 믿지 말라고 알린다.
        msg.pose.covariance[0] = 0.02 ** 2      # x
        msg.pose.covariance[7] = 0.02 ** 2      # y
        msg.pose.covariance[14] = 1e6           # z — 평면 로봇이라 안 씀
        msg.pose.covariance[21] = 1e6           # roll
        msg.pose.covariance[28] = 1e6           # pitch
        msg.pose.covariance[35] = 0.03 ** 2     # yaw
        self.pub.publish(msg)

        t = TransformStamped()
        t.header = msg.header
        t.child_frame_id = 'base_link'
        t.transform.translation.x = self.x
        t.transform.translation.y = self.y
        t.transform.rotation = msg.pose.pose.orientation
        self.tf.sendTransform(t)


def main():
    rclpy.init()
    rclpy.spin(WheelOdometry())


if __name__ == '__main__':
    main()

map → 위치 추정이 보정하는 odom → 휠 오도메트리가 연속적으로 갱신하는 base_link

odom 은 연속적이지만 드리프트하고, map 은 튀지만 드리프트하지 않습니다. 두 성질을 한 프레임이 동시에 가질 수는 없어서 둘로 나눠 놓은 것입니다.

주의 odom → base_link 를 발행하는 노드는 시스템에 하나뿐이어야 합니다. 오도메트리 노드와 시뮬레이터가 동시에 이 변환을 내보내면 TF 트리가 두 값 사이에서 떨립니다. ros2 run tf2_tools view_frames 로 누가 무엇을 발행하는지 먼저 확인하십시오.

6. 보정 — 상수를 손으로 재서 고친다

앞에서 계통 오차는 잴 수 있고 고칠 수 있다고 했습니다. 이제 실제로 재 봅니다. 필요한 것은 줄자와 바닥에 붙일 테이프뿐입니다.

순서가 중요합니다. 반드시 직선 먼저, 회전 나중입니다. 반대로 하면 두 오차가 섞여 어느 상수를 얼마나 고쳐야 할지 알 수 없게 됩니다.

① 직선 시험 — 바퀴 반지름을 고칩니다.

바닥에 10 m 를 표시하고 로봇을 아주 천천히 직진시킵니다. 천천히 가는 이유는 미끄러짐을 최대한 배제하기 위해서입니다.

멈춘 뒤 오도메트리 보고값과 줄자 실측값을 비교합니다.

새 반지름 = 현재 반지름 × (실제 거리 ÷ 보고 거리)

보고가 10.00 m 인데 실제로 9.87 m 를 갔다면 반지름에 0.987 을 곱합니다. 최소 두 번, 왕복으로 해서 값이 비슷하게 나오는지 확인하십시오.

② 제자리 회전 시험 — 축간거리를 고칩니다.

로봇 위에 방향 표시를 하나 붙이고 오도메트리가 3600도(열 바퀴)를 보고할 때까지 제자리 회전시킵니다. 열 바퀴를 도는 이유는 한 바퀴로는 오차가 너무 작아 눈에 안 보이기 때문입니다.

멈춘 뒤 실제로 몇 도를 돌았는지 봅니다.

새 축간거리 = 현재 축간거리 × (보고 각도 ÷ 실제 각도)

3600도를 보고했는데 실제로는 3480도만 돌았다면 0.29 × (3600 ÷ 3480) = 0.30 으로 고칩니다. 덜 돌았으면 축간거리를 늘린다고 외워 두면 편합니다.

③ 양방향 사각형 시험 — 좌우 비대칭을 봅니다.

한 변 4 m 의 사각형을 시계 방향으로 다섯 번, 반시계 방향으로 다섯 번 돌고 출발점으로 돌아왔을 때의 오차를 표시합니다.

두 방향의 오차가 서로 반대쪽으로 뭉쳐 있으면 좌우 바퀴 반지름이 다른 것이고, 같은 쪽으로 뭉쳐 있으면 축간거리가 틀린 것입니다. 흩어져 있으면 그것이 여러분 로봇의 비계통 오차입니다.

마지막이 가장 중요합니다. 보정으로 없앨 수 있는 것은 계통 오차뿐입니다. 보정을 다 하고도 남는 그 흩어짐이 이 로봇의 진짜 실력이고, 그 값이 바로 분산에 넣어야 할 숫자입니다. 보정을 하는 진짜 이유는 오차를 0 으로 만들기 위해서가 아니라 남은 오차가 얼마인지 정직하게 알기 위해서입니다.

세 번씩 재서 값이 비슷하게 나오는지 확인하는 것이 중요합니다. 매번 크게 다르면 계통 오차가 아니라 미끄러짐을 보고 있는 것입니다.

"""직선·회전 시험 결과에서 새 상수를 계산한다."""

def new_radius(radius, reported_m, measured_m):
    """직선 시험: 보고 거리와 실측 거리로 바퀴 반지름을 고친다."""
    return radius * measured_m / reported_m


def new_wheel_base(base, reported_deg, measured_deg):
    """회전 시험: 덜 돌았으면 축간거리를 늘린다."""
    return base * reported_deg / measured_deg


RADIUS, BASE = 0.0500, 0.2900

print("① 직선 시험 — 10 m 를 지시")
for measured in (9.87, 9.90, 9.88):
    r = new_radius(RADIUS, 10.00, measured)
    print(f"   보고 10.00 m, 실측 {measured:.2f} m → 반지름 {r:.5f} m "
          f"({(r / RADIUS - 1) * 100:+.1f} %)")

print("\n② 회전 시험 — 오도메트리가 3600도를 보고할 때까지")
for measured in (3480, 3495, 3470):
    b = new_wheel_base(BASE, 3600, measured)
    print(f"   보고 3600도, 실측 {measured}도 → 축간거리 {b:.4f} m "
          f"({(b / BASE - 1) * 100:+.1f} %)")

print("\n③ 보정 뒤 남는 오차 — 이 값을 covariance 에 쓴다")
residual = [0.031, 0.028, 0.035, 0.026, 0.033]      # 10 m 당 남은 오차 [m]
mean = sum(residual) / len(residual)
print(f"   10 m 당 평균 {mean * 100:.1f} cm → 1 m 당 약 {mean / 10 * 100:.2f} cm")

# 실행 결과
# ① 직선 시험 — 10 m 를 지시
#    보고 10.00 m, 실측 9.87 m → 반지름 0.04935 m (-1.3 %)
#    보고 10.00 m, 실측 9.90 m → 반지름 0.04950 m (-1.0 %)
#    보고 10.00 m, 실측 9.88 m → 반지름 0.04940 m (-1.2 %)
#
# ② 회전 시험 — 오도메트리가 3600도를 보고할 때까지
#    보고 3600도, 실측 3480도 → 축간거리 0.3000 m (+3.4 %)
#    보고 3600도, 실측 3495도 → 축간거리 0.2987 m (+3.0 %)
#    보고 3600도, 실측 3470도 → 축간거리 0.3009 m (+3.8 %)
#
# ③ 보정 뒤 남는 오차 — 이 값을 covariance 에 쓴다
#    10 m 당 평균 3.1 cm → 1 m 당 약 0.31 cm

운용 조건 준비 → 직선 시험으로 반지름 확정 → 회전 시험으로 축간거리 확정 → 양방향 사각형 시험 → 남은 오차를 공분산에 반영

보정은 이 순서를 지켜야 합니다. 순서를 바꾸면 두 오차가 섞여 어느 상수를 고칠지 알 수 없게 됩니다.

주의 보정은 로봇이 실제로 일할 때와 같은 조건에서 해야 합니다. 짐을 싣고 다니는 로봇을 빈 상태로 보정하면 바퀴가 눌린 만큼 그대로 오차가 됩니다. 바닥재도 마찬가지입니다. 타일에서 보정한 값은 카펫에서 다시 틀립니다. 계절이 바뀌어 타이어 공기압이 변하면 반지름도 변하므로 주기적으로 다시 재십시오.

7. 실제 Encoder는 어떻게 생겼는가

Encoder는 ‘센서 하나’라기보다 회전축, 자석 또는 코드 디스크, 검출 소자와 신호 처리 회로가 결합된 장치입니다. 소형 이동 로봇에는 모터 뒤에 붙은 자기식 Encoder가 흔하고, 산업용 모터에는 밀폐형 광학·자기식 Encoder가 쓰입니다. 바퀴 허브나 모터 축에 직접 설치하면 기어 백래시의 영향을 다르게 받으므로 Encoder가 기어의 어느 쪽에 있는지도 반드시 확인해야 합니다.

왼쪽은 기어모터 축에 붙는 자기식 Encoder 보드, 가운데는 슬롯 디스크를 광센서가 읽는 증분식 Encoder, 오른쪽은 바퀴 허브의 코드 디스크를 읽는 형태를 재현한 교육용 이미지입니다. 아래 자를 통해 부품의 대략적인 크기를 비교할 수 있습니다.

8. A상·B상·Index와 Quadrature 해독

A와 B 신호의 위상 순서로 방향을 판별합니다. 정방향 상태 전이는 보통 00 → 01 → 11 → 10 → 00, 역방향은 그 반대입니다. 배선이나 드라이버 정의에 따라 부호는 바뀔 수 있으므로 로봇 바퀴를 앞으로 돌려 실제 카운트가 증가하는지 확인해야 합니다.

Index 또는 Z 신호는 한 바퀴에 한 번 나타나는 기준 펄스입니다. 증분식 Encoder의 원점을 잡거나 누락 카운트를 검출하는 데 활용할 수 있습니다. 모든 바퀴 Encoder가 Index를 제공하는 것은 아닙니다.

# 이전 AB 상태와 현재 AB 상태를 4비트로 묶어 증감량을 찾는다.
QUAD_TABLE = {
    0b0001: +1, 0b0111: +1, 0b1110: +1, 0b1000: +1,
    0b0010: -1, 0b1011: -1, 0b1101: -1, 0b0100: -1,
}

def decode(previous_ab, current_ab):
    transition = (previous_ab << 2) | current_ab
    return QUAD_TABLE.get(transition, 0)

00 → 11처럼 한 번에 두 비트가 바뀐 전이는 정상 속도에서는 발생하지 않습니다. 이런 불법 전이를 세면 전기 잡음, Sampling 부족이나 인터럽트 누락을 진단할 수 있습니다.

9. PPR, CPR, 기어비와 데이터시트 읽기

PPR·CPR은 제조사마다 뜻이 다르게 쓰이므로 약어만 믿지 말고 ‘어느 축 기준인지’, ‘상승 에지만 세는지’, ‘A/B 네 에지를 모두 세는지’를 읽어야 합니다. 모터 축 Encoder가 12 pulse/rev이고 감속비가 50:1이며 4체배라면 바퀴 한 바퀴는 12 × 50 × 4 = 2,400 count입니다.

가장 확실한 검증은 바퀴에 표시를 하고 정확히 10바퀴 돌려 누적 카운트를 10으로 나누는 것입니다. 한 바퀴만 재면 시작·끝 위치의 한 카운트 오차 비율이 큽니다.

# JointState를 사용하는 드라이버라면 위치·속도 단위 확인
ros2 topic echo /joint_states --once
ros2 topic hz /joint_states
ros2 topic info /joint_states --verbose

10. 전기 연결과 신호 품질

Encoder 출력은 push-pull, open-collector, line-driver 차동 출력 등으로 나뉩니다. 전압 레벨이 제어기 입력과 맞지 않으면 손상되거나 임계값 근처에서 카운트가 튑니다. 긴 케이블과 모터가 있는 환경에서는 A·/A, B·/B 차동 신호와 종단 저항이 잡음에 유리합니다.

신호 케이블은 모터 전원선과 떨어뜨리고, 필요하면 꼬임선·차폐·Schmitt trigger 입력을 사용합니다. 소프트웨어 debounce를 무조건 적용하면 고속의 정상 펄스까지 없앨 수 있으므로 신호 대역을 계산한 뒤 사용해야 합니다.

11. Overflow, 원자적 읽기와 Timestamp

작은 MCU의 16비트 카운터는 빠르게 overflow합니다. 누적값 자체를 빼기보다 모듈러 차이를 올바르게 계산하거나 overflow 인터럽트로 상위 비트를 확장해야 합니다. 여러 바이트 카운트를 읽는 동안 인터럽트가 값을 바꾸지 않도록 원자적 snapshot도 필요합니다.

def signed_delta_u16(current, previous):
    """16-bit unsigned counter의 wrap-around를 포함한 부호 있는 차이."""
    delta = (current - previous) & 0xFFFF
    return delta - 0x10000 if delta & 0x8000 else delta

assert signed_delta_u16(3, 65533) == 6
assert signed_delta_u16(65533, 3) == -6

좌우 바퀴는 가능한 한 같은 시각에 snapshot해야 합니다. 왼쪽을 읽고 10 ms 뒤 오른쪽을 읽으면 회전 중 두 바퀴가 서로 다른 구간을 표현합니다. 메시지의 header.stamp는 publish 시각이 아니라 이 snapshot의 측정 시각이어야 합니다.

12. 저속과 고속에서 속도를 측정하는 두 방법

고정 시간창 안의 펄스 수를 세는 방식은 고속에서 안정적이지만 저속에서는 한 카운트 차이가 큰 속도 계단이 됩니다. 반대로 연속 펄스 사이의 시간을 재는 방식은 저속에서 정밀하지만 고속에서 타이머 해상도와 인터럽트 부하의 영향을 받습니다.

속도에 저역 통과 필터를 쓰면 잡음은 줄지만 제어 지연이 늘어납니다. PID 속도 제어에서는 필터 전후의 step response와 정지 판정 시간을 함께 측정해야 합니다.

13. 정확한 차동구동 적분

중간각 근사는 작은 회전량에서 충분히 정확합니다. 한 주기의 회전량이 커질 수 있는 저주기 시스템이라면 원호의 정확한 식을 사용할 수 있습니다.

import math

def integrate_exact(x, y, yaw, dl, dr, wheel_base):
    ds = 0.5 * (dl + dr)
    dyaw = (dr - dl) / wheel_base
    if abs(dyaw) < 1e-9:
        return x + ds * math.cos(yaw), y + ds * math.sin(yaw), yaw
    radius = ds / dyaw
    x += radius * (math.sin(yaw + dyaw) - math.sin(yaw))
    y -= radius * (math.cos(yaw + dyaw) - math.cos(yaw))
    return x, y, yaw + dyaw

적분 전에는 dt, 카운트 변화량, 물리적으로 가능한 최대 바퀴 속도를 검사하십시오. 통신 재연결 뒤 누적 카운트가 0으로 초기화되면 이를 실제 대회전으로 해석하지 않도록 reset 감지가 필요합니다.

14. ROS 2 JointState에서 Odometry까지

일반적인 데이터 흐름은 다음과 같습니다.

Encoder 하드웨어 → MCU 카운터 → 하드웨어 드라이버 → /joint_states 또는 좌우 Tick 토픽 → 차동구동 Odometry → /odom과 odom → base_link

sensor_msgs/JointState.position은 관절 위치 rad, velocity는 rad/s가 원칙입니다. 바퀴 관절 이름과 URDF의 joint 이름이 정확히 일치해야 robot_state_publisher가 휠 TF를 갱신할 수 있습니다.

ros2 interface show sensor_msgs/msg/JointState
ros2 interface show nav_msgs/msg/Odometry
ros2 topic echo /odom --field pose.pose --once
ros2 topic echo /odom --field twist.twist
ros2 run tf2_ros tf2_echo odom base_link

Odometry 메시지와 TF는 같은 상태·같은 timestamp로 만들어야 합니다. 별도 timer가 서로 다른 시각의 값을 발행하면 소비 노드에서 두 로봇 자세가 동시에 존재하는 것처럼 보입니다.

15. Covariance를 측정해 채우기

공분산은 Encoder 분해능만으로 결정되지 않습니다. 실제 바닥, 타이어, 속도와 짐 조건에서 기준 경로를 여러 번 주행한 뒤 참값과의 잔차로 추정해야 합니다. 이동 거리가 길수록, 회전이 많을수록 불확실성이 커지는 모델이 현실적입니다.

import statistics

errors_x = [0.031, 0.024, 0.038, 0.029, 0.035]
variance_x = statistics.variance(errors_x)
print(f"x variance = {variance_x:.8f} m^2")

평면 로봇에서 사용하지 않는 z·roll·pitch를 0 분산으로 두면 필터가 완벽한 측정으로 오해할 수 있습니다. 매우 큰 분산을 사용하거나 소비 필터 설정에서 해당 변수를 명시적으로 제외합니다.

16. Encoder와 IMU·LiDAR를 융합하는 이유

Wheel Odometry는 평평하고 마찰이 일정한 바닥에서 빠르고 부드럽지만 미끄러짐을 모릅니다. IMU 자이로는 바닥 미끄러짐과 다른 방식으로 회전을 관측하지만 bias가 적분됩니다. LiDAR·카메라·GNSS는 외부 기준을 주지만 느리거나 환경에 따라 실패합니다.

robot_localization 같은 EKF에 모든 변수를 무조건 넣지 마십시오. 같은 Wheel Odometry에서 계산한 pose와 twist를 독립 센서처럼 중복 융합하면 정보를 두 번 센 셈이 됩니다. 센서 출처와 상관관계를 먼저 정리해야 합니다.

17. 실습: Encoder·Odometry 감시 노드

다음 노드는 timestamp, 순간 속도, 정지 중 움직임과 큰 점프를 검사합니다. 실제 Tick 메시지 형식에 맞게 callback 입력만 바꾸어 사용할 수 있습니다.

#!/usr/bin/env python3
import math
import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry


class OdomMonitor(Node):
    def __init__(self):
        super().__init__('odom_monitor')
        self.previous_stamp = None
        self.create_subscription(Odometry, '/odom', self.on_odom, 20)

    def on_odom(self, msg):
        stamp = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
        if self.previous_stamp is not None:
            dt = stamp - self.previous_stamp
            if dt <= 0.0:
                self.get_logger().error(f'timestamp 역행 또는 중복: dt={dt:.6f}')
            elif dt > 0.1:
                self.get_logger().warning(f'Odometry 갱신 지연: {dt:.3f} s')
        self.previous_stamp = stamp

        v = msg.twist.twist.linear.x
        w = msg.twist.twist.angular.z
        if not math.isfinite(v) or not math.isfinite(w):
            self.get_logger().error('속도에 NaN 또는 무한대가 있습니다.')
        if abs(v) > 2.0 or abs(w) > 6.0:
            self.get_logger().warning(f'물리 범위 초과 v={v:.2f}, w={w:.2f}')
        if msg.header.frame_id != 'odom' or msg.child_frame_id != 'base_link':
            self.get_logger().warning('frame_id 계약을 확인하십시오.')


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


if __name__ == '__main__':
    main()

18. 실제 로봇 점검표와 실패 대응

배포 전에는 바퀴를 들어 올린 시험과 바닥 주행 시험을 분리합니다. 공중 시험은 부호·카운트·통신을 확인하지만 실제 반지름·미끄러짐·짐 영향을 보여 주지 못합니다.

한쪽 Encoder가 멈췄는데 명령은 계속되면 로봇이 급회전할 수 있습니다. 좌우 명령과 측정 속도의 불일치, 같은 값이 오래 유지되는 freeze, timestamp 중단을 watchdog으로 감시하고 안전 정지로 연결하십시오.

연습 문제

1. 사용 중인 Encoder의 검출 방식, 출력 회로, PPR·CPR 정의와 장착 축을 조사하세요.
2. 바퀴를 정확히 10회전시켜 한 바퀴당 count와 반복 오차를 측정하세요.
3. A/B 상태 전이표를 구현하고 불법 전이 횟수를 기록하세요.
4. 16-bit counter가 정·역방향으로 overflow하는 단위 시험을 작성하세요.
5. 저속 펄스 주기법과 고속 count 창 방식의 속도 오차를 비교하세요.
6. 좌우 snapshot 시간차가 회전 Odometry에 만드는 오차를 계산하세요.
7. 중간각 근사와 정확한 원호 적분을 여러 dt에서 비교하세요.
8. /joint_states, /odom, TF의 이름·단위·timestamp 계약을 자동 검사하세요.
9. 10 m 직진 시험으로 좌우 유효 반지름을 보정하세요.
10. 양방향 10회전 시험으로 유효 축간거리를 보정하세요.
11. 시계·반시계 사각형 시험으로 계통 오차와 비계통 오차를 분리하세요.
12. 타일·카펫·경사·짐 유무에 따른 최종 자세 오차를 비교하세요.
13. 반복 주행 잔차로 pose와 twist 공분산을 추정하세요.
14. 한쪽 Encoder freeze·jump·단절을 주입해 watchdog을 검증하세요.
15. Encoder·IMU·LiDAR 융합에서 사용할 변수와 제외할 변수를 근거와 함께 정하세요.

참고 자료

• sensor_msgs/JointState 메시지
• nav_msgs/Odometry 메시지
• REP-103: 표준 단위와 좌표계
• REP-105: 이동 로봇 좌표 프레임
• diff_drive_controller 공식 문서
• robot_localization 공식 문서

19. 확인 퀴즈 15문항