순기구학 — 관절 각도에서 손끝 위치로, 동차변환행렬과 DH 파라미터

들어가며 — 각도를 넣으면 손끝이 어디인지 알려 주는 함수
좌표계, 회전행렬, 그리고 평행이동
동차변환행렬이 왜 4×4인가
프레임을 연쇄하면 왜 곱셈이 되는가
2링크 팔의 순기구학을 손으로 유도하기
DH 파라미터 — 링크 하나를 네 숫자로 적기
회전 표현의 트레이드오프 — 오일러각, 회전행렬, 쿼터니언
부호 하나가 틀리면 실제로 무슨 일이 생기는가
마치며 — 좌표계를 정확히 적어 두는 것이 절반입니다

들어가며 — 각도를 넣으면 손끝이 어디인지 알려 주는 함수

로봇 팔의 구조를 정하고 서보를 달았다고 하겠습니다. 이제 각 관절의 각도를 읽을 수 있습니다. 어깨 30도, 팔꿈치 45도.

그러면 손끝은 어디에 있습니까.

이 질문의 답을 구하는 것이 순기구학입니다. 그리고 로봇 팔에서 하는 모든 계산의 출발점입니다. 역기구학도, 궤적 계획도, 충돌 검사도, 카메라가 본 물체를 팔의 좌표로 옮기는 일도 전부 이 계산 위에 쌓입니다.

다행히 순기구학은 쉽습니다. 각도를 넣으면 위치가 하나 나오고, 언제나 나오고, 곱셈 몇 번이면 끝납니다. 관절이 여섯 개든 열 개든 원리가 같습니다.

문제는 링크가 늘어날 때 삼각함수 식이 감당할 수 없게 길어진다는 것입니다. 2링크는 손으로 쓸 수 있고 3링크도 어찌어찌 되지만, 축이 3차원에서 서로 다른 방향을 보는 6링크는 손으로 못 씁니다. 그래서 사람이 쓸 수 있는 표기법이 필요하고, 그것이 동차변환행렬과 DH 파라미터입니다.

이 글은 그 두 도구가 왜 그런 모양인지를 숫자로 설명합니다.

좌표계, 회전행렬, 그리고 평행이동

로봇 팔에서 좌표계는 링크마다 하나씩 붙습니다. 어깨에 하나, 위팔에 하나, 아래팔에 하나, 손끝에 하나. 각 좌표계는 자기 링크에 고정되어 함께 움직입니다.

이렇게 하는 이유는 각 링크 안에서는 모든 것이 상수이기 때문입니다. 손끝이 손목 좌표계에서 어디에 있는지는 관절이 어떻게 움직이든 변하지 않습니다. 변하는 것은 좌표계들 사이의 관계뿐입니다. 그래서 문제가 "복잡한 형상"에서 "좌표계 사이의 변환 몇 개"로 줄어듭니다.

좌표계 사이의 관계는 두 가지로 이루어집니다. 얼마나 돌아가 있는가, 그리고 얼마나 떨어져 있는가.

회전은 행렬입니다. 2차원에서 반시계로 θ만큼 도는 회전은,

R(θ) = [ cos θ   -sin θ ]
       [ sin θ    cos θ ]

3차원에서 z축 둘레의 회전은,

Rz(θ) = [ cos θ   -sin θ   0 ]
        [ sin θ    cos θ   0 ]
        [   0        0     1 ]

이 행렬의 열을 하나씩 읽으면 의미가 분명해집니다. 첫 번째 열은 회전된 좌표계의 x축이 원래 좌표계에서 어느 방향을 가리키는지이고, 두 번째 열은 y축, 세 번째 열은 z축입니다. 회전행렬은 세 축의 방향을 나란히 적어 놓은 것입니다.

평행이동은 벡터의 덧셈입니다. 좌표계가 t만큼 옮겨져 있으면 점의 좌표에 t를 더합니다.

여기에 불편한 점이 있습니다. 회전은 곱셈이고 평행이동은 덧셈입니다. 둘을 함께 적으면,

p_world = R · p_local + t

이 형태로는 여러 단계를 연쇄할 때 식이 지저분해집니다. 세 단계를 이으면,

p = R1·(R2·(R3·p + t3) + t2) + t1
  = R1·R2·R3·p + R1·R2·t3 + R1·t2 + t1

