역기구학 — 가고 싶은 곳에서 관절 각도를 거꾸로 구하기

들어가며 — "저 컵을 여기 놓아라"를 각도 여섯 개로 옮기기
역기구학이 순기구학보다 어려운 이유
2링크 팔의 해석해 — 코사인 법칙으로 끝까지 풀기
자코비안 — 관절 속도가 손끝 속도로 옮겨 가는 행렬
의사역행렬로 반복해서 푸는 수치해
특이점 — 물리적으로 무엇이 일어나는가
감쇠 최소자승 — 특이점 근처에서 살아남기
여유자유도와 널스페이스
마치며 — 역기구학은 답을 구하는 문제가 아니라 답을 고르는 문제입니다

들어가며 — "저 컵을 여기 놓아라"를 각도 여섯 개로 옮기기

순기구학 편에서 만든 계산기는 이렇게 씁니다. 어깨 30도, 팔꿈치 45도를 넣으면 손끝이 (0.212028, 0.244889)에 있다고 알려 줍니다. 각도를 아무거나 넣어도 답이 하나 나오고, 계산은 곱셈 몇 번이면 끝납니다.

그런데 실제로 하고 싶은 일은 반대 방향입니다. 컵이 (0.25, 0.15)에 있으니 손끝을 거기에 놓아 달라는 것이고, 관절이 몇 도가 되어야 하는지는 우리가 구해야 합니다.

이 방향이 역기구학(IK)입니다. 그리고 순기구학보다 비교할 수 없이 어렵습니다.

어려움의 종류가 여러 가지라는 점이 중요합니다. 계산이 복잡한 것이 아니라, 문제 자체의 성질이 다릅니다. 어떤 좌표에는 답이 아예 없고, 어떤 좌표에는 두 개가 있고, 어떤 팔에서는 무한히 많습니다. 그리고 답이 있는 곳에서도 답 근처의 성질이 자리마다 달라서, 어떤 자세에서는 손끝을 1밀리미터 옮기는 데 관절이 0.15도만 돌면 되고 다른 자세에서는 같은 1밀리미터에 20.8도가 필요합니다.

이 글은 그 네 가지 어려움을 하나씩 다룹니다. 그리고 실제로 쓸 수 있는 코드로 끝냅니다.

역기구학이 순기구학보다 어려운 이유

순기구학은 함수입니다. 관절 각도 벡터를 넣으면 손끝 위치가 하나 나옵니다. 정의역의 모든 점에 대해 정확히 하나의 값이 있습니다.

역기구학은 그 함수의 역을 구하는 문제인데, 이 함수는 일대일도 아니고 위로의 함수도 아닙니다.

해가 없는 경우. 팔 길이의 합보다 먼 점은 어떤 각도로도 닿을 수 없습니다. 위팔 0.20미터, 아래팔 0.15미터인 팔의 최대 도달 거리는 0.35미터입니다. 0.40미터 떨어진 점을 요구하면 방정식에 실수해가 없습니다. 안쪽에도 못 닿는 영역이 있습니다. 두 링크의 길이 차이인 0.05미터보다 가까운 점은 팔꿈치를 아무리 접어도 닿지 않습니다.

해가 여러 개인 경우. 도달 가능한 점 대부분에는 정확히 두 개의 해가 있습니다. 팔꿈치를 위로 꺾은 자세와 아래로 꺾은 자세입니다. 6자유도 산업용 로봇에서는 이 분기가 세 군데(어깨, 팔꿈치, 손목)에서 일어나 최대 여덟 개의 해가 나옵니다.

해가 무한한 경우. 관절 수가 작업 차원보다 많으면 해가 연속적으로 무한히 많습니다. 3링크 평면 팔로 2차원 위치만 지정하면 남는 자유도 하나만큼 팔이 자유롭게 꿈틀거릴 수 있습니다. 사람 팔이 그렇습니다. 손을 한자리에 고정한 채 팔꿈치를 위아래로 움직여 보면 바로 확인됩니다.

닫힌 해가 존재하지 않는 경우. 일반적인 6자유도 팔의 역기구학은 대수적으로 풀면 16차 방정식이 되고, 닫힌 형태의 해는 특정 조건에서만 존재합니다. 손목의 세 축이 한 점에서 만나는 구조(구형 손목)가 그 조건 중 가장 널리 쓰이는 것이고, 산업용 로봇 대부분이 이 구조를 택하는 실질적인 이유가 여기에 있습니다. 그렇지 않은 팔은 수치해법으로 풀어야 합니다.

성질	순기구학	역기구학
해의 개수	항상 정확히 하나	0개, 여러 개, 무한개
닫힌 해	언제나 존재	구조에 따라 다름
계산 방식	행렬 곱 몇 번	해석해 또는 반복 수치해
계산 시간	일정	자세와 초기값에 따라 변함
실패 조건	없음	작업공간 밖, 수렴 실패, 특이점
자세 선택	불필요	여러 해 중 하나를 골라야 함

마지막 줄이 실무에서 가장 많은 사고를 만듭니다. 해가 두 개일 때 어느 쪽을 고르느냐가 팔 전체의 자세를 결정하고, 매 주기 독립적으로 풀다가 선택이 바뀌면 팔이 한순간에 뒤집힙니다.

2링크 팔의 해석해 — 코사인 법칙으로 끝까지 풀기

가장 단순한 경우를 완전히 풀어 보겠습니다. 링크 두 개, 회전 조인트 두 개, 평면 위의 위치 두 개. 미지수 두 개에 방정식 두 개입니다.

순기구학은 이랬습니다.

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

핵심 요령은 θ1을 먼저 없애는 것입니다. 두 식을 각각 제곱해서 더합니다.

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

대괄호 안은 코사인의 차 공식이므로 cos(θ2)가 됩니다.

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

이것이 삼각형에 대한 코사인 법칙입니다. 두 링크와 어깨-손끝을 잇는 선분이 삼각형을 이루고, θ2가 그 사잇각의 보각입니다. 정리하면,

D = cos(θ2) = (x² + y² - L1² - L2²) / (2·L1·L2)

숫자를 넣어 보겠습니다. 목표를 (0.212028, 0.244889)로 두면,

x² + y² = 0.0449559 + 0.0599706 = 0.1049264
L1² + L2² = 0.0400000 + 0.0225000 = 0.0625000
2·L1·L2  = 2 × 0.20 × 0.15 = 0.06

D = (0.1049264 - 0.0625000) / 0.06 = 0.0424264 / 0.06 = 0.7071068

D가 0.7071068입니다. cos(45도)입니다.

여기서 θ2 = arccos(D)로 끝내면 안 됩니다. arccos는 0도에서 180도만 돌려주므로 음의 해를 잃어버립니다. 대신 사인을 함께 구해 atan2에 넣습니다.

sin(θ2) = ±√(1 - D²)
θ2 = atan2(±√(1 - D²), D)

부호 두 개가 곧 두 해입니다.

√(1 - 0.7071068²) = √(1 - 0.5) = √0.5 = 0.7071068

θ2 = atan2(+0.7071068, 0.7071068) = +45도
θ2 = atan2(-0.7071068, 0.7071068) = -45도

이제 θ1입니다. 손끝을 향하는 방향에서, 삼각형 안쪽 각만큼 빼면 됩니다.

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

앞의 항은 어깨에서 손끝을 바라보는 각도이고, 뒤의 항은 위팔이 그 시선에서 얼마나 벌어져 있는지입니다. θ2 = +45도인 경우로 계산하면,

atan2(0.244889, 0.212028) = 49.1136도
atan2(0.15 × 0.7071068, 0.20 + 0.15 × 0.7071068)
  = atan2(0.1060660, 0.3060660) = 19.1136도

θ1 = 49.1136 - 19.1136 = 30.0000도

정확히 30도가 나옵니다. 순기구학에서 출발했던 그 각도입니다.

θ2 = -45도를 넣으면 뒤의 항이 atan2(-0.1060660, 0.3060660) = -19.1136도가 되어,

θ1 = 49.1136 - (-19.1136) = 68.2271도

두 번째 해는 (68.2271도, -44.9999도)입니다. 팔꿈치가 반대로 꺾인 자세이고, 손끝은 정확히 같은 자리에 있습니다.

import numpy as np

L1, L2 = 0.20, 0.15


def ik_2link(x, y, elbow=+1):
    """2링크 평면 팔의 해석해. elbow=+1이 elbow-down, -1이 elbow-up입니다."""
    D = (x * x + y * y - L1 * L1 - L2 * L2) / (2 * L1 * L2)
    inner = 1.0 - D * D
    if inner < 0:
        raise ValueError(f"작업공간 밖입니다. D={D:.6f}, 1-D²={inner:.6f}")
    theta2 = np.arctan2(elbow * np.sqrt(inner), D)
    theta1 = np.arctan2(y, x) - np.arctan2(L2 * np.sin(theta2), L1 + L2 * np.cos(theta2))
    return np.array([theta1, theta2])


def fk_2link(q):
    return np.array([L1 * np.cos(q[0]) + L2 * np.cos(q[0] + q[1]),
                     L1 * np.sin(q[0]) + L2 * np.sin(q[0] + q[1])])


target = np.array([0.212028, 0.244889])
for elbow, name in ((+1, "elbow-down"), (-1, "elbow-up  ")):
    q = ik_2link(*target, elbow=elbow)
    print(f"{name}  θ1={np.degrees(q[0]):9.4f}도  θ2={np.degrees(q[1]):9.4f}도  FK검산={np.round(fk_2link(q), 6)}")

try:
    ik_2link(0.40, 0.0)
except ValueError as e:
    print("(0.40, 0.00) ->", e)

실행 결과입니다.

elbow-down  θ1=  30.0001도  θ2=  44.9999도  FK검산=[0.212028 0.244889]
elbow-up    θ1=  68.2271도  θ2= -44.9999도  FK검산=[0.212028 0.244889]
(0.40, 0.00) -> 작업공간 밖입니다. D=1.625000, 1-D²=-1.640625

마지막 줄이 작업공간 밖의 좌표가 방정식에서 어떻게 드러나는지 보여 줍니다. (0.40, 0)을 넣으면,

D = (0.16 - 0.0625) / 0.06 = 1.625
1 - D² = 1 - 2.640625 = -1.640625

D가 1을 넘습니다. 코사인이 1을 넘을 수는 없으므로 이 목표를 만족하는 각도가 존재하지 않는다는 뜻이고, 제곱근 안이 음수가 되어 계산이 멈춥니다.

이 검사를 생략하면 어떤 일이 벌어지는지가 중요합니다. numpy에서 음수의 제곱근은 예외가 아니라 nan이고, nan은 조용히 퍼집니다. θ2가 nan이 되고, θ1도 nan이 되고, 서보에 각도를 쓰는 함수까지 nan이 도착합니다. C 계열에서 nan을 정수로 변환한 결과는 정의되지 않으며, 아두이노에서 servo.write()에 그 값이 들어가면 관절이 예측할 수 없는 위치로 튑니다. 역기구학 함수의 첫 줄은 언제나 도달 가능성 검사여야 합니다.

두 해 중 무엇을 고를지도 코드가 정해야 합니다. 실무에서 흔한 규칙은 세 가지입니다. 하나, 하드웨어의 관절 한계를 벗어나는 해를 버립니다. 둘, 남은 해 중 현재 자세에서 관절 각도 변화량이 가장 작은 것을 고릅니다. 셋, 그래도 남으면 정해진 자세(예를 들어 항상 elbow-up)를 고정합니다. 두 번째 규칙이 특히 중요한데, 이것이 없으면 궤적 중간에 해가 바뀌면서 팔이 뒤집힙니다.

자코비안 — 관절 속도가 손끝 속도로 옮겨 가는 행렬

해석해는 아름답지만 2링크나 구형 손목처럼 구조가 특별한 팔에서만 구할 수 있습니다. 일반적인 팔에서는 다른 접근이 필요하고, 그 출발점이 자코비안입니다.

발상은 이렇습니다. 순기구학 p = f(q)를 미분하면,

ṗ = J(q) · q̇

여기서 J는 손끝 좌표를 관절 각도로 편미분한 값들을 모아 놓은 행렬입니다. 성분으로 쓰면,

J[i][j] = ∂(p의 i번째 성분) / ∂(q의 j번째 성분)

자코비안은 그냥 편미분들을 격자로 늘어놓은 것이고, 어려운 정의가 따로 있는 것이 아닙니다.

2링크 팔의 자코비안을 손으로 구해 보겠습니다. x를 θ1로 미분하면,

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

행렬로 모으면,

J = [ -L1·s1 - L2·s12    -L2·s12 ]
    [  L1·c1 + L2·c12     L2·c12 ]

s1은 sin(θ1), s12는 sin(θ1+θ2)의 줄임입니다.

이 행렬의 행렬식을 계산하면 놀랍도록 단순한 결과가 나옵니다.

det(J) = (-L1·s1 - L2·s12)(L2·c12) - (-L2·s12)(L1·c1 + L2·c12)
       = -L1·L2·s1·c12 - L2²·s12·c12 + L1·L2·s12·c1 + L2²·s12·c12
       = L1·L2·(s12·c1 - s1·c12)
       = L1·L2·sin(θ2)

행렬식이 θ1과 전혀 무관하고 오직 θ2에만 의존합니다. 어깨를 어디로 돌리든 팔의 성질은 팔꿈치 각도로만 결정된다는 뜻이고, 물리적으로 당연합니다. 어깨를 돌리는 것은 팔 전체를 회전시키는 것뿐이니까요.

그리고 θ2 = 0이면 행렬식이 0입니다. 팔이 완전히 펴진 자세입니다. 여기서 자코비안이 특이해집니다.

의사역행렬로 반복해서 푸는 수치해

자코비안이 있으면 역기구학을 미분 방정식으로 바꿔 풀 수 있습니다.

현재 관절 각도가 q이고 손끝이 f(q)에 있는데 목표가 p_target이라면, 남은 오차는 e = p_target - f(q)입니다. 이 오차를 없애려면 손끝을 e만큼 움직여야 하고, 그러려면 관절을 얼마나 움직여야 하는지가 J·Δq = e입니다.

J가 정사각이고 가역이면 Δq = J⁻¹·e입니다. 그런데 관절 수와 작업 차원이 다르면 정사각이 아닙니다. 이때 쓰는 것이 무어-펜로즈 의사역행렬 J⁺입니다.

관절이 작업 차원보다 많으면(여유자유도가 있으면) J⁺ = Jᵀ(J·Jᵀ)⁻¹이고, 이 해는 ‖Δq‖가 최소인 해입니다. 관절이 부족하면 J⁺ = (Jᵀ·J)⁻¹Jᵀ이고, 이 해는 오차를 최소로 만드는 해입니다. 두 경우 모두 numpy.linalg.pinv가 알아서 처리합니다.

전체 알고리즘은 이렇게 됩니다.

1. 초기 각도 q를 정한다 (보통 현재 자세)
2. e = p_target - f(q) 를 계산한다
3. ‖e‖가 충분히 작으면 종료
4. Δq = J(q)⁺ · e
5. q = q + Δq, 2번으로

한 줄씩 보면 이것은 다변수 뉴턴 방법입니다. 손끝 오차라는 비선형 함수의 영점을 찾는 반복입니다.

이 방법의 장점은 팔의 구조를 전혀 가리지 않는다는 것입니다. 자코비안만 계산할 수 있으면 관절이 몇 개든, 어떤 순서로 붙었든 똑같이 동작합니다. 단점은 세 가지입니다. 수렴이 보장되지 않고, 초기값에 따라 다른 해로 가고, 특이점 근처에서 폭발합니다.

마지막 문제가 심각하므로 절을 나누겠습니다.

특이점 — 물리적으로 무엇이 일어나는가

특이점은 자코비안의 랭크가 떨어지는 자세입니다. 정의만 보면 추상적인데, 물리적으로는 아주 구체적인 일이 벌어집니다.

어떤 방향으로는 손끝이 갈 수 없게 됩니다. 팔을 완전히 펴고 손끝을 어깨에서 더 멀어지는 쪽으로 밀어 보십시오. 어느 관절을 어떻게 돌려도 그 방향으로는 1밀리미터도 못 갑니다. 이미 최대한 뻗었기 때문입니다. 자코비안의 상이 2차원에서 1차원으로 줄어든 것이 이 상황입니다.

특이점의 종류는 6자유도 팔에서 세 가지로 정리됩니다.

종류	자세	일어나는 일
팔꿈치 특이점	팔이 완전히 펴짐	반지름 방향으로 못 나감
어깨 특이점	손목 중심이 1축 회전축 위에 놓임	1축이 어디를 향해도 손목 위치가 같아짐
손목 특이점	손목의 4축과 6축이 일직선	두 축이 같은 회전을 만들어 하나가 잉여가 됨

세 번째 것이 실무에서 가장 자주 만나는 문제입니다. 손목 축 두 개가 겹치면 그 둘은 같은 일을 하게 되고, 제어기는 둘을 서로 반대로 미친 듯이 돌리기 시작합니다. 산업용 로봇에서 티칭 중에 손목이 갑자기 한 바퀴 도는 사고가 대부분 이것입니다.

특이점은 이분법이 아니라 연속적인 정도의 문제입니다. 그 정도를 재는 표준적인 척도가 요시카와의 조작성 지수입니다.

w = √(det(J·Jᵀ))

정사각 자코비안에서는 |det(J)|와 같습니다. 그리고 더 실용적인 척도는 특이값 분해에서 나오는 가장 작은 특이값과 조건수입니다. 앞에서 만든 2링크 팔로 확인해 보겠습니다.

import numpy as np

L1, L2 = 0.20, 0.15


def jacobian(q):
    s1, c1 = np.sin(q[0]), np.cos(q[0])
    s12, c12 = np.sin(q[0] + q[1]), np.cos(q[0] + q[1])
    return np.array([[-L1 * s1 - L2 * s12, -L2 * s12],
                     [ L1 * c1 + L2 * c12,  L2 * c12]])


print(" θ2      det(J)      L1·L2·sin(θ2)    σ1        σ2       조건수")
for d2 in (90, 45, 10, 2, 0):
    J = jacobian(np.radians([30, d2]))
    sigma = np.linalg.svd(J, compute_uv=False)
    cond = sigma[0] / sigma[1] if sigma[1] > 1e-12 else float("inf")
    print(f"{d2:4}도  {np.linalg.det(J):10.7f}  {L1*L2*np.sin(np.radians(d2)):12.7f}"
          f"  {sigma[0]:8.6f}  {sigma[1]:8.6f}  {cond:10.1f}")

실행 결과입니다.

 θ2      det(J)      L1·L2·sin(θ2)    σ1        σ2       조건수
  90도   0.0300000     0.0300000  0.269451  0.111337         2.4
  45도   0.0212132     0.0212132  0.351840  0.060292         5.8
  10도   0.0052094     0.0052094  0.379341  0.013733        27.6
   2도   0.0010470     0.0010470  0.380731  0.002750       138.5
   0도  -0.0000000     0.0000000  0.380789  0.000000         inf

두 번째와 세 번째 열이 모든 행에서 일치합니다. 손으로 유도한 det(J) = L1·L2·sin(θ2)가 맞았습니다. 마지막 행의 -0.0000000은 부동소수점의 부호 있는 0이고, 값은 0입니다.

읽어야 할 것은 오른쪽 세 열입니다. σ1은 팔꿈치 각도가 변해도 0.27에서 0.38 사이에서 거의 그대로인데, σ2는 0.111에서 0으로 떨어집니다. 조건수는 2.4에서 무한대로 갑니다.

작은 특이값이 그 방향으로의 "기어비"입니다. σ2 = 0.00275라는 것은 그 방향으로 손끝을 1미터 움직이려면 관절이 1/0.00275 = 364 라디안 돌아야 한다는 뜻입니다. 1밀리미터라면 0.364 라디안, 20.8도입니다. 다른 방향(σ1 = 0.3807)으로 1밀리미터는 0.0026 라디안, 0.15도면 됩니다. 같은 1밀리미터가 방향에 따라 139배 차이가 납니다.

감쇠 최소자승 — 특이점 근처에서 살아남기

이제 앞의 반복법이 왜 폭발하는지 명확합니다. Δq = J⁺·e에서 J⁺는 특이값의 역수를 포함하고, 특이값이 0으로 가면 역수가 발산합니다.

숫자로 보겠습니다. θ2 = 1도인 자세에서 손끝을 x 방향으로 1밀리미터 움직이라고 하면,

