Kinematics of robot manipulators - inverse kinematics

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 specified joint angles. In this tutorial we consider the problem in the opposite direction, that is we know the intended co-ordinates and orientation of the tool tip 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

Geometric solutions are straightforward where there are a limited number of links. We present an example using our robot model to give a complete solution for a limited range of positions 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.

Numerical solutions apply iterative mathematical processes requiring complex software and are beyond the scope of these tutorials.

Six axis robot model

Figs.1(a) and (b) below shows the robot model with six revolute joints used in this tutorial. This model is similar to one used in previous tutorials for forward kinematics, but in this model there are no offsets between joints. Also, the rotational axes of joints 4, 5 and 6 intersect to provide a spherical wrist configuration for these joints. This arrangement simplifies the analytical method for inverse kinematics presented below.

X₀, Y₀ and Z₀ are the real world co-ordinates of the robot base. The datum pose for the robot is a horizontal extension aligned with the X₀ axis. All co-ordinate frames and rotations of joints are in accordance with the right hand rule. Thus counter-clockwise rotations are positive and clockwise rotations negative

Geometric solution for inverse kinematics

The basis for this example, originally drawn in CAD, is the pose shown above in Fig.2. The known co-ordinates of the origin of frame 5 relative to the origin of the base frame* are (in unspecified length units):

• x₅ = 695


• y₅ = 781


• z₅ = - 401

*In previous tutorials these real world co-ordinates would be designated by a twin suffix x₅₀, y₅₀, z₅₀. In this application we omit the 0 suffix for simplicity.

The tool tip is 110 units vertically below the origin of frame 5. In this configuration joint 4 and joint 6 do not rotate. Consequently joint angle θ₅ is determined by joint angles θ₂ and θ₃. In effect, this configuration has three degrees of freedom.

Figs.3(a) and 3(b) below illustrate the geometry of the robot in the specified configuration. Note that the "elbow" at joint 3 in Fig.3(b) is in the up position. The inverse kinematic problem is to find joint angles θ₁, θ₂, θ₃ and θ₅.

Directions of rotation of joints 2, 3 and 5 illustrated in Fig.3(b) are determined from the horizontal datum position.

To find θ₁

From Fig.3(a):   θ₁ = tan^-1(z₅ / x₅) = tan^-1(401/ 695):   gives   θ₁ = 29.98°.   Rotation is +ve (counter-clockwise) with the Y₀ axis coming out the page on Fig.3(a).

To find θ₂

From Fig.3(a):   θ₂ = (φ + λ₃)

Also from Fig.3(a):   r = z₅ / sin θ₁ = (401) / sin (29.98°) = 802

From the robot model:   a₁ = 160

From Fig.3(b):   tan^-1φ = tan^-1(y₅ - a₁) / r) = tan^-1(621 / 802) gives:   φ = 37.74°

Also:   (L₁)² = r² + (y₅ - a₁)² = (802)² + (781 - 160)²

gives L₁ = √[(802)² + (621)² ] = 1014

From the robot model:   L₂ = 400,   L₃ = 630

Using the law of cosines for triangle with sides L₁, L₂, L₃ and internal angles λ₁, λ₂, λ₃  gives:

(L₃)² = (L₁)² + (L₂)² - 2L₁.L₂.cos λ₃

Thus   θ₂ = (φ + λ₃) = (37.74° + 12.47°)

gives   θ₂ = 50.21°   Rotation is +ve (counter-clockwise) with the Z₀ axis coming out the page on Fig.3(b).

To find θ₃

Using the law of cosines for triangle with sides L₁, L₂, L₃ and internal angles λ₁, λ₂, λ₃   gives:

(L₁)² = (L₂)² + (L₃)² - 2L₂.L₃.cos λ₁

giving:   θ₃ = (180° - λ₁ ) = (180° - 159.66°)

Thus   θ₃ = - 20.34°   Rotation is -ve (clockwise) with the Z₀ axis coming out the page on Fig.3(b)

To find θ₅

From Fig.3(b):   θ₅ = (90° + β)

Also from Fig.3(b)   β = (θ₂ - θ₃) = (50.21° - 20.34°) = 29.87°

Giving   θ₅ = (90° + 29.87°)

Thus   θ₅ = 119.87°   Rotation is -ve (clockwise) with the Z₀ axis coming out the page on Fig.3(b).

Alternative solution for θ₂, θ₃, and θ₅

The above solution for joint angles θ₂, θ₃, and θ₅ is not unique. Fig.3(c) below illustrates another pose providing the same tool tip co-ordinates where elbow joint 3 points down.

In the elbow down configuration angles are computed as follows (elbow up configuration in brackets).

θ₂ = 25.27° +ve   (50.21° +ve)

θ₃ = 20.34° +ve   (20.34° -ve)

θ₅ = 119.87° -ve   (135.61° -ve)

Consideration of offsets

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

When revolute joint 1 rotates towards the mid-way angular target point from opposite directions with clockwise or counter-clockwise rotations the joint rotations θ₁ are not the same. As a consequence inverse kinematics must define the direction of rotation, normally stated as right hand (counter-clockwise) or left hand (clockwise). In this example rotations to a mid-way target point from starting points 90° apart are θ_1(RH) = 53° and θ_1(LH) = 37°

Analytic solution for inverse kinematics

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

1. Find a geometric solution for joint angles θ₁, θ₂ and θ₃. For this example we apply the results from the geometric method outlined above for θ₁, θ₂ and θ₃. We then derive the rotational transformation matrix R⁰₃ (see previous tutorial) using these values.


