회전은 곱셈, 이동은 덧셈이었다
지난 장에서 우리는 로봇 몸체의 점들을 회전시키고, 그 결과를 로봇의 위치만큼 옮겼습니다. 코드로 쓰면 대략 이런 모양이었습니다.
world_points = local_points @ R.T + robot_position이 한 줄 안에는 두 가지 일이 들어 있습니다. 먼저 local_points @ R.T로 로봇 기준 좌표를 회전합니다. 그다음 + robot_position으로 월드 좌표계의 적절한 위치로 옮깁니다.
수학적으로도 같은 흐름입니다.
[ mathbf{p}{world} = Rmathbf{p}{robot} + mathbf{t} ]
여기서 (R)은 회전 행렬이고, (mathbf{t})는 이동 벡터입니다. 이 표현은 이해하기 쉽습니다. 회전하고, 이동한다. 아주 자연스럽습니다.
그런데 로봇공학에서는 이런 변환이 하나만 등장하지 않습니다. 로봇 몸체가 있고, 몸체에 센서가 붙어 있고, 센서가 어떤 물체를 보고 있으며, 그 물체의 위치를 다시 월드 좌표계로 바꿔야 할 때가 많습니다.
예를 들어 이런 질문을 생각해 봅시다.
로봇 앞쪽에 카메라가 달려 있습니다. 카메라는 자기 기준으로 물체가 오른쪽 앞에 있다고 말합니다. 그러면 그 물체는 월드 좌표계에서 어디에 있을까요?
이때 필요한 변환은 하나가 아닙니다. 센서 기준 좌표를 로봇 기준 좌표로 바꾸고, 다시 로봇 기준 좌표를 월드 기준 좌표로 바꿔야 합니다.
[ text{센서 기준} rightarrow text{로봇 기준} rightarrow text{월드 기준} ]
회전은 행렬 곱으로 처리하고 이동은 덧셈으로 따로 처리하면, 변환이 여러 개 이어질수록 식이 점점 길어집니다. 이 문제를 깔끔하게 해결해 주는 도구가 바로 동차 좌표계입니다.
동차 좌표계를 사용하면 회전과 이동을 하나의 행렬 곱으로 묶을 수 있습니다.
좌표에 1을 하나 더 붙이는 이상한 아이디어
2D 위치는 보통 두 숫자로 표현합니다.
[ mathbf{p} = begin{bmatrix} x \ y end{bmatrix} ]
동차 좌표계에서는 여기에 숫자 1을 하나 더 붙입니다.
[ mathbf{p}_h = begin{bmatrix} x \ y \ 1 end{bmatrix} ]
처음 보면 조금 이상합니다. 2D 평면 위의 점인데 왜 숫자가 3개가 되었을까요? 로봇이 갑자기 3차원으로 올라간 것은 아닙니다. 마지막의 1은 실제 공간의 축이라기보다, 이동을 행렬 곱 안에 넣기 위한 장치입니다.
일반적인 2D 회전 행렬은 (2 times 2)입니다.
[ R = begin{bmatrix} costheta & -sintheta \ sintheta & costheta end{bmatrix} ]
이 행렬은 점을 회전시킬 수는 있지만, 점을 옮기지는 못합니다. 원점을 중심으로 빙글 돌릴 뿐입니다.
반면 동차 좌표를 쓰면 다음과 같은 (3 times 3) 행렬을 만들 수 있습니다.
[ T = begin{bmatrix} costheta & -sintheta & t_x \ sintheta & costheta & t_y \ 0 & 0 & 1 end{bmatrix} ]
이 행렬을 동차 좌표에 곱해 봅시다.
[ begin{bmatrix} x' \ y' \ 1 end{bmatrix}
begin{bmatrix} costheta & -sintheta & t_x \ sintheta & costheta & t_y \ 0 & 0 & 1 end{bmatrix} begin{bmatrix} x \ y \ 1 end{bmatrix} ]
곱셈 결과를 풀어 쓰면 다음과 같습니다.
[ x' = xcostheta - ysintheta + t_x ]
[ y' = xsintheta + ycostheta + t_y ]
앞부분은 회전이고, 마지막의 (t_x, t_y)는 이동입니다. 즉, 원래는 따로 처리했던 회전과 이동이 하나의 행렬 곱 안에 들어갔습니다.
이것이 동차 좌표계의 핵심입니다.
2D 좌표에
1을 붙이면, 회전과 이동을 하나의3x3행렬 곱으로 표현할 수 있다.
3x3 변환 행렬 만들기
이제 Python으로 직접 변환 행렬을 만들어 보겠습니다. 먼저 NumPy를 불러오고, 위치와 각도를 받아 동차 변환 행렬을 반환하는 함수를 작성합니다.
import numpy as np
def transform_matrix(x, y, theta):
c = np.cos(theta)
s = np.sin(theta)
T = np.array([
[c, -s, x],
[s, c, y],
[0, 0, 1]
])
return T여기서 x, y는 이동량이고, theta는 회전 각도입니다. 각도 단위는 라디안입니다. 도 단위가 익숙하다면 np.deg2rad()를 사용하면 됩니다.
theta = np.deg2rad(30)
T = transform_matrix(2.0, 1.0, theta)
print(T)출력은 대략 다음과 비슷합니다.
[[ 0.8660254 -0.5 2. ]
[ 0.5 0.8660254 1. ]
[ 0. 0. 1. ]]왼쪽 위의 2x2 부분은 회전 행렬입니다.
[[ 0.866, -0.5 ],
[ 0.5 , 0.866]]오른쪽 끝 열의 위쪽 두 값은 이동 벡터입니다.
[2.0, 1.0]마지막 행 [0, 0, 1]은 동차 좌표 변환을 유지하기 위한 약속입니다. 이 행 덕분에 변환 후에도 마지막 값이 계속 1로 남습니다.
점 하나를 변환해 보기
로봇 기준 좌표계에 점 하나가 있다고 해 봅시다.
p_robot = np.array([1.0, 0.0, 1.0])이 점은 로봇 앞쪽으로 1m 떨어진 위치입니다. 마지막의 1.0은 동차 좌표를 만들기 위해 붙인 값입니다.
로봇이 월드 좌표계에서 (2, 1)에 있고, 30도 회전해 있다면 다음과 같이 월드 좌표로 바꿀 수 있습니다.
T_world_robot = transform_matrix(2.0, 1.0, np.deg2rad(30))
p_world = T_world_robot @ p_robot
print(p_world)결과는 다음과 비슷합니다.
[2.8660254 1.5 1. ]마지막 값 1을 제외하면, 월드 좌표는 (2.866, 1.5)입니다.
여기서 T_world_robot이라는 이름을 눈여겨보면 좋습니다. 이 책에서는 다음과 같은 이름 규칙을 사용하겠습니다.
T_world_robot은 로봇 기준 좌표를 월드 기준 좌표로 바꾸는 변환입니다.
즉,
p_world = T_world_robot @ p_robot처럼 읽으면 됩니다.
이름이 길어 보일 수 있지만, 변환이 여러 개 이어질수록 이런 이름이 실수를 줄여 줍니다. 로봇공학에서는 "이 좌표가 어느 기준에서 표현된 것인가?"가 매우 중요합니다.
여러 점을 한 번에 변환하기
실제로는 점 하나보다 여러 점을 한 번에 다루는 경우가 많습니다. 예를 들어 로봇 몸체를 삼각형으로 표현해 봅시다. 로봇 기준 좌표계에서 앞쪽 꼭짓점과 뒤쪽 두 꼭짓점을 정의합니다.
robot_shape = np.array([
[ 0.5, 0.0],
[-0.3, 0.25],
[-0.3, -0.25],
[ 0.5, 0.0]
])각 점에 동차 좌표의 1을 붙이는 함수를 만들겠습니다.
def to_homogeneous(points):
ones = np.ones((points.shape[0], 1))
return np.hstack([points, ones])반대로 동차 좌표에서 다시 일반 2D 좌표만 꺼내는 함수도 만들겠습니다.
def from_homogeneous(points_h):
return points_h[:, :2]여러 점을 변환할 때는 한 가지 주의가 필요합니다. 수식에서는 보통 점을 세로 벡터로 두고 다음처럼 씁니다.
[ mathbf{p}' = Tmathbf{p} ]
하지만 NumPy에서 여러 점을 다룰 때는 각 점을 행으로 쌓는 방식이 편합니다.
points = np.array([
[x1, y1],
[x2, y2],
[x3, y3]
])이 경우에는 행렬을 오른쪽에 곱하는 형태가 되므로 T.T를 사용합니다.
def transform_points(T, points):
points_h = to_homogeneous(points)
transformed_h = points_h @ T.T
return from_homogeneous(transformed_h)이 함수 하나면 로봇 몸체 점들을 월드 좌표로 쉽게 바꿀 수 있습니다.
T_world_robot = transform_matrix(2.0, 1.0, np.deg2rad(30))
robot_world = transform_points(T_world_robot, robot_shape)
print(robot_world)지난 장에서 사용했던 방식은 다음과 비슷했습니다.
world_points = local_points @ R.T + robot_position동차 좌표를 사용하면 같은 일을 이렇게 씁니다.
world_points = transform_points(T_world_robot, local_points)겉으로는 단순히 함수로 감싼 것처럼 보이지만, 중요한 변화가 있습니다. 이제 회전과 이동이 하나의 행렬 T_world_robot 안에 들어갔습니다. 이 행렬은 나중에 다른 변환 행렬과 곱해서 계속 이어 붙일 수 있습니다.
그림으로 확인하기
눈으로 확인해 봅시다. 로봇 기준 좌표계에서 만든 삼각형이 월드 좌표계에서 어디에 놓이는지 그려 보겠습니다.
import matplotlib.pyplot as plt
T_world_robot = transform_matrix(2.0, 1.0, np.deg2rad(30))
robot_world = transform_points(T_world_robot, robot_shape)
plt.figure(figsize=(6, 6))
plt.plot(robot_world[:, 0], robot_world[:, 1], "b-")
plt.scatter(robot_world[:, 0], robot_world[:, 1], color="blue")
plt.scatter([2.0], [1.0], color="red", label="robot position")
plt.axis("equal")
plt.grid(True)
plt.xlabel("x")
plt.ylabel("y")
plt.legend()
plt.show()삼각형의 중심 근처에 빨간 점이 있고, 삼각형의 앞부분은 30도 방향을 향해 있을 것입니다. 로봇 기준에서는 단순히 앞을 향하던 점들이, 월드 기준에서는 로봇의 위치와 방향에 맞게 변환되었습니다.
여기서 중요한 감각은 이것입니다.
로봇 모양 자체는 바뀌지 않았습니다. 로봇 기준 좌표계에 정의된 점들도 그대로입니다. 바뀐 것은 그 점들을 바라보는 기준입니다. 동차 변환 행렬은 "이 기준에서 표현된 점을 저 기준으로 옮겨서 보자"라는 역할을 합니다.
변환 행렬을 곱한다는 것
동차 좌표계의 진짜 힘은 변환 행렬끼리 곱할 수 있다는 데 있습니다.
예를 들어 센서가 로봇 몸체에 붙어 있다고 해 봅시다. 센서는 로봇 중심에서 앞쪽으로 0.4m, 왼쪽으로 0.1m 떨어져 있고, 로봇 정면보다 20도 왼쪽을 바라보고 있습니다.
이 관계를 로봇 기준 좌표계에서 표현하면 다음과 같습니다.
T_robot_sensor = transform_matrix(
x=0.4,
y=0.1,
theta=np.deg2rad(20)
)이 행렬은 센서 기준 좌표를 로봇 기준 좌표로 바꾸는 변환입니다.
p_robot = T_robot_sensor @ p_sensor한편 로봇이 월드 좌표계에서 (2, 1)에 있고, 30도 회전해 있다면 다음 변환을 가집니다.
T_world_robot = transform_matrix(
x=2.0,
y=1.0,
theta=np.deg2rad(30)
)이 행렬은 로봇 기준 좌표를 월드 기준 좌표로 바꿉니다.
p_world = T_world_robot @ p_robot그렇다면 센서 기준 좌표를 월드 기준 좌표로 바꾸려면 어떻게 하면 될까요?
먼저 센서에서 로봇으로, 그다음 로봇에서 월드로 가면 됩니다.
[ mathbf{p}{world} = T{world_robot} T_{robot_sensor} mathbf{p}_{sensor} ]
Python에서는 이렇게 씁니다.
T_world_sensor = T_world_robot @ T_robot_sensor이제 센서 기준 좌표는 바로 월드 좌표로 바뀝니다.
p_world = T_world_sensor @ p_sensor이것이 변환 체인입니다. 여러 좌표계 사이의 관계를 행렬 곱으로 이어 붙이는 방식입니다.
[ text{sensor} rightarrow text{robot} rightarrow text{world} ]
코드로는 다음처럼 읽을 수 있습니다.
T_world_sensor = T_world_robot @ T_robot_sensor오른쪽에서 왼쪽으로 적용된다고 생각하면 됩니다. 먼저 T_robot_sensor가 센서 좌표를 로봇 좌표로 바꾸고, 그다음 T_world_robot이 로봇 좌표를 월드 좌표로 바꿉니다.
센서의 위치를 월드 좌표로 구하기
센서 자체의 위치는 센서 좌표계의 원점입니다. 센서 기준에서 센서의 원점은 당연히 (0, 0)입니다. 동차 좌표로 쓰면 다음과 같습니다.
sensor_origin = np.array([0.0, 0.0, 1.0])이 점을 월드 좌표로 바꾸면 센서가 월드에서 어디에 있는지 알 수 있습니다.
T_world_robot = transform_matrix(2.0, 1.0, np.deg2rad(30))
T_robot_sensor = transform_matrix(0.4, 0.1, np.deg2rad(20))
T_world_sensor = T_world_robot @ T_robot_sensor
sensor_origin = np.array([0.0, 0.0, 1.0])
sensor_world = T_world_sensor @ sensor_origin
print(sensor_world)결과의 앞 두 값이 센서의 월드 좌표입니다.
print(sensor_world[:2])센서 방향도 알고 싶다면 센서 좌표계에서 앞쪽에 있는 점 하나를 함께 변환하면 됩니다. 예를 들어 센서 기준으로 앞쪽 0.3m 위치의 점을 잡습니다.
sensor_forward = np.array([0.3, 0.0, 1.0])
origin_world = T_world_sensor @ sensor_origin
forward_world = T_world_sensor @ sensor_forward
print(origin_world[:2])
print(forward_world[:2])origin_world에서 forward_world로 향하는 선을 그리면 센서가 월드에서 어느 방향을 보고 있는지 알 수 있습니다.
로봇 몸체와 센서를 함께 그리기
이제 로봇 몸체와 센서를 한 그림에 그려 보겠습니다. 로봇 몸체는 삼각형으로, 센서는 작은 선분으로 표현하겠습니다.
import numpy as np
import matplotlib.pyplot as plt
def transform_matrix(x, y, theta):
c = np.cos(theta)
s = np.sin(theta)
return np.array([
[c, -s, x],
[s, c, y],
[0, 0, 1]
])
def to_homogeneous(points):
ones = np.ones((points.shape[0], 1))
return np.hstack([points, ones])
def from_homogeneous(points_h):
return points_h[:, :2]
def transform_points(T, points):
points_h = to_homogeneous(points)
transformed_h = points_h @ T.T
return from_homogeneous(transformed_h)
robot_shape = np.array([
[ 0.5, 0.0],
[-0.3, 0.25],
[-0.3, -0.25],
[ 0.5, 0.0]
])
sensor_line = np.array([
[0.0, 0.0],
[0.3, 0.0]
])
T_world_robot = transform_matrix(2.0, 1.0, np.deg2rad(30))
T_robot_sensor = transform_matrix(0.4, 0.1, np.deg2rad(20))
T_world_sensor = T_world_robot @ T_robot_sensor
robot_world = transform_points(T_world_robot, robot_shape)
sensor_world = transform_points(T_world_sensor, sensor_line)
plt.figure(figsize=(6, 6))
plt.plot(robot_world[:, 0], robot_world[:, 1], "b-", label="robot body")
plt.plot(sensor_world[:, 0], sensor_world[:, 1], "r-", linewidth=3, label="sensor direction")
plt.scatter(sensor_world[0, 0], sensor_world[0, 1], color="red")
plt.axis("equal")
plt.grid(True)
plt.xlabel("x")
plt.ylabel("y")
plt.legend()
plt.show()이 그림에서 파란 삼각형은 로봇 몸체이고, 빨간 선은 센서가 바라보는 방향입니다. 센서는 로봇 몸체에 붙어 있으므로 로봇이 움직이면 센서도 함께 움직여야 합니다.
동차 좌표계에서는 이 관계가 매우 자연스럽습니다. 센서의 상대 위치는 T_robot_sensor에 한 번만 정의해 두면 됩니다. 이후 로봇의 위치와 방향이 바뀔 때마다 T_world_robot만 새로 만들고, 두 행렬을 곱해 T_world_sensor를 다시 계산하면 됩니다.
T_world_sensor = T_world_robot @ T_robot_sensor이 한 줄이 "로봇에 붙어 있는 센서도 함께 움직인다"는 사실을 표현합니다.
로봇이 움직이면 센서도 함께 움직인다
이번에는 로봇이 여러 위치로 움직일 때 센서 위치가 어떻게 갱신되는지 확인해 보겠습니다. 센서의 로봇 기준 위치는 고정되어 있습니다.
T_robot_sensor = transform_matrix(0.4, 0.1, np.deg2rad(20))이 값은 로봇을 조립할 때 정해지는 값이라고 생각하면 됩니다. 센서를 로봇 앞쪽에 나사로 고정했다면, 로봇 기준에서 센서의 위치는 계속 같습니다.
반면 로봇의 월드 기준 위치와 방향은 시간에 따라 바뀝니다.
poses = [
(0.0, 0.0, np.deg2rad(0)),
(1.0, 0.3, np.deg2rad(15)),
(2.0, 0.8, np.deg2rad(30)),
(3.0, 1.5, np.deg2rad(45)),
]각 자세에서 로봇과 센서를 함께 그려 보겠습니다.
plt.figure(figsize=(7, 6))
for x, y, theta in poses:
T_world_robot = transform_matrix(x, y, theta)
T_world_sensor = T_world_robot @ T_robot_sensor
robot_world = transform_points(T_world_robot, robot_shape)
sensor_world = transform_points(T_world_sensor, sensor_line)
plt.plot(robot_world[:, 0], robot_world[:, 1], "b-")
plt.plot(sensor_world[:, 0], sensor_world[:, 1], "r-", linewidth=2)
plt.scatter([x], [y], color="black", s=20)
plt.axis("equal")
plt.grid(True)
plt.xlabel("x")
plt.ylabel("y")
plt.title("Robot and attached sensor")
plt.show()그림을 보면 로봇 몸체가 이동하고 회전할 때 센서도 자연스럽게 따라 움직입니다. 센서를 따로 움직이는 코드는 작성하지 않았습니다. 단지 변환 행렬을 곱했을 뿐입니다.
이것이 동차 좌표계가 로봇공학에서 강력한 이유입니다. 로봇 몸체, 바퀴, 카메라, LiDAR, 로봇 팔의 관절, 손끝 같은 요소들이 모두 "어떤 좌표계가 어떤 좌표계에 붙어 있는가"로 표현될 수 있습니다.
센서가 본 물체를 월드 좌표로 바꾸기
센서의 위치만 구하는 것으로 끝나지 않습니다. 실제 로봇에서는 센서가 무언가를 관측합니다. 예를 들어 센서 기준 좌표계에서 물체가 앞쪽 1m, 오른쪽 0.2m에 있다고 해 봅시다.
여기서 주의할 점이 있습니다. 이 책에서는 2D 좌표에서 x축을 앞쪽, y축을 왼쪽 방향으로 생각해 왔습니다. 따라서 오른쪽 0.2m는 y = -0.2로 표현할 수 있습니다.
object_sensor = np.array([1.0, -0.2, 1.0])이 물체의 월드 좌표는 다음처럼 구합니다.
T_world_robot = transform_matrix(2.0, 1.0, np.deg2rad(30))
T_robot_sensor = transform_matrix(0.4, 0.1, np.deg2rad(20))
T_world_sensor = T_world_robot @ T_robot_sensor
object_world = T_world_sensor @ object_sensor
print(object_world[:2])과정은 단순합니다.
[ mathbf{p}{world} = T{world_robot} T_{robot_sensor} mathbf{p}_{sensor} ]
센서 기준에서 본 물체 좌표가 있고, 센서가 로봇 어디에 붙어 있는지 알고, 로봇이 월드에서 어디에 있는지 알면 물체의 월드 좌표를 계산할 수 있습니다.
이 흐름은 앞으로 카메라 좌표와 로봇 좌표 변환을 다룰 때 다시 등장합니다. 카메라가 본 물체 위치를 로봇 기준 또는 월드 기준으로 바꾸는 문제는 결국 변환 체인의 문제입니다.
변환 순서는 왜 중요할까
행렬 곱에서는 순서가 중요합니다. 일반적인 숫자 곱셈에서는 2 * 3과 3 * 2가 같습니다. 하지만 행렬에서는 대부분 다음이 성립하지 않습니다.
[ AB ne BA ]
변환 행렬도 마찬가지입니다. 회전한 뒤 이동하는 것과, 이동한 뒤 회전하는 것은 결과가 다릅니다.
간단한 예를 들어 보겠습니다.
T_move = transform_matrix(2.0, 0.0, 0.0)
T_rotate = transform_matrix(0.0, 0.0, np.deg2rad(90))
p = np.array([1.0, 0.0, 1.0])
p1 = T_move @ T_rotate @ p
p2 = T_rotate @ T_move @ p
print(p1[:2])
print(p2[:2])p1은 먼저 점을 90도 회전한 뒤, 월드 x 방향으로 2만큼 이동한 결과입니다. p2는 먼저 월드 x 방향으로 2만큼 이동한 뒤, 그 결과 전체를 원점 기준으로 90도 회전한 결과입니다.
둘은 다릅니다.
로봇공학에서 변환 순서를 헷갈리면 센서가 엉뚱한 곳에 붙거나, 로봇이 목표와 반대 방향으로 움직이는 일이 생깁니다. 그래서 변환 행렬을 만들 때는 항상 다음 질문을 해야 합니다.
이 행렬은 어느 좌표계의 점을 어느 좌표계로 바꾸는가?
예를 들어 T_world_robot은 로봇 좌표를 월드 좌표로 바꿉니다. T_robot_sensor는 센서 좌표를 로봇 좌표로 바꿉니다. 그러면 둘을 곱한 T_world_robot @ T_robot_sensor는 센서 좌표를 월드 좌표로 바꿉니다.
T_world_sensor = T_world_robot @ T_robot_sensor가운데의 robot이 자연스럽게 이어진다고 생각하면 좋습니다.
world <- robot <- sensor따라서 결과는 world <- sensor, 즉 센서에서 월드로 가는 변환입니다.
역변환: 월드 좌표를 로봇 좌표로 되돌리기
지금까지는 로봇 기준 좌표를 월드 좌표로 바꾸는 변환을 주로 다뤘습니다. 그런데 반대 방향도 자주 필요합니다.
예를 들어 월드 좌표계에서 목표점이 (4, 2)에 있다고 합시다. 로봇 입장에서는 이 목표가 자기 앞에 있는지, 왼쪽에 있는지, 오른쪽에 있는지 알아야 합니다. 그러려면 월드 좌표를 로봇 좌표로 바꿔야 합니다.
동차 변환 행렬의 반대 방향은 역행렬로 구할 수 있습니다.
T_robot_world = np.linalg.inv(T_world_robot)그러면 월드 좌표의 점을 로봇 기준으로 바꿀 수 있습니다.
goal_world = np.array([4.0, 2.0, 1.0])
goal_robot = T_robot_world @ goal_world
print(goal_robot[:2])이 결과에서 x가 양수면 목표는 로봇 앞쪽에 있습니다. y가 양수면 목표는 로봇의 왼쪽에 있고, y가 음수면 오른쪽에 있습니다.
물론 2D 회전 행렬의 성질을 이용하면 역변환을 직접 만들 수도 있습니다. 하지만 처음에는 np.linalg.inv()를 사용해도 괜찮습니다. 중요한 것은 "변환 행렬에는 반대 방향 변환도 있다"는 감각입니다.
다만 역행렬을 쓸 때는 이름을 분명히 해야 합니다.
T_world_robot = transform_matrix(2.0, 1.0, np.deg2rad(30))
T_robot_world = np.linalg.inv(T_world_robot)T_world_robot과 T_robot_world는 서로 반대 방향입니다. 이름이 비슷하므로 코드를 작성할 때 특히 조심해야 합니다.
작은 실습: 로봇이 보는 목표의 위치
이번에는 로봇이 월드 좌표계의 목표를 자기 기준으로 어떻게 보는지 확인해 보겠습니다.
robot_x = 2.0
robot_y = 1.0
robot_theta = np.deg2rad(30)
T_world_robot = transform_matrix(robot_x, robot_y, robot_theta)
T_robot_world = np.linalg.inv(T_world_robot)
goal_world = np.array([4.0, 2.0, 1.0])
goal_robot = T_robot_world @ goal_world
print("goal in robot frame:", goal_robot[:2])이 결과는 목표점이 로봇 기준에서 어디에 있는지 알려 줍니다. 이제 로봇은 이 값을 사용해 판단할 수 있습니다.
만약 goal_robot[1]이 양수라면 목표는 로봇 왼쪽에 있습니다. 음수라면 오른쪽에 있습니다. goal_robot[0]이 크다면 목표가 앞쪽 멀리 있는 것이고, 0에 가까우면 로봇의 옆이나 근처에 있는 것입니다.
이런 계산은 목표 추종 제어에서도 유용합니다. 월드 좌표에서 목표 방향을 직접 계산할 수도 있지만, 로봇 기준으로 목표를 바꿔 놓으면 "앞으로 얼마나 가야 하는가", "왼쪽으로 얼마나 돌아야 하는가"를 더 직관적으로 판단할 수 있습니다.
동차 좌표계로 다시 쓰는 로봇 시뮬레이션 구조
지금까지 만들었던 로봇 시뮬레이션에서는 로봇의 상태를 보통 다음 세 값으로 표현했습니다.
robot_pose = np.array([x, y, theta])이 표현은 여전히 중요합니다. 로봇의 위치와 방향을 저장하기에는 가장 간단합니다. 하지만 로봇 몸체 점을 그리거나 센서 위치를 계산할 때는 매번 이 포즈를 변환 행렬로 바꿔 주면 좋습니다.
def pose_to_transform(pose):
x, y, theta = pose
return transform_matrix(x, y, theta)그러면 시뮬레이션의 한 스텝은 이런 흐름을 가질 수 있습니다.
pose = np.array([2.0, 1.0, np.deg2rad(30)])
T_world_robot = pose_to_transform(pose)
T_world_sensor = T_world_robot @ T_robot_sensor
robot_world = transform_points(T_world_robot, robot_shape)
sensor_world = transform_points(T_world_sensor, sensor_line)로봇의 포즈가 바뀌면 T_world_robot도 바뀝니다. 그리고 그에 따라 T_world_sensor도 바뀝니다. 하지만 T_robot_sensor는 바뀌지 않습니다. 센서는 로봇 몸체에 고정되어 있기 때문입니다.
이 구조를 이해하면 로봇 모델이 복잡해져도 겁낼 필요가 없습니다. 바퀴가 붙어 있든, 카메라가 붙어 있든, 팔 끝에 그리퍼가 붙어 있든, 결국 핵심은 같습니다.
기준 좌표계 사이의 관계를 변환 행렬로 표현하고, 필요한 순서대로 곱한다.
자주 하는 실수들
동차 좌표계는 강력하지만, 처음에는 몇 가지 실수를 자주 하게 됩니다.
첫 번째는 마지막에 1을 붙이지 않는 실수입니다. 3x3 변환 행렬은 [x, y, 1] 형태의 동차 좌표와 곱해야 합니다. [x, y]만 있으면 행렬 크기가 맞지 않습니다.
두 번째는 행렬 곱 순서를 바꾸는 실수입니다. T_world_robot @ T_robot_sensor와 T_robot_sensor @ T_world_robot은 전혀 다른 의미입니다. 앞의 표현은 센서 좌표를 월드 좌표로 바꾸는 자연스러운 체인이지만, 뒤의 표현은 좌표계 의미가 맞지 않습니다.
세 번째는 NumPy에서 여러 점을 행으로 쌓아 놓고 T가 아니라 T.T를 곱해야 한다는 점을 잊는 실수입니다. 점 하나를 세로 벡터처럼 다룰 때는 T @ p를 사용합니다. 여러 점을 행렬의 행으로 쌓아 놓았을 때는 points_h @ T.T를 사용했습니다.
정리하면 다음과 같습니다.
# 점 하나: shape이 (3,)
p_world = T_world_robot @ p_robot
# 여러 점: shape이 (N, 2)
points_world = transform_points(T_world_robot, points_robot)함수를 만들어 두면 이런 실수를 줄일 수 있습니다.
이번 장의 핵심 정리
동차 좌표계는 처음에는 "좌표에 왜 1을 붙이지?"라는 이상한 아이디어처럼 보입니다. 하지만 그 작은 1 덕분에 이동과 회전을 하나의 행렬 곱으로 묶을 수 있습니다.
2D 로봇공학에서 동차 변환 행렬은 다음과 같은 모양을 가집니다.
[ T = begin{bmatrix} costheta & -sintheta & x \ sintheta & costheta & y \ 0 & 0 & 1 end{bmatrix} ]
이 행렬은 로봇의 위치와 방향을 한 번에 표현합니다. 또한 센서, 물체, 로봇 몸체처럼 서로 다른 좌표계 사이의 관계를 변환 체인으로 연결할 수 있게 해 줍니다.
T_world_sensor = T_world_robot @ T_robot_sensor이 한 줄에는 중요한 의미가 담겨 있습니다. 센서가 로봇에 붙어 있고, 로봇이 월드 안에서 움직인다면, 센서의 월드 위치는 두 변환을 곱해서 구할 수 있습니다.
앞으로 로봇공학을 계속 배우다 보면 이런 변환 체인을 자주 만나게 됩니다. 카메라 좌표에서 로봇 좌표로, 로봇 좌표에서 지도 좌표로, 로봇 팔의 베이스에서 손끝 좌표로 변환하는 문제들이 모두 같은 원리를 따릅니다.
다음 장에서는 2D 평면을 넘어 3D 회전의 기초를 살펴봅니다. 3D에서는 로봇이 좌우로 도는 것뿐 아니라 앞뒤로 기울고, 옆으로 기울 수도 있습니다. 이때 등장하는 Roll, Pitch, Yaw는 동차 변환과 함께 로봇의 자세를 표현하는 중요한 언어가 됩니다.
이 노트는 보도·비평·교육·연구 목적으로 작성되었습니다. 저작권 문의가 있으시면 에서 알려주세요.