Python으로 역운동학(Inverse Kinematics) 구현하기: '손끝을 여기에 두고 싶다'에서 관절 각도를 구하는 방법

순운동학(Forward Kinematics)에서는 '관절 각도 → 손끝 위치'를 삼각 함수만으로 계산했습니다. 실제 로봇 제어에서 보통 필요한 것은 역방향, 즉 '이 위치에 손끝을 두고 싶다. 관절을 몇 도로 해야 할까?'입니다. 이것이 **역운동학(Inverse Kinematics, IK)**이며, 순운동학과 달리 유일하게 해를 구할 수 없거나 아예 해가 없는 등 다소 복잡한 문제입니다[1]. 이 글에서는 2관절 암을 소재로 기하학적으로 푸는 방법과 수치적으로 푸는 방법을 모두 Python으로 구현합니다.

왜 역운동학은 어려울까요

순운동학은 '각도를 넣으면 좌표가 하나 나오는' 함수였습니다. 역운동학은 그 역함수를 구하는 것인데, 삼각 함수는 주기적이기 때문에 같은 손끝 위치에 도달하는 관절 각도의 조합이 여러 개일 수 있습니다. 게다가 팔의 길이보다 먼 곳은 물리적으로 애초에 닿을 수 없습니다.

역운동학에서 발생하는 3가지 경우

🎯 해가 하나인 경우 (팔을 곧게 편 한계 위치) → 🔀 해가 여러 개인 경우 (팔꿈치를 올리든 내리든 도달 가능) → 🚫 해가 없는 경우 (팔의 길이보다 멀거나 너무 가까움)

준비

pip install numpy

① 기하학적으로 풀기: 코사인 법칙으로 관절 각도 역산하기

길이 l1와 l2의 2관절 암에서, 손끝을 (x, y)에 두고 싶다고 가정합니다. 원점부터 목표까지의 거리 r와 팔꿈치 각도(두 번째 관절의 각도) 사이에는 삼각형의 코사인 법칙 관계가 있습니다.

import numpy as np
def inverse_kinematics(x, y, l1=1.0, l2=0.7, elbow="up"):
r2 = x**2 + y**2
...

arccos는 항상 0~π 범위만 반환하므로, 부호(sign)를 반전시킴으로써 팔꿈치를 올린 해와 내린 해 모두 만들 수 있습니다. 이것이 '해가 여러 개'인 정체입니다.

② 순운동학으로 검산하기

이전에 구현했던 순운동학에 대입하여, 정말 목표에 도달했는지 확인합니다.

def forward_kinematics(theta1, theta2, l1=1.0, l2=0.7):
x1 = l1 * np.cos(theta1)
y1 = l1 * np.sin(theta1)
...

up : theta=(-9.6, 81.8)도 손끝=[1.2 0.5]
down: theta=(54.8, -81.8)도 손끝=[1.2 0.5]
닿지 않는 목표: None

팔꿈치를 올리든 내리든 관절 각도를 사용해 순운동학에 통과시키면 제대로 목표 (1.2, 0.5)에 일치합니다. 팔 길이의 합 l1+l2=1.7보다 먼 (5, 5)는 None——해가 없음이 올바르게 감지되고 있습니다.

③ 수치적으로 풀기: 자코비 행렬로 조금씩 가깝게 만들기

2관절이라면 기하학적으로 풀 수 있지만, 관절이 6개나 7개가 되면 닫힌 식을 만드는 것이 힘들어집니다. 그래서 실무에서 자주 사용되는 것이 **자코비 행렬(Jacobian)**을 사용하여 목표에 조금씩 다가가는 수치 해법입니다.

