Tieåu luaän moân hoïc: Robot coâng nghieäp vaø ngöôøi maùy

1. Giôùi thieäu veà Robot Scara

Robot SCARA ra ñôøi vaøo naêm 1979, ñaây laø moät kieåu tay maùy coù caáu taïo ñaëc
bieät ñöôïc söû duïng nhieàu trong caùc coâng vieäc laép raùp caùc taûi troïng nhoû theo
phöông thaúng ñöùng.
Robot SCARA bao goàm hai khôùp noái vaø hai caùnh tay. Treân hai khôùp noái seõ gaén
hai ñoäng cô ñeå cung caáp moment τ 1 ,τ 2 ñieàu khieån hai caùnh tay l1,l2 nhö hình
veõ.

l2
θ2
l1
θ1

Hình 1: Hình chieáu baèng Robot Scara

2. Moâ hình ñoäng hoïc cuûa Robot SCARA
Phöông trình ñoäng hoïc cuûa Robot SCARA nhö sau:
.. . .
M ( q ) q + C ( q, q ) q = τ (1)
Trong ñoù q1 , q 2 laø goùc quay cuûa hai truïc ñoäng cô DC vaø θ 1 ,θ 2 laø goùc quay cuûa
hai khôùp xoay cuûa robot. Trong ñoù:
⎡ I 1 + ml12 − ml1l 2 cos(k e (q1 + q 2 ))⎤
M (q ) = ⎢ ⎥
⎣− ml1l 2 cos(k e (q1 + q 2 )) I 2 + I 3 + ml 22 ⎦

⎡ .
⎤
.
V1 ml1l 2 k e sin( k e (q1 + q 2 )) q 2 ⎥
C ( q, q ) = ⎢
⎢ml l k sin(k (q + q )) q ⎥
.

⎣ 12 e e 1 2 1 V2 ⎦

Quan heä giöõa [q1 q 2 ] vaø [θ 1 θ 2 ] nhö sau:

⎡θ 1 ⎤ ⎡− k e 0 ⎤ ⎡ q1 ⎤
⎢θ ⎥ = ⎢ k ⎥⎢ ⎥ ( 2)
⎣ 2 ⎦ ⎣ e k e ⎦ ⎣q 2 ⎦
Caùc thoâng soá ñöôïc cho nhö sau:
I 1 = 1.8 Kg .m 2 : Moment quaùn tính cuûa caùnh tay 1
I 2 = 0.041Kg .m 2 : Moment quaùn tính cuûa caùnh tay 2
I 3 = 0.134 Kg .m 2 : Moment quaùn tính cuûa taûi troïng
m = 10 Kg : Khoái löôïng taûi

Hoïc vieân: Nguyeãn Minh Hoaø Lôùp: ÑKHKT K14 Trang 1

 Tieåu luaän moân hoïc: Robot coâng nghieäp vaø ngöôøi maùy

l1 = 0.4m : Chieàu daøi cuûa caùnh tay 1

l 2 = 0.35m : Chieàu daøi cuûa caùnh tay 2
k e = 0.5 : Heä soá truyeàn ñoäng cuûa khôùp noái 2
V1 = 5 Kg .m 2 / s : Heä soá ma saùt cuûa khôùp noái 1
V2 = 3Kg .m 2 / s : Heä soá ma saùt cuûa khôùp noái 2
Tín hieäu vaøo: Moment τ 1 ,τ 2 cuûa 2 ñoäng cô DC(hoaëc ñieän aùp)
Tín hieäu ra: Goùc θ 1 ,θ 2 cuûa 2 caùnh tay robot SCARA.
3. Heä phöông trình bieán traïng thaùi moâ taû robot
Bieán ñoåi phöông trình (1) vaø (2) ta ñöôïc:
⎡ .. 2 .. 2 . . ⎤
⎢ − m 2 2 2
l l k
1 2 e AB q 1 − ( I 2 + I 3 + ml 2
2 ) ml l k
1 2 e B q 2 − ( I 2 + I 3 + ml 2
2 )V q
1 1 − ml l V
1 2 2 A q 2 +⎥
⎢( I + I + ml 2 )τ + ml l Aτ ⎥
..
q1 = ⎣ 2 3 2 1 1 2 2 ⎦
[( I 1 + ml12 )( I 2 + I 3 + ml 22 ) − m 2 l12 l 22 A 2 ]
⎡ .. 2 .. 2 . . ⎤
⎢− ( I 1 + ml1 )ml1l 2 k e B q 1 − m l1 l 2 AB q 2 − ml1l 2V1 A q 1 − ( I 1 + ml1 )V2 q 2 + ⎥
2 2 2 2 2

