목표 추종 로봇 만들기

앞 장까지 우리는 로봇이 목표를 향해 움직이기 위해 필요한 재료를 하나씩 모았습니다. 위치는 좌표로 표현했고, 목표까지의 차이는 벡터로 계산했습니다. 벡터의 크기는 거리였고, atan2로 목표 방향각을 구했습니다. 현재 방향과 목표 방향의 차이도 계산했습니다.

이제 남은 일은 하나입니다. 이 계산을 한 번만 하는 것이 아니라, 로봇이 움직이는 동안 계속 반복하는 것입니다.

로봇은 처음부터 정답 경로를 완벽히 알고 움직이지 않습니다. 지금 위치를 보고, 목표가 어디 있는지 다시 계산하고, 조금 움직이고, 또다시 계산합니다. 이 반복이 충분히 빠르게 일어나면 로봇은 마치 목표를 이해하고 스스로 찾아가는 것처럼 보입니다.

이번 장에서는 좌표, 벡터, 거리, 각도를 모두 묶어 목표점까지 이동하는 2D 목표 추종 로봇을 만듭니다. 2부에서 배운 내용이 하나의 작은 로봇 행동으로 완성되는 장입니다.

목표 추종은 반복 계산이다

목표 추종 로봇은 이름 그대로 목표를 따라가는 로봇입니다. 여기서 목표는 움직일 수도 있고 고정되어 있을 수도 있지만, 이번 장에서는 먼저 고정된 목표점 하나를 사용하겠습니다.

로봇이 해야 할 일은 단순해 보입니다.

목표가 있다 → 목표 방향을 본다 → 그쪽으로 간다 → 가까워지면 멈춘다

하지만 이 문장을 로봇이 이해할 수 있는 계산으로 바꾸면 조금 더 구체적입니다.

현재 위치와 목표 위치를 비교한다.
목표까지의 거리와 방향을 계산한다.
현재 방향과 목표 방향의 차이를 계산한다.
거리와 각도 오차를 이용해 속도 명령을 만든다.
속도 명령으로 로봇 위치와 방향을 업데이트한다.
목표 근처에 도착했는지 확인한다.

이것이 목표 추종의 핵심 루프입니다. 루프란 같은 일을 반복하는 구조입니다. 로봇 시뮬레이션에서는 보통 아주 짧은 시간 간격 dt마다 이 루프를 실행합니다.

예를 들어 dt = 0.1이라면 0.1초마다 로봇의 상태를 새로 계산합니다. 10초 동안 움직이면 이 계산을 100번 반복하는 셈입니다. 로봇이 똑똑해 보이는 이유는 한 번에 대단한 계산을 하기 때문이 아니라, 작은 계산을 끈질기게 반복하기 때문입니다.

우리가 만들 로봇의 규칙

이번 장에서 만들 로봇은 2D 평면 위를 움직입니다. 로봇의 상태는 세 개의 숫자로 표현합니다.

[ (x, y, theta) ]

여기서 x와 y는 로봇의 위치입니다. theta는 로봇이 바라보는 방향입니다. theta는 라디안 단위로 표현하며, 월드 좌표계의 x축 양의 방향을 0으로 둡니다.

로봇은 두 가지 명령을 받습니다.

[ v ]

[ omega ]

v는 전진 속도입니다. 로봇이 현재 바라보는 방향으로 얼마나 빠르게 앞으로 갈지를 나타냅니다. omega는 각속도입니다. 로봇이 얼마나 빠르게 회전할지를 나타냅니다. 보통 omega는 오메가라고 읽습니다.

이 로봇은 옆으로 미끄러지듯 움직이지 않고, 자신이 바라보는 방향으로 전진한다고 가정합니다. 자동차나 차동 구동 모바일 로봇을 단순화한 모델이라고 생각하면 됩니다.

시간 간격 dt 동안 로봇의 상태는 다음처럼 바뀝니다.

[ theta_{new} = theta + omega cdot dt ]

[ x_{new} = x + v cos(theta) cdot dt ]

[ y_{new} = y + v sin(theta) cdot dt ]

처음에는 식이 많아 보이지만, 뜻은 간단합니다. 로봇은 먼저 회전하고, 자신이 바라보는 방향으로 조금 전진합니다. cos(theta)는 x 방향 성분을 만들고, sin(theta)는 y 방향 성분을 만듭니다.

이 업데이트 방식은 아주 짧은 시간 동안 속도가 일정하다고 보고 위치를 조금씩 옮기는 방법입니다. 나중에 미분과 적분을 배우면 이 구조가 더 자연스럽게 보일 것입니다. 지금은 "작은 시간 동안 조금 움직인다" 정도로 이해하면 충분합니다.

현재 위치와 목표점 설정하기

먼저 로봇의 시작 위치와 목표점을 정해 봅시다.

import numpy as np

position = np.array([0.0, 0.0])
theta = np.deg2rad(90.0)
target = np.array([6.0, 4.0])

로봇은 원점 (0, 0)에서 시작합니다. 목표는 (6, 4)입니다. 로봇의 초기 방향은 90도입니다. 즉, 처음에는 위쪽을 바라보고 있습니다.

그런데 목표점 (6, 4)는 로봇의 오른쪽 위에 있습니다. 로봇이 바로 목표를 바라보고 있지는 않습니다. 따라서 로봇은 먼저 오른쪽으로 몸을 돌리면서 목표를 향해 움직여야 합니다.

목표 추종에서 가장 먼저 계산할 값은 현재 위치에서 목표까지의 벡터입니다.

diff = target - position
print(diff)

출력은 다음과 같습니다.

[6. 4.]

이 벡터는 로봇에서 목표로 향하는 화살표입니다. 이 화살표의 크기가 거리이고, 이 화살표의 각도가 목표 방향입니다.

distance = np.linalg.norm(diff)
target_angle = np.arctan2(diff[1], diff[0])

print('거리:', distance)
print('목표 방향각:', np.rad2deg(target_angle))

결과는 다음과 비슷합니다.

거리: 7.211102550927978
목표 방향각: 33.690067525979785

로봇은 현재 90도를 바라보고 있는데, 목표 방향은 약 33.69도입니다. 따라서 로봇은 시계 방향으로 돌아야 합니다. 이 차이를 각도 오차로 계산합니다.

각도 오차는 가장 짧은 회전 방향이어야 한다

각도 오차는 목표 방향에서 현재 방향을 뺀 값입니다.

angle_error = target_angle - theta

하지만 앞 장에서 보았듯이 각도는 원처럼 이어져 있습니다. 181도와 -179도는 숫자로는 멀어 보이지만 실제 방향은 거의 같습니다. 그래서 각도 오차는 항상 -π부터 π 사이로 정리해 주는 것이 좋습니다.

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

이 함수를 사용하면 현재 방향과 목표 방향의 차이를 안전하게 계산할 수 있습니다.

angle_error = normalize_angle(target_angle - theta)

print('각도 오차:', np.rad2deg(angle_error))

현재 방향이 90도이고 목표 방향이 약 33.69도라면 각도 오차는 약 -56.31도입니다. 음수라는 것은 시계 방향으로 회전해야 한다는 뜻입니다.

여기서 중요한 감각이 하나 있습니다. 로봇은 "목표가 오른쪽 위에 있다"라고 생각하지 않습니다. 로봇이 보는 것은 이런 숫자입니다.

목표까지 거리: 7.21
목표 방향각: 33.69도
현재 방향: 90.00도
각도 오차: -56.31도

이 숫자를 속도 명령으로 바꾸면 로봇이 움직입니다.

회전 명령 만들기

로봇이 목표를 바라보게 하려면 각도 오차를 줄이면 됩니다. 가장 단순하고 많이 쓰이는 방법은 오차에 비례해서 회전 속도를 주는 것입니다.

angular_speed = angle_gain * angle_error

각도 오차가 크면 빠르게 돌고, 각도 오차가 작으면 천천히 돕니다. 이런 방식을 비례 제어라고 합니다. 비례 제어는 말 그대로 오차에 비례해 명령을 만드는 방법입니다.

