Robotics Kinematics and Dynamics/Serial Manipulator Position Kinematics

Forward Position Kinematics
The forward position kinematics problem can be stated as follows: given the different joint angles, what is the position of the end-effector? With the previous sections in mind, the answer is rather simple: construct the different transformation matrices and combine them in the right way, the result being $$^{ee}_{bs}T \,$$, where $$\{bs\}$$ is the base frame of the robot manipulator.

Solution
Suppose the mutual orientation matrices between adjacent links are known. (As the fixed parameters of each link are known, and the joint angles are a given to the problem, these can be calculated. One possible way to do this would be to make use of the Denavit-Hartenberg convention.) The transformation that relates the last and first frames in a serial manipulator arm, and thus, the solution to the forward kinematics problem, is then represented by the compound homogeneous transformation matrix. The axes are moving, thus, the compound homogeneous transformation matrix is found by premultiplying the individual transformation matrices:
 * $$^{ee}_{bs}T = \, ^{N}_{0}T = \, ^{1}_{0}T \, ^{2}_{1}T \ldots \, ^{N-1}_{N-2}T \,^{N}_{N-1}T$$

The Planar Three-Link Manipulator


The equations below use 3 &times; 3 pose matrices, as this is just a 2-dimensional case (cf. the figure on the right).

The pose of the first link, relative to the reference frame, is given by (recall the elementary rotation about the z-axis from the previous section):

T_1(\theta_1) = \begin{pmatrix} \cos(\theta_1) & -\sin(\theta_1) & 0 \\ \sin(\theta_1) & \cos(\theta_1) & 0 \\ 0 & 0 & 1 \end{pmatrix} $$

The pose of the second link, relative to the first link, is given by:

T_2(\theta_2) = \begin{pmatrix} \cos(\theta_2) & -\sin(\theta_2) & l_1 \\ \sin(\theta_2) & \cos(\theta_2) & 0 \\ 0 & 0 & 1 \end{pmatrix} $$

This corresponds to a rotation by an angle $$\theta_2$$ and a translation by a distance $$l_1$$, where $$l_1$$ is the length of the first link.

The pose of the third link, relative to the second link, is given by:

T_3(\theta_3) = \begin{pmatrix} \cos(\theta_3) & -\sin(\theta_3) & l_2 \\ \sin(\theta_3) & \cos(\theta_3) & 0 \\ 0 & 0 & 1 \end{pmatrix} $$

The pose of the end effector, relative to the third link, is given by:

T_4 = \begin{pmatrix} 1 & 0 & l_3 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{pmatrix} $$

The solution to the forward kinematics problem is then:
 * $$^{ee}_{bs}T = \, ^4_0T(\theta_1, \theta_2, \theta_3) =

\begin{pmatrix} c_{123} & -s_{123} & l_1 c_1 + l_2 c_{12} + l_3 c_{123} \\ s_{123} & c_{123} & l_1 s_1 + l_2 s_{12} + l_3 s_{123} \\ 0 & 0 & 1 \end{pmatrix} $$

Hence:

\begin{pmatrix} x \\ y \\ 1 \end{pmatrix} = T \begin{pmatrix} 0 \\ 0 \\ 1 \end{pmatrix} $$

The resulting kinematic equations are:

\begin{array}{l} x = l_1 \cos \theta_1 + l_2 \cos(\theta_1 + \theta_2) + l_3 \cos(\theta_1 + \theta_2 + \theta_3) \\ y = l_1 \sin \theta_1 + l_2 \sin(\theta_1 + \theta_2) + l_3 \sin(\theta_1 + \theta_2 + \theta_3) \end{array} $$

Inverse Position Kinematics
The inverse kinematics problem is the opposite of the forward kinematics problem and can be summarized as follows: given the desired position of the end effector, what combinations of the joint angles can be used to achieve this position?