관절이 여섯 개면 항이 여섯 개로 늘어나고, 각 항마다 앞선 회전들의 곱이 붙습니다. 손으로 다루기 어렵고 코드로 옮기기도 번거롭습니다.

동차변환행렬이 왜 4×4인가

해법은 의외로 단순합니다. 좌표에 1을 하나 덧붙입니다.

3차원 점 (x, y, z)를 (x, y, z, 1)로 씁니다. 그리고 4×4 행렬을 이렇게 만듭니다.

T = [ R11  R12  R13  tx ]
    [ R21  R22  R23  ty ]
    [ R31  R32  R33  tz ]
    [  0    0    0    1 ]

왼쪽 위 3×3이 회전행렬, 오른쪽 위 3×1이 평행이동, 아래 한 줄은 항상 0 0 0 1입니다.

이 행렬에 (x, y, z, 1)을 곱하면 무슨 일이 일어나는지 첫 행만 계산해 보겠습니다.

R11·x + R12·y + R13·z + tx·1

회전의 첫 행에 평행이동의 첫 성분이 더해졌습니다. 정확히 R·p + t의 첫 성분입니다. 마지막 행은 0·x + 0·y + 0·z + 1·1 = 1이라서 덧붙인 1이 그대로 유지됩니다.

곱해지는 대상에 상수 1이 들어 있기 때문에, 그 열의 성분들이 덧셈처럼 동작합니다. 이것이 4×4의 전부입니다. 마지막 행의 0 0 0 1은 결과가 여전히 유효한 점이 되도록 유지해 주는 장치입니다.

숫자로 확인해 보겠습니다. 프레임 2가 프레임 1에 대해 z축 둘레로 45도 돌아 있고 x 방향으로 0.20미터 떨어져 있습니다. 프레임 2에서 손끝은 자기 x축 위 0.15미터 지점에 있습니다.

import numpy as np
np.set_printoptions(precision=6, suppress=True)


def transform_z(theta, tx):
    """z축 둘레로 theta만큼 돌린 뒤 x축으로 tx만큼 옮기는 4×4 동차변환입니다."""
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s, 0, tx],
                     [s,  c, 0, 0],
                     [0,  0, 1, 0],
                     [0,  0, 0, 1]])


T = transform_z(np.radians(45), 0.20)
print("T (프레임2 -> 프레임1):")
print(T)

p_local = np.array([0.15, 0.0, 0.0, 1.0])     # 프레임2에서 본 손끝
p_world = T @ p_local
print("프레임2에서의 손끝:", p_local[:3])
print("프레임1에서의 손끝:", np.round(p_world[:3], 6))
print("손으로 구한 값     :", np.round([0.20 + 0.15*np.cos(np.radians(45)),
                                        0.15*np.sin(np.radians(45)), 0.0], 6))

실행 결과입니다.

T (프레임2 -> 프레임1):
[[ 0.707107 -0.707107  0.        0.2     ]
 [ 0.707107  0.707107  0.        0.      ]
 [ 0.        0.        1.        0.      ]
 [ 0.        0.        0.        1.      ]]
프레임2에서의 손끝: [0.15 0.   0.  ]
프레임1에서의 손끝: [0.306066 0.106066 0.      ]
손으로 구한 값     : [0.306066 0.106066 0.      ]

손으로 계산하면,

x = 0.20 + 0.15 × cos(45도) = 0.20 + 0.106066 = 0.306066
y =  0   + 0.15 × sin(45도) =  0   + 0.106066 = 0.106066

행렬이 준 값과 같습니다.

동차변환행렬에는 편리한 성질이 하나 더 있습니다. 역변환이 값싸다는 것입니다. 일반적인 4×4 행렬의 역행렬은 계산이 무겁지만, 동차변환행렬은 구조를 이용해 이렇게 쓸 수 있습니다.

T⁻¹ = [ Rᵀ   -Rᵀ·t ]
      [ 0      1   ]

회전행렬의 역은 전치이므로(직교행렬이므로) 곱셈 몇 번이면 됩니다. 손끝 좌표계에서 본 물체의 위치를 베이스 좌표계로 옮기는 것과 그 반대가 모두 값싸다는 뜻이고, 카메라가 손목에 달린 팔에서 매 프레임 하는 계산이 정확히 이것입니다.

프레임을 연쇄하면 왜 곱셈이 되는가

이제 왜 이 표현이 로봇 팔에 딱 맞는지가 드러납니다.

베이스에서 링크 1로 가는 변환을 T01, 링크 1에서 링크 2로 가는 변환을 T12라고 하겠습니다. 링크 2 좌표계의 점 p를 베이스 좌표계로 옮기려면,

p_1 = T12 · p
p_0 = T01 · p_1 = T01 · T12 · p

변환을 이어 붙이는 것이 행렬을 곱하는 것과 정확히 같습니다. 앞에서 본 지저분한 전개식이 사라졌습니다.

관절이 여섯 개면,

T06 = T01 · T12 · T23 · T34 · T45 · T56

이것이 순기구학의 전부입니다. 각 변환에 관절 각도가 하나씩 들어 있고, 곱하면 손끝의 위치와 자세가 나옵니다. 위치는 결과 행렬의 오른쪽 위 3×1이고, 자세는 왼쪽 위 3×3입니다.

곱셈의 순서에 주의해야 합니다. 행렬 곱은 교환법칙이 성립하지 않으므로 T01·T12와 T12·T01은 다릅니다. 순서를 기억하는 요령은 첨자를 보는 것입니다. 안쪽 첨자가 맞물려야 합니다. T01·T12에서 1과 1이 맞닿고, 남는 것이 0과 2라서 결과는 T02입니다.

2링크 팔의 순기구학을 손으로 유도하기

행렬을 쓰기 전에 가장 단순한 경우를 삼각함수로 풀어 두면 나중에 검산이 됩니다.

평면 위에 링크 두 개가 있습니다. 위팔 L1 = 0.20 m, 아래팔 L2 = 0.15 m. 어깨 각도 θ1은 x축에서 잰 절대각이고, 팔꿈치 각도 θ2는 위팔에 대한 상대각입니다.

팔꿈치의 위치부터 구합니다. 어깨에서 θ1 방향으로 L1만큼 갔으므로,

팔꿈치 = (L1·cos θ1, L1·sin θ1)

손끝은 팔꿈치에서 다시 L2만큼 갑니다. 이때 아래팔이 향하는 절대 방향은 θ1 + θ2입니다. 상대각을 절대각으로 바꾸는 이 한 줄이 핵심입니다.

x = L1·cos(θ1) + L2·cos(θ1 + θ2)
y = L1·sin(θ1) + L2·sin(θ1 + θ2)

θ1 = 30도, θ2 = 45도를 넣으면,

x = 0.20 × cos(30도) + 0.15 × cos(75도)
  = 0.20 × 0.8660254 + 0.15 × 0.2588190
  = 0.1732051 + 0.0388229
  = 0.2120279

y = 0.20 × sin(30도) + 0.15 × sin(75도)
  = 0.20 × 0.5000000 + 0.15 × 0.9659258
  = 0.1000000 + 0.1448889
  = 0.2448889

손끝은 (0.212028, 0.244889)입니다.

링크를 하나 더 붙이면 같은 규칙이 반복됩니다. L3 = 0.10 m, θ3 = -60도라면 세 번째 링크의 절대 방향은 30 + 45 - 60 = 15도이고,

x = 0.2120279 + 0.10 × cos(15도) = 0.2120279 + 0.0965926 = 0.3086205
y = 0.2448889 + 0.10 × sin(15도) = 0.2448889 + 0.0258819 = 0.2707708

패턴이 보입니다. 각 링크는 자기 앞까지의 각도 합 방향으로 자기 길이만큼 나아갑니다. 평면 팔이라면 이것으로 충분하고, 실제로 관절 수가 몇이든 같은 식을 씁니다.

그런데 관절 축이 서로 다른 방향을 보는 3차원 팔에서는 이 방식이 무너집니다. 어깨의 첫 축은 수직인데 두 번째 축은 수평이고, 손목의 세 축은 또 서로 직교합니다. 각도를 그냥 더할 수 없습니다. 여기서부터 행렬이 필요합니다.

DH 파라미터 — 링크 하나를 네 숫자로 적기

링크 하나를 잇는 변환에는 원래 여섯 개의 자유도가 필요합니다. 회전 셋에 평행이동 셋. 그런데 로봇의 링크는 아무렇게나 붙어 있지 않습니다. 관절 축이 있고, 링크는 그 축들을 잇습니다. 이 구조를 이용해 좌표계를 규칙에 따라 놓으면 여섯 개가 네 개로 줄어듭니다.

데나빗-하텐베르크 규약이 그 규칙이고, 네 개의 숫자는 이렇습니다.

기호	이름	뜻
`a`	링크 길이	이웃한 두 관절 축 사이의 공통 수선 길이
`α`	링크 비틀림	공통 수선 둘레로 잰 두 관절 축 사이의 각도
`d`	링크 오프셋	관절 축을 따라 잰 두 공통 수선 사이의 거리
`θ`	관절각	관절 축 둘레로 잰 두 공통 수선 사이의 각도

회전 관절에서는 θ가 변수이고 나머지 셋이 상수입니다. 직동 관절에서는 d가 변수이고 나머지 셋이 상수입니다. 관절 하나에 변수 하나라는 대응이 깔끔하게 유지됩니다.

이 네 숫자로 만드는 변환은 네 개의 기본 변환을 이어 붙인 것입니다.

A_i = Rot_z(θ_i) · Trans_z(d_i) · Trans_x(a_i) · Rot_x(α_i)

전개하면 이렇게 됩니다.

A = [ cos θ   -sin θ·cos α    sin θ·sin α    a·cos θ ]
    [ sin θ    cos θ·cos α   -cos θ·sin α    a·sin θ ]
    [   0          sin α          cos α          d   ]
    [   0            0              0            1   ]

여기서 반드시 알아야 할 함정이 있습니다. DH 규약은 하나가 아닙니다.

위의 것은 표준(고전) DH이고, 크레이그의 교과서가 쓰는 수정 DH는 곱의 순서가 다릅니다.

표준 DH:  A_i = Rot_z(θ_i) · Trans_z(d_i) · Trans_x(a_i) · Rot_x(α_i)
수정 DH:  A_i = Rot_x(α_{i-1}) · Trans_x(a_{i-1}) · Rot_z(θ_i) · Trans_z(d_i)

두 규약은 좌표계를 붙이는 위치가 다르고, 그래서 같은 로봇의 파라미터 표가 서로 다릅니다. 수정 DH에서는 a와 α에 i-1 첨자가 붙는데, 이것이 표를 읽을 때 규약을 구분하는 가장 빠른 단서입니다. 논문에서 가져온 표를 표준 DH 코드에 그대로 넣으면 팔이 엉뚱한 모양이 되고, 원인을 찾는 데 며칠이 걸립니다.

참고로 표준 DH의 곱 순서를 Trans_z(d)·Rot_z(θ)·Trans_x(a)·Rot_x(α)로 적은 자료도 흔합니다. 이것은 틀린 것이 아니라 같은 것입니다. Rot_z와 Trans_z는 같은 축을 다루므로 교환 가능하고, Trans_x와 Rot_x도 마찬가지입니다.

3링크 평면 팔의 DH 표를 써 보겠습니다. 모든 관절 축이 평면에 수직으로 나란하므로 비틀림과 오프셋이 전부 0입니다.

i	`a_i`	`θ_i`
1	0.20	변수 `θ1`
2	0.15	변수 `θ2`
3	0.10	변수 `θ3`

코드로 확인합니다.

import numpy as np
np.set_printoptions(precision=6, suppress=True)


def dh_transform(a, alpha, d, theta):
    """표준(고전) DH 규약의 링크 변환 행렬입니다.
    Rot_z(theta) · Trans_z(d) · Trans_x(a) · Rot_x(alpha) 를 전개한 형태입니다."""
    ct, st = np.cos(theta), np.sin(theta)
    ca, sa = np.cos(alpha), np.sin(alpha)
    return np.array([[ct, -st * ca,  st * sa, a * ct],
                     [st,  ct * ca, -ct * sa, a * st],
                     [0.,       sa,       ca,      d],
                     [0.,       0.,       0.,     1.]])


def forward_kinematics(dh_rows, q):
    """DH 표의 각 행은 (a, alpha, d, theta_offset)입니다."""
    T = np.eye(4)
    for (a, alpha, d, offset), theta in zip(dh_rows, q):
        T = T @ dh_transform(a, alpha, d, offset + theta)
    return T


PLANAR_3R = [(0.20, 0.0, 0.0, 0.0),
             (0.15, 0.0, 0.0, 0.0),
             (0.10, 0.0, 0.0, 0.0)]

q = np.radians([30, 45, -60])
T = forward_kinematics(PLANAR_3R, q)
print("DH 연쇄가 준 변환 행렬:")
print(T)

