6.100 가중 최소 제곱법과 댐핑 최소 제곱법

1. 개요

로봇공학에서 역기구학 문제나 파라미터 추정 문제를 풀 때, 단순 최소 제곱법만으로는 물리적 제약 조건을 반영하기 어렵거나 수치적 불안정성이 발생하는 경우가 많다. 가중 최소 제곱법(Weighted Least Squares, WLS)은 각 측정값 또는 방정식에 서로 다른 중요도를 부여하여 해를 구하는 기법이며, 댐핑 최소 제곱법(Damped Least Squares, DLS)은 특이점 근방에서의 수치적 불안정성을 완화하기 위해 정규화 항을 도입하는 기법이다. 이 절에서는 두 기법의 수학적 정의, 유도 과정, 그리고 로봇공학에서의 응용을 상세히 다룬다.

2. 일반 최소 제곱 문제의 복습

$m \times n$ 행렬 $\mathbf{A}$ 와 관측 벡터 $\mathbf{b} \in \mathbb{R}^m$ 에 대해 일반 최소 제곱 문제는 다음과 같이 정의된다.

$\min_{\mathbf{x}} \| \mathbf{A}\mathbf{x} - \mathbf{b} \|_2^2$

정규 방정식을 통해 해를 구하면 다음과 같다.

$\mathbf{A}^\top \mathbf{A} \mathbf{x} = \mathbf{A}^\top \mathbf{b}$

$\mathbf{A}^\top \mathbf{A}$ 가 가역일 때 해는

$\hat{\mathbf{x}} = (\mathbf{A}^\top \mathbf{A})^{-1} \mathbf{A}^\top \mathbf{b}$

이다. 그러나 이 방법은 모든 방정식에 동일한 가중치를 부여하며, $\mathbf{A}$ 가 랭크 결핍(rank deficient)이거나 특이값이 매우 작은 경우에 수치적 불안정성을 보인다.

3. 가중 최소 제곱법 (WLS)

3.1 정의

가중 최소 제곱법은 양의 정부호(positive definite) 가중 행렬 $\mathbf{W} \in \mathbb{R}^{m \times m}$ 를 도입하여 다음 최적화 문제를 푼다.

$\min_{\mathbf{x}} (\mathbf{A}\mathbf{x} - \mathbf{b})^\top \mathbf{W} (\mathbf{A}\mathbf{x} - \mathbf{b})$

이를 가중 잔차 노름의 최소화 문제로 표현하면

$\min_{\mathbf{x}} \| \mathbf{A}\mathbf{x} - \mathbf{b} \|_{\mathbf{W}}^2$

이다. 여기서 $\| \mathbf{v} \|_{\mathbf{W}}^2 = \mathbf{v}^\top \mathbf{W} \mathbf{v}$ 는 가중 노름의 제곱이다.

3.2 정규 방정식의 유도

목적 함수를 $\mathbf{x}$ 에 대해 미분하여 0으로 놓으면

$\frac{\partial}{\partial \mathbf{x}} \left[ (\mathbf{A}\mathbf{x} - \mathbf{b})^\top \mathbf{W} (\mathbf{A}\mathbf{x} - \mathbf{b}) \right] = 2 \mathbf{A}^\top \mathbf{W} (\mathbf{A}\mathbf{x} - \mathbf{b}) = \mathbf{0}$

정리하면 가중 정규 방정식이 된다.

$\mathbf{A}^\top \mathbf{W} \mathbf{A} \mathbf{x} = \mathbf{A}^\top \mathbf{W} \mathbf{b}$

$\mathbf{A}^\top \mathbf{W} \mathbf{A}$ 가 가역이면 해는 다음과 같다.

$\hat{\mathbf{x}}_{\text{WLS}} = (\mathbf{A}^\top \mathbf{W} \mathbf{A})^{-1} \mathbf{A}^\top \mathbf{W} \mathbf{b}$

3.3 가중 행렬의 구성

