6.90 역기구학 해법에서의 자코비안 활용

역기구학(inverse kinematics)은 원하는 말단 장치의 위치와 자세가 주어졌을 때 이를 실현하는 관절 변수를 구하는 문제이다. 해석적 닫힌 형태(closed-form)의 해가 존재하지 않는 일반적인 경우, 자코비안 행렬에 기반한 수치적 반복 해법이 널리 사용된다. 본 절에서는 자코비안을 활용한 역기구학의 수치적 해법을 다룬다.

1. 역기구학 문제의 정식화

순기구학 함수 $\mathbf{f}: \mathbb{R}^n \to \mathbb{R}^m$ 이 주어졌을 때, 역기구학 문제는 원하는 작업 공간 목표 $\mathbf{x}_d \in \mathbb{R}^m$ 에 대해 다음을 만족하는 관절 변수 $\mathbf{q}^*$ 를 구하는 것이다.

$\mathbf{f}(\mathbf{q}^*) = \mathbf{x}_d$

이 문제는 일반적으로 비선형 방정식의 해를 구하는 문제에 해당하며, 자코비안을 이용한 선형화에 기초하여 반복적으로 해를 개선한다.

2. 뉴턴-랩슨 방법

뉴턴-랩슨(Newton-Raphson) 방법은 비선형 방정식의 근을 구하는 대표적인 반복 해법이다. 작업 공간 오차를 다음과 같이 정의한다.

$\mathbf{e}(\mathbf{q}) = \mathbf{x}_d - \mathbf{f}(\mathbf{q})$

현재 추정값 $\mathbf{q}_k$ 에서의 오차를 소멸시키기 위한 관절 변수 보정량 $\delta\mathbf{q}_k$ 를 자코비안의 1차 선형 근사로부터 구한다.

$\mathbf{e}(\mathbf{q}_k) \approx J(\mathbf{q}_k) \, \delta\mathbf{q}_k$

정방 비특이 자코비안( $m = n$ , $\det(J) \neq 0$ )인 경우, 보정량은 다음과 같다.

$\delta\mathbf{q}_k = J^{-1}(\mathbf{q}_k) \, \mathbf{e}(\mathbf{q}_k)$

관절 변수를 갱신한다.

$\mathbf{q}_{k+1} = \mathbf{q}_k + \delta\mathbf{q}_k$

이 과정을 $\|\mathbf{e}(\mathbf{q}_k)\| < \epsilon$ (허용 오차) 또는 최대 반복 횟수에 도달할 때까지 반복한다. 뉴턴-랩슨 방법은 초기 추정값이 해에 충분히 가까운 경우 2차(quadratic) 수렴 속도를 보인다.

3. 자코비안 역행렬 방법

정방 자코비안에 대해 역행렬을 직접 이용하는 방법이다. 반복 갱신식은 다음과 같다.

$\mathbf{q}_{k+1} = \mathbf{q}_k + \alpha \, J^{-1}(\mathbf{q}_k) \, \mathbf{e}(\mathbf{q}_k)$

여기서 $\alpha \in (0, 1]$ 는 스텝 크기(step size) 매개변수로서 수렴 안정성을 조절한다. $\alpha = 1$ 이면 순수한 뉴턴-랩슨 방법이 되며, $\alpha < 1$ 이면 감쇠 뉴턴(damped Newton) 방법이 된다. 스텝 크기를 줄이면 수렴 속도는 감소하나 수렴 영역(basin of convergence)이 확대된다.

4. 자코비안 전치 방법

자코비안의 역행렬 계산이 불가능하거나 비용이 높은 경우, 자코비안의 전치 행렬을 대안으로 사용할 수 있다.

$\mathbf{q}_{k+1} = \mathbf{q}_k + \alpha \, J^\top(\mathbf{q}_k) \, \mathbf{e}(\mathbf{q}_k)$

이 방법은 경사 하강법(gradient descent)의 한 형태로 해석할 수 있다. 오차의 제곱 노름 $\frac{1}{2}\|\mathbf{e}\|^2$ 를 비용 함수로 볼 때, $J^\top \mathbf{e}$ 는 이 비용 함수의 관절 공간에서의 음의 경사 방향에 해당한다.

