2. Numerical Solution of Inverse Kinematics

◆ Introduction

n개의 joint가 있다고 할 때, θ_1~θ_n까지의 joint angle이 존재한다.

θ_j는 j번째 joint의 회전 각도. (회전축을 알고있다고 가정)

 

IK를 풀기 위해, end effector가 최대한 target position에 가깝도록 하는 joint angle들이 결정되어야 함.

 

< 몇 가지 개념 정리 >

• s_1~s_k : root에 대한 end effector들의 상대 좌표라 하자.
• s 벡터 : end effector position들을 {s_1, s_2, ... , s_k}^T 열벡터 형태로 표현.
• t 벡터 : target position들을 {t_1, t_2, ... , t_k)^T 열벡터 형태로 표현함. t_i는 i번째 end effector의 target position인 것.
• θ 벡터 : joint angle들을 {θ_1,...,θ_n}^T 열벡터 형태로 표현함
• e_i : i번째 end effector의 변화량, 즉 얼마나 변해야 하는지(desired change in position)를 나타내는 e_i = t_i - s_i이다.

 

- Forward Kinematics(FK) : 모든 joint들의 joint angle θ가 주어졌을 때, 어떤 함수(s = f(θ))를 돌렸더니 end effector들의 position이 결정되는 것이 FK

 

- Inverse Kinematics (IK) : desired end effector position(end effector가 있어야할 위치)이 주어졌을 때, 어떤 함수(θ = f^(-1)(s_d))를 돌렸더니 모든 joint들의 joint angle θ가 결정되는 것이 IK

 

IK 는 엄청난 비선형 함수다. 해가 없을 수도 있고, 여러개일 수도 있다. 최적 해가 하나가 아닐수도 있다.

 

IK 문제를 이해하기 위해서, 우선 kinetic angular variables의 개념을 이해해야 함!

 

---

◆ Kinetic Angular Variables

원을 그리면서 도는 파티클이 있다 하자

이 파티클의 position은 카르테시안 좌표 (x,y) (직교 벡터 두개로 표현되는 모든 것)로 표현할 수 있따.

그러나, x, y position만으로는 얘가 이 원 위에서 어떻게 운동하고 있는지는 알 수 없음.

 

이런 원형 움직임을 쉽게 표현하기 위해,  θ를 도입해 표현하는 방법이 있다.

반지름 R과 회전각 θ를 사용해 단위시간동안 점이 얼마나 회전했는지를 표현해줄 수 있음!

 

기준 축이 x일때, 반지름 R과 회전각 θ를 사용하여 x = Rcosθ, y = Rsinθ로 표현할 수 있음 ㅇㅇ(CCW가 +, CW가 -)

 

 

Δθ = θ(t+Δt) − θ(t)

Δθ는 이 타임 스탭이 지나감에 따라 θ가 얼마나 움직였는지를 의미함.

이를 통해, 아주 짧은 시간동안 θ의 변화량인 각속도ω를 정의할 수 있다.

각속도(angular velocity) ω = dθ/dt

 

일정한(constant한) 각속도 ω로 움직이는 모션의 경우,

θ(t) = θ_i​+ω(t − t_i​) 또는 Δθ = ωΔt로 표현할 수 있음. (θ_i는 초기 각도, t−t_i​는 시간 변화)

 

즉, θ(t) = Δt동안 얼마나 움직였는지 위치(θ_i​에서 출발하여 각속도 ω로 t−t_i​만큼 이동한 것)를 나타냄.

Δθ = 각속도 ω로 Δt동안 이동한 거리.

 

 

또, 각속도 ω가 시간에 따라 바뀌는 경우,

우리는 각 가속도를 정의할 수 있당

각 가속도(angular acceleration) α = dω/dt

일정한 각 가속도 α로 움직이는 모션의 경우, 다음 두 공식을 적용할 수 있음

 

또, 우리는 부채꼴에서 중심각 θ에 대응하는 호의 길이 L = rθ (θ는 라디안)인 것을 알고있다.

Δθ에 대응하는 호의 길이 s = R|Δθ| 라 할 수 있음. Δθ가 매우 작은 경우, s는 |Δr|에 근접함.

 

s = R|Δθ|의 양 변을 Δt로 나누면, 속도 v = R|Δθ/Δt| = R|ω|가 나옴.

