6.102 의사 역행렬의 로봇 역기구학 응용

1. 개요

로봇 역기구학(inverse kinematics)은 원하는 말단 장치(end-effector)의 자세로부터 관절 변수를 구하는 문제이다. 해석적 해가 존재하지 않는 일반적인 로봇 구조에서는 야코비안(Jacobian)의 의사 역행렬(pseudoinverse)을 이용한 수치적 반복법이 널리 사용된다. 이 절에서는 의사 역행렬의 정의와 성질을 복습하고, 역기구학 문제에 적용하는 구체적인 방법론을 다룬다.

2. 야코비안과 미분 기구학

로봇 매니퓰레이터에서 말단 장치의 태스크 공간 속도 $\dot{\mathbf{x}} \in \mathbb{R}^m$ 와 관절 속도 $\dot{\boldsymbol{\theta}} \in \mathbb{R}^n$ 의 관계는 야코비안 행렬 $\mathbf{J}(\boldsymbol{\theta}) \in \mathbb{R}^{m \times n}$ 을 통해 다음과 같이 표현된다.

$\dot{\mathbf{x}} = \mathbf{J}(\boldsymbol{\theta}) \dot{\boldsymbol{\theta}}$

역기구학 문제는 원하는 태스크 공간 속도 $\dot{\mathbf{x}}_d$ 가 주어졌을 때 대응하는 관절 속도 $\dot{\boldsymbol{\theta}}$ 를 구하는 것이다. 이 문제의 구조는 야코비안의 차원과 랭크에 따라 달라진다.

3. 의사 역행렬의 정의

3.1 Moore-Penrose 의사 역행렬

임의의 행렬 $\mathbf{A} \in \mathbb{R}^{m \times n}$ 에 대해 Moore-Penrose 의사 역행렬 $\mathbf{A}^+ \in \mathbb{R}^{n \times m}$ 는 다음 네 가지 조건을 동시에 만족하는 유일한 행렬이다.

$\mathbf{A} \mathbf{A}^+ \mathbf{A} = \mathbf{A}$
$\mathbf{A}^+ \mathbf{A} \mathbf{A}^+ = \mathbf{A}^+$
$(\mathbf{A} \mathbf{A}^+)^\top = \mathbf{A} \mathbf{A}^+$
$(\mathbf{A}^+ \mathbf{A})^\top = \mathbf{A}^+ \mathbf{A}$

3.2 특이값 분해를 통한 계산

$\mathbf{A}$ 의 특이값 분해가 $\mathbf{A} = \mathbf{U} \boldsymbol{\Sigma} \mathbf{V}^\top$ 이면

$\mathbf{A}^+ = \mathbf{V} \boldsymbol{\Sigma}^+ \mathbf{U}^\top$

여기서 $\boldsymbol{\Sigma}^+$ 는 $\boldsymbol{\Sigma}$ 의 0이 아닌 특이값의 역수를 취하고 전치한 행렬이다. 구체적으로 $\boldsymbol{\Sigma} = \text{diag}(\sigma_1, \sigma_2, \dots, \sigma_r, 0, \dots, 0)$ 이면

$\boldsymbol{\Sigma}^+ = \text{diag}(1/\sigma_1, 1/\sigma_2, \dots, 1/\sigma_r, 0, \dots, 0)^\top$

이다.

4. 세 가지 경우의 역기구학

4.1 경우 1: 정방 비특이 야코비안 ( $m = n$ , $\text{rank}(\mathbf{J}) = n$ )

정방 야코비안이 비특이일 때 의사 역행렬은 일반 역행렬과 동일하다.

$\dot{\boldsymbol{\theta}} = \mathbf{J}^{-1} \dot{\mathbf{x}}_d$

유일한 해가 존재하며 태스크 오차는 0이다.

4.2 경우 2: 과결정 시스템 ( $m > n$ )

태스크 공간 차원이 관절 차원보다 큰 경우이다. 일반적으로 정확한 해가 존재하지 않으므로, 최소 제곱 의미에서의 최적해를 구한다.

$\dot{\boldsymbol{\theta}} = \mathbf{J}^+ \dot{\mathbf{x}}_d = (\mathbf{J}^\top \mathbf{J})^{-1} \mathbf{J}^\top \dot{\mathbf{x}}_d$

이는 잔차 $\| \mathbf{J} \dot{\boldsymbol{\theta}} - \dot{\mathbf{x}}_d \|_2$ 를 최소화하는 해이다. $\mathbf{J}$ 가 열 풀 랭크(full column rank)일 때 $\mathbf{J}^\top \mathbf{J}$ 가 가역이므로 위 식이 성립한다.

4.3 경우 3: 과소결정(여유 자유도) 시스템 ( $m < n$ )

