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:
- Suppose that a target position of the end point is \vec{p}_{\rm G}.
- 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.
- Using current angles, we calculate J^{\#}.
- Calculate \delta \vec{\theta}=J^{\#} \delta\vec{p}.
- Update the quantities, \vec{\theta} \leftarrow \vec{\theta} + \delta \vec{\theta}, \vec{p} \leftarrow \vec{p} + \delta \vec{p}.
- 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
- Forward Kinematics (in Japanese)
- Inverse Kinematics by means of Jacobian (in Japanese)
- Kinematics Problem of Robot (in Japanese)
