Animation Kinematics [2] : 역기구학 (Inverse Kinematics)

https://www.youtube.com/watch?v=p46QWyHQE6M

1. The Moore-Penrose Pseudo Inverse [1]

Moore-Penrose 역행렬(또는 pseudo inverse)은 역행렬이 존재하지 않을 때에도 유사한 역행렬을 구할 수 있도록 하는 개념으로, 주로 데이터 분석과 선형 회귀 분석에서 유용하게 사용된다. 이 역행렬은 특정 행렬에 대해 가능한 pseudo inverse가 유일하다. 예를 들어, 행렬이 실수 또는 복소수로 구성되어 있을 때 각 원소가 달라지지 않는 유일한 형태로 정의된다. 또한, 가역적이라는 것은 행렬 AAA가 역행렬 A−1A^{-1}A−1을 가질 수 있을 정도로 조건을 만족할 때 사용되는 용어이다. 즉, AAA와 A−1A^{-1}A−1을 곱하면 단위 행렬이 된다. 만약 행렬 AAA가 가역적이라면 Moore-Penrose 역행렬 A+A^+A+ (여기서 +++ 기호는 Moore-Penrose pseudo inverse를 의미함)는 일반 역행렬과 동일해지며, A+=A−1A^+ = A^{-1}A+=A−1이 성립한다. 예를 들어, 2x2 단위 행렬의 경우 A=IA = IA=I일 때 A+=A−1=IA^+ = A^{-1} = IA+=A−1=I와 같아지므로 pseudo inverse와 일반 역행렬이 동일하게 작동한다.

Moore-Penrose 역행렬의 정의

Moore-Penrose 역행렬 A+A^+A+는 다음의 네 가지 조건을 만족해야 한다:

1. AA+A=A
2. A+AA+=A+A^+AA^+ = A^+A+AA+=A+
3. (AA+)∗=AA+(AA^+)^* = AA^+(AA+)∗=AA+
4. (A+A)∗=A+A(A^+A)^* = A^+A(A+A)∗=A+A

여기서 ∗*∗는 켤레 전치(conjugate transpose)를 의미한다.

최소 제곱 해(Least Squares Solution)

Moore-Penrose 역행렬은 특히 "최소 제곱" 문제에서 유용하다. 최소 제곱 문제는 주어진 데이터에 가장 잘 맞는 선형 모델을 찾는 방법으로, 과잉 결정(overdetermined) 문제에서 자주 발생한다. 예를 들어, 다음과 같은 선형 방정식이 있다고 가정하자:

Ax=bAx = bAx=b

여기서 AAA는 m×nm \times nm×n 행렬, bbb는 m×1m \times 1m×1 벡터이다. 만약 m>nm > nm>n이라면, 일반적으로 AAA는 가역적이지 않다. 이 경우, 우리는 AxAxAx가 bbb와 가장 가깝도록 xxx를 찾고 싶다. 이를 위해 Moore-Penrose 역행렬을 사용하여 다음과 같이 쓸 수 있다:

x=A+bx = A^+bx=A+b

여기서 xxx는 주어진 데이터 bbb에 대한 최적의 해를 나타내며, 이는 최소 제곱 오차를 최소화하는 값을 제공한다.

SVD를 사용한 Moore-Penrose 역행렬 계산

Moore-Penrose 역행렬을 계산하는 한 가지 방법은 **특이값 분해(Singular Value Decomposition, SVD)**를 사용하는 것이다. SVD는 임의의 행렬 AAA를 다음과 같이 분해할 수 있다:

A=UΣV∗A = U \Sigma V^*A=UΣV∗

• UUU: m×mm \times mm×m 직교 행렬
• Σ\SigmaΣ: m×nm \times nm×n 대각 행렬 (특이값이 대각선에 위치)
• V∗V^*V∗: n×nn \times nn×n 직교 행렬

Moore-Penrose 역행렬 A+A^+A+는 SVD를 통해 다음과 같이 계산할 수 있다:

