Case Study

Computing the Forward Kinematics of 6DOF Robotic Arm

The manipulator of an industrial robot consists of a series of joints

and links. Robot anatomy deals with the study of different joints and links
and other aspects of the manipulator's physical construction. A robotic
joint provides relative motion between two links of the robot. Each joint
provides a certain degree-of freedom (DOF) of motion.

In many cases, only one degree-of-freedom is associated with each

joint. Therefore, the robot's complexity can be classified according to the
total number of degrees-of-freedom they possess. Each joint connected to
two links (input and output link). Joint provides controlled relative
movement between these two links. A robotic link is the rigid component
of the robot manipulator. Starting from the fixed base, a joint-link
numbering scheme is depicted in Figure 1. The fixed base and its
connection to the first joint is named as link-0. The first joint in this
sequence is joint-1. Link-0 is the input link for joint-1, while the output
link from joint-1 is link-1, which leads to joint- 2. Thus, link 1 is also the
output link for joint-1 and the input link for joint-2. This joint-link-
numbering scheme continuously used for all other joints and links in the
similar way.
 Fig. 1 The designed 6DOF manipulator coordinates of all joints.

1. Forward Kinematics

The study of kinematic problem of a robot can be carried out by

commonly used method is based on Denavit-Hartenberg parameters
(DH). This method is a systematic in nature and more suitable for
modeling serial manipulators. DH method has been used to develop the
kinematic model of the robot in this work because of its versatility and
acceptability for modeling of any number of joints and links of a serial
manipulator regardless of complexity.

Procedure to follow using Denavit-Hartenberg convention

The following is a summary of the procedure to follow when faced

with a new manipulator mechanism, see Fig. 2:

1- Identify the joints axes and assign the Z axis pointing along the i-th
joint axis.
 2- Identify the common perpendiculars of the joints axes. At the point
where the common perpendicular meets the i-th axis, assign the
link-frame origin.
3- Assign the Xi axis pointing along the common perpendicular.
4- Assign the Yi axis to complete a right-hand coordinate system.
5- Assign frame {0} to match with frame {1} when the first joint
variable is zero.
6- For the last frame {n}, choose an origin location and X direction so
that as many as possible linkage parameters become zero. The last
method is called Right Handed Coordinate System (RHCS).

Fig. 2 Link frames for any robotic arm.

If the link frames have been attached to the links according to the
above convention, the following definitions of the link parameters are
valid:

αi-1 = the angle from Zi-1 to Zi measured about Xi-1 … (link twist);

ai-1 = the distance from Zi-1 to Zi measured along Xi-1 .. (link length);

di = the distance from Xi-1 to Xi measured along Zi … (link offset);

θi = the angle from Xi-1 to Xi measured about Zi … (joint angle).

Note that, we usually choose ai-1 >= 0, because it corresponds to a

distance; but αi-1, di, and θi are signed quantities

(3.1)
Also, Equation 1 shows the general transformation matrix is:

cosθi -sinθi 0 ai-1

i-1 sinθi cosαi-1, cosθi cosαi-1, sinαi-1 sinαi-1 di
Ti = [ ] (1)
sinθi sinαi-1, cosθi sinαi-1, cosαi-1 cosαi-1 di
0 0 0 1

This transformation will be a function of all joint variables. From

the values of the link parameters (DH-table), the individual link
transformation matrices i-1Ti can be computed, see section 3.4.

Applying DH parameters to the proposed robotic arm shown in

Figure 1 and tabulated in Table 1 and Table 2:

Table 1 The dimensions of the designed model

Symbol a1 a2 a3 a4 d4
Length(mm) 85 44 95 17 160

Table 2 The D-H table for the designed robot

i i-1 ai-1 di i
1 0 0 0 1
2 −90° a2 0 −90°+2
3 0 a3 0 3

4 −90° a4 d4 4

