Professional Documents
Culture Documents
497 Scara1
497 Scara1
l2
θ2
l1
θ1
⎡ .
⎤
.
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 ⎦
⎢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
]
⎡ .
⎤
.
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 ⎦
⎡ 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⎦
⎡ .. .
⎤
⎡τ 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
τ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:
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 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
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.