가중 행렬 $\mathbf{W}$ 는 대각 행렬로 설정하는 경우가 많다.

$\mathbf{W} = \text{diag}(w_1, w_2, \dots, w_m)$

여기서 $w_i > 0$ 은 $i$ 번째 관측 방정식의 가중치이다. 통계적 관점에서, 각 관측의 분산이 $\sigma_i^2$ 일 때 가중치를 $w_i = 1/\sigma_i^2$ 로 설정하면 가우스-마르코프 정리(Gauss-Markov theorem)에 의해 최적 선형 불편 추정량(BLUE)을 얻는다.

더 일반적으로, 관측 오차의 공분산 행렬이 $\boldsymbol{\Sigma}$ 일 때 가중 행렬은 다음과 같이 설정한다.

$\mathbf{W} = \boldsymbol{\Sigma}^{-1}$

3.4 로봇공학에서의 해석

로봇 역기구학에서 야코비안 $\mathbf{J} \in \mathbb{R}^{m \times n}$ 과 원하는 태스크 공간 속도 $\dot{\mathbf{x}}_d$ 에 대해

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

를 풀 때, 태스크 공간의 각 자유도에 서로 다른 우선순위를 부여하고자 한다면 가중 최소 제곱법을 적용한다.

$\dot{\boldsymbol{\theta}}_{\text{WLS}} = (\mathbf{J}^\top \mathbf{W} \mathbf{J})^{-1} \mathbf{J}^\top \mathbf{W} \dot{\mathbf{x}}_d$

여기서 $\mathbf{W}$ 의 대각 원소를 크게 설정하면 해당 태스크 방향의 추종 정확도가 높아진다. 예를 들어 위치 정확도가 자세 정확도보다 중요한 경우, 위치에 해당하는 가중치를 크게 설정한다.

4. 댐핑 최소 제곱법 (DLS)

4.1 동기

특이 배치(singular configuration) 근방에서 야코비안의 조건수(condition number)가 급격히 증가하면, 의사 역행렬(pseudoinverse)을 통한 해가 매우 큰 관절 속도를 생성하게 된다. 이는 물리적으로 실현 불가능하며 로봇 시스템에 위험하다. 댐핑 최소 제곱법은 이러한 문제를 해결하기 위해 제안되었다.

4.2 정의

댐핑 최소 제곱법은 다음 정규화된 최적화 문제를 푼다.

$\min_{\mathbf{x}} \left( \| \mathbf{A}\mathbf{x} - \mathbf{b} \|_2^2 + \lambda^2 \| \mathbf{x} \|_2^2 \right)$

여기서 $\lambda > 0$ 는 댐핑 파라미터(damping parameter) 또는 정규화 파라미터이다. 첫 번째 항은 잔차를 최소화하고, 두 번째 항은 해의 크기를 제한한다.

4.3 정규 방정식의 유도

목적 함수 $f(\mathbf{x}) = \| \mathbf{A}\mathbf{x} - \mathbf{b} \|_2^2 + \lambda^2 \| \mathbf{x} \|_2^2$ 를 전개하면

$f(\mathbf{x}) = \mathbf{x}^\top \mathbf{A}^\top \mathbf{A} \mathbf{x} - 2\mathbf{b}^\top \mathbf{A} \mathbf{x} + \mathbf{b}^\top \mathbf{b} + \lambda^2 \mathbf{x}^\top \mathbf{x}$

$\mathbf{x}$ 에 대해 미분하면

$\frac{\partial f}{\partial \mathbf{x}} = 2(\mathbf{A}^\top \mathbf{A} + \lambda^2 \mathbf{I})\mathbf{x} - 2\mathbf{A}^\top \mathbf{b} = \mathbf{0}$

따라서 댐핑 정규 방정식은 다음과 같다.

$(\mathbf{A}^\top \mathbf{A} + \lambda^2 \mathbf{I}) \mathbf{x} = \mathbf{A}^\top \mathbf{b}$

