hongtlee 님의 블로그

Inverse Kinematics 본문

Study/Robotics

Inverse Kinematics

hongtlee 2025. 3. 6. 16:00

Inverse Kinematics 종류

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

 

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


Process

Newton - Raphson method

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

Tfk(θ)=e[S1]θ1e[S2]θ2e[S6]θ6M

MSE(3) : 0 위치일 때 엔드 이펙터 자세
- 0 위치일 때 스크류축 : S1S6
- 관절 변수 : θ1θ6

스크류 축 S=[ωv]R6 의 4×4 행렬 표현 [S]

[S]=[[ω]v00]se(3)

where,

[ω]=[0ω3ω2ω30ω1ω2ω10]so(3)

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

T=e[S]θ=[e[ω]θG(θ)v01]SE(3)

where,

e[ω]θ=I+sinθ[ω]+(1cosθ)[ω]2

(로드리게스 공식)

G[θ]=Iθ+(1cosθ)[ω]+(θsinθ)[ω]2

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

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

Tfk(θ)=e[S1]θ1e[S2]θ2e[S6]θ6M

---

2. Target matrix와 초기 matrix의 차이에 해당하는 matrix (Tdiff) 구하기

Tdiff=Tfk(θ)1×TtargetM

---

3. Tdiff의 행렬 로그로부터 트위스트 구하기

Tdiff=[Rp01]SE(3)

log(Tdiff)=[S]θse(3)

[S]θ=[[ω]θvθ00]

Using 로드리게스 공식, 회전 행렬 R로 부터  ωθ을 구함

R=e[ω]θ=I+sinθ[ω]+(1cosθ)[ω]2

sinθ0 인 경우

[ω]=RRT2sinθ

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

θ=cos1(tr(R)12)

sinθ=0 인 경우  
  - kπ,k가 짝수이면 R=Iω가 정의되지 않음.
  - kπ,k가 홀수이면 tr(R)=1 이며,

R=I+2ˆωˆωT2I

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

한편,

Tdiff=e[S]θ=[e[ω]θG(θ)v01]=[Rp01]

이므로,

G(θ)v=p

따라서,

v=G(θ)1p

이렇게 Tdiff으로부터 트위스트 V=(ω,v)을 얻었음.

---

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

충분히 작은 에러값 ϵωϵv에 대해



이면 반복.

---

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

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

V=J(θ)θ˙

즉,

θ˙=J1(θ)V

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

θ˙=J(θ)V

단위시간에 대해 θ˙는 dθ가 된다.

따라서,

θi+1=θi+J(θi)V

---

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

(ω<ϵωandv<ϵv)ori>max_iterater

까지 반복한다.

이때, 얻은 θ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을 활용할 수 있어진다.

 

q˙=Jveq˙ -> Jve+(InJJ)q˙0.

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

 

자유도가 남으므로, InJJ에 값이 존재한다. 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

'Study > Robotics' 카테고리의 다른 글

MoveL Test (check possibility)  (0) 2025.03.11
조작성 타원체(Manipulability)  (0) 2025.03.10
Jacobian  (0) 2025.03.06
POE - Forward Kinematics  (1) 2025.03.06
Lie Theory(2)  (1) 2025.03.05