로봇에게 목표 지점을 알려 주었다고 해 봅시다. 사람이라면 지도를 보고 대충 말할 수 있습니다. "저쪽으로 조금만 가면 돼." 하지만 로봇은 저쪽이라는 말을 이해하지 못합니다. 로봇에게 필요한 것은 숫자입니다.
목표가 얼마나 떨어져 있는지, 어느 방향에 있는지, 지금 로봇이 바라보는 방향에서 얼마나 돌아야 하는지. 이 세 가지를 숫자로 계산할 수 있으면 로봇은 드디어 목표를 향해 움직이기 시작합니다.
이번 장에서는 두 점 사이의 거리와 방향각을 계산합니다. 여기서 배우는 내용은 단순히 목표점으로 이동하는 데만 쓰이지 않습니다. 장애물을 피할 때도, 센서로 본 물체의 위치를 해석할 때도, 나중에 위치 추정과 SLAM을 배울 때도 계속 등장하는 기본 감각입니다.
로봇이 알고 싶은 첫 번째 질문: 얼마나 멀리 있나
2D 평면 위에 로봇과 목표가 있다고 합시다.
robot = [2.0, 1.0]
goal = [8.0, 5.0]로봇의 위치는 ((2, 1)), 목표의 위치는 ((8, 5))입니다. 로봇이 가장 먼저 알고 싶은 것은 목표까지의 거리입니다.
눈으로 보면 목표는 오른쪽 위에 있습니다. 하지만 로봇에게는 오른쪽 위라는 말보다 좌표 차이가 더 중요합니다. 목표와 로봇의 좌표를 빼면 다음과 같습니다.
dx = goal[0] - robot[0] # 8.0 - 2.0
dy = goal[1] - robot[1] # 5.0 - 1.0
print(dx, dy)결과는 다음과 같습니다.
6.0 4.0이 말은 목표가 로봇보다 x축 방향으로 6만큼, y축 방향으로 4만큼 떨어져 있다는 뜻입니다. 여기서 중요한 점은 거리 계산이 갑자기 튀어나온 공식이 아니라는 것입니다. 거리 계산은 결국 좌표 차이에서 시작합니다.
x 방향 차이와 y 방향 차이를 알면 두 점은 직각삼각형의 두 변처럼 볼 수 있습니다. 로봇에서 목표까지의 직선거리는 그 직각삼각형의 빗변입니다. 그래서 피타고라스 정리를 사용합니다.
[ 거리 = sqrt{dx^2 + dy^2} ]
Python으로 계산하면 다음과 같습니다.
import numpy as np
robot = np.array([2.0, 1.0])
goal = np.array([8.0, 5.0])
diff = goal - robot
distance = np.sqrt(diff[0]2 + diff[1]2)
print(diff)
print(distance)출력은 다음과 비슷합니다.
[6. 4.]
7.211102550927978목표는 로봇으로부터 약 7.21만큼 떨어져 있습니다. 여기서 단위는 상황에 따라 달라집니다. 시뮬레이션에서 한 칸을 1m로 정했다면 7.21m이고, 한 칸을 10cm로 정했다면 72.1cm입니다. 수학은 단위를 정해 주지 않습니다. 단위를 정하는 것은 우리가 만든 세계의 규칙입니다.
NumPy에는 벡터의 길이를 계산하는 함수도 있습니다.
distance = np.linalg.norm(diff)
print(distance)np.linalg.norm()은 벡터의 크기를 계산합니다. 7장에서 배운 벡터의 크기와 같은 개념입니다. 즉, 두 점 사이의 거리는 목표 위치에서 로봇 위치를 뺀 벡터의 크기입니다.
distance = np.linalg.norm(goal - robot)이 한 줄은 앞으로 매우 자주 보게 됩니다. 로봇이 목표에 도착했는지 확인할 때도, 장애물이 너무 가까운지 판단할 때도, 센서로 본 물체가 얼마나 떨어져 있는지 계산할 때도 이 구조가 반복됩니다.
거리 공식은 로봇의 도착 판단이 된다
거리 계산은 단순한 측정에서 끝나지 않습니다. 로봇에게 거리는 행동을 바꾸는 기준이 됩니다.
예를 들어 목표까지 거리가 충분히 작으면 로봇은 멈춰야 합니다. 정확히 목표 좌표와 일치할 때까지 계속 움직이게 만들면 문제가 생길 수 있습니다. 컴퓨터 계산에는 작은 오차가 있고, 로봇의 이동도 연속적이기 때문입니다. 그래서 보통은 목표 주변에 작은 원을 하나 그려 두고, 그 안에 들어오면 도착했다고 판단합니다.
threshold = 0.2
if distance < threshold:
print('목표에 도착했습니다.')
else:
print('아직 이동해야 합니다.')여기서 threshold는 허용 오차입니다. 목표점에 정확히 바늘처럼 꽂히지 않아도, 반지름 0.2 안에 들어오면 성공으로 보는 것입니다.
이 감각은 실제 로봇에서도 중요합니다. 바퀴는 미끄러질 수 있고, 센서는 조금씩 틀릴 수 있으며, 모터는 명령한 만큼 완벽하게 움직이지 않을 수 있습니다. 그래서 로봇 제어에서는 '완벽히 같은 위치'보다 '충분히 가까운 위치'가 더 현실적인 목표가 됩니다.
로봇이 알고 싶은 두 번째 질문: 어느 방향에 있나
거리만 알면 로봇은 답답합니다. 목표까지 7.21만큼 떨어져 있다는 사실은 알았지만, 어느 쪽으로 가야 하는지는 아직 모릅니다.
방향을 알려면 다시 좌표 차이를 봐야 합니다.
direction_vector = goal - robot
print(direction_vector)[6. 4.]이 벡터는 로봇에서 목표를 향하는 화살표입니다. 시작점은 로봇이고, 끝점은 목표입니다. 이 벡터에는 두 가지 정보가 함께 들어 있습니다. 하나는 방향이고, 다른 하나는 거리입니다.
목표를 향해 일정한 속도로 움직이고 싶다면 방향만 남기고 크기는 1로 만드는 것이 좋습니다. 이것이 단위 벡터입니다.
direction = goal - robot
distance = np.linalg.norm(direction)
unit_direction = direction / distance
print(unit_direction)
print(np.linalg.norm(unit_direction))결과는 다음과 비슷합니다.
[0.83205029 0.5547002 ]
1.0unit_direction은 목표를 향하는 순수한 방향입니다. 크기가 1이므로 여기에 원하는 속도를 곱하면 속도 벡터가 됩니다.
speed = 0.5
velocity = speed * unit_direction
print(velocity)이 구조는 이전 장들에서 만든 점 로봇 시뮬레이터와 자연스럽게 연결됩니다.
dt = 0.1
robot = robot + velocity * dt거리, 방향 벡터, 단위 벡터, 속도 벡터가 하나의 흐름으로 이어집니다.
목표 위치 - 현재 위치 → 목표 방향 벡터 → 단위 벡터 → 속도 벡터 → 위치 업데이트로봇의 이동은 거창해 보이지만, 가장 기본은 이 한 줄의 생각입니다. 목표에서 나를 빼면 내가 가야 할 방향이 나온다.
그런데 로봇은 몸의 방향도 가지고 있다
점 하나가 미끄러지듯 움직이는 시뮬레이션에서는 목표 방향 벡터만 있으면 충분했습니다. 하지만 실제 로봇은 보통 몸의 방향을 가지고 있습니다. 자동차처럼 앞쪽이 있고, 바퀴 달린 모바일 로봇도 앞을 향해 움직이는 경우가 많습니다.
로봇이 현재 오른쪽을 바라보고 있는데 목표가 위쪽에 있다면, 로봇은 먼저 몸을 돌려야 합니다. 이때 필요한 것이 방향각입니다.
방향각은 x축을 기준으로 어느 정도 회전한 방향인지를 나타내는 각도입니다. 2D 평면에서는 보통 반시계 방향을 양수로 둡니다.
예를 들어 다음과 같이 생각할 수 있습니다.
x축 양의 방향, 즉 오른쪽은 0도입니다.
y축 양의 방향, 즉 위쪽은 90도입니다.
x축 음의 방향, 즉 왼쪽은 180도 또는 -180도입니다.
y축 음의 방향, 즉 아래쪽은 -90도입니다.
Python의 삼각함수는 보통 도가 아니라 라디안을 사용합니다. 라디안은 각도를 나타내는 또 다른 단위입니다. 180도는 (pi) 라디안이고, 90도는 (frac{pi}{2}) 라디안입니다.
NumPy에서는 다음처럼 변환할 수 있습니다.
angle_deg = 90
angle_rad = np.deg2rad(angle_deg)
print(angle_rad)
print(np.rad2deg(angle_rad))1.5707963267948966
90.0처음에는 라디안이 낯설 수 있습니다. 하지만 Python의 np.sin(), np.cos(), np.arctan2() 같은 함수는 라디안을 기본으로 사용하므로, 로봇 시뮬레이션에서는 라디안에 익숙해지는 것이 좋습니다.
atan2: 방향각을 구하는 안전한 방법
목표 방향 벡터가 [6, 4]라면 이 벡터의 각도는 어떻게 구할까요?
직각삼각형을 떠올리면 기울기처럼 dy / dx를 생각할 수 있습니다. 그리고 아크탄젠트, 즉 탄젠트의 반대 계산을 사용하면 각도를 구할 수 있습니다.
하지만 로봇에서는 단순히 arctan(dy / dx)만 쓰면 위험합니다. 이유는 사분면 때문입니다.
예를 들어 [1, 1]과 [-1, -1]을 봅시다. 두 벡터는 완전히 반대 방향입니다. 하지만 dy / dx는 둘 다 1입니다.
[ 1, 1] → dy / dx = 1 / 1 = 1
[-1, -1] → dy / dx = -1 / -1 = 1비율만 보면 두 방향이 같아 보입니다. 하지만 실제로는 하나는 오른쪽 위, 다른 하나는 왼쪽 아래입니다. 로봇이 이 차이를 구분하지 못하면 목표가 뒤쪽에 있는데도 앞쪽에 있다고 착각할 수 있습니다.
그래서 우리는 atan2를 사용합니다.
angle = np.arctan2(dy, dx)atan2는 y 차이와 x 차이를 따로 받습니다. 그래서 벡터가 어느 사분면에 있는지까지 고려해 올바른 방향각을 계산합니다.
vectors = [
np.array([1.0, 1.0]),
np.array([-1.0, 1.0]),
np.array([-1.0, -1.0]),
np.array([1.0, -1.0]),
]
for v in vectors:
dx, dy = v
angle = np.arctan2(dy, dx)
print(v, angle, np.rad2deg(angle))출력은 다음과 비슷합니다.
[1. 1.] 0.7853981633974483 45.0
[-1. 1.] 2.356194490192345 135.0
[-1. -1.] -2.356194490192345 -135.0
[ 1. -1.] -0.7853981633974483 -45.0네 방향 모두 dy / dx의 값만으로는 헷갈릴 수 있지만, atan2는 정확히 구분합니다. 로봇이 목표를 향해 회전할 때는 거의 항상 atan2를 사용한다고 생각해도 좋습니다.
두 점 사이의 방향각 계산하기
이제 로봇 위치와 목표 위치를 넣으면 목표 방향각을 계산하는 함수를 만들어 봅시다.
import numpy as np
def angle_to_target(position, target):
diff = target - position
dx = diff[0]
dy = diff[1]
return np.arctan2(dy, dx)
robot = np.array([2.0, 1.0])
goal = np.array([8.0, 5.0])
target_angle = angle_to_target(robot, goal)
print(target_angle)
print(np.rad2deg(target_angle))출력은 다음과 비슷합니다.
0.5880026035475675
33.690067525979785즉, 목표는 로봇 기준으로 월드 x축에서 약 33.69도 기울어진 방향에 있습니다.
여기서 주의해야 할 점이 있습니다. 이 각도는 월드 좌표계에서 본 목표 방향각입니다. 로봇이 현재 어디를 바라보고 있는지는 아직 고려하지 않았습니다. 로봇이 이미 33.69도를 바라보고 있다면 그대로 전진하면 됩니다. 하지만 로봇이 0도를 바라보고 있다면 약 33.69도 회전해야 합니다. 로봇이 90도를 바라보고 있다면 오히려 시계 방향으로 돌아야 합니다.
그래서 다음 질문이 필요합니다.
목표 방향은 알았다. 그런데 현재 내 몸 방향과 얼마나 차이가 나지?현재 방향과 목표 방향의 차이
로봇의 현재 방향을 theta라고 하겠습니다. theta도 라디안으로 표현합니다.
theta = np.deg2rad(0.0)목표 방향각이 target_angle이라면, 단순한 각도 차이는 다음과 같습니다.
angle_error = target_angle - theta이 값이 양수이면 반시계 방향으로 돌면 되고, 음수이면 시계 방향으로 돌면 됩니다.
print(np.rad2deg(angle_error))현재 방향이 0도이고 목표 방향이 약 33.69도라면, 각도 오차는 약 33.69도입니다. 로봇은 왼쪽으로 조금 회전하면 됩니다.
그런데 각도에는 재미있는 함정이 있습니다. 179도와 -179도를 생각해 봅시다. 숫자로만 보면 차이는 -358도처럼 보일 수 있습니다. 하지만 실제 평면에서는 179도와 -179도는 거의 같은 왼쪽 방향입니다. 둘 사이의 가장 짧은 회전은 358도가 아니라 2도입니다.
로봇에게 "358도 돌아!"라고 명령하면 로봇은 거의 한 바퀴를 빙글 돌게 됩니다. 사실은 살짝만 돌면 되는데 말입니다. 그래서 각도 차이를 구할 때는 항상 각도를 일정한 범위로 정리해야 합니다.
보통 로봇 제어에서는 각도 오차를 (-pi)부터 (pi) 사이로 맞춥니다. 도 단위로 말하면 -180도부터 180도 사이입니다. 이렇게 하면 로봇은 언제나 가장 짧은 방향으로 회전할 수 있습니다.
각도 정규화하기
각도를 (-pi)부터 (pi) 사이로 정리하는 함수를 만들어 봅시다.
def normalize_angle(angle):
return (angle + np.pi) % (2 * np.pi) - np.pi처음 보면 조금 이상해 보일 수 있습니다. 핵심은 원형으로 이어지는 각도를 한 바퀴 범위 안에 다시 넣는 것입니다. %는 나머지 연산입니다. 각도가 너무 크거나 너무 작아도 2π 단위로 감아서 원하는 범위에 넣습니다.
간단히 확인해 보겠습니다.
test_angles_deg = [0, 90, 180, 181, 270, 360, 540, -181, -270]
for deg in test_angles_deg:
rad = np.deg2rad(deg)
normalized = normalize_angle(rad)
print(deg, '→', np.rad2deg(normalized))출력은 다음과 비슷합니다.
0 → 0.0
90 → 90.0
180 → -180.0
181 → -178.99999999999997
270 → -90.0
360 → 0.0
540 → -180.0
-181 → 178.99999999999997
-270 → 90.0181도는 -179도로 바뀝니다. 이 말은 반시계 방향으로 181도 도는 것보다 시계 방향으로 179도 도는 편이 더 짧다는 뜻입니다. 270도는 -90도가 됩니다. 한 바퀴를 거의 다 도는 대신 반대 방향으로 90도만 돌면 됩니다.
이제 각도 오차를 안전하게 계산할 수 있습니다.
angle_error = normalize_angle(target_angle - theta)로봇의 회전 제어에서는 이 한 줄이 매우 중요합니다. 각도를 정규화하지 않으면 로봇이 목표 앞에서 괜히 빙글빙글 도는 이상한 행동을 할 수 있습니다.
회전 명령으로 바꾸기
각도 오차를 구했으면 이제 회전 명령을 만들 수 있습니다. 가장 단순한 방법은 각도 오차에 비례해서 회전 속도를 주는 것입니다.
angular_speed = k * angle_error여기서 angular_speed는 로봇의 회전 속도입니다. 단위는 보통 rad/s입니다. k는 얼마나 적극적으로 돌 것인지를 정하는 값입니다. 이런 식으로 오차에 비례해 명령을 주는 방식을 비례 제어라고 부릅니다. 어렵게 들리지만 생각은 간단합니다.
오차가 크면 많이 돌고, 오차가 작으면 조금 돈다.
코드로 만들어 봅시다.
def compute_angular_speed(theta, target_angle, gain=2.0):
angle_error = normalize_angle(target_angle - theta)
return gain * angle_error테스트해 보겠습니다.
robot = np.array([2.0, 1.0])
goal = np.array([8.0, 5.0])
target_angle = angle_to_target(robot, goal)
for theta_deg in [0, 30, 90, 180, -170]:
theta = np.deg2rad(theta_deg)
angle_error = normalize_angle(target_angle - theta)
angular_speed = compute_angular_speed(theta, target_angle, gain=2.0)
print('현재 방향:', theta_deg)
print('목표 방향:', np.rad2deg(target_angle))
print('각도 오차:', np.rad2deg(angle_error))
print('회전 속도:', angular_speed)
print()현재 방향이 목표 방향보다 작으면 양의 회전 속도가 나옵니다. 반대로 현재 방향이 목표 방향보다 크면 음의 회전 속도가 나옵니다. 이 부호가 곧 회전 방향입니다.
양수 회전 속도는 반시계 방향 회전입니다.
음수 회전 속도는 시계 방향 회전입니다.
0에 가까운 회전 속도는 거의 목표 방향을 바라보고 있다는 뜻입니다.
여기서 로봇 제어의 첫 번째 감각이 생깁니다. 로봇은 목표 방향을 한 번에 맞히려고 하지 않습니다. 매 순간 조금씩 오차를 줄입니다. 카메라가 영상을 계속 찍듯이, 로봇도 매 시간 간격마다 현재 위치와 방향을 보고 다시 계산합니다.
회전 속도 제한하기
비례 제어는 단순하고 강력하지만, 오차가 아주 크면 회전 속도도 지나치게 커질 수 있습니다. 실제 로봇의 모터는 무한히 빠르게 돌 수 없습니다. 시뮬레이션에서도 너무 큰 회전 속도는 움직임을 불안정하게 만들 수 있습니다.
그래서 회전 속도에 제한을 걸어 줍니다.
def clip(value, min_value, max_value):
return np.clip(value, min_value, max_value)NumPy의 np.clip()은 값이 너무 작으면 최소값으로, 너무 크면 최대값으로 잘라 줍니다.
def compute_angular_speed(theta, target_angle, gain=2.0, max_speed=1.5):
angle_error = normalize_angle(target_angle - theta)
angular_speed = gain * angle_error
angular_speed = np.clip(angular_speed, -max_speed, max_speed)
return angular_speed이제 회전 속도는 항상 -1.5와 1.5 사이에 머뭅니다. 실제 로봇에서는 이런 제한이 매우 중요합니다. 아무리 목표가 급해도 로봇이 낼 수 있는 속도에는 한계가 있기 때문입니다.
방향을 먼저 맞추고 앞으로 가기
목표를 향해 이동하는 방법에는 여러 가지가 있습니다. 가장 단순한 점 로봇은 목표 방향으로 바로 미끄러지듯 움직일 수 있습니다. 하지만 앞뒤 방향이 있는 로봇은 보통 다음 순서로 움직입니다.
먼저 목표 방향을 바라본다. 그다음 앞으로 간다.
이를 코드로 표현하려면 현재 방향 theta를 사용해 전진 속도를 계산해야 합니다. 로봇이 바라보는 방향으로 전진할 때 속도 벡터는 다음과 같습니다.
[ v_x = v cos(theta) ]
[ v_y = v sin(theta) ]
Python 코드로는 다음과 같습니다.
linear_speed = 1.0
vx = linear_speed * np.cos(theta)
vy = linear_speed * np.sin(theta)하지만 목표를 전혀 바라보지 않은 상태에서 무작정 전진하면 목표에서 멀어질 수도 있습니다. 그래서 각도 오차가 클 때는 천천히 가거나 아예 회전만 하도록 만들 수 있습니다.
def compute_linear_speed(distance, angle_error, max_speed=1.0):
if abs(angle_error) > np.deg2rad(30):
return 0.0
return min(max_speed, distance)이 함수는 목표 방향과 30도 이상 어긋나 있으면 전진하지 않습니다. 로봇이 어느 정도 목표를 바라보면 앞으로 움직입니다. min(max_speed, distance)를 사용한 이유는 목표에 가까워질수록 속도를 줄이기 위해서입니다. 목표가 바로 앞에 있는데도 빠르게 달리면 목표를 지나칠 수 있습니다.
이제 거리와 각도 오차를 함께 사용해 이동 명령을 만들 수 있습니다.
def compute_control(position, theta, target):
diff = target - position
distance = np.linalg.norm(diff)
target_angle = np.arctan2(diff[1], diff[0])
angle_error = normalize_angle(target_angle - theta)
angular_speed = np.clip(2.0 * angle_error, -1.5, 1.5)
linear_speed = compute_linear_speed(distance, angle_error, max_speed=1.0)
return linear_speed, angular_speed, distance, angle_error이 함수는 로봇 제어의 아주 작은 뇌처럼 동작합니다. 현재 위치와 방향, 목표 위치를 넣으면 전진 속도와 회전 속도를 내보냅니다.
작은 시뮬레이션으로 확인하기
이제 실제로 로봇이 목표를 향해 회전하고 이동하는 모습을 시뮬레이션해 봅시다. 아직 완벽한 로봇은 아니지만, 거리와 각도 계산이 어떻게 행동으로 이어지는지 확인하기에는 충분합니다.
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, max_speed=1.0):
if abs(angle_error) > np.deg2rad(30):
return 0.0
return min(max_speed, distance)
def compute_control(position, theta, target):
diff = target - position
distance = np.linalg.norm(diff)
target_angle = np.arctan2(diff[1], diff[0])
angle_error = normalize_angle(target_angle - theta)
angular_speed = np.clip(2.0 * angle_error, -1.5, 1.5)
linear_speed = compute_linear_speed(distance, angle_error, max_speed=1.0)
return linear_speed, angular_speed, distance, angle_error
position = np.array([0.0, 0.0])
theta = np.deg2rad(120.0)
target = np.array([6.0, 4.0])
dt = 0.1
trajectory = [position.copy()]
for step in range(300):
linear_speed, angular_speed, distance, angle_error = compute_control(
position, theta, target
)
if distance < 0.1:
print('도착:', step)
break
theta = theta + angular_speed * dt
theta = normalize_angle(theta)
velocity = np.array([
linear_speed * np.cos(theta),
linear_speed * np.sin(theta),
])
position = position + velocity * dt
trajectory.append(position.copy())
trajectory = np.array(trajectory)
plt.figure(figsize=(6, 6))
plt.plot(trajectory[:, 0], trajectory[:, 1], 'b-', label='trajectory')
plt.scatter(target[0], target[1], c='red', s=100, label='target')
plt.scatter(trajectory[0, 0], trajectory[0, 1], c='green', s=100, label='start')
plt.axis('equal')
plt.grid(True)
plt.legend()
plt.show()이 코드를 실행하면 로봇이 처음에는 목표를 향해 몸을 돌리고, 방향이 어느 정도 맞으면 앞으로 이동하는 궤적을 볼 수 있습니다. 시작 방향을 120도로 두었기 때문에 로봇은 처음부터 목표를 바라보고 있지 않습니다. 그래서 바로 직선으로 가지 않고, 먼저 방향을 정렬한 뒤 목표로 다가갑니다.
여기서 우리는 중요한 사실을 확인합니다. 로봇의 움직임은 매 순간 다음 계산의 반복입니다.
현재 위치 확인 → 목표까지 거리 계산 → 목표 방향각 계산 → 현재 방향과 비교 → 회전 및 전진 명령 생성 → 위치 업데이트이 반복이 충분히 빠르게 일어나면 로봇은 마치 스스로 목표를 알고 움직이는 것처럼 보입니다.
그림으로 각도 오차 보기
숫자만 보면 각도 오차가 잘 와닿지 않을 수 있습니다. 이번에는 로봇의 현재 방향과 목표 방향을 함께 그려 보겠습니다.
import numpy as np
import matplotlib.pyplot as plt
position = np.array([2.0, 1.0])
theta = np.deg2rad(80.0)
target = np.array([8.0, 5.0])
diff = target - position
target_angle = np.arctan2(diff[1], diff[0])
angle_error = normalize_angle(target_angle - theta)
heading = np.array([np.cos(theta), np.sin(theta)])
target_dir = diff / np.linalg.norm(diff)
plt.figure(figsize=(6, 6))
plt.scatter(position[0], position[1], c='blue', s=100, label='robot')
plt.scatter(target[0], target[1], c='red', s=100, label='target')
plt.arrow(
position[0], position[1],
heading[0], heading[1],
head_width=0.2,
length_includes_head=True,
color='blue',
label='robot heading'
)
plt.arrow(
position[0], position[1],
target_dir[0], target_dir[1],
head_width=0.2,
length_includes_head=True,
color='red',
label='target direction'
)
plt.plot([position[0], target[0]], [position[1], target[1]], 'k--', alpha=0.4)
plt.text(position[0] + 0.2, position[1] - 0.4,
f'error = {np.rad2deg(angle_error):.1f} deg')
plt.axis('equal')
plt.grid(True)
plt.legend()
plt.show()파란 화살표는 로봇이 현재 바라보는 방향입니다. 빨간 화살표는 목표를 향하는 방향입니다. 두 화살표 사이의 차이가 바로 각도 오차입니다. 로봇 제어는 이 빨간 화살표와 파란 화살표를 겹치게 만드는 과정이라고 볼 수 있습니다.
atan2를 사용할 때 자주 하는 실수
atan2는 매우 유용하지만, 처음 사용할 때 실수가 자주 나옵니다. 특히 인자의 순서를 조심해야 합니다.
NumPy의 np.arctan2()는 다음 순서를 사용합니다.
np.arctan2(y, x)즉, y가 먼저이고 x가 나중입니다. 우리가 좌표를 말할 때는 보통 x, y 순서로 말하기 때문에 헷갈리기 쉽습니다.
angle = np.arctan2(dy, dx) # 올바른 사용다음처럼 쓰면 각도가 완전히 달라질 수 있습니다.
angle = np.arctan2(dx, dy) # 실수하기 쉬운 코드또 하나의 실수는 목표와 현재 위치를 반대로 빼는 것입니다.
diff = target - position # 로봇에서 목표로 향하는 벡터이것을 반대로 쓰면 목표에서 로봇으로 향하는 벡터가 됩니다.
diff = position - target # 반대 방향 벡터두 벡터는 180도 차이가 납니다. 로봇이 목표를 향해 가야 하는데 목표에서 도망가는 일이 생길 수 있습니다.
마지막으로 도와 라디안을 섞는 실수도 많습니다. np.sin(), np.cos(), np.arctan2()는 라디안을 사용합니다. 사람이 읽기 좋게 출력할 때만 np.rad2deg()로 도 단위로 바꾸면 됩니다.
거리와 각도 계산을 함수로 정리하기
앞으로 여러 장에서 반복해서 쓸 수 있도록 함수들을 정리해 보겠습니다.
import numpy as np
def distance_between(a, b):
return np.linalg.norm(b - a)
def direction_vector(a, b):
return b - a
def unit_direction(a, b):
diff = direction_vector(a, b)
distance = np.linalg.norm(diff)
if distance == 0:
return np.array([0.0, 0.0])
return diff / distance
def angle_between_points(a, b):
diff = b - a
return np.arctan2(diff[1], diff[0])
def normalize_angle(angle):
return (angle + np.pi) % (2 * np.pi) - np.pi
def angle_error(current_angle, target_angle):
return normalize_angle(target_angle - current_angle)사용 예시는 다음과 같습니다.
robot = np.array([2.0, 1.0])
goal = np.array([8.0, 5.0])
theta = np.deg2rad(20.0)
d = distance_between(robot, goal)
dir_vec = direction_vector(robot, goal)
unit_dir = unit_direction(robot, goal)
target_angle = angle_between_points(robot, goal)
err = angle_error(theta, target_angle)
print('거리:', d)
print('방향 벡터:', dir_vec)
print('단위 방향:', unit_dir)
print('목표 방향각:', np.rad2deg(target_angle))
print('각도 오차:', np.rad2deg(err))이 함수들은 작지만 강력합니다. 이 책의 뒤쪽에서 목표 추종, 장애물 회피, 경로 최적화, 위치 추정으로 넘어가도 기본 구조는 크게 달라지지 않습니다. 로봇이 세상을 숫자로 이해하려면 결국 위치 차이를 계산하고, 거리와 방향을 해석해야 하기 때문입니다.
거리와 각도는 로봇 판단의 언어다
이번 장에서 우리는 두 점 사이의 거리와 방향각을 계산했습니다. 처음에는 단순히 피타고라스 정리와 atan2를 사용한 것처럼 보였지만, 사실 로봇의 행동을 만드는 핵심 부품들을 만든 것입니다.
거리 계산은 목표에 가까워졌는지 판단하게 해 줍니다. 방향 벡터는 어디로 움직여야 하는지 알려 줍니다. atan2는 목표가 어느 사분면에 있든 정확한 방향각을 구해 줍니다. 각도 오차는 로봇이 어느 쪽으로 얼마나 회전해야 하는지 알려 줍니다.
로봇은 사람처럼 '저쪽', '조금', '거의 다 왔다'라고 느끼지 못합니다. 대신 로봇은 다음과 같은 숫자를 봅니다.
거리: 7.21
목표 방향각: 33.69도
현재 방향: 20.00도
각도 오차: 13.69도이 숫자가 쌓이면 행동이 됩니다. 행동이 반복되면 이동이 됩니다. 이동이 기록되면 궤적이 됩니다. 그리고 궤적을 이해하기 시작하면 우리는 로봇을 단순한 점이 아니라, 세상 속에서 판단하며 움직이는 존재로 다룰 수 있게 됩니다.
다음 장에서는 이번 장에서 만든 거리와 각도 계산을 바탕으로 목표 추종 로봇을 만들 것입니다. 이제 로봇은 목표가 어디 있는지 계산할 수 있습니다. 다음 단계는 그 계산을 매 순간 반복해, 목표에 도착할 때까지 스스로 움직이게 만드는 것입니다.
이 노트는 보도·비평·교육·연구 목적으로 작성되었습니다. 저작권 문의가 있으시면 에서 알려주세요.