하지만 회전 속도를 무한히 크게 만들 수는 없습니다. 실제 로봇의 모터에는 한계가 있고, 시뮬레이션에서도 너무 큰 값은 움직임을 거칠게 만듭니다. 그래서 np.clip()으로 회전 속도에 제한을 걸어 줍니다.

def compute_angular_speed(angle_error, angle_gain=2.0, max_angular_speed=2.0):
    angular_speed = angle_gain * angle_error
    angular_speed = np.clip(
        angular_speed,
        -max_angular_speed,
        max_angular_speed
    )
    return angular_speed

이제 각도 오차가 아무리 커도 회전 속도는 -2.0부터 2.0 사이에 머뭅니다. 단위는 rad/s입니다.

한번 확인해 봅시다.

for error_deg in [-90, -30, 0, 30, 90]:
    error_rad = np.deg2rad(error_deg)
    omega = compute_angular_speed(error_rad)
    print(error_deg, 'deg ->', omega, 'rad/s')

각도 오차가 음수이면 회전 속도도 음수입니다. 각도 오차가 양수이면 회전 속도도 양수입니다. 오차가 0이면 회전하지 않습니다. 로봇 제어에서 부호는 방향을 나타내는 아주 중요한 정보입니다.

거리 기반 전진 속도 만들기

회전 명령만으로는 목표에 도착할 수 없습니다. 로봇은 앞으로도 가야 합니다. 전진 속도는 목표까지의 거리를 이용해 만들 수 있습니다.

가장 단순한 방법은 항상 같은 속도로 가는 것입니다.

linear_speed = 1.0

하지만 이 방법은 목표 근처에서 문제가 생길 수 있습니다. 목표가 바로 앞에 있는데도 계속 빠르게 달리면 목표를 지나칠 수 있습니다. 지나친 뒤에는 다시 돌아오고, 또 지나치고, 이런 식으로 목표 주변에서 흔들릴 수 있습니다.

그래서 목표에 가까워질수록 속도를 줄이는 규칙을 만들겠습니다.

def compute_distance_speed(distance, distance_gain=1.0, max_linear_speed=1.0):
    speed = distance_gain * distance
    speed = np.clip(speed, 0.0, max_linear_speed)
    return speed

이 함수는 거리가 멀면 최대 속도로 가고, 거리가 가까워지면 거리만큼 속도를 줄입니다. 예를 들어 distance_gain=1.0, max_linear_speed=1.0이라면 거리가 5일 때는 속도가 1.0으로 제한됩니다. 거리가 0.3이면 속도는 0.3이 됩니다.

하지만 아직 한 가지 문제가 남아 있습니다. 로봇이 목표를 완전히 반대 방향으로 등지고 있는데도 전진하면 오히려 목표에서 멀어질 수 있습니다. 그래서 각도 오차가 너무 클 때는 전진 속도를 줄이거나 0으로 만드는 것이 안전합니다.

다음 함수는 거리와 각도 오차를 함께 사용합니다.

def compute_linear_speed(
    distance,
    angle_error,
    distance_gain=1.0,
    max_linear_speed=1.0,
    turn_slowdown_angle=np.deg2rad(60.0)
):
    speed = distance_gain * distance
    speed = np.clip(speed, 0.0, max_linear_speed)

    if abs(angle_error) > turn_slowdown_angle:
        speed = 0.0

    return speed

이 규칙은 아주 직관적입니다. 목표 방향과 60도 이상 어긋나 있으면 일단 앞으로 가지 않고 회전에 집중합니다. 목표 방향을 어느 정도 바라보게 되면 전진을 시작합니다.

이렇게 하면 로봇이 처음부터 이상한 방향으로 튀어나가는 것을 막을 수 있습니다. 사람도 길을 잘못 보고 있을 때는 먼저 몸을 돌린 뒤 걷습니다. 이 로봇도 같은 방식으로 움직입니다.

도착 판정 반경 만들기

