종합 실습 - 사각형 경로 주행 시뮬레이터

이 장에서 배우는 것

지금까지 좌표 변환, 회전행렬, 동차 변환, 차동 구동 기구학, 센서, 필터, 구동기, PID, 라이다 스캔, 목표점 주행을 순서대로 따로 다뤘다. 이번 장에서는 그 요소 중 오도메트리(odometry), PID, 회전 계산을 한 프로그램 안에 묶어서 로봇이 정사각형 경로를 한 바퀴 도는 시뮬레이터를 만든다. 실제 하드웨어 없이도 지금까지 배운 계산이 어떻게 맞물려 동작하는지, 그리고 작은 계산 오차가 어디서부터 쌓이는지를 코드로 직접 확인하는 것이 이 장의 목표다.

• 직진과 회전을 번갈아 반복하는 사각형 주행 루틴을 설계한다
• 오도메트리로 위치와 방향을 누적 추정하는 계산을 직접 구현한다
• 방향 오차를 PID로 줄이는 과정을 단계별로 재현한다
• 휠 반지름 보정 오차가 오도메트리 추정에 쌓이는 과정을 수치로 확인한다
• 이 책 이후에 더 살펴볼 만한 주제를 정리한다

문제 상황

창고 순찰 로봇을 만든다고 하자. 앞 장들에서 오도메트리 계산, 방향 PID, 제자리 회전을 각각 따로 테스트했을 때는 모두 기대한 값을 내놓았다. 문제는 이 세 가지를 이어서 "1 m 직진 → 90도 회전"을 네 번 반복하는 프로그램으로 묶었을 때 나타난다. 시뮬레이터를 끝까지 돌리면 로봇이 출발점으로 정확히 돌아와야 하는데, 계산된 최종 위치가 원점에서 살짝 벗어나 있다.

코드에 버그가 있는 것처럼 보이지만, 각 함수를 따로 실행하면 모두 정상이다. 원인은 로직이 아니라 숫자 하나, 즉 오도메트리가 인코더 눈금을 거리로 바꿀 때 쓰는 휠 반지름 값이 실제보다 살짝 크게 잡혀 있었다는 데 있다. 이런 종류의 오차는 개별 함수 테스트로는 드러나지 않고, 여러 구간을 이어 붙였을 때만 쌓여서 보인다. 이 장에서는 이 상황을 그대로 재현해서 왜 이런 오차가 생기는지, 그리고 실무에서 이런 오차를 어떻게 확인하는지를 다룬다.

직진과 회전을 반복하는 사각형 경로

정사각형 경로는 "한 변 직진" 4번과 "코너에서 90도 회전" 4번으로 나눌 수 있다. 직진 구간에서는 오도메트리가 이동 거리를 누적하고, 코너에서는 목표 방향과 현재 방향의 차이(오차)를 PID로 줄여 각속도 명령을 만든다. 방향 오차 계산에는 앞 장에서 쓴 wrap_angle 함수를 그대로 재사용한다. 각도를 라디안 기준 -π ~ π 범위로 감싸 두면, 359도에서 1도로 회전할 때 오차가 358도가 아니라 -2도로 나와 항상 짧은 쪽으로 회전한다.

이번 실습에서는 방향 제어에 비례(P) 항만 쓰는 PID를 사용한다. 회전은 마찰이나 부하 변화가 크지 않은 제자리 동작이라 적분 항 없이도 오차가 꾸준히 줄어들기 때문이다. 아래 코드의 pid_turn_demo는 첫 코너에서 방향 오차가 어떻게 줄어드는지 5스텝만 뽑아 보여준다.

오도메트리 오차가 쌓이는 이유

오도메트리는 엔코더 눈금 수를 거리로 바꿀 때 휠 반지름(또는 둘레) 값을 곱해서 쓴다. 이 값이 실제보다 2% 크게 잡혀 있다면, 로봇이 실제로 1 m를 갔는데 오도메트리는 1.02 m를 갔다고 추정한다. 회전량도 같은 값으로 환산되므로, 실제로 90도를 돌았는데 오도메트리는 91.8도를 돌았다고 추정한다. 문제는 이 방향 추정치를 그대로 다음 구간의 직진 방향으로 쓴다는 점이다. 방향이 조금씩 틀어진 채로 이동 거리를 적분하면, 위치 오차는 변의 개수를 지날 때마다 방향이 바뀌며 쌓여서 도착점이 출발점에서 벗어나게 된다.