Two types of solutions can be considered: a closed-form solution and a numerical solution. Closed-form or analytical solutions are sets of equations that fully describe the connection between the end-effector position and the joint angles. Numerical solutions are found through the use of numerical algorithms, and can exist even when no closed-form solution is available. There may also be multiple solutions, or no solution at all.

Example: Planar Three-Link Manipulator
The inverse kinematics problem for this 2D manipulator can quite easily be solved algebraically.

From the earlier results (for simplicity, the displacement over the distance $$l_3$$ shall be omitted here):

^3_0 T = \begin{pmatrix} c_{123} & -s_{123} & 0 & l_1 c_1 + l_2 c_{12} \\ s_{123} & c_{123} & 0 & l_1 s_1 + l_2 s_{12} \\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \\ \end{pmatrix} $$

Now assume a given end-effector orientation in the following form:

^{ee}_{bs} T = \begin{pmatrix} c_{\phi} & -s_{\phi} & 0 & x \\ s_{\phi} & c_{\phi} & 0 & y \\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \\ \end{pmatrix} $$

Equating the two previous expressions results in:

\begin{array}{lll} c_{\phi} & = & c_{123} \\ s_{\phi} & = & s_{123} \\ x & = & l_1 c_1 + l_2 c_{12} \\ y & = & l_1 s_1 + l_2 s_{12} \\ \end{array} $$

As:

\begin{array}{lll} c_{12} & = & c_1c_2 - s_1 s_2 \\ s_{12} & = & c_1 s_2 + s_1 c_2 \\ \end{array} $$, squaring both the expressions for $$x$$ and $$y$$ and adding them, leads to:

x^2 + y^2 = l_1^2 + l_2^2 + 2 l_1 l_2 c_2 $$

Solving for $$c_2$$ leads to:

c_2 = \frac{x^2 + y^2 - l_1^2 - l_2^2}{2 l_1 l_2} $$, while $$s_2$$ equals:

s_2 = \pm \sqrt{1 - c_2^2} $$, and, finally, $$\theta_2$$:

\theta_2 = \mbox{Atan2}(s_2, c_2) $$

Note: The choice of the sign for $$s_2$$ corresponds with one of the two solutions in the figure above.

The expressions for $$x$$ and $$y$$ may now be solved for $$\theta_1$$. In order to do so, write them like this:

\begin{array}{lll} x & = & k_1 c_1 - k_2 s_1 \\ y & = & k_1 s_1 + k_2 c_1 \\ \end{array} $$ where $$k_1 = l_1 + l_2 c_2$$, and $$k_2 = l_2 s_2$$.

Let:

\begin{array}{lll} r & = & \sqrt{k_1^2 + k_2^2} \\ \gamma & = & \mbox{Atan2}(k2,k1) \\ \end{array} $$

Then:

\begin{array}{lll} k_1 & = & r \cos \gamma \\ k_2 & = & r \sin \gamma \\ \end{array} $$

Applying these to the above equations for $$x$$ and $$y$$:

\begin{array}{lll} x/r & = & \cos \gamma \, c_1 + \sin \gamma \, s_1 \\ y/r & = & \cos \gamma \, s_1 + \sin \gamma \, c_1 \\ \end{array} $$, or:

\begin{array}{lll} \cos (\gamma + \theta_1) & = & \frac{x}{r} \\ \sin (\gamma + \theta_1) & = & \frac{y}{r} \\ \end{array} $$

Thus:

\gamma + \theta_1 = \mbox{Atan2}(y,x) $$

Hence:

\theta_1 = \mbox{Atan2}(y,x) - \mbox{Atan2}(k_2,k_1) $$

Note: If $$x = y = 0$$, $$\theta_1$$ actually becomes arbitrary.

$$\theta_3$$ may now be solved from the first two equations for $$s_{\phi}$$ and $$c_{\phi}$$:

\theta_3 = \phi - \theta_1 - \theta_2 = \mbox{Atan2}(s_{\phi},c_{\phi}) - \theta_1 - \theta_2 $$