로봇이 목표점에 정확히 도착하기를 기다리면 문제가 생깁니다. 컴퓨터에서 실수는 완벽히 딱 맞아떨어지지 않을 수 있고, 시간 간격 dt 때문에 로봇은 매번 조금씩 건너뛰며 움직입니다. 실제 로봇이라면 바퀴 미끄러짐과 센서 오차도 있습니다.

그래서 목표점 주변에 작은 원을 하나 만든다고 생각합니다. 로봇이 그 원 안에 들어오면 도착했다고 판단합니다.

arrival_radius = 0.1

if distance < arrival_radius:
    linear_speed = 0.0
    angular_speed = 0.0

이 arrival_radius는 도착 판정 반경입니다. 목표점 자체가 바늘 끝이라면, 도착 반경은 목표 주변의 작은 과녁입니다. 로봇 제어에서는 완벽한 한 점보다 충분히 가까운 영역이 훨씬 현실적입니다.

도착 반경을 너무 작게 잡으면 로봇이 목표 근처에서 계속 미세하게 움직일 수 있습니다. 반대로 너무 크게 잡으면 목표에 충분히 가까이 가지 않았는데도 도착했다고 판단할 수 있습니다. 시뮬레이션에서는 보통 0.05에서 0.2 정도의 값을 먼저 실험해 보면 좋습니다. 물론 세계의 단위를 어떻게 정했는지에 따라 적절한 값은 달라집니다.

제어 함수 하나로 묶기

이제 목표 추종에 필요한 계산을 하나의 함수로 묶겠습니다. 이 함수는 로봇의 현재 위치, 현재 방향, 목표 위치를 받아 전진 속도와 회전 속도를 반환합니다.

import numpy as np


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


def compute_linear_speed(
    distance,
    angle_error,
    distance_gain=1.0,
    max_linear_speed=1.0,
    turn_slowdown_angle=np.deg2rad(60.0)
):
    speed = distance_gain * distance
    speed = np.clip(speed, 0.0, max_linear_speed)

    if abs(angle_error) > turn_slowdown_angle:
        speed = 0.0

    return speed


def compute_angular_speed(
    angle_error,
    angle_gain=2.0,
    max_angular_speed=2.0
):
    angular_speed = angle_gain * angle_error
    angular_speed = np.clip(
        angular_speed,
        -max_angular_speed,
        max_angular_speed
    )
    return angular_speed


def compute_control(
    position,
    theta,
    target,
    arrival_radius=0.1
):
    diff = target - position
    distance = np.linalg.norm(diff)

    if distance < arrival_radius:
        return 0.0, 0.0, distance, 0.0, True

    target_angle = np.arctan2(diff[1], diff[0])
    angle_error = normalize_angle(target_angle - theta)

    linear_speed = compute_linear_speed(distance, angle_error)
    angular_speed = compute_angular_speed(angle_error)

    arrived = False
    return linear_speed, angular_speed, distance, angle_error, arrived

반환값을 자세히 보면 다음과 같습니다.

linear_speed  : 전진 속도
angular_speed : 회전 속도
distance      : 목표까지 남은 거리
angle_error   : 목표 방향과 현재 방향의 차이
arrived       : 도착 여부

arrived는 도착했는지 알려 주는 불리언 값입니다. 불리언은 True 또는 False를 가지는 값입니다. 목표 반경 안에 들어오면 True가 됩니다.

이제 로봇의 작은 두뇌가 생겼습니다. 현재 상태와 목표를 넣으면 로봇이 어떤 속도로 움직여야 하는지 알려 줍니다.

로봇 상태 업데이트하기

제어 함수가 속도 명령을 만들었다면, 이제 그 명령으로 로봇 상태를 업데이트해야 합니다.

def update_robot(position, theta, linear_speed, angular_speed, dt):
    theta = theta + angular_speed * dt
    theta = normalize_angle(theta)

    direction = np.array([
        np.cos(theta),
        np.sin(theta)
    ])

    position = position + linear_speed * direction * dt

    return position, theta

여기서 direction은 로봇이 현재 바라보는 방향의 단위 벡터입니다.

direction = [cos(theta), sin(theta)]

