로봇이 고개를 돌린다는 것
로봇이 평면 위에서 움직인다고 상상해 보자. 로봇의 위치는 (x, y)로 나타낼 수 있다. 그런데 위치만으로는 부족하다. 같은 위치에 있어도 로봇이 오른쪽을 바라볼 수도 있고, 위쪽을 바라볼 수도 있고, 목표물을 등지고 있을 수도 있다.
그래서 2D 로봇의 상태를 보통 다음처럼 표현한다.
robot_pose = [x, y, theta]여기서 theta는 로봇의 방향 각도다. 위치가 "로봇이 어디에 있는가"를 말한다면, 각도는 "로봇이 어디를 바라보는가"를 말한다.
앞 장에서는 행렬이 좌표를 바꾸는 도구라는 감각을 익혔다. 이번에는 그중에서도 로봇공학에서 가장 자주 등장하는 변환인 회전을 집중해서 다룬다. 회전은 단순히 그림을 빙글 돌리는 기능이 아니다. 로봇의 몸체 방향, 센서 방향, 바퀴가 밀어내는 전진 방향, 카메라가 바라보는 방향을 모두 설명하는 핵심 연산이다.
로봇이 방향을 바꾼다는 말은 두 가지 관점으로 볼 수 있다.
점이나 물체 자체를 회전시킨다.
좌표축, 즉 바라보는 기준을 회전시킨다.
로봇 기준 좌표를 월드 기준 좌표로 바꾼다.
이 세 가지는 겉으로는 다르게 보이지만, 결국 같은 회전 행렬에서 출발한다.
도와 라디안
우리는 일상에서 각도를 말할 때 보통 도(degree)를 사용한다. 직각은 90도, 한 바퀴는 360도다. 그래서 "로봇을 90도 돌린다"라는 표현은 아주 자연스럽다.
하지만 Python의 np.sin, np.cos 같은 삼각함수는 도가 아니라 라디안(radian)을 사용한다.
라디안은 원의 반지름과 호의 길이로 각도를 표현하는 방법이다. 반지름이 1인 원에서 호의 길이가 1이 되는 각도가 1라디안이다. 처음에는 낯설지만, 수학과 물리에서는 라디안이 훨씬 자연스럽다. 회전, 각속도, 미분을 다룰 때 식이 간단해지기 때문이다.
도와 라디안의 관계는 다음과 같다.
180도 = π 라디안
360도 = 2π 라디안
90도 = π/2 라디안Python에서는 NumPy가 변환 함수를 제공한다.
import numpy as np
angle_deg = 90
angle_rad = np.deg2rad(angle_deg)
print(angle_rad)
print(np.pi / 2)출력값은 둘 다 약 1.5708이다. 즉 90도는 약 1.5708라디안이다.
반대로 라디안을 도로 바꿀 수도 있다.
angle_rad = np.pi
angle_deg = np.rad2deg(angle_rad)
print(angle_deg)결과는 180.0이다.
로봇 시뮬레이션을 만들 때 사람이 입력하는 각도는 도가 편할 때가 많다. 예를 들어 "45도 방향으로 로봇을 놓고 싶다"라고 생각하기 쉽다. 그러나 계산에 들어가기 전에는 라디안으로 바꾸어야 한다.
theta = np.deg2rad(45)
print(np.cos(theta))
print(np.sin(theta))여기서 실수하기 쉬운 부분이 있다. np.cos(90)은 90도의 코사인이 아니다. Python은 90을 90라디안으로 해석한다.
print(np.cos(90)) # 90라디안의 cos
print(np.cos(np.deg2rad(90))) # 90도의 cos두 값은 전혀 다르다. 로봇이 엉뚱한 방향으로 움직일 때는 각도 단위를 가장 먼저 의심해 보아야 한다.
삼각함수로 방향을 만든다
2D 평면에서 각도 theta가 주어졌을 때, 그 방향을 가리키는 단위 벡터는 다음과 같다.
[cos(theta), sin(theta)]왜 그럴까? 원점을 중심으로 반지름이 1인 원을 생각해 보자. 각도 theta만큼 돌아간 원 위의 점은 x좌표가 cos(theta), y좌표가 sin(theta)가 된다.
import numpy as np
for deg in [0, 30, 45, 90, 180]:
theta = np.deg2rad(deg)
direction = np.array([np.cos(theta), np.sin(theta)])
print(deg, direction)대략 다음과 같은 결과가 나온다.
0 [1. 0. ]
30 [0.866... 0.5 ]
45 [0.707... 0.707... ]
90 [0. 1. ]
180 [-1. 0. ]0도에서는 오른쪽, 90도에서는 위쪽, 180도에서는 왼쪽을 가리킨다. Matplotlib의 기본 좌표계에서 x축은 오른쪽, y축은 위쪽이므로 그림과도 잘 맞는다.
이 벡터는 길이가 1이다. 그래서 방향만 나타낸다. 로봇이 그 방향으로 속도 v만큼 움직인다면 다음처럼 이동 벡터를 만들 수 있다.
speed = 0.3
move = speed * direction로봇의 전진 방향을 계산할 때 이 식이 계속 등장한다.
2D 회전 행렬 만들기
점 하나를 원점 기준으로 theta만큼 회전시키는 행렬은 다음과 같다.
R(theta) = [[cos(theta), -sin(theta)],
[sin(theta), cos(theta)]]이 행렬을 2D 벡터에 곱하면 벡터가 반시계 방향으로 theta만큼 회전한다. 여기서 반시계 방향이 양수 방향이다.
Python 함수로 만들어 보자.
import numpy as np
def rotation_matrix(theta):
c = np.cos(theta)
s = np.sin(theta)
return np.array([
[c, -s],
[s, c]
])이제 점 [1, 0]을 90도 회전해 보자.
p = np.array([1.0, 0.0])
theta = np.deg2rad(90)
R = rotation_matrix(theta)
p_rotated = R @ p
print(p_rotated)결과는 거의 다음과 같다.
[0. 1.]실제로는 컴퓨터 계산 오차 때문에 6.123234e-17처럼 매우 작은 숫자가 나올 수 있다.
[6.123234e-17 1.000000e+00]이 값은 0이라고 생각해도 된다. 부동소수점 계산에서는 이런 아주 작은 오차가 자연스럽게 생긴다.
이번에는 [2, 0]을 90도 회전해 보자.
p = np.array([2.0, 0.0])
p_rotated = R @ p
print(p_rotated)결과는 [0, 2]에 가깝다. 방향은 바뀌었지만 길이는 그대로다. 회전 행렬은 벡터의 길이를 바꾸지 않는다. 이름 그대로 회전만 시킨다.
회전 전후를 그림으로 확인하기
숫자만 보면 회전이 잘 되었는지 감이 덜 온다. Matplotlib으로 직접 그려 보자.
import numpy as np
import matplotlib.pyplot as plt
def rotation_matrix(theta):
c = np.cos(theta)
s = np.sin(theta)
return np.array([
[c, -s],
[s, c]
])
p = np.array([2.0, 0.0])
theta = np.deg2rad(60)
R = rotation_matrix(theta)
p_rotated = R @ p
plt.figure(figsize=(6, 6))
plt.axhline(0, color="gray", linewidth=1)
plt.axvline(0, color="gray", linewidth=1)
plt.grid(True)
plt.axis("equal")
plt.quiver(0, 0, p[0], p[1], angles="xy", scale_units="xy", scale=1,
color="blue", label="original")
plt.quiver(0, 0, p_rotated[0], p_rotated[1], angles="xy", scale_units="xy", scale=1,
color="red", label="rotated")
plt.xlim(-3, 3)
plt.ylim(-3, 3)
plt.legend()
plt.show()파란 화살표는 원래 벡터이고, 빨간 화살표는 회전된 벡터다. 60도만큼 반시계 방향으로 돌아간 모습을 볼 수 있다.
이 그림에서 중요한 점은 화살표의 끝점만 움직인 것이 아니라, 벡터 전체가 원점을 중심으로 회전했다는 것이다. 로봇 몸체의 각 꼭짓점도 같은 방식으로 회전시킬 수 있다.
여러 점을 한꺼번에 회전시키기
로봇은 점 하나가 아니다. 물론 단순한 시뮬레이션에서는 로봇을 하나의 점으로 표현할 수 있지만, 방향을 보여 주려면 삼각형이나 화살표 모양이 더 좋다.
로봇 몸체를 로컬 좌표계에서 삼각형으로 만들어 보자. 여기서 로컬 좌표계란 로봇 몸에 붙어 있는 좌표계다. 로봇의 앞쪽을 로컬 x축 양의 방향으로 둔다.
robot_shape = np.array([
[0.6, 0.0], # 앞쪽 꼭짓점
[-0.4, 0.25], # 뒤쪽 왼쪽
[-0.4, -0.25], # 뒤쪽 오른쪽
[0.6, 0.0] # 다시 앞쪽으로 돌아와 선을 닫음
])이 좌표들은 아직 월드 좌표가 아니다. 로봇이 자기 몸을 기준으로 봤을 때의 모양이다. 로봇이 어느 위치에 있든, 어느 방향을 바라보든, 로봇 자신에게 앞은 항상 [0.6, 0.0] 쪽이다.
이제 이 로컬 좌표들을 회전시켜 보자.
theta = np.deg2rad(45)
R = rotation_matrix(theta)
rotated_shape = robot_shape @ R.T여기서 R.T가 등장했다. 앞에서는 점 하나를 R @ p로 회전시켰다. 그때는 벡터 p를 세로 벡터처럼 생각했다.
하지만 robot_shape는 여러 점을 행(row)으로 쌓아 놓은 배열이다.
[[x1, y1],
[x2, y2],
[x3, y3]]각 행을 회전시키려면 다음처럼 계산한다.
rotated_shape = robot_shape @ R.T점 하나를 세로 벡터로 놓고 R @ p를 하느냐, 여러 점을 행으로 놓고 points @ R.T를 하느냐의 차이다. 결과는 같은 회전이다.
그림으로 확인해 보자.
import numpy as np
import matplotlib.pyplot as plt
def rotation_matrix(theta):
c = np.cos(theta)
s = np.sin(theta)
return np.array([
[c, -s],
[s, c]
])
robot_shape = np.array([
[0.6, 0.0],
[-0.4, 0.25],
[-0.4, -0.25],
[0.6, 0.0]
])
theta = np.deg2rad(45)
R = rotation_matrix(theta)
rotated_shape = robot_shape @ R.T
plt.figure(figsize=(6, 6))
plt.axhline(0, color="gray", linewidth=1)
plt.axvline(0, color="gray", linewidth=1)
plt.grid(True)
plt.axis("equal")
plt.plot(robot_shape[:, 0], robot_shape[:, 1], "b--", label="local shape")
plt.plot(rotated_shape[:, 0], rotated_shape[:, 1], "r-", label="rotated shape")
plt.xlim(-1, 1)
plt.ylim(-1, 1)
plt.legend()
plt.show()파란 점선 삼각형은 회전 전 모양이고, 빨간 삼각형은 45도 회전한 모양이다. 로봇의 앞쪽 꼭짓점이 오른쪽 위를 향하게 된다.
회전한 뒤 이동시키기
로봇은 보통 원점에만 있지 않다. 예를 들어 로봇의 월드 좌표 위치가 [2, 1]이고, 방향이 45도라고 해 보자.
로컬 좌표에 있는 로봇 몸체를 월드 좌표에 그리려면 두 단계를 거친다.
1. 로봇의 방향만큼 회전한다.
2. 로봇의 위치만큼 이동한다.코드로 쓰면 다음과 같다.
robot_position = np.array([2.0, 1.0])
robot_theta = np.deg2rad(45)
R = rotation_matrix(robot_theta)
robot_world = robot_shape @ R.T + robot_positionrobot_shape @ R.T는 로봇 몸체를 방향에 맞게 돌린다. + robot_position은 회전된 몸체를 월드 좌표의 실제 위치로 옮긴다.
전체 그림을 그려 보자.
import numpy as np
import matplotlib.pyplot as plt
def rotation_matrix(theta):
c = np.cos(theta)
s = np.sin(theta)
return np.array([
[c, -s],
[s, c]
])
def transform_robot_shape(robot_shape, position, theta):
R = rotation_matrix(theta)
return robot_shape @ R.T + position
robot_shape = np.array([
[0.6, 0.0],
[-0.4, 0.25],
[-0.4, -0.25],
[0.6, 0.0]
])
robot_position = np.array([2.0, 1.0])
robot_theta = np.deg2rad(45)
robot_world = transform_robot_shape(robot_shape, robot_position, robot_theta)
plt.figure(figsize=(7, 7))
plt.axhline(0, color="gray", linewidth=1)
plt.axvline(0, color="gray", linewidth=1)
plt.grid(True)
plt.axis("equal")
plt.plot(robot_world[:, 0], robot_world[:, 1], "r-", linewidth=2)
plt.scatter(robot_position[0], robot_position[1], color="black", label="robot position")
plt.xlim(0, 4)
plt.ylim(-1, 3)
plt.legend()
plt.show()이제 로봇 아이콘은 원점이 아니라 (2, 1)에 놓이고, 45도 방향을 바라본다.
여기서 이미 중요한 패턴이 완성되었다.
world_points = local_points @ R.T + position이 식은 앞으로 계속 등장한다. 로봇 몸체, 센서 위치, 바퀴 위치, 카메라가 보는 점을 월드 좌표로 바꿀 때 같은 구조를 사용한다.
로봇의 전방 벡터를 월드 방향으로 바꾸기
로봇은 자기 기준으로 움직인다. 바퀴가 앞으로 굴러가면 로봇은 자기 앞쪽으로 간다. 로봇 입장에서 전방은 항상 로컬 좌표의 x축 양의 방향이다.
forward_local = np.array([1.0, 0.0])그런데 월드 좌표에서 로봇이 실제로 어느 방향으로 움직이는지는 로봇의 각도에 따라 달라진다. 로봇이 0도를 바라보면 월드 x축 방향으로 움직인다. 90도를 바라보면 월드 y축 방향으로 움직인다.
로컬 전방 벡터를 월드 전방 벡터로 바꾸려면 회전 행렬을 곱하면 된다.
robot_theta = np.deg2rad(30)
R = rotation_matrix(robot_theta)
forward_local = np.array([1.0, 0.0])
forward_world = R @ forward_local
print(forward_world)결과는 다음과 비슷하다.
[0.8660254 0.5 ]이 값은 [cos(30도), sin(30도)]와 같다. 즉 로봇이 30도를 바라볼 때 전진 방향은 오른쪽 위를 향한다.
이제 전진 속도를 곱하면 월드 좌표에서의 속도 벡터가 된다.
speed = 0.2
velocity_world = speed * forward_world
print(velocity_world)로봇 위치 업데이트는 다음처럼 쓸 수 있다.
dt = 0.1
robot_position = robot_position + velocity_world * dt전체를 함수로 정리하면 더 깔끔하다.
def forward_vector(theta):
R = rotation_matrix(theta)
return R @ np.array([1.0, 0.0])
def update_position(position, theta, speed, dt):
direction = forward_vector(theta)
return position + speed * direction * dt이 함수는 단순하지만 로봇 이동 시뮬레이션의 핵심을 담고 있다. 로봇은 자기 기준으로 "앞으로 가라"는 명령을 받지만, 실제 위치 업데이트는 월드 좌표에서 일어나기 때문이다.
방향을 가진 로봇을 움직여 보기
이제 회전 행렬을 사용해 로봇 아이콘을 그리고, 전방 벡터를 따라 움직이는 간단한 예제를 만들어 보자.
import numpy as np
import matplotlib.pyplot as plt
def rotation_matrix(theta):
c = np.cos(theta)
s = np.sin(theta)
return np.array([
[c, -s],
[s, c]
])
def transform_robot_shape(robot_shape, position, theta):
R = rotation_matrix(theta)
return robot_shape @ R.T + position
def forward_vector(theta):
R = rotation_matrix(theta)
return R @ np.array([1.0, 0.0])
robot_shape = np.array([
[0.6, 0.0],
[-0.4, 0.25],
[-0.4, -0.25],
[0.6, 0.0]
])
position = np.array([0.0, 0.0])
theta = np.deg2rad(35)
speed = 0.2
dt = 0.1
trajectory = []
for _ in range(80):
trajectory.append(position.copy())
direction = forward_vector(theta)
position = position + speed * direction * dt
trajectory = np.array(trajectory)
robot_world = transform_robot_shape(robot_shape, position, theta)
plt.figure(figsize=(7, 7))
plt.axhline(0, color="gray", linewidth=1)
plt.axvline(0, color="gray", linewidth=1)
plt.grid(True)
plt.axis("equal")
plt.plot(trajectory[:, 0], trajectory[:, 1], "b--", label="trajectory")
plt.plot(robot_world[:, 0], robot_world[:, 1], "r-", linewidth=2, label="robot")
plt.scatter(position[0], position[1], color="black")
plt.xlim(-0.5, 2.5)
plt.ylim(-0.5, 2.5)
plt.legend()
plt.show()로봇은 35도 방향을 유지한 채 직선으로 움직인다. 로봇 아이콘의 앞쪽과 궤적의 방향이 일치해야 한다. 만약 로봇 아이콘은 오른쪽 위를 보는데 궤적은 다른 방향으로 뻗는다면, 회전 행렬을 적용하는 방식이나 각도 단위를 다시 확인해야 한다.
로컬 좌표와 글로벌 좌표
로봇공학에서 가장 중요한 감각 중 하나는 "이 벡터가 어느 좌표계 기준인가?"를 계속 묻는 것이다.
로컬 좌표는 로봇 몸에 붙어 있는 좌표다. 로봇이 돌면 로컬 좌표축도 같이 돈다. 로봇 기준으로 앞은 언제나 [1, 0]이다. 왼쪽은 [0, 1], 뒤는 [-1, 0]이다.
글로벌 좌표, 또는 월드 좌표는 세상에 고정된 좌표다. 지도 위의 좌표라고 생각하면 된다. 로봇이 돌아도 월드 x축과 y축은 움직이지 않는다.
예를 들어 로봇이 90도 방향을 바라보고 있다고 하자. 로봇 기준 전방 벡터는 여전히 [1, 0]이다. 하지만 월드 좌표에서 이 전방은 [0, 1]이다.
theta = np.deg2rad(90)
forward_local = np.array([1.0, 0.0])
forward_world = rotation_matrix(theta) @ forward_local
print(forward_world)로봇 기준의 벡터를 월드 기준으로 바꿀 때는 다음 식을 사용한다.
v_world = R(theta) @ v_local반대로 월드 기준의 벡터를 로봇 기준으로 바꾸고 싶다면 어떻게 해야 할까? 회전을 되돌리면 된다. 회전 행렬의 좋은 성질 중 하나는 역행렬이 전치 행렬과 같다는 것이다.
v_local = R(theta).T @ v_world전치 행렬은 행과 열을 바꾼 행렬이다. 회전 행렬에서는 이 전치 행렬이 회전을 거꾸로 돌리는 역할을 한다.
예를 들어 월드 좌표에서 목표가 로봇의 어느 방향에 있는지 알고 싶을 때 이 변환이 필요하다.
robot_position = np.array([2.0, 1.0])
robot_theta = np.deg2rad(45)
target_position = np.array([4.0, 1.0])
v_world = target_position - robot_position
R = rotation_matrix(robot_theta)
v_local = R.T @ v_world
print("world vector:", v_world)
print("local vector:", v_local)v_world는 월드 좌표에서 본 목표 방향이다. v_local은 로봇 기준에서 본 목표 방향이다. v_local[0]이 양수이면 목표는 로봇 앞쪽에 있다. v_local[1]이 양수이면 목표는 로봇 왼쪽에 있다. 이 정보는 로봇이 목표를 향해 회전할 때 매우 유용하다.
점을 회전시키는 관점과 좌표축을 회전시키는 관점
회전 행렬을 처음 배울 때 헷갈리는 부분이 있다. 어떤 설명에서는 점이 회전한다고 말하고, 어떤 설명에서는 좌표축이 회전한다고 말한다.
점이 회전한다는 관점에서는 원점에 고정된 좌표축을 그대로 두고, 점이나 물체를 돌린다. 예를 들어 [1, 0]이라는 점을 90도 회전하면 [0, 1]이 된다.
좌표축이 회전한다는 관점에서는 물체는 그대로 있는데, 그것을 바라보는 기준 좌표계가 바뀐다. 로봇 좌표계와 월드 좌표계 사이의 변환이 여기에 가깝다. 로봇이 90도 돌아서 위쪽을 바라보면, 로봇의 x축은 월드 y축 방향을 가리킨다.
두 관점은 서로 연결되어 있다. 로봇의 로컬 좌표에 있는 점을 월드 좌표로 바꿀 때는 "로봇 몸에 붙은 점들을 로봇 방향만큼 회전시킨 뒤 위치를 더한다"고 생각하면 쉽다.
p_world = R @ p_local + robot_position여러 점을 행으로 쌓았다면 다음처럼 쓴다.
points_world = points_local @ R.T + robot_position반대로 월드 좌표의 점을 로봇 기준으로 보고 싶다면 위치를 먼저 빼고, 회전을 되돌린다.
p_local = R.T @ (p_world - robot_position)이 세 식은 평면 로봇을 다룰 때 기본 도구처럼 쓰인다.
회전 행렬에서 자주 하는 실수
회전 행렬 자체는 짧지만, 실제 코드에서는 작은 실수가 로봇을 이상한 방향으로 보낸다. 특히 다음 네 가지를 조심해야 한다.
도와 라디안을 섞어 사용한다.
sin앞의 부호를 반대로 쓴다.R @ p와points @ R.T를 혼동한다.로컬 좌표 벡터인지 월드 좌표 벡터인지 확인하지 않는다.
가장 흔한 실수는 각도 단위다. 사람이 보기 좋은 90도를 그대로 np.sin(90)에 넣으면 90라디안으로 계산된다. 항상 np.deg2rad를 사용하거나, 내부 계산에서는 처음부터 라디안만 쓰는 습관을 들이는 것이 좋다.
두 번째는 회전 방향이다. 여기서 사용한 행렬은 양의 각도에서 반시계 방향으로 회전한다.
[[cos(theta), -sin(theta)],
[sin(theta), cos(theta)]]만약 다음처럼 sin의 부호가 바뀌면 회전 방향이 반대가 된다.
[[ cos(theta), sin(theta)],
[-sin(theta), cos(theta)]]이 행렬은 -theta만큼 회전하는 것과 같다.
세 번째는 NumPy 배열 모양의 문제다. 점 하나를 벡터로 다룰 때는 R @ p가 자연스럽다. 하지만 여러 점을 (N, 2) 모양의 배열로 쌓아 놓았다면 points @ R.T가 편하다.
마지막으로 좌표계 기준을 확인해야 한다. 같은 [1, 0]이라도 로봇 기준이면 "로봇의 앞"이고, 월드 기준이면 "세상의 오른쪽"이다. 로봇이 회전하면 이 둘은 더 이상 같은 방향이 아니다.
작은 실험: 각도를 바꿔 가며 로봇 그리기
각도에 따라 로봇 몸체가 어떻게 바뀌는지 한 번에 그려 보자.
import numpy as np
import matplotlib.pyplot as plt
def rotation_matrix(theta):
c = np.cos(theta)
s = np.sin(theta)
return np.array([
[c, -s],
[s, c]
])
def transform_robot_shape(robot_shape, position, theta):
R = rotation_matrix(theta)
return robot_shape @ R.T + position
robot_shape = np.array([
[0.6, 0.0],
[-0.4, 0.25],
[-0.4, -0.25],
[0.6, 0.0]
])
angles_deg = [0, 30, 60, 90, 135, 180]
plt.figure(figsize=(8, 6))
plt.axhline(0, color="gray", linewidth=1)
plt.axvline(0, color="gray", linewidth=1)
plt.grid(True)
plt.axis("equal")
for i, deg in enumerate(angles_deg):
position = np.array([i * 1.2, 0.0])
theta = np.deg2rad(deg)
shape_world = transform_robot_shape(robot_shape, position, theta)
plt.plot(shape_world[:, 0], shape_world[:, 1], linewidth=2)
plt.text(position[0] - 0.2, -0.6, f"{deg}°")
plt.xlim(-1, len(angles_deg) * 1.2)
plt.ylim(-1.2, 1.2)
plt.show()같은 로봇 모양이 각도에 따라 다르게 놓인다. 위치는 옆으로 조금씩 옮겼지만, 핵심은 회전이다. 로봇의 앞쪽 꼭짓점이 각도에 맞게 돌아가는지 확인하면 된다.
이번에는 모든 로봇을 같은 위치에 겹쳐서 그려 보자.
plt.figure(figsize=(6, 6))
plt.axhline(0, color="gray", linewidth=1)
plt.axvline(0, color="gray", linewidth=1)
plt.grid(True)
plt.axis("equal")
position = np.array([0.0, 0.0])
for deg in angles_deg:
theta = np.deg2rad(deg)
shape_world = transform_robot_shape(robot_shape, position, theta)
plt.plot(shape_world[:, 0], shape_world[:, 1], linewidth=2, label=f"{deg}°")
plt.xlim(-1, 1)
plt.ylim(-1, 1)
plt.legend()
plt.show()겹쳐 보면 로봇이 원점을 중심으로 빙글 도는 것처럼 보인다. 로봇 몸체의 모든 꼭짓점이 같은 회전 행렬로 변환되기 때문이다.
로봇 이동 코드에 회전 행렬 넣기
앞에서 만들었던 목표 추종 로봇은 목표 방향 각도를 계산하고, 그 각도에 맞춰 움직였다. 이제 그 움직임을 회전 행렬 관점으로 다시 쓸 수 있다.
로봇의 상태를 다음처럼 둔다.
position = np.array([0.0, 0.0])
theta = 0.0선속도와 각속도가 주어졌다고 하자. 선속도는 로봇이 자기 앞 방향으로 가는 속도이고, 각속도는 방향이 변하는 속도다.
linear_speed = 0.5
angular_speed = np.deg2rad(30)
dt = 0.1위치를 업데이트하려면 먼저 로봇의 로컬 전방 벡터를 월드 방향으로 바꾼다.
forward_world = rotation_matrix(theta) @ np.array([1.0, 0.0])
position = position + linear_speed * forward_world * dt방향은 각속도만큼 바뀐다.
theta = theta + angular_speed * dt이 둘을 반복하면 로봇은 곡선을 그리며 움직인다.
import numpy as np
import matplotlib.pyplot as plt
def rotation_matrix(theta):
c = np.cos(theta)
s = np.sin(theta)
return np.array([
[c, -s],
[s, c]
])
def transform_robot_shape(robot_shape, position, theta):
R = rotation_matrix(theta)
return robot_shape @ R.T + position
robot_shape = np.array([
[0.5, 0.0],
[-0.3, 0.2],
[-0.3, -0.2],
[0.5, 0.0]
])
position = np.array([0.0, 0.0])
theta = 0.0
linear_speed = 0.5
angular_speed = np.deg2rad(30)
dt = 0.1
trajectory = []
for _ in range(100):
trajectory.append(position.copy())
forward_world = rotation_matrix(theta) @ np.array([1.0, 0.0])
position = position + linear_speed * forward_world * dt
theta = theta + angular_speed * dt
trajectory = np.array(trajectory)
robot_world = transform_robot_shape(robot_shape, position, theta)
plt.figure(figsize=(7, 7))
plt.grid(True)
plt.axis("equal")
plt.plot(trajectory[:, 0], trajectory[:, 1], "b--", label="trajectory")
plt.plot(robot_world[:, 0], robot_world[:, 1], "r-", linewidth=2, label="robot")
plt.scatter(position[0], position[1], color="black")
plt.xlim(-0.5, 3)
plt.ylim(-0.5, 3)
plt.legend()
plt.show()각속도가 0이면 로봇은 직선으로 움직인다. 각속도가 양수이면 반시계 방향으로 방향을 바꾸며 곡선을 그린다. 각속도가 음수이면 시계 방향으로 돈다.
이 코드는 단순하지만 실제 이동 로봇 모델의 뼈대와 닮아 있다. 로봇은 자기 몸 기준으로 속도 명령을 받고, 회전 행렬을 통해 그 명령이 월드 좌표의 움직임으로 바뀐다.
회전 행렬이 보존하는 것
회전 행렬은 벡터의 방향을 바꾸지만 길이는 바꾸지 않는다. 이것은 로봇공학에서 매우 중요한 성질이다. 로봇 몸체의 모양을 회전시켰는데 크기가 찌그러지면 안 된다. 센서가 로봇 앞 30cm에 붙어 있다면 로봇이 어느 방향을 보더라도 센서는 여전히 로봇 중심에서 30cm 떨어져 있어야 한다.
간단히 확인해 보자.
p = np.array([0.6, 0.2])
theta = np.deg2rad(123)
R = rotation_matrix(theta)
p_rotated = R @ p
print(np.linalg.norm(p))
print(np.linalg.norm(p_rotated))두 값은 거의 같다. 회전 전후로 원점에서 점까지의 거리가 유지된다.
두 벡터 사이의 각도 관계도 유지된다. 즉 로봇 몸체의 앞쪽, 왼쪽, 뒤쪽 점들이 서로 이루는 모양은 회전 후에도 그대로다. 그래서 회전 행렬은 로봇의 자세를 표현하기에 적합하다.
핵심 정리
2D 회전은 로봇의 방향을 다루는 가장 기본적인 도구다. 로봇의 몸체를 그릴 때도, 로봇의 전방 속도를 월드 좌표 속도로 바꿀 때도, 목표물이 로봇 기준 어느 쪽에 있는지 계산할 때도 회전 행렬이 등장한다.
Python의 삼각함수는 라디안을 사용한다. 사람이 생각하기 편한 도 단위는 np.deg2rad로 바꾸어 계산해야 한다. 각도 단위가 틀리면 로봇은 아무리 좋은 제어식을 써도 엉뚱한 방향으로 움직인다.
2D 회전 행렬은 다음과 같다.
R(theta) = [[cos(theta), -sin(theta)],
[sin(theta), cos(theta)]]점 하나를 회전시킬 때는 다음처럼 쓴다.
p_rotated = R @ p여러 점을 (N, 2) 배열로 쌓아 두었다면 다음처럼 쓴다.
points_rotated = points @ R.T로봇 로컬 좌표의 점을 월드 좌표로 바꾸려면 회전한 뒤 위치를 더한다.
points_world = points_local @ R.T + robot_position로봇의 전방 벡터를 월드 이동 방향으로 바꾸려면 로컬 전방 [1, 0]에 회전 행렬을 곱한다.
forward_world = R @ np.array([1.0, 0.0])이제 로봇은 단순한 점이 아니라 방향을 가진 물체가 되었다. 위치에 각도가 더해졌고, 각도는 회전 행렬을 통해 실제 움직임과 그림으로 연결되었다. 다음 단계에서는 회전과 이동을 하나의 행렬 안에 함께 담아, 더 복잡한 좌표 변환을 깔끔하게 표현할 수 있다.
이 노트는 보도·비평·교육·연구 목적으로 작성되었습니다. 저작권 문의가 있으시면 에서 알려주세요.