Chng 4 - Robot cng nghip

TS Phan Tn Tng

Chng 4 ng hc v tr Robot
1. Bi ton ng hc (v tr) thun (forward kinematics)
Tay my l 1 chui ng h bao gm cc khu v khp
Cc thng s khng thay i gi tr khi tay my hot ng gi l THAM
S ( Vd: chiu di cc khu )
Cc thng s thay i gi tr khi tay my hot ng gi l BIN KHP
(Vd: gc hp bi 2 khu ti 1 khp)
Cc khp thng dng trong tay my l KHP TRT v KHP QUAY
(l khp loi 5)
Bi ton thun m t v tr v hng ca im End Effector di dng
hm s ca cc bin khp

Chng 4 - Robot cng nghip

TS Phan Tn Tng

Hm s m t v tr ca im End Effector
0

p = 0Tn (q ).n p

Vi

0u
T (q) =
0

v
0

w
0

Vd1: C cu tay my nh hnh bn

C1 2
S
0
T (q ) = 1 2
0
0

S1 2
C1 2
0
0

0 a1C1
0 a1S1
1
0

xE
C1 2
y
S
0
p = E = 0T2 (q ).2 p = 1 2
zE
0
1
0

+ a2C1 2
+ a2 S1 2

S1 2
C1 2
0
0

0 a1C1 + a2C1 2 0 a1C1

0 a1S1 + a2 S1 2 0 a1S1
. =
1
0
0
1
0
1

+ a2C1 2
+ a2 S1 2

Chng 4 - Robot cng nghip

1.1 Qui tc Denavit Hartenberg

TS Phan Tn Tng

H to gn ln cc khu nh sau:

Trc Zi t dc theo trc khp i+1

Trc Xi t dc theo phng php tuyn chung gia Zi-1 v Zi, hng
t khp i n khp i+1

Trc Yi vung gc vi Xi v Yi theo qui tc bn tay phi

Gc to Oi l giao ca trc Zi v php tuyn chung ca trc Zi-1 v Zi

Gc to Oi l giao ca trc Zi-1 v php tuyn chung ca trc Zi-1 v

Zi

Cc thng s Denavit Hartenberg

Khong cch gia 2 khp lin tip theo phng Xi l ai (tham s)

Khong cch gia 2 khp lin tip theo phng Zi-1 l di (tham s hoc
bin khp)

Gc quay quanh trc Xi gia trc Zi-1 v trc Zi l I (tham s)

Gc quay quanh trc Zi-1 gia trc Xi-1 v trc Xi l I (tham s hoc 3
bin khp)

Chng 4 - Robot cng nghip

TS Phan Tn Tng

nh ngha h to v cc thng s Denavit Hartenberg

Chng 4 - Robot cng nghip

TS Phan Tn Tng

Xc nh ma trn chuyn i t h to th i v h to th i-1

Ma trn chuyn i t h to th i v h to trung gian int. Xoay
quanh trc Xint gc I sau tnh tin theo trc Xint on ai .

1 0
0 C
i
int
Ai =
0 Si
0 0

0
Si
Ci
0

ai
0

0
1

Ma trn chuyn i t h trung gian int v h i-1. Xoay quanh trc Zi gc

i sau tnh tin theo trc Zint on di .

i 1

Aint

Ci
S
= i
0

Si
Ci
0
0

0
0 0

1 di
0 1
0

Chng 4 - Robot cng nghip

TS Phan Tn Tng

Vy ma trn chuyn i t h i v h i-1 l

Ci
S
i 1
Ai =i 1Aint int Ai = i
0

Si Ci
Ci Ci
Si

Si Si
Ci Si
Ci

ai Ci
ai Si

di
1

Tm li ti khu th i ta c ma trn chuyn i t h th i v h i-1 nh

trn vi cc thng s Denavit Hartenberg c xc nh trong bng
thng s DH nh sau

Chng 4 - Robot cng nghip

TS Phan Tn Tng

Phng trnh ng hc thun (v tr) ca tay my l

