[GE] 4. Newton Method For IK

Jaeyoung Seon·2022년 6월 11일
0

[2022-1H] 게임공학

목록 보기
4/8
post-thumbnail

1. Introduction

1-1. Newton Methods [1]

Newton Method는 object function f(x)f(x)2차 테일러 급수를 기반으로 함.

f(x+σ)f(x)+[f(x)]Tσ+12σTHf(x)σf(x+\sigma)\approx f(x)+[\nabla f(x)]^T\sigma+\frac{1}{2}\sigma^TH_f(x)\sigma (\approx : 근사. approximation)

Hf(x)H_f(x)ffHessian Matrix임. (multi-variative 함수의 2차 미분)

하지만 Hessian matrix를 그대로 계산하는 것은 computational cost가 크고 복잡하기 때문에, 직접 계산하지 않고 꼼수를 써서 Hessian matrix의 근사값을 구하는 방법이 존재함. → gradient value를 기반으로

잘 알려진 method로는 Broyden’s method, Powell’s method, Broyden-Fletcher-Goldfarb-Shanno (BFGS) method 가 있음.

<Newton Method의 장단점>

  • 장점
    • erratic discontinuity 없이 부드러운 모션을 만들어낼 수 있음.
    • singularity problem에 구애받지 않음. (object function을 향해 수렴하는 형태이기 때문. Jacobian은 분모가 0이 되어 무한대로 발산하는 문제가 발생했었음)
  • 단점
    • 구현하기 힘듦.
    • 각 iteration마다 엄청난 양의 computational cost를 요구함. (오버헤드가 큼)

2. Basics

2-1. Taylor Series [2]

함수가 “무한대로 미분가능 (infinitely differentiable)”하다고 가정하면, 테일러 급수 (Taylor Series)라는 power series를 만들 수 있음.


Power Series? [3]
다음과 같은 형태를 갖는 무한급수.

n=0an(xc)n=a0+a1(xc)+a2(xc)2+\sum_{n=0}^\infin a_n(x-c)^n=a_0+a_1(x-c)+a_2(x-c)^2+\cdots

(이때 ana_n은 n번째 식의 계수, cc는 상수값)


power series의 특수한 형태가 테일러 급수라고 볼 수 있음.

함수 f(x)f(x)를 근사하기 위해 특정 점 f(a)f(a)에서 근사를 진행함. (점에 가까워질수록 원본 함수와 비슷한 형태를 띠고, 멀어질수록 원본의 형태와 달라짐)

f(x)=f(a)+f(a)1!(xa)+f(a)2!(xa)2+f(a)3!(xa)3+=n=0fn(a)n!(xa)nf(x)=f(a)+\frac{f'(a)}{1!}(x-a)+\frac{f''(a)}{2!}(x-a)^2+\frac{f'''(a)}{3!}(x-a)^3+\cdots=\sum_{n=0}^\infin\frac{f^n(a)}{n!}(x-a)^n테일러 급수

a=0a=0인 경우, 이 급수를 Maclaurin series (맥클로린 급수)라고 하며, 다음과 같음.

f(x)=f(0)+f(0)x+f(0)2!x2+f(0)3!x3++f(n)(0)n!xn+f(x)=f(0)+f'(0)x+\frac{f''(0)}{2!}x^2+\frac{f'''(0)}{3!}x^3+\cdots+\frac{f^{(n)}(0)}{n!}x^n+\cdots

2-2. Gradient [4]

multivariable function f(x,y,)f(x,y,\dots)에서 Gradient란 ‘각 파라미터에 대한 편미분으로 이루어진 벡터’이며, f\nabla f로 표기함.

f=[fxfy]\nabla f=\begin{bmatrix}\frac{\partial f}{\partial x} \\ \frac{\partial f}{\partial y} \\ \vdots\end{bmatrix}

ex) f(x,y)=2x24xyf(x,y)=2x^2-4xyf=[4x4y4x]\nabla f=\begin{bmatrix}4x-4y \\ -4x\end{bmatrix}

<gradient의 기하학적 의미>

특정 점 (x0,y0,)(x_0,y_0,\dots)에 서있을 때, f(x0,y0,)\nabla f(x_0,y_0,\dots)“어느 방향으로 가야 함수가 가장 빠르게 증가하는지”에 대한 것임.

즉, 각 파라미터에 해당하는 방향으로 가장 빠르게 이동할 수 있는 벡터가 gradient인 것.

f\nabla f는 위 그림처럼 vector field로 나타낼 수 있음. → gradient : 파란색 화살표

그림에서 보면 gradient는 “함수가 가장 크게 변하는 방향의 벡터”를 나타내고 있고, 하얀색에서 검은색으로 갈수록 값이 증가한다는 것을 의미함. [5]

2-3. Interpreting the gradient [4]

gradient의 기하학적인 해석.

input ff가 2차원이라고 가정하고, (x0,y0)(x_0,y_0) 점에서의 gradient를 다음과 같이 표현할 수 있음.

f(x0,y0)=[fx(x0,y0)fy(x0,y0)]\nabla f(x_0,y_0)=\begin{bmatrix}\frac{\partial f}{\partial x}(x_0,y_0) \\ \frac{\partial f}{\partial y}(x_0,y_0)\end{bmatrix}

이렇게 되면 이 벡터는 점 (x0,y0)(x_0,y_0)에서의 동작을 나타내게 됨.

위 그림에서 (x0,y0)(x_0,y_0) 점 위에 서있다고 가정.

이때 곡면 위를 “걷는다”는 것은 언덕의 slope를 따라서 걷는 것이라고 할 수 있고, y-direction으로 걷고 있는 경우 fy\frac{\partial f}{\partial y}의 slope로 걷고 있다는 것을 의미.

gradient vector의 방향으로 걷는다는 것은 “가장 빠르게 언덕을 올라갈 수 있는 방향으로 걷고 있다.”는 뜻.

2-4. Hessian [6]

scalar-valued function의 2차 편미분을 element로 한 Square Matrix

(Hf)i,j=2fxi xj(\text{H}_f)_{i,j}=\frac{\partial^2 f}{\partial x_i \ \partial x_j}

Hf(x,y,z)=[2fx x2fx y2fx z2fy x2fy y2fy z2fz x2fz y2fz z]\text{H}_{f(x,y,z)}=\begin{bmatrix}\frac{\partial^2 f}{\partial x \ \partial x} & \frac{\partial^2 f}{\partial x \ \partial y} & \frac{\partial^2 f}{\partial x \ \partial z} \\ \frac{\partial^2 f}{\partial y \ \partial x} & \frac{\partial^2 f}{\partial y \ \partial y} & \frac{\partial^2 f}{\partial y \ \partial z} \\ \frac{\partial^2 f}{\partial z \ \partial x} & \frac{\partial^2 f}{\partial z \ \partial y} & \frac{\partial^2 f}{\partial z \ \partial z}\end{bmatrix}

벡터 x\text{x}를 input으로 받아서 스칼라 f(x)Rf(\text{x})\in \R을 반환하는 함수 f:RnRf:\R^n\rightarrow\R이 있다고 가정. (f(x,y,z)f(x,y,z))

  • ff의 모든 2차미분이 미분가능하고 연속일 때, Hessian matrix H\text{H}n×nn\times n square matrix임. ((x,y,z)(x,y,z) → 3x3 matrix)
  • 편미분은 순서 상관 없이 xy=yx\partial x\partial y=\partial y\partial x이므로 Hessian matrix는 대칭 행렬임. → SVD로 쉽게 바꿀 수 있음.
  • Hessian matrix의 행렬식을 Hessian determinant라고 함. → 뭔가를 증명할 때 많이 쓰임.
  • 함수 ff의 Hessian matrix는 ff의 “gradient의 Jacobian matrix”임.
    • 즉, H(f(x))=J(f(x))\text{H}(f(x))=J(\nabla f(x))

Practice

Q1. 점 (3, 3, 2)(3,\ -3, \ 2)에서 x2y+4zex+y=f(x,y,z)x^2y+4ze^{x+y}=f(x,y,z)의 gradient vector를 구하라.

A. f=[fxfyfz]=[2xy+4zex+yx2+4zex+y4ex+y]\nabla f= \begin{bmatrix} \frac{\partial f}{\partial x} \\ \frac{\partial f}{\partial y} \\ \frac{\partial f}{\partial z} \end{bmatrix}= \begin{bmatrix} 2xy+4ze^{x+y} \\ x^2+4ze^{x+y} \\ 4e^{x+y} \end{bmatrix}