1. Σ\SigmaΣ의 대각 원소 중 0이 아닌 원소에 대한 역수를 취한 행렬 Σ+\Sigma^+Σ+를 생성한다. 이는 Σ\SigmaΣ의 대각선 원소를 역수로 대체하고 나머지는 0으로 유지하는 행렬이다.
2. 최종적으로 A+A^+A+는 다음과 같이 구할 수 있다:

A+=VΣ+U∗A^+ = V \Sigma^+ U^*A+=VΣ+U∗

이 방법을 통해 행렬 AAA의 Moore-Penrose 역행렬을 효율적으로 구할 수 있다.

결론

Moore-Penrose 역행렬은 역행렬이 존재하지 않는 경우에도 유용한 대안으로, 데이터 분석과 최소 제곱 문제에서 필수적으로 사용된다. SVD를 활용하면 이러한 역행렬을 쉽게 계산할 수 있으며, 이는 선형 회귀 모델을 구축하는 데 있어 매우 중요한 도구임을 알 수 있다.

1-1. Pseudo-Inverse and Least Squares

least square는 “overdeterminded system”을 푸는 방법임. → 미지수의 개수보다 식의 개수가 더 많은 경우

x,y를 “관측값”이라고 하는데, 관측값이 많고, 이 함수가 linear라는 것을 확신할 때, 관측값 사이의 error를 최소화하는 방식으로 linear function을 찾는 과정이 least square

관측을 계속 하다 보면 error에 따라 기준값보다 커지거나 작아지는 일이 발생하는데, 이러한 error를 최소화하는 linear 식을 찾는 것.

---

작은 오브젝트 (점과 같은)가 평면 위에서 움직이고 있다고 가정함.

이 점이 직선 y=dx+c 위에서 움직이고 있는 것 같다고 생각할 수 있고, 각각 (x1​,y1​),(x2​,y2​),(x3​,y3​)에서 관측함. → 식에 대입

c+dx1​=y1​c+dx2​=y2​c+dx3​=y3​

이때 error가 없으면 이 식을 풀 수 있는 상태가 되고, 식 2개만 있어도 풀 수 있음. but error가 있으면 이 식을 풀 수 없는 상태가 됨.

least square의 아이디어는 sum of the squares of the errors (SSE)를 최소화하는 것.

---

평면 위에서 움직이는 점을 y=Ax+e와 같은 형태로 표현할 수 있음. (error 값 추가) → error는 여러 개 나올 수 있으므로 e와 같은 벡터 형태임. e=⎣⎢⎢⎢⎢⎡​e1​e2​⋮em​​⎦⎥⎥⎥⎥⎤​

이때 eTe를 최소화하는 것이 목표.

ex) [e1​​e2​​e3​​]⎣⎢⎡​e1​e2​e3​​⎦⎥⎤​

(e=y−Ax)

스칼라에서는 xT=x

편미분을 해서 0이 되는 지점이 최솟값이 될 것. → ∂b∂​bTXTXb=2XTXb [2]

이때 y=Ax의 형태였으므로 A를 넘기면 x=A+y가 됨. 따라서

⭐A+=(ATA)−1AT

---

Practice

Q. 다음과 같은 방정식이 있음.

x1​+3x2​=175x1​+7x2​=1911x1​+13x2​=23

이 방정식을 Mx=y​와 같은 행렬 형태로 쓸 수 있는데, 이때 M의 Moore-Penrose Psudoinverse를 구하라. (A+=(ATA)−1AT)

A. ⎣⎢⎡​1511​3713​⎦⎥⎤​[x1​x2​​]=⎣⎢⎡​171923​⎦⎥⎤​, A=⎣⎢⎡​1511​3713​⎦⎥⎤​ ⇒ A+=([13​57​1113​]⎣⎢⎡​1511​3713​⎦⎥⎤​)−1[13​57​1113​]=[−15279​15265​​−15233​15231​​389​−385​​]

---

1-2. Pseudo Inverse and SVD

least square 뿐만 아니라 SVD를 사용해도 pseudoinverse를 구할 수 있음. SVD를 사용해서 역행렬을 구하는 것을 먼저 볼 것임.

