In the previous two tutorials we examined methods of forward
kinematics for a six axis robot model where we computed the real
world co-ordinates of the tool tip for known joint angles. In this tutorial we consider the problem in the
opposite direction, that is we know the intended co-ordinates of the
end effector and from this information compute the joint angles.
This procedure is called ** inverse kinematics**.

Unlike forward kinematics, there is no generic solution for inverse kinematics that can be applied to all configurations of robot manipulators. Text books generally state three categories of solutions:

- geometric
- analytic
- numerical

Numerical solutions apply iterative processes requiring complex software and are outside the scope of this series of tutorials.

Geometric methods are straightforward where there are a limited number of links. We present an example using the robot model to give a complete solution but only for a limited range of orientations of the tool tip.

Analytic solutions exist in varying degrees of complexity, depending on the number of joints but there is no single generic solution for a six axis robot. We will outline a method using transformation matrices that is valid under certain conditions.

Fig.1 below shows a six axis robot model similar to the one used
in the tutorials for forward kinematics. However, in this model
there are no offsets between joints 0 & 1, joints 1 & 2 and
joints 2 & 3. Also, the rotational axes of joints 3, 4 and 5
now intersect to provide a * spherical wrist
*configuration for the final three joints. Note that the
origins of co-ordinate frames 3, 4 and 5 are now coincident; which
assists the analytical method for inverse kinematics explained later.

The basis for this example is the pose for the tool tip shown
above in
Fig.2. The co-ordinates of the origin of frame 5* relative to
the base
frame are
(in unspecified length units): x_{5} = 695, y_{5} =
781, z_{5} = -401. The tool tip is 110 units
vertically below this point. In this configuration joint 4 and joint 6 do
not rotate. Consequently joint angle θ_{5} is determined by joint
angles θ_{2} and θ_{3}. In effect, this
configuration has three degrees
of freedom.

* These co-ordinates are also the origin of frames 4 and 6. The choice of frame 5 to designate this point is arbitrary.

Fig.3 below illustrates the geometry of the robot in the required
configuration with the joints aligned with the X axis in the datum
position. Note that the "elbow" at joint 3 is in the up
position. The inverse kinematic problem is to find joint angles θ_{1},
θ_{2}, θ_{3 },θ_{5}

*For θ*_{1}

From the top view θ_{1} = tan^{-1}(z_{5} /
x_{5}) = tan^{-1}(401/
695)
gives θ_{1}
= 29.98° The
diagram confirms rotation is +ve (Y_{0} axis coming out the
page)

*For θ*_{2}

From the top view r = z_{5} / sin θ_{1}
= (401) / sin (29.98°) = 802

From the side view on direction A tan^{-1}φ =
tan^{-1}(y_{5} - a_{1}) / r) = tan^{-1}(621 / 802)
gives φ = 37.74°

Also L_{1}^{2}
= r^{2} + (y_{5 }-^{ }a_{1})^{2}
= (802)^{2} + (781 - 160)^{2}

gives L_{1} = √[(802)^{2} +
(621)^{2} ] = 1015L2

From the robot model L_{2} = 400
L_{3} = 630

Using the law of cosines for triangle with sides L_{1}, L_{2},
L_{3} and internal angles λ_{1} λ_{2} λ_{3}
gives L_{3}^{2} = L_{1}^{2}
+ L_{2}^{2} - 2L_{1}.L_{2}.cos λ_{3}

and thus θ_{2}
= (φ + λ_{3 }) = (37.74° + 12.47°) = 50.21°
The diagram confirms rotation is +ve (Z_{1}
axis coming out the page)

*For θ*_{3}

Using the law of cosines for triangle with sides L_{1}, L_{2},
L_{3} and internal angles λ_{1} λ_{2} λ_{3}
gives L_{1}^{2} = L_{2}^{2}
+ L_{3}^{2} - 2L_{2}.L_{3}.cos λ_{1}

gives θ_{3} = (180° - λ_{1} ) = (180° - 159.66°)
= 20.34°
The diagram shows joint rotation is -ve.
Thus
θ_{3}
= - 20.34°

*For θ*_{5}

β = (θ_{2} - θ_{3}) = (50.21° - 20.34°) = 29.87°
and θ_{5} -= (90° + β)
gives
θ_{5} = 119.87°

The diagram shows joint rotation is
-ve. Thus **θ**_{5}
= - 119.87°

However, this solution for the joint angles is not unique. Fig.4 below illustrates another valid solution where the elbow joint is pointing down.

In the elbow down configuration:

θ_{2}
= (φ - λ_{3 }) = 25.27° (+ve rotation)

θ_{3} = 20.34° (+ve
rotation)

θ_{5} = (β + θ_{3}
+ 90°) = (θ_{2} + θ_{3}
+ 90°) = 135.61° (-ve rotation)

Other variable aspects of the robot configuration, such as offsets, introduce multiple inverse kinematic solutions. The robot model in the example above has no offsets but consider Fig.5 below where links are offset.

When the arms rotate towards the target from opposite sides the joint angles are not the same. As a consequence inverse kinematics must
define the direction of rotation, normally stated as *right hand* or
*left hand.*

We carry forward the above solution for joint angles θ_{1},
θ_{2}, θ_{3 } to the next section as part of an
inverse kinematic solution where all six joint angles rotate.