관절 수가 태스크 공간 차원보다 큰 여유 자유도(redundant) 로봇의 경우이다. 무한히 많은 해가 존재하며, 의사 역행렬은 관절 속도의 노름이 최소인 해를 제공한다.

$\dot{\boldsymbol{\theta}} = \mathbf{J}^+ \dot{\mathbf{x}}_d = \mathbf{J}^\top (\mathbf{J} \mathbf{J}^\top)^{-1} \dot{\mathbf{x}}_d$

이는 $\mathbf{J} \dot{\boldsymbol{\theta}} = \dot{\mathbf{x}}_d$ 를 만족하는 모든 해 중에서 $\| \dot{\boldsymbol{\theta}} \|_2$ 를 최소화하는 해이다. $\mathbf{J}$ 가 행 풀 랭크(full row rank)일 때 $\mathbf{J} \mathbf{J}^\top$ 가 가역이므로 위 식이 성립한다.

5. 의사 역행렬 역기구학의 최소 노름 성질

5.1 최소 노름 해의 증명

$\mathbf{J} \dot{\boldsymbol{\theta}} = \dot{\mathbf{x}}_d$ 를 만족하는 임의의 해 $\dot{\boldsymbol{\theta}}$ 를 $\dot{\boldsymbol{\theta}} = \mathbf{J}^+ \dot{\mathbf{x}}_d + (\mathbf{I} - \mathbf{J}^+ \mathbf{J}) \mathbf{z}$ 로 분해할 수 있다. 여기서 $\mathbf{z} \in \mathbb{R}^n$ 는 임의의 벡터이다. $(\mathbf{I} - \mathbf{J}^+ \mathbf{J})$ 는 $\mathbf{J}$ 의 영 공간(null space)으로의 직교 사영 행렬이므로

$\| \dot{\boldsymbol{\theta}} \|_2^2 = \| \mathbf{J}^+ \dot{\mathbf{x}}_d \|_2^2 + \| (\mathbf{I} - \mathbf{J}^+ \mathbf{J}) \mathbf{z} \|_2^2$

이 성립한다. 교차항이 소멸하는 이유는 $\mathbf{J}^+ \dot{\mathbf{x}}_d$ 가 $\mathbf{J}$ 의 행 공간(row space)에 속하고 $(\mathbf{I} - \mathbf{J}^+ \mathbf{J}) \mathbf{z}$ 가 영 공간에 속하여 직교하기 때문이다. 따라서 $\mathbf{z} = \mathbf{0}$ 일 때, 즉 $\dot{\boldsymbol{\theta}} = \mathbf{J}^+ \dot{\mathbf{x}}_d$ 일 때 노름이 최소가 된다.

6. 수치적 역기구학 반복 알고리즘

6.1 속도 수준 반복법 (Resolved Rate Method)

Whitney(1969)가 제안한 분해 속도법은 다음과 같다.

현재 관절 각도 $\boldsymbol{\theta}_k$ 에서 순기구학으로 말단 위치 $\mathbf{x}_k = f(\boldsymbol{\theta}_k)$ 를 계산한다.
태스크 공간 오차를 계산한다: $\mathbf{e}_k = \mathbf{x}_d - \mathbf{x}_k$
야코비안 $\mathbf{J}(\boldsymbol{\theta}_k)$ 를 계산한다.
관절 갱신량을 계산한다.

$\Delta \boldsymbol{\theta}_k = \mathbf{J}^+(\boldsymbol{\theta}_k) \mathbf{e}_k$

관절 각도를 갱신한다: $\boldsymbol{\theta}_{k+1} = \boldsymbol{\theta}_k + \alpha \Delta \boldsymbol{\theta}_k$

여기서 $\alpha \in (0, 1]$ 는 스텝 크기(step size)이며, 수렴 안정성을 위해 도입한다.

6.2 수렴 조건

위 알고리즘의 수렴은 야코비안의 특이값과 관련된다. $\mathbf{J}$ 가 상수라 가정하면, 오차의 갱신은

$\mathbf{e}_{k+1} \approx (\mathbf{I} - \alpha \mathbf{J} \mathbf{J}^+) \mathbf{e}_k$

이므로, $\mathbf{J}$ 가 풀 랭크이면 $\mathbf{J} \mathbf{J}^+ = \mathbf{I}$ 가 되어 $\alpha = 1$ 에서 한 단계에 수렴한다. 비선형성 때문에 실제로는 여러 번의 반복이 필요하다.

7. 여유 자유도 로봇에서의 영 공간 활용

$m < n$ 인 여유 자유도 로봇에서는 영 공간 사영을 활용하여 부가 목적을 달성할 수 있다.

$\dot{\boldsymbol{\theta}} = \mathbf{J}^+ \dot{\mathbf{x}}_d + (\mathbf{I} - \mathbf{J}^+ \mathbf{J}) \dot{\boldsymbol{\theta}}_0$