아래 완성 코드는 이 상황을 두 개의 경로로 나눠 계산한다. true_square_path는 오차 없이 이상적으로 도는 실제 경로, odometry_square_path는 휠 반지름이 2% 크게 잡혔다고 가정한 오도메트리 추정 경로다. 두 경로를 비교하면 계산 오차가 눈에 보이는 위치 오차로 바뀌는 과정을 확인할 수 있다.

완성 코드

import numpy as np

ODO_SCALE = 1.02       # 오도메트리가 가정하는 휠 반지름이 실제보다 2% 크다
SIDE_LENGTH = 1.0      # m, 정사각형 한 변의 길이
TURN_ANGLE = np.pi / 2  # rad, 코너에서 실제로 회전해야 하는 양


def wrap_angle(angle):
    return (angle + np.pi) % (2 * np.pi) - np.pi


def pid_turn_demo(target_deg, kp, dt, steps):
    target = np.radians(target_deg)
    heading = 0.0
    trace = []
    for step in range(1, steps + 1):
        error = wrap_angle(target - heading)
        heading = wrap_angle(heading + kp * error * dt)
        trace.append((step, np.degrees(error), np.degrees(heading)))
    return trace


def true_square_path():
    heading = 0.0
    pos = np.array([0.0, 0.0])
    corners = []
    for _ in range(4):
        pos = pos + SIDE_LENGTH * np.array([np.cos(heading), np.sin(heading)])
        heading = wrap_angle(heading + TURN_ANGLE)
        corners.append((pos[0], pos[1], np.degrees(heading) % 360))
    return corners


def odometry_square_path():
    heading = 0.0
    pos = np.array([0.0, 0.0])
    est_side = SIDE_LENGTH * ODO_SCALE
    est_turn = TURN_ANGLE * ODO_SCALE
    corners = []
    for _ in range(4):
        pos = pos + est_side * np.array([np.cos(heading), np.sin(heading)])
        heading = heading + est_turn
        corners.append((pos[0], pos[1], np.degrees(heading) % 360))
    return corners, pos, heading


def main():
    print("=== 방향 PID 수렴 과정 (첫 코너, P 게인만 사용) ===")
    for step, error_deg, heading_deg in pid_turn_demo(90.0, kp=2.0, dt=0.1, steps=5):
        print(f"step {step}: 오차 {error_deg:7.3f}도 -> 회전 후 방향 {heading_deg:7.3f}도")

    true_corners = true_square_path()
    est_corners, final_pos, final_heading = odometry_square_path()

    print()
    print("=== 실제 경로와 오도메트리 추정 경로 비교 ===")
    print("[실제 경로]")
    for i, (tx, ty, th) in enumerate(true_corners, start=1):
        print(f"  코너{i}: x={tx:.3f} y={ty:.3f} 방향={th:.3f}도")
    print("[오도메트리 추정 경로]")
    for i, (ex, ey, eh) in enumerate(est_corners, start=1):
        print(f"  코너{i}: x={ex:.3f} y={ey:.3f} 방향={eh:.3f}도")

    drift = np.hypot(final_pos[0], final_pos[1])
    heading_error = np.degrees(final_heading) % 360
    print()
    print(f"한 바퀴 후 위치 오차(드리프트): {drift:.3f} m")
    print(f"한 바퀴 후 방향 오차: {heading_error:.3f}도")


if __name__ == "__main__":
    main()

줄별 해설

• ODO_SCALE은 오도메트리가 실제 이동량을 몇 배로 잘못 추정하는지를 나타낸다. 1.02는 휠 반지름을 2% 크게 잡았다는 뜻이고, 이 값이 거리와 회전량 계산에 그대로 곱해진다.
• wrap_angle은 각도를 -π ~ π 범위로 감싼다. 목표 방향과 현재 방향의 차이를 구할 때 이 함수를 거치지 않으면 359도 근처에서 큰 오차 값이 나와 반대 방향으로 회전하는 버그가 생긴다.
• pid_turn_demo는 목표 각도(도)를 라디안으로 바꾼 뒤, 매 스텝마다 오차에 비례 게인 kp를 곱해 각속도 명령을 만들고, 그만큼 dt 동안 회전시킨다. 오차와 회전 후 방향을 함께 기록해 수렴 과정을 눈으로 볼 수 있게 했다.
• true_square_path는 오차 없이 정확히 1 m씩 직진하고 정확히 90도씩 회전한다고 가정한 이상적인 경로다. 실제 로봇이 계산대로만 움직인다면 이 함수가 계산한 코너에 도달한다.
• odometry_square_path는 같은 구조를 쓰되 이동 거리와 회전량에 ODO_SCALE을 곱한다. 여기서 중요한 점은 위치를 누적할 때 직전 스텝에서 추정한 방향(heading)을 그대로 쓴다는 것이다. 방향 추정 자체가 조금씩 틀어져 있으므로, 그 틀어진 방향으로 다음 변의 이동을 적분하게 된다.
• main은 먼저 PID 수렴 과정을 5스텝만 출력하고, 이어서 실제 경로와 오도메트리 추정 경로를 코너별로 나란히 출력한 뒤, 한 바퀴를 돈 뒤의 위치 오차(드리프트)와 방향 오차를 계산해 보여준다.

