Post on 19-Aug-2020
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.