의사역행렬이 주는 |Δq| = 0.626520 rad = 35.8970도

1밀리미터를 위해 관절이 35.9도 돌아야 합니다. 제어 주기가 100Hz라면 이것을 10밀리초 안에 해내라는 명령이고, 초당 3590도입니다. 흔한 취미용 서보의 무부하 속도가 초당 400도 안팎이니 아홉 배 너머입니다. 실제로는 서보가 최대 속도로 밀어붙이다가 궤적을 완전히 놓치고, 관절 한계에 부딪히거나 전원이 무너지면서 보드가 리셋됩니다.

해법은 정확도를 조금 포기하는 것입니다. 역행렬을 구할 때 대각선에 작은 값을 더합니다.

Δq = Jᵀ·(J·Jᵀ + λ²·I)⁻¹·e

이것이 감쇠 최소자승(DLS)이고, 로봇공학에서는 나카무라·하나후사와 왐플러의 1986년 연구로, 수치해석 일반에서는 레벤버그-마쿼트 방법으로 알려져 있습니다.

λ가 하는 일을 특이값으로 보면 명확합니다. 원래는 각 특이값이 1/σ로 역전되는데, 감쇠를 넣으면 σ/(σ² + λ²)가 됩니다. σ가 λ보다 훨씬 크면 1/σ와 거의 같고, σ가 0으로 가면 이 값도 0으로 갑니다. 작은 특이값 방향의 증폭만 골라서 억제하고 나머지는 건드리지 않습니다.

import numpy as np

L1, L2 = 0.20, 0.15


def fk(q):
    return np.array([L1 * np.cos(q[0]) + L2 * np.cos(q[0] + q[1]),
                     L1 * np.sin(q[0]) + L2 * np.sin(q[0] + q[1])])


def jacobian(q):
    s1, c1 = np.sin(q[0]), np.cos(q[0])
    s12, c12 = np.sin(q[0] + q[1]), np.cos(q[0] + q[1])
    return np.array([[-L1 * s1 - L2 * s12, -L2 * s12],
                     [ L1 * c1 + L2 * c12,  L2 * c12]])


J = jacobian(np.radians([30, 1]))          # 거의 다 펴진, 특이점 바로 앞의 자세
dx = np.array([0.001, 0.0])                # x 방향으로 1mm 가고 싶습니다

dq = np.linalg.pinv(J) @ dx
print(f"의사역행렬  |Δq|={np.linalg.norm(dq):.6f} rad = {np.degrees(np.linalg.norm(dq)):8.4f}도"
      f"  100Hz 환산 {np.degrees(np.linalg.norm(dq))/0.01:8.1f}도/초"
      f"  실제 이동 {np.linalg.norm(J @ dq)*1000:.4f}mm")

for lam in (0.001, 0.005, 0.01, 0.05):
    dq = J.T @ np.linalg.solve(J @ J.T + lam * lam * np.eye(2), dx)
    print(f"DLS λ={lam:<6} |Δq|={np.linalg.norm(dq):.6f} rad = {np.degrees(np.linalg.norm(dq)):8.4f}도"
          f"  100Hz 환산 {np.degrees(np.linalg.norm(dq))/0.01:8.1f}도/초"
          f"  실제 이동 {np.linalg.norm(J @ dq)*1000:.4f}mm")

실행 결과입니다.

의사역행렬  |Δq|=0.626520 rad =  35.8970도  100Hz 환산   3589.7도/초  실제 이동 1.0000mm
DLS λ=0.001  |Δq|=0.409784 rad =  23.4789도  100Hz 환산   2347.9도/초  실제 이동 0.7585mm
DLS λ=0.005  |Δq|=0.044071 rad =   2.5251도  100Hz 환산    252.5도/초  실제 이동 0.5113mm
DLS λ=0.01   |Δq|=0.011702 rad =   0.6705도  100Hz 환산     67.0도/초  실제 이동 0.5077mm
DLS λ=0.05   |Δq|=0.001394 rad =   0.0799도  100Hz 환산      8.0도/초  실제 이동 0.4992mm

이 표가 거래 조건을 그대로 보여 줍니다.

