Cinematica Inversa

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-Hs 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.
a2=221
a3=221
d5=145
d1=349
a1=16
Figure 4.2 Frame Assignments
50
Table 4.1 D-H Parameter for SCORBOT ER V Plus
Joint
ai
di
/2
16
349
221
221
/2
/2+
145
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
Base
155
Shoulder
-35to +130
Elbow
130
Wrist Pitch
130
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)
is included in the kinematics equations and
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
T2:
Translation along zi-1 axis by distance di
T3:
Translation by distance ai along xi axis and
T4:
Rotation by angle
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.
c1
s
o
T1 = 1
0
0
0
0
-1
0
c2
T2 = s2
0
0
-s2
c2
0
0
0 a2 c2
0 a2 s2
1
0
0
1
c3
T3 = s3
0
0
-s3
c3
0
0
0 a3 c3
0 a3 s3
1
0
0
1
-s4
T4 = c4
0
0
0 c4
0 s4
1 1
0 0
c5
4
T5 = s5
0
0
-s5
c5
0
0
-s1
c1
0
0
a1 c1
a1 s1
d1
1
0
0
0
1
0 d5
0 0
1 0
0 1
The overall complex homogeneous matrix of transformation is as given

below.
Te = oT5 = oT1 1T2 2T3 3T4 4T5
-s1 s5 - c1 s234c5 -s1 c5 +c1s234 s5
0
T5 = c1 s5 -s1 s234c5 c1 c5 +s1 s234s5
-c234 c5
c234 s5
0
0
c1 c234
s1c234
-s234
0
c1 (a1 +a2 c2 +a3c23+c234 d5 )

s1 (a1 +a2 c2 +a3c23 +c234 d5 )
d1 -a2 s2 -a3 s23-s234d5
1
56
Where ci = cos
and si = sin
,ci jk= cos (
)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
nx -s 1s5- c 1s234 c5
ny
c 1s5-s 1s234 c5
nz
-c 234c 5
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
ox - s 1 c5+c 1s234 s 5
oy
c 1 c5+s 1s234 s 5
oz
c234 s5
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 c 1c234
ay s1c 234
az
-s 234
The kinematics equations of approach vector shown in Table

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 + a 2c2+ a 3c23 +c234d 5)
11
py s1 (a 1 + a 2c2+ a 3c23 +c 234d 5)
12
pz
d1 - a2s2- a3s 23-s234d 5
The kinematics equations of approach vector shown in Table

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,
px
c1 (a1 +a2 c2 +a3 c23 +c234 d5 )
py = s1 (a1 +a2 c2 +a3 c23 +c234 d5 )
pz
d1 -a2 s2 -a3 s23 -s234 d5
4.2
VERIFICATION OF THE MODEL

At home position
1=0,
2=
-120.27,
3=95.03,
4=88.81
and
=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
T1 =
1
0
0
0 0 16
0 1
0
1 0 349
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
T2 =
0.504805
0.863233
0
0
0.863233
0.504805
0
0
0
0
1
0
111.562
190.775
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
T3 =
0.0883423
0.99609
0
0
0.99609
0.0883423 0
0
1
0
0
19.5237
220.136
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
T4 =
0.999797
0.0201442
0
0
0 0.0201442 0
0 0.999797 0
1
0
0
0
0
1
The output values (Transformation matrix) for fourth link are

