7.99 뉴턴 방법의 로봇 역기구학 응용

1. 역기구학 문제의 정의

역기구학(inverse kinematics, IK)은 로봇 말단 장치(end-effector)의 원하는 위치와 자세가 주어질 때, 이를 달성하는 관절 변수 $\mathbf{q} \in \mathbb{R}^n$ 을 구하는 문제이다. 순방향 기구학 함수 $\mathbf{f}(\mathbf{q}): \mathbb{R}^n \to \mathbb{R}^m$ 이 관절 변수로부터 말단 장치의 작업 공간 좌표 $\mathbf{x}_d \in \mathbb{R}^m$ 을 산출할 때, 역기구학은 다음의 비선형 방정식계를 푸는 것이다.

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

6자유도 매니퓰레이터에서 위치와 자세를 모두 지정하면 $m = 6$ 이다. 이 비선형 방정식계는 일반적으로 해석적 해를 갖지 않으므로, 수치적 반복법이 필요하다.

2. 뉴턴-라프슨 방법의 적용

비선형 방정식 $\mathbf{f}(\mathbf{q}) - \mathbf{x}_d = \mathbf{0}$ 에 뉴턴-라프슨(Newton-Raphson) 방법을 적용하면 다음의 갱신 규칙을 얻는다.

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

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

여기서 $\mathbf{J}(\mathbf{q}_k) = \frac{\partial \mathbf{f}}{\partial \mathbf{q}}\big\vert_{\mathbf{q}_k} \in \mathbb{R}^{m \times n}$ 은 기하학적 야코비안 행렬, $\mathbf{e}_k = \mathbf{x}_d - \mathbf{f}(\mathbf{q}_k)$ 는 작업 공간 오차이다.

3. 정방 시스템의 경우 ( $m = n$ )

관절 수와 작업 공간 차원이 동일한 정방(square) 시스템에서 야코비안이 비특이이면 다음과 같이 뉴턴 스텝을 구한다.

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

이 방법은 초기 추정이 해에 충분히 가까우면 이차 수렴을 달성한다. 6자유도 매니퓰레이터에서 $6 \times 6$ 야코비안의 역행렬은 $O(m^3)$ 에 계산 가능하며, 로봇 역기구학에서의 실시간 해법으로 충분히 빠르다.

4. 과결정 시스템의 경우 ( $m < n$ , 여유 자유도)

관절 수가 작업 공간 차원보다 큰 여유 자유도(redundant) 시스템에서는 무한히 많은 해가 존재한다. 최소 노름 해(minimum-norm solution)는 야코비안의 의사 역행렬(pseudoinverse)을 사용하여 구한다.

$\Delta\mathbf{q}_k = \mathbf{J}^T(\mathbf{q}_k)[\mathbf{J}(\mathbf{q}_k)\mathbf{J}^T(\mathbf{q}_k)]^{-1}\mathbf{e}_k = \mathbf{J}^\dagger(\mathbf{q}_k)\mathbf{e}_k$

여유 자유도를 활용하여 부차적 목표(관절 한계 회피, 장애물 회피, 조작성 최대화 등)를 달성하기 위해 영공간 투영(null-space projection)이 추가된다.

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

여기서 $\mathbf{q}_0$ 는 부차적 목표를 향한 그래디언트이고, $(\mathbf{I} - \mathbf{J}^\dagger\mathbf{J})$ 는 야코비안의 영공간으로의 투영 행렬이다.

5. 최적화 기반 역기구학

역기구학을 비선형 최소 제곱 문제로 정식화하면 다음과 같다.

$\min_{\mathbf{q}} \quad \frac{1}{2}\lVert \mathbf{f}(\mathbf{q}) - \mathbf{x}_d \rVert^2$

이 문제에 가우스-뉴턴법을 적용하면 다음의 갱신을 얻는다.

$(\mathbf{J}^T\mathbf{J})\Delta\mathbf{q}_k = \mathbf{J}^T\mathbf{e}_k$