의사역행렬은 요구한 1밀리미터를 정확히 만들어 냅니다. 대신 관절이 35.9도 움직여야 합니다.

λ = 0.01이면 관절 운동이 0.67도로 54배 줄어듭니다. 대신 실제 이동은 0.51밀리미터로, 요구한 것의 절반입니다.

절반만 갔다는 것이 실패처럼 보이지만 그렇지 않습니다. 이것은 한 주기의 이야기입니다. 다음 주기에 남은 오차를 다시 보고 또 절반쯤 갑니다. 손끝은 목표를 향해 조금 느리게, 그러나 확실히 접근합니다. 반면 의사역행렬은 한 주기 만에 도착하려다 관절이 물리적 한계를 넘고, 그 결과 손끝은 목표는커녕 엉뚱한 곳으로 갑니다.

λ 고르기에는 실용적인 지침이 있습니다. 고정값을 쓴다면 작업 정밀도와 최대 관절 속도로부터 역산합니다. 더 나은 방법은 가변 감쇠로, 특이점에서 먼 곳에서는 λ = 0, 가장 작은 특이값이 임계값 아래로 내려가면 그때부터 λ를 키우는 것입니다. 이렇게 하면 평상시 정확도를 잃지 않으면서 특이점 근처에서만 보호가 걸립니다.

여유자유도와 널스페이스

마지막 조각입니다. 관절이 작업 차원보다 많으면 어떻게 되는가.

3링크 평면 팔에 2차원 위치만 지정한다고 하겠습니다. 자코비안은 2행 3열이고, 랭크가 2입니다. 랭크-널리티 정리에 따라 널스페이스의 차원은 3 - 2 = 1입니다.

널스페이스에 있는 관절 속도 벡터는 J·Δq = 0을 만족합니다. 관절은 움직이는데 손끝은 제자리에 있는 운동입니다. 손을 책상에 붙인 채 팔꿈치를 위아래로 움직이는 그 동작입니다.

이것을 쓰는 방법이 널스페이스 사영입니다.

Δq = J⁺·e + (I - J⁺·J)·z

첫 항이 손끝을 목표로 데려가고, 둘째 항이 손끝에 아무 영향을 주지 않으면서 z가 원하는 방향으로 자세를 밀어 줍니다. (I - J⁺·J)가 널스페이스로 사영하는 행렬입니다.

z에 무엇을 넣느냐가 여유자유도를 쓰는 방식을 정합니다. 관절 한계 중앙에서 멀어질수록 커지는 값을 넣으면 팔이 한계를 피해 다니고, 장애물까지의 거리를 넣으면 손끝 경로를 유지한 채 몸통을 비켜 갑니다. 조작성 지수를 넣으면 특이점에서 스스로 멀어집니다.

import numpy as np

LINKS = (0.20, 0.15, 0.10)


def fk3(q):
    a = np.cumsum(q)
    return np.array([sum(L * np.cos(t) for L, t in zip(LINKS, a)),
                     sum(L * np.sin(t) for L, t in zip(LINKS, a))])


def jacobian3(q):
    a = np.cumsum(q)
    J = np.zeros((2, 3))
    for j in range(3):
        J[0, j] = -sum(LINKS[i] * np.sin(a[i]) for i in range(j, 3))
        J[1, j] =  sum(LINKS[i] * np.cos(a[i]) for i in range(j, 3))
    return J


def solve_ik(target, q0, lam=0.05, max_iter=200, tol=1e-6):
    """감쇠 최소자승 반복법. 특이점을 지나가도 발산하지 않습니다."""
    q = np.array(q0, dtype=float)
    for k in range(max_iter):
        error = target - fk3(q)
        if np.linalg.norm(error) < tol:
            return q, k
        J = jacobian3(q)
        q = q + J.T @ np.linalg.solve(J @ J.T + lam * lam * np.eye(2), error)
    return q, max_iter


target = np.array([0.30, 0.20])
q, iters = solve_ik(target, [0.1, 0.5, 0.0])
print(f"수렴 {iters}회  θ={np.round(np.degrees(q), 4)}도  FK검산={np.round(fk3(q), 8)}")

