Kinematics 〜 Theory 〜

in Japanese

Introduction

　In this page, I describe a brief explanation on the kinematics used in the robotics. See also the next page as to the implementation of a simple six-axis-arm system in C++.

Forward Kinematics

　Let us consider the 3-dimensional homogeneous coordinate system. In the system, the rotation matrices around x, y, and z axises are given by $$\begin{equation} R_{x}(\theta) = \left( \begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & \cos{\theta} & -\sin{\theta} & 0 \\ 0 & \sin{\theta} & \cos{\theta} & 0 \\ 0 & 0 & 0 & 1 \end{array} \right), \end{equation}$$ $$\begin{equation} R_{y}(\theta) = \left( \begin{array}{cccc} \cos{\theta} & 0 & \sin{\theta} & 0 \\ 0 & 1 & 0 & 0 \\ -\sin{\theta} & 0 & \cos{\theta} & 0 \\ 0 & 0 & 0 & 1 \end{array} \right), \end{equation}$$ and $$\begin{equation} R_{z}(\theta) = \left( \begin{array}{cccc} \cos{\theta} & -\sin{\theta} & 0 & 0 \\ \sin{\theta} & \cos{\theta} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array} \right). \end{equation}$$ Moreover, the translation matrix $L(\vec{l})$ with a directional vector $\vec{l}$ is obtained by $$\begin{equation} L(\vec{l}) = \left( \begin{array}{cccc} 1 & 0 & 0 & l_x \\ 0 & 1 & 0 & l_y \\ 0 & 0 & 1 & l_z \\ 0 & 0 & 0 & 1 \end{array} \right). \end{equation}$$ Suppose that we have an initial state of a three-axis-arm system shown in the below figure.

In the figure, $n_i$ and $l_i$ indicate a joint and a length of each component, respectively. $n_1$ rotates $\theta_1$ in counterclockwise direction around $x$ axis, $n_2$ rotates $\theta_2$ in counterclockwise direction around $y$ axis, and $n_3$ rotates $\theta_3$ in counterclockwise direction around $x$ axis. $\vec{l}_1=(0,0,l_1)$, $\vec{l}_2=(0,0,l_2)$, and $\vec{l}_3=(0,0,l_3)$. A position vector of an end point $n_4$ takes the form $$\begin{equation} \vec{p}_{4} = R_x(\theta_1) L(\vec{l}_1) R_y(\theta_2) L(\vec{l}_2) R_x(\theta_3) L(\vec{l}_3) \left( \begin{array}{c} 0 \\ 0 \\ 0 \\ 1 \end{array} \right). \label{sample-case} \end{equation}$$ It must be noted that parameters $\theta_i$ and $\vec{l}_i$ are defined in the local coordinate system of the $i$-th joint. We can expand eq.($\ref{sample-case}$) to the case of $n$-axis-arm system as $$\begin{equation} \vec{p} = R_{a_{1}}(\theta_1) L(\vec{l}_1) R_{a_{2}}(\theta_2) L(\vec{l}_2) \cdots R_{a_{n}}(\theta_n) L(\vec{l}_n) \left( \begin{array}{c} 0 \\ 0 \\ 0 \\ 1 \end{array} \right), \end{equation}$$ where $a_{i}\in \{x,y,z\}$.

Inverse Kinematcis

