티스토리 뷰
출처 : https://www.youtube.com/watch?v=mStw-w6q8H0&t=32s
미분 방정식의 가장 기본적인 원리는 어떠한 함수의 해를 구하기 위해 어떠한 함수 위의 점 한 군데서 접선을 그었을 때, 극한으로 간다면 0이 되는 지점에서 해당 기울기의 값으로 x라는 해를 표현할 수 있다는 것이다.
f(x) = f(x0) + f'(x0) (x-x0) +h0t0(추가적인 오차)
요건대, 오차를 무시하게 된다면 f(x)~= f(x0) +f'(x0)*(x-x0) =0 이라는 값으로 근사화 시킬 수 있다.
그러므로 x는,
x = - [f'(x0)]^-1 * f(x0) +x0로 표현할 수 있다.
이건 사실 x1을 구한 것이다.
이 과정을 반복한다.
xk+1 = xk - [f'(xk)]^-1 * f(xk)
이 과정을 충분히 반복하면, 이 값은 x*라고 하는 최적해에 근접하게 된다.
이제 f(x)= 0을 전체 상수공간에서 적용된다고 가정하자.
그러므로 f(x) ~= f(x0)+(df /dx(=편미분)) * x0(x-x0)
따라서 df/dx = (n*n)의 매트릭스라고 두고 해당 공식을 풀면,
xk+1 = xk - [(df/dx)*(xk)]^-1 * f(xk)
한마디로 f'(xk) = (df/dx)*xk가 된 것.
어떠한 점에서 다음 점을 표현하는 보간 해는, 해당 점의 미분을 inverse 한 것으로 표현할 수 있다,
Inverse Kinematic에 적용
df/dx를 가장 먼저 찾아야 한다.
호모지니어스 matrix T가 f(세타)로 표현된다고 하자.
6*6은 너무 많으므로, 6개의 공식으로 변환한다.
log(T) = log(f(세타))
6개의 공식과, 6개의 unknown이 나온다.
이 공식은 너무 messy하기 때문에, 풀 수 없다.
그래서 더 좋은 방법을 고려한다.
An iterative procedure based on the Jacobian of the forward kinematics has been developed for general 6-dof spatial open chian
Vb = f^-1 * fdot = [Jb(세타) * 세타dot]
Vb * 델타t = f^-1 * 델타 f = [Jb(세타) * 델타 세타]
델타 t를 곱해주었을 때 전부 변하는 이유는 당연히도 dot은 시간에 관한 미분이기 떄문.
쉽게 말해서 fk+1 - fk만 남는다는 소리
이는 T^-1 * (T-f(세타0) = [Jb(세타) * 델타 세타]로도 기술이 가능하다.
여기서 이제
I(대각행렬) - T^-1* f(세타0) = [[Jb(세타) * 델타 세타]
가 되는데, 여기서 T^-1 * f(세타0)는 우리가 알고 있는 값이다.
T^-1 * f(세타0)를 e^[s] 라는 값으로 치환해보자.
그러면 S는 로그를 취함으로서 알 수 있따.
[S] = log (T^-1 * f(세타0))
그렇게 되면,e^[S] = I + [S] + ...(높은 order의 term은 무시)가 되고,
- [S] = [Jb(세타) *델타 세타
알고리즘 = Given initial 세타, T (-SE(3)
(1) S = log T^-1 * f(세타)
(2) Solve -S = Jb(세타) * 델타 세타 j
for 델타 세타j
(3) 세타 <- 세타 + 델타 세타
(4) Repeat (go to (1)) until convergence
Can also use Js(세타)
이 알고리즘은 꽤나 빠르고, 실시간 해를 찾을 수 있을 정도로 효율적이다.
import numpy as np
def algorithm(theta_init, T):
theta = theta_init
convergence_threshold = 1e-6 # 수렴 기준
max_iterations = 100 # 최대 반복 횟수
for iteration in range(max_iterations):
# (1) S = log(T^-1 * f(theta))
T_inv = np.linalg.inv(T)
f_theta = f(theta) # f(theta) 함수 정의 필요
S = np.matmul(T_inv, f_theta)
S = np.log(S)
# (2) Solve -S = Jb(theta) * delta_theta for delta_theta
Jb_theta = Jb(theta) # Jb(theta) 함수 정의 필요
delta_theta = np.linalg.solve(Jb_theta, -S)
# (3) theta <- theta + delta_theta
theta = theta + delta_theta
# 수렴 조건 확인
if np.linalg.norm(delta_theta) < convergence_threshold:
break
return theta