(3, 3, 2)(3,\ -3,\ 2)(12, 5, 4)(-12, \ 5, \ 4)

Q2. F(x,y)=ln(x)ln(y)F(x,y)=\ln(x)\ln(y)일 때, FF의 Hessian은?

A. HF=[2Fxx2Fxy2Fyx2Fyy]=[ln(y)x21xy1xyln(x)y2]\text{H}_F= \begin{bmatrix} \frac{\partial^2 F}{\partial x \partial x} & \frac{\partial^2 F}{\partial x \partial y} \\ \frac{\partial^2 F}{\partial y \partial x} & \frac{\partial^2 F}{\partial y \partial y} \end{bmatrix}= \begin{bmatrix} -\frac{\ln(y)}{x^2} & \frac{1}{xy} \\ \frac{1}{xy} & -\frac{\ln(x)}{y^2} \end{bmatrix}


2-5. Jacobian and Hessian

H(f(x))=J(f(x))\text{H}(f(x))=J(\nabla f(x))

f(x,y)=x3+2x2y+3xy+4y3f(x,y)=x^3+2x^2y+3xy+4y^3에서의 H(f(x))\text{H}(f(x))를 구해보자.

  • fx=3x2+4xy+3y\frac{\partial f}{\partial x}=3x^2+4xy+3y, fy=12y2+2x2+3x\frac{\partial f}{\partial y}=12y^2+2x^2+3x (1차 편미분)
  • 2f2x=6x+4y\frac{\partial^2 f}{\partial^2 x}=6x+4y, 2f2y=24y\frac{\partial^2 f}{\partial^2 y}=24y, 2fxy=2fyx=4x+3\frac{\partial^2 f}{\partial x\partial y}=\frac{\partial^2 f}{\partial y\partial x}=4x+3 (2차 편미분)
  • [2f2x2fxy2fyx2f2y]=[6x+4y4x+34x+324y]\therefore \begin{bmatrix} \frac{\partial^2 f}{\partial^2 x} & \frac{\partial^2 f}{\partial x\partial y} \\ \frac{\partial^2 f}{\partial y\partial x} & \frac{\partial^2 f}{\partial^2 y} \end{bmatrix}= \begin{bmatrix} 6x+4y & 4x+3 \\ 4x + 3 & 24y \end{bmatrix}

같은 함수에 대해 J(f(x))J(\nabla f(x))를 구해보자.

  • fx=3x2+4xy+3y\frac{\partial f}{\partial x}=3x^2+4xy+3y, fy=12y2+2x2+3x\frac{\partial f}{\partial y}=12y^2+2x^2+3x (1차 편미분)

f(x,y)=[fx(x,y)fy(x,y)]=[3x2+4xy+3y12y2+2x2+3x]\nabla f(x,y)= \begin{bmatrix} \frac{\partial f}{\partial x}(x,y) \\ \frac{\partial f}{\partial y}(x,y) \end{bmatrix}= \begin{bmatrix} 3x^2+4xy+3y \\ 12y^2+2x^2+3x \end{bmatrix}

  • J([f1f2f3])=[f1xf1yf1zf2xf2yf2zf3xf3yf3z]J(\begin{bmatrix}f_1\\f_2\\f_3\end{bmatrix})= \begin{bmatrix} \frac{\partial f_1}{\partial x} & \frac{\partial f_1}{\partial y} & \frac{\partial f_1}{\partial z} \\ \frac{\partial f_2}{\partial x} & \frac{\partial f_2}{\partial y} & \frac{\partial f_2}{\partial z} \\ \frac{\partial f_3}{\partial x} & \frac{\partial f_3}{\partial y} & \frac{\partial f_3}{\partial z} \end{bmatrix} (Jacobian의 정의)

  • J([fxfy])=[2f2x2fxy2fyx2f2y]=[6x+4y4x+34x+324y]J(\begin{bmatrix}\frac{\partial f}{\partial x}\\\frac{\partial f}{\partial y}\end{bmatrix})= \begin{bmatrix} \frac{\partial^2 f}{\partial^2 x} & \frac{\partial^2 f}{\partial x \partial y} \\ \frac{\partial^2 f}{\partial y \partial x} & \frac{\partial^2 f}{\partial^2 y} \end{bmatrix}= \begin{bmatrix} 6x+4y & 4x+3 \\ 4x+3 & 24y \end{bmatrix}

