동차 변환 - 이동과 회전을 한 행렬로
이 장에서 배우는 것
회전행렬을 다룬 앞 장에서는 좌표계 사이의 방향 차이를 2x2 행렬로 표현했다. 그런데 로봇의 좌표계는 방향만 다른 것이 아니라 위치도 다르다. 로봇 본체는 월드 원점에서 떨어진 곳에 있고, 센서는 다시 로봇 본체에서 떨어진 곳에 붙어 있다. 이동과 회전을 각각 따로 계산하면 코드가 금방 복잡해지고 순서를 착각하기 쉽다. 이 장에서는 이동과 회전을 하나의 3x3 행렬로 묶는 동차 변환(homogeneous transform)을 익히고, 행렬 곱만으로 여러 좌표계를 이어 붙이는 방법을 다룬다.
- 3x3 동차 변환 행렬이 회전 블록과 이동 벡터를 어떻게 한 번에 담는지 이해한다
- 두 좌표계 사이의 변환을 행렬 곱으로 합성(composition)하는 방법을 익힌다
- 변환 행렬의 역변환을 구해 반대 방향 좌표 계산을 수행한다
- 로봇 본체 좌표계에 붙은 센서의 오프셋을 월드 좌표로 환산하는 코드를 작성한다
문제 상황
2바퀴 차동 구동 로봇에 거리 센서 하나를 달았다고 하자. 센서는 로봇 정중앙이 아니라 본체 중심에서 앞으로 0.15m, 왼쪽으로 0.05m 떨어진 자리에 장착되어 있다. 센서는 "내 앞 0.8m에 장애물이 있다"고 보고하는데, 이 수치는 어디까지나 센서 좌표계 기준이다. 지도를 그리거나 경로를 계획하는 모듈은 장애물의 월드 좌표를 원한다.
로봇의 회전만 고려한다면 앞 장의 회전행렬로 충분하다. 하지만 실제로는 로봇 자체가 월드 원점에서 벗어난 위치에 있고 방향도 돌아가 있으며, 센서는 다시 로봇 기준으로 떨어진 위치에 있다. "로봇 위치만큼 더하고, 센서 오프셋만큼 또 더하고, 그 사이 방향은 따로 곱해서" 식으로 계산하다 보면 더하는 순서와 회전을 적용하는 순서가 뒤섞이기 쉽다. 로봇에 IMU, 카메라, 여러 개의 거리 센서가 붙으면 이런 계산이 센서 개수만큼 반복되고, 실수는 그만큼 늘어난다. 동차 변환은 이 계산을 하나의 규칙으로 통일해 준다.
동차 좌표와 3x3 변환 행렬
2x2 회전행렬은 원점을 원점으로 보낸다. 즉 아무리 회전행렬을 곱해도 이동은 표현할 수 없다. 이동까지 행렬 곱 하나로 표현하려면 좌표를 한 차원 늘려서 (x, y, 1) 형태로 쓴다. 이렇게 늘린 좌표를 동차 좌표라 부르고, 이 좌표에 곱하는 3x3 행렬을 동차 변환 행렬이라 부른다. 형태는 다음과 같다.
| c -s x |
| s c y |
| 0 0 1 |왼쪽 위 2x2 블록(c, -s, s, c)은 회전행렬 그대로다. 오른쪽 위 2x1 열(x, y)은 이동 벡터다. 맨 아래 행(0, 0, 1)은 동차 좌표 계산이 성립하도록 고정된 값이다. 이 행렬에 동차 좌표 (px, py, 1)을 곱하면 회전과 이동이 한 번의 행렬곱으로 함께 적용된다.
| 구역(행,열) | 이름 | 의미 | 로봇 예시 |
|---|---|---|---|
| (1,1)~(2,2) | 회전 블록 R | 두 좌표계 사이의 방향 차이 | cos θ, sin θ |
| (1,3), (2,3) | 이동 벡터 t | 두 좌표계 원점 사이의 거리 | x=2.0, y=1.0 |
| (3,1)~(3,3) | 고정 행 | 동차 좌표 계산이 성립하도록 고정 | 0, 0, 1 |
변환의 합성 - 좌표계를 체인으로 잇기
로봇에는 서로 다른 좌표계가 겹겹이 붙어 있다. 월드 좌표계 위에 로봇 좌표계가 있고, 로봇 좌표계 위에 센서 좌표계가 있다. 각 단계의 관계를 동차 변환 행렬로 적어두면, 센서에서 월드까지 전체 변환은 두 행렬을 곱하는 것만으로 얻어진다.
T_world_sensor = T_world_robot @ T_robot_sensor여기서 곱셈 순서가 중요하다. 동차 변환 행렬의 곱은 교환법칙이 성립하지 않으므로, 항상 "월드에 가까운 변환을 왼쪽에" 놓고 그 뒤에 자식 좌표계로 가는 변환을 이어 곱한다. T_world_robot은 로봇 좌표계에서 본 점을 월드 좌표로 바꾸는 행렬이고, 여기에 T_robot_sensor를 오른쪽에 곱하면 센서 좌표계에서 본 점을 곧바로 월드 좌표로 바꾸는 행렬이 만들어진다. 좌표계가 셋, 넷으로 늘어나도 같은 방식으로 행렬을 이어 곱하면 된다.
역변환 - 반대 방향으로 좌표 바꾸기
때로는 반대 방향 계산이 필요하다. 예를 들어 월드 좌표로 주어진 목표 지점이 로봇의 앞에 있는지 뒤에 있는지 판단하려면, 목표 지점을 로봇 좌표계로 옮겨봐야 한다. 이럴 때는 T_world_robot의 역변환을 구한다.
동차 변환 행렬은 일반적인 3x3 행렬 역행렬 계산(numpy.linalg.inv)을 그대로 써도 되지만, 회전 블록이 항상 직교 행렬이라는 성질을 이용하면 더 간단하고 정확하게 구할 수 있다. 회전행렬 R의 역행렬은 전치행렬 Rᵀ와 같고, 이동 벡터 t는 -Rᵀt로 바뀐다.
| Rᵀ -Rᵀt |
| 0 1 |이 형태를 그대로 코드로 옮기면 나눗셈이나 반복 계산 없이 역변환을 구할 수 있다.
완성 코드
다음은 하나의 파일로 바로 실행되는 예제다(파일 이름은 homogeneous_transform.py로 가정한다). 로봇은 월드 좌표 (2.0, 1.0)에 있고 30도 회전해 있으며, 거리 센서는 로봇 기준 앞으로 0.15m, 왼쪽으로 0.05m 떨어진 곳에 붙어 있다.
import numpy as np
def make_transform(x, y, theta):
c, s = np.cos(theta), np.sin(theta)
return np.array([
[c, -s, x],
[s, c, y],
[0.0, 0.0, 1.0],
])
def transform_point(t, x, y):
wx, wy, _ = t @ np.array([x, y, 1.0])
return wx, wy
def invert_transform(t):
r = t[:2, :2]
p = t[:2, 2]
r_inv = r.T
inv = np.eye(3)
inv[:2, :2] = r_inv
inv[:2, 2] = -r_inv @ p
return inv
def print_matrix(title, t):
print(title)
for row in t:
print("".join(f"{v:8.3f}" for v in row))
print()
def main():
robot_x, robot_y, robot_deg = 2.0, 1.0, 30.0
t_world_robot = make_transform(robot_x, robot_y, np.radians(robot_deg))
sensor_dx, sensor_dy = 0.15, 0.05
t_robot_sensor = make_transform(sensor_dx, sensor_dy, 0.0)
t_world_sensor = t_world_robot @ t_robot_sensor
print("=== 좌표 변환 예제: 거리 센서 오프셋 ===")
print(f"로봇 pose (world): x={robot_x:.3f}, y={robot_y:.3f}, theta={robot_deg:.1f}도")
print()
print_matrix("[로봇->월드] 변환 행렬 T_world_robot", t_world_robot)
print_matrix("[센서->월드] 변환 행렬 T_world_sensor (합성)", t_world_sensor)
obstacle_sx, obstacle_sy = 0.8, 0.0
ox, oy = transform_point(t_world_sensor, obstacle_sx, obstacle_sy)
print(f"센서가 본 장애물의 월드 좌표: ({ox:.3f}, {oy:.3f})")
target_x, target_y = 3.0, 2.5
t_robot_world = invert_transform(t_world_robot)
tx, ty = transform_point(t_robot_world, target_x, target_y)
print(f"목표점의 로봇 기준 좌표: ({tx:.3f}, {ty:.3f})")
if __name__ == "__main__":
main()
줄별 해설
make_transform(x, y, theta)는 회전각과 이동량을 받아 3x3 동차 변환 행렬을 만든다. 왼쪽 위 2x2는 회전행렬이고, 오른쪽 위 열이 이동 벡터, 마지막 행은 항상 (0, 0, 1)로 고정한다.
transform_point(t, x, y)는 점 (x, y)를 동차 좌표 (x, y, 1)로 확장해 행렬을 곱하고, 결과에서 앞의 두 값만 꺼내 돌려준다. 세 번째 값은 항상 1로 남으므로 _로 버린다.
invert_transform(t)는 회전 블록의 전치행렬 r.T를 새 회전으로 쓰고, 이동 벡터는 -r_inv @ p로 계산한다. 일반적인 역행렬 계산보다 빠르고, 부동소수점 오차도 더 적게 쌓인다.
print_matrix는 행렬을 고정 폭(8.3f)으로 출력해 어떤 실행 환경에서도 같은 모양의 표가 나오게 한다.
main()에서는 로봇 pose로 t_world_robot을 만들고, 센서 오프셋으로 t_robot_sensor를 만든 다음 두 행렬을 곱해 t_world_sensor를 얻는다. 이 합성 행렬로 센서가 감지한 장애물(센서 앞 0.8m)을 월드 좌표로 바꾸고, 반대로 역변환을 이용해 월드 좌표의 목표점을 로봇 기준 좌표로 바꾼다.
실행 결과
$ python3 homogeneous_transform.py
=== 좌표 변환 예제: 거리 센서 오프셋 ===
로봇 pose (world): x=2.000, y=1.000, theta=30.0도
[로봇->월드] 변환 행렬 T_world_robot
0.866 -0.500 2.000
0.500 0.866 1.000
0.000 0.000 1.000
[센서->월드] 변환 행렬 T_world_sensor (합성)
0.866 -0.500 2.105
0.500 0.866 1.118
0.000 0.000 1.000
센서가 본 장애물의 월드 좌표: (2.798, 1.518)
목표점의 로봇 기준 좌표: (1.616, 0.799)
실무에서 자주 틀리는 것
변환 곱셈 순서를 반대로 쓰는 실수
동차 변환 행렬의 곱은 교환법칙이 성립하지 않는다. 부모 좌표계로 가는 변환을 오른쪽에 두면 완전히 다른 결과가 나온다.
t_world_sensor = t_robot_sensor @ t_world_robot # 잘못된 순서t_world_sensor = t_world_robot @ t_robot_sensor # 월드에 가까운 변환을 왼쪽에동차 좌표에 1을 빼먹는 실수
점을 변환할 때 좌표 길이를 2로 두면 3x3 행렬과 곱할 수 없어 그 자리에서 오류가 난다.
p = t_world_sensor @ np.array([0.8, 0.0]) # 길이가 2라서 계산 자체가 실패한다p = t_world_sensor @ np.array([0.8, 0.0, 1.0]) # 동차 좌표로 확장해야 한다이동을 먼저, 회전을 나중에 적용하는 실수
회전과 이동을 손으로 나눠 계산하면 적용 순서가 결과를 바꾼다. 올바른 순서는 먼저 회전한 다음 이동을 더하는 것이며, 동차 변환 행렬은 이 순서를 자동으로 지켜준다.
# 이동을 먼저 더하고 나중에 회전시키면 다른 점이 나온다
shifted_x, shifted_y = sensor_dx + 0.8, sensor_dy
wx = shifted_x * np.cos(theta) - shifted_y * np.sin(theta)# 동차 변환 행렬 하나로 회전 후 이동이 한 번에 적용된다
wx, wy = transform_point(t_world_sensor, 0.8, 0.0)각도 단위를 라디안으로 바꾸지 않는 실수
make_transform은 라디안을 받는다. 도(degree) 값을 그대로 넘기면 회전이 크게 어긋난다.
t = make_transform(0.15, 0.05, 30.0) # 30을 라디안으로 착각t = make_transform(0.15, 0.05, np.radians(30.0)) # 도를 라디안으로 변환한눈에 보기
| 하고 싶은 일 | 수식 | 코드 | 주의할 점 |
|---|---|---|---|
| 두 변환 잇기 | T_world_sensor = T_world_robot · T_robot_sensor | t_world_robot @ t_robot_sensor | 곱셈 순서가 곧 좌표계 순서다 |
| 점을 다른 좌표계로 옮기기 | p' = T · [x, y, 1] | transform_point(t, x, y) | 좌표 끝에 1을 반드시 붙인다 |
| 반대 방향 변환 구하기 | T⁻¹ = [Rᵀ, −Rᵀt; 0, 1] | invert_transform(t) | np.linalg.inv보다 빠르고 정확하다 |
연습 문제
- IMU가 로봇 뒤쪽으로 0.1m, 오른쪽으로 0.03m 떨어진 자리에 붙어 있다. 로봇이 월드 좌표 (1.0, 0.0)에서 90도 방향을 보고 있을 때, IMU의 월드 좌표를 손으로 계산하고 본문 코드를 고쳐서 확인하라.
- 변환 T_a_b(좌표계 b에서 본 점을 a로 바꿈)와 T_b_c가 있을 때, T_c_a를 T_a_b와 T_b_c의 역행렬로 어떻게 표현할 수 있는지 식으로 써라.
- 좌표를 (x, y) 대신 (x, y, 1)로 확장해야 이동까지 행렬 곱 하나로 표현할 수 있는 이유를 한 문단으로 설명하라.
invert_transform의 결과가np.linalg.inv의 결과와 같은지 확인하는 검증 코드를 추가하라.
정답과 해설
- 로봇 pose에서 c=cos90°≈0, s=sin90°≈1이므로 회전행렬은 [[0,-1],[1,0]]이다. 오프셋 (dx, dy) = (-0.1, -0.03)(뒤쪽은 -x, 오른쪽은 -y)을 회전시키면 (0.03, -0.1)이 되고, 여기에 로봇 위치 (1.0, 0.0)을 더하면 IMU의 월드 좌표는 (1.030, -0.100)이다.
- 역행렬의 곱셈 순서는 뒤집힌다. T_a_c = T_a_b @ T_b_c이므로 T_c_a = invert(T_a_c) = invert(T_b_c) @ invert(T_a_b)다.
- 2x2 행렬 곱은 선형 변환이라 원점을 항상 원점으로 보내므로, 어떤 2x2 행렬을 곱해도 이동을 표현할 수 없다. 좌표를 (x, y, 1)로 확장하면 3x3 행렬의 마지막 열이 "1"과 곱해져 그대로 더해지는 효과를 내고, 이 덕분에 이동이 회전과 함께 하나의 행렬 곱으로 표현된다.
assert np.allclose(invert_transform(t), np.linalg.inv(t))처럼 두 결과를 비교하면 된다.invert_transform은 회전행렬이 직교 행렬이라는 성질을 이용하므로np.linalg.inv보다 계산이 적고, 반올림 오차로 마지막 행이 (0, 0, 1)에서 벗어나는 일도 없다.