정규 방정식(normal equation) $\mathbf{J}^T\mathbf{J}\Delta\mathbf{q} = \mathbf{J}^T\mathbf{e}$ 의 해는 의사 역행렬에 의한 해와 동치이다. 추가적인 비용 항을 도입하여 관절 한계나 선호 자세를 반영할 수 있다.

$\min_{\mathbf{q}} \quad \frac{1}{2}\lVert \mathbf{f}(\mathbf{q}) - \mathbf{x}_d \rVert^2_{\mathbf{W}_x} + \frac{1}{2}\lVert \mathbf{q} - \mathbf{q}_{ref} \rVert^2_{\mathbf{W}_q}$

여기서 $\mathbf{W}_x$ 와 $\mathbf{W}_q$ 는 가중 행렬, $\mathbf{q}_{ref}$ 는 기준 관절 자세이다. 이 가중 최소 제곱의 해는 다음과 같다.

$\Delta\mathbf{q}_k = (\mathbf{J}^T\mathbf{W}_x\mathbf{J} + \mathbf{W}_q)^{-1}(\mathbf{J}^T\mathbf{W}_x\mathbf{e}_k + \mathbf{W}_q(\mathbf{q}_{ref} - \mathbf{q}_k))$

6. 특이점 처리

야코비안이 특이(singular)하거나 근특이(near-singular)인 자세에서는 역행렬이 존재하지 않거나 수치적으로 불안정하다. 이를 처리하기 위한 방법은 다음과 같다.

6.1 댐핑된 최소 제곱법(Damped Least Squares, DLS)

레벤버그-마쿼트 정규화를 적용한 것으로, 요시카와(Yoshikawa)와 나카무라(Nakamura)에 의해 로봇 역기구학에 도입되었다.

$\Delta\mathbf{q}_k = (\mathbf{J}^T\mathbf{J} + \lambda^2\mathbf{I})^{-1}\mathbf{J}^T\mathbf{e}_k = \mathbf{J}^T(\mathbf{J}\mathbf{J}^T + \lambda^2\mathbf{I})^{-1}\mathbf{e}_k$

댐핑 인자 $\lambda > 0$ 은 특이점 근방에서 관절 속도의 폭주를 방지하며, 특이점에서 멀어지면 $\lambda \to 0$ 으로 설정하여 정밀한 추종을 달성한다. $\lambda$ 의 적응적 선택을 위해 조작성 지표(manipulability measure) $w = \sqrt{\det(\mathbf{J}\mathbf{J}^T)}$ 에 기반한 다음의 스케줄이 사용된다.

$\lambda^2 = \begin{cases} \lambda_0^2(1 - (w/w_0)^2) & \text{if } w < w_0 \\ 0 & \text{if } w \geq w_0 \end{cases}$

6.2 특이값 분해(SVD) 기반 접근

야코비안의 특이값 분해 $\mathbf{J} = \mathbf{U}\boldsymbol{\Sigma}\mathbf{V}^T$ 를 수행하고, 작은 특이값에 해당하는 성분을 선택적으로 감쇠하거나 제거하는 방법이다. 이 접근법은 특이점의 구조를 직접적으로 파악할 수 있으며, 방향에 따라 다른 감쇠를 적용할 수 있다.

7. 수렴 속도와 초기 추정

뉴턴-라프슨 기반 역기구학의 수렴은 초기 추정 $\mathbf{q}_0$ 의 품질에 크게 의존한다. 실시간 궤적 추종에서는 이전 시각의 관절 각도를 초기 추정으로 사용하면, 갱신량이 작으므로 1~3회의 반복으로 충분한 수렴을 달성하는 경우가 많다. 대폭적인 자세 변화 시에는 여러 초기점에서 시작하여 다수의 해를 탐색하고, 최적의 해를 선택하는 전략이 사용된다.

8. 참고 문헌

Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. (2009). Robotics: Modelling, Planning and Control. Springer.
Nakamura, Y., & Hanafusa, H. (1986). “Inverse Kinematic Solutions with Singularity Robustness for Robot Manipulator Control.” Journal of Dynamic Systems, Measurement, and Control, 108(3), 163–171.
Buss, S. R. (2004). “Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoinverse and Damped Least Squares Methods.” Technical Report, University of California, San Diego.
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.

version: 1.0