Figure 4.18 Transformation matrix for fourth link in FKM front panel
65
T5 =
1
0
0
0
0
1
0
0
0 145
0
0
1
0
0
1
The output values (Transformation matrix) for fourth link are

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 = oT1 1T2 2T3 3T4 4T5
T5 =
0.895678
0
0.444704
0
0
1
0
0
0.444704 168.805
0
0
0.895678 504.174
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,
and
4)
have been considered as
4.4
2,
is independent of other angles.
CREATION OF DATABASE
Now, for every combination of
1,
2,
and
values, the X , Y
and Z coordinates are deduced using the formulae for forward kinematics.
The end effector starts in the configuration
4=88.81
and
1=0,
2=
-120.27,
3=95.03,
=0. Its movement is limited within the range maximum
68
1=155,
-35,
2=
130,
3=-130,
3=130,
4=-130
4=130
and
and
=570 and also minimum
1=-155,
2=
=-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
Joint coordinates
RoboCell data
S.No
1
X(mm) Y(mm) Z(mm)
-120.27
95.03
88.81
169.03
504.33
-8.93
107.87
-8.93
200
20
-8.88
89.59
9.29
270.01
20.01
-2.95
65.09
27.87
340
20
45
-9.35
105.05
-5.70
45.02 149.99
149.99
19.99
45
-6.17
76.21
19.95 45.02 220.01
220.01
20.01
45
14.78
20.48
54.73 45.02
290
290
20.01
67.19
-9.63
100.80
-1.17
88.91
211.44
20.01
21.56
-4.43
69.91
24.53
305.13
120.56
20.02
10
38.10
-15.86
98.92
-83.06
315
247
190
70
Table 4.8 Home position and other set of joint parameters in FKM
Output
Joint Coordinates
LabVIEW Output
S.No
1
X(mm)
Y(mm)
Z(mm)
-120.27
95.03
88.81
169.345
503.639
-8.93
107.87
-8.93
199.707
20.0278
-8.88
89.59
9.29
269.81
20.0069
-2.95
65.09
27.87
339.781
19.9489
45
-9.35
105.05
-5.70
45.02
149.769 149.864 20.0266
45
-6.17
76.21
19.95
45.02
219.835 219.974 20.0005
45
14.78
20.48
54.73
45.02
289.85
290.033 19.9736
67.19
-9.63
100.80
-1.17
88.7315
211.26
21.56
-4.43
69.91
24.53
304.89
120.522 19.9656
10
38.10
-15.86
98.92
-83.06
314.83
246.995 190.024
20.0338
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.

Cinematica Inversa

Transféré par

Informations du document

Titre original

Copyright

Formats disponibles

Partager ce document

Partager ou intégrer le document

Options de partage

Avez-vous trouvé ce document utile ?

Ce contenu est-il inapproprié ?

Droits d'auteur :

Formats disponibles

Cinematica Inversa

Transféré par

Droits d'auteur :

Formats disponibles

48

Assigning the Coordinate Frames in FKM

Frame Assignment and Structure

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

Figure 4.1 Identified Robot Arm Joints

Figure 4.2 Frame Assignments

Where i =Joint Number,

i=Twist angle, ai=link length, di=link offset,

Figure 4.3 D-H parameters shown in block diagram

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

is included in the kinematics equations and

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

Figure 4.5 Joint angles shown in FKM block diagram

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

homogeneous transformation matrix can easily be developed considering

transformation can be formed by consecutive applications of simple

A rotation about zi-1 axis by an angle

Translation along zi-1 axis by distance di

Translation by distance ai along xi axis and

The overall complex homogeneous matrix of transformation is as given

c1 (a1 +a2 c2 +a3c23+c234 d5 )

,ci jk= cos (

)and sijk = sin

Transformation matrix in LabVIEW model block diagram is shown in Figure

Figure 4.7 Transformation Matrix in LabVIEW model block diagram

Figure 4.8 Transformation Matrix in LabVIEW Model Front Panel

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

Figure 4.9 Kinematics Equations Inside Formula Node in Block Diagram

The kinematics equations of normal vector shown in Table 4.3

Figure 4.10 Normal Vector in the Formula Node

The kinematics equations of orientation vector shown in Table

Figure 4.11 Orientation Vector in the Formula Node

Table 4.5 Kinematics Equations of Approach Vector

The kinematics equations of approach vector shown in Table

Figure 4.12 Approach Vector in the Formula Node

px c1 (a1 + a 2c2+ a 3c23 +c234d 5)

py s1 (a 1 + a 2c2+ a 3c23 +c 234d 5)

d1 - a2s2- a3s 23-s234d 5

The kinematics equations of approach vector shown in Table

Figure 4.13 Position Vector in the Formula Node

VERIFICATION OF THE MODEL

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

The output values (Transformation matrix) for second link are

The output values (Transformation matrix) for fourth link are

The output values (Transformation matrix) for fourth link are

The overall transformation matrix is shown in the front panel as

Figure 4.20 Overall Transformation Matrix in FKM Front Panel

COMPUTATION OF THE MODEL

Figure 4.21 Front Panel of FKM

Figure 4.22 Block Diagram of FKM

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

have been considered as

is independent of other angles.

=0. Its movement is limited within the range maximum

=570 and also minimum

=-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

X(mm) Y(mm) Z(mm)

19.95 45.02 220.01

149.769 149.864 20.0266

219.835 219.974 20.0005