⎢ml l Aτ + ( I + ml 2 )τ ⎥
..
q2 = ⎣ 12 1 1 1 2 ⎦ (3)
[( I 1 + ml12 )( I 2 + I 3 + ml 22 ) − m 2 l12 l 22 A 2 ]
Vôùi A = cos(k e (q1 + q 2 )) B = sin(k e (q1 + q 2 ))
Ñaët caùc bieán traïng thaùi nhö sau:
x1 = q1
. .
x1 = x 2 = q 1 ( 4)
x3 = q 2
. .
x 3 = x4 = q 2
Töø (3) vaø (4) ta ñöôïc heä phöông trình bieán traïng thaùi moâ taû robot sau:
.
x1 = x 2
⎡− m 2 l12 l 22 k e ABx 22 − ( I 2 + I 3 + ml 22 )ml1l 2 k e Bx 42 − ( I 2 + I 3 + ml 22 )V1 x 2 − ml1l 2V2 Ax 4 + ⎤
⎢ ⎥
⎢( I 2 + I 3 + ml 2 )τ 1 + ml1l 2 Aτ 2 ⎦⎥
2
.
x2 = ⎣
[
( I 1 + ml1 )( I 2 + I 3 + ml 2 ) − m l1 l 2 A
2 2 2 2 2 2
] (5)

.
x 3 = x4
⎡− ( I 1 + ml12 )ml1l 2 k e Bx 22 − m 2 l12 l 22 ABx 42 − ml1l 2V1 Ax 2 − ( I 1 + ml12 )V2 x 4 + ⎤
⎢ ⎥
⎢ml1l 2 Aτ 1 + ( I 1 + ml1 )τ 2 ⎦⎥
2
.
x4 = ⎣
[
( I 1 + ml1 )( I 2 + I 3 + ml 2 ) − m l1 l 2 A
2 2 2 2 2 2
]

Hoïc vieân: Nguyeãn Minh Hoaø Lôùp: ÑKHKT K14 Trang 2

 Tieåu luaän moân hoïc: Robot coâng nghieäp vaø ngöôøi maùy

4. Quyõ ñaïo ñaàu muùt caùnh tay Robot SCARA

Moái lieân heä giöõa caùc khôùp noái 1 vaø 2 coù toaï ñoä goùc [q1 q 2 ] T vôùi caùc caùnh tay coù
toïa ñoä goùc [θ 1 θ 2 ] T , ñöôïc moâ taû bôûi phöông trình (2)
⎡θ 1 ⎤ ⎡− k e 0 ⎤ ⎡ q1 ⎤
⎢θ ⎥ = ⎢ k ⎥⎢ ⎥
⎣ 2 ⎦ ⎣ e k e ⎦ ⎣q 2 ⎦
Toïa ñoä ñaàu muùt cuûa caùnh tay robot SCARA ñöôïc xaùc ñònh bôûi phöông trình
sau:
x = l1 cosθ 1 + l 2 cos(θ 1 + θ 2 )
y = l1 sin θ 1 + l 2 sin(θ 1 + θ 2 ) (6)
Caùc phöông trình treân aùp duïng cho baøi toaùn ñieàu khieån theo khoâng gian khôùp
noái. Cho pheùp xaùc ñònh caëp goùc [θ 1 θ 2 ] T töø toïa ñoä (x,y) cho tröôùc cuûa ñaàu muùt
caùnh tay robot SCARA trong khoâng gian hoaït ñoäng.
Trong quaù trình ñieàu khieån robot, öùng vôùi moät vò trí (x,y) trong khoâng gian hoaït
ñoäng coù theå coù nhieàu caëp goùc trong khoâng gian khôùp noái. Do ñoù, ta phaûi choïn
löïa moät caëp nghieäm phuø hôïp vôùi yeâu caàu ñieàu khieån. Coù nhieàu caùch choïn löïa
tuyø theo ñoái töôïng vaø muïc ñích:
- Toång quaõng ñöôøng dòch chuyeån cuûa caùc khôùp ngaén nhaát.
- Giaûm bôùt aûnh höôûng töông taùc giöõa caùc khôùp.
- Naêng löôïng tieâu thuï thaáp nhaát.
- Giôùi haïn ñoäng hoïc cuûa heä thoáng.
Robot SCARA vôùi hai khôùp noái öùng vôùi moãi toïa ñoä (x,y) cuûa ñaàu muùt caùnh tay
chuùng ta seõ coù hai nghieäm cho caùc caëp goùc. Do ñoù, ta seõ choïn caëp goùc sao cho
toång bieán thieân cuûa caëp goùc töø vò trí k-1 ñeán vò trí k laø nhoû nhaát.
min (θ 1 (k ) − θ 1 (k − 1) + θ 2 (k ) − θ 2 (k − 1) ) (7 )
Giôùi haïn khoâng gian hoaït ñoäng cuûa heä thoáng thoâng qua caëp goùc [q1 q 2 ] T
− π ≤ θ1 ≤ π
−π ≤ θ2 ≤ π (8)
5. Ñieàu khieån robot SCARA duøng moment tính(computed torque)
Trong phöông phaùp moment tính, ta phaûi xaây döïng hai voøng ñieàu khieån. Voøng
hoài tieáp trong ñöôïc xaây döïng döïa treân ñoäng löïc hoïc cuûa heä thoáng ñeå buø taát caû
caùc thaønh phaàn phi tuyeán cuûa heä thoáng. Voøng hoài tieáp ngoaøi xaây döïng döïa treân
sai leäch giöõa tín hieäu ra vaø tín hieäu ñaët. Chöùc naêng cuûa voøng hoài tieáp trong laø
tuyeán tính hoaù moái lieân heä giöõa tín hieäu ñaët vaø tín hieäu ra trong khi voøng hoài
tieáp ngoaøi laøm oån ñònh hoaù heä thoáng.