실행 결과

$ python3 square_patrol_sim.py
=== 방향 PID 수렴 과정 (첫 코너, P 게인만 사용) ===
step 1: 오차  90.000도 -> 회전 후 방향  18.000도
step 2: 오차  72.000도 -> 회전 후 방향  32.400도
step 3: 오차  57.600도 -> 회전 후 방향  43.920도
step 4: 오차  46.080도 -> 회전 후 방향  53.136도
step 5: 오차  36.864도 -> 회전 후 방향  60.509도

=== 실제 경로와 오도메트리 추정 경로 비교 ===
[실제 경로]
  코너1: x=1.000 y=0.000 방향=90.000도
  코너2: x=1.000 y=1.000 방향=180.000도
  코너3: x=0.000 y=1.000 방향=270.000도
  코너4: x=0.000 y=0.000 방향=0.000도
[오도메트리 추정 경로]
  코너1: x=1.020 y=0.000 방향=91.800도
  코너2: x=0.988 y=1.019 방향=183.600도
  코너3: x=-0.030 y=0.955 방향=275.400도
  코너4: x=0.066 y=-0.060 방향=7.200도

한 바퀴 후 위치 오차(드리프트): 0.089 m
한 바퀴 후 방향 오차: 7.200도

표로 정리하면 코너마다 실제 값과 추정 값이 조금씩 벌어지는 것이 더 뚜렷이 보인다.

코너별 실제 경로와 오도메트리 추정 경로 비교

코너	실제 (x, y, 방향)	추정 (x, y, 방향)
1	1.000, 0.000, 90.000도	1.020, 0.000, 91.800도
2	1.000, 1.000, 180.000도	0.988, 1.019, 183.600도
3	0.000, 1.000, 270.000도	-0.030, 0.955, 275.400도
4	0.000, 0.000, 0.000도	0.066, -0.060, 7.200도

실무에서 자주 틀리는 것

도(degree)와 라디안(radian)을 섞어 계산한다

목표 각도는 도 단위로 받아 놓고 현재 방향은 라디안으로 관리하는 경우 오차 계산이 완전히 틀어진다.

틀린 코드

error = 90 - heading  # heading은 라디안인데 90은 도(度) 단위다
w_cmd = kp * error

고친 코드

error = np.radians(90) - heading  # 단위를 라디안으로 맞춘다
w_cmd = kp * error

코너마다 PID 적분항을 리셋하지 않는다

적분(I) 항을 쓰는 PID를 코너마다 새로 만들지 않고 재사용하면, 이전 코너에서 쌓인 오차 이력이 다음 코너의 회전에 영향을 준다.

틀린 코드

turn_pid = PID(kp=2.0, ki=0.5, kd=0.1)
for corner in range(4):
    rotate_to(robot, target[corner], turn_pid)  # 적분항이 코너마다 이어진다

고친 코드

turn_pid = PID(kp=2.0, ki=0.5, kd=0.1)
for corner in range(4):
    turn_pid.reset()  # 코너마다 오차 이력을 비운다
    rotate_to(robot, target[corner], turn_pid)

오도메트리 값을 실제 위치처럼 그대로 믿는다

오도메트리는 어디까지나 추정값이다. 이 값만으로 "목표에 도착했다"고 판단하면, 이번 장에서 본 것처럼 캘리브레이션 오차가 있을 때 실제로는 도착하지 못했는데도 도착했다고 착각할 수 있다.

틀린 코드

if odometry_pose[0] > 0.99:  # 오도메트리 값만 보고 도착을 판단한다
    stop_robot()

고친 코드

# 오도메트리는 추정값이라는 것을 감안해 여유 범위를 두거나
# 별도 센서(라이다·비전)로 보정한 위치와 함께 판단한다
if abs(odometry_pose[0] - target_x) < ARRIVAL_TOLERANCE:
    stop_robot()

