종합 프로젝트: 2D 로봇 시뮬레이터 완성

흩어져 있던 조각을 하나의 로봇으로 묶기

지금까지 우리는 로봇을 이루는 수학의 조각들을 하나씩 다루었다. 위치는 벡터로 표현했고, 방향은 각도와 단위 벡터로 다루었다. 좌표계가 달라지면 동차 변환 행렬로 위치를 옮겼고, 로봇이 움직이면 속도와 시간 간격으로 다음 위치를 계산했다. 센서는 늘 조금씩 틀리기 때문에 확률과 노이즈를 사용했고, 더 나은 판단을 위해 추정과 최적화의 감각도 익혔다.

이제 마지막으로 그 조각들을 하나의 작은 시스템으로 연결해 보자. 이번 장에서 만들 시뮬레이터는 화면 위의 점 하나가 아니다. 로봇은 자기 위치를 추정하고, 여러 목표점을 따라가며, 센서 노이즈 때문에 흔들리는 측정값을 받는다. 그리고 우리가 만든 좌표 변환 도구를 이용해 센서가 본 물체를 다시 월드 좌표계 위에 그린다.

이 프로젝트에서 다룰 세계는 단순한 2D 평면이다. 하지만 그 안에는 실제 로봇 시스템의 중요한 구조가 모두 들어 있다. 로봇은 실제 위치를 가지고 있지만 그 위치를 완벽히 알지는 못한다. 엔코더는 이동량을 알려 주지만 조금씩 틀리고, 절대 위치 센서는 가끔씩 더 큰 흔들림을 가진 값을 준다. 로봇 앞쪽에 붙은 센서는 주변 랜드마크를 자기 기준 좌표로 관측한다. 우리는 이 모든 정보를 모아 실제 궤적과 추정 궤적을 비교한다.

우리가 만들 시뮬레이터의 구조

이번 시뮬레이터에는 세 가지 좌표계가 등장한다.

월드 좌표계는 지도 전체의 기준이다. 목표점, 랜드마크, 실제 로봇 위치는 모두 월드 좌표계에 놓인다. 로봇 좌표계는 로봇 몸체를 기준으로 한 좌표계다. 로봇의 앞쪽이 보통 x축 양의 방향이고, 왼쪽이 y축 양의 방향이다. 센서 좌표계는 로봇 앞에 달린 센서의 기준이다. 센서가 로봇 중심보다 앞에 붙어 있다면, 센서 좌표계는 로봇 좌표계에서 조금 이동한 위치에 있다.

동차 변환 행렬로 쓰면 관계는 다음과 같다.

T_world_sensor = T_world_robot @ T_robot_sensor

이 식은 센서 좌표계의 점을 월드 좌표계로 옮길 때 사용한다.

p_world = T_world_sensor @ p_sensor

반대로 월드에 있는 랜드마크가 센서에서 어떻게 보이는지 알고 싶다면 역변환을 쓴다.

p_sensor = inverse(T_world_sensor) @ p_world

이 구조는 카메라, LiDAR, 로봇 팔, 모바일 로봇에서 반복해서 등장한다. 장비가 복잡해져도 핵심 질문은 같다. "이 점은 어느 좌표계에서 표현된 점인가?" 그리고 "어느 좌표계로 옮겨야 하는가?"

실제 위치와 추정 위치를 나누어 생각하기

시뮬레이션에서는 우리가 신처럼 모든 것을 알 수 있다. 그래서 실제 로봇 위치를 정확히 저장할 수 있다. 하지만 로봇 자신은 실제 위치를 직접 알지 못한다. 로봇은 엔코더와 센서값을 통해 자기 위치를 추정할 뿐이다.

따라서 코드에서는 두 가지 pose를 따로 관리한다.

true_pose = 실제 로봇 위치
est_pose  = 로봇이 믿고 있는 추정 위치

true_pose는 시뮬레이션 세계의 정답이다. est_pose는 노이즈가 섞인 센서값으로 계산한 결과다. 목표 추종 제어도 일부러 est_pose를 기준으로 하게 만들 것이다. 현실의 로봇도 정답 위치가 아니라 자신이 추정한 위치를 기준으로 움직이기 때문이다.

pose는 다음처럼 세 숫자로 표현한다.

[x, y, theta]

여기서 x, y는 월드 좌표계에서의 위치이고, theta는 로봇이 바라보는 방향이다.

전체 코드 먼저 보기

아래 코드는 이번 장의 최종 결과물이다. 하나의 Python 파일이나 Jupyter Notebook 셀에 넣고 실행할 수 있다. NumPy와 Matplotlib만 사용한다.

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Polygon

rng = np.random.default_rng(7)


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


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


def inv_T(T):
    R = T[:2, :2]
    t = T[:2, 2]

    Ti = np.eye(3)
    Ti[:2, :2] = R.T
    Ti[:2, 2] = -R.T @ t
    return Ti


def transform_points(T, points):
    points = np.asarray(points, dtype=float)

    if points.size == 0:
        return points.reshape(0, 2)

    points_h = np.c_[points, np.ones(len(points))]
    transformed = (T @ points_h.T).T
    return transformed[:, :2]


def update_pose(pose, v, w, dt):
    x, y, theta = pose

    x = x + v * np.cos(theta) * dt
    y = y + v * np.sin(theta) * dt
    theta = wrap_angle(theta + w * dt)

    return np.array([x, y, theta])


def waypoint_controller(pose, target, params):
    position = pose[:2]
    theta = pose[2]

    to_target = target - position
    distance = np.linalg.norm(to_target)

    target_angle = np.arctan2(to_target[1], to_target[0])
    heading_error = wrap_angle(target_angle - theta)

    angle_factor = max(0.0, np.cos(heading_error))
    v = params['k_v'] * distance * angle_factor
    v = np.clip(v, 0.0, params['max_v'])

    if abs(heading_error) > params['turn_in_place_angle']:
        v = 0.0

    w = params['k_w'] * heading_error
    w = np.clip(w, -params['max_w'], params['max_w'])

    return v, w, distance, heading_error


def visible_landmarks(landmarks_world, T_world_sensor, sensor_range, fov):
    T_sensor_world = inv_T(T_world_sensor)
    landmarks_sensor = transform_points(T_sensor_world, landmarks_world)

    distances = np.linalg.norm(landmarks_sensor, axis=1)
    bearings = np.arctan2(landmarks_sensor[:, 1], landmarks_sensor[:, 0])

    visible = (
        (landmarks_sensor[:, 0] > 0.0) &
        (distances < sensor_range) &
        (np.abs(bearings) < fov / 2)
    )

    return landmarks_sensor[visible], visible


def robot_shape(pose, scale=1.0):
    local_shape = np.array([
        [0.35, 0.00],
        [-0.25, 0.20],
        [-0.18, 0.00],
        [-0.25, -0.20]
    ]) * scale

    T_world_robot = make_T(pose[0], pose[1], pose[2])
    return transform_points(T_world_robot, local_shape)


def draw_robot(ax, pose, color, label):
    body = Polygon(
        robot_shape(pose),
        closed=True,
        facecolor=color,
        edgecolor='black',
        alpha=0.65,
        label=label
    )
    ax.add_patch(body)


# -----------------------------
# 1. 지도, 목표점, 랜드마크 설정
# -----------------------------

waypoints = np.array([
    [1.5, 0.5],
    [3.0, 1.8],
    [2.1, 3.2],
    [0.0, 2.7],
    [-1.2, 1.0],
    [0.0, 0.0]
])

landmarks = np.array([
    [0.8, 1.8],
    [2.4, 0.4],
    [3.4, 2.7],
    [1.2, 3.6],
    [-0.9, 2.4],
    [-1.6, 0.2],
    [0.2, -0.7]
])

# 센서는 로봇 중심보다 앞쪽 0.3m 위치에 달려 있다고 가정한다.
T_robot_sensor = make_T(0.30, 0.0, 0.0)


# -----------------------------
# 2. 제어기와 센서 파라미터
# -----------------------------

controller_params = {
    'k_v': 1.0,
    'k_w': 2.5,
    'max_v': 0.8,
    'max_w': 2.0,
    'turn_in_place_angle': np.deg2rad(70),
    'arrival_radius': 0.25
}

dt = 0.1
max_steps = 1200

# 엔코더 노이즈: 속도와 각속도가 조금씩 흔들린다고 가정한다.
odom_v_std = 0.03
odom_w_std = np.deg2rad(2.0)

# 절대 위치 센서 노이즈: GPS처럼 가끔 위치를 알려 주지만 꽤 흔들린다.
position_sensor_std = 0.20
heading_sensor_std = np.deg2rad(5.0)
position_sensor_period = 5

# 센서가 랜드마크를 관측할 때의 좌표 노이즈
landmark_sensor_std = 0.05
sensor_range = 2.2
sensor_fov = np.deg2rad(100)

# 간단한 예측-보정 필터의 보정 강도
position_gain = 0.20
heading_gain = 0.15


# -----------------------------
# 3. 초기 상태
# -----------------------------

true_pose = np.array([0.0, 0.0, 0.0])
est_pose = np.array([0.15, -0.10, np.deg2rad(8.0)])

target_index = 0

true_history = []
est_history = []
position_measurements = []
sensor_cloud_world = []


# -----------------------------
# 4. 시뮬레이션 루프
# -----------------------------

for step in range(max_steps):
    if target_index >= len(waypoints):
        break

    target = waypoints[target_index]

    # 로봇은 실제 위치가 아니라 추정 위치를 기준으로 목표를 따라간다.
    v_cmd, w_cmd, distance, heading_error = waypoint_controller(
        est_pose,
        target,
        controller_params
    )

    if distance < controller_params['arrival_radius']:
        target_index += 1
        continue

    # 실제 로봇은 명령을 거의 따르지만, 아주 작은 실행 오차가 있다.
    v_real = max(0.0, v_cmd + rng.normal(0.0, 0.01))
    w_real = w_cmd + rng.normal(0.0, np.deg2rad(0.5))
    true_pose = update_pose(true_pose, v_real, w_real, dt)

    # 엔코더는 실제 움직임을 측정하지만 노이즈가 섞인다.
    v_odom = v_real + rng.normal(0.0, odom_v_std)
    w_odom = w_real + rng.normal(0.0, odom_w_std)

    # 예측 단계: 엔코더만 믿고 다음 위치를 추정한다.
    est_pose = update_pose(est_pose, v_odom, w_odom, dt)

    # 보정 단계: 일정 주기마다 절대 위치 센서가 들어온다.
    if step % position_sensor_period == 0:
        measured_position = true_pose[:2] + rng.normal(
            0.0,
            position_sensor_std,
            size=2
        )
        measured_heading = wrap_angle(
            true_pose[2] + rng.normal(0.0, heading_sensor_std)
        )

        position_measurements.append(measured_position)

        est_pose[:2] = (
            (1.0 - position_gain) * est_pose[:2] +
            position_gain * measured_position
        )

        heading_error_measured = wrap_angle(measured_heading - est_pose[2])
        est_pose[2] = wrap_angle(
            est_pose[2] + heading_gain * heading_error_measured
        )

    # 센서 좌표계에서 랜드마크를 관측한다.
    T_world_robot_true = make_T(true_pose[0], true_pose[1], true_pose[2])
    T_world_sensor_true = T_world_robot_true @ T_robot_sensor

    landmarks_sensor, visible_mask = visible_landmarks(
        landmarks,
        T_world_sensor_true,
        sensor_range,
        sensor_fov
    )

    if len(landmarks_sensor) > 0:
        noisy_landmarks_sensor = landmarks_sensor + rng.normal(
            0.0,
            landmark_sensor_std,
            size=landmarks_sensor.shape
        )

        # 로봇은 추정 위치를 이용해 센서 관측값을 월드 좌표로 되돌린다.
        T_world_robot_est = make_T(est_pose[0], est_pose[1], est_pose[2])
        T_world_sensor_est = T_world_robot_est @ T_robot_sensor
        noisy_landmarks_world_est = transform_points(
            T_world_sensor_est,
            noisy_landmarks_sensor
        )
        sensor_cloud_world.append(noisy_landmarks_world_est)

    true_history.append(true_pose.copy())
    est_history.append(est_pose.copy())


# -----------------------------
# 5. 결과 정리
# -----------------------------

true_history = np.array(true_history)
est_history = np.array(est_history)

if len(position_measurements) > 0:
    position_measurements = np.array(position_measurements)
else:
    position_measurements = np.empty((0, 2))

if len(sensor_cloud_world) > 0:
    sensor_cloud_world = np.vstack(sensor_cloud_world)
else:
    sensor_cloud_world = np.empty((0, 2))

time = np.arange(len(true_history)) * dt
position_error = np.linalg.norm(
    true_history[:, :2] - est_history[:, :2],
    axis=1
)