따라서 H(f(x))=J(f(x))\text{H}(f(x))=J(\nabla f(x))


3. Newton-Raphson Method

3-1. Newton-Raphson Method (also known as Newton method) and IK

IK의 목표는 sdf(θ)=0s_d-f(\theta)=0을 만족하는 joint angle θ\theta를 구하는 것이었음. (sds_d : end effector의 desired position)

위 식의 solution을 θd\theta_d (desired theta)로 표현할 수 있음. 해를 구하는 과정에서 Newton method를 적용할 수 있고, 이는 곧 root finding method임.

(root finding → 특정 함수값을 알고 있을 때, 출력값이 0이 되는 input를 찾아나가는 과정)

  • 처음에는 초기값 θ0\theta_0를 설정함. (추측값. initial guess)
    • 이에 따라 sdf(θ0)s_d-f(\theta_0)를 계산할 수 있음.

  • θ0\theta_0이 함수와 만나는 지점에서의 slope (기울기)를 미분을 통해 구할 수 있음. fθ(θ0)-\frac{\partial f}{\partial\theta}(\theta_0). 이 기울기가 θ\theta축과 만나는 지점을 θ1\theta_1으로 설정함.
    • slope = f(θ1)f(θ0)θ1θ0\frac{f(\theta_1)-f(\theta_0)}{\theta_1-\theta_0}
    • θ0\theta_0θ1\theta_1으로 만들기 위한 Δθ\Delta\theta가 존재함. Δθ=[fθ(θ0)]1[sdf(θ0)]\Delta\theta=[\frac{\partial f}{\partial\theta}(\theta_0)]^{-1}[s_d-f(\theta_0)] (1 / 편미분 * 원래 함수)

⇒ 이 과정을 반복하다보면 점이 θd\theta_d에 가까워짐.

<Newton method의 단점>

  • 함수의 형태가 복잡한 경우 guessing이 제대로 되지 않음.
  • 초기값이 desired value에서 너무 먼 경우 guessing이 제대로 되지 않음.

Practice

Q. Newton-Raphson method를 사용하여 초기 estimation에서의 one-step improvement를 찾아라.

Q1. f(x)=ex4f(x)=e^x-4, initial estimate x0=1.5x_0=1.5

Q2. g(x)=x32x4g(x)=x^3-2x-4, initial estimate x0=2.5x_0=2.5

A.

Q1. slope = f(x0)-f'(x_0)

Δx=x1x0=f(x0)f(x0)\Delta x=x_1-x_0=-\frac{f(x_0)}{f'(x_0)}

fx=ex\frac{\partial f}{\partial x}=e^x

Δx=ex4ex\Delta x = \frac{e^x-4}{e^x}

(update function : x1=x0Δxx_1 = x_0 - \Delta x)

(update function을 x0+Δxx_0+\Delta x로 정의할 수도 있고, x0Δxx_0 - \Delta x로 정의할 수도 있음)

x1=1.5e1.54e1.5\therefore x_1=1.5-\frac{e^{1.5}-4}{e^{1.5}}

Q2.

gx=3x22\frac{\partial g}{\partial x}=3x^2-2

x1=x0g(x0)g(x0)=2.5(2.5)32×(2.5)43×(2.5)22\therefore x_1=x_0-\frac{g(x_0)}{g'(x_0)}=2.5-\frac{(2.5)^3-2\times(2.5)-4}{3\times(2.5)^2-2}


Reference

[1] http://www.cs.cmu.edu/~15464-s13/lectures/lecture6/iksurvey.pdf

[2] https://machinelearningmastery.com/a-gentle-introduction-to-taylor-series/

[3] https://en.wikipedia.org/wiki/Power_series

[4] https://www.khanacademy.org/math/multivariable-calculus/multivariable-derivatives/partial-derivative-andgradient-articles/a/the-gradient

[5] https://en.wikipedia.org/wiki/Gradient

[6] https://en.wikipedia.org/wiki/Hessian_matrix

profile
재밌는 인생을 살자!

0개의 댓글