즉 Δθ가 같더라도 R이 커지면 이동 거리도 커지고 속도도 빨라짐을 알 수 있음. (당연함ㅇㅇ)

 

 

2차원에서의 각속도 예시를 보자.

극좌표 위에 점 p(r, Φ)가 있따.

점 p에 V라는 힘이 가해졌을 때, V는 V⊥와 V∥로 나뉠 것임. 이때 V∥는 r벡터와 평행하므로 속도에 영향 미치지 않음.

따라서 점 p의 회전에 영향을 준 힘은 V⊥만큼임! 

이때 각속도 ω는 다음과 같다.

 

그럼, 이걸 3차원으로 옮기면! ???

3차원에서 회전 각은 axis-angle 형식으로 표현할 수 있다.

u는 어떤 벡터를 기준으로 회전할 것인지(회전축. 단위벡터), θ는 u를 기준으로 얼마만큼 이동했는지를 의미함.

 

3차원상에서 각속도(orbital angular velocity)는 2차원 각속도에다 u벡터를 곱한 형식으로 표현됨.

이때 θ는 r과 v 사이의 각이므로, 다음과 같이 cross product 형태로 쓸 수 있음.

벡터 a, b가 있을 때, a×b의 크기는 두 벡터로 이루어진 평행사변형의 넓이 (quantity)임.

근데 여기에 벡터 n을 곱해주면 외적 벡터가 됨. 이 아이디어를 적용한 것이 위의 식. (r을 아래 위로 곱해준 이유)

이를 응용하면, ω와 r의 외적 ω×r은 다음과 같이 v⊥가 됨!

기억하자!

• ω = r×v/r²
• ω×r = v⊥

※ 각운동량과 관성 모멘트

각운동량은 물체의 회전 관성과 특정 축 주위의 회전 속도의 곱을 나타내는 벡터량이다. 그러나 만약 물체가 어떤 한 평면 위에서 회전한다면, 우리는 그것을 스칼라인것처럼 다룰 수 있다.(pseudoscalar)

선운동량 p = mv였던 것처럼, 각운동량 L은 Iω (I는 관성 모멘트, ω는 각속도)로 표현할 수 있따.

(관성 모멘트는 물체의 운동 변화에 대한 저항을 특징짓는 값입니다. 이는 질량이 회전 축 주위에 어떻게 분포되는지에 따라 달라지며, 선택한 축에 따라 다를 수 있습니다.)

 

내가 어떤 힘 v를 가했을 때, 내부가 얼마나 채워져있느냐에 따라 저항하는 힘(관성 모멘트)이 달라지므로 ω도 달라진다! 라는 느낌인듯.

 

결론은, 각운동량이 관성 모멘트에 영향을 받는다~ 라는 것

 

---

◆ 기초 개념 - Gradient

각 변수들에 대한 편미분으로 이루어진 열벡터

IK를 풀기위한 기초 개념이 아직 남아있다. 그중 Gradient와 Hessian에 대해 알아볼 것!

 

스칼라 값을 가진 다변수 함수 f(x,y,...)의 gradient는 ∇f로 나타낼 수 있다.

하나의 열벡터 ∇f에 모든 partial derivate(편미분) 정보를 저장한다.

f(x,y) = 2x² - 4xy일때 gradient ∇f는 다음과 같다.

함수의 어떤 한 점 (x₁,y₁,...)에 서 있을 때, gradient ∇f(x₁,y₁,...)벡터는 그 점에서 함수를 가장 빠르게 max로 만드는 방향을 알려준다. 

언덕에 가장 빨리 올라가는 방법은 젤 가파른 곳으로 올라가는 것!

gradient 방향으로 가면 가장 빨리 max 지점에 도달할 수 있음

 

---

◆ 기초 개념 - Hessian

헤시안은 각 변수들에 대해 2차 편미분(second-order partial derivative)을 한 matrix

헤시안은 Symmetric matrix 성질을 띔

헤시안은, 함수의 Gradient ∇f를 구한 다음에 그 녀석의 자코비안을 구한 것과 똑같음. (gradient의 jacobian)

즉, H(f(x)) = J(∇f(x))

 

f(x,y) = x³ + 2x²y + 3xy + y³ 이라 하자.

x, y로 한번 편미분하면,