5 90° 0 0 90°+5
6 −90° 0 0 6
Rewrite equation (1); the general transformation matrix is:
cosθi -sinθi 0 ai-1
i-1 sinθi cosαi-1, cosθi cosαi-1, sinαi-1 sinαi-1 di
Ti = [ ]
sinθi sinαi-1, cosθi sinαi-1, cosαi-1 cosαi-1 di
0 0 0 1
We can formulate the transformation matrix between each two successive
frames as follow:
c1 -s1 0 0 s2 c2 0 a2 c3 -s3 0 a3
s c1 0 0 1 0 0 1 0 2 s c3 0 0
0
T1= [ 1 ] ; T2 = [ ] ; T3 = [ 3 ]
0 0 1 0 c2 -s2 0 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1
c4 -s4 0 a4 -s5 -c5 0 0 c6 -s6 0 0
3 0 0 1 d4 4 0 0 -1 0 5 0 0 1 0
T4= [ ] ; T5 = [ ] ; T6 = [ ]
-s4 -c4 0 0 c5 -s5 0 0 -s6 -c6 0 0
0 0 0 1 0 0 0 1 0 0 0 1

We now form 0T6 by matrix multiplication of the individual link

matrices. We start by multiplying 4T5 and 5T6, the result is 4T6 which is
multiplied by 3T4 and so on until we get 0T6 [48]:

B
TW= 0T6 = 0T1.1T2.2T3.3T4.4T5.5T6 (2)

Where
nx ox ax px
B
ny oy ay py
TW =0T6= [ ] (3)
nz oz az Pz
0 0 0 1

Where Px, Py and PZ are the global coordinates indicating the spatial
position of the end-effector. Using MATLAB programming to multiply
the individual matrices.

Then, the results are;

nx = c6 *(c5 *c1 *c23 -s5 *(s1 *s4 +c4 *c1 *s23 ))+s6 *(c4 *s1 -s4 *c1 *s23 )
ny = c6 *(c5 *s1 *c23 +s5 *(c1 *s4 -c4 *s1 *s23 ))-s6 *(c1 *c4 +s4 *s1 *s23 )
nz = -c6 *(c5 *s23 +c4 *s5 *c23 )-s4 *s6 *c23
ox = c6 *(c4 *s1 -s4 *c1 *s23 )-s6 *(c5 *c1 *c23 -s5 *(s1 *s4 +c4 *c1 *s23 ))
oy = -s6 *(c5 *s1 *c23 +s5 *(c1 *s4 -c4 *s1 *s23 ))-c6 *(c1 *c4 +s4 *s1 *s23 )
oz = s6 *(c5 *s23 +c4 *s5 *c23 )-c6 *s4 *c23
ax = -s5 *c1 *c23 -c5 *(s1 *s4 +c4 *c1 *s23 )
ay = c5 *(c1 *s4 -c4 *s1 *s23 )-s5 *s1 *c23
az = s5 *s23 -c4 *c5 *c23
Px = a2 *c1 +a4 *c1 *s23 +d4 *c1 *c23 +a3 *c1 *s2
Py = a2 *s1 +a4 *s1 *s23 +d4 *s1 *c23 +a3 *s1 *s2
Pz = a3 *c2 +a4 *c23 -d4 *s23

Where:
cn : cos(θn) , sn : sin(θn)

The last equations can be noted as equation (4) to simplify using it.
These equations give the forward kinematics of the designed arm robot.
Knowing the robot variables (θ1, θ2, θ3, θ4, θ5, θ6) then 0T6 will be
identified, and the position and orientation of the robot wrist relative to
the base frame is known.

Example:
Compute the total transformation matrix of 6DOF robotic arm which
have: θ1 = 45, θ2 = 30, θ3 = 60, θ4 = 20, θ5 = 30, θ6 = 70

Solution
The transformation matrix can be found by substitution in equation
(4) so:
 0.2421 0.5704 -0.7849 0.0767
0 -0.9239 -0.1114 -0.3660 0.0767
T6 = [ ]
-0.2962 0.8138 0.5000 -0.0777
0 0 0 1

2. Inverse Kinematics (IK)

IK problem computes the joint angles required to achieve the given

position and orientation. In contrast to the forward kinematics, IK does
not have a unique solution. The solutions which ensure collision-free
operation and minimum joint motion are considered more optimum.

I will upload its calculation in another file