# 손으로 유도한 닫힌 형태와 비교합니다.
a = np.cumsum(q)
x = 0.20 * np.cos(a[0]) + 0.15 * np.cos(a[1]) + 0.10 * np.cos(a[2])
y = 0.20 * np.sin(a[0]) + 0.15 * np.sin(a[1]) + 0.10 * np.sin(a[2])
print(f"닫힌 형태:  x={x:.6f}  y={y:.6f}")
print(f"DH 결과  :  x={T[0,3]:.6f}  y={T[1,3]:.6f}")
print("두 결과가 같은가:", np.allclose([x, y], T[:2, 3]))
print(f"손끝 방향(세 각의 합) = {np.degrees(a[2]):.4f}도, "
      f"행렬에서 읽은 값 = {np.degrees(np.arctan2(T[1,0], T[0,0])):.4f}도")

실행 결과입니다.

DH 연쇄가 준 변환 행렬:
[[ 0.965926 -0.258819  0.        0.308621]
 [ 0.258819  0.965926  0.        0.270771]
 [ 0.        0.        1.        0.      ]
 [ 0.        0.        0.        1.      ]]
닫힌 형태:  x=0.308621  y=0.270771
DH 결과  :  x=0.308621  y=0.270771
두 결과가 같은가: True
손끝 방향(세 각의 합) = 15.0000도, 행렬에서 읽은 값 = 15.0000도

손으로 유도한 (0.308621, 0.270771)과 행렬 연쇄가 준 값이 일치합니다. 그리고 결과 행렬의 왼쪽 위 3×3에서 읽은 손끝 방향이 세 각의 합인 15도와 같습니다. 위치만이 아니라 자세까지 한 번에 나온다는 것이 행렬 방식의 이점입니다.

현대 도구를 쓰면 이 표를 그대로 옮길 수 있습니다. 피터 코크의 Robotics Toolbox for Python(PyPI roboticstoolbox-python, 2026년 7월 기준 1.3.1, 파이썬 3.10 이상)에서는 표준 DH 링크를 RevoluteDH, 수정 DH 링크를 RevoluteMDH로 구분하고, 이 둘을 한 로봇에 섞을 수 없습니다. 규약이 플래그가 아니라 클래스로 갈린다는 점이 오히려 안전 장치가 됩니다. DHRobot으로 묶은 뒤 .fkine(q)를 호출하면 위 코드가 한 줄이 됩니다.

ROS 생태계는 DH 대신 URDF를 씁니다. 링크와 조인트를 XML로 적고, 조인트마다 부모 링크에 대한 위치와 축을 명시합니다.

<joint name="elbow" type="revolute">
  <parent link="upper_arm"/>
  <child link="forearm"/>
  <origin xyz="0.20 0 0" rpy="0 0 0"/>
  <axis xyz="0 0 1"/>
  <limit lower="-2.6" upper="2.6" effort="1.5" velocity="3.0"/>
</joint>

URDF에서 조인트 타입으로 쓸 수 있는 문자열은 정확히 여섯 개입니다. revolute, continuous, prismatic, fixed, floating, planar. 그리고 limit 요소에서 effort와 velocity는 필수, lower와 upper는 선택입니다. 회전 범위가 무한한 관절이라면 continuous를 쓰고 상하한을 생략합니다.

URDF가 DH보다 나은 점은 좌표계를 규칙에 맞춰 놓을 필요가 없다는 것입니다. 원점을 편한 곳에 두고 축 방향만 정확히 적으면 됩니다. 대신 파라미터가 관절당 여섯 개로 늘어납니다. 사람이 CAD에서 치수를 옮겨 적기에는 URDF가 훨씬 편하고, 손으로 식을 유도하기에는 DH가 편합니다.

회전 표현의 트레이드오프 — 오일러각, 회전행렬, 쿼터니언

위치는 숫자 세 개로 끝나지만 자세는 그렇지 않습니다. 3차원 회전은 자유도가 셋인데, 그 셋을 어떻게 적을지에 여러 선택지가 있고 각각 대가가 다릅니다.

표현	저장하는 수	특이점	보간	합성 비용	주로 쓰는 곳
회전행렬	9 (제약 6개)	없음	직접은 불가	행렬곱 27회	계산 내부, 기구학
오일러각 (rpy)	3	짐벌락	자연스러우나 위험	행렬로 바꿔야 함	사람이 읽고 쓰는 곳, URDF
축-각	4 (또는 3)	각도 0에서 축 미정의	보통	중간	회전 벡터, 미소 회전
쿼터니언	4 (제약 1개)	없음	slerp로 자연스러움	곱셈 16회	자세 추정, 보간, 저장

