48

CHAPTER 4

FORWARD KINEMATICS MODEL

4.1 STAGE I: DEVELOPING FORWARD KINEMATICS

MODEL

4.1.1 Introduction

The solution to the forward kinematics problem consists of finding

the value of the end position of TCP. This solution is a function of 5 joint

values, and D-H parameters. There are several methods to resolve this

problem. In this research, it is done using the homogeneous transformation

matrices method and D-H’s systematic representation of reference systems.

Although the final position can be found geometrically, the method proposed

in this work offers a response which could relate the position of the end of

each link in the kinematics chain compared to the previous or the global

reference system in order to define the position of each articulation in the

robot.

4.1.2 Assigning the Coordinate Frames in FKM

4.1.2.1 Frame Assignment and Structure

The joints of the mechanical arm of SCORBOT ER V plus are

identified in Figure 4.1. D-H parameter according to this model is given in

Table 4.1. The kinematics model is shown in Figure 4.2 with frame

assignments according to the D-H notations.

49

Figure 4.1 Identified Robot Arm Joints

Range of each joint is shown in Table 4.2. The LabVIEW Model is

developed from the 12 kinematics equations shown in Table 4.3 to Table 4.6.

d1=3

49

a2=221 a3=221 d5=145a1=16

Figure 4.2 Frame Assignments

50

Table 4.1 D-H Parameter for SCORBOT ER V Plus

Joint i ai di i

1 /2 16 349 1

2 0 221 0 2

3 0 221 0 3

4 /2 0 0 /2+ 4

5 0 0 145 5

Where i =Joint Number, i=Twist angle, ai=link length, di=link offset,

i=joint angle

Table 4.2 Range of Joints for SCORBOT ER V Plus

Joint Joint Name Range

1 Base ±155

2 Shoulder -35to +130

3 Elbow ±130

4 Wrist Pitch ±130

5 Wrist Roll ±570

51

In LabVIEW FKM the D-H parameters are set as constants as

shown in Figure 4.3 and Figure 4.4.

Figure 4.3 D-H parameters shown in block diagram

52

Figure 4.4 D-H parameters shown in block diagram (Enlarged)

i is included in the kinematics equations and i is given as inputs

for computation of the forward kinematics analysis as shown in Figure 4.5

and Figure 4.6.

53

Figure 4.5 Joint angles shown in FKM block diagram

54

Figure 4.6 Joint angles shown in FKM block diagram (Enlarged)

4.1.2.2 Transformation matrix

After establishing D-H coordinate system for each link, a

homogeneous transformation matrix can easily be developed considering

frame {i-1} and frame {i} transformation consisting of four basic

transformations. The overall complex homogeneous matrix of

transformation can be formed by consecutive applications of simple

transformations. This transformation consists of four basic transformations.

T1: A rotation about zi-1 axis by an angle i

T2: Translation along zi-1 axis by distance di

T3: Translation by distance ai along xi axis and

T4: Rotation by angle i about xi axis.

55

From the transformation matrix, the position and orientation of the

end effector is extracted with respect to base. It is given as shown below.

oT1 =

c1 0 -s1 a1c1s1 0 c1 a1s1

0 -1 0 d10 0 0 1

1T2 =c2 -s2 0 a2c2s2 c2 0 a2s20 0 1 00 0 0 1

2T3 =c3 -s3 0 a3c3s3 c3 0 a3s30 0 1 00 0 0 1

3T4 =-s4 0 c4 0c4 0 s4 00 1 1 00 0 0 1

4T5 =c5 -s5 0 d5s5 c5 0 00 0 1 00 0 0 1

The overall complex homogeneous matrix of transformation is as given

below.

Te = oT5 = oT11T2

2T33T4

4T5

0T5 =

-s1s5- c1s234c5 -s1c5+c1s234s5 c1c234 c1(a1+a2c2+a3c23+c234d5)c1s5-s1s234c5 c1c5+s1s234s5 s1c234 s1(a1+a2c2+a3c23+c234d5)

-c234c5 c234s5 -s234 d1-a2s2-a3s23-s234d50 0 0 1

56

Where ci = cos i and si = sin i ,ci jk= cos ( i j k )and sijk = sin i. The

Transformation matrix in LabVIEW model block diagram is shown in Figure

4.7 and front panel is shown in Figure 4.8.

Figure 4.7 Transformation Matrix in LabVIEW model block diagram

Figure 4.8 Transformation Matrix in LabVIEW Model Front Panel

57

4.1.2.3 Kinematics Equations

The kinematics model in the form of overall transformation

matrix is expressed in 12 kinematics equations as shown in the Table 4.3

to Table 4.6. In the FKM all these equations are expressed inside the

formulae node as shown in Figure 4.9.

Figure 4.9 Kinematics Equations Inside Formula Node in Block Diagram

58

Table 4.3 Kinematics equations of normal vector

Equation Number RHS LHS Component

1 nx -s1s5- c1s234 c5 X

2 ny c1s5-s1s234 c5 Y

3 nz -c234c5 Z

The kinematics equations of normal vector shown in Table 4.3

are expressed inside the formulae node as shown in Figure 4.10.

Figure 4.10 Normal Vector in the Formula Node

Normal vector – forms a right handed set of vectors.

59

Table 4.4 Kinematics Equations of Orientation Vector

Equation Number RHS LHS Component

4 ox - s1 c5+c1s234 s5 X

5 oy c1 c5+s1s234 s5 Y

6 oz c234 s5 Z

The kinematics equations of orientation vector shown in Table

4.4 are expressed inside the formulae node as shown in Figure 4.11.

Figure 4.11 Orientation Vector in the Formula Node

Orientation vector – the orientation of the hand from finger tip to finger

tip.

60

Table 4.5 Kinematics Equations of Approach Vector

Equation Number RHS LHS Component

7 ax c1c234 X

8 ay s1c234 Y

9 az -s234 Z

The kinematics equations of approach vector shown in Table

4.5 are expressed inside the formulae node as shown in Figure 4.12.

Figure 4.12 Approach Vector in the Formula Node

61

Approach vector – the direction a hand would approach the

object. Where (n, o, a) is an ortho normal triplet representing the

orientation and p is the vector reflecting the position.

Table 4.6 Kinematics Equations of Position Vector

Equation Number RHS LHS Component

10 px c1 (a1 + a2c2+ a3c23 +c234d5) X

11 py s1 (a1 + a2c2+ a3c23 +c234d5) Y

12 pz d1 - a2s2- a3s23-s234d5 Z

The kinematics equations of approach vector shown in Table

4.6 are expressed inside the formulae node as shown in Figure 4.13.

Figure 4.13 Position Vector in the Formula Node

62

Position vector – the 3D position. The general position vector of

SCORBOT ER V plus is given by,

pxpypz

=c1(a1+a2c2+a3c23+c234d5)s1(a1+a2c2+a3c23+c234d5)

d1-a2s2-a3s23-s234d5

4.2 VERIFICATION OF THE MODEL

At home position 1=0, 2= -120.27, 3=95.03, 4=88.81 and 5 =0.

The output values are compared and validated with AutoCAD 2007 3D

model. The input values in the front panel are shown in Figure 4.14.

Figure 4.14 Joint angles at home position as inputs in the FKM

Transformation matrix for link 1 is

oT1 =

1 0 0 160 0 1 0

1 0 3490 0 0 1

63

The output values (Transformation matrix) for first link are shown

in the front panel (Figure 4.15).

Figure 4.15 Transformation matrix for first link in FKM front panel

Transformation matrix for link 2 is

1T2 =

0.504805 0.863233 0 111.5620.863233 0.504805 0 190.775

0 0 1 00 0 0 1

The output values (Transformation matrix) for second link are

shown in the front panel (Figure 4.16).

Figure 4.16 Transformation matrix for second link in FKM front panel

64

Transformation matrix for link 3 is

2T3 =

0.0883423 0.99609 19.52370.99609 0.0883423 0 220.136

0 0 1 00 0 0 1

The output values (Transformation matrix) for third link are shown

in the front panel (Figure 4.17).

Figure 4.17 Transformation Matrix for Third Link in FKM Front Panel

Transformation matrix for link 4 is

3T4 =

0.999797 0 0.0201442 00.0201442 0 0.999797 0

0 1 0 00 0 0 1

The output values (Transformation matrix) for fourth link are shown in the front panel (Figure 4.18).

Figure 4.18 Transformation matrix for fourth link in FKM front panel

65