i 1

p = Tn p = A1 A 2 ...
0

A i ...

n 1

An

Vd2: Xt c cu tay my nh hnh bn

Bng thng s DH ca tay my

Ma trn chuyn i t h 2 v h 1 v t h 1 v h 0

C 2
S
1
A2 = 2
0
0

S 2
C 2
0
0

0 a2C 2
0 a2 S 2

1
0
0
1

C1
S
0
A1 = 1
0
0

S1
C1
0
0

0 a1C1
0 a1S1

1
0
0
1

Chng 4 - Robot cng nghip

TS Phan Tn Tng

Vy ma trn chuyn i t h to 2 v h to 0 l

(
(

)
)

C1 C 2 S1 S 2 C1 S 2 + S1 C 2 0 a2 C1 C 2 S1 S 2 + a1C1

S C + C S
0
C
C
S
S
a
S
C
C
S
a
S

+
+
1 2
1 2
1 2
2 1 2
1 2
1 1
0
A2 = 0A11 A2 = 1 2
0
0
1
0

0
0
0
1

C1 2 S1 2 0 a2C1 2 + a1C1
S

+
0
C
a
S
a
S
1 2
2 1 2
1 1
0
T2 = 0A2 = 1 2
0
1
0
0

0
0
1

Phng trnh ng hc (v tr) ca tay my

C1 2
xE
S
y
E
0
0
2
p = = T2 . p = 1 2
0
zE
0
1

S1 2
C1 2
0
0

0 a1C1 + a2C1 2 0 a1C1

0 a1S1 + a2 S1 2 0 a1S1
. =
1
0
0
1
0
1

+ a2C1 2
+ a2 S1 2

Chng 4 - Robot cng nghip

TS Phan Tn Tng

Cc bc khi xc nh ma trn chuyn i theo phng php Denavit

Hartenberg

Qui c khu th i nm gia khp th i v i+1

Bc 1: xc nh cc ng tm trc cc khp

Chng 4 - Robot cng nghip

TS Phan Tn Tng

Bc 2: xc nh kch thc ca php tuyn chung gia ng tm trc

khp th i v i+1 v gi l ai.
10

Chng 4 - Robot cng nghip

TS Phan Tn Tng

Nu 2 trc khp th i v i +1 song song th c th chn ai bt k. Nhng

tt nht nn chn ai giao vi ai-1
11

Chng 4 - Robot cng nghip

TS Phan Tn Tng

Nu ng tm 2 trc khp th i v i+1 ct nhau th on ai tr thnh 1

im v c php tuyn chung vung gc vi trc khp i v i +1
12

Chng 4 - Robot cng nghip

TS Phan Tn Tng

Bc 3: xc nh gc cu h to Oi l giao ca ng ai v trc khp

th i +1. Gc ca h to trung gian Hi l giao ca ng ai v trc
13
khp i.

Chng 4 - Robot cng nghip

TS Phan Tn Tng

Nu trc khp i v i+1 ct nhau th Hi Oi

14

Chng 4 - Robot cng nghip

TS Phan Tn Tng

Vi khp 1 (khp ni gi c nh) th chn O0 H1

15

Chng 4 - Robot cng nghip

TS Phan Tn Tng

Bc 4: Xc nh h to u tin (h 0) t ti khp 1 ca khu 1.

Chn trc Z0 ca h to u tin c phng trng vi trc khp 1
16

Chng 4 - Robot cng nghip

TS Phan Tn Tng

Chn trc X0 ca h to u tin theo phng php tuyn chung

hng t im H1 n im O1
17

Chng 4 - Robot cng nghip

TS Phan Tn Tng

Chn trc Y0 ca h to u tin hp vi trc X0 v Z0 va chn theo

quy tc bn tay phi
18

Chng 4 - Robot cng nghip

TS Phan Tn Tng

Bc 5: Xc nh h to th i ca khu th i (t ti khp i+1). Chn

trc Zi ca h to th i c phng trng vi trc khp i+1
19

Chng 4 - Robot cng nghip