여기서 $\dot{\boldsymbol{\theta}}_0 \in \mathbb{R}^n$ 는 부가 목적에 해당하는 관절 속도이다. $(\mathbf{I} - \mathbf{J}^+ \mathbf{J})$ 가 영 공간 사영 행렬이므로, $\dot{\boldsymbol{\theta}}_0$ 를 영 공간에 사영한 성분만 더해져 주 태스크에 영향을 주지 않는다.

7.1 부가 목적의 예시

부가 목적	$\dot{\boldsymbol{\theta}}_0$ 의 설정
관절 한계 회피	$\dot{\boldsymbol{\theta}}_0 = k \nabla_{\boldsymbol{\theta}} w(\boldsymbol{\theta})$
특이점 회피	$\dot{\boldsymbol{\theta}}_0 = k \nabla_{\boldsymbol{\theta}} \sqrt{\det(\mathbf{J}\mathbf{J}^\top)}$
장애물 회피	$\dot{\boldsymbol{\theta}}_0 = k \nabla_{\boldsymbol{\theta}} d_{\min}(\boldsymbol{\theta})$
에너지 최소화	$\dot{\boldsymbol{\theta}}_0 = -k \nabla_{\boldsymbol{\theta}} E(\boldsymbol{\theta})$

여기서 $w(\boldsymbol{\theta})$ 는 관절 한계로부터의 거리에 관한 함수, $d_{\min}$ 은 장애물까지의 최소 거리, $E(\boldsymbol{\theta})$ 는 에너지 함수이며, $k > 0$ 는 이득 상수이다.

8. 위치 수준 역기구학

위 속도 수준의 방법을 위치 수준으로 확장하면, 목표 자세 $\mathbf{x}_d$ 에 도달하기 위한 반복 알고리즘이 된다.

$\boldsymbol{\theta}_{k+1} = \boldsymbol{\theta}_k + \mathbf{J}^+(\boldsymbol{\theta}_k) (\mathbf{x}_d - f(\boldsymbol{\theta}_k))$

이는 뉴턴-랩슨법(Newton-Raphson method)의 변형으로, $f(\boldsymbol{\theta})$ 가 비선형 순기구학 함수일 때 그 선형화를 반복적으로 적용하는 것이다.

8.1 회전 표현의 처리

위치 오차는 단순한 벡터 차이로 계산할 수 있으나, 자세(orientation) 오차는 회전 표현에 따라 특별한 처리가 필요하다. 회전 행렬 $\mathbf{R}_d$ 와 $\mathbf{R}_k$ 에 대해 오차 회전 행렬 $\mathbf{R}_e = \mathbf{R}_d \mathbf{R}_k^\top$ 을 구하고, 이를 등가 축-각도 표현으로 변환하여 각속도 오차 벡터를 얻는다.

$\mathbf{e}_o = \theta_e \hat{\mathbf{k}}_e$

여기서 $\theta_e$ 와 $\hat{\mathbf{k}}_e$ 는 $\mathbf{R}_e$ 의 등가 회전 각도와 축이다.

9. 구현 시 고려 사항

9.1 야코비안의 수치 계산

해석적 야코비안이 구하기 어려운 경우, 수치 미분을 통해 야코비안을 근사할 수 있다.

$J_{ij} \approx \frac{f_i(\boldsymbol{\theta} + \delta \mathbf{e}_j) - f_i(\boldsymbol{\theta})}{\delta}$

여기서 $\mathbf{e}_j$ 는 $j$ 번째 단위 벡터이고 $\delta$ 는 소량의 섭동이다.

9.2 SVD 기반 구현

의사 역행렬을 직접 계산하는 대신, SVD를 통해 계산하는 것이 수치적으로 안정적이다.

$\mathbf{J}^+ = \mathbf{V} \boldsymbol{\Sigma}^+ \mathbf{U}^\top$

작은 특이값( $\sigma_i < \epsilon$ )을 0으로 처리하는 절삭(truncated) SVD를 사용하면 수치적 안정성을 더욱 높일 수 있다.

10. 참고 문헌

Whitney, D. E. (1969). “Resolved Motion Rate Control of Manipulators and Human Prostheses.” IEEE Transactions on Man-Machine Systems, 10(2), 47–53.
Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. (2009). Robotics: Modelling, Planning and Control. Springer.
Nakamura, Y. (1991). Advanced Robotics: Redundancy and Optimization. Addison-Wesley.
Golub, G. H., & Van Loan, C. F. (2013). Matrix Computations (4th ed.). Johns Hopkins University Press.
Buss, S. R. (2004). “Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoinverse and Damped Least Squares methods.” Department of Mathematics, University of California, San Diego.

v 0.1