A∈Rm×m일 때, 직교 행렬 U∈Rm×m,V∈Rm×m에서 A=U∑VT로 표현할 수 있음. 이때 ∑은 singular value의 대각행렬임.

이때 σi​는 singular value, U와 V의 column을 각각 left singular vector & right singular vector라고 함.

U,V는 직교행렬이기 때문에 inverse == transpose임. (UT=U−1) 이를 통해 A−1=V∑−1UT임을 도출할 수 있음. (원래는 V−T∑−1U−1)

이때 ∑−1은 singular value의 역수에 대한 대각행렬임.

---

but ∑−1이 존재하지 않는 경우도 있음. (비가역)

원래 Ax=y에서 x=A−1y로 역행렬을 곱해서 x의 값이 복원되어야 하는데, 대각행렬의 값이 0인 경우 어떤 값을 곱해도 0이 되기 때문에 “복원 가능”이라는 말이 성립하지 않음. (0⋅x=0과 같은 논리)

따라서 “모든 값을 복원”하지 않고, 복원이 가능한 형태를 가진 값들만 최대한 복원하는 것이 pseudoinverse A+임. (완화된, generalized inverse)

A∈Rm×n이라고 가정. (위에서보다 차원이 늘어남.) U∈Rm×m, V∈Rn×n으로 분해하여 A=U∑VT처럼 쓸 수 있음.

이때 A의 pseudoinverse A+=V∑+UT임. (원래 inverse와 동일하게 사용)

(∑−1과 형태는 유사하지만, invertible한 값만 invert한다는 것이 차이)

---

2. Jacobian Inverse Methods

2-1. Jacobian Pseudo-inverse

결국 우리는 IK에서 목표 지점이 주어졌을 때, 이에 지점에 도달하기 위한 joint angle을 각각 구해야 하고, 그 과정에서 Jacobian 행렬을 사용했었음.

즉, JΔθ=e을 풀어야 하는 것.

처음에 Jacobian을 얘기할 때 least-square 방식으로 푼다고 이야기했었는데, 결국 pseudo-inverse를 구하는 것이 값들의 error를 최소화하는 방향으로 진행하는 것이므로 Jacobian의 pseudo-inverse를 통해 IK를 풀 수 있음.

pseudo-inverse를 구하기 위해 JΔθ=e 이 식을 조작함.

JTJΔθ=JTe → JTJ를 우변으로 넘기면 → Δθ=(JTJ)−1JTe=J+e

J가 “full row rank”일 때에만 pseudo-inverse가 가능하며, JTJ나 JJT는 invertible함.

---

Full row rank?

m×n 행렬의 rank r=m일 때, 이 행렬은 full row rank를 가짐. 즉, 행렬의 각 row vector들이 서로 선형독립이면 full row rank를 가짐.

column rank → 서로 선형독립인 최대 column의 수

row rank → 서로 선형독립인 최대 row의 수

또한, column rank과 row rank 중 큰 값을 갖는 rank가 m, n 중에서 큰 값과 일치한다면 full rank라고 함.

---

3. Damped Least Squares (DLS) [3]

pseudo-inverse method의 문제점을 해결하기 위해 쓰이는 방법.

무슨 문제?

여기서 singular value가 0이 되면 문제가 발생함. (값이 무한대로 발산함)

DLS는 singular value가 0이 되는 것을 막기 위해 약간의 값을 더해줌.

Levenberg-Margquardt method라고도 하며, Wampler [5], Nakamura & Hanfusa [4]의 논문에서 처음 IK에 사용되었음.

---

DLS에서는 ∣∣JΔθ−e∣∣2+λ2∣∣Δθ∣∣2을 최소화함. → λ2∣∣Δθ∣∣2이라는 새로운 값이 추가됨.

λ∈R : non-zero damping constant. 직접 결정하는 경우도 있고, 자동으로 선택해주기도 함.