각 항목의 대가를 하나씩 보겠습니다.

회전행렬은 계산에 가장 편합니다. 벡터를 돌리는 것이 곱셈 한 번이고, 합성도 곱셈 한 번입니다. 대신 아홉 개의 수로 세 개의 자유도를 표현하므로 여섯 개의 제약이 붙습니다. 열들이 서로 직교해야 하고 길이가 1이어야 합니다. 부동소수점 오차가 쌓이면 이 제약이 조금씩 깨지고, 그대로 두면 물체가 미묘하게 늘어나거나 찌그러집니다. 그래서 주기적으로 정규직교화가 필요합니다.

오일러각은 사람에게 가장 친절합니다. 롤 30도, 피치 0도, 요 90도라고 하면 누구나 그림을 그릴 수 있습니다. URDF의 rpy 속성이 이것이고, 규약은 고정축 기준으로 x, y, z 순서, 행렬로는 Rz(yaw)·Ry(pitch)·Rx(roll)입니다.

문제는 짐벌락입니다. 두 번째 각이 90도가 되면 첫 번째 축과 세 번째 축이 같은 방향을 보게 되어, 서로 다른 각도 조합이 같은 회전을 만듭니다.

import numpy as np
np.set_printoptions(precision=6, suppress=True)


def rotation_rpy(roll, pitch, yaw):
    """URDF의 rpy와 같은 규약입니다. 고정축 x, y, z 순서로 돌리는 것이
    행렬로는 Rz(yaw) · Ry(pitch) · Rx(roll)과 같습니다."""
    cr, sr = np.cos(roll), np.sin(roll)
    cp, sp = np.cos(pitch), np.sin(pitch)
    cy, sy = np.cos(yaw), np.sin(yaw)
    Rx = np.array([[1, 0, 0], [0, cr, -sr], [0, sr, cr]])
    Ry = np.array([[cp, 0, sp], [0, 1, 0], [-sp, 0, cp]])
    Rz = np.array([[cy, -sy, 0], [sy, cy, 0], [0, 0, 1]])
    return Rz @ Ry @ Rx


def rpy_from_rotation(R):
    """회전행렬에서 rpy를 되뽑습니다. 분모가 cos(pitch)라는 점이 핵심입니다."""
    pitch = np.arcsin(np.clip(-R[2, 0], -1.0, 1.0))
    roll = np.arctan2(R[2, 1], R[2, 2])
    yaw = np.arctan2(R[1, 0], R[0, 0])
    return np.degrees([roll, pitch, yaw])


A = rotation_rpy(np.radians(0),   np.radians(90), np.radians(30))
B = rotation_rpy(np.radians(-30), np.radians(90), np.radians(0))
print("roll=0, pitch=90, yaw=30 과 roll=-30, pitch=90, yaw=0")
print("  두 행렬이 같은가:", np.allclose(A, B), " 최대 차이:", np.abs(A - B).max())

# 짐벌락 바로 앞에서, 아주 작은 자세 변화가 rpy를 얼마나 흔드는지 봅니다.
base = rotation_rpy(np.radians(0), np.radians(89.9), np.radians(30))
print("\n(0, 89.9, 30)을 되뽑으면:", np.round(rpy_from_rotation(base), 4))
for eps in (0.001, 0.01, 0.1):
    perturbed = rotation_rpy(np.radians(eps), 0.0, 0.0) @ base
    print(f"  x축으로 {eps:>5}도 흔들면 rpy = {np.round(rpy_from_rotation(perturbed), 4)}"
          f"   (행렬 최대 차이 {np.abs(perturbed - base).max():.6f})")

실행 결과입니다.

roll=0, pitch=90, yaw=30 과 roll=-30, pitch=90, yaw=0
  두 행렬이 같은가: True  최대 차이: 3.0616169978683824e-17

(0, 89.9, 30)을 되뽑으면: [ 0.  89.9 30. ]
  x축으로 0.001도 흔들면 rpy = [ 0.4937 89.8995 30.4937]   (행렬 최대 차이 0.000017)
  x축으로  0.01도 흔들면 rpy = [ 4.715  89.8946 34.715 ]   (행렬 최대 차이 0.000175)
  x축으로   0.1도 흔들면 rpy = [30.     89.8268 60.    ]   (행렬 최대 차이 0.001745)