2. The second part requires joints 4, 5 and 6 to be in a spherical wrist configuration (see above). Because of this, rotation matrix R³₆ alone defines transformations from co-ordinate frame 6 to co-ordinate frame 3. The elements of R³₆ are expressed in terms of sine and cosine functions of joint angles θ₄, θ₅ and θ₆. Note that the pose of joints 4, 5, and 6 in this example are not the same as applied in the previous example,specifically joints 4 and 6 are rotated.
   
   We also specify the orientation of the co-ordinate frame for joint 6 in the specified pose which determines the rotational transformation matrix R⁰₆ from frame 6 to the base frame.
   
   The method is based on the following clever piece of linear algebra:
   
   R⁰₆ = R⁰₃ • R³₆
   
   multiply both sides by (R⁰₃)^-1 (the inverse matrix of R⁰₃)
   
   gives   (R⁰₃)^-1 • R⁰₆ = (R⁰₃)^-1 • R⁰₃ • R³₆
   
   and as (R⁰₃)^-1 • R⁰₃ = I (the identity matrix)   it follows:
   
   R³₆ = (R⁰₃)^-1 • R⁰₆.
   
   This relationship is the basis of the procedure outlined below to resolve joint angles θ₄, θ₅ and θ₆ where we compute (R⁰₃)^-1 • R⁰₆ and use the equivalence with known matrix R³₆ to solve for θ₄, θ₅ and θ₆.

Worked example of analytic method

We will work through this procedure using joint angles θ₁, θ₂ and θ₃ from the previous worked example (elbow up configuration). For convenience joint angles are rounded to:

• θ₁ = 30.0° (+ve)


• θ₂ = 50.0° (+ve)


• θ₃ = 20.0° (-ve)

Note that joint angles θ₄, θ₅ and θ₆ are different from the joint angles applied in the previous example.

Step 1 - derive rotation matrix R⁰₃

R⁰₁ is rotation of +30° about the Y axis;   R¹₂ is rotation of +50° about the Z axis;   R²₃ is rotation of -20° about the Z axis;

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

Step 2 - derive rotation matrix R⁰₆

Fig.5 below shows the horizontal datum position for the robot model as shown in Fig.1(b). Co-ordinates X₀, Y₀, Z₀ represent the real world co-ordinate frame of the robot base.

Fig.6 below shows the specified tool tip pose with the co-ordinate system for frame 6. For simplicity the transformation of frame 6 is a single (clockwise) rotation of -90° about the Y₀ axis. Fig.7 below provides a more detailed view of the tool tip at the extremity of joint 6 in datum and specified pose conditions.

In a previous tutorial we found the rotational transformation matrix R_y(θ) about the Y axis to be:

which for θ = -90° gives:

Step 3 - compute rotation matrix R³₆

Step 4 - derive rotation matrix R³₆ from first principles

Firstly, note that:

• R³₄ is rotation of θ₄ about the X axis


• R⁴₅ is rotation of θ₅ about the Z axis


• R⁵₆ is rotation of θ₆ about the X axis

Using rotational transformation matrices   R_x(θ),   R_y(θ)   R_z(θ)

This operation is not entirely straightforward since multiple solutions are possible for each element.

First consider the element r₁₁ (row 1, column 1).

Gives:   cosθ₅ = - 0.433

There are two possible solutions:   θ₅ = (i) cos^-1(-0.433) = 115.7°   or (ii) cos^-1(-0.433) = -115.7°

Choose   θ₅ = -115.7°   giving sinθ₅ = - 0.901

Next consider element r_{12 (row 1, column 2)}

Gives:   - (sinθ₅.cosθ₆) = 0.5

Giving   cosθ₆ = - 0.5 / (- 0.901) = 0.555

Thus   θ₆ = (i) cos^-1(0.555) = 56.3°   or (ii) cos^-1(0.555) = - 56.3°

Choose   θ₆ = 56.3°   giving sinθ₆ = 0.832

As a check compute element r₁₃ (row1, column 3)

sinθ₅.sinθ₆ = (- 0.901).(0.832) = - 0.75 (correct)

θ₆ = - 56.3° would be incorrect as sin( - 56.3°) is -ve, making r₁₃ = +0.75

Next consider element r₂₁ (row 2, column 1) = cosθ₄.sinθ₅

Gives:   cosθ₄ = 0.25 / ( - 0.901) = - 0.277

Thus θ₄ = cos^-1(- 0.277) = (i) 106.1° or (ii) cos^-1(- 0.277) = - 106.1°

Choose   θ₄ = - 106.1°   giving sinθ₄ = - 0.961

As a check compute element r₃₁ (row 3, column 1)

sinθ₄.sinθ₅ = (- 0.961) (- 0.901) = 0.866 (correct)

θ₄ = 106.1° would be incorrect as sin(106.1°) is +ve, making r₃₁ = -0.866

Thus the complete solution for rotation angles to effect the specified robot pose is:

Calculations of matrix elements r₂₂, r₂₃, r₃₂ and r₃₃ provide additional checks to validate this solution.

It can be shown that a valid solution also occurs with θ₅ = + 115.7°

the resulting joint angles are:

This completes the short series of tutorials on kinematics of robot manipulators.

Reurn to:  Robotics content

return to home page

I welcome feedback at:

alistair@alistairstutorials.co.uk