회전 표현 - 각도·회전행렬
이 장에서 배우는 것
앞 장에서는 로봇의 위치와 이동을 벡터로 표현하는 방법을 다뤘다. 위치만으로는 로봇을 완전히 설명할 수 없다. 로봇이 어느 방향을 보고 있는지, 즉 자세(orientation)도 함께 알아야 목표 지점으로 방향을 트는 명령을 계산할 수 있다. 이 장에서는 평면 위 회전을 행렬로 표현하는 방법과, 각도를 다룰 때 반드시 마주치는 "각도 넘침" 문제를 해결하는 정규화 기법을 배운다. 마지막으로 3차원 회전을 오일러각으로 표현할 때 생기는 함정도 짧게 살펴본다.
- 2D 회전행렬의 구조와 성질(직교성, 행렬식, 역행렬)을 설명할 수 있다
- 각도를 -π~π(-180°~180°) 범위로 정규화하는 이유와 구현 방법을 안다
- 목표각과 현재각의 차이에서 최단 회전 방향을 구할 수 있다
- 오일러각으로 3D 회전을 표현할 때 생기는 짐벌락(gimbal lock)을 코드로 재현하고 설명할 수 있다
문제 상황
차동 구동 로봇을 제자리에서 여러 번 회전시키는 코드를 작성한다고 하자. 회전 명령을 내릴 때마다 각도를 계속 더해 헤딩(heading, 로봇이 바라보는 방향)을 갱신하면, 몇 번만 돌아도 값이 360도를 훌쩍 넘는다. 이 값을 그대로 목표각과 비교하거나 제어 명령을 계산하는 데 쓰면 문제가 생긴다. 예를 들어 현재 헤딩이 340도(사실상 -20도와 같은 방향)이고 목표가 10도라면, 단순히 뺄셈만 한 "목표 - 현재 = -330도"는 로봇이 거의 한 바퀴를 돌아야 한다고 말하지만, 실제로는 30도만 돌면 충분하다.
3차원으로 넘어가면 문제가 하나 더 생긴다. IMU(관성 측정 장치) 같은 센서는 자세를 롤(roll)·피치(pitch)·요(yaw) 세 각도, 즉 오일러각으로 알려준다. 이 표현은 직관적이지만 특정 각도 부근에서 두 회전축이 같은 효과를 내는 지점이 있다. 로봇이 경사로에서 크게 기울어 피치가 90도에 가까워지면, 롤과 요를 구분해 쓰던 제어 코드가 갑자기 이상하게 동작할 수 있다. 이 장에서는 이 두 문제, 즉 각도 정규화와 오일러각의 함정을 코드로 직접 확인한다.
2D 회전행렬
로봇 몸체 좌표계는 앞 장의 약속을 그대로 따른다. x축은 로봇 전방, y축은 로봇 좌측, 양의 각도는 z축(위쪽) 기준 오른손 법칙에 따라 반시계 방향이다. 이 좌표계에서 벡터 (x, y)를 각도 θ만큼 반시계 방향으로 회전시키는 행렬은 다음과 같다.
R(θ) = [[cosθ, -sinθ], [sinθ, cosθ]]
이 식은 단위원 위의 점 (1, 0)이 각도 θ만큼 돌면 (cosθ, sinθ)로 이동한다는 사실에서 바로 나온다. 임의의 벡터 v = (x, y)를 R(θ)와 곱하면 v를 x축 성분과 y축 성분으로 나눈 뒤 각각 회전시켜 다시 합친 결과가 나온다. 회전행렬은 다음 네 가지 성질을 항상 만족하며, 이 성질은 코드를 검증할 때 유용하다.
| 성질 | 수식 | 로봇 코드에서의 의미 |
|---|---|---|
| 직교행렬 | R^T R = I | 회전은 벡터의 길이를 바꾸지 않는다 |
| 행렬식 1 | det(R) = 1 | 반사(뒤집힘) 없이 순수 회전만 한다 |
| 역행렬 | R(θ)^-1 = R(-θ) = R(θ)^T | 반대로 돌릴 때 전치만 하면 된다 |
| 합성 | R(θ1)R(θ2) = R(θ1+θ2) | 연속 회전은 각도를 더한 것과 같다 |
역행렬 성질이 특히 실용적이다. 로봇을 θ만큼 돌린 결과를 원래대로 되돌리고 싶을 때, 새로 삼각함수를 계산해 R(-θ)를 만들 필요 없이 이미 가진 행렬을 전치(행과 열을 바꾸는 것)하기만 하면 된다.
각도 정규화 (-π ~ π)
각도는 원 위의 위치와 같아서 370도와 10도는 같은 방향을 가리킨다. 그런데 회전을 계속 누적하는 코드는 이 사실을 자동으로 반영하지 않는다. 헤딩을 계속 더하기만 하면 값은 한없이 커지거나 작아진다. 이 값을 사람이 이해하기 쉽고 비교하기 쉬운 범위, 보통 -180도~180도(라디안으로는 -π~π)로 접어 넣는 과정을 각도 정규화라 부른다.
정규화 공식은 다음과 같다.
normalize(θ) = (θ + π) mod 2π - π
동작 원리는 이렇다. θ에 π를 더하면 기준점이 이동해 mod 2π 연산이 [0, 2π) 범위로 값을 접는다. 그 뒤 다시 π를 빼면 결과가 (-π, π] 범위 안에 들어온다. 이 공식이 성립하려면 나머지 연산이 나누는 수(2π)와 같은 부호를 돌려줘야 한다. 파이썬과 numpy의 % 연산자는 이런 성질(플로어 나머지, floor modulo)을 가지고 있어서 음수 각도에도 이 공식이 그대로 통한다. C나 C++의 fmod는 부호가 피제수를 따르는 다른 규칙을 쓰므로 같은 식을 그대로 옮기면 틀린다.
| 원시 각도(도) | 정규화 각도(도) | 설명 |
|---|---|---|
| 190 | -170 | 180을 살짝 넘으면 음수 쪽으로 접힌다 |
| 400 | 40 | 360을 한 번 빼면 범위 안에 들어온다 |
| 650 | -70 | 360을 두 번 빼야 범위 안에 들어온다 |
| 340 | -20 | 큰 양수도 실제로는 짧은 음의 회전이다 |
목표각과 현재각의 차이를 구할 때는 순서가 중요하다. 먼저 단순히 빼고, 그 결과를 정규화해야 최단 회전 방향과 회전량이 함께 나온다. 정규화를 먼저 각 각도에 따로 적용한 뒤 빼면 경계값 근처에서 최단 경로가 아닌 결과가 나올 수 있다.
3D 회전 맛보기 - 오일러각의 함정
3차원에서는 회전축이 x, y, z 세 개로 늘어난다. 각 축에 대한 회전행렬을 따로 정의한 뒤 순서대로 곱하면 임의의 3D 회전을 표현할 수 있다. 이 장에서는 로봇 자세를 다룰 때 흔히 쓰는 순서인 요-피치-롤(yaw-pitch-roll, ZYX 순서)을 기준으로 삼는다.
R = Rz(yaw) · Ry(pitch) · Rx(roll)
여기서 곱셈 순서를 바꾸면 완전히 다른 회전이 나온다는 점을 기억해야 한다. 행렬 곱셈은 교환법칙이 성립하지 않기 때문이다. 문제는 순서가 아니라 이 표현 자체에도 있다. 피치가 정확히 90도가 되면, 요 회전과 롤 회전이 같은 축을 회전시키는 효과를 내면서 두 각도를 서로 구분할 수 없게 된다. 이 현상을 짐벌락이라 부른다. 이름은 물리적인 짐벌(gimbal, 회전 고리) 장치에서 왔지만, 소프트웨어에서는 세 각도로 3D 회전을 표현하는 방식 자체가 가진 수학적 특이점이다.
구체적으로, 피치=90도일 때 z축 방향 벡터를 이 회전행렬에 넣으면 결과는 요와 롤의 차이(yaw - roll)에만 의존한다. 즉 (yaw=30°, roll=0°)와 (yaw=60°, roll=30°)처럼 서로 다른 두 조합이 완전히 같은 자세를 만든다. 자세 추정 코드가 이 값을 각각 따로 적분하거나 미분하면, 특이점 근처에서 작은 잡음에도 값이 크게 튀는 현상이 나타난다. 실무에서는 쿼터니언(quaternion)이나 회전행렬 자체를 저장·보간하는 방식으로 이 문제를 피하지만, 그 내용은 이 책의 범위를 넘어간다. 이 장에서는 짐벌락이 왜 일어나는지 직접 확인하는 것으로 충분하다.
완성 코드
다음 코드를 rotation_demo.py로 저장한다. numpy만 있으면 실행된다.
import numpy as np
def rot2d(theta):
c, s = np.cos(theta), np.sin(theta)
return np.array([[c, -s],
[s, c]])
def normalize_angle(theta):
return (theta + np.pi) % (2 * np.pi) - np.pi
def rot3d_zyx(yaw, pitch, roll):
cz, sz = np.cos(yaw), np.sin(yaw)
cy, sy = np.cos(pitch), np.sin(pitch)
cx, sx = np.cos(roll), np.sin(roll)
rz = np.array([[cz, -sz, 0.0],
[sz, cz, 0.0],
[0.0, 0.0, 1.0]])
ry = np.array([[cy, 0.0, sy],
[0.0, 1.0, 0.0],
[-sy, 0.0, cy]])
rx = np.array([[1.0, 0.0, 0.0],
[0.0, cx, -sx],
[0.0, sx, cx]])
return rz @ ry @ rx
def main():
heading = 0.0
faulty_turn = np.radians(130.0)
for _ in range(5):
heading += faulty_turn
heading_n = normalize_angle(heading)
print("[1] 누적 헤딩 정규화")
print(f" 원시 누적각: {np.degrees(heading):.1f}도")
print(f" 정규화 후: {np.degrees(heading_n):.1f}도")
target = np.radians(170.0)
current = np.radians(-170.0)
raw_diff = target - current
turn_cmd = normalize_angle(raw_diff)
print("[2] 목표-현재 각도 차이")
print(f" 단순 뺄셈: {np.degrees(raw_diff):.1f}도")
print(f" 정규화 후: {np.degrees(turn_cmd):.1f}도")
forward = np.array([1.0, 0.0])
rotated = rot2d(turn_cmd) @ forward
print(f" 회전행렬 적용 결과: ({rotated[0]:.3f}, {rotated[1]:.3f})")
v = np.array([0.0, 0.0, 1.0])
case_a = rot3d_zyx(np.radians(30.0), np.radians(90.0), np.radians(0.0)) @ v
case_b = rot3d_zyx(np.radians(60.0), np.radians(90.0), np.radians(30.0)) @ v
print("[3] 짐벌락 재현 (피치=90도)")
print(f" yaw=30, roll=0 결과: ({case_a[0]:.3f}, {case_a[1]:.3f}, {case_a[2]:.3f})")
print(f" yaw=60, roll=30 결과: ({case_b[0]:.3f}, {case_b[1]:.3f}, {case_b[2]:.3f})")
if __name__ == "__main__":
main()
줄별 해설
rot2d는 각도 θ 하나를 받아 2×2 회전행렬을 만든다. c, s = np.cos(theta), np.sin(theta)처럼 코사인과 사인을 한 번씩만 계산해 재사용한다.
normalize_angle은 앞서 설명한 (θ+π) mod 2π - π 공식을 그대로 옮긴 것이다. 파이썬의 %가 플로어 나머지이므로 음수 θ에도 그대로 동작한다.
rot3d_zyx는 세 축의 회전행렬 rz, ry, rx를 각각 만든 뒤 rz @ ry @ rx 순서로 곱한다. 이 순서가 요-피치-롤 표현의 정의이며, 순서를 바꾸면 다른 표현이 된다.
[1] 블록은 130도씩 5번 회전 명령이 누적된 상황을 흉내낸다. 원시 누적각은 650도까지 커지지만, normalize_angle을 거치면 -70도라는 실제 방향이 드러난다.
[2] 블록은 목표각 170도와 현재각 -170도의 차이를 구한다. 단순 뺄셈은 340도라는 큰 값을 주지만, 정규화하면 -20도, 즉 반대 방향으로 20도만 돌면 된다는 사실을 알 수 있다. 이 정규화된 각도를 rot2d에 넣어 전방 벡터 (1, 0)을 실제로 회전시켜 검증한다.
[3] 블록은 피치를 90도로 고정한 채 (yaw=30°, roll=0°)와 (yaw=60°, roll=30°) 두 조합으로 z축 벡터를 회전시킨다. 두 조합의 요-롤 차이가 똑같이 30도이므로 결과 벡터가 완전히 같게 나오며, 이것이 짐벌락의 구체적인 모습이다.
실행 결과
$ python3 rotation_demo.py
[1] 누적 헤딩 정규화
원시 누적각: 650.0도
정규화 후: -70.0도
[2] 목표-현재 각도 차이
단순 뺄셈: 340.0도
정규화 후: -20.0도
회전행렬 적용 결과: (0.940, -0.342)
[3] 짐벌락 재현 (피치=90도)
yaw=30, roll=0 결과: (0.866, 0.500, 0.000)
yaw=60, roll=30 결과: (0.866, 0.500, 0.000)
실무에서 자주 틀리는 것
각도를 정규화하지 않고 그대로 비교한다
누적된 헤딩을 정규화 없이 임계값과 비교하면, 값이 360도를 넘는 순간부터 비교 결과가 틀어진다.
heading = 0.0
for cmd in turn_log:
heading += cmd
if heading > threshold:
stop()
heading = 0.0
for cmd in turn_log:
heading += cmd
heading = normalize_angle(heading)
if heading > threshold:
stop()
각도 차이의 부호를 버린다
abs()로 오차 크기만 남기면 회전 방향 정보가 사라져, 로봇이 항상 한쪽으로만 돌게 된다.
error = abs(target - current)
turn_speed = error * k
error = normalize_angle(target - current)
turn_speed = error * k
3D 회전행렬의 곱셈 순서를 뒤바꾼다
축 회전행렬을 곱하는 순서는 정의의 일부다. 순서를 바꾸면 이름은 같아도 다른 회전이 나온다.
R = Rx @ Ry @ Rz
R = Rz @ Ry @ Rx
짐벌락 근처에서 오일러각을 그대로 미분한다
피치가 90도 근처일 때 롤·요 각도를 그대로 시간 미분해 각속도를 구하면, 작은 잡음에도 값이 크게 튄다.
roll_rate = (roll_now - roll_prev) / dt
pitch_rate = (pitch_now - pitch_prev) / dt
if abs(abs(pitch_now) - np.pi / 2) < np.radians(5):
use_fallback_estimate()
else:
roll_rate = (roll_now - roll_prev) / dt
한눈에 보기
| 개념 | 핵심 코드 | 성질/값 | 주의점 |
|---|---|---|---|
| 2D 회전행렬 | rot2d(theta) | R^T R = I, det(R) = 1 | 각도 부호는 반시계 방향 기준 |
| 각도 정규화 | normalize_angle(theta) | 결과 범위 (-π, π] | 파이썬의 floor-mod에 의존한 식이다 |
| 최단 회전 명령 | normalize_angle(target - current) | 부호=방향, 크기=회전량 | 뺄셈 먼저, 정규화는 그 다음 |
| 3D 오일러각(ZYX) | rot3d_zyx(yaw, pitch, roll) | Rz @ Ry @ Rx 순서 고정 | 피치=±90°에서 짐벌락 발생 |
연습 문제
normalize_angle공식을 손으로 적용해 -400도와 725도를 각각 -180도~180도 범위로 정규화하라.- 회전행렬이 R(θ)^-1 = R(θ)^T를 만족하는 이유를 R(θ)의 성분을 이용해 설명하라. 이 성질이 코드에서 왜 유용한지도 함께 적어라.
- 완성 코드의
[3]블록에서 피치를 89.9도로 바꾸면 case_a와 case_b 결과가 여전히 거의 같은 이유, 피치를 45도로 바꾸면 두 결과가 뚜렷이 달라지는 이유를 설명하라. - 목표각 -175도, 현재각 175도 사이의 최단 회전각을
normalize_angle로 구하는 코드를 작성하고 그 값을 계산하라.
정답과 해설
- -400도: (-400+180)=-220, -220을 360으로 플로어 나머지 연산하면 140, 140-180=-40이므로 정규화 결과는 -40도다. 725도: (725+180)=905, 905를 360으로 나눈 나머지는 185, 185-180=5이므로 정규화 결과는 5도다.
- R(θ)=[[cosθ,-sinθ],[sinθ,cosθ]]의 전치는 [[cosθ,sinθ],[-sinθ,cosθ]]이다. cos(-θ)=cosθ, sin(-θ)=-sinθ이므로 이 전치행렬은 정확히 R(-θ)와 같다. 회전행렬은 열벡터가 서로 직교하는 단위벡터(직교행렬)이므로 일반적으로 R^T R = I가 성립하며, 이는 R^-1 = R^T와 같은 말이다. 코드에서는 역회전이 필요할 때 삼각함수를 다시 계산하지 않고 행과 열만 바꾸면 되므로 계산이 가볍다.
- 본문에서 유도했듯이 피치=90도일 때 결과는 (yaw-roll)에만 의존한다. 피치=89.9도에서는 cos(89.9°)가 약 0.0017로 0에 매우 가깝지만 완전히 0은 아니어서, 이론적으로는 yaw와 roll을 구분할 수 있지만 그 구분이 거의 0에 수렴하는 항에 실려 있어 잡음에 극도로 민감하다(근사적 짐벌락). 피치=45도에서는 cos(45°)가 약 0.707로 충분히 커서 yaw와 roll이 서로 다른 축에 뚜렷한 영향을 주므로 case_a와 case_b가 분명히 달라진다.
diff = normalize_angle(np.radians(-175.0) - np.radians(175.0))를 계산하면 raw_diff는 -350도이고, 정규화하면 (-350+180)=-170, -170을 360으로 플로어 나머지 연산하면 190, 190-180=10이므로 결과는 10도다. 즉 -175도에서 175도로 가려면 10도만 돌면 된다.