theta가 0이면 direction은 [1, 0]입니다. 오른쪽을 향합니다. theta가 90도이면 direction은 [0, 1]에 가까워집니다. 위쪽을 향합니다. 즉, 삼각함수를 사용해 각도를 방향 벡터로 바꾸는 것입니다.

그다음 위치 업데이트는 다음 한 줄입니다.

position = position + linear_speed * direction * dt

이 한 줄 안에는 우리가 지금까지 배운 벡터 감각이 모두 들어 있습니다. 방향 벡터에 속도를 곱하면 속도 벡터가 되고, 거기에 시간 간격을 곱하면 이번 순간에 이동한 작은 변위가 됩니다. 그 변위를 현재 위치에 더하면 새로운 위치가 됩니다.

전체 시뮬레이터 만들기

이제 모든 조각을 합쳐 보겠습니다. 로봇은 시작점에서 출발해 목표점까지 이동합니다. 매 시간 간격마다 제어 명령을 계산하고, 상태를 업데이트하고, 궤적을 저장합니다.

import numpy as np
import matplotlib.pyplot as plt


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


def compute_linear_speed(
    distance,
    angle_error,
    distance_gain=1.0,
    max_linear_speed=1.0,
    turn_slowdown_angle=np.deg2rad(60.0)
):
    speed = distance_gain * distance
    speed = np.clip(speed, 0.0, max_linear_speed)

    if abs(angle_error) > turn_slowdown_angle:
        speed = 0.0

    return speed


def compute_angular_speed(
    angle_error,
    angle_gain=2.0,
    max_angular_speed=2.0
):
    angular_speed = angle_gain * angle_error
    angular_speed = np.clip(
        angular_speed,
        -max_angular_speed,
        max_angular_speed
    )
    return angular_speed


def compute_control(
    position,
    theta,
    target,
    arrival_radius=0.1
):
    diff = target - position
    distance = np.linalg.norm(diff)

    if distance < arrival_radius:
        return 0.0, 0.0, distance, 0.0, True

    target_angle = np.arctan2(diff[1], diff[0])
    angle_error = normalize_angle(target_angle - theta)

    linear_speed = compute_linear_speed(distance, angle_error)
    angular_speed = compute_angular_speed(angle_error)

    return linear_speed, angular_speed, distance, angle_error, False


def update_robot(position, theta, linear_speed, angular_speed, dt):
    theta = theta + angular_speed * dt
    theta = normalize_angle(theta)

    direction = np.array([
        np.cos(theta),
        np.sin(theta)
    ])

    position = position + linear_speed * direction * dt

    return position, theta


position = np.array([0.0, 0.0])
theta = np.deg2rad(90.0)
target = np.array([6.0, 4.0])

arrival_radius = 0.1
dt = 0.1
max_steps = 500

trajectory = [position.copy()]
theta_history = [theta]
distance_history = []
speed_history = []

for step in range(max_steps):
    linear_speed, angular_speed, distance, angle_error, arrived = compute_control(
        position,
        theta,
        target,
        arrival_radius=arrival_radius
    )

    distance_history.append(distance)
    speed_history.append(linear_speed)

    if arrived:
        print(f'목표에 도착했습니다. step={step}, distance={distance:.3f}')
        break

    position, theta = update_robot(
        position,
        theta,
        linear_speed,
        angular_speed,
        dt
    )

    trajectory.append(position.copy())
    theta_history.append(theta)

trajectory = np.array(trajectory)

이 코드를 실행하면 로봇은 목표에 도착할 때까지 움직입니다. trajectory에는 로봇이 지나간 위치들이 저장됩니다. 이제 이 궤적을 그림으로 확인해 보겠습니다.

plt.figure(figsize=(7, 7))

plt.plot(
    trajectory[:, 0],
    trajectory[:, 1],
    'b-',
    linewidth=2,
    label='trajectory'
)

plt.scatter(
    trajectory[0, 0],
    trajectory[0, 1],
    c='green',
    s=120,
    label='start'
)

plt.scatter(
    target[0],
    target[1],
    c='red',
    s=120,
    label='target'
)