• damping constant를 잘 골라서 numerically stable하게 만들어야 함.
• damping constant가 너무 작으면 singular value가 0이 되어 발산할 위험이 있으므로 적당히 큰 값을 골라야 함.
• 너무 크면 convergence rate가 너무 느려짐. → 알맞은 해를 찾는데 오래 걸린다는 뜻

∣∣JΔθ−e∣∣2+λ2∣∣Δθ∣∣2를 행렬로 쓰면 다음과 같음.

∣∣(JλI​)Δθ−(e0​)∣∣

또한 normal equation 형태로 나타내면

(JλI​)T(JλI​)Δθ=(JλI​)T(e0​)

---

Normal Equation?

Least Square Cost Function을 분석적으로 접근하기 위한 식. 앞서 least square에서 error를 최소화하려면 eTe를 최소화해야한다고 했고, 여러 식을 거쳐 최종적으로 AAx=ATy 또는 x=(ATA)−1ATy라는 식이 나왔는데, 이 식이 normal equation임.

이때 ATA를 normal matrix라고 함.

---

위 식을 다시 정리하면

(JTJ+λ2I)Δθ=JTe

DLS에서는 JTJ+λ2I가 non-singular이기 때문에 solution을 다음과 같이 쓸 수 있음.

Δθ=(JTJ+λ2I)−1JTe=JT(JJT+λ2I)−1e (두 가지 form 모두 가능함. 보통 뒤에 있는 식을 사용함 → 가로로 긴 행렬보다는 세로로 긴 행렬을 쓰는 것이 효율적이기 때문)

3-1. Pseudo-inverse Damped Least Squares

JJT+λ2I를 구해야 하는데, SVD를 통해 구할 수 있음. (J=U∑VT)

JJT+λ2I=(U∑VT)(V∑TUT)+λ2I

J가 m×n 행렬일 때, U는 m×m, ∑은 m×n, V는 n×n 행렬임.

• U, V는 orthogonal matrix이기 때문에 UT=U−1, VT=V−1
• ∑T는 n×m diagonal matrix이며, 대각선에는 singular value σi​이 있음.
• ∑∑T는 m×m 행렬이며, 대각선에는 singular value의 제곱 σi2​이 있음.

따라서 JJT+λ2I=(U∑VT)(V∑TUT)+λ2I=U(∑∑T+λ2I)UT이며, ∑∑T+λ2I는 대각선 값이 σi2​+λ2인 diagonal matrix임. (λ는 상수)

결국 (JJT+λ2I)−1을 구해야 하는데, 이는 다음과 같이 계산할 수 있음.

E : 대각선 값이 ei,i​=σi2​+λ2σi​​인 n×m diagonal matrix

결론적으로, DLS의 solution은 다음과 같음.

<DLS vs pseudo-inverse>

• σi​가 0으로 가면 값이 무한대로 발산하는 pseudo-inverse와는 달리, DLS는 σi2​+λ2을 통해 값이 0이 되지 않도록 보호하기 때문에 numerically stable함.
• σi​가 충분히 커지면 σi2​+λ2σi​​은 σi​1​와 거의 비슷해짐. 즉, singular value가 충분히 커서 singularity problem에서 멀어지면, least square 방식과 pseudo-inverse는 동일하게 작동함.
• but, σi​가 작아지면서 0에 수렴할수록 DLS와 pseudo-inverse는 확연한 차이를 보임.

---

4. Performance Comparison

4-1. Jacobian Transpose vs. Jacobian DLS vs. Jacobian SVD-DLS [6]

Jacobian Transpose는 Jacobian pseudo-inverse와 비슷하지만 numerically easy하기 때문에 사용하는 알고리즘임.

SVD-DLS로 갈수록 iteration이 줄어드는데, “적은 iteration으로 원하는 목표에 도달할 수 있다”는 것을 의미함.

실행 시간 또한 SVD-DLS가 가장 짧은 것을 볼 수 있음.

Frames per second는 1프레임을 그리는 데 걸리는 시간을 의미하며, 단순 pseudo-inverse의 속도가 가장 빠름을 알 수 있음.

(이 기술들은 다소 old한 기술들임)