Transformation matrix for link 5 is

4T5 =

1 0 0 1450 1 0 00 0 1 00 0 0 1

The output values (Transformation matrix) for fourth link are

shown in the front panel (Figure 4.19).

Figure 4.19 Transformation Matrix for Fifth Link in FKM Front Panel

The homogeneous matrix which specifies the location of the 5th

coordinate frame with respect to the base coordinate system will be

Te = oT5 = oT11T2

2T33T4

4T5

0T5 =

0.895678 0 0.444704 168.8050 1 0 0

0.444704 0 0.895678 504.1740 0 0 1

The overall transformation matrix is shown in the front panel as

shown in Figure 4.20.

66

Figure 4.20 Overall Transformation Matrix in FKM Front Panel

4.3 COMPUTATION OF THE MODEL

The front panel and block diagram of FKM are shown in Figures 4.21

and 4.22.

Figure 4.21 Front Panel of FKM

67

Figure 4.22 Block Diagram of FKM

For data generation, the FKM (considering all the combinations of

five angles for the representation of robotic arm) is run to store the output

values. But to calculate the forward kinematics, only four angles ( 1, 2, 3

and 4) have been considered as 5 is independent of other angles.

4.4 CREATION OF DATABASE

Now, for every combination of 1, 2, 3 and 4 values, the X , Y

and Z coordinates are deduced using the formulae for forward kinematics.

The end effector starts in the configuration 1=0, 2= -120.27, 3=95.03,

4=88.81 and 5 =0. Its movement is limited within the range maximum

68

1=155, 2= 130, 3=130, 4=130 and 5 =570 and also minimum 1=-155, 2=

-35, 3=-130, 4=-130 and 5 =-570. The values both from RoboCell and

LabVIEW are given in Table 4.7 and Table 4.8.

Figure 4.23 RoboCell SCORBOT ER V Plus at Home Position

Figure 4.24 Line Diagram of SCORBOT ER V Plus at Home Position

69

The results obtained from FKM are compared and found correct

with RoboCell (Figure 4.23) and AutoCAD model (Figure 4.24).

Table 4.7 Home position and other set of joint parameters in RoboCell

S.NoJoint coordinates RoboCell data

1 2 3 4 5 X(mm) Y(mm) Z(mm)

1 0 -120.27 95.03 88.81 0 169.03 0 504.33

2 0 -8.93 107.87 -8.93 0 200 0 20

3 0 -8.88 89.59 9.29 0 270.01 0 20.01

4 0 -2.95 65.09 27.87 0 340 0 20

5 45 -9.35 105.05 -5.70 45.02 149.99 149.99 19.99

6 45 -6.17 76.21 19.95 45.02 220.01 220.01 20.01

7 45 14.78 20.48 54.73 45.02 290 290 20.01

8 67.19 -9.63 100.80 -1.17 0 88.91 211.44 20.01

9 21.56 -4.43 69.91 24.53 0 305.13 120.56 20.02

10 38.10 -15.86 98.92 -83.06 0 315 247 190

70

Table 4.8 Home position and other set of joint parameters in FKM

Output

S.NoJoint Coordinates LabVIEW Output

1 2 3 4 5 X(mm) Y(mm) Z(mm)

1 0 -120.27 95.03 88.81 0 169.345 0 503.639

2 0 -8.93 107.87 -8.93 0 199.707 0 20.0278

3 0 -8.88 89.59 9.29 0 269.81 0 20.0069

4 0 -2.95 65.09 27.87 0 339.781 0 19.9489

5 45 -9.35 105.05 -5.70 45.02 149.769 149.864 20.0266

6 45 -6.17 76.21 19.95 45.02 219.835 219.974 20.0005

7 45 14.78 20.48 54.73 45.02 289.85 290.033 19.9736

8 67.19 -9.63 100.80 -1.17 0 88.7315 211.26 20.0338

9 21.56 -4.43 69.91 24.53 0 304.89 120.522 19.9656

10 38.10 -15.86 98.92 -83.06 0 314.83 246.995 190.024

Again all the results obtained from FKM are compared with

RoboCell and AutoCAD models and are found correct.

Using the data obtained from FKM the reachability analysis, path

and workspace analysis are done in Stage II and Stage III respectively and the

methods have been explained in Chapter 5 and Chapter 6.