print(f'시뮬레이션 시간: {time[-1]:.1f}초')
print(f'마지막 실제 위치: {true_history[-1]}')
print(f'마지막 추정 위치: {est_history[-1]}')
print(f'마지막 위치 추정 오차: {position_error[-1]:.3f} m')


# -----------------------------
# 6. 시각화
# -----------------------------

fig, (ax_map, ax_error) = plt.subplots(
    1,
    2,
    figsize=(13, 6),
    gridspec_kw={'width_ratios': [2.0, 1.0]}
)

ax_map.plot(
    waypoints[:, 0],
    waypoints[:, 1],
    'o--',
    color='orange',
    linewidth=2,
    label='waypoints'
)

ax_map.scatter(
    landmarks[:, 0],
    landmarks[:, 1],
    marker='*',
    s=150,
    color='black',
    label='landmarks'
)

ax_map.plot(
    true_history[:, 0],
    true_history[:, 1],
    color='tab:blue',
    linewidth=2.5,
    label='true path'
)

ax_map.plot(
    est_history[:, 0],
    est_history[:, 1],
    '--',
    color='tab:red',
    linewidth=2,
    label='estimated path'
)

if len(position_measurements) > 0:
    ax_map.scatter(
        position_measurements[:, 0],
        position_measurements[:, 1],
        marker='x',
        s=35,
        color='tab:green',
        alpha=0.55,
        label='noisy position sensor'
    )

if len(sensor_cloud_world) > 0:
    ax_map.scatter(
        sensor_cloud_world[:, 0],
        sensor_cloud_world[:, 1],
        s=10,
        color='purple',
        alpha=0.25,
        label='landmark measurements'
    )

draw_robot(ax_map, true_history[-1], 'tab:blue', 'final true robot')
draw_robot(ax_map, est_history[-1], 'tab:red', 'final estimated robot')

ax_map.set_title('2D robot simulation')
ax_map.set_xlabel('x [m]')
ax_map.set_ylabel('y [m]')
ax_map.axis('equal')
ax_map.grid(True)
ax_map.legend(loc='best')

ax_error.plot(time, position_error, color='purple')
ax_error.set_title('position estimation error')
ax_error.set_xlabel('time [s]')
ax_error.set_ylabel('error [m]')
ax_error.grid(True)

plt.tight_layout()
plt.show()

코드의 핵심 1: 동차 변환으로 좌표계를 연결하기

이 프로젝트에서 가장 중요한 함수는 make_T, inv_T, transform_points다. 이 세 함수는 좌표 변환의 기본 도구다.

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

이 행렬에는 회전과 이동이 함께 들어 있다. 왼쪽 위의 2×2 부분은 회전이고, 오른쪽 마지막 열의 x, y는 이동이다. 그래서 로봇의 pose가 [x, y, theta]로 주어지면 곧바로 T_world_robot을 만들 수 있다.

센서는 로봇 중심보다 앞쪽에 붙어 있다.

T_robot_sensor = make_T(0.30, 0.0, 0.0)

이 말은 센서 원점이 로봇 좌표계에서 x 방향으로 0.30m 앞에 있다는 뜻이다. 로봇이 월드에서 움직이면 센서도 함께 움직인다. 센서의 월드 좌표 변환은 다음 한 줄로 만들어진다.

T_world_sensor = T_world_robot @ T_robot_sensor

변환 순서를 눈여겨보자. 먼저 센서 좌표에서 로봇 좌표로 가고, 그다음 로봇 좌표에서 월드 좌표로 간다. 그래서 행렬 곱의 순서가 T_world_robot @ T_robot_sensor가 된다. 순서를 바꾸면 의미가 달라진다. 행렬 곱은 교환법칙이 성립하지 않는다.

코드의 핵심 2: 목표 추종 제어

목표 추종 제어는 로봇이 현재 위치에서 목표점까지의 방향을 계산하고, 그 방향을 향해 회전하며 전진하게 만드는 방법이다.

to_target = target - position
distance = np.linalg.norm(to_target)
target_angle = np.arctan2(to_target[1], to_target[0])
heading_error = wrap_angle(target_angle - theta)

to_target은 로봇에서 목표까지의 벡터다. 이 벡터의 크기가 목표까지의 거리이고, atan2로 얻은 각도가 목표 방향이다. 로봇이 현재 바라보는 방향 theta와 목표 방향의 차이가 heading_error다.

제어기에서는 로봇이 목표 방향을 크게 벗어나 있을 때 무리하게 전진하지 않도록 했다.