circle = plt.Circle(
    target,
    arrival_radius,
    color='red',
    alpha=0.2,
    label='arrival radius'
)
plt.gca().add_patch(circle)

plt.axis('equal')
plt.grid(True)
plt.xlabel('x')
plt.ylabel('y')
plt.legend()
plt.show()

초록색 점은 시작 위치입니다. 빨간 점은 목표 위치입니다. 빨간색 반투명 원은 도착 판정 반경입니다. 파란 선은 로봇이 지나간 궤적입니다.

처음 로봇은 위쪽을 바라보고 있으므로 목표를 바로 향하지 않습니다. 그래서 초반에는 회전하면서 방향을 맞춥니다. 목표 방향과 충분히 가까워지면 앞으로 이동합니다. 목표 근처에 가까워질수록 전진 속도가 줄어들고, 도착 반경 안에 들어오면 멈춥니다.

로봇을 화살표로 그려 보기

궤적만 보면 로봇이 어디를 바라보며 움직였는지 알기 어렵습니다. 이번에는 중간중간 로봇의 방향을 화살표로 표시해 보겠습니다.

def heading_vector(theta):
    return np.array([
        np.cos(theta),
        np.sin(theta)
    ])

이 함수는 방향각을 단위 벡터로 바꿉니다. 이제 궤적 위에 몇 개의 화살표를 그려 보겠습니다.

plt.figure(figsize=(7, 7))

plt.plot(
    trajectory[:, 0],
    trajectory[:, 1],
    'b-',
    linewidth=2,
    label='trajectory'
)

plt.scatter(target[0], target[1], c='red', s=120, label='target')
plt.scatter(trajectory[0, 0], trajectory[0, 1], c='green', s=120, label='start')

