2D에서 3D로 넘어간다는 것
앞 장까지 우리는 2D 평면에서 로봇의 위치와 방향을 다루었다. 2D 로봇의 자세는 보통 다음처럼 표현할 수 있었다.
pose_2d = [x, y, theta]여기서 x, y는 위치이고, theta는 로봇이 평면에서 어느 방향을 보고 있는지를 나타내는 각도였다. 바닥 위를 움직이는 작은 모바일 로봇이라면 이 표현만으로도 꽤 많은 것을 할 수 있다.
하지만 세상은 평면보다 조금 더 심술궂다. 드론은 위아래로 움직이고, 기울어진 채 날아간다. 로봇 팔의 손목은 여러 방향으로 꺾이고 돌아간다. 카메라는 로봇 위에 비스듬히 달릴 수도 있다. 이때는 단순히 theta 하나로 방향을 표현할 수 없다.
3D 공간에서 물체의 자세를 표현하려면 세 방향의 회전을 생각해야 한다. 그래서 3D 로봇의 자세는 흔히 다음처럼 표현한다.
pose_3d = [x, y, z, roll, pitch, yaw]앞의 x, y, z는 위치다. 뒤의 roll, pitch, yaw는 자세, 즉 물체가 3차원 공간에서 얼마나 기울어지고 돌아갔는지를 나타낸다.
처음 보면 이름부터 낯설다. 하지만 너무 어렵게 생각할 필요는 없다. 2D에서 theta가 "위에서 내려다봤을 때 얼마나 돌았는가"를 나타냈다면, 3D에서는 "앞뒤로 얼마나 기울었는가", "좌우로 얼마나 기울었는가", "위에서 봤을 때 얼마나 돌았는가"를 나누어 보는 것이다.
Roll, Pitch, Yaw의 직관
이 장에서는 오른손 좌표계를 기준으로 설명한다. 간단히 말해 x축, y축, z축이 서로 직각이고, 오른손의 엄지, 검지, 중지를 각각 세 축으로 펼쳤을 때의 방향 관계를 따른다고 생각하면 된다.
로봇공학에서는 상황에 따라 축의 정의가 달라질 수 있지만, 여기서는 직관을 위해 다음과 같은 기준을 사용하자.
x축: 로봇의 앞쪽y축: 로봇의 왼쪽z축: 위쪽
이 기준에서 Roll, Pitch, Yaw는 다음과 같다.
Roll은
x축 주변 회전이다.Pitch는
y축 주변 회전이다.Yaw는
z축 주변 회전이다.
드론을 떠올리면 이해하기 쉽다. 드론이 정면을 향해 날고 있다고 하자. 드론이 좌우로 기울어지면 Roll이다. 앞으로 고개를 숙이거나 뒤로 젖히면 Pitch다. 제자리에서 나침반 방향을 바꾸듯이 좌우로 방향을 틀면 Yaw다.
비행기를 떠올려도 좋다. 비행기가 날개를 한쪽으로 기울이면 Roll, 기수의 코를 위아래로 들면 Pitch, 수평면에서 방향을 바꾸면 Yaw다.
2D 로봇에서 우리가 다루던 theta는 사실상 Yaw에 해당한다. 바닥 위 로봇은 대체로 위아래로 기울지 않는다고 가정하므로 Roll과 Pitch를 무시하고 Yaw 하나만 썼던 것이다.
이 장에서 사용할 회전 약속
3D 회전은 약속이 중요하다. 같은 Roll, Pitch, Yaw라는 말을 쓰더라도 축을 어떻게 잡는지, 회전을 어떤 순서로 적용하는지에 따라 결과가 달라질 수 있다.
이 장에서는 다음 약속을 사용한다.
p_rotated = R @ p즉, 점 p를 열벡터처럼 생각하고 회전 행렬 R을 왼쪽에서 곱한다. 그리고 Roll, Pitch, Yaw를 하나의 회전 행렬로 합칠 때는 다음 순서를 사용한다.
R = Rz(yaw) @ Ry(pitch) @ Rx(roll)행렬 곱에서는 오른쪽에 있는 것이 먼저 적용된다. 따라서 위 식은 점에 대해 먼저 Roll을 적용하고, 그다음 Pitch를 적용하고, 마지막으로 Yaw를 적용한다는 뜻이다.
p_rotated = Rz(yaw) @ Ry(pitch) @ Rx(roll) @ p이 약속을 처음부터 완벽히 외울 필요는 없다. 지금은 "3D 회전에서는 순서를 정해야 하고, 이 책에서는 Rz @ Ry @ Rx를 사용한다" 정도로 받아들이면 충분하다.
x축 주변 회전: Roll
먼저 x축 주변 회전을 보자. x축 주변으로 돈다는 것은 x값은 그대로 두고, y-z 평면에서 점이 회전한다는 뜻이다.
Roll 각도를 a라고 하면 x축 회전 행렬은 다음과 같다.
import numpy as np
def rot_x(a):
c = np.cos(a)
s = np.sin(a)
return np.array([
[1, 0, 0],
[0, c, -s],
[0, s, c]
])이 행렬을 보면 첫 번째 행과 첫 번째 열이 x축을 그대로 보존한다는 것을 알 수 있다. 대신 y와 z가 서로 섞인다. 바로 이 "서로 섞인다"는 감각이 회전 행렬의 핵심이다.
간단히 점 하나를 돌려 보자.
point = np.array([0.0, 1.0, 0.0])
roll = np.deg2rad(90)
R = rot_x(roll)
rotated = R @ point
print(rotated)결과는 대략 다음과 같다.
[0. 0. 1.]원래 점은 y축 방향에 있었다. 이것을 x축 주변으로 90도 회전시키면 z축 방향으로 올라간다. 즉, x축은 고정된 회전축이고, 그 주변에서 y축 방향이 z축 방향으로 돌아간 것이다.
y축 주변 회전: Pitch
Pitch는 y축 주변 회전이다. 이번에는 y값이 유지되고, x-z 평면에서 점이 움직인다.
def rot_y(a):
c = np.cos(a)
s = np.sin(a)
return np.array([
[ c, 0, s],
[ 0, 1, 0],
[-s, 0, c]
])점 [1, 0, 0]을 y축 주변으로 90도 회전시켜 보자.
point = np.array([1.0, 0.0, 0.0])
pitch = np.deg2rad(90)
R = rot_y(pitch)
rotated = R @ point
print(rotated)결과는 다음과 비슷하다.
[ 0. 0. -1.]x축 방향에 있던 점이 -z축 방향으로 이동했다. 여기서 부호가 헷갈릴 수 있다. 3D 회전에서는 오른손 법칙을 따른다. 엄지를 회전축의 양의 방향으로 두었을 때, 나머지 손가락이 감기는 방향이 양의 회전 방향이다.
처음부터 부호를 모두 머릿속으로 계산하려고 하면 금방 지친다. 대신 코드를 실행해 보고, 그림으로 확인하면서 감각을 쌓는 편이 훨씬 좋다.
z축 주변 회전: Yaw
Yaw는 z축 주변 회전이다. 2D에서 사용했던 회전과 가장 비슷하다. z값은 그대로 두고, x-y 평면에서 점이 회전한다.
def rot_z(a):
c = np.cos(a)
s = np.sin(a)
return np.array([
[c, -s, 0],
[s, c, 0],
[0, 0, 1]
])2D 회전 행렬과 거의 같은 모양이다. 다만 3D 행렬이므로 z축을 유지하기 위한 한 줄과 한 열이 추가되었다.
point = np.array([1.0, 0.0, 0.0])
yaw = np.deg2rad(90)
R = rot_z(yaw)
rotated = R @ point
print(rotated)결과는 다음과 비슷하다.
[0. 1. 0.]x축 방향의 점이 y축 방향으로 돌아갔다. 이것이 우리가 2D 평면에서 익숙하게 보던 회전이다.
Roll, Pitch, Yaw를 하나의 행렬로 합치기
이제 세 회전을 합쳐 보자. 로봇이 Roll, Pitch, Yaw를 모두 가진다면 최종 회전 행렬은 다음처럼 만들 수 있다.
def rpy_to_matrix(roll, pitch, yaw):
return rot_z(yaw) @ rot_y(pitch) @ rot_x(roll)예를 들어 Roll 30도, Pitch 20도, Yaw 45도인 자세를 만들어 보자.
roll = np.deg2rad(30)
pitch = np.deg2rad(20)
yaw = np.deg2rad(45)
R = rpy_to_matrix(roll, pitch, yaw)
np.set_printoptions(precision=3, suppress=True)
print(R)실행하면 다음과 같은 3×3 행렬이 나온다.
[[ 0.664 -0.491 0.563]
[ 0.664 0.733 -0.144]
[-0.342 0.47 0.814]]숫자만 보면 조금 차갑게 느껴진다. 하지만 이 행렬은 "물체가 3D 공간에서 어떤 자세를 하고 있는가"를 담고 있다. 더 구체적으로 말하면, 원래 물체의 x, y, z축이 회전 후 월드 좌표계에서 어느 방향을 가리키는지를 알려 준다.
회전 행렬의 각 열을 보면 회전된 축의 방향을 알 수 있다.
x_axis = R[:, 0]
y_axis = R[:, 1]
z_axis = R[:, 2]
print("rotated x-axis:", x_axis)
print("rotated y-axis:", y_axis)
print("rotated z-axis:", z_axis)이 관점은 로봇을 다룰 때 매우 유용하다. 로봇 팔의 손끝 좌표계, 카메라 좌표계, 드론의 몸체 좌표계는 모두 "자기만의 축"을 가지고 있다. 회전 행렬은 그 축들이 월드 좌표계에서 어떻게 놓여 있는지 알려 주는 지도와 같다.
회전 행렬이 제대로 되었는지 확인하기
회전 행렬은 아무 3×3 행렬이나 될 수 없다. 진짜 회전 행렬이라면 길이와 직각 관계를 보존해야 한다. 물체를 회전시킨다고 해서 막대의 길이가 늘어나거나, 직각이 찌그러지면 안 되기 때문이다.
좋은 회전 행렬은 다음 성질을 가진다.
R.T @ R이 단위 행렬에 가깝다.행렬식
det(R)이 1에 가깝다.각 열벡터의 길이가 1이고 서로 직각이다.
Python으로 확인해 보자.
I = R.T @ R
det = np.linalg.det(R)
print(I)
print(det)결과는 대략 다음과 같다.
[[ 1. -0. 0.]
[-0. 1. 0.]
[ 0. 0. 1.]]
1.0컴퓨터 계산에서는 아주 작은 오차가 생길 수 있으므로 정확히 0이나 1이 나오지 않아도 괜찮다. 0.999999999처럼 나오면 정상이다.
이 성질은 나중에 로봇 시뮬레이션이나 센서 데이터를 다룰 때 중요하다. 회전 행렬이 조금씩 망가지면 로봇의 축이 서로 직각이 아니게 되고, 위치 변환 결과도 이상해진다.
여러 점을 한 번에 회전시키기
앞에서는 점 하나를 열벡터처럼 다루었다. 하지만 시뮬레이션에서는 여러 점을 한 번에 회전시키는 일이 많다. 예를 들어 작은 3D 박스나 좌표축을 그릴 때는 점들이 여러 개 필요하다.
NumPy에서 여러 점을 행 단위로 저장하면 다음과 같은 모양이 된다.
points = np.array([
[1.0, 0.0, 0.0],
[0.0, 1.0, 0.0],
[0.0, 0.0, 1.0]
])각 행이 하나의 점이다. 이때 회전 행렬을 적용하려면 앞 장에서 했던 것처럼 전치 행렬을 오른쪽에 곱하면 된다.
rotated_points = points @ R.T
print(rotated_points)점 하나를 R @ p로 회전시키는 것과, 여러 점을 points @ R.T로 회전시키는 것은 같은 계산을 배열 모양에 맞춰 쓴 것이다.
이 패턴은 앞으로도 자주 나온다.
# 점 하나: shape (3,)
p_rotated = R @ p
# 여러 점: shape (N, 3)
points_rotated = points @ R.T둘 중 무엇이 더 옳다기보다, 데이터를 어떤 모양으로 저장했는지에 따라 자연스러운 표현이 달라진다.
3D 좌표축을 그려 보기
숫자로만 회전을 보면 감이 잘 오지 않는다. 이번에는 Matplotlib의 3D 기능을 사용해 회전된 좌표축을 그려 보자.
import matplotlib.pyplot as plt
def draw_frame(ax, R=None, origin=None, length=1.0, label=""):
if R is None:
R = np.eye(3)
if origin is None:
origin = np.zeros(3)
colors = ["r", "g", "b"]
names = ["x", "y", "z"]
for i in range(3):
axis = R[:, i] * length
ax.quiver(
origin[0], origin[1], origin[2],
axis[0], axis[1], axis[2],
color=colors[i], arrow_length_ratio=0.1
)
end = origin + axis
ax.text(end[0], end[1], end[2], f"{label}{names[i]}")
def set_equal_3d(ax, limit=1.5):
ax.set_xlim([-limit, limit])
ax.set_ylim([-limit, limit])
ax.set_zlim([-limit, limit])
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")이제 원래 좌표축과 회전된 좌표축을 함께 그려 보자.
roll = np.deg2rad(30)
pitch = np.deg2rad(20)
yaw = np.deg2rad(45)
R = rpy_to_matrix(roll, pitch, yaw)
fig = plt.figure(figsize=(7, 7))
ax = fig.add_subplot(111, projection="3d")
draw_frame(ax, np.eye(3), length=1.0, label="world_")
draw_frame(ax, R, length=1.0, label="body_")
set_equal_3d(ax)
ax.set_title("World frame and rotated body frame")
plt.show()빨간색은 x축, 초록색은 y축, 파란색은 z축이다. 원래 축과 회전된 축을 비교하면 Roll, Pitch, Yaw가 단순한 숫자가 아니라 "로봇 몸체가 공간 속에서 놓인 방식"이라는 사실이 조금 더 분명해진다.
드론이라면 회전된 body_x가 드론의 앞쪽을 의미할 수 있다. 카메라라면 회전된 축들이 카메라가 바라보는 방향과 이미지 평면의 방향을 의미할 수 있다. 로봇 팔의 손끝이라면 손끝 도구가 어느 방향을 향하는지 나타낸다.
회전 순서가 결과를 바꾼다
2D 회전에서는 회전축이 하나뿐이었다. 그래서 30도 돌리고 20도 더 돌리면 50도 회전한 것과 같았다. 순서를 바꿔도 결과가 달라지지 않았다.
하지만 3D에서는 다르다. x축으로 먼저 돌린 다음 z축으로 도는 것과, z축으로 먼저 돌린 다음 x축으로 도는 것은 일반적으로 결과가 다르다.
직접 확인해 보자.
roll = np.deg2rad(40)
pitch = np.deg2rad(25)
yaw = np.deg2rad(60)
R_zyx = rot_z(yaw) @ rot_y(pitch) @ rot_x(roll)
R_xyz = rot_x(roll) @ rot_y(pitch) @ rot_z(yaw)
print("R_zyx")
print(R_zyx)
print("R_xyz")
print(R_xyz)두 행렬은 서로 다르다. 점 하나에 적용해 보면 차이가 더 분명하다.
p = np.array([1.0, 0.5, 0.2])
p1 = R_zyx @ p
p2 = R_xyz @ p
print("p1:", p1)
print("p2:", p2)
print("difference:", p1 - p2)같은 Roll, Pitch, Yaw 값을 사용했는데도 최종 위치가 달라진다. 이것은 계산 실수가 아니다. 3D 회전의 본질적인 특징이다.
책상 위에 스마트폰을 놓고 직접 실험해도 좋다. 먼저 화면을 위로 둔 상태에서 오른쪽으로 기울인 다음, 책상 위에서 방향을 돌려 보자. 이번에는 먼저 책상 위에서 방향을 돌리고, 그다음 오른쪽으로 기울여 보자. 최종적으로 스마트폰이 향하는 방향이 달라질 수 있다.
3D 회전은 "얼마나 돌았는가"뿐 아니라 "어떤 순서로 돌았는가"까지 함께 봐야 한다.
로봇 자세와 4×4 변환 행렬
앞 장에서 우리는 2D 동차 좌표계를 사용해 회전과 이동을 하나의 행렬로 묶었다. 3D에서도 같은 생각을 사용할 수 있다. 다만 행렬 크기가 3×3에서 4×4로 커진다.
3D에서 회전 행렬 R과 위치 벡터 t가 있을 때, 변환 행렬 T는 다음처럼 만들 수 있다.
def make_transform_3d(R, t):
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = t
return T예를 들어 로봇의 위치가 [2, 1, 0.5]이고 자세가 Roll 10도, Pitch 20도, Yaw 30도라면 다음과 같이 표현할 수 있다.
roll = np.deg2rad(10)
pitch = np.deg2rad(20)
yaw = np.deg2rad(30)
R = rpy_to_matrix(roll, pitch, yaw)
t = np.array([2.0, 1.0, 0.5])
T = make_transform_3d(R, t)
print(T)이제 로봇 몸체 좌표계의 한 점을 월드 좌표계로 옮길 수 있다.
p_body = np.array([1.0, 0.0, 0.0, 1.0])
p_world = T @ p_body
print(p_world)여기서 p_body의 마지막 1.0은 동차 좌표를 위한 값이다. 앞 장의 2D 동차 좌표와 같은 역할을 한다. 덕분에 회전과 이동을 한 번의 행렬 곱으로 처리할 수 있다.
앞으로 카메라 좌표와 로봇 좌표를 변환할 때도 이 구조가 그대로 등장한다. 카메라가 로봇 손끝에 붙어 있다면, 카메라 좌표계의 점을 손끝 좌표계로 바꾸고, 다시 로봇 베이스 좌표계로 바꾸고, 마지막으로 월드 좌표계로 바꿀 수 있다. 이 모든 과정의 핵심이 바로 회전 행렬과 변환 행렬이다.
오일러 각이 편한 이유와 불편한 이유
Roll, Pitch, Yaw처럼 세 개의 각도로 3D 자세를 표현하는 방식을 넓게는 오일러 각 표현이라고 부른다. 오일러 각은 사람이 이해하기 쉽다. 드론이 얼마나 기울었는지, 자동차가 어느 방향을 보는지, 카메라가 위아래로 얼마나 숙여졌는지 말로 설명하기 좋다.
예를 들어 다음 표현은 직관적이다.
roll_deg = 5
pitch_deg = -10
yaw_deg = 90이 값만 봐도 "조금 오른쪽이나 왼쪽으로 기울었고, 살짝 아래를 보며, 동쪽 또는 북쪽 같은 특정 방향을 향하고 있겠구나" 하고 상상할 수 있다.
하지만 오일러 각에는 불편한 점도 있다. 가장 큰 문제는 특정 자세에서 회전축 두 개가 겹쳐지는 현상이 생길 수 있다는 것이다. 이것을 짐벌락이라고 한다.
짐벌락을 직관적으로 이해하기
짐벌락은 이름만 들으면 거창하지만, 핵심은 간단하다.
3D 회전을 세 번의 축 회전으로 나누어 표현할 때, 어떤 자세에서는 두 회전축이 같은 역할을 하게 된다. 그러면 세 개의 각도를 가지고 있는 것처럼 보이지만 실제로는 독립적인 회전 방향 하나를 잃어버린다.
카메라가 고개를 정확히 위로 들었다고 생각해 보자. 평소에는 좌우로 방향을 돌리는 Yaw와, 카메라 화면을 기울이는 Roll이 서로 다른 움직임처럼 느껴진다. 그런데 카메라가 하늘을 똑바로 보고 있으면 "좌우로 돌린다"와 "화면을 돌린다"의 구분이 흐려진다. 축들이 겹쳐 보이기 때문이다.
우리의 회전 약속 R = Rz(yaw) @ Ry(pitch) @ Rx(roll)에서는 Pitch가 90도 근처일 때 이런 문제가 나타난다. 코드로 확인해 보자.
pitch = np.deg2rad(90)
roll1 = np.deg2rad(30)
yaw1 = np.deg2rad(10)
roll2 = np.deg2rad(60)
yaw2 = np.deg2rad(40)
R1 = rpy_to_matrix(roll1, pitch, yaw1)
R2 = rpy_to_matrix(roll2, pitch, yaw2)
print(np.allclose(R1, R2))
print(R1)
print(R2)두 경우를 보면 Roll과 Yaw 값은 다르지만, 특정 조건에서는 같은 회전 행렬을 만들 수 있다. 위 예에서는 roll - yaw의 차이가 같기 때문에 Pitch가 90도일 때 같은 자세가 된다.
이것이 짐벌락의 무서운 점이다. 로봇이 실제로 이상해진 것이 아니다. 3D 자세 자체가 사라진 것도 아니다. 문제는 자세를 표현하는 방식에서 생긴다. 세 각도로 자세를 설명하려고 했는데, 어느 순간 두 각도가 서로 독립적이지 않게 되는 것이다.
그래서 로봇 제어, 드론 자세 추정, 3D 그래픽스, SLAM 같은 분야에서는 오일러 각만으로 모든 계산을 처리하지 않는 경우가 많다. 사람이 보기 좋은 출력값으로는 Roll, Pitch, Yaw를 사용하되, 내부 계산은 회전 행렬이나 쿼터니언을 사용하는 식이다.
쿼터니언은 왜 등장했을까
쿼터니언은 3D 회전을 표현하는 또 다른 방법이다. 이름부터 조금 낯설고, 처음 보면 수식도 친절하지 않다. 하지만 로봇공학에서 쿼터니언은 매우 자주 등장한다.
쿼터니언은 네 개의 숫자로 회전을 표현한다.
q = [w, x, y, z]여기서 w는 스칼라 부분, x, y, z는 벡터 부분이라고 부른다. 단위 쿼터니언은 길이가 1인 쿼터니언이며, 3D 회전을 안정적으로 표현하는 데 사용된다.
가장 직관적인 출발점은 축-각 표현이다. 어떤 축을 중심으로 몇 도 회전하는지로 3D 회전을 표현할 수 있다. 축 벡터를 axis, 회전 각도를 angle이라고 하면 쿼터니언은 다음처럼 만들 수 있다.
def quat_from_axis_angle(axis, angle):
axis = np.asarray(axis, dtype=float)
axis = axis / np.linalg.norm(axis)
half = angle / 2.0
w = np.cos(half)
xyz = axis * np.sin(half)
return np.array([w, xyz[0], xyz[1], xyz[2]])예를 들어 z축 주변으로 90도 회전하는 쿼터니언은 다음과 같다.
q = quat_from_axis_angle([0, 0, 1], np.deg2rad(90))
print(q)쿼터니언은 네 개의 숫자를 쓰지만, 아무 네 숫자나 가능한 것은 아니다. 회전을 표현하는 쿼터니언은 보통 길이가 1이 되도록 정규화해서 사용한다.
def normalize_quat(q):
return q / np.linalg.norm(q)쿼터니언을 회전 행렬로 바꾸기
쿼터니언은 계산에 좋지만, 점을 직접 회전시키거나 좌표축을 그릴 때는 회전 행렬이 편하다. 그래서 쿼터니언과 회전 행렬 사이를 오가는 일이 많다.
다음 함수는 단위 쿼터니언을 회전 행렬로 바꾼다.
def quat_to_matrix(q):
q = normalize_quat(q)
w, x, y, z = q
return np.array([
[1 - 2*(y*y + z*z), 2*(x*y - z*w), 2*(x*z + y*w)],
[ 2*(x*y + z*w), 1 - 2*(x*x + z*z), 2*(y*z - x*w)],
[ 2*(x*z - y*w), 2*(y*z + x*w), 1 - 2*(x*x + y*y)]
])확인해 보자.
q = quat_from_axis_angle([0, 0, 1], np.deg2rad(90))
R_from_q = quat_to_matrix(q)
R_from_z = rot_z(np.deg2rad(90))
print(R_from_q)
print(R_from_z)
print(np.allclose(R_from_q, R_from_z))두 행렬은 같은 회전을 나타낸다. 즉, 쿼터니언은 회전 행렬과 경쟁하는 완전히 다른 세계라기보다, 같은 회전을 다른 방식으로 저장하는 표현이라고 볼 수 있다.
쿼터니언이 특히 유용한 순간
쿼터니언이 자주 쓰이는 이유는 단순히 멋있어 보여서가 아니다. 실제로 3D 회전을 다루다 보면 쿼터니언이 편한 순간이 많다.
짐벌락 문제를 피하면서 자세를 표현할 수 있다.
회전 행렬보다 적은 숫자 4개로 자세를 저장할 수 있다.
두 자세 사이를 부드럽게 보간하기 좋다.
여러 회전을 누적할 때 수치적으로 다루기 좋다.
여기서 "보간"은 중간 자세를 자연스럽게 만드는 것을 뜻한다. 예를 들어 드론 카메라가 한 방향에서 다른 방향으로 부드럽게 돌아가야 한다고 해 보자. 시작 자세와 끝 자세만 알고 있을 때, 중간 프레임의 자세를 어떻게 만들 것인가?
Roll, Pitch, Yaw를 각각 직선적으로 섞으면 중간에 이상한 회전이 생길 수 있다. 특히 각도가 180도 근처를 지나거나 짐벌락 근처를 지날 때 문제가 더 눈에 띈다. 쿼터니언은 이런 회전 보간을 더 자연스럽게 처리하는 데 자주 사용된다.
쿼터니언 보간 중 대표적인 방식이 SLERP다. SLERP는 구면 선형 보간이라는 뜻인데, 이름은 어렵지만 감각은 단순하다. 회전을 평평한 직선 위에서 섞는 것이 아니라, 회전들이 놓인 둥근 공간 위에서 일정한 속도로 움직이듯 섞는 방법이다.
맛보기로 간단한 SLERP 함수를 작성해 보자.
def slerp(q0, q1, t):
q0 = normalize_quat(q0)
q1 = normalize_quat(q1)
dot = np.dot(q0, q1)
# q와 -q는 같은 회전을 나타낸다.
# 더 짧은 경로로 보간하기 위해 부호를 맞춘다.
if dot < 0.0:
q1 = -q1
dot = -dot
dot = np.clip(dot, -1.0, 1.0)
# 두 쿼터니언이 거의 같으면 단순 선형 보간으로 충분하다.
if dot > 0.9995:
return normalize_quat((1 - t) * q0 + t * q1)
omega = np.arccos(dot)
sin_omega = np.sin(omega)
a = np.sin((1 - t) * omega) / sin_omega
b = np.sin(t * omega) / sin_omega
return a * q0 + b * q1시작 자세를 "회전 없음", 끝 자세를 "z축 주변 120도 회전"으로 두고 중간 쿼터니언을 만들어 보자.
q_start = np.array([1.0, 0.0, 0.0, 0.0])
q_end = quat_from_axis_angle([0, 0, 1], np.deg2rad(120))
for t in np.linspace(0, 1, 5):
q_mid = slerp(q_start, q_end, t)
R_mid = quat_to_matrix(q_mid)
print("t =", round(t, 2))
print(R_mid)지금은 결과 행렬이 줄줄이 출력될 뿐이지만, 이 행렬들로 좌표축을 그리면 시작 자세에서 끝 자세까지 부드럽게 돌아가는 모습을 볼 수 있다.
쿼터니언도 마법은 아니다
쿼터니언은 강력하지만, 처음 배울 때는 오히려 Roll, Pitch, Yaw보다 덜 직관적이다. q = [0.707, 0, 0, 0.707]이라는 숫자를 보고 곧바로 "아, z축으로 90도 회전이구나" 하고 느끼기는 어렵다.
또한 쿼터니언에는 주의할 점도 있다. q와 -q가 같은 회전을 나타낸다. 그리고 계산 중 길이가 1에서 조금씩 벗어날 수 있으므로 정규화가 필요하다. 회전 순서를 합성할 때도 쿼터니언 곱의 순서를 신경 써야 한다.
따라서 실전에서는 보통 다음처럼 역할을 나누어 사용한다. 사람이 읽고 설정하기 쉬운 값은 Roll, Pitch, Yaw로 표현한다. 좌표 변환이나 점 회전에는 회전 행렬을 사용한다. 자세 누적, 보간, 센서 융합처럼 안정성이 중요한 내부 계산에는 쿼터니언을 사용한다.
서로 경쟁하는 도구라기보다, 상황에 맞게 꺼내 쓰는 도구 상자라고 생각하면 된다.
작은 드론 자세 시뮬레이션
이번 장에서 배운 내용을 작은 예제로 묶어 보자. 드론의 몸체 좌표계를 만들고, Roll, Pitch, Yaw를 바꾸며 드론이 어떻게 기울어지는지 확인한다.
import numpy as np
import matplotlib.pyplot as plt
roll = np.deg2rad(20)
pitch = np.deg2rad(-15)
yaw = np.deg2rad(45)
R_body = rpy_to_matrix(roll, pitch, yaw)
fig = plt.figure(figsize=(7, 7))
ax = fig.add_subplot(111, projection="3d")
draw_frame(ax, np.eye(3), length=1.0, label="world_")
draw_frame(ax, R_body, length=1.0, label="drone_")
set_equal_3d(ax, limit=1.5)
ax.set_title("Drone attitude with Roll, Pitch, Yaw")
plt.show()여기서 drone_x는 드론의 앞쪽, drone_y는 왼쪽, drone_z는 위쪽이라고 생각할 수 있다. Roll 값을 바꾸면 드론이 좌우로 기울어진다. Pitch 값을 바꾸면 앞뒤로 숙이거나 젖혀진다. Yaw 값을 바꾸면 수평면에서 바라보는 방향이 바뀐다.
값을 직접 바꿔 보자.
experiments = [
(0, 0, 0),
(30, 0, 0),
(0, 30, 0),
(0, 0, 30),
(30, 30, 30),
]
for r_deg, p_deg, y_deg in experiments:
R = rpy_to_matrix(
np.deg2rad(r_deg),
np.deg2rad(p_deg),
np.deg2rad(y_deg)
)
print(f"roll={r_deg}, pitch={p_deg}, yaw={y_deg}")
print(R)
print()처음에는 행렬의 숫자가 낯설다. 하지만 같은 값을 그림으로 반복해서 보면 조금씩 연결된다. Roll을 바꿨을 때 어떤 축이 움직이는지, Pitch를 바꿨을 때 앞쪽 축이 어디로 향하는지, Yaw를 바꿨을 때 2D 회전과 어떤 점이 비슷한지 관찰해 보자.
이런 감각이 쌓이면 나중에 카메라 좌표계, 로봇 팔의 손끝 좌표계, IMU 센서의 자세 데이터를 볼 때 훨씬 덜 막막해진다.
이번 장에서 기억할 것
3D 회전은 처음 배울 때 복잡하게 느껴진다. 하지만 핵심은 분명하다. 2D에서는 회전축이 하나였고, 3D에서는 회전축이 세 개가 되었다. Roll은 x축, Pitch는 y축, Yaw는 z축 주변 회전이다.
각 축 회전은 3×3 행렬로 표현할 수 있다. 이 행렬들을 곱하면 최종 자세를 나타내는 회전 행렬을 만들 수 있다. 다만 3D 회전에서는 순서가 중요하다. 같은 각도라도 어떤 순서로 회전하느냐에 따라 결과가 달라진다.
오일러 각, 즉 Roll/Pitch/Yaw 표현은 사람이 이해하기 쉽지만 짐벌락 문제가 생길 수 있다. 그래서 실전 로봇 시스템에서는 회전 행렬과 쿼터니언을 함께 사용한다. 쿼터니언은 직관적으로 읽기는 어렵지만, 3D 자세를 안정적으로 표현하고 부드럽게 보간하는 데 강력하다.
다음 장에서는 이 3D 회전 감각을 실제 로봇 문제로 가져간다. 카메라가 본 물체의 위치를 로봇 기준으로 바꾸려면 어떻게 해야 할까? 로봇 손끝에 붙은 카메라와 로봇 베이스 사이의 관계는 어떻게 표현할까? 이제 우리는 그 질문에 답할 준비를 하고 있다.
이 노트는 보도·비평·교육·연구 목적으로 작성되었습니다. 저작권 문의가 있으시면 에서 알려주세요.