Hoïc vieân: Nguyeãn Minh Hoaø Lôùp: ÑKHKT K14 Trang 3

 Tieåu luaän moân hoïc: Robot coâng nghieäp vaø ngöôøi maùy

5.1 Caáu truùc cuûa boä ñieàu khieån moment tính

Heä thoáng tuyeán tính hoaù

Voøng ñieàu khieån trong

Chieán löôïc Boä ñieàu Hoài tieáp tuyeán SCARA
ñieàu khieån khieån PD tính hoaù ROBOT

Voøng ñieàu khieån ngoaøi

Hình 2: Sô ñoà heä thoáng ñieàu khieån duøng moment tính

5.1.1 Hoài tieáp tuyeán tính hoaù
Moâ hình robot SCARA ñöôïc cho bôûi (1) nhö sau:
.. . .
M ( q ) q + C ( q, q ) q = τ
Vôùi
⎡ I 1 + ml12 − ml1l 2 cos(k e (q1 + q 2 ))⎤
M (q ) = ⎢ ⎥
⎣− ml1l 2 cos(k e (q1 + q 2 )) I 2 + I 3 + ml 22 ⎦

⎡ .
⎤
.
V1 ml1l 2 k e sin( k e (q1 + q 2 )) q 2 ⎥
C ( q, q ) = ⎢
⎢ml l k sin(k (q + q )) q ⎥
.

⎣ 12 e e 1 2 1 V2 ⎦
Ñeå tuyeán tính hoaù vaø phaân ly, ta choïn luaät ñieàu khieån τ goàm hai thaønh phaàn
nhö sau:
τ = τ L +τ N (9)
Vôùi
.. .
τ N = M n q+ C q (10)
..
τL = Md q (11)

τN .
N ( q, q )

τL +
SCARA
ROBOT
+ q
Hình 3: Sô ñoà caáu truùc hoài tieáp tuyeán tính hoaù
⎡ 0 − ml1l 2 cos(k e (q1 + q 2 ))⎤
Mn = ⎢ ⎥
⎣− ml1l 2 cos(k e (q1 + q 2 )) 0 ⎦

Hoïc vieân: Nguyeãn Minh Hoaø Lôùp: ÑKHKT K14 Trang 4

 Tieåu luaän moân hoïc: Robot coâng nghieäp vaø ngöôøi maùy

⎡ I + ml12 0 ⎤
Md = ⎢ 1 2⎥
⎣ 0 I 1 + I 3 + ml 2 ⎦
⎡ .
⎤
.
V1 ml1l 2 k e sin( k e (q1 + q 2 )) q 2 ⎥
C ( q, q ) = ⎢
⎢ml l k sin(k (q + q )) q ⎥
.

⎣ 12 e e 1 2 1 V2 ⎦
5.1.2 Ñieàu khieån PD
..
Ñaët y = q vôùi y laø tín hieäu vaøo môùi, ta coù luaät ñieàu khieån ñöôïc moâ taû nhö sau:
τL = Md y (12)
Muïc ñích ñieàu khieån laø tín hieäu ra q (t ) baùm theo quyõ ñaïo thieát keá q d (t )
.. .
Ta coù: r = q d + K p q d + K d q d (13)
Choïn luaät ñieàu khieån
.
y = −K p q − K d q+ r (14)
Thay (13) vaøo (14) ta ñöôïc
.. .
e+ K d e+ K p (15)
e = qd − q
Giaû thuyeát Kp vaø Kd laø caùc ma traän döông xaùc ñònh, oån ñònh tieäm caän
Töø (10) vaø (11) ta ñöôïc
⎡ .. . .. 2 ⎤
⎡τ N 1 ⎤ ⎢ − ml1l 2 A q 2 + V1 q 1 + ml1l 2 k e B q 2 ⎥
⎢τ ⎥ = ⎢
⎣ N 2 ⎦ ⎢− ml l A q + V q + ml l k B q ⎥⎥
.. . .. 2

