Project 3

1) Thc hin vic tnh ton ng hc cho robot 3 trc bao gm vic chn h trc, thng s ng
hc, links transformation v cc phng trnh ng hc.

FORWARD KINEMATICS: S dng cng thc:

cos1
sin
1
i
Ti 1
0

0
Cho bit:

sin 1 cos1

sin 1 sin i

cos1 cos1

cos1 sin 1

sin 1

cos1

a i cos1
a i sin 1
di

Fig. D-H convention joint-coordinate diagram

Table 1. Kinematics parameters of 3 axis robot
Axis

di (mm)

ai (mm)

d1 = 0

a1 = 0

1 = /2

d2 = 0

a2 = 250

2 = 0

a3 = 150
3

d3 = 0

a3 = 200
a3 = 250

3 = 0

CM rng:
r11
r
T03 21
r31

r12

r13

r22

r23

r32

r33

Px
Py
Pz

Where

r11 C1C 2 C 3 C1S 2 S3

r21 S1C 2 C 3 S1S 2 S3
r31 S 2 C 3 C 2 S3
r12 C1C 2 S3 C1S 2 C 3
r22 S1C 2 S3 S1S 2 C 3
r32 S 2S3 C 2 C 3
r13 S1
r23 C1

r33 0
PX a 3 C1C 2 C 3 - a 3 C1S 2 S3 a 2 C1C 2

[II]

PY a 3S1C 2 C 3 - a 3S1S 2S3 a 2S1C 2

[III]

PZ a 3S 2 C 3 a 3 C 2 S3 a 2 S 2

[IV]

Where: C i cos( i ); Si sin( i ) (i 1..3)

INVERSE KINEMATICS
SOLVING FOR 1
To solve for 1 , consider the following equation:

(T01 ) 1 T03 (T01 ) 1 T01T12 T23

(T01 ) 1 T03 T12 T23

C1
0

S1

S1
0
C1
0

0 0 r11
1 0 r21
0 0 r31

0 1 0

r12

r13

r22

r23

r32

r33

C1r11 S1 r21 C1r12 S1r22

r31
r32

S1 r11 C1r21 S1r12 C1r22

0
0

C 2 C 3 S 2 S 3
S C C S
2 3
2 3

Px C 2
Py S 2

Pz 0

1 0

S2

0 a 2 C 2 C 3
0 a 2S 2 S3
1
0 0

0
1 0

C2
0
0

C1Px S1Py

r33
Pz

S1r13 C1r23 S1Px C1Py

0
1

C 2 S 3 S 2 C 3 0 a 3 C 2 C 3 a 3S 2 S 3 a 2 C 2
S 2S3 C 2 C 3 0 a 3S 2 C 3 a 3 C 2S3 a 2 C 2

0
1
0

0
0
1

S3
C3
0
0

0 a 3C3
0 a 3S3
1
0

0
1

C1r13 S1r23

(A)

CM rng:

1 A tan 2( Py , Px ) A tan 2(0,1)

[V]

SOLVING FOR 2

(T12 ) 1 (T01 ) 1 T03 (T12 ) 1 (T01 ) 1 T01T12 T23 T23

(T12 ) 1 (T01 ) 1 T03 T23
C2
S
2
0

S2
C2
0
0

0 a 2 C1
0
0 0
1
0 S1

0
1 0

S1
0
C1
0

0 0 r11
1 0 r21
0 0 r31

0 1 0

r12

r13

r22

r23

r32

r33

p x C 3
p y S3

pz 0

1 0

S3
C3
0
0

0 a 3C3
0 a 3S3
1
0

0
1

 r '11
r '
21
r '31

r '12

r '13

r ' 22

r ' 23

r '32

r '33

P ' x C 3
P' y S3

P' z 0

1 0

S3
C3
0
0

0 a 3C3
0 a 3S3
1
0

0
1

(B)

r '11 C 2 C1 r11 C 2 S1 r21 S 2 r31

r ' 21 S 2 C1 r11 S 2 S1 r21 C 2 r31
r '31 S1 r11 C1 r21
r '12 C 2 C1 r12 C 2 S1 r22 S 2 r32
r ' 22 S 2 C1 r12 S 2 S1 r22 C 2 r32
r '32 S1 r12 C1 r22
r '13 C 2 C1 r13 C 2 S1 r23 S 2 r33
r ' 23 S 2 C1 r13 S 2 S1 r23 C 2 r33

r '33 S1 r13 C1 r23

P' x C 2 C1 Px C 2 S1 Py S 2 Pz a 2
P' y S 2 C1 Px S 2 S1 Py C 2 Pz

P' z S1 Px C1 Py

CM rng:
2 A tan 2(K, R' 2 K 2 ) A tan 2(B, A)

[VI]

SOLVING FOR 3
Equating the (1, 4) and (2,4) elements from both sides of the equation (B)

CM rng:

 3 A tan 2(S3 , C 3 )

[VII]

2) S dng hoc 3DCAD hoc Matlab m phng ng hc cho robot cu 1. Cc thng s SV t

cho ly. Thc hin kim tra s ng n ca vic tnh ton cu 1 v chng trnh m phng
ca cu 2.

Check the simulation and kinematics equations:

V d s dng CATIA:

Fig. Kinematics check for a3=150mm

Fig. Kinematics check for a3=200mm

Fig. Kinematics check for a3=250mm