J = jacobian3(q)
print(f"J의 크기 {J.shape}, 랭크 {np.linalg.matrix_rank(J)}, 널스페이스 차원 {3 - np.linalg.matrix_rank(J)}")

N = np.eye(3) - np.linalg.pinv(J) @ J     # 널스페이스 사영 행렬
z = np.array([1.0, 1.0, 1.0])             # 아무 방향이나 넣어 봅니다
dq_null = N @ z
print(f"널스페이스 방향 Δq = {np.round(dq_null, 6)}")
print(f"이 방향의 손끝 속도 J·Δq = {np.round(J @ dq_null, 12)}")

step = 0.05 * dq_null / np.linalg.norm(dq_null)
print(f"관절을 {np.round(np.degrees(step), 4)}도 움직였을 때")
print(f"  손끝 이동량 = {np.linalg.norm(fk3(q + step) - fk3(q)) * 1000:.6f} mm")

실행 결과입니다.

수렴 6회  θ=[-7.5005 67.2642 15.0619]도  FK검산=[0.29999971 0.19999971]
J의 크기 (2, 3), 랭크 2, 널스페이스 차원 1
널스페이스 방향 Δq = [ 0.038069 -0.231649  0.463807]
이 방향의 손끝 속도 J·Δq = [0. 0.]
관절을 [ 0.2098 -1.2766  2.556 ]도 움직였을 때
  손끝 이동량 = 0.059555 mm

세 관절이 각각 0.21도, -1.28도, 2.56도 움직였는데 손끝은 0.06밀리미터밖에 안 움직였습니다. 완전히 0이 아닌 이유는 널스페이스가 그 한 점에서의 접선 방향이고 유한한 걸음에는 2차 오차가 남기 때문입니다. 걸음을 절반으로 줄이면 이 오차는 4분의 1이 됩니다.

수렴 반복이 6회였다는 점도 눈여겨볼 만합니다. 초기 자세에서 목표까지 꽤 멀었는데도 여섯 번이면 마이크로미터 수준으로 붙습니다. 뉴턴 계열 방법의 수렴은 이렇게 빠릅니다. 물론 특이점을 통과하거나 목표가 작업공간 밖이면 이야기가 달라지므로, 실제 코드에는 반복 횟수 상한과 최종 오차 검사가 반드시 있어야 합니다. 위 함수가 max_iter에 도달해도 조용히 값을 돌려준다는 점을 눈치채셨다면 정확합니다. 실전 코드라면 그 경우를 호출자가 구분할 수 있어야 합니다.

마치며 — 역기구학은 답을 구하는 문제가 아니라 답을 고르는 문제입니다

이 글을 관통하는 것은 하나의 사실입니다. 역기구학에는 답이 여러 개이거나 없습니다.

그래서 실제로 하는 일의 대부분은 방정식을 푸는 것이 아니라, 어떤 답을 원하는지 명시하는 것입니다. elbow-up인가 elbow-down인가. 관절 한계를 넘는 해는 버릴 것인가. 직전 자세에서 가장 가까운 해를 고를 것인가. 여유자유도가 있다면 남는 자유를 무엇에 쓸 것인가. 특이점 근처에서 정확도와 관절 속도 중 무엇을 포기할 것인가.

이 선택들을 코드에 명시하지 않으면 선택이 사라지는 것이 아니라, 부동소수점 연산 순서가 대신 골라 줍니다. 그리고 그 선택이 주기마다 바뀌는 것이 팔이 갑자기 뒤집히는 이유입니다.

수치해법을 쓸 때 마지막으로 기억할 것은, 반복이 수렴했다는 것과 답이 쓸 만하다는 것이 다른 이야기라는 점입니다. 수렴한 각도가 관절 한계 안에 있는지, 도중의 경로가 자기 몸을 통과하지 않는지, 그 자세의 조건수가 감당할 만한지는 전부 별도로 확인해야 합니다.

여기까지가 "어디로 가야 하는가"의 답입니다. 그 각도로 실제로 부드럽게 움직이는 일은 제어 루프 편에서 이어집니다. 그리고 이 글에 나온 의사역행렬, 특이값 분해, 조건수 같은 도구를 어느 수준까지 공부해야 하는지는 로봇공학에 필요한 수학 편에 순서대로 정리해 두었습니다.