회전 종료 조건에 상한(타임아웃)을 두지 않는다

임계값을 지나치게 촘촘하게 잡으면 부동소수점 오차 때문에 조건을 영영 만족하지 못해 루프가 끝나지 않을 수 있다. 최대 반복 횟수를 함께 둬야 한다.

틀린 코드

while abs(error) > 0.001:  # 임계값이 너무 촘촘해 부동소수 오차로 못 빠져나온다
    error = target - heading
    heading += kp * error * dt

고친 코드

for _ in range(MAX_STEPS):  # 최대 반복 횟수를 둬서 무한 루프를 막는다
    if abs(error) < np.radians(0.5):
        break
    error = target - heading
    heading += kp * error * dt

한눈에 보기

이 장에서 조합한 요소와 확인한 결과

구성 요소	이 장에서 한 일	확인한 결과
사각형 경로 설계	직진 4회와 회전 4회를 순서대로 배치	오도메트리 기준 한 변 1.020 m, 회전 91.800도로 계산됨
오도메트리	엔코더 기반으로 위치·방향을 누적 추정	휠 반지름 오차 2%가 거리와 회전량에 그대로 곱해져 쌓임
방향 PID	회전 오차를 비례 제어로 줄임	5스텝 만에 오차가 90도에서 36.864도로 줄어듦
결과 분석	실제 경로와 추정 경로를 코너별로 비교	한 바퀴 후 위치 오차 약 0.089 m, 방향 오차 7.200도

이 장에서 쓴 시뮬레이터는 오차의 원인을 눈에 보이게 만드는 데 목적이 있을 뿐, 실제 오차를 줄여 주지는 않는다. 오도메트리만으로는 시간이 지날수록 누적 오차를 피할 수 없다는 것이 이 장의 결론이다. 이 책 이후로는 여러 센서 추정값을 하나로 합치는 센서 퓨전과 칼만 필터, 지도를 만들며 동시에 위치를 추정하는 SLAM, 실제 로봇의 휠 반지름·베이스 폭을 측정해 보정하는 캘리브레이션 절차, 그리고 ROS/ROS2처럼 이런 계산을 표준화해 둔 로봇 소프트웨어 프레임워크를 살펴보는 것을 권한다.

연습 문제

1. ODO_SCALE을 1.02에서 1.05로 바꾸면 한 바퀴 후 드리프트가 커지는지 작아지는지, 그 이유와 함께 설명하라.
2. pid_turn_demo에서 kp를 2.0에서 4.0으로, dt는 그대로 0.1초로 두고 1~2스텝의 오차와 회전 후 방향을 직접 계산하라.
3. 코너마다 PID 적분항을 리셋하지 않으면 어떤 문제가 생기는지, 이 장의 코드를 근거로 설명하라.
4. true_square_path와 odometry_square_path의 차이를 한 문장으로 설명하고, 실제 로봇에서 이 차이를 좁히기 위해 추가로 살펴볼 만한 기법을 하나 제시하라.

정답과 해설

1. 커진다. 매 구간마다 실제 이동량에 곱해지는 배율이 1.02에서 1.05로 커지므로, 거리와 회전량 모두 더 크게 잘못 추정되고, 네 구간을 지나며 그 오차가 더 크게 쌓인다.
2. kp*dt=0.4이므로 오차는 0.6배씩 줄어든다. 1스텝: 오차 90.000도로 회전해 방향은 36.000도, 2스텝: 남은 오차는 54.000도이고 이번 스텝에서 54.000*0.4=21.600도를 더 돌아 방향은 57.600도가 된다. kp=2.0일 때보다 훨씬 빠르게 목표에 다가간다.
3. 이전 코너에서 쌓인 오차 이력(적분항)이 남아 있는 상태로 다음 코너의 회전이 시작되므로, 실제로는 오차가 없는데도 컨트롤러가 계속 보정 명령을 내보내 로봇이 목표 각도를 지나쳐 회전(오버슈트)하거나 진동할 수 있다.
4. true_square_path는 오차 없이 이상적으로 계산한 경로이고, odometry_square_path는 휠 반지름 보정 오차가 그대로 곱해져 누적된 추정 경로다. 이 차이를 좁히려면 IMU나 라이다 같은 다른 센서로 추정치를 보정하는 센서 퓨전(칼만 필터 등)을 추가로 살펴볼 수 있다.