해는 다음과 같다.

$\hat{\mathbf{x}}_{\text{DLS}} = (\mathbf{A}^\top \mathbf{A} + \lambda^2 \mathbf{I})^{-1} \mathbf{A}^\top \mathbf{b}$

4.4 등가 형태

위 해는 다음과 동치이다.

$\hat{\mathbf{x}}_{\text{DLS}} = \mathbf{A}^\top (\mathbf{A} \mathbf{A}^\top + \lambda^2 \mathbf{I})^{-1} \mathbf{b}$

이 등가 관계는 행렬 항등식 $(\mathbf{A}^\top \mathbf{A} + \lambda^2 \mathbf{I})^{-1} \mathbf{A}^\top = \mathbf{A}^\top (\mathbf{A}\mathbf{A}^\top + \lambda^2 \mathbf{I})^{-1}$ 으로부터 유도된다. $m < n$ 인 경우 오른쪽 형태가 계산상 더 효율적이다.

4.5 특이값 분해를 통한 해석

$\mathbf{A}$ 의 특이값 분해(SVD)를 $\mathbf{A} = \mathbf{U} \boldsymbol{\Sigma} \mathbf{V}^\top$ 라 하면, 댐핑 최소 제곱 해는 다음과 같이 표현된다.

$\hat{\mathbf{x}}_{\text{DLS}} = \sum_{i=1}^{r} \frac{\sigma_i}{\sigma_i^2 + \lambda^2} (\mathbf{u}_i^\top \mathbf{b}) \mathbf{v}_i$

여기서 $\sigma_i$ 는 $\mathbf{A}$ 의 $i$ 번째 특이값, $\mathbf{u}_i$ 와 $\mathbf{v}_i$ 는 각각 좌측 및 우측 특이벡터이며, $r = \text{rank}(\mathbf{A})$ 이다.

일반 의사 역행렬의 경우 계수가 $1/\sigma_i$ 인 반면, 댐핑 최소 제곱법에서는 $\sigma_i / (\sigma_i^2 + \lambda^2)$ 이다. $\sigma_i \gg \lambda$ 이면 $\sigma_i / (\sigma_i^2 + \lambda^2) \approx 1/\sigma_i$ 로 일반 의사 역행렬과 유사하고, $\sigma_i \ll \lambda$ 이면 $\sigma_i / (\sigma_i^2 + \lambda^2) \approx \sigma_i / \lambda^2 \approx 0$ 으로 해당 방향의 기여가 억제된다.

조건	필터 계수	해석
$\sigma_i \gg \lambda$	$\approx 1/\sigma_i$	일반 의사 역행렬과 동일
$\sigma_i \approx \lambda$	$\sigma_i / (\sigma_i^2 + \lambda^2)$	부분적 감쇠
$\sigma_i \ll \lambda$	$\approx \sigma_i / \lambda^2$	강하게 감쇠

5. 로봇 역기구학에서의 댐핑 최소 제곱법

5.1 댐핑 역기구학

야코비안 $\mathbf{J}$ 에 대해 댐핑 의사 역행렬(damped pseudoinverse)을 다음과 같이 정의한다.

$\mathbf{J}_{\text{DLS}}^{\dagger} = \mathbf{J}^\top (\mathbf{J} \mathbf{J}^\top + \lambda^2 \mathbf{I})^{-1}$

이를 이용한 역기구학 해는

$\dot{\boldsymbol{\theta}} = \mathbf{J}_{\text{DLS}}^{\dagger} \dot{\mathbf{x}}_d = \mathbf{J}^\top (\mathbf{J} \mathbf{J}^\top + \lambda^2 \mathbf{I})^{-1} \dot{\mathbf{x}}_d$

이다.

5.2 댐핑 파라미터의 선택

댐핑 파라미터 $\lambda$ 의 선택은 정확도와 안정성 사이의 트레이드오프를 결정한다.