⎣ 1 2 1 2 2 1 2 e 1⎦

A = cos(k e (q1 + q 2 )) B = sin(k e (q1 + q 2 ))

⎡ .. .
⎤
⎡τ L1 ⎤ ⎢ ( I 1 + ml12 )(q d 1 + K d e1 + K p e1 ) ⎥
⎢τ ⎥ ⎢ =
⎣ L2 ⎦ + +
..
+
.
⎥
d 2 + K p e2 )⎦
2
(
⎣ 2 I I 3 ml 2 )( q d2 K e

e = qd − q
. . .
e = qd − q

Hoïc vieân: Nguyeãn Minh Hoaø Lôùp: ÑKHKT K14 Trang 5

 Tieåu luaän moân hoïc: Robot coâng nghieäp vaø ngöôøi maùy

τN .
N ( q, q )

τL +
Chieán löôïc SCARA
PD
ñieàu khieån ROBOT
+ q

Hình 4: Sô ñoà khoái heä thoáng ñieàu khieån duøng moment tính
5.2 Thöïc hieän moâ phoûng heä thoáng ñieàu khieån robot SCARA duøng moment
tính
5.2.1 Xaây döïng sô ñoà moâ phoûng
Xaây döïng moâ hình robot SCARA theo phöông trình (1) treân Simulink nhö sau:

Hình 5: Moâ hình robot SCARA

Hoïc vieân: Nguyeãn Minh Hoaø Lôùp: ÑKHKT K14 Trang 6

 Tieåu luaän moân hoïc: Robot coâng nghieäp vaø ngöôøi maùy

Hình 6: Sô ñoà ñieàu khieån duøng moment tính

Hình 7: Sô ñoà khoái phaàn hoài tieáp tuyeán tính hoaù

Hoïc vieân: Nguyeãn Minh Hoaø Lôùp: ÑKHKT K14 Trang 7

 Tieåu luaän moân hoïc: Robot coâng nghieäp vaø ngöôøi maùy

Hình 8: Sô ñoà khoái boä ñieàu khieån PD

5.2.2 Keát quaû moâ phoûng

5.2.2.1 Ñaùp öùng böôùc:

Tin hieu qd1-q1 Tin hieu qd2-q2

1 1

0.8 0.8

0.6 0.6

0.4 0.4

0.2 0.2

0 0
0 20 40 60 0 20 40 60

Hình 9: Ñaùp öùng böôùc cuûa q1 vaø q2

Hoïc vieân: Nguyeãn Minh Hoaø Lôùp: ÑKHKT K14 Trang 8

 Tieåu luaän moân hoïc: Robot coâng nghieäp vaø ngöôøi maùy

Hình 10: Moment cuûa 2 ñoäng cô khi tín hieäu vaøo laø haøm böôùc

Nhaän xeùt: Ñaùp öùng böôùc khoâng coù voït loá, loaïi boû ñöôïc söï aûnh höôûng laån nhau
giöõa hai khôùp noái
5.2.2.2 Baùm theo quyõ ñaïo hình sin:

q1 q2
qd1 qd2
qd1-q1 qd2-q2

1 1

0.5 0.5

0 0

-0.5 -0.5

-1 -1
0 10 20 0 5 10

Hình 11: Ñaùp öùng ngoõ ra q1 vaø q2 khi tín hieäu vaøo laø hình sin

Hoïc vieân: Nguyeãn Minh Hoaø Lôùp: ÑKHKT K14 Trang 9

 Tieåu luaän moân hoïc: Robot coâng nghieäp vaø ngöôøi maùy

Hình 12: Moment cuûa 2 ñoäng cô DC 1 vaø DC2 khi tín hieäu vaøo laø sin

Nhaän xeùt: Ngoû ra cuûa heä thoáng baùm theo tín hieäu vaøo hình sin raát toát,gaàn nhö
khoâng coù sai soá,heä phaân ly toát caùc khôùp khoâng bò aûnh höôûng laån nhau.

Hoïc vieân: Nguyeãn Minh Hoaø Lôùp: ÑKHKT K14 Trang 10