앞의 두 줄이 짐벌락 자체입니다. 완전히 다른 두 각도 조합이 같은 회전이고, 차이가 3×10⁻¹⁷이니 부동소수점 정밀도 수준에서 동일합니다.

뒤의 세 줄이 실무에서 더 아픈 부분입니다. 피치가 89.9도인 자세에서 팔을 x축으로 0.01도만 흔들었는데 되뽑은 롤이 0도에서 4.715도로, 요가 30도에서 34.715도로 움직였습니다. 입력 0.01도에 출력 4.7도, 470배 증폭입니다. 0.1도를 흔들면 롤이 30도, 요가 60도가 됩니다.

원인은 되뽑는 식의 분모에 있습니다. 롤과 요를 나누는 데 쓰이는 양이 cos(pitch)이고, 피치가 90도로 가면 이것이 0으로 갑니다. 0에 가까운 수로 나누는 계산은 입력의 작은 잡음을 크게 부풀립니다.

실제 증상은 이렇습니다. 팔이 이 자세 근처를 지나갈 때 엔코더 잡음이나 부동소수점 오차만으로도 롤과 요 값이 크게 튀고, 그 값을 그대로 보간하면 손목이 한 바퀴 돕니다. 물리적으로는 팔이 거의 안 움직였는데 제어기는 60도를 이동하라는 지령을 받은 셈입니다. 손목 관절이 잔뜩 감기는 사고의 상당수가 여기서 나옵니다.

쿼터니언은 짐벌락이 없습니다. 네 개의 수 (w, x, y, z)에 제약이 하나(길이 1)뿐이라 자유도 셋에 딱 맞고, 표현이 연속적입니다. 두 자세 사이의 보간(slerp)이 자연스럽고, 회전 합성이 행렬보다 값쌉니다.

대가는 두 가지입니다. 사람이 숫자만 보고 자세를 상상할 수 없다는 것이 하나이고, q와 -q가 같은 회전을 나타내는 이중 덮개가 다른 하나입니다. 두 쿼터니언 사이를 보간할 때 부호를 맞춰 주지 않으면 짧은 길 대신 먼 길로 돌아갑니다.

실무의 정답은 대체로 이렇습니다. 저장과 보간은 쿼터니언으로, 계산은 회전행렬로, 사람에게 보여 주는 것만 오일러각으로. 세 가지를 다 쓰되 각자 잘하는 자리에만 두는 것입니다.

부호 하나가 틀리면 실제로 무슨 일이 생기는가

순기구학 코드에서 가장 흔한 버그는 복잡한 것이 아닙니다. 부호 하나, 각도 규약 하나입니다.

관절 각도의 방향이 뒤집힌 경우를 보겠습니다. 서보를 조립할 때 기어를 반대쪽에서 물리면 지령 각도가 커질 때 관절이 반대로 돕니다. 코드는 그대로인데 θ2의 부호만 뒤집힌 셈입니다.

import numpy as np

L1, L2 = 0.20, 0.15


def fk(theta1, theta2):
    return np.array([L1 * np.cos(theta1) + L2 * np.cos(theta1 + theta2),
                     L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2)])


good = fk(np.radians(30), np.radians(45))
bad = fk(np.radians(30), np.radians(-45))          # θ2의 부호만 뒤집었습니다
print(f"올바른 손끝  : ({good[0]:.6f}, {good[1]:.6f})")
print(f"부호 뒤집힘  : ({bad[0]:.6f}, {bad[1]:.6f})")
print(f"어긋난 거리  : {np.linalg.norm(good - bad) * 1000:.3f} mm")
print(f"이론값 2·L2·sin(45도) = {2 * L2 * np.sin(np.radians(45)) * 1000:.3f} mm")

print("\n원점 근처에서는 잘 보이지 않습니다:")
for d2 in (1, 5, 15, 45, 90):
    g, b = fk(np.radians(30), np.radians(d2)), fk(np.radians(30), np.radians(-d2))
    print(f"  θ2={d2:3}도 -> 오차 {np.linalg.norm(g - b) * 1000:7.3f} mm")

실행 결과입니다.

올바른 손끝  : (0.212028, 0.244889)
부호 뒤집힘  : (0.318094, 0.061177)
어긋난 거리  : 212.132 mm
이론값 2·L2·sin(45도) = 212.132 mm