TS Phan Tn Tng

Chn trc Xi ca h to th i theo phng php tuyn chung hng

t im Hi n im Oi
20

Chng 4 - Robot cng nghip

TS Phan Tn Tng

Nu ng tm 2 trc khp th i v i+1 ct nhau th trc Xi c phng

vung gc trc khp i v khp i+1v tt nht l hp vi trc Xi-1 mt gc
nhn
21

Chng 4 - Robot cng nghip

TS Phan Tn Tng

Chn trc Yi ca h to th i hp vi trc Xi v Zi va chn theo quy

tc bn tay phi
22

Chng 4 - Robot cng nghip

TS Phan Tn Tng

Bc 6: xc nh h to trung gian ca khu th i (t ti khp th i).

H to trung gian c gc to l im Hi, trc Xint Xi, trc Zint Zi-1,
trc Yint hp vi trc Xint v Zint theo qui tc bn tay phi. Gc I l gc
hp bi trc Zi-1 v Zi c chiu dng l chiu dng quay quanh trc Xi
23
t Zi-1 nZi

Chng 4 - Robot cng nghip

TS Phan Tn Tng

Gc i hp bi trc Xi-1 v trc Xi. Chiu dng l chiu dng quay

quanh trc Zi-1 t Xi-1 n trc Xi.
24

Chng 4 - Robot cng nghip

nh ngha kch thc ai:

TS Phan Tn Tng

ai = H i Oi
25

Chng 4 - Robot cng nghip

nh ngha kch thc di:

TS Phan Tn Tng

d i = H i Oi 1
26

Chng 4 - Robot cng nghip

TS Phan Tn Tng

1.2 Khng gian hot ng ca robot

Khng gian hot ng ca robot (workspace) l vng m im tc ng
cui c th di chuyn n c.
Lu cn phn bit:
Khng gian c th vi n c (reachable workspace) l vng m im
tc ng cui c th di chuyn n c m khngquan tm n hng.
Khng gian lm vic c tnh n hng (dexterous workspace) l tp
con ca khng gian c th vi n c (reachable workspace).

27

Chng 4 - Robot cng nghip

TS Phan Tn Tng

2. Phng trnh ng hc ca cc robot in hnh

2.1 Robot Stanford

Robot Stanford c 6 bc t do. Robot c 6 khp ( 5 khp xoay v 1

khp trt ).
28

Chng 4 - Robot cng nghip

TS Phan Tn Tng

29

Chng 4 - Robot cng nghip

TS Phan Tn Tng

30

Chng 4 - Robot cng nghip

TS Phan Tn Tng

31

Chng 4 - Robot cng nghip

TS Phan Tn Tng

32

Chng 4 - Robot cng nghip

TS Phan Tn Tng

2.2 Robot Elbow

33

Chng 4 - Robot cng nghip

TS Phan Tn Tng

34

Chng 4 - Robot cng nghip

TS Phan Tn Tng

35

Chng 4 - Robot cng nghip

TS Phan Tn Tng

36

Chng 4 - Robot cng nghip

TS Phan Tn Tng

37

Chng 4 - Robot cng nghip

TS Phan Tn Tng

2.3 Robot Puma

38

Chng 4 - Robot cng nghip

TS Phan Tn Tng

39

Chng 4 - Robot cng nghip

TS Phan Tn Tng

40

Chng 4 - Robot cng nghip

TS Phan Tn Tng

41

Chng 4 - Robot cng nghip

TS Phan Tn Tng

3. Bi ton ng hc ngc (inverse kinematics )

Bi ton ng hc thun tay my cho php xc nh v tr v hng ca
phn cng tc (im tc ng cui hay end effector) theo cc bin khp.
Bi ton ng hc ngc tay my cho php xc nh cc bin khp
theo v tr v hng ca phn cng tc.
Bi ton ngc c th khng c li gii, c nhiu li gii v c th c
li gii bng ton hc nhng khng thc hin c trong thc t v l do
kt cu khng cho php thc hin.

42