The analytic method described below treats the inverse kinematics solution for a six axis robot in two parts.

Firstly, a solution for joint angles θ_{1},
θ_{2}, θ_{3} is found. For this example
we used the geometric method outlined above to find θ_{1},
θ_{2}, θ_{3} We then derive rotation matrix R^{0}_{3}
and insert the values of θ_{1},
θ_{2}, θ_{3}

The second part requires joints 4, 5 and 6 to be in a spherical
wrist configuration (see above). Because of this, rotation
matrix R^{3}_{6}
alone defines transformations
between co-ordinate frame 3 and co-ordinate frame 6. The elements
of R^{3}_{6} are expressed in terms of sine and
cosine functions of joint angles θ_{4},
θ_{5}, θ_{6}

We specify the orientation of co-ordinate frame 6 which defines
the orientation of the tool tip and also defines rotation matrix R^{0}_{6}
which is the rotational transformation between the base frame and
frame 6.

Now a clever bit of linear algebra:

R^{0}_{6} = R^{0}_{3} . R^{3}_{6}

multiply both sides by R^{0}_{3}^{-1}
(the inverse of R^{0}_{3}) gives
R^{0}_{3}^{-1}.R^{0}_{6} = R^{0}_{3}^{-1}.R^{0}_{3}
. R^{3}_{6}

as R^{0}_{3}^{-1}.R^{0}_{3}
= I (identity matrix) it follows R^{3}_{6}
= R^{0}_{3}^{-1}.R^{0}_{6}

We then compute R^{0}_{3}^{-1}.R^{0}_{6}
and use the equivalence with previously determined
matrix R^{3}_{6} to
solve for θ_{4},
θ_{5}, θ_{6}

We will work through this procedure using the parameters of the
previous example (elbow up). For convenience joint angles are
rounded to: θ_{1} = 30° (+ve), θ_{2} = 50° (+ve), θ_{3}
= 20° (-ve).

**Rotation matrix R**^{0}_{3}

R^{0}_{1} is rotation of +30° about the Y axis;
R^{1}_{2} is rotation of +50° about the Z axis;
R^{2}_{3} is rotation of -20° about the Z axis;

The inverse of a rotation matrix is its transpose, thus:

*Rotation matrix R*^{0}_{6}

Fig.6 below shows the orientation of the tool tip at datum
position (frame 0) and at the required pose (frame 6). Column vectors
at frame 6 show transformations to frame 0 by application of
rotation matrix R^{0}_{6 }. We deduce these
column vectors directly from the respective co-ordinate axes.
In this case the X_{0} and Z_{0} axes are rotated
through 90°. Finding column vectors for rotations through
other angles would require a more detailed diagram.

This stage provides good insight into transformations between
reference frames. R^{0}_{6} transforms
vectors in frame 6 to vectors in frame 0.

*Compute rotation matrix R*^{3}_{6}

*Derive rotation matrix R*^{3}_{6}*
from first principles*

R^{3}_{4} is rotation of θ_{4} about the
X_{4} axis; R^{4}_{5} is
rotation of θ_{5} about the Z_{5} axis;
R^{5}_{6} is rotation of θ_{6} about the X_{6}
axis;

This next stage is not entirely straightforward since multiple
solutions are possible for each element. First consider the element r_{11}

cosθ_{5} = - 0.433 There are two
possible solutions: θ_{5} = (i) cos^{-1}(-0.433)
= 115.7° (ii) cos^{-1}(-0.433) = -115.7°

Choose θ_{5} = -115.7° giving sinθ_{5}
= - 0.901

Consider element r_{12} - (sinθ_{5}
cosθ_{6}) = 0.5

gives cosθ_{6} = - 0.5 / (-
0.901) = 0.555 thus θ_{6} =
cos^{-1}(0.555) = (i) 56.3° or (ii) - 56.3°

Choose θ_{6} = 56.3°
giving sinθ_{6} = 0.832

Check element r_{13} sinθ_{5}
sinθ_{6} = (- 0.901) (0.832) = - 0.75 (correct)
choosing θ_{6} = - 56.3° would be incorrect as
sin( - 56.3°) is -ve

Consider element r_{21} = cosθ_{4} sinθ_{5}

gives cosθ_{4} = 0.25 / ( - 0.901) = - 0.277
thus θ_{4} = cos^{-1}(- 0.277) = (i) 106.1° or
(ii) - 106.1°

Choose θ_{4} = - 106.1°
giving sinθ_{4} = - 0.961

Check element r_{31}
sinθ_{4} sinθ_{5} = (- 0.961) (- 0.901) =
0.866 (correct) choosing θ_{4}
= 106.1° would be incorrect as sin(106.1°) is +ve

To conclude: ** θ**_{4}**
= - 106.1° θ**_{5}**
= -115.7° θ**_{6}**
= 56.3°**

Elements r_{22} r_{23} r_{32} r_{33}
provide check sums to validate this solution.

A valid solution also occurs for θ_{5}
= + 115.7° the resulting joint angles being ** θ**_{4}**
= - 73.9° θ**_{5}**
= 115.7° θ**_{6}**
= -123.7°**

Fig.7 below shows the robot model: (a) at datum position, (b) pose with joint angles from this example, (c) top view of pose showing orientation of the tool tip as defined in Fig.6.

I welcome feedback at: