Robotics - Chapter 4. Manipulator Inverse Kinematics
이전 포스팅 [3. Manipulator Kinematics]에서는 관절 변수의 값이 주어진 경우, 좌표계의 변환을 통해 Manipulator의 마지막(End-effector) 좌표계의 position과 orientation, 즉 pose를 계산하는 정기구학(Forward kinematics)에 대해서 알아보았다. 이번 포스팅에서는 정기구학의 반대 개념으로 목표하는 pose 주어졌을 때, Manipulator의 End-effector가 원하는 목표 pose에 일치할 수 있도록 하는 관절 변수의 조합을 구하는 역기구학(Inverse kinematics)에 대해 알아볼 것이다.
[관절 변수]는 Manipulator의 관절 상태를 의미하고, position과 orientation은 직교 좌표 공간상에서 표현되는 값이다. 서로 다른 속성을 가지는 두 공간에서, 정기구학은 관절 공간을 통해 직교 좌표 공간상의 Manipulator의 상태를 해석할 수 있고, 역기구학은 직교 좌표 공간의 정보를 통해 관절 공간상의 Manipulator의 상태를 해석할 수 있다.
링크계에 대한 대표적 이름
[3장]에서는 편의를 위해 각 링크계를 베이스에서부터 순서대로 {0}, {1}, {2} … 등과 같이 숫자 형태로 나타내었다. 하지만 숫자 대신 통상적으로 이용되는 명칭을 사용하는 경우도 매우 빈번하다. 크게 고정부분과 이동부분으로 나누어지며, 각 링크계의 표현법은 다음과 같다.
고정부분
1. 베이스부(기저계)
베이스부는 Manipulator가 지면에 고정되어 설치되는 부분을 가정해서 설정하고, Manipulator의 기준 좌표계의 역할을 한다. 보통 {0}으로 시작하는 링크계를 의미한다. 이런 베이스부는 {$B$}로 표시하며, 기저계라는 이름으로 사용하기도 한다.
2. 스테이션부(정지계)
스테이션부는 Manipulator의 작업 대상물이 놓여져 있는 곳을 의미하며, 작업 대상물에 대한 절대/기준 좌표계이다. 모든 작업 대상물은 바로 이 스테이션부에서 상대적으로 측정된 위치에 따라 좌표계가 결정된다. {$S$}로 표시하며, 움직이지 않는 기준 좌표계이기 때문에 정지계라고도 한다.
이동부분
1. 손목부(손목계)
Manipulator를 구성하는 링크 중 마지막으로 연결된 부분을 가리키며, 다양한 End-effector가 결합될 수 있는 부분이다. 보통 손목 부분은 사람의 손목과 같이, 한 점에서 2자유도 또는 3자유도를 가져 다양한 자세를 취할 수 있다. {$W$}로 표시하며, Manipulator 자체의 원점을 이 손목계의 원점과 일치시키는 경우가 많다. Manipulator의 손목계는 기저계 {$B$}를 기준으로 변환 행렬의 연쇄적인 곱에 의해서 결정되므로 {$W$} $= {}^BT_W$가 된다.
2. 말단부(공구계)
손목부에 부착되는 End-effector의 끝 지점에 부여되는 좌표계이며 공구계 {$T$}로 표시한다. Manipulator의 최종 목적지는 직교 좌표 공간상에서 이 공구계의 원점과 일치한다. End-effector는 다양한 크기와 형상을 가지고 있기 때문에 ${}^BT_T$를 계산해놓더라도 End-effector가 바뀌면 다시 처음부터 계산해야 한다. 따라서 변하지 않는 손목계까지(${}^BT_W$)만 사전에 계산해놓고, End-effector에 따라 손목계로부터 상대적인 좌표계(${}^WT_T$)를 따로 정의하여 ${}^BT_T = {}^BT_W {}^WT_T$ 와 같이 사용하는 것이 일반적이다.
3. 물체부(목표계)
목표하는 지점을 표시하는 좌표계로 {$G$}를 사용한다. Manipulator의 말단부 즉, 공구계의 끝점은 최종적으로 목표계의 좌표와 일치해야 한다.
작업공간
Manipulator의 End-effector가 도달할 수 있는 범위는 작업공간(Workspace)이라고 한다. Manipulator의 작업공간은 Manipulator의 관절 변수 값을 적절하게 조절하여 End-effector가 Base를 기준으로 도달할 수 있는 모든 공간이라고 할 수 있다.
작업공간은 도달가능 작업공간(Reachable workspace)과 자유자재 작업공간(Dextrous workspace)으로 구분된다. 도달가능 작업공간은 Manipulator의 End-effector가 최소 하나의 방위로 도달할 수 있는 공간을 말하고, 자유자재 작업공간은 Manipulator의 End-effector가 모든 방위에서 도달할 수 있는 공간을 의미한다.
사람으로 말하자면 손을 뻗어서 닿을 수 있는 모든 공간이 도달가능 작업공간이고, 단순히 도달하는 것 뿐만 아니라 어떠한 방향에서도 도달할 수 있는 공간이 자유자재 작업공간이다. 즉, (도달가능 작업공간) $\supset$ (자유자재 작업공간)이라는 관계를 가진다.
복수개의 해를 갖는 역기구학
기구학적인 해를 찾는 과정에서 특정 position과 orientation을 만족시키는 해가 유일하지 않다는 문제가 있을 수 있다. 3차원 공간에서 7자유도 이상의 Manipulator의 경우, 링크 길이와 관절 변수 값의 범위가 적절하다면 넓은 자유자재 작업공간을 갖기 때문에, 해당 공간에서는 한가지 pose에 대해 복수개의 해를 갖게 된다.
3차원 공간에서 객체가 자유롭게 움직이기 위해서는 6자유도가 필요하다. 우선 어느 점으로 움직이는데 3자유도가 필요하고, 해당 위치에서 세 축을 중심으로 회전해 특정 방위를 갖는데 3자유도가 필요하다. 따라서 Manipulator가 6자유도를 가지는 경우, 작업공간 내에 특정 position과 orientation을 만족시키는 해는 한가지가 된다. 하지만 7자유도 이상을 가지면 End-effector의 position과 orientation이 고정된 상태에서도 추가 자유도를 통해 Manipulator의 나머지 부분이 움직일 수 있다. 즉, 특정 pose에 대해 복수개의 해를 가지게 된다.
Manipulator의 최종적인 자세를 결정할 때, 추가적으로 유효하거나 적절하다고 판단할 수 있는 기준 또는 고려 사항들이 필요한 경우가 있다. 예를 들어, 작업공간 내의 장애물과 충돌을 피할 수 있는 조건 또는 이동 거리를 최소화할 수 있는 조건 등을 만족하는 해를 선택하는 것이다.
해석적 방법(Analytical method)으로 기구학 방정식의 해를 유도하기 어려운 경우에는 대안적으로 수치 해석적 방법(Numerical method)을 이용하여 해를 발견할 수 있다. 하지만, 특정 주기마다 수행하는 반복적인 연산에 시간이 요구되고 연산이 종료될 때까지는 Manipulator의 다음 동작이 이루어지지 않는다. 그러므로 고속의 실시간 작업을 요구하는 자동화 산업 현장에는 수치 해석적인 방법을 적용하기에 한계가 있다.
따라서 수치 해석적인 방법 대신에 해석적인 방법인 기하학적(Geometric) 방법이나 대수학적(Algebraic) 방법을 주로 사용하여 역기구학의 해를 찾는다. 일반적으로 End-effector의 위치에서 시작하여 베이스부까지 순차적으로 이동하면서 찾고자 하는 관절 변수 이외의 다른 관절들을 최대한 고립시키고 단순화한 후, 기하학적 또는 대수학적 연산을 통해서 해를 찾아 나간다.
역기구학의 대수학적 풀이 방법과 절차
아래 그림과 같은 2자유도 평면형 Manipulator에서 역기구학을 풀어가는 방법과 순서에 대해 살펴보자.
앞 장에서 배웠던 [D-H 표기법]를 사용하여 위 Manipulator의 D-H parameters를 표현하면 아래 표와 같다.
| $i-1$ | 링크($i$) | $\alpha_{i-1}$ | $a_{i-1}$ | $d_i$ | $\theta_i$ |
|---|---|---|---|---|---|
| 0 | 1 | 0 | 0 | 0 | $\theta_1$ |
| 1 | 2 | 0 | $L_1$ | 0 | $\theta_2$ |
| 2 | 3 | 0 | $L_2$ | 0 | 0 |
또한 앞서 구했던 [${}^{i-1}T_i$]식을 이용하여 Manipulator의 기저계({$B$})를 기준으로 손목계({$W$})에 대한 완전한 기구학식 ${}^BT_W={}^0T_3={}^0T_1{}^1T_2{}^2T_3$ 을 나타낼 수 있다. 우선 ${}^{i-1}T_i$는 다음과 같다.
\[\def\arraystretch{1.2} {}^{i-1}T_{i} =\begin{bmatrix} \cos\theta_i & -\sin\theta_i & 0 & a_{i-1} \\ \sin\theta_i\cos\alpha_{i-1} & \cos\theta_i\cos\alpha_{i-1} & -\sin\alpha_{i-1} & -\sin\alpha_{i-1}d_i \\ \sin\theta_i\sin\alpha_{i-1} & \cos\theta_i\sin\alpha_{i-1} & \cos\alpha_{i-1} & \cos\alpha_{i-1}d_i \\ 0 & 0 & 0 & 1\end{bmatrix}\]${}^{i-1}T_i$을 활용하여 ${}^0T_1,{}^1T_2,{}^2T_3$을 구해보면 다음과 같다.
\[\def\arraystretch{1.2} \displaylines{ {}^{0}T_{1} =\begin{bmatrix} \cos\theta_1 & -\sin\theta_1 & 0 & 0 \\ \sin\theta_1 & \cos\theta1 & 0 & 0 \\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}, ~ {}^{1}T_{2} =\begin{bmatrix} \cos\theta_2 & -\sin\theta_2 & 0 & L_1 \\ \sin\theta_2 & \cos\theta_2 & 0 & 0 \\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}, ~ {}^{2}T_{3} =\begin{bmatrix} 1 & 0 & 0 & L_2 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} }\]그리고 이를 통해 최종적으로 ${}^0T_3$를 구할 수 있다.
\[\def\arraystretch{1.2} \begin{align*} {}^0T_3&={}^0T_1{}^1T_2{}^2T_3 \\&= \begin{bmatrix} c_{12} & -s_{12} & 0 & L_1c_1+L_2c_{12} \\ s_{12} & c_{12} & 0 & L_1s_1+L_2s_{12} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{align*}\]이때 $c_1 = \cos\theta_1, c_{12} = \cos(\theta_1 + \theta_2), s_1 = \sin\theta_1, s_{12} = \sin(\theta_1 + \theta_2)$ 이다.
말단부(손목계)의 위치 좌표를 ($x,y$)로, 말단부의 방향을 원점계(기저계)를 기준으로 회전한 각도($\phi=\theta_1 + \theta_2$)로 표현하면, ${}^BT_W$를 다음과 같은 형태로 나타낼 수 있다.
\[\def\arraystretch{1.3} \begin{align*} & {}^{B}T_{W} &&= \begin{bmatrix} c_{\phi} & -s_{\phi} & 0 & x \\ s_{\phi} & c_{\phi} & 0 & y \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\[4pt] =& ~~{}^{0}T_{3} &&= \begin{bmatrix} c_{12} & -s_{12} & 0 & L_1c_1+L_2c_{12} \\ s_{12} & c_{12} & 0 & L_1s_1+L_2s_{12} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ \end{align*}\]위로부터 다음과 같은 식이 얻어진다.
\[\displaylines{ x = L_1c_1+L_2c_{12} \\ y = L_1s_1+L_2s_{12} }\]위 두 식의 양변을 제곱하고 더해보자.
\[\begin{align*} x^2+y^2 &= (L_1^2c_1^2 + 2L_1L_2c_1c_{12} + L_2^2c_{12}^2) + (L_1^2s_1^2 + 2L_1L_2s_1s_{12} + L_2^2s_{12}^2) \\[3pt] &=L_1^2(c_1^2+s_1^2) + 2L_1L_2(c_1c_{12} + s_1s_{12}) + L_2^2(c_1^2+s_1^2) \\[3pt] & = L_1^2 + L_2^2 + 2L_1L_2c_2 \end{align*}\]$c_2$에 대해 정리하면 다음과 같은 식을 얻을 수 있다.
\[c_2 = {x^2 + y^2 -L_1^2 -L_2^2\over 2L_1L_2}\]위 식이 해를 갖기 위해서는 우변이 -1과 +1 사이의 값을 가져야 한다( $\because -1\le c_2 \le 1$ ). 따라서, 이 범위를 초과하는 경우에는 목표점 ($x,y$)가 Manipulator의 도달 가능 영역이 아니라고 판단할 수 있다. 만약 해를 갖는다면, 삼각함수의 법칙에 의해 $s_2$를 구할 수 있고, 구한 두 값과 $\text{atan2}$ 함수를 이용하면 $\theta_2$를 구할 수 있다.
\[\displaylines{ s_2 = \pm\sqrt{1-c_2^2}\\[2pt] \theta_2 = \text{atan2}(s_2, c_2) }\]이때, $s_2$의 부호에 따라서 $\theta_2>0$ 또는 $\theta_2 <0$ 두가지 경우가 있을 수 있으므로 상황에 가장 적합한 값을 선택해야 한다. 아래 그림처럼 $\theta_2>0$ 인 경우에는 팔꿈치(두번째 관절)가 하향 자세(elbow down)를 취하게 되고, $\theta_2 <0$ 인 경우에는 팔꿈치가 상향 자세(elbow up)를 취하게 된다.
일반적인 $\text{arctan}$ 함수는 입력값의 비율을 계산해 각도를 구하기 때문에, 사분면의 정보가 손실되어 정확한 방향을 알 수 없다. 반면, $\text{atan2}$ 함수는 두 값의 부호를 모두 고려하여 각도를 계산하기 때문에, 모든 사분면에서의 각도를 정확히 구할 수 있다. 특히 $x=0$인 경우에도 오류 없이 각도를 계산할 수 있어, 로봇 팔이 특정 방향에 정렬되었을 때 발생할 수 있는 문제를 해결해 준다. 이러한 이유로 $\text{arctan}$ 함수 대신 $\text{atan2}$ 함수가 역기구학에서 사용된다.
$\theta_2$가 결정되면 $x$와 $y$에 관한 식은 $\theta_1$에 대한 방정식이 되므로 다음과 같이 변형할 수 있다.
\[\begin{align*} x & = L_1c_1 + L_2c_{12} & ~~~~y & = L_1s_1 + L_2s_{12}\\ & = L_1c_1 + L_2(c_1c_2 - s_1s_2) & &= L_1s_1 + L_2(c_1s_2 + s_1c_2)\\ & = (L_1 + L_2c_2)c_1 -(L_2s_2)s_1 & & = (L_1 + L_2c_2)s_1 +(L_2s_2)c_1 \end{align*}\]변수의 순서를 맞추어서 위 식을 정리하면, 다음과 같다.
\[\begin{align*} x&= (L_1 + L_2c_2)c_1 -(L_2s_2)s_1 = &(L_1 + L_2c_2)&c_1 &&- &(L_2s_2)&s_1 \\[2pt] y&= (L_1 + L_2c_2)s_1 +(L_2s_2)c_1 = &(L_2s_2)&c_1 &&+ &(L_1 + L_2c_2)&s_1\\ \end{align*}\]그리고 이를 행렬로 나타내면 다음과 같다.
\[\def\arraystretch{1.2} \begin{bmatrix} L_1 + L_2c_2 & -L_2s_2 \\ L_2s_2 & L_1 + L_2c_2 \\ \end{bmatrix} \begin{bmatrix} c_1 \\ s_1 \\ \end{bmatrix} = \begin{bmatrix} x \\ y \\ \end{bmatrix}\]크래머 규칙(Cramer’s rule)을 이용하면 위 식으로부터 $c_1$과 $s_1$을 구할 수 있다.
\[\def\arraystretch{1.2} \displaylines{ c_1 = {1\over D} \begin{vmatrix} x & -L_2s_2 \\ y & L_1 + L_2c_2 \\ \end{vmatrix} ,~ s_1 = {1\over D} \begin{vmatrix} L_1 + L_2c_2 & x \\ L_2s_2 & y \\ \end{vmatrix} }\]이때 $D$는 행렬식을 의미하며 구체적인 식은 다음과 같다.
\[\begin{align*} D &= \det\begin{pmatrix} L_1 + L_2c_2 & -L_2s_2 \\ L_2s_2 & L_1 + L_2c_2 \\ \end{pmatrix} \\ &=L_1^2 + L_2^2 + 2L_1L_2c_2 \end{align*}\]$c_1$과 $s_1$를 구했으므로 $\text{atan2}$ 함수를 이용하면 $\theta_1$을 구할 수 있다. ( $\theta_1 = \text{atan2}(s_1, c_1)$ )
위 방법 말고도, 앞서 나왔던 아래 식을 통해 $\theta_2$를 구하는 다른 대수학적 풀이 방법을 사용할 수 있다.
\[\begin{align*} x&= (L_1 + L_2c_2)c_1 -(L_2s_2)s_1 = &(L_1 + L_2c_2)&c_1 &&- &(L_2s_2)&s_1 \\[2pt] y&= (L_1 + L_2c_2)s_1 +(L_2s_2)c_1 = &(L_2s_2)&c_1 &&+ &(L_1 + L_2c_2)&s_1\\ \end{align*}\]위 식에서 $m =L_1 + L_2c_2~, ~ n = L_2s_2$ 로 설정하면, 더 간단하게 표현할 수 있다.
\[\begin{align*} x&= (L_1 + L_2c_2)c_1 - (L_2s_2)s_1 = mc_1 + ns_1\\[2pt] y&= (L_2s_2)c_1 + (L_1 + L_2c_2)s_1 = nc_1 + ms_1\\ \end{align*}\]그리고 이 식을 바탕으로 아래 그림에서 삼각형의 닮음 성질을 이용하여 다른 특성들을 도출할 수 있다. (아래 그림에서 $\Delta$는 $\theta_1$과 크기가 같은 동위각을 표현하고 있다.)
위 그림에서 굵은 검은색 선으로 이루어진 직각삼각형의 밑변의 길이를 $m$, 높이의 길이를 $n$으로 정의할 수 있다. 빗변의 길이는 피타고라스의 정리에 의해 $\gamma = \sqrt{m^2+n^2}$ 이 된다. 그리고 해당 직각삼각형의 한 각의 크기는 $\text{atan2}$ 함수를 이용해 $\rho = \text{atan2}(n,m)$ 와 같이 나타낼 수 있다.
이번에는 빗변($\gamma$)을 공유하고, $X$축과 이루는 각의 크기가 $\rho + \theta_1$ 인 직각삼각형을 살펴보자. 최종 목표점 ($x,y$)는 다음과 같이 표현될 수 있다.
\[\begin{align*} x & = \gamma\cos(\rho+\theta_1) \\[2pt] y & = \gamma\sin(\rho+\theta_1) \end{align*}\]마찬가지로 $\text{atan2}$ 함수를 이용하면 $\rho + \theta_1$ 값을 구할 수 있고, 앞서 구한 $\rho$ 값을 대입하여 최종적으로 $\theta_1$을 다음과 같이 정의할 수 있게 된다.
\[\begin{align*} \rho + \theta_1 &= \text{atan2}({y\over\gamma}, {x\over\gamma}) \\ &= \text{atan2}(y, x) \\[7pt] \theta_1 &= \text{atan2}(y,x) - \rho \\ &= \text{atan2}(y,x) - \text{atan2}(n,m) \end{align*}\]역기구학의 기하학적 풀이 방법과 절차
앞서 살펴보았던 풀이는 대수학적으로 계산한 역기구학의 해이다. 단순한 Manipulator에서는 대수학적 방법으로 역기구학의 해를 구하는 것이 가능하지만, Manipulator의 자유도가 높아지고 형상이 복잡해질수록 대수학적 접근 방법이 매우 난해해진다.
Manipulator의 End-effector가 직교 좌표계 상의 위치와 방위로 주어지면, Manipulator의 공간적 형상을 일반적인 평면 기하학 문제로 분해하여 접근할 수 있다. 평면 기하학적 방법은 Manipulator의 역기구학 해를 찾는 보편화된 방법이기도 하다.
앞에서 다루었던 2자유도 평면형 Manipulator에 대한 역기구학 해를 기하학적 방법으로 유도해보자.
먼저 위와 같이 Manipulator의 기저(Base)부의 원점과 마지막 링크의 끝점(말단부)을 연결하면 두 링크를 포함하는 평면 삼각형을 표현할 수 있으며, 해당 삼각형에서 삼각함수의 제2 코사인 법칙을 적용하여 다음과 같은 식을 정의할 수 있다.
\[\begin{align*} x^2 + y^2 &= L_1^2 + L_2^2 -2L_1L_2\cos(\pi-\theta_2) \\[2pt] &= L_1^2 + L_2^2 +2L_1L_2\cos(\theta_2) \\[5pt] \cos(\theta_2) &= {x^2 + y^2 - (L_1^2 + L_2^2)\over2L_1L_2} \end{align*}\]기하학적으로 나타낸 Manipulator의 삼각형이 성립되기 위해서는 Manipulator의 기저부의 원점과 마지막 링크의 끝점을 연결한 선분의 길이인 $\sqrt{x^2+y^2}$ 이 항상 링크 길이의 합 $L_1+L_2$ 와 같거나 작아야 한다. 이러한 조건을 만족할 때, $\theta_2$는 $0^\circ \le \theta_2 \le 180 ^\circ$ 사이의 값을 가지게 된다. 대칭해는 대수학적의 풀이에서와 마찬가지로 팔꿈치의 상향 자세인 $-\theta_2$로 구할 수 있다.
$\theta_1$에 대한 기하학적 접근 방법은 그림에서와 같이 $\alpha$와 $\beta$를 정의함으로써 계산할 수 있다. $\alpha$는 $\text{atan2}$ 함수를 사용하여 구할 수 있고, $\beta$는 $\theta_2$를 구할 때와 마찬가지로 제2 코사인 법칙을 적용하여 구할 수 있다.
\[\begin{align*} \alpha &= \text{atan2}(y,x) \\ \beta &= \arccos({x^2 + y^2 + L_1^2 - L_2^2\over2\sqrt{x^2 + y^2}L_1}) \end{align*}\]앞에서 구한 $\theta_2$의 제약 조건과 마찬가지로 $\beta$ 값의 범위가 $0^\circ \le \beta \le 180 ^\circ$ 를 만족시키도록 해야만 한다. 이와 같은 제약 조건은 기하학적으로 역기구학의 해를 계산할 때 반드시 고려되어야 하는 사항이며, 이를 만족하면 Manipulator의 동작 범위 또는 작업 공간 내에서 만들어낼 수 있는 기구학적 해임을 보장한다. 최종적으로 제약 조건을 반영한 $\theta_1$에 대한 식은 다음과 같다.
\[x = \begin{cases} \alpha + \beta &\text{if }~ \theta_2 < 0 \\ \alpha - \beta &\text{if }~ \theta_2 \ge 0 \end{cases}\]