Chng 4 - Robot cng nghip

TS Phan Tn Tng

3.1 Cch gii trc tip

Gi s im E c to (xE yE), chiu di cc khu l a1, a2 v cc bin
khp l 1, 2 . Ta c h phng trnh sau:

xE = a1. cos1 + a2 . cos(1 + 2 )

y E = a1.sin 1 + a2 .sin(1 + 2 )

43

Chng 4 - Robot cng nghip

TS Phan Tn Tng

Bnh phng 2 v ri cng li (nh p dng cng thc cng) ta c:

x E2 + y E2 a12 a22
cos 2 =
2.a1.a2

sin 2 = 1 cos 2 2

Nn

2 = A tan 2(sin 2 , cos 2 )

V ta c

(a1 + a2 cos 2 ) y E a2 sin 2 x E

sin 1 =
x E2 + y E2
(a1 + a2 cos 2 ) x E + a2 sin 2 y E
cos 1 =
2
2
xE + y E

1 = A tan 2(sin 1 , cos1 )

44

Chng 4 - Robot cng nghip

TS Phan Tn Tng

3.2 Trng hp robot n bc t do

Ma trn chuyn h to t h n v h 0 l

Tn = A1 A2 ....

n 1

An

Ta lp c h phng trnh:

Nu 6 bc t do:

A11 0T6 =1T6

A11 0Tn =1Tn

A21 1Tn = 2Tn

1 11
A2 T6 = 2T6

.....................

.....................

n2

n 1
1 n 2
An 1 Tn = Tn

1 5
5
A5 T6 = T6

Da vo cc phng trnh trn ta gii ra cc bin khp

45

Chng 4 - Robot cng nghip

TS Phan Tn Tng

V d c cu 2 bc t do

C 2
S
1
A2 = 2
0
0

C1
S
0
A1 = 1
0
0

S 2
C 2
0
0
S1

C1
0
0

u x
u
0
T2 = 0A2 = y
u z
0

0 a2C 2
0 a2 S 2

1
0
0
1
0 a1C1
0 a1S1

1
0
0
1
v x wx xE C1 2
v y w y y E S1 2
=
v z wz z E 0
0 0
1 0

S1 2

C1 2
0

0
1

xE
yE

0
1

46

Chng 4 - Robot cng nghip

C1
S
1
0 1
A1 =
0
0

S1
C1
0
0

C1 S1
S
C1

0 1 0
1
A1 T2 =
0
0
0
0

C 2 S 2
S
C 2
2
0 1 0

A1 T2 =
0
0
0
0

TS Phan Tn Tng

0 a1
0 0

1 0

0 1

A RBT
M ARBT A q

A 1 B
TB = TA = LLL M LLL
0 0 0 M

0 a1 C1 2 S1 2
0 0 S1 2 C1 2
.
0
1 0 0

0
0 1 0
0 xE C1 + y E S1 a1
0 xE S1 + y E C1

1
0

0
1

0
0
1
0

xE
yE

47

Chng 4 - Robot cng nghip

TS Phan Tn Tng

1
1 0
A1 T2 = A2

ng nht ho 2 v ta c

cos 2 =

2
xE

2
yE

2
a1

2
a2

2.a1.a2
2 = A tan 2(sin 2 , cos 2 )

sin 2 = 1 cos 2 2

(a1 + a2 cos 2 ) y E a2 sin 2 x E

sin 1 =
x E2 + y E2
(a1 + a2 cos 2 ) x E + a2 sin 2 y E
cos 1 =
2
2
xE + y E

1 = A tan 2(sin 1 , cos1 )

48

Chng 4 - Robot cng nghip

TS Phan Tn Tng

H phng trnh ng hc ngc ca robot Elbow

Xem sch Robot cng nghip ca GS TSKH Nguyn Thin Phc trang
126 - 130
H phng trnh ng hc ngc ca robot Stanford
Xem sch Robot cng nghip ca GS TSKH Nguyn Thin Phc trang
130 - 134

HT CHNG 4

49