$\lambda$ 가 작은 경우: 잔차가 작아 추종 정확도가 높지만, 특이점 근방에서 관절 속도가 커질 수 있다.
$\lambda$ 가 큰 경우: 관절 속도가 제한되어 안정적이지만, 태스크 공간 추종 오차가 커진다.

Nakamura와 Hanafusa(1986)가 제안한 적응적 댐핑 기법에서는 야코비안의 최소 특이값 $\sigma_{\min}$ 에 따라 $\lambda$ 를 조절한다.

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

여기서 $\epsilon$ 은 특이점 판별 임계값이고 $\lambda_{\max}$ 는 최대 댐핑 값이다.

6. 가중 댐핑 최소 제곱법

가중 최소 제곱법과 댐핑 최소 제곱법을 결합하면 가중 댐핑 최소 제곱법(Weighted Damped Least Squares)이 된다.

$\min_{\mathbf{x}} \left( (\mathbf{A}\mathbf{x} - \mathbf{b})^\top \mathbf{W}_t (\mathbf{A}\mathbf{x} - \mathbf{b}) + \mathbf{x}^\top \mathbf{W}_j \mathbf{x} \right)$

여기서 $\mathbf{W}_t$ 는 태스크 공간 가중 행렬, $\mathbf{W}_j$ 는 관절 공간 가중 행렬이다. 해는 다음과 같다.

$\hat{\mathbf{x}} = (\mathbf{A}^\top \mathbf{W}_t \mathbf{A} + \mathbf{W}_j)^{-1} \mathbf{A}^\top \mathbf{W}_t \mathbf{b}$

로봇 역기구학에서는 다음과 같이 적용한다.

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

$\mathbf{W}_j$ 를 적절히 설정하면 특정 관절의 움직임을 제한하거나, 관절 한계 근방에서 해당 관절의 가중치를 높여 한계 회피를 유도할 수 있다.

7. 수치적 비교

다음 표는 $2 \times 2$ 야코비안의 특이값이 $\sigma_1 = 1.0$ , $\sigma_2 = 0.01$ 인 경우, 각 기법에 따른 해의 특성을 비교한 것이다.

기법	$\sigma_2$ 에 대한 증폭	$\lVert \dot{\boldsymbol{\theta}} \rVert$	태스크 오차
의사 역행렬	$1/\sigma_2 = 100$	매우 큼	0
DLS ( $\lambda = 0.1$ )	$\sigma_2/(\sigma_2^2 + 0.01) \approx 0.91$	적절	소량
DLS ( $\lambda = 0.5$ )	$\sigma_2/(\sigma_2^2 + 0.25) \approx 0.04$	매우 작음	큼

8. 알고리즘 요약

가중 댐핑 최소 제곱 역기구학 알고리즘:

현재 관절 각도 $\boldsymbol{\theta}$ 에서 야코비안 $\mathbf{J}(\boldsymbol{\theta})$ 를 계산한다.
$\mathbf{J}$ 의 특이값 분해 또는 최소 특이값 $\sigma_{\min}$ 을 구한다.
$\sigma_{\min}$ 에 따라 댐핑 파라미터 $\lambda$ 를 설정한다.
태스크 가중 $\mathbf{W}_t$ 와 관절 가중 $\mathbf{W}_j = \lambda^2 \mathbf{I}$ (또는 관절별 가중)를 설정한다.
관절 속도를 계산한다.

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

관절 각도를 갱신한다: $\boldsymbol{\theta} \leftarrow \boldsymbol{\theta} + \dot{\boldsymbol{\theta}} \Delta t$
수렴 조건을 확인하고, 미달이면 1단계로 돌아간다.

9. 참고 문헌

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.
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.
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.
Golub, G. H., & Van Loan, C. F. (2013). Matrix Computations (4th ed.). Johns Hopkins University Press.
Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. (2009). Robotics: Modelling, Planning and Control. Springer.

v 0.1