[modern robotics] Forward Kinematics

Forward Kinematics

이번 장은 3장에서 학습한 screw axes와 PoE를 이용하여 매니퓰레이터의 FK를 구하는 방법에 대해서 학습해보려고 한다. FK(Forward Kinematics, 순기구학, 정기구학)는 end-effector의 position과 orientation을 각 조인트의 $\theta$를 통해 구하는 것이다. 기존 Craig의 Robotics는 D-H파라미터를 통해 FK를 구하는 게 일반적이었지만, 요즘 논문은 거진 다 modern robotics의 PoE로 설명되어 있다.

$$\theta_1,\theta_2...\theta_n \rightarrow (x,y,\phi) $$

기본적인 trigonometry개념으로도 2차원의 open chain인 3R planar는 아래의 식과 같이 쉽게 풀 수 있다, 그럼에도 불구하고 이렇게 간단해 보이는 2차원 3R planar의 IK는 굉장히 까다로워진다. 하지만 모던로보틱스에서 제시하는 PoE의 개념을 적용하게 되면, 쉽게 FK와 IK를 모두 구할 수 있게 된다.

$$\small x = L_1\cos\theta_1 + L_2\cos(\theta_1+\theta_2) + L_3\cos(\theta_1+\theta_2+\theta_3), $$

$$\small y = L_1\sin\theta_1 + L_2\sin(\theta_1+\theta_2) + L_3\sin(\theta_1+\theta_2+\theta_3), $$

$$\small \phi = \theta_1+\theta_2+\theta_3 $$

PoE를 이용하여 Forward Kinematics를 구하는 순서는 다음과 같다.

 

1. 모든 Joint 각도를 0으로 고정한 뒤 body frame의 position과 orientation을 구한다. $\small M \in SE(3)$
2. 하나의 Joint를 제외 한 나머지 Joint의 각도를 0으로 유지(고정)한 뒤, 지정한 Joint를 1 rad/s으로 회전 시켰을 때 origin에서 발생하는 선속도를 구한다. $\omega$
3. Screw Axis의 v 를 구하는데 두가지 방법이 있다.
   • 회전하고자 하는 조인트를 제외한 나머지 조인트의 각도를 0으로 고정한 뒤, $\omega$를 1 rad/s으로 회전시켰을 때, origin이 회전하는 선속도를 직관적으로 구할 수 있다.
   • 두 번째로, algebraically 하게 origin부터 회전하고자 하는 joint까지의 거리 벡터 $q$를 구한 뒤, $v=\omega \times(-q)$를 통해 구 할 수도 있다.
4. 구해진 Screw axis $S$는 $se(3)$의 matrix로 표현이 가능하며, 여기서 $[w]$는 skew-symmetric matrix이다.
   $$\ [S]=\left[\begin{matrix}[w] &v \\0_{3x1} &0 \end{matrix}\right ] \in se(3) $$
5. 구해진 matrix를 활용하여 screw motion의 matrix exponential로 표현하면 아래의 식으로 표현이 가능하다.
   $$ e^{[S]\theta} = \left [ \begin{matrix} e^{[\omega]\theta} &G(\theta)v \\ 0_{3x1} &1 \end{matrix} \right] $$$$ e^{[\omega]\theta}= I+\sin\theta [w] + (1-\cos\theta)[\omega]^2 $$$$\small G(\theta)= \theta I+(1-\cos\theta)[\omega]+(\theta-\sin\theta)[\omega]^2 $$
6. 결론적으로 각 조인트의 Screw axis $(\omega, v)$를 통해 아래와 같은 식을 만들어 Transform matrix를 구할 수 있다.
   $$\ T_{04} = e^{\left[S_1\right]\theta_1}e^{\left[S_2\right]\theta_2}e^{\left[S_3\right]\theta_3}M $$

---

Product of Exponentials Formula

PoE는 두 가지 방법으로 FK를 구현할 수 있다. (Screw Axes in the Base frame, Screw Axes in End effector frame).

그전에 먼저, PoE 공식을 이용하기 위해서 다음과 같은 작업을 해줘야 한다.

 

• Assign a stationary frame {s} at the fixed frame and body frame {b} at the end-effector.
• Describe $\small M$, when the robot is at its zero position.
• Use frame {i} for links $i$ at joints $i$

First Formulation : Screw Axes in the Base Frame

FK in Space(fixed) Frame에 필요한 elements와 공식

 

• End-effector configuration $\small M \in SE(3)$ when the robot at its home(zero) position. (Space Frame에서 구한 M)
• Screw axes $\small S_1, S_2, \cdot\cdot\cdot, S_n $ expressed in the fixed frame. (해당 조인트를 돌렸을때 space frame의 origin을 기준으로 나오는 선속도)
• Joint variables : $\small \theta_1, \cdot\cdot\cdot,\theta_n $.
• FK in Space : $\ T = e^{\left[S_1\right]\theta_1} \cdot\cdot\cdot e^{\left[S_n\right]\theta_n}M $

Second Formulation : Screw Axes in the End-Effector Frame

FK in Body Frame에 필요한 elements와 공식

 

• End-effector configuration $\small M \in SE(3)$ when the robot at its home(zero) position. (Space Frame에서 구한 M)
• Screw axes $\small B_1, B_2, \cdot\cdot\cdot, B_n $ expressed in the body frame. (해당 조인트를 돌렸을때 body frame의 origin을 기준으로 나오는 선속도)
• Joint variables : $\small \theta_1, \cdot\cdot\cdot,\theta_n $.
• FK in Space : $\ T = Me^{\left[B_1\right]\theta_1} \cdot\cdot\cdot e^{\left[B_n\right]\theta_n} $

---

The Universal Robot Description Format (URDF)

URDF는 ROS에서 사용되는 포맷으로 XML문법을 따르게 되어 있다. 로봇의 Kinematics, inertial properties, link geometry가 표현되어 있고 기본적인 구성요소(노드)로 Joint와 Link가 있다. 위 그림을 보면 금방 파악 가능하다.

 

• Joint : 두 링크를 연결하는 부분으로 로봇의 Kinematics를 표현하며, parent link와 child link를 넣어줘야 한다.
• Link : 링크는 로봇의 mass, origin, inertia 등 mass properties를 넣어서 구성해야 한다.

---

마지막으로 간단한 코멘트를 넣어보자면 :

1. Craig책에서 설명한 방법이 익숙하고 편 할 수 있지만, POE를 통해 FK/IK를 할 때 Screw Axis만 구하면 되기 때문에, 훨씬 쉽고 접근성이 좋으니 꼭 숙지해놓으면 좋을 것 같다.

2. Craig의 Robotics책도 마찬가지지만, FK/IK는 직접 손으로 많이 풀어볼수록 이해에 도움이 되고 Kinematics에 친밀해지는 거 같다. 코드로 구현할 때에도 손으로 많이 풀어본 경험이 도움이 많이 되었던 기억이 있다.