$\nabla_{\mathbf{q}} \left(\frac{1}{2}\|\mathbf{e}\|^2\right) = -J^\top \mathbf{e}$

자코비안 전치 방법의 장점은 역행렬이나 의사 역행렬의 계산이 불필요하고 특이점 근방에서도 수치적으로 안정하다는 것이다. 단점은 수렴 속도가 느리고 스텝 크기 $\alpha$ 의 적절한 선택이 중요하다는 것이다.

5. 자코비안 의사 역행렬 방법

비정방 자코비안( $m \neq n$ )의 경우, 의사 역행렬(pseudoinverse)을 사용한다.

5.1 여유 자유도 시스템 ( $m < n$ )

관절 수가 작업 공간 차원보다 많은 여유 자유도(redundant) 시스템에서는 무한히 많은 해가 존재한다. 최소 노름 해를 제공하는 우측 의사 역행렬을 사용한다.

$\delta\mathbf{q}_k = J^\dagger(\mathbf{q}_k) \, \mathbf{e}(\mathbf{q}_k)$

$J^\dagger = J^\top (J J^\top)^{-1}$

이 해는 $\|\delta\mathbf{q}\|$ 를 최소화하면서 오차 방정식 $J \delta\mathbf{q} = \mathbf{e}$ 를 만족하는 해이다.

5.2 과결정 시스템 ( $m > n$ )

작업 공간 요구가 관절 수보다 많은 경우, 일반적으로 정확한 해가 존재하지 않으며 최소 제곱 의미에서의 최적 해를 구한다.

$\delta\mathbf{q}_k = (J^\top J)^{-1} J^\top \, \mathbf{e}(\mathbf{q}_k)$

6. 댐핑 최소 제곱법

특이점 근방에서 자코비안의 조건수(condition number)가 급격히 증가하면 표준 역행렬 또는 의사 역행렬 방법은 수치적 불안정을 유발한다. 이를 해결하기 위해 댐핑 최소 제곱법(damped least squares, DLS)을 사용한다. 이 방법은 Levenberg-Marquardt 알고리즘이라고도 한다.

$\delta\mathbf{q}_k = J^\top (J J^\top + \lambda^2 I)^{-1} \, \mathbf{e}(\mathbf{q}_k)$

여기서 $\lambda > 0$ 는 댐핑 계수(damping factor)이다. 이는 다음의 최적화 문제의 해에 해당한다.

$\min_{\delta\mathbf{q}} \left(\|J \delta\mathbf{q} - \mathbf{e}\|^2 + \lambda^2 \|\delta\mathbf{q}\|^2\right)$

댐핑 계수 $\lambda$ 의 효과는 다음과 같다.

$\lambda$ 값	효과
$\lambda \to 0$	표준 의사 역행렬에 수렴
$\lambda$ 크면	자코비안 전치 방법에 근접, 보정량 감소
적응적 $\lambda$	특이점 근방에서 증가, 비특이 영역에서 감소

적응적 댐핑 계수의 대표적인 설정은 자코비안의 최소 특이값 $\sigma_{\min}$ 에 기반한다.

$\lambda^2 = \begin{cases} 0 & \text{if } \sigma_{\min} \geq \epsilon_0 \\ \lambda_{\max}^2 \left(1 - \left(\frac{\sigma_{\min}}{\epsilon_0}\right)^2\right) & \text{if } \sigma_{\min} < \epsilon_0 \end{cases}$

여기서 $\epsilon_0$ 는 특이점 판정 임계값, $\lambda_{\max}$ 는 최대 댐핑 계수이다.

7. 수렴 판정 기준

반복 해법의 수렴은 다음 기준 중 하나 이상을 만족할 때 판정한다.

위치 오차 기준: $\|\mathbf{p}_d - \mathbf{p}(\mathbf{q}_k)\| < \epsilon_p$
자세 오차 기준: $\|\Delta\boldsymbol{\Phi}\| < \epsilon_o$ 또는 회전 행렬 오차에 기반한 기준
관절 보정량 기준: $\|\delta\mathbf{q}_k\| < \epsilon_q$
최대 반복 횟수: $k > k_{\max}$