def jacobian(theta1, theta2, l1=1.0, l2=0.7):
# 손끝 좌표(x,y)를 관절 각도(theta1,theta2)로 편미분한 2x2 행렬
return np.array([
...

수치해: [-9.58 81.79]도 손끝= [1.2 0.5]

기하학적 해법의 '팔꿈치 올림' 해와 거의 일치했습니다. 자코비 행렬은 '관절 각도를 약간 움직였을 때, 손끝이 어느 방향으로 얼마나 움직이는지'를 나타내는 행렬이며, 그 **유사 역행렬(pseudo-inverse)**을 사용함으로써 '손끝을 이 오차만큼 움직이게 하려면, 관절 각도를 어떻게 바꿔야 하는지'를 역산할 수 있습니다[2].

요약

• 역운동학 = 손끝 위치에서 관절 각도를 역산하는 계산. 순운동학과 달리 해가 여러 개일 수도 있고 해가 없을 수도 있다.
• 2관절 암은 코사인 법칙으로 기하학적으로 풀 수 있으며,
  arccos의 부호로 팔꿈치 올림/내림의 두 해를 만들 수 있다. - 목표까지의 거리가 팔 길이 범위 외라면 해 없음(None) - 관절이 늘어나 식이 만들기 어려울 때는, 자코비 행렬의 유사 역행렬을 이용해 조금씩 다가가는 수치 해법을 사용할 수 있다.

더 자세히 보기 (배경 및 이론)

역운동학(Inverse Kinematics)의 해는 일반적으로 관절 수와 메커니즘의 대칭성에 따라 달라집니다. 6자유도(DoF) 일반 암의 경우, 기하학적 트릭(마지막 3축을 한 점에 집중시키는 '스페리컬 손목' 등)을 사용하면 해석적인 해(closed-form solution)를 얻을 수 있으며, 최대 8가지의 해를 가질 수 있음이 알려져 있습니다[3]. 관절 수가 더 많은 과잉 자유도 암(인간의 팔과 유사한 7축 등)의 경우, 해가 무수히 많고 해석적인 식으로 풀 수 없기 때문에, 본 기사에서 다룬 자코비 행렬법이나 이를 확장한 감쇠 최소 제곱법(Damped Least Squares, DLS)[4]이 실무에서 널리 사용됩니다. DLS는 순운동학(Forward Kinematics) 기사에서 언급된 특이점(자코비 행렬이 퇴화하여 유사 역행렬이 불안정해지는 자세) 근처에서도 오차항에 작은 감쇠 계수를 추가하여 수치적인 발산을 억제함으로써 안정적으로 수렴하는 방법입니다.

다음 단계 🌸

2관절 암의 좌표 계산은 순운동학을 Python으로 구현하고, 암을 움직이는 제어는 PID 제어를 Python으로 구현하며, 로봇의 기술 형식은 URDF로 로봇을 표현해 보세요.

순운동학은 관절 각도 $
ightarrow$ 엔드 이펙터 위치에 대한 단일 사상(unique mapping)이지만, 역운동학은 그 역사상을 구하는 문제이며 일반적으로 비선형 방정식을 풀어야 합니다. 해의 개수와 존재성은 메커니즘(자유도 수나 링크 배치)에 따라 달라집니다. ↩︎

자코비 행렬 $J$는 엔드 이펙터 좌표를 관절 각도로 편미분한 행렬이며, 관절 속도와 엔드 이펙터 속도의 관계를 $dx = J ullet d heta$로 나타냅니다. 반대로 $d heta = J^+ ullet dx$ ($J^+$는 유사 역행렬)를 통해 엔드 이펙터를 목표에 가깝게 만드는 각도 변화량을 구할 수 있습니다. ↩︎

손목이 스페리컬(3축이 한 점에서 만나는) 구조인 6자유도 암의 경우, 위치 결정과 자세 결정을 분리할 수 있어 기하학적으로 해석적인 식으로 풀립니다. 분기(팔꿈치 올리기/내리기, 손목 방향 등) 조합에 따라 최대 8가지의 해를 가질 수 있습니다. ↩︎

감쇠 최소 제곱법은 오차 $d heta = J^T(JJ^T + ext{λ}^2 I)^{-1} ullet dx$와 같이 작은 정규화 항 $ ext{λ}^2$을 추가하여, 특이점 근처에서 자코비 행렬이 거의 특이하더라도 관절 각도가 발산하지 않도록 하는 방법입니다. 로봇 제어 분야에서 널리 사용됩니다. ↩︎

토론 (Discussion)