arrow_step = max(1, len(trajectory) // 10)

for i in range(0, len(trajectory), arrow_step):
    pos = trajectory[i]
    th = theta_history[i]
    h = heading_vector(th)

    plt.arrow(
        pos[0], pos[1],
        0.4 * h[0], 0.4 * h[1],
        head_width=0.15,
        length_includes_head=True,
        color='black',
        alpha=0.7
    )

circle = plt.Circle(target, arrival_radius, color='red', alpha=0.2)
plt.gca().add_patch(circle)

plt.axis('equal')
plt.grid(True)
plt.xlabel('x')
plt.ylabel('y')
plt.legend()
plt.show()

화살표를 보면 로봇이 단순히 점으로 이동한 것이 아니라, 몸의 방향을 바꾸며 목표를 향해 갔다는 사실이 드러납니다. 이것이 앞 장에서 다룬 각도 계산이 필요한 이유입니다. 목표 방향 벡터만으로도 점 로봇은 움직일 수 있지만, 방향을 가진 로봇은 현재 몸 방향과 목표 방향의 차이를 반드시 계산해야 합니다.

남은 거리와 속도 변화 보기

좋은 목표 추종 로봇은 목표에 가까워질수록 차분해져야 합니다. 목표 근처에서도 계속 빠르게 움직이면 목표를 지나치거나 주변을 흔들릴 수 있습니다.

이번에는 시간에 따라 남은 거리와 전진 속도가 어떻게 변하는지 그려 보겠습니다.

time = np.arange(len(distance_history)) * dt

plt.figure(figsize=(10, 4))

plt.subplot(1, 2, 1)
plt.plot(time, distance_history)
plt.xlabel('time [s]')
plt.ylabel('distance to target')
plt.grid(True)
plt.title('Distance')

plt.subplot(1, 2, 2)
plt.plot(time, speed_history)
plt.xlabel('time [s]')
plt.ylabel('linear speed')
plt.grid(True)
plt.title('Speed')

plt.tight_layout()
plt.show()

거리 그래프는 시간이 지나면서 점점 줄어듭니다. 전진 속도는 처음에는 최대 속도 근처에 있다가 목표에 가까워지면 작아집니다. 물론 초반에 로봇이 목표 방향에서 많이 어긋나 있었다면 속도가 잠깐 0으로 유지될 수 있습니다. 그때 로봇은 앞으로 가지 않고 몸을 돌리는 중입니다.

이 그래프는 로봇의 행동을 이해하는 데 큰 도움이 됩니다. 그림에서 궤적만 보면 "잘 갔다"라고 느낄 수 있지만, 거리와 속도를 함께 보면 로봇이 얼마나 안정적으로 목표에 접근했는지 더 분명하게 볼 수 있습니다.

목표 추종 파라미터 조정하기

로봇의 움직임은 몇 가지 파라미터에 따라 달라집니다. 파라미터란 알고리즘의 성격을 정하는 조절값입니다. 같은 목표 추종 코드라도 파라미터를 바꾸면 로봇이 조심스럽게 움직일 수도 있고, 성급하게 움직일 수도 있습니다.

예를 들어 angle_gain이 너무 작으면 로봇이 목표 방향을 천천히 맞춥니다. 반대로 너무 크면 방향을 급하게 바꾸며 흔들릴 수 있습니다. arrival_radius가 너무 작으면 목표 근처에서 오래 머물며 미세 조정을 할 수 있고, 너무 크면 목표에 덜 가까워졌는데도 도착했다고 판단할 수 있습니다.

제어 알고리즘을 배울 때 중요한 태도는 "한 번에 완벽한 값을 찾겠다"가 아닙니다. 먼저 안전한 값을 넣고, 그래프와 궤적을 보며 조금씩 조정하는 것입니다. 로봇은 숫자로 움직이지만, 좋은 숫자를 찾는 과정에는 실험이 필요합니다.

다른 시작 방향으로 실험하기

이제 시작 방향을 바꿔 보며 로봇이 어떻게 움직이는지 확인해 봅시다. 목표는 그대로 두고, 초기 방향만 바꾸겠습니다.

def run_simulation(initial_theta_deg):
    position = np.array([0.0, 0.0])
    theta = np.deg2rad(initial_theta_deg)
    target = np.array([6.0, 4.0])

    dt = 0.1
    max_steps = 500
    arrival_radius = 0.1

    trajectory = [position.copy()]

    for step in range(max_steps):
        linear_speed, angular_speed, distance, angle_error, arrived = compute_control(
            position,
            theta,
            target,
            arrival_radius=arrival_radius
        )

        if arrived:
            break

        position, theta = update_robot(
            position,
            theta,
            linear_speed,
            angular_speed,
            dt
        )

        trajectory.append(position.copy())

    return np.array(trajectory)

여러 초기 방향을 한 번에 그려 보겠습니다.

initial_angles = [0, 90, 180, -90]
target = np.array([6.0, 4.0])

plt.figure(figsize=(7, 7))

for angle in initial_angles:
    traj = run_simulation(angle)
    plt.plot(
        traj[:, 0],
        traj[:, 1],
        linewidth=2,
        label=f'theta={angle} deg'
    )

plt.scatter(target[0], target[1], c='red', s=120, label='target')
plt.scatter(0, 0, c='green', s=120, label='start')

plt.axis('equal')
plt.grid(True)
plt.xlabel('x')
plt.ylabel('y')
plt.legend()
plt.show()

초기 방향이 다르면 초반 궤적도 달라집니다. 처음부터 목표를 어느 정도 바라보고 있으면 곧장 이동합니다. 반대로 목표와 반대 방향을 보고 있으면 먼저 제자리에서 회전하거나 큰 곡선을 그릴 수 있습니다. 하지만 제어 함수가 제대로 동작한다면 결국 모두 같은 목표 근처로 모입니다.

이 장면은 목표 추종의 중요한 특징을 보여 줍니다. 로봇은 처음 상태가 조금 달라도 매 순간 오차를 다시 계산합니다. 그래서 목표를 향해 계속 수정해 나갈 수 있습니다. 이처럼 현재 상태를 보고 명령을 다시 만드는 구조를 피드백이라고 합니다. 피드백은 로봇 제어의 핵심 개념입니다.

자주 생기는 이상한 움직임

목표 추종 로봇을 직접 만들다 보면 예상과 다른 움직임이 자주 나옵니다. 대부분은 좌표, 각도, 속도 중 하나가 엉킨 경우입니다.

다음 문제들은 특히 자주 만납니다.

• 로봇이 목표에서 멀어지면 target - position을 position - target으로 잘못 썼는지 확인합니다.

• 로봇이 엉뚱한 방향으로 회전하면 np.arctan2(dy, dx)의 인자 순서를 확인합니다.

• 로봇이 목표 근처에서 빙글빙글 돌면 각도 정규화와 arrival_radius를 확인합니다.

• 로봇이 너무 거칠게 움직이면 angle_gain, distance_gain, 최대 속도를 줄여 봅니다.

• 로봇이 너무 느리면 최대 속도와 게인 값을 조금씩 키워 봅니다.

이런 문제를 만났을 때는 코드를 한꺼번에 의심하지 말고, 중간 숫자를 출력해 보는 것이 좋습니다.

print('position:', position)
print('distance:', distance)
print('angle_error deg:', np.rad2deg(angle_error))
print('v:', linear_speed, 'omega:', angular_speed)

로봇의 행동은 결국 이 숫자들에서 나옵니다. 숫자가 납득되면 움직임도 납득됩니다. 숫자가 이상하면 움직임도 이상합니다. 디버깅은 로봇이 무슨 생각을 하는지 들여다보는 과정입니다.

목표 추종 로봇의 전체 흐름 다시 보기

이제 우리가 만든 로봇의 전체 흐름을 다시 정리해 봅시다.

1. 현재 위치 position과 방향 theta를 가진다.
2. 목표 위치 target과 현재 위치의 차이 diff를 계산한다.
3. diff의 크기로 목표까지의 거리 distance를 구한다.
4. atan2로 목표 방향각 target_angle을 구한다.
5. target_angle과 theta의 차이로 angle_error를 구한다.
6. angle_error로 회전 속도 omega를 만든다.
7. distance와 angle_error로 전진 속도 v를 만든다.
8. v와 omega로 position과 theta를 업데이트한다.
9. distance가 arrival_radius보다 작으면 멈춘다.

이 흐름은 단순하지만 강력합니다. 많은 로봇 알고리즘이 이 구조를 더 정교하게 만든 형태입니다. 경로 추종은 목표점이 하나가 아니라 여러 개가 되는 문제입니다. 장애물 회피는 목표를 향하되 위험한 영역을 피하는 문제입니다. 위치 추정은 현재 위치 자체가 불확실할 때 더 믿을 만한 위치를 찾는 문제입니다.

하지만 출발점은 같습니다. 로봇은 현재 위치와 목표 위치를 비교하고, 오차를 줄이는 방향으로 움직입니다.

2부의 마지막 퍼즐 조각

이번 장에서는 목표점까지 스스로 이동하는 2D 로봇을 만들었습니다. 겉으로 보면 작은 시뮬레이션이지만, 그 안에는 2부에서 배운 핵심 개념이 모두 들어 있습니다.

위치는 좌표로 표현했습니다. 목표와 현재 위치의 차이는 벡터로 계산했습니다. 벡터의 크기는 남은 거리였습니다. atan2는 목표 방향각을 알려 주었습니다. 각도 정규화는 로봇이 가장 짧은 방향으로 회전하게 만들었습니다. 거리 기반 속도 조절은 목표 근처에서 안정적으로 감속하게 했습니다. 도착 판정 반경은 로봇이 목표 근처에서 멈출 수 있게 했습니다.

이제 로봇은 단순히 점을 그리는 대상이 아닙니다. 목표를 보고, 방향을 계산하고, 속도를 정하고, 도착하면 멈추는 작은 행동 주체가 되었습니다.

다음 부터는 행렬과 회전을 다룹니다. 지금까지는 좌표와 각도를 직접 계산했습니다. 앞으로는 행렬을 사용해 좌표를 이동시키고 회전시키는 방법을 배웁니다. 행렬은 처음에는 딱딱한 숫자 표처럼 보이지만, 로봇에게는 좌표계를 옮기고 몸체와 센서를 연결하는 강력한 변환 도구가 됩니다.