Inverse Kinematics

Inverse Kinematics 종류

• 기하학적 기법
  • 로봇의 기하학적 구조를 기반으로 삼각함수나 행렬 연산을 통해 해석적 해를 구함.
  • 특정 로봇 구조에서만 가능
  • 수학적 분석으로 정확한 해를 갖고 매우 빠른 방법
  • 구조가 단순한 로봇에 사용됨
• 수치-해석적 기법
  • 반복적 계산으로 수렴하며 초가값을 기반으로 근사값 계산
  • 대부분의 로봇에 적용가능
  • 초기값이 적절하면 빠르게 수렴
  • 비선형 방정식 풀이에 적합하며 높은 정확도를 보임
  예시)
  1. 역자코비안 풀기
  2. 최적화 기반 방법(Optimization-Based Approach)
• 휴리스틱 기법
  • 경험적 규칙과 데이터를 기반으로 복잡한 환경에서 작동
  • 다중 해를 자연스럽게 탐색하며 특이점 문제를 효과적으로 회피.

 

휴리스틱 기법이 자코비안을 사용한 수치해석적 기법에 비해 빠르다.

---

Process

Newton - Raphson method

1. 초기 Joint list에 해당하는 Forward Kinematics 구하기

$T_{fk}(\theta) = e^{[S_1]\theta_1} e^{[S_2]\theta_2}  \cdots e^{[S_6]\theta_6} M$

- $M \in SE(3)$ : 0 위치일 때 엔드 이펙터 자세
- 0 위치일 때 스크류축 : $S_1 \cdots S_6$
- 관절 변수 : $\theta_1 \cdots \theta_6$

스크류 축 $S = \begin{bmatrix}\omega \\ v\end{bmatrix} \in \mathbb{R}^6$ 의 4×4 행렬 표현 $[S]$

\[
[S] = \begin{bmatrix}[\omega] & v \\ 0 & 0\end{bmatrix} \in \mathfrak{se}(3)
\]

where,

