흩어진 조각을 하나의 로봇으로 묶기
지금까지 우리는 로봇을 이루는 작은 수학 조각들을 하나씩 배웠다. 위치는 x, y라는 숫자로 표현했고, 목표를 향하는 방향은 벡터로 계산했다. 두 점 사이의 거리는 피타고라스 정리로 구했고, 방향 각도는 atan2로 얻었다. 속도는 위치의 변화였고, 위치는 속도를 시간에 따라 조금씩 누적해 계산했다.
이 장에서는 그 조각들을 하나로 묶는다. 이제 로봇은 하나의 목표점만 바라보지 않는다. 여러 목표점을 순서대로 방문하면서 경로를 따라간다. 우리는 로봇의 현재 위치, 방향, 속도를 시간 루프 안에서 계속 업데이트하고, 마지막에는 로봇이 어떤 길을 지나갔는지 그림으로 확인할 것이다.
이번 장의 목표는 거창한 로봇 소프트웨어를 만드는 것이 아니다. 핵심은 "로봇이 움직인다"는 말을 수식과 코드로 직접 경험하는 것이다. 로봇의 머릿속에는 이런 질문이 계속 반복된다.
"지금 나는 어디에 있지? 다음 목표점은 어디지? 그쪽을 보려면 얼마나 돌아야 하지? 앞으로 얼마나 빠르게 가야 하지?"
이 질문에 답하는 코드가 바로 이동 시뮬레이터다.
이번 프로젝트의 전체 구조
이번 장에서 만들 시뮬레이션은 2D 평면 위에서 움직이는 작은 이동 로봇이다. 로봇은 방향을 가지고 있으며, 자동차나 차륜형 로봇처럼 현재 바라보는 방향으로 전진한다고 가정한다.
로봇의 상태는 다음 세 값으로 표현한다.
state = np.array([x, y, theta])여기서 x, y는 월드 좌표계에서의 위치이고, theta는 로봇이 바라보는 방향 각도다. theta는 라디안 단위로 저장한다. 라디안은 각도를 표현하는 또 다른 단위로, Python의 삼각함수 np.sin, np.cos, np.atan2는 기본적으로 라디안을 사용한다.
로봇은 매 시간 간격 dt마다 다음 과정을 반복한다.
현재 목표점을 확인한다.
현재 위치에서 목표점까지의 벡터를 계산한다.
거리 오차와 방향 오차를 구한다.
선속도와 각속도 명령을 만든다.
속도를 적분해 새로운 위치와 방향을 계산한다.
목표점에 충분히 가까워지면 다음 목표점으로 바꾼다.
여기서 선속도는 앞으로 가는 속도이고, 각속도는 회전하는 속도다. 예를 들어 v = 1.0이면 초당 1m 전진한다는 뜻이고, w = 0.5이면 초당 0.5라디안씩 방향이 바뀐다는 뜻이다.
수식으로 쓰면 다음과 같다.
x_next = x + v * cos(theta) * dt
y_next = y + v * sin(theta) * dt
theta_next = theta + w * dt이 식은 적분의 가장 기본적인 형태다. 아주 작은 시간 동안 속도가 거의 일정하다고 보고, 속도 × 시간만큼 위치를 더한다. 복잡해 보이지만, 사실은 "조금 움직이고, 다시 계산하고, 또 조금 움직이는" 방식이다.
필요한 라이브러리 준비하기
먼저 NumPy와 Matplotlib을 불러온다. NumPy는 벡터와 배열 계산을 맡고, Matplotlib은 로봇의 경로와 속도 변화를 그리는 데 사용한다.
import numpy as np
import matplotlib.pyplot as plt시뮬레이션에서는 각도를 자주 다룬다. 그런데 각도에는 한 가지 귀찮은 문제가 있다. 예를 들어 179도와 -179도는 숫자로는 차이가 커 보이지만, 실제 방향은 거의 비슷하다. 로봇이 이 차이를 잘못 이해하면 아주 조금만 돌면 될 상황에서 반대쪽으로 크게 회전할 수 있다.
그래서 각도 오차를 항상 -π에서 π 사이로 정리하는 함수를 만든다.
def normalize_angle(angle):
"""각도를 -pi ~ pi 범위로 정리한다."""
return (angle + np.pi) % (2 * np.pi) - np.pi이 함수는 앞으로 로봇의 방향 오차를 계산할 때 계속 사용한다. 로봇 시뮬레이션에서 작은 함수 하나가 움직임의 안정성을 크게 바꾸기도 한다.
여러 목표점을 배열로 만들기
이제 로봇이 방문할 목표점들을 만든다. 하나의 목표점은 [x, y] 형태의 벡터이고, 여러 목표점은 2차원 NumPy 배열로 표현할 수 있다.
waypoints = np.array([
[0.0, 0.0],
[2.0, 1.0],
[4.0, 1.0],
[5.0, 3.0],
[3.0, 4.0],
[1.0, 3.0],
[0.0, 5.0]
])이 배열은 로봇이 따라가야 할 지도처럼 생각할 수 있다. 첫 번째 점에서 출발해 두 번째 점으로 가고, 두 번째 점에 도착하면 세 번째 점으로 향한다. 이렇게 목표점을 이어 만든 경로를 흔히 웨이포인트 경로라고 한다.
웨이포인트는 로봇에게 "정확히 이 선 위만 지나가라"라고 말하지 않는다. 대신 "이 점들을 순서대로 지나가라"라고 말한다. 그래서 로봇의 실제 궤적은 목표점을 잇는 직선과 조금 다를 수 있다. 특히 방향 전환이 필요한 코너에서는 둥글게 돌아갈 수 있다. 이것은 오히려 자연스럽다. 실제 로봇도 즉시 방향을 바꿀 수 없기 때문이다.
웨이포인트를 먼저 그림으로 확인해 보자.
plt.figure(figsize=(6, 6))
plt.plot(waypoints[:, 0], waypoints[:, 1], "k--", label="waypoint path")
plt.scatter(waypoints[:, 0], waypoints[:, 1], c="red", s=60, label="waypoints")
for i, p in enumerate(waypoints):
plt.text(p[0] + 0.05, p[1] + 0.05, str(i), fontsize=12)
plt.axis("equal")
plt.grid(True)
plt.xlabel("x [m]")
plt.ylabel("y [m]")
plt.legend()
plt.show()여기서 waypoints[:, 0]은 모든 목표점의 x 좌표이고, waypoints[:, 1]은 모든 목표점의 y 좌표다. NumPy 배열을 사용하면 이렇게 여러 점의 좌표를 한 번에 꺼낼 수 있다.
코너에서 속도를 줄이는 감각
경로를 따라 움직일 때 모든 구간에서 같은 속도로 달리면 문제가 생길 수 있다. 직선 구간에서는 빠르게 가도 괜찮지만, 급하게 꺾이는 코너에서는 속도를 줄이는 편이 좋다. 너무 빠르게 들어가면 목표점을 지나쳐 크게 돌아오거나, 진동하듯 흔들릴 수 있다.
각 웨이포인트에서 경로가 얼마나 꺾이는지 계산해 보자. 가운데 점 p_curr를 기준으로 이전 점에서 들어오는 벡터와 다음 점으로 나가는 벡터의 각도를 비교하면 된다.
def waypoint_turn_angles(waypoints):
"""각 웨이포인트에서 경로가 꺾이는 정도를 계산한다."""
n = len(waypoints)
angles = np.zeros(n)
for i in range(1, n - 1):
p_prev = waypoints[i - 1]
p_curr = waypoints[i]
p_next = waypoints[i + 1]
v_in = p_curr - p_prev
v_out = p_next - p_curr
heading_in = np.atan2(v_in[1], v_in[0])
heading_out = np.atan2(v_out[1], v_out[0])
angles[i] = abs(normalize_angle(heading_out - heading_in))
return angles이 함수의 결과는 각 웨이포인트에서 얼마나 방향을 바꿔야 하는지를 라디안으로 알려준다. 직선에 가까우면 값이 작고, 급격하게 꺾이면 값이 커진다.
turn_angles = waypoint_turn_angles(waypoints)
print(np.degrees(turn_angles))라디안 값이 익숙하지 않다면 np.degrees로 도 단위로 바꿔 보면 된다. 예를 들어 90도 근처라면 거의 직각으로 꺾이는 코너라는 뜻이다.
이 값을 이용해 목표 속도를 정해 보자. 많이 꺾이는 점에서는 천천히, 완만한 점에서는 빠르게 움직이도록 한다.
def waypoint_target_speeds(turn_angles, max_speed=1.2, min_speed=0.35):
"""코너가 급할수록 목표 속도를 낮춘다."""
# turn_angles가 pi에 가까울수록 sharpness는 1에 가까워진다.
sharpness = turn_angles / np.pi
speeds = max_speed - (max_speed - min_speed) * sharpness
return np.clip(speeds, min_speed, max_speed)
waypoint_speeds = waypoint_target_speeds(turn_angles)
print(waypoint_speeds)이 속도는 "반드시 이 속도로 가라"라는 명령이라기보다, 각 목표점 근처에서 참고할 권장 속도다. 나중에 제어 명령을 만들 때 거리, 방향 오차와 함께 사용한다.
로봇 상태와 시뮬레이션 설정
이제 로봇의 초기 상태를 정한다. 이번 예제에서는 첫 번째 웨이포인트에서 출발한다고 가정한다.
state = np.array([
waypoints[0, 0], # x
waypoints[0, 1], # y
0.0 # theta
], dtype=float)로봇의 방향 theta = 0.0은 x축 양의 방향을 바라본다는 뜻이다. 즉, 오른쪽을 보고 출발한다.
시뮬레이션에 필요한 파라미터도 정한다.
dt = 0.05 # 시간 간격 [s]
total_time = 80.0 # 최대 시뮬레이션 시간 [s]
arrival_radius = 0.15 # 목표점 도착 판정 거리 [m]
max_linear_speed = 1.2 # 최대 선속도 [m/s]
max_angular_speed = 2.5 # 최대 각속도 [rad/s]
k_distance = 1.2 # 거리 오차에 대한 선속도 이득
k_angle = 3.0 # 방향 오차에 대한 각속도 이득여기서 이득이라는 말이 처음에는 조금 딱딱하게 들릴 수 있다. 이득은 오차에 얼마나 민감하게 반응할지를 정하는 숫자다. 예를 들어 k_angle이 크면 로봇은 방향 오차가 조금만 생겨도 빠르게 회전하려고 한다. 반대로 너무 작으면 목표 방향으로 천천히 돌아서 답답하게 움직인다.
arrival_radius는 목표점에 "충분히 가까워졌다"고 판단하는 거리다. 실제 로봇에서는 센서 오차와 제어 오차가 있기 때문에 목표점 좌표에 정확히 0.000m 오차로 도착하기 어렵다. 그래서 작은 반경 안에 들어오면 도착으로 인정한다.
거리와 방향 오차로 이동 명령 만들기
로봇 제어의 중심은 현재 상태와 목표점 사이의 차이를 계산하는 것이다. 목표점이 target이고 로봇의 현재 위치가 position이라면, 목표 방향 벡터는 다음과 같다.
error_vec = target - position이 벡터에는 두 가지 정보가 들어 있다. 벡터의 크기는 목표점까지의 거리이고, 벡터의 방향은 로봇이 향해야 할 방향이다.
distance = np.linalg.norm(error_vec)
target_heading = np.atan2(error_vec[1], error_vec[0])그리고 현재 로봇 방향 theta와 목표 방향 target_heading의 차이가 방향 오차다.
heading_error = normalize_angle(target_heading - theta)이제 이 오차를 속도 명령으로 바꾼다. 각속도는 방향 오차에 비례하도록 만든다.
w = k_angle * heading_error
w = np.clip(w, -max_angular_speed, max_angular_speed)np.clip은 값이 너무 커지지 않도록 제한한다. 실제 로봇에도 모터의 한계가 있기 때문에 무한히 빠르게 회전할 수 없다. 시뮬레이션에서도 속도 제한을 두면 움직임이 훨씬 현실적으로 보인다.
선속도는 거리 오차에 비례하도록 만들되, 방향이 크게 틀어져 있을 때는 줄인다. 로봇이 목표를 등지고 있는데 무작정 앞으로 달리면 경로에서 크게 벗어날 수 있기 때문이다.
angle_slowdown = max(0.0, np.cos(heading_error))
v = k_distance * distance * angle_slowdown
v = np.clip(v, 0.0, max_linear_speed)여기서 np.cos(heading_error)를 사용한 점이 재미있다. 방향 오차가 0이면 cos(0) = 1이므로 속도를 그대로 낸다. 방향 오차가 90도 근처면 cos 값이 0에 가까워져서 앞으로 거의 가지 않는다. 목표가 뒤쪽에 있으면 cos 값이 음수가 될 수 있는데, 이 예제에서는 후진하지 않도록 max(0.0, ...)으로 막았다.
이제 이 내용을 함수로 묶자.
def compute_control(state, target, desired_speed,
k_distance=1.2, k_angle=3.0,
max_linear_speed=1.2, max_angular_speed=2.5):
"""현재 상태와 목표점으로부터 선속도 v, 각속도 w를 계산한다."""
x, y, theta = state
position = np.array([x, y])
error_vec = target - position
distance = np.linalg.norm(error_vec)
target_heading = np.atan2(error_vec[1], error_vec[0])
heading_error = normalize_angle(target_heading - theta)
w = k_angle * heading_error
w = np.clip(w, -max_angular_speed, max_angular_speed)
angle_slowdown = max(0.0, np.cos(heading_error))
distance_speed = k_distance * distance
v = min(distance_speed, desired_speed, max_linear_speed)
v = v * angle_slowdown
v = np.clip(v, 0.0, max_linear_speed)
return v, w, distance, heading_error이 함수는 네 가지 값을 반환한다. v와 w는 로봇에게 줄 이동 명령이고, distance와 heading_error는 기록과 분석에 사용할 값이다. 시뮬레이션은 움직이는 것만큼이나 나중에 관찰하는 것이 중요하다.
속도를 적분해 상태 업데이트하기
제어 명령을 계산했다면 이제 로봇의 상태를 업데이트해야 한다. 로봇이 현재 방향 theta를 바라보고 있고 선속도 v로 움직인다면, x 방향 속도는 v * cos(theta), y 방향 속도는 v * sin(theta)다.
def update_state(state, v, w, dt):
"""선속도와 각속도를 이용해 로봇 상태를 한 스텝 업데이트한다."""
x, y, theta = state
x = x + v * np.cos(theta) * dt
y = y + v * np.sin(theta) * dt
theta = theta + w * dt
theta = normalize_angle(theta)
return np.array([x, y, theta])이 함수는 이전 장에서 배운 적분의 코드 버전이다. 연속적인 시간을 완벽하게 계산하는 대신, 아주 작은 시간 간격 dt로 나누어 조금씩 더한다. dt가 충분히 작으면 움직임은 부드럽게 보인다.
물론 dt를 무조건 작게 하는 것이 항상 좋은 것은 아니다. 너무 작으면 계산 횟수가 많아진다. 반대로 너무 크면 로봇이 점프하듯 움직이거나 목표점을 지나칠 수 있다. 시뮬레이션에서는 정확도와 계산량 사이의 균형을 잡아야 한다.
전체 시뮬레이션 루프 만들기
이제 모든 부품을 연결할 차례다. 로봇은 현재 목표점 인덱스 target_index를 가지고 있다. 목표점에 가까워지면 인덱스를 하나 증가시켜 다음 목표점으로 이동한다.
첫 번째 웨이포인트는 출발점이므로, 실제 추종 목표는 두 번째 점부터 시작한다.
target_index = 1시뮬레이션 중에는 로봇의 궤적, 속도, 시간, 목표점 인덱스 등을 기록한다.
trajectory = []
time_history = []
linear_speed_history = []
angular_speed_history = []
distance_history = []
heading_error_history = []
target_index_history = []이제 반복문을 작성한다.
state = np.array([waypoints[0, 0], waypoints[0, 1], 0.0], dtype=float)
target_index = 1
time = 0.0
trajectory = []
time_history = []
linear_speed_history = []
angular_speed_history = []
distance_history = []
heading_error_history = []
target_index_history = []
num_steps = int(total_time / dt)
for step in range(num_steps):
# 모든 목표점을 방문했다면 종료한다.
if target_index >= len(waypoints):
break
target = waypoints[target_index]
desired_speed = waypoint_speeds[target_index]
v, w, distance, heading_error = compute_control(
state,
target,
desired_speed,
k_distance=k_distance,
k_angle=k_angle,
max_linear_speed=max_linear_speed,
max_angular_speed=max_angular_speed
)
# 현재 값을 기록한다.
trajectory.append(state.copy())
time_history.append(time)
linear_speed_history.append(v)
angular_speed_history.append(w)
distance_history.append(distance)
heading_error_history.append(heading_error)
target_index_history.append(target_index)
# 목표점에 도착했으면 다음 목표점으로 바꾼다.
if distance < arrival_radius:
target_index += 1
continue
# 속도를 적분해 상태를 업데이트한다.
state = update_state(state, v, w, dt)
time += dt
trajectory = np.array(trajectory)
time_history = np.array(time_history)
linear_speed_history = np.array(linear_speed_history)
angular_speed_history = np.array(angular_speed_history)
distance_history = np.array(distance_history)
heading_error_history = np.array(heading_error_history)
target_index_history = np.array(target_index_history)
print("visited targets:", target_index, "/", len(waypoints) - 1)
print("final state:", state)
print("simulation time:", time)이 반복문이 바로 로봇 시뮬레이터의 심장이다. 한 줄씩 보면 복잡하지 않다. 목표를 보고, 명령을 만들고, 움직이고, 기록한다. 이 단순한 흐름이 수백 번, 수천 번 반복되면서 하나의 궤적이 만들어진다.
여기서 중요한 점은 위치를 직접 "순간이동"시키지 않는다는 것이다. 로봇은 목표점으로 바로 가지 않는다. 매 순간 속도 명령을 받고, 그 속도가 시간에 따라 누적되면서 이동한다. 이것이 앞에서 배운 미분과 적분의 감각이 시뮬레이션 안으로 들어오는 순간이다.
로봇 궤적 시각화하기
이제 로봇이 실제로 어떤 길을 지나갔는지 그려 보자.
plt.figure(figsize=(7, 7))
# 목표 경로
plt.plot(waypoints[:, 0], waypoints[:, 1], "k--", linewidth=1.5, label="waypoint path")
plt.scatter(waypoints[:, 0], waypoints[:, 1], c="red", s=70, label="waypoints")
# 로봇 실제 궤적
plt.plot(trajectory[:, 0], trajectory[:, 1], "b", linewidth=2.5, label="robot trajectory")
# 웨이포인트 번호 표시
for i, p in enumerate(waypoints):
plt.text(p[0] + 0.06, p[1] + 0.06, str(i), fontsize=11)
plt.axis("equal")
plt.grid(True)
plt.xlabel("x [m]")
plt.ylabel("y [m]")
plt.title("Waypoint Following Simulation")
plt.legend()
plt.show()그림을 보면 로봇의 실제 궤적이 검은 점선과 완전히 일치하지 않을 수 있다. 이것은 오류라기보다 로봇 모델의 특성이다. 로봇은 방향을 가진 물체이고, 현재 바라보는 방향으로 전진한다. 그래서 목표점으로 향하기 전에 먼저 몸을 돌려야 한다.
만약 코너에서 궤적이 너무 크게 부풀어 오른다면 몇 가지 원인을 생각해 볼 수 있다. 선속도가 너무 빠를 수도 있고, 각속도 제한이 너무 낮을 수도 있다. 또는 k_angle이 작아서 방향을 늦게 바꾸는 것일 수도 있다.
이처럼 시뮬레이션은 숫자의 행동을 눈으로 확인하게 해 준다. 파라미터 하나를 바꾸면 그래프의 모양이 달라지고, 그래프의 모양은 다시 수학적 의미를 알려 준다.
로봇의 방향까지 함께 그리기
궤적만 보면 로봇이 어느 방향을 바라보며 움직였는지 알기 어렵다. 일정한 간격으로 로봇의 방향 화살표를 함께 그려 보자.
plt.figure(figsize=(7, 7))
plt.plot(waypoints[:, 0], waypoints[:, 1], "k--", linewidth=1.5, label="waypoint path")
plt.scatter(waypoints[:, 0], waypoints[:, 1], c="red", s=70, label="waypoints")
plt.plot(trajectory[:, 0], trajectory[:, 1], "b", linewidth=2.5, label="robot trajectory")
arrow_step = max(1, len(trajectory) // 25)
arrow_length = 0.25
for i in range(0, len(trajectory), arrow_step):
x, y, theta = trajectory[i]
dx = arrow_length * np.cos(theta)
dy = arrow_length * np.sin(theta)
plt.arrow(x, y, dx, dy,
head_width=0.08,
head_length=0.10,
color="blue",
alpha=0.6,
length_includes_head=True)
plt.axis("equal")
plt.grid(True)
plt.xlabel("x [m]")
plt.ylabel("y [m]")
plt.title("Robot Pose Along Trajectory")
plt.legend()
plt.show()화살표를 보면 로봇이 목표점을 향해 먼저 회전하고, 방향이 맞아 갈수록 앞으로 나아가는 모습을 볼 수 있다. 특히 코너 근처에서는 화살표 방향이 부드럽게 바뀐다. 이것은 theta_next = theta + w * dt라는 아주 단순한 식이 만들어 낸 결과다.
시간별 속도 그래프 보기
로봇의 경로가 괜찮아 보이더라도, 속도 그래프를 확인해야 한다. 실제 로봇에서는 위치만큼이나 속도가 중요하다. 너무 갑작스러운 속도 변화는 모터에 부담을 주고, 바퀴가 미끄러지거나 제어가 불안정해질 수 있다.
먼저 선속도와 각속도를 시간에 따라 그려 보자.
fig, axes = plt.subplots(2, 1, figsize=(9, 6), sharex=True)
axes[0].plot(time_history, linear_speed_history, "b")
axes[0].set_ylabel("linear speed v [m/s]")
axes[0].grid(True)
axes[1].plot(time_history, angular_speed_history, "g")
axes[1].set_ylabel("angular speed w [rad/s]")
axes[1].set_xlabel("time [s]")
axes[1].grid(True)
plt.suptitle("Robot Speed Commands")
plt.show()선속도 그래프가 코너 근처에서 낮아진다면 로봇이 회전을 위해 속도를 줄이고 있다는 뜻이다. 각속도 그래프는 방향 전환이 필요한 순간에 크게 나타난다. 만약 각속도가 계속 최대값에 붙어 있다면, 로봇이 회전 능력의 한계에 자주 걸리고 있다는 뜻이다. 이때는 선속도를 낮추거나 max_angular_speed를 높이는 실험을 해 볼 수 있다.
이번에는 거리 오차와 방향 오차도 함께 보자.
fig, axes = plt.subplots(2, 1, figsize=(9, 6), sharex=True)
axes[0].plot(time_history, distance_history, "r")
axes[0].set_ylabel("distance to target [m]")
axes[0].grid(True)
axes[1].plot(time_history, np.degrees(heading_error_history), "m")
axes[1].set_ylabel("heading error [deg]")
axes[1].set_xlabel("time [s]")
axes[1].grid(True)
plt.suptitle("Tracking Errors")
plt.show()거리 오차는 한 목표점에 가까워질수록 줄어들다가, 다음 목표점으로 바뀌면 다시 커진다. 그래서 톱니 모양처럼 보일 수 있다. 방향 오차는 목표 방향과 로봇의 현재 방향 차이를 나타낸다. 방향 오차가 너무 오래 크게 남아 있다면 로봇이 목표를 늦게 바라보고 있다는 뜻이다.
좋은 시뮬레이션은 예쁜 궤적 하나만 보여 주지 않는다. 위치, 속도, 오차를 함께 보여 주어야 한다. 로봇이 어디로 갔는지뿐 아니라, 어떻게 갔는지를 봐야 하기 때문이다.
목표점 전환 순간 확인하기
로봇이 어떤 목표점을 따라가고 있었는지 시간에 따라 확인하면 디버깅에 도움이 된다. 목표점 전환이 너무 자주 일어나거나, 어떤 목표점에서 오래 머문다면 파라미터를 다시 조정해야 한다.
plt.figure(figsize=(9, 3))
plt.step(time_history, target_index_history, where="post")
plt.xlabel("time [s]")
plt.ylabel("target index")
plt.title("Current Target Index")
plt.grid(True)
plt.show()계단 모양 그래프가 하나씩 올라가면 로봇이 목표점을 순서대로 방문하고 있다는 뜻이다. 만약 어떤 값에서 멈춰 있다면 그 목표점에 도착하지 못했다는 뜻이다. 이때는 arrival_radius가 너무 작거나, 속도와 회전 파라미터가 맞지 않을 수 있다.
전체 코드를 하나로 정리하기
여기까지 만든 코드를 하나의 실행 가능한 형태로 정리해 보자. Jupyter Notebook에서는 셀을 나누어 실행해도 좋고, .py 파일 하나로 저장해도 좋다.
import numpy as np
import matplotlib.pyplot as plt
def normalize_angle(angle):
return (angle + np.pi) % (2 * np.pi) - np.pi
def waypoint_turn_angles(waypoints):
n = len(waypoints)
angles = np.zeros(n)
for i in range(1, n - 1):
p_prev = waypoints[i - 1]
p_curr = waypoints[i]
p_next = waypoints[i + 1]
v_in = p_curr - p_prev
v_out = p_next - p_curr
heading_in = np.atan2(v_in[1], v_in[0])
heading_out = np.atan2(v_out[1], v_out[0])
angles[i] = abs(normalize_angle(heading_out - heading_in))
return angles
def waypoint_target_speeds(turn_angles, max_speed=1.2, min_speed=0.35):
sharpness = turn_angles / np.pi
speeds = max_speed - (max_speed - min_speed) * sharpness
return np.clip(speeds, min_speed, max_speed)
def compute_control(state, target, desired_speed,
k_distance=1.2, k_angle=3.0,
max_linear_speed=1.2, max_angular_speed=2.5):
x, y, theta = state
position = np.array([x, y])
error_vec = target - position
distance = np.linalg.norm(error_vec)
target_heading = np.atan2(error_vec[1], error_vec[0])
heading_error = normalize_angle(target_heading - theta)
w = k_angle * heading_error
w = np.clip(w, -max_angular_speed, max_angular_speed)
angle_slowdown = max(0.0, np.cos(heading_error))
distance_speed = k_distance * distance
v = min(distance_speed, desired_speed, max_linear_speed)
v = v * angle_slowdown
v = np.clip(v, 0.0, max_linear_speed)
return v, w, distance, heading_error
def update_state(state, v, w, dt):
x, y, theta = state
x = x + v * np.cos(theta) * dt
y = y + v * np.sin(theta) * dt
theta = theta + w * dt
theta = normalize_angle(theta)
return np.array([x, y, theta])
# 1. 목표점 배열
waypoints = np.array([
[0.0, 0.0],
[2.0, 1.0],
[4.0, 1.0],
[5.0, 3.0],
[3.0, 4.0],
[1.0, 3.0],
[0.0, 5.0]
])
# 2. 코너 기반 목표 속도
turn_angles = waypoint_turn_angles(waypoints)
waypoint_speeds = waypoint_target_speeds(turn_angles)
# 3. 시뮬레이션 설정
state = np.array([waypoints[0, 0], waypoints[0, 1], 0.0], dtype=float)
target_index = 1
dt = 0.05
total_time = 80.0
arrival_radius = 0.15
max_linear_speed = 1.2
max_angular_speed = 2.5
k_distance = 1.2
k_angle = 3.0
# 4. 기록 배열
trajectory = []
time_history = []
linear_speed_history = []
angular_speed_history = []
distance_history = []
heading_error_history = []
target_index_history = []
# 5. 시간 루프
num_steps = int(total_time / dt)
time = 0.0
for step in range(num_steps):
if target_index >= len(waypoints):
break
target = waypoints[target_index]
desired_speed = waypoint_speeds[target_index]
v, w, distance, heading_error = compute_control(
state,
target,
desired_speed,
k_distance=k_distance,
k_angle=k_angle,
max_linear_speed=max_linear_speed,
max_angular_speed=max_angular_speed
)
trajectory.append(state.copy())
time_history.append(time)
linear_speed_history.append(v)
angular_speed_history.append(w)
distance_history.append(distance)
heading_error_history.append(heading_error)
target_index_history.append(target_index)
if distance < arrival_radius:
target_index += 1
continue
state = update_state(state, v, w, dt)
time += dt
# 6. NumPy 배열로 변환
trajectory = np.array(trajectory)
time_history = np.array(time_history)
linear_speed_history = np.array(linear_speed_history)
angular_speed_history = np.array(angular_speed_history)
distance_history = np.array(distance_history)
heading_error_history = np.array(heading_error_history)
target_index_history = np.array(target_index_history)
print("Final target index:", target_index)
print("Final state:", state)
print("Simulation time:", time)
# 7. 결과 시각화
fig, axes = plt.subplots(2, 2, figsize=(12, 10))
ax = axes[0, 0]
ax.plot(waypoints[:, 0], waypoints[:, 1], "k--", label="waypoint path")
ax.scatter(waypoints[:, 0], waypoints[:, 1], c="red", s=60, label="waypoints")
ax.plot(trajectory[:, 0], trajectory[:, 1], "b", linewidth=2, label="trajectory")
ax.set_aspect("equal", adjustable="box")
ax.set_xlabel("x [m]")
ax.set_ylabel("y [m]")
ax.set_title("Robot Trajectory")
ax.grid(True)
ax.legend()
ax = axes[0, 1]
ax.plot(time_history, linear_speed_history, "b", label="v")
ax.plot(time_history, angular_speed_history, "g", label="w")
ax.set_xlabel("time [s]")
ax.set_title("Speed Commands")
ax.grid(True)
ax.legend()
ax = axes[1, 0]
ax.plot(time_history, distance_history, "r")
ax.set_xlabel("time [s]")
ax.set_ylabel("distance [m]")
ax.set_title("Distance to Current Target")
ax.grid(True)
ax = axes[1, 1]
ax.plot(time_history, np.degrees(heading_error_history), "m")
ax.set_xlabel("time [s]")
ax.set_ylabel("heading error [deg]")
ax.set_title("Heading Error")
ax.grid(True)
plt.tight_layout()
plt.show()이 코드는 이번 장의 핵심 내용을 모두 담고 있다. 웨이포인트 생성, 방향 계산, 속도 명령 생성, 시간 적분, 궤적 기록, 결과 시각화가 하나의 흐름으로 연결되어 있다.
파라미터를 바꾸며 로봇의 성격 관찰하기
시뮬레이션의 재미는 파라미터를 바꾸는 순간 시작된다. 같은 경로라도 파라미터에 따라 로봇의 성격이 완전히 달라진다.
k_angle을 크게 하면 로봇은 목표 방향을 빠르게 바라본다. 하지만 너무 크면 방향이 지나치게 흔들릴 수 있다. k_angle을 작게 하면 움직임은 부드러워 보일 수 있지만, 코너에서 늦게 돌아 크게 벗어날 수 있다.
max_linear_speed를 높이면 전체적으로 빨라진다. 하지만 빠른 로봇은 코너에서 더 많은 회전 능력을 필요로 한다. 선속도만 높이고 각속도 제한은 그대로 두면, 로봇이 목표점을 지나쳐 돌아오는 모습이 나타날 수 있다.
arrival_radius도 중요하다. 이 값이 너무 작으면 로봇이 목표점 주변에서 계속 맴돌 수 있다. 반대로 너무 크면 목표점에 제대로 가까워지기 전에 다음 목표점으로 넘어간다. 실제 로봇에서는 이 값이 센서 정확도와 제어 성능에 맞게 정해진다.
다음과 같은 실험을 해 보면 좋다.
k_angle을1.0,3.0,6.0으로 바꾸고 궤적을 비교한다.max_linear_speed를0.5,1.2,2.0으로 바꾸고 코너에서의 움직임을 본다.arrival_radius를0.05,0.15,0.4로 바꾸고 목표점 전환이 어떻게 달라지는지 확인한다.dt를0.01,0.05,0.2로 바꾸고 시뮬레이션이 얼마나 매끄럽게 보이는지 비교한다.
이 실험들은 단순한 숫자 장난이 아니다. 실제 로봇을 다룰 때도 비슷한 고민을 한다. 빠르게 움직이고 싶지만 너무 빠르면 불안정해진다. 정확히 도착하고 싶지만 너무 엄격하면 목표 근처에서 떨릴 수 있다. 부드럽게 움직이고 싶지만 반응이 느리면 장애물이나 코너에 대응하기 어렵다.
이번 프로젝트에 들어 있는 수학
겉으로 보기에는 Python 코드 몇 줄이지만, 그 안에는 지금까지 배운 수학이 촘촘히 들어 있다.
위치는 벡터로 표현했다. 로봇의 현재 위치 [x, y]와 목표점 [target_x, target_y]는 모두 2차원 벡터다. 목표 방향은 target - position이라는 벡터 뺄셈으로 얻었다.
거리는 벡터의 크기로 계산했다. np.linalg.norm(error_vec)는 목표점까지 얼마나 남았는지 알려 준다. 이 값이 작아지면 로봇은 목표점에 가까워지고 있다는 뜻이다.
방향은 atan2로 계산했다. np.atan2(dy, dx)는 목표 벡터가 어느 각도를 향하는지 알려 준다. 그리고 normalize_angle은 각도 오차가 이상하게 튀지 않도록 정리해 주었다.
미분과 적분도 함께 사용했다. 속도는 위치가 시간에 따라 변하는 비율이고, 위치 업데이트는 속도를 시간 간격 dt만큼 누적한 결과다. 우리가 사용한 업데이트 식은 가장 단순한 수치 적분이다.
제어의 기본 감각도 들어 있다. 거리 오차와 방향 오차를 보고 선속도와 각속도를 만들었다. 오차가 크면 크게 반응하고, 오차가 작으면 천천히 반응한다. 로봇은 목표를 향해 "생각하며" 움직이는 것처럼 보이지만, 실제로는 오차를 계속 줄이는 계산을 반복하고 있을 뿐이다.
시뮬레이션이 실패할 때 읽는 신호
로봇 시뮬레이션은 처음부터 완벽하게 움직이지 않아도 괜찮다. 오히려 실패한 움직임은 좋은 선생님이다. 로봇이 이상하게 움직인다면 그래프를 보고 원인을 추측해 보자.
로봇이 목표점 주변을 빙글빙글 돈다면 선속도가 너무 빠르거나 도착 반경이 너무 작을 수 있다. 목표를 향해 충분히 회전하기 전에 앞으로 나아가면서 목표점을 지나치는 것이다.
로봇이 너무 느리게 움직인다면 max_linear_speed, k_distance, 또는 waypoint_speeds를 확인해야 한다. 거리 오차가 충분한데도 선속도 그래프가 낮게 유지된다면 속도를 제한하는 요소가 무엇인지 찾아보자.
로봇이 방향을 늦게 바꾼다면 k_angle이 작거나 max_angular_speed가 낮을 수 있다. 반대로 방향이 좌우로 심하게 흔들린다면 k_angle이 너무 크거나 dt가 너무 클 수 있다.
이런 분석 습관은 앞으로 센서 노이즈, 위치 추정, 경로 최적화를 배울 때도 계속 중요해진다. 로봇공학에서 좋은 개발자는 단지 코드를 실행하는 사람이 아니라, 그래프를 읽고 로봇의 상태를 상상할 수 있는 사람이다.
더 현실적인 로봇으로 가는 작은 확장
이번 장의 로봇은 아직 단순하다. 바퀴의 미끄러짐도 없고, 센서 오차도 없고, 장애물도 없다. 하지만 구조는 꽤 튼튼하다. 여기에 조금씩 현실적인 요소를 더하면 더 흥미로운 시뮬레이터로 발전시킬 수 있다.
예를 들어 로봇 위치에 작은 노이즈를 추가하면 센서 오차가 있는 상황을 만들 수 있다. 속도 명령에 제한뿐 아니라 가속도 제한을 추가하면 갑자기 속도가 바뀌지 않는 더 현실적인 로봇이 된다. 장애물을 추가하고, 장애물에 가까워질수록 비용을 크게 만들면 경로 최적화 문제로 이어진다.
앞으로 우리는 센서가 왜 틀리는지, 틀린 센서값을 어떻게 확률로 다루는지 배운다. 그때 이번 장의 시뮬레이터는 좋은 실험장이 된다. 지금은 로봇의 실제 위치를 그대로 알고 있다고 가정했지만, 실제 로봇은 자기 위치를 완벽하게 알지 못한다. 바퀴가 조금 미끄러지고, 센서에는 노이즈가 섞이고, 시간이 지나면 오차가 쌓인다.
이제 질문은 바뀐다.
"로봇은 어디에 있는가?"
에서
"로봇은 자신이 어디에 있다고 믿는가?"
로 넘어간다.
그 질문을 이해하려면 확률이 필요하다. 다음 부에서는 로봇의 센서가 왜 항상 조금씩 틀리는지, 그리고 그 불확실성을 어떻게 다루는지 배운다.
정리
이번 장에서는 좌표, 벡터, 각도, 속도, 적분을 하나로 묶어 여러 목표점을 따라 움직이는 2D 로봇 시뮬레이터를 만들었다. 로봇은 현재 위치와 목표점 사이의 벡터를 계산하고, 거리와 방향 오차를 이용해 선속도와 각속도를 정했다. 그런 다음 시간 간격 dt마다 속도를 누적해 위치와 방향을 업데이트했다.
중요한 것은 코드보다 흐름이다. 로봇은 매 순간 목표를 다시 보고, 오차를 다시 계산하고, 조금 움직인다. 이 단순한 반복이 궤적을 만들고, 궤적은 로봇의 행동이 된다.
이제 우리는 움직이는 로봇의 기본 뼈대를 갖게 되었다. 다음 단계는 이 로봇의 눈과 귀를 의심하는 것이다. 센서는 언제나 조금씩 틀리고, 그 틀림은 로봇의 판단을 흔든다. 그래서 다음 장부터는 노이즈, 편향, 드리프트, 불확실성이라는 새로운 세계로 들어간다.
이 노트는 보도·비평·교육·연구 목적으로 작성되었습니다. 저작권 문의가 있으시면 에서 알려주세요.