　Setting lengths to constant values, the vector $\vec{p}$ becomes a function with $n$ angles in the form $$\begin{equation} \vec{p}= \vec{f}(\theta_1,\theta_2,\cdots,\theta_n). \end{equation}$$ Its deviation with respect to $\{\theta_1,\theta_2,\cdots,\theta_n\}$ is written as $$\begin{eqnarray} \delta\vec{p} &=& \vec{f}(\theta_1+\delta\theta_1,\theta_2+\delta\theta_2,\cdots,\theta_n+\delta\theta_n) - \vec{f}(\theta_1,\theta_2,\cdots,\theta_n)\nonumber \\ &=&\sum_{i=1}^{n}\;\frac{\partial\vec{f}}{\partial\theta_i}\;\delta\theta_i + \frac{1}{2}\sum_{i=1}^{n}\sum_{j=1}^{n}\;\frac{\partial^2\vec{f}}{\partial\theta_i\partial\theta_j}\;\delta\theta_i\;\delta\theta_j +\cdots. \end{eqnarray}$$ Ignoring the terms more than first derivative, we can obtain the form $$\begin{eqnarray} \left( \begin{array}{c} \delta p_x \\ \delta p_y \\ \delta p_z \end{array} \right) &=& \left(\frac{\partial\vec{f}}{\partial\theta_1},\frac{\partial\vec{f}}{\partial\theta_2},\cdots,\frac{\partial\vec{f}}{\partial\theta_n}\right) \left( \begin{array}{c} \delta \theta_1 \\ \delta \theta_2 \\ \vdots \\ \delta \theta_n \end{array} \right) \nonumber \\ &=& \left( \begin{array}{cccc} \frac{\partial f_1}{\partial \theta_1} & \frac{\partial f_1}{\partial \theta_2} & \cdots & \frac{\partial f_1}{\partial \theta_n} \\ \frac{\partial f_2}{\partial \theta_1} & \frac{\partial f_2}{\partial \theta_2} & \cdots & \frac{\partial f_2}{\partial \theta_n} \\ \frac{\partial f_3}{\partial \theta_1} & \frac{\partial f_3}{\partial \theta_2} & \cdots & \frac{\partial f_3}{\partial \theta_n} \end{array} \right) \left( \begin{array}{c} \delta \theta_1 \\ \delta \theta_2 \\ \vdots \\ \delta \theta_n \end{array} \right) \nonumber \\ &\equiv& J \left( \begin{array}{c} \delta \theta_1 \\ \delta \theta_2 \\ \vdots \\ \delta \theta_n \end{array} \right), \end{eqnarray}$$ where $J$ is called the Jacobian matrix. In this case, it is $3 \times n$ matrix. A partial differential $\frac{\partial \vec{f}}{\partial \theta_i}$ is calculated as $$\begin{equation} \frac{\partial \vec{f}}{\partial \theta_i} = R_{a_{1}}(\theta_1) L(\vec{l}_1) \cdots \frac{dR_{a_{i}}(\theta_i)}{d\theta_i} L(\vec{l}_i) \cdots R_{a_{n}}(\theta_n) L(\vec{l}_n) \left( \begin{array}{c} 0 \\ 0 \\ 0 \\ 1 \end{array} \right). \end{equation}$$ By introducing the inverse matrix $J^{-1}$, we can obtain the deviation vector $\delta \vec{\theta}$ of angles as $$\begin{equation} \delta \vec{\theta} = J^{-1}\;\delta\vec{p}. \end{equation}$$ When ${\rm rank}(J)>3$, the inverse matrix $J^{-1}$ is not uniquely determined. In other words, in a case where the number of joints are greater than 3, the end position ($\vec{p}$) is achieved by different postures. The typical method to determine a unique posture is to use a pseudoinverse matrix $J^{\#}$ as $$\begin{equation} \delta \vec{\theta} = J^{\#} \delta\vec{p}, \end{equation}$$ where $$\begin{equation} J^{\#}=J^{T}\;(J\;J^{T})^{-1}. \end{equation}$$ The procedure to solve the inverse kinematics problem is as follows:

1. Suppose that a target position of the end point is $\vec{p}_{\rm G}$.
2. Calculate a displacement $\vec{d} = \vec{p}_{\rm G}-\vec{p}$ where $\vec{p}$ indicates the current positoin of the end point. Using $\vec{d}$, we make a small deviation $\delta \vec{p}=\alpha\;\vec{d}/|\vec{d}|$ where $\alpha$ is an appropriate positive value.
3. Using current angles, we calculate $J^{\#}$.
4. Calculate $\delta \vec{\theta}=J^{\#} \delta\vec{p}$.
5. Update the quantities, $\vec{\theta} \leftarrow \vec{\theta} + \delta \vec{\theta}, \vec{p} \leftarrow \vec{p} + \delta \vec{p}$.
6. After updating them, if the displacement $|\vec{d}| = |\vec{p}_{\rm G}-\vec{p}|$ is not in an acceptable error range $\epsilon$, return to 2.

References

1. Forward Kinematics (in Japanese)
2. Inverse Kinematics by means of Jacobian (in Japanese)
3. Kinematics Problem of Robot (in Japanese)