bài giảng động học vị trí robot - ts. phan tấn tùng

1
Chng
Chng
4
4
-
-
Robot
Robot
công
công
nghi
nghi


p
p
TS
TS
Phan
Phan
T
T


n
n
T
T
ù
ù

ng
ng
Chng
Chng
4
4




ng
ng
h
h


c
c
v
v


tr
tr
í
í
Robot
Robot
1. Bài toán đng hc(v trí) thun (forward kinematics)
• Tay máy là 1 chui đng h bao gm các khâu và khp

• Các thông s không thay đigiátr khi tay máy hot đng gilàTHAM
S ( Vd: chiu dài các khâu )
• Các thông s thay đigiátr khi tay máy hot đng gilàBIN KHP
(Vd: góc hpbi2 khâuti1 khp)
• Các khpthng dùng trong tay máy là KHP TRT và KHP QUAY
(là khploi5)
• Bài toán thun mô tà v trí và hng ca đim End Effector didng
hàm s ca các binkhp
2
Chng
Chng
4
4
-
-
Robot
Robot
công
công
nghi
nghi


p
p
TS
TS
Phan
Phan
T

T


n
n
T
T
ù
ù
ng
ng
Hàm s mô t v trí ca đim End Effector
Vi
Vd1: C cutaymáynh hình bên
pqTp
n
n
).(
00
=
⎥
⎦
⎤
⎢
⎣
⎡
=
1000
)(
0000

0
pwvu
qT
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
+
+
−
=
1000
0100
0
0
)(
2112121
2112121
21
21
0
θθθθθθθ

θθθθθθθ
SaSaCS
CaCaSC
qT
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
+
+
=
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣

⎡
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
+
+
−
==
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
=

1
0
1
0
0
0
.
1000
0100
0
0
).(
1
211
211
2112121
2112121
21
21
21
21
2
2
00
θθθ
θθθ
θθθθθθθ
θθθθθθθ
SaSa
CaCa

SaSaCS
CaCaSC
pqT
z
y
x
p
E
E
E
3
Chng
Chng
4
4
-
-
Robot
Robot
công
công
nghi
nghi


p
p
TS
TS
Phan

Phan
T
T


n
n
T
T
ù
ù
ng
ng
1.1 Qui tc Denavit Hartenberg
H tođgn lên các khâu nh sau:
• TrcZ
i
đtdctheotrckhpi+1
• TrcX
i
đtdctheophng pháp tuyn chung giaZ
i-1
và Z
i
, hng
t khpi đnkhpi+1
• TrcY
i
vuông góc viX
i

và Y
i
theo qui tcbàntayphi
• GctođO
i
là giao catrcZ
i
và pháp tuyn chung nh nhtca
trcZ
i-1
và Z
i
• GctođO
i’
là giao catrcZ
i-1
và pháp tuyn chung nh nhtca
trcZ
i-1
và Z
i
Các thông s Denavit Hartenberg
• Khong cách gia2 khpliêntiptheophng X
i
là a
i
(tham s)
• Khong cách gia2 khpliêntiptheophng Z
i-1
là d

i
(binkhp)
• Góc quay quanh trcX
i
giatrcZ
i-1
và trcZ
i
là α
I
(tham s)
• Góc quay quanh trcZ
i-1
giatrcX
i-1
và trcX
i
là θ
I
(binkhp)
4
Chng
Chng
4
4
-
-
Robot
Robot
công

công
nghi
nghi


p
p
TS
TS
Phan
Phan
T
T


n
n
T
T
ù
ù
ng
ng
nh nghah tođvà các thông s Denavit Hartenberg
5
Chng
Chng
4
4
-

-
Robot
Robot
công
công
nghi
nghi


p
p
TS
TS
Phan
Phan
T
T


n
n
T
T
ù
ù
ng
ng
⎥
⎥
⎥

⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
−
=
−
1000
100
00
00
int
1
i
ii
ii
i
d
CS
SC
A
θθ
θθ
⎥
⎥

⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
−
=
1000
00
00
001
int
ii
ii
i
i
CS
SC
a
A
αα
αα
Xác đnh ma trn chuyn đit h tođth i v h tođth i-1
Ma trnchuyn đit h tođth i v h tođtrung gian int. Xoay
quanh trcX

int
góc α
I
sau đótnh tintheotrcX
int
đona
i
.
Ma trnchuyn đit h trunggianintv h i-1. Xoay quanh trcZ
i
góc
θ
i
sau đótnh tintheotrcZ
int
đond
i
.
6
Chng
Chng
4
4
-
-
Robot
Robot
công
công
nghi

nghi


p
p
TS
TS
Phan
Phan
T
T


n
n
T
T
ù
ù
ng
ng
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢

⎢
⎣
⎡
−
−
==
−−
1000
0
int
int
11
iii
iiiiiii
iiiiiii
i
i
i
i
dCS
SaSCCCS
CaSSCSC
AAA
αα
θαθαθθ
θαθαθθ
Vyma trnchuyn đit h i v h i-1 là
Tóm litikhâuth i ta có ma trnchuyn đit h th i v h i-1 nh
trên vi các thông s Denavit Hartenberg đc xác đnh trong bng
thông s DH nh sau

7
Chng
Chng
4
4
-
-
Robot
Robot
công
công
nghi
nghi


p
p
TS
TS
Phan
Phan
T
T


n
n
T
T
ù

ù
ng
ng
Phng trình đng hcthun(v trí) cataymáylà
pAAAApTp
n
n
n
i
in
n
11
2
1
1
000

−
−
==
Vd2: Xét c cutaymáynh hình bên
Bng thông s DH cataymáy
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢

⎢
⎢
⎣
⎡
−
=
1000
0100
0
0
222
222
2
2
2
1
θθθ
θθθ
SaCS
CaSC
A
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢

⎢
⎣
⎡
−
=
1000
0100
0
0
111
111
1
1
1
0
θθθ
θθθ
SaCS
CaSC
A
Ma trnchuyn đit h 2 v h 1 và t h 1 v h 0
8
Chng
Chng
4
4
-
-
Robot
Robot

công
công
nghi
nghi


p
p
TS
TS
Phan
Phan
T
T


n
n
T
T
ù
ù
ng
ng
(
)
(
)
()
⎥

⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
++−+
+
−
+
−
−
==
1000
0100
0
0
1212121212121
1212121212121
12
12
2
1
1
0

2
0
θθθθθθθθθθθθθ
θθθθθθθθθθθθθ
SaSCCSaSSCCSCCS
CaSSCCaCSSCSSCC
AAA
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
+
+
−
==
1000
0100
0
0
1212121
1212121
12

12
2
0
2
0
θθθθθθθ
θθθθθθθ
SaSaCS
CaCaSC
AT
Vyma trnchuyn đit h tođ2 v h tođ0 là
Phng trình đông hc(v trí) cataymáy
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
+
+
=
⎥
⎥
⎥

⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
+
+
−
==
⎥
⎥
⎥
⎥
⎦

⎤
⎢
⎢
⎢
⎢
⎣
⎡
=
1
0
1
0
0
0
.
1000
0100
0
0
.
1
211
211
2112121
2112121
21
21
21
21
2

2
00
θθθ
θθθ
θθθθθθθ
θθθθθθθ
SaSa
CaCa
SaSaCS
CaCaSC
pT
z
y
x
p
E
E
E
9
Chng
Chng
4
4
-
-
Robot
Robot
công
công
nghi

nghi


p
p
TS
TS
Phan
Phan
T
T


n
n
T
T
ù
ù
ng
ng
Các bc khi xác đnh ma trnchuyn đitheophng pháp Denavit
Hartenberg
Qui ckhâuth i nmgiakhpth i và i+1
Bc 1: xác đnh các đng tâm trc các khp
10
Chng
Chng
4
4

-
-
Robot
Robot
công
công
nghi
nghi


p
p
TS
TS
Phan
Phan
T
T


n
n
T
T
ù
ù
ng
ng
Bc 2: xác đnh kích thcca pháp tuyn chung nh nhtgia đng
tâm trckhpth i và i+1 và gilàa

i
.
11
Chng
Chng
4
4
-
-
Robot
Robot
công
công
nghi
nghi


p
p
TS
TS
Phan
Phan
T
T


n
n
T

T
ù
ù
ng
ng
Nu2 trckhpth i và i +1 song song thì có th chna
i
btk. Nhng
ttnht nên chna
i
giao via
i-1
12
Chng
Chng
4
4
-
-
Robot
Robot
công
công
nghi
nghi


p
p
TS

TS
Phan
Phan
T
T


n
n
T
T
ù
ù
ng
ng
Nu đng tâm 2 trckhpth i và i+1 ctnhauthìđona
i
tr thành 1
đimcóphng vuông góc vitrckhpi vài +1
13
Chng
Chng
4
4
-
-
Robot
Robot
công
công

nghi
nghi


p
p
TS
TS
Phan
Phan
T
T


n
n
T
T
ù
ù
ng
ng
Bc 3: xác đnh gccu h tođO
i
là giao ca đng a
i
và trckhp
th i +1. Gccah tođtrung gian H
i
là giao ca đng a

i
và trc
khpi.
14
Chng
Chng
4
4
-
-
Robot
Robot
công
công
nghi
nghi


p
p
TS
TS
Phan
Phan
T
T


n
n

T
T
ù
ù
ng
ng
Nutrckhp i và i+1 ctnhauthìH
i
≡ O
i
15
Chng
Chng
4
4
-
-
Robot
Robot
công
công
nghi
nghi


p
p
TS
TS
Phan

Phan
T
T


n
n
T
T
ù
ù
ng
ng
Vikhp1 (khpnigiácđnh) thì chnO
0
≡ H
1
16
Chng
Chng
4
4
-
-
Robot
Robot
công
công
nghi
nghi



p
p
TS
TS
Phan
Phan
T
T


n
n
T
T
ù
ù
ng
ng
Bc4: Xácđnh h tođđutiên(h 0) đttikhp1 cakhâu1.
ChntrcZ
0
cah tođđutiêncóphng trùng vitrckhp1
17
Chng
Chng
4
4
-

-
Robot
Robot
công
công
nghi
nghi


p
p
TS
TS
Phan
Phan
T
T


n
n
T
T
ù
ù
ng
ng
ChntrcX
0
cah tođđutiêntheophng pháp tuyn chung nh

nhthng tđimH
1
đn đimO
1
18
Chng
Chng
4
4
-
-
Robot
Robot
công
công
nghi
nghi


p
p
TS
TS
Phan
Phan
T
T


n

n
T
T
ù
ù
ng
ng
ChntrcY
0
cah tođđutiênhpvitrcX
0
và Z
0
vachntheo
quy tcbàntayphi
19
Chng
Chng
4
4
-
-
Robot
Robot
công
công
nghi
nghi



p
p
TS
TS
Phan
Phan
T
T


n
n
T
T
ù
ù
ng
ng
Bc5: Xácđnh h tođth i cakhâuth i (đttikhpi+1). Chn
trcZ
i
cah tođth i có phng trùng vitrckhpi+1
20
Chng
Chng
4
4
-
-
Robot

Robot
công
công
nghi
nghi


p
p
TS
TS
Phan
Phan
T
T


n
n
T
T
ù
ù
ng
ng
ChntrcX
i
cah tođth i theo phng pháp tuyn chung nh nht
hng tđimH
i

đn đimO
i
21
Chng
Chng
4
4
-
-
Robot
Robot
công
công
nghi
nghi


p
p
TS
TS
Phan
Phan
T
T


n
n
T

T
ù
ù
ng
ng
Nu đng tâm 2 trckhpth i và i+1 ctnhauthìtrcX
i
có phng
vuông góc trckhpi vàkhp i+1và ttnhtlàhpvitrcX
i-1
mtgóc
nhn
22
Chng
Chng
4
4
-
-
Robot
Robot
công
công
nghi
nghi


p
p
TS

TS
Phan
Phan
T
T


n
n
T
T
ù
ù
ng
ng
ChntrcY
i
cah tođth i hpvitrcX
i
và Z
i
vachn theo quy
tcbàntayphi
23
Chng
Chng
4
4
-
-

Robot
Robot
công
công
nghi
nghi


p
p
TS
TS
Phan
Phan
T
T


n
n
T
T
ù
ù
ng
ng
Bc 6: xác đnh h tođtrung gian cakhâuth i (đttikhpth i).
H tođtrung gian có gctođlà đimH
i
, trcX

int
≡
X
i
, trcZ
int
≡ Z
i-1
,
trcY
int
hpvitrcX
int
và Z
int
theo qui tcbàntayphi. Góc α
I
là góc
hpbitrcZ
i-1
và Z
i
có chiudng là chiudng quay quanh trcX
i
24
Chng
Chng
4
4
-

-
Robot
Robot
công
công
nghi
nghi


p
p
TS
TS
Phan
Phan
T
T


n
n
T
T
ù
ù
ng
ng
Góc θ
i
hpbitrcX

i-1
và trcX
i
. Chiudng là chiudng quay
quanh trcZ
i-1
.
25
Chng
Chng
4
4
-
-
Robot
Robot
công
công
nghi
nghi


p
p
TS
TS
Phan
Phan
T
T



n
n
T
T
ù
ù
ng
ng
nh nghakíchthca
i
:
iii
OHa
=