자세 오차의 정량화에서 주의할 점은, 오일러 각의 차이가 회전 오차를 정확히 반영하지 않을 수 있다는 것이다. 회전 행렬 오차에 기반한 자세 오차 벡터는 다음과 같이 계산된다.

$\mathbf{e}_o = \frac{1}{2}\left(\mathbf{n} \times \mathbf{n}_d + \mathbf{s} \times \mathbf{s}_d + \mathbf{a} \times \mathbf{a}_d\right)$

여기서 $(\mathbf{n}, \mathbf{s}, \mathbf{a})$ 와 $(\mathbf{n}_d, \mathbf{s}_d, \mathbf{a}_d)$ 는 각각 현재와 목표 회전 행렬의 열 벡터이다.

8. 초기값 선택 전략

자코비안 기반 반복 해법은 비선형 최적화이므로 초기값 $\mathbf{q}_0$ 의 선택이 수렴 여부와 수렴 속도에 큰 영향을 미친다. 초기값 선택 전략에는 다음이 있다.

현재 관절 위치: 실시간 제어에서 가장 자연스러운 선택이며, 연속적 경로 추종 시 이전 시각의 관절 값을 사용한다.
해석적 근사 해: 간략화된 모델에 대한 해석적 해를 초기 추정값으로 사용한다.
다중 초기점: 여러 초기값에서 독립적으로 반복을 수행하여 가장 좋은 해를 선택한다. 다중 해가 존재하는 경우에 유용하다.

9. 여유 자유도의 활용

$m < n$ 인 여유 자유도 시스템에서 의사 역행렬 해에 영 공간 성분을 추가하여 부차 목표를 동시에 추구할 수 있다.

$\delta\mathbf{q}_k = J^\dagger \mathbf{e}_k + (I - J^\dagger J) \, \mathbf{q}_0$

여기서 $(I - J^\dagger J)$ 는 자코비안의 영 공간으로의 투영 행렬이고, $\mathbf{q}_0 \in \mathbb{R}^n$ 는 부차 목표를 반영하는 임의의 벡터이다. 대표적인 부차 목표로는 관절 한계 회피, 특이점 회피, 장애물 회피 등이 있다.

10. 알고리즘 요약

자코비안 기반 역기구학의 반복 알고리즘을 정리하면 다음과 같다.

입력: 목표 $\mathbf{x}_d$ , 초기 추정 $\mathbf{q}_0$ , 허용 오차 $\epsilon$ , 최대 반복 수 $k_{\max}$

$k \leftarrow 0$
repeat

$\mathbf{e}_k \leftarrow \mathbf{x}_d - \mathbf{f}(\mathbf{q}_k)$ (작업 공간 오차 계산)
$J_k \leftarrow J(\mathbf{q}_k)$ (자코비안 계산)
$\delta\mathbf{q}_k \leftarrow J_k^{-1} \mathbf{e}_k$ 또는 $J_k^\dagger \mathbf{e}_k$ 또는 DLS (보정량 계산)
$\mathbf{q}_{k+1} \leftarrow \mathbf{q}_k + \alpha \, \delta\mathbf{q}_k$ (관절 변수 갱신)
$k \leftarrow k + 1$

until $\|\mathbf{e}_k\| < \epsilon$ or $k > k_{\max}$

출력: $\mathbf{q}_k$ (역기구학 해)

11. 참고 문헌

Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. (2009). Robotics: Modelling, Planning and Control. Springer.
Craig, J. J. (2005). Introduction to Robotics: Mechanics and Control. 3rd ed. Pearson Prentice Hall.
Buss, S. R. (2004). “Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoinverse and Damped Least Squares Methods.” IEEE Journal of Robotics and Automation, 17(1–19).
Nakamura, Y. (1991). Advanced Robotics: Redundancy and Optimization. Addison-Wesley.
Wampler, C. W. (1986). “Manipulator Inverse Kinematic Solutions Based on Vector Formulations and Damped Least-Squares Methods.” IEEE Transactions on Systems, Man, and Cybernetics, 16(1), 93–101.

v 0.1