이때 gradient는 다음과 같음.

x로 편미분, y로 편미분, 반대 편미분하면

이 값들로 다음 헤시안 matrix를 구성할 수 있다.

이때 gradient의 jacobian 계산해봐도 같음~

 

참고로 변수가 3개인 경우

 

---

◆ Jacobian Inverse Methods

지금까지 IK를 위한 기초 개념들(kinetic angular variables, gradient, hessian)을 공부했다. 다시 IK로 넘어와보자!

 

IK 풀이법 중, Jacobian solution들은 IK 문제를 선형근사해서 푼다.

bone 이동 및 joint angle 시스템 변화에 따른 end effector의 움직임을 상대적으로 모델링함.

오른쪽 그림처럼 선형근사(Linear approximation)한다~

J(θ)는 편미분 θ만큼 움직일 때 end effector가 움직인 거리를 선형근사한 것! 을 의미함. ?

이거같은데..

s는 end effector의 position(위치)이고, θ는 각도. 위치변화량(거리)을 각도로 미분하면 각속도.

p는 join들의 현재 position이고, v는 회전축의 unit vector.

 

 

IK 문제를 풀기 위해서는 Jacobian 역행렬이 필요함. 왜 필요한지는 FK 문제에서 비롯됨

θ를 넣었을 때 end effector position들의 열벡터인 s벡터를 내놓는 f(θ)가 있다고 하자. (s = f(θ))

양 변을 t에 대해 미분하고, 분모 분자에 dθ를 곱하면 J(θ)형태를 만들수 있다.

 

충분히 짧은 시간 t에 대한 end effector의 움직임은 Jacobian이 dθ 움직인 것과 똑같다는 것을 의미.

따라서, Δs ≒ JΔθ임을 알 수 있음.

 

end effector들 위치 변화 Δs는 점점 end effector의 목표 위치인 e = t - s에 최대한 근접하도록 움직여야 함

그러므로 Δs에 e를 대입하면, FK 문제를 푸는 것은 e = JΔθ 를 푸는 것이라 할 수 있음.

 

따라서, IK 문제를 푸는 것은 다음과 같이 표현할 수 있음.

 

---

◆ Jacobian Pseudo Inverse Methods

그런데... 역행렬을 푸는 것은 어렵다.

 

J는 정사각 행렬이나 가역 행렬이 아닐 수도 있음.

가역 행렬 이더라도, singularity problem(==unreachable. 못 닿는 상황)이 있을 수도 있음.

이런 문제를 해결하기 위해서 Jacobian Pseudo-inverse J⁺가 등장함!

 

J⁺를 푸는 법은, 관측값들이 있을 때 에러를 최소화하는 피팅 함수를 찾는 것이다.

노이즈가 어느정도는 있다고 가정하고 그 노이즈를 최소화하는 답안을 찾아내는 것. (least square 방식)

=> 정확한 값은 아닐지라도, 해가 무조건 나옴. 

예시)

나는 이 함수가 무조건 y = mx + b의 꼴이라는 것을 알고 있다고 하자.

그럼 여러개의 mx + b 선을 그어 보면서, 어떤 게 가장 관측값들과의 에러가 적은지(best fit)를 찾아내는 것.

이런 방식으로 e = JΔθ 를 풀어냄.

 

pseudo inverse는, (I−J⁺J)가 J의 null space로 projection하도록 하는 J⁺를 의미함. (J⁺J=I를 만족하는 J⁺)

null space란, matrix와 곱했을때 0을 만들어주는 것을 의미함.

A의 null space가 B일 때, AB = 0

 

 

J⁺를 사용하면, 항상 해가 나옴.

• e가 J 범위 안에 있는 경우에는, e = JΔθ를 만족하는 유니크 솔루션 Δθ가 나옴.
• e가 J 범위 안에 없는 경우에는, JΔθ - e의 값을 최소화하는 해 Δθ를 가짐.

 

 

IK 문제를 풀기 위해 J⁺를 사용한 접근법으로는 이런 애들이 있다~ 다음 차시부터 차근차근 배울 것임

• Analytical Jacobian Method (usually used for the simple robot problem)
• Moore-Penrose Method
• Jacobian Transpose Method
• Damped Least Square Method (DLS)
• Singular Value Decomposition (SVD) Method