\[
[\omega] = \begin{bmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \end{bmatrix} \in \mathfrak{so}(3)
\]

스크류 축을 중심으로 $\theta$ 만큼 이동할 때 강체 이동은 다음과 같다.

\[
T = e^{[S]\theta} = \begin{bmatrix}e^{[\omega]\theta} & G(\theta)v \\ 0 & 1\end{bmatrix} \in SE(3)
\]

where,

\[
e^{[\omega]\theta} = I + \sin\theta[\omega]+(1-\cos\theta)[\omega]^2
\]

(로드리게스 공식)

\[
G[\theta] = I\theta + (1-\cos\theta)[\omega]+(\theta-\sin\theta)[\omega]^2
\]

각 스크류축 및 관절 변수에 대해 지수곱 표현으로 Homogeneous Transformation Matrix을 구한 후 곱해주면, 관절 1 부터 관절 6까지에 해당하는 $T$를 구할 수 있음.

여기에 0위치에 해당하는 엔드이펙터 자세를 곱해주면, 관절 변수($\theta_1 \cdots \theta_6$)일 때 엔드이펙터 자세를 구할 수 있음.

\[
T_{fk}(\theta) = e^{[S_1]\theta_1} e^{[S_2]\theta_2}  \cdots e^{[S_6]\theta_6} M
\]

---

2. Target matrix와 초기 matrix의 차이에 해당하는 matrix ($T_{diff}$) 구하기

\[
T_{diff} = T_{fk}(\theta)^{-1} \times T_{target M}
\]

---

3. $T_{diff}$의 행렬 로그로부터 트위스트 구하기

\[
T_{diff} = \begin{bmatrix}R & p \\ 0 & 1\end{bmatrix} \in SE(3)
\]

\[
\log(T_{diff}) = [S]\theta \in \mathfrak{se}(3)
\]

\[
[S]\theta =\begin{bmatrix}[\omega]\theta & v\theta \\ 0 & 0\end{bmatrix}
\]

Using 로드리게스 공식, 회전 행렬 R로 부터  $\omega$ 와 $\theta$을 구함

\[
R = e^{[\omega]\theta} = I + \sin\theta[\omega] + (1-\cos\theta)[\omega]^2
\]

- $\sin\theta \neq 0$ 인 경우

\[
[\omega] = \frac{R - R^T}{2\sin\theta}
\]

- 회전행렬 $R$의 대각합(trace)을 이용하면,

\[
\theta = \cos^{-1} \left(\frac{\text{tr}(R) - 1}{2}\right)
\]

- $\sin\theta = 0$ 인 경우  
  - $k\pi, k$가 짝수이면 $R = I$, $\omega$가 정의되지 않음.
  - $k\pi, k$가 홀수이면 $\text{tr}(R) = -1$ 이며,

\[
R = I + 2 \hat{\omega} \hat{\omega}^T - 2 I
\]

이 행렬의 성질을 이용하여 $\omega$ 를 계산할 수 있음.

한편,

\[
T_{diff} = e^{[S]\theta} = \begin{bmatrix}e^{[\omega]\theta} & G(\theta)v \\ 0 & 1\end{bmatrix} =  \begin{bmatrix}R & p \\ 0 & 1\end{bmatrix}
\]

이므로,

\[
G(\theta)v = p
\]

따라서,

\[
v = G(\theta)^{-1} p
\]

이렇게 $T_{diff}$으로부터 트위스트 $V = (\omega, v)$을 얻었음.

---

4. 트위스트 값으로부터 반복 조건 확인

충분히 작은 에러값 $\epsilon_\omega$, $\epsilon_v$에 대해

\[
\lVert\omega\rVert > \epsilon_\omega \quad \text{or} \quad \lVert v\rVert > \epsilon_v
\]

이면 반복.

---

5. 자코비안의 유사역행렬을 이용해 다음 Joint list 구하기

주어진 관절 속도에 대해 트위스트는 다음과 같이 주어진다.

\[
V = J(\theta)\dot\theta
\]

즉,

\[
\dot\theta = J^{-1}(\theta)V
\]

자코비안의 역행렬은 항상 존재하지 않을 수 있으므로, 유사역행렬(pseudoinverse) $J^\dagger$을 이용한다. 유사역행렬은 행렬의 특이값 분해를 통해 얻을 수 있다.

\[
\dot\theta = J^\dagger(\theta)V
\]

단위시간에 대해 $\dot\theta$는 $d\theta$가 된다.

따라서,

\[
\theta^{i+1}=\theta^i+J^\dagger(\theta^i)V
\]

---

6. 새로운 Joint list로 과정 1 ~ 5 반복

\[
(\lVert\omega\rVert < \epsilon_\omega \quad \text{and} \quad \lVert v\rVert < \epsilon_v) \quad \text{or} \quad i > \text{max_iterater}
\]

까지 반복한다.

이때, 얻은 $\theta^i$가 최종 result이다.

 

---

Newton-Raphson방식으로 다음 해를 찾아서, 초기 위치가 굉장히 중요하다. 만약 초기위치가 좋지 않다면 해를 구하지 못하는 경우가 발생한다.

 

초기위치에 덜 민감하게 invese kinematics을 구하고자 한다면, Runge-Kutta algorithm을 사용하는 것이 좋다.

 

    // Runge-Kutta 4차 알고리듬
    auto F = [&](const Eigen::Matrix<double, 6, 1>& theta) {
        Tsb_temp = FK_Body(M, Blist, theta);
        Tbs_temp = se3Inverse(Tsb_temp);
        Tbd_temp = Tbs_temp * targetMat;
        twist_b_skew_temp = SE3Tose3(Tbd_temp);
        twist_b_temp = se3ToVec6(twist_b_skew_temp);

        Jb = Jacobian_Body(Blist, theta);
        Jb_pseudoinv = computeJacobianInverse(Jb);
        return Jb_pseudoinv * twist_b;
        };

    ...
    
     k1 = F(eigentheta);
     k2 = F(eigentheta + 0.5 * dt * k1);
     k3 = F(eigentheta + 0.5 * dt * k2);
     k4 = F(eigentheta + dt * k3);

     eigentheta += (dt / 6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
     ...

 

대신 Newton-Raphson방식보다 시간이 오래 걸린다. 또한 휴리스틱 역기구학 기법인 Fabrik 등에 비하면 굉장히 느리다.

 

자코비안을 통해 역기구학 해를 얻은 경우, 하나의 해를 구할 수 있는데,

해의 분기에 따라 역기구학 해는 16개의 해를 가질 수 있다.

 

모든 해를 구하는 알고리즘에 대해서 공부하고 있다.

---

Redundant Manipulators

Joint가 7개인데 6축 로봇이라던가 자유도가 남는 로봇이 있다면, Null space을 활용할 수 있어진다.

 

$\dot{q} = J^\dagger v_e \Rightarrow \dot{q}$ -> $J^\dagger v_e + (I_n - J^\dagger J) \dot{q}_0.$

비용함수로써 초기 위치와 가장 근접한 해를 선택하도록 추가 항($I_n - J^\dagger J$)을 더해준다.

 

자유도가 남으므로, $I_n - J^\dagger J$에 값이 존재한다. UR5e처럼 6 joint에 6DOF라면 자연스래 I-I가 되어 0이 된다.

    // Convenient utilization of redundant DOFs 계산
    auto computeSecondaryObjectiveGradient = [&](const Eigen::Matrix<double, 6, 1>& q) -> Eigen::Matrix<double, 6, 1> {
        // Manipulability gradient
        Eigen::MatrixXd Jb = Jacobian_Body(Blist, q);
        Eigen::MatrixXd JJT = Jb * Jb.transpose();

        // Gradient of manipulability measure
        double manipulability = std::sqrt(JJT.determinant());
        Eigen::Matrix<double, 6, 1> grad;
        grad.setZero();

        for (int i = 0; i < q.size(); ++i) {
            // Numerical gradient computation
            Eigen::Matrix<double, 6, 1> q_perturbed = q;
            double epsilon = 1e-6;
            q_perturbed[i] += epsilon;

            Eigen::MatrixXd Jb_perturbed = Jacobian_Body(Blist, q_perturbed);
            double manipulability_perturbed = std::sqrt((Jb_perturbed * Jb_perturbed.transpose()).determinant());

            grad[i] = (manipulability_perturbed - manipulability) / epsilon;
        }
        return grad;
    }
 	    	// Secondary objective: joint limit 중심 유지
        Eigen::Matrix<double, 6, 1> q_dot_0 = k0 * computeSecondaryObjectiveGradient(eigentheta);

        // Null-space projection
        delta_theta = Jb_pseudoinv * twist_b + (Eigen::Matrix<double, 6, 6>::Identity() - Jb_pseudoinv * Jb) * q_dot_0;

 

이런 식으로 구현해주면 된다.

---

Reference

1. Lynch, K. M., & Park, F. C. (2017). Modern robotics. Cambridge University Press.

2. Sciavicco, L., Villani, L., SICILIANO, B., & ORIOLO, G. (2010). Robotics: modelling, planning and control. Springer.

3. https://ropiens.tistory.com/198