원점 근처에서는 잘 보이지 않습니다:
  θ2=  1도 -> 오차   5.236 mm
  θ2=  5도 -> 오차  26.147 mm
  θ2= 15도 -> 오차  77.646 mm
  θ2= 45도 -> 오차 212.132 mm
  θ2= 90도 -> 오차 300.000 mm

45도에서 212밀리미터 어긋납니다. 팔 전체 길이가 350밀리미터인데 말입니다. 그리고 오차의 크기가 정확히 2·L2·sin(θ2)인 것도 확인됩니다. 손끝이 위팔의 연장선에 대해 거울처럼 반사되기 때문입니다.

마지막 표가 이 버그의 성격을 말해 줍니다. 영점 근처에서는 거의 안 보입니다. θ2 = 1도에서 오차는 5밀리미터이고, 조립 공차나 백래시로 착각하기 딱 좋습니다. 그래서 홈 자세 근처에서만 테스트하면 통과하고, 실제 작업 각도로 가면 완전히 빗나갑니다.

같은 성격의 버그가 몇 가지 더 있습니다.

라디안과 도의 혼동. numpy의 삼각함수는 전부 라디안을 받습니다. 30을 그대로 넣으면 30 라디안, 즉 1719도로 해석되어 팔이 네 바퀴 넘게 돈 자세가 계산됩니다. 다행히 이 버그는 결과가 워낙 엉뚱해서 금방 드러납니다.

절대각과 상대각의 혼동. 위 식의 θ2는 위팔에 대한 상대각입니다. 엔코더가 절대각을 주는 팔에서 그 값을 그대로 넣으면 θ1이 두 번 더해집니다. θ1 = 0일 때는 두 규약이 같은 값을 주므로 이 버그도 홈 자세에서는 안 보입니다.

영점 오프셋. DH 표의 θ는 규약이 정한 기준에서 잰 각도이고, 서보가 보고하는 각도는 기구적 영점에서 잰 값입니다. 둘이 다르면 상수만큼 어긋나고, 그 상수를 DH 표의 오프셋 항에 넣어야 합니다. 위 코드에서 offset + theta로 쓴 부분이 그 자리입니다.

이 네 가지를 한꺼번에 잡는 검증 방법이 하나 있습니다. 관절 하나만 움직여 보는 것입니다. 나머지를 0으로 두고 한 관절씩 알려진 각도로 돌린 뒤, 계산이 예측한 손끝 위치와 자로 잰 실제 위치를 비교합니다. 여섯 개를 동시에 움직여 놓고 원인을 찾으려 하면 어느 관절의 문제인지 구분할 수 없습니다.

마치며 — 좌표계를 정확히 적어 두는 것이 절반입니다

순기구학의 수학은 어렵지 않습니다. 회전행렬에 한 줄과 한 열을 덧붙여 평행이동까지 곱셈으로 만들고, 그것들을 순서대로 곱합니다. 그게 전부입니다.

어려운 부분은 수학이 아니라 규약입니다. 각도를 어디에서 재는가, 어느 방향이 양수인가, 절대각인가 상대각인가, 어느 DH 규약인가, 영점이 어디인가. 이 다섯 가지를 문서에 적어 두지 않으면 몇 주 뒤의 자신이 반드시 틀립니다.

그리고 순기구학이 틀리면 그 위의 모든 것이 조용히 틀립니다. 역기구학은 순기구학의 역이므로 같은 부호 오류를 그대로 물려받고, 자코비안은 순기구학의 편미분이므로 역시 물려받습니다. 카메라 보정도, 충돌 검사도, 궤적 계획도 마찬가지입니다. 위층에서 이상한 증상을 며칠 쫓다가 결국 DH 표의 부호 하나였던 경우가 흔합니다.

그래서 순기구학을 만들고 나면 반드시 자로 재 보십시오. 관절 하나씩, 알려진 각도로, 실제 거리와 비교해서. 계산과 자가 5밀리미터 안에서 맞으면 그 위에 무엇을 쌓아도 됩니다.

다음 단계는 이 함수를 거꾸로 뒤집는 일입니다. 역기구학 편에서 왜 그것이 훨씬 어려운지, 그리고 여기서 만든 행렬들이 어떻게 자코비안으로 이어지는지를 다룹니다. 이 글에 나온 선형대수와 삼각법을 어느 수준까지 알아야 하는지는 로봇공학에 필요한 수학 편에 정리해 두었습니다.