angle_factor = max(0.0, np.cos(heading_error))
v = params['k_v'] * distance * angle_factor

목표가 로봇 정면에 있으면 cos(heading_error)가 1에 가깝다. 그러면 로봇은 잘 전진한다. 목표가 옆이나 뒤에 있으면 값이 작아지고, 로봇은 먼저 회전하려고 한다. 특히 방향 오차가 너무 크면 선속도 v를 0으로 만들어 제자리 회전에 가깝게 움직인다.

이 작은 조건 하나가 궤적을 훨씬 자연스럽게 만든다. 로봇이 목표를 등진 채 옆으로 미끄러지듯 이동하지 않고, 먼저 몸을 돌린 뒤 앞으로 나아가기 때문이다.

코드의 핵심 3: 노이즈가 있는 센서 만들기

현실의 센서는 언제나 흔들린다. 이 시뮬레이터에서는 세 종류의 노이즈를 넣었다.

• 엔코더 노이즈: 로봇이 실제로 얼마나 움직였는지 측정할 때 생기는 오차

• 절대 위치 센서 노이즈: GPS나 외부 위치 추적 장치처럼 전역 위치를 측정할 때 생기는 오차

• 랜드마크 센서 노이즈: 로봇 앞 센서가 물체 위치를 관측할 때 생기는 오차

가우시안 노이즈는 다음처럼 만든다.

rng.normal(0.0, standard_deviation)

평균이 0이라는 것은 오차가 한쪽으로 계속 치우치지는 않는다는 뜻이다. 표준편차가 클수록 측정값은 더 넓게 흔들린다.

엔코더 노이즈는 추정 위치를 조금씩 흔들리게 만든다.

v_odom = v_real + rng.normal(0.0, odom_v_std)
w_odom = w_real + rng.normal(0.0, odom_w_std)
est_pose = update_pose(est_pose, v_odom, w_odom, dt)

이 과정은 적분 오차를 만든다. 속도가 조금 틀린 채로 계속 누적되면 위치 오차는 시간이 지날수록 커질 수 있다. 그래서 중간중간 절대 위치 센서로 보정한다.

코드의 핵심 4: 간단한 예측-보정 필터

이번 장에서 구현한 추정기는 완전한 칼만 필터는 아니다. 하지만 구조는 칼만 필터와 닮아 있다. 먼저 엔코더로 예측하고, 절대 위치 센서가 들어오면 보정한다.

예측 단계는 다음과 같다.

est_pose = update_pose(est_pose, v_odom, w_odom, dt)

이 단계에서는 "로봇이 이만큼 움직였을 것이다"라고 계산한다. 문제는 엔코더에 노이즈가 있기 때문에 예측만 계속하면 오차가 누적된다는 점이다.

보정 단계에서는 측정값을 일부 섞는다.

est_pose[:2] = (
    (1.0 - position_gain) * est_pose[:2] +
    position_gain * measured_position
)

position_gain이 0.20이라면 추정값의 80%와 측정값의 20%를 섞는다는 뜻이다. 측정값을 너무 많이 믿으면 추정 위치가 센서 노이즈를 따라 이리저리 흔들린다. 반대로 측정값을 너무 적게 믿으면 엔코더 누적 오차를 충분히 줄이지 못한다.

칼만 필터에서는 이 가중치를 불확실성에 따라 자동으로 계산한다. 여기서는 단순하게 고정된 값을 사용했다. 그래도 예측과 보정이라는 핵심 감각은 충분히 들어 있다.

각도 보정은 그냥 빼면 안 된다. 각도는 -π와 π 근처에서 갑자기 값이 튈 수 있기 때문이다. 그래서 방향 오차는 항상 wrap_angle로 정리한다.

heading_error_measured = wrap_angle(measured_heading - est_pose[2])
est_pose[2] = wrap_angle(
    est_pose[2] + heading_gain * heading_error_measured
)

각도를 다룰 때는 이 습관이 중요하다. 로봇이 거의 같은 방향을 보고 있는데 숫자로는 179도와 -179도처럼 보일 수 있다. 이때 단순히 빼면 358도의 큰 차이가 나지만, 실제로는 2도 차이에 가깝다.

코드의 핵심 5: 센서가 본 랜드마크를 다시 지도에 그리기

랜드마크 센서는 월드 좌표를 직접 주지 않는다. 센서 기준 좌표를 준다. 예를 들어 센서가 어떤 물체를 [1.0, 0.2]에서 봤다면, 그것은 "센서 앞쪽 1.0m, 왼쪽 0.2m"라는 뜻이다.

시뮬레이션에서는 먼저 실제 센서 위치를 이용해 월드의 랜드마크를 센서 좌표로 바꾼다.

T_world_sensor_true = T_world_robot_true @ T_robot_sensor
landmarks_sensor, visible_mask = visible_landmarks(
    landmarks,
    T_world_sensor_true,
    sensor_range,
    sensor_fov
)

그리고 센서 노이즈를 더한다.

noisy_landmarks_sensor = landmarks_sensor + rng.normal(
    0.0,
    landmark_sensor_std,
    size=landmarks_sensor.shape
)

여기까지가 로봇이 실제로 받은 관측값이라고 생각할 수 있다. 이제 로봇은 이 센서 좌표를 지도 위에 표시해야 한다. 그런데 로봇은 실제 위치를 모르므로 추정 위치를 사용한다.

T_world_robot_est = make_T(est_pose[0], est_pose[1], est_pose[2])
T_world_sensor_est = T_world_robot_est @ T_robot_sensor
noisy_landmarks_world_est = transform_points(
    T_world_sensor_est,
    noisy_landmarks_sensor
)

이렇게 만든 보라색 점들은 실제 랜드마크 주변에 흩어진다. 추정 위치가 정확하고 센서 노이즈가 작으면 보라색 점들은 검은색 별표 근처에 모인다. 반대로 위치 추정이 흔들리거나 센서 노이즈가 크면 보라색 점들은 더 넓게 퍼진다.

여기서 중요한 사실이 하나 있다. 센서가 틀려도 지도 위 점이 흔들리고, 로봇 위치 추정이 틀려도 지도 위 점이 흔들린다. 즉 센서 데이터의 품질은 센서 자체만의 문제가 아니다. 로봇의 pose 추정이 나쁘면 좋은 센서도 나쁜 지도처럼 보일 수 있다.

결과 그래프 읽기

코드를 실행하면 왼쪽에는 로봇의 지도와 궤적이, 오른쪽에는 위치 추정 오차가 나타난다.

왼쪽 지도에서 파란 선은 실제 로봇 궤적이다. 빨간 점선은 추정 궤적이다. 주황색 원과 점선은 로봇이 따라가야 하는 웨이포인트다. 초록색 x 표시는 노이즈가 섞인 절대 위치 센서 측정값이다. 검은색 별표는 실제 랜드마크이고, 보라색 점들은 센서 관측값을 월드 좌표계로 되돌려 그린 결과다.

오른쪽 그래프는 시간에 따른 위치 추정 오차를 보여 준다. 오차가 계속 커지기만 한다면 엔코더 적분이 드리프트하고 있다는 뜻이다. 오차가 어느 정도 커졌다가 다시 줄어든다면 절대 위치 센서가 보정 역할을 하고 있다는 뜻이다. 오차가 지나치게 요동친다면 position_gain이 너무 크거나 위치 센서 노이즈가 너무 클 수 있다.

이제 그래프는 단순한 그림이 아니라 로봇 시스템의 건강 상태를 보여 주는 계기판이 된다.

실험해 볼 파라미터

최종 프로젝트의 장점은 숫자를 바꾸며 로봇 행동을 직접 관찰할 수 있다는 점이다. 다음 값을 바꾸면 결과가 눈에 띄게 달라진다.

• odom_v_std, odom_w_std를 키우면 엔코더 예측이 더 빨리 틀어진다.

• position_sensor_std를 키우면 초록색 위치 측정값이 더 넓게 흩어진다.

• position_gain을 키우면 추정 궤적이 측정값을 더 적극적으로 따라간다.

• position_gain을 줄이면 추정 궤적은 부드러워지지만 드리프트 보정이 느려진다.

• landmark_sensor_std를 키우면 보라색 랜드마크 관측점이 더 퍼진다.

• sensor_range와 sensor_fov를 바꾸면 로봇이 볼 수 있는 랜드마크 수가 달라진다.

• max_v, max_w, k_v, k_w를 바꾸면 목표 추종 궤적이 부드럽거나 공격적으로 변한다.

특히 position_gain은 센서 퓨전의 감각을 익히기에 좋은 값이다. 0에 가깝게 두면 로봇은 엔코더를 거의 전적으로 믿는다. 1에 가깝게 두면 절대 위치 센서를 거의 그대로 따라간다. 어느 쪽도 항상 정답은 아니다. 노이즈가 작은 센서는 더 믿어도 되고, 노이즈가 큰 센서는 조심해서 믿어야 한다.

이 프로젝트에 들어 있는 로봇 수학

이 장의 코드는 길어 보이지만, 안에 들어 있는 수학은 우리가 이미 배운 것들이다.

좌표와 벡터는 로봇의 위치, 목표점, 랜드마크를 표현하는 데 사용되었다. target - position은 목표 방향 벡터이고, 그 크기는 목표까지의 거리다. atan2는 목표 방향 각도를 계산했다.

행렬과 동차 좌표계는 로봇 좌표계, 센서 좌표계, 월드 좌표계를 연결했다. T_world_robot @ T_robot_sensor는 로봇 몸체에 붙은 센서가 월드에서 어디에 있는지 알려 주었다.

미분과 적분의 감각은 update_pose에 들어 있다. 속도 v, 각속도 w, 시간 간격 dt로 다음 pose를 계산했다. 이산 시간 시뮬레이션에서는 작은 변화량을 계속 누적해서 움직임을 만든다.

확률은 센서 노이즈로 들어왔다. rng.normal로 만든 가우시안 노이즈는 현실의 측정값이 왜 흔들리는지 보여 주었다. 표준편차를 바꾸면 센서 신뢰도가 달라졌다.

추정은 예측-보정 구조로 들어왔다. 엔코더로 다음 위치를 예측하고, 절대 위치 센서로 일부 보정했다. 이것은 더 정교한 칼만 필터와 SLAM으로 이어지는 기본 리듬이다.

최적화의 감각도 숨어 있다. 목표 추종 제어는 매 순간 목표까지의 거리와 방향 오차를 줄이는 선택을 한다. 경로 최적화까지 넣지는 않았지만, 로봇이 더 짧고 부드럽고 안전한 경로를 찾으려면 결국 비용을 정의하고 줄여야 한다.

다음 단계로 확장하기

이 시뮬레이터는 완성된 끝이라기보다 더 큰 로봇 시스템으로 가는 출발점이다. 여기서 몇 가지 기능을 추가하면 훨씬 현실적인 프로젝트가 된다.

장애물을 넣고 충돌 비용을 계산하면 경로 계획 문제가 된다. 랜드마크 관측값으로 로봇 위치를 보정하면 위치 추정 문제가 더 흥미로워진다. 랜드마크 위치까지 함께 추정하면 SLAM의 입구에 들어선다. 예측-보정 필터를 고정 gain 방식이 아니라 칼만 필터로 바꾸면 불확실성을 수학적으로 관리할 수 있다. 로봇 모델을 2D 점 로봇에서 차동구동 로봇으로 바꾸면 바퀴 속도와 회전 반경도 다룰 수 있다.

하지만 어떤 방향으로 확장하든 기본은 변하지 않는다. 좌표계를 정확히 정의하고, 변환 순서를 지키고, 센서값의 불확실성을 생각하고, 추정값과 실제값의 차이를 시각화해야 한다.

마지막 정리

이 책의 목표는 어려운 공식을 많이 외우는 것이 아니었다. 로봇이 움직이고, 보고, 판단하는 과정을 수학으로 이해하는 것이 목표였다. 처음에는 점 하나의 위치를 그리는 것에서 시작했다. 이제는 목표점을 따라 움직이고, 센서 노이즈를 받고, 자기 위치를 추정하고, 관측값을 지도 위에 다시 그리는 작은 로봇 시스템을 만들었다.

로봇공학의 수학은 교과서 속 기호로만 존재하지 않는다. x, y, theta는 로봇의 몸이 되고, 행렬은 좌표계를 잇는 다리가 되며, 확률은 센서의 흔들림을 설명하는 언어가 된다. Python은 그 모든 것을 실험할 수 있는 작업대다.

이제 코드를 조금씩 바꿔 보자. 노이즈를 키우고, 목표점을 옮기고, 센서 위치를 바꾸고, gain을 조절해 보자. 로봇이 이상하게 움직이면 그래프를 보고 이유를 추적해 보자. 그 과정에서 수학은 더 이상 외워야 할 공식이 아니라, 로봇을 이해하기 위한 감각이 된다.