ROBOT công nghiệp - Chương 8: Thiết kế quỹ đạo robot
Khâu chấp hành cuối của robot chỉ cần đạt được vị trí và hướng tại các điểm nút (điểm tựa: Knot point). Đây chính là phương pháp điều khiển điểm (PTP). Tại đó bàn tay robot thực hiện các thao tác cầm nắm đối tượng hoặc buông thả đối tượng.
92
Robot c«ng nghiÖp
Ch−¬ng VIII
ThiÕt kÕ quÜ ®¹o robot.
(Trajectory Planing)
Trong c¸c øng dông c«ng nghiÖp cña robot, ta th−êng gÆp hai tr−êng
hîp sau :
Tr−êng hîp 1 : Kh©u chÊp hµnh cuèi cña robot chØ cÇn ®¹t ®−îc vÞ trÝ vµ
h−íng t¹i c¸c ®iÓm nót (®iÓm tùa : Knot point). §©y chÝnh lµ ph−¬ng ph¸p
®iÒu khiÓn ®iÓm (PTP). T¹i ®ã, bµn tay robot thùc hiÖn c¸c thao t¸c cÇm n¾m
®èi t−îng hoÆc bu«ng nh¶ ®èi t−îng. §©y lµ tr−êng hîp cña c¸c robot thùc
hiÖn c«ng viÖc vËn chuyÓn vµ trao ®æi ph«i liÖu trong mét hÖ thèng tù ®éng
linh ho¹t robot ho¸. Bµn tay robot kh«ng trùc tiÕp tham gia vµo c¸c nguyªn
c«ng c«ng nghÖ nh− hµn, c¾t kim lo¹i ... C¸c ®iÓm nót lµ môc tiªu quan träng
nhÊt, cßn d¹ng ®−êng ®i tíi c¸c ®iÓm nót lµ vÊn ®Ò thø yÕu. Trong tr−êng hîp
nÇy Robot th−êng ®−îc lËp tr×nh b»ng ph−¬ng ph¸p d¹y häc (Teach and
playback mode). Trong tr−êng hîp nÇy kh«ng cÇn tÝnh to¸n ph−¬ng tr×nh ®éng
häc hoÆc ®éng häc ng−îc robot, chuyÓn ®éng mong muèn ®−îc ghi l¹i nh−
mét tËp hîp c¸c gãc khíp (thùc tÕ lµ tËp hîp c¸c gi¸ trÞ m· ho¸ cña biÕn khíp)
®Ó robot thùc hiÖn l¹i (Playback) khi lµm viÖc.
Tr−êng hîp 2 : Kh©u chÊp hµnh cuèi cña robot ph¶i x¸c ®Þnh ®−êng ®i
qua c¸c ®iÓm nót theo thêi gian thùc. §ã lµ tr−êng hîp c¸c tay m¸y trùc tiÕp
thùc hiÖn c¸c nguyªn c«ng c«ng nghÖ nh− s¬n, hµn, c¾t kim lo¹i ... VÊn ®Ò
thiÕt kÕ quü ®¹o cho c¸c robot trong tr−êng hîp nÇy lµ rÊt quan träng. Nã
quyÕt ®Þnh trùc tiÕp chÊt l−îng thùc hiÖn c¸c nguyªn c«ng c«ng nghÖ mµ robot
®¶m nhËn. Trong ch−¬ng nÇy, chóng ta ®Ò cËp ®Õn bµi to¸n thiÕt kÕ quü ®¹o
víi mét sè quü ®¹o ®iÓn h×nh. C¸c quü ®¹o nÇy kh«ng chØ cã ý nghÜa trong
tr−êng hîp øng dông thø hai mµ nã bao hµm mét ý nghÜa chung cho mäi
robot, v× ngay c¶ tr−êng hîp ®¬n gi¶n nh− c¸c robot thuéc øng dông thø nhÊt
còng thùc hiÖn nh÷ng chuyÓn ®éng quü ®¹o c¬ b¶n mµ chóng ta sÏ nghiªn cøu
d−íi ®©y.
8.1. C¸c kh¸i niÖm vÒ quü ®¹o robot :
§Ó x¸c ®Þnh ®−îc ®−êng ®i mong muèn cña robot theo thêi gian, quü
®¹o cã thÓ ®−îc tÝnh to¸n thiÕt kÕ trong mét hÖ to¹ ®é truyÒn thèng Oxyz
(Cartesian Space) hoÆc thiÕt kÕ trong kh«ng gian biÕn khíp (kh«ng gian
tr−êng vect¬ c¸c to¹ ®é suy réng cña robot), ch¼ng h¹n víi robot 6 bËc tù do
th× X = [θ1 , θ 2 , θ 3 , θ 4 .θ 5 , θ 6 ] T . ThiÕt kÕ quü ®¹o ë ®©y ®−îc hiÓu lµ x¸c ®Þnh qui
luËt chuyÓn ®éng cña c¸c biÕn khíp ®Ó ®iÒu khiÓn chuyÓn ®éng cña tõng khíp
vµ tæng hîp thµnh chuyÓn ®éng chung cña robot theo mét quü ®¹o ®· ®−îc
x¸c ®Þnh.
TS. Ph¹m §¨ng Ph−íc
93
Robot c«ng nghiÖp
Quü ®¹o cÇn thiÕt kÕ nhÊt thiÕt ph¶i ®i qua mét sè ®iÓm nót cho tr−íc (Ýt
nhÊt lµ ®iÓm ®Çu vµ ®iÓm cuèi). Ngoµi c¸c ®iÓm nót chÝnh, ta cßn cã thÓ chän
thªm c¸c ®iÓm nót phô gäi lµ ®iÓm dÉn h−íng (via point) ®Ó tr¸nh c¸c ch−íng
ng¹i vËt.
Khi thiÕt kÕ quü ®¹o trong kh«ng gian biÕn khíp, t¹i mçi ®iÓm nót ph¶i
x¸c ®Þnh gi¸ trÞ cña c¸c biÕn khíp b»ng ph−¬ng ph¸p tÝnh to¸n ®éng häc
ng−îc. Thêi gian yªu cÇu cña mçi ®o¹n quü ®¹o (gi÷a 2 ®iÓm nót) lµ gièng
nhau cho tÊt c¶ c¸c khíp v× vËy yªu cÇu tÊt c¶ c¸c khíp ph¶i ®¹t ®Õn ®iÓm nót
®ång thêi. Ngoµi viÖc yªu cÇu thêi gian ph¶i gièng nhau cho c¸c khíp, viÖc
x¸c ®Þnh c¸c hµm quü ®¹o cña mçi biÕn khíp kh«ng phô thuéc vµo c¸c hµm
cña c¸c khíp kh¸c. V× vËy viÖc thiÕt kÕ quü ®¹o trong kh«ng gian biÕn khíp
®¬n gi¶n vµ dÔ tÝnh to¸n h¬n khi m« t¶ trong hÖ to¹ ®é §Òc¸c.
Quü ®¹o thiÕt kÕ ph¶i ®¶m b¶o c¸c ®iÒu kiÖn liªn tôc (continous
conditions) bao gåm :
+ Liªn tôc vÒ vÞ trÝ (Position)
+ Liªn tôc vÒ tèc ®é (Velocity)
+ Liªn tôc vÒ gia tèc (Acceleration).
x(t)
qi(t2)...
x2 C¸c ®iÓm nót
x1
xf-1
xo
xf
t
tf-1 tf
to t1 t2
H×nh 8.1. TÝnh liªn tôc cña quü ®¹o robot.
§Ó thiÕt kÕ quü ®¹o robot, ng−êi ta th−êng dïng ph−¬ng ph¸p xÊp xØ
c¸c ®a thøc bËc n, c¸c quÜ ®¹o th−êng gÆp lµ :
+ QuÜ ®¹o CS (Cubic Segment) : T−¬ng ®−¬ng ®a thøc bËc 3;
+ Quü ®¹o LS (linear Segment) : T−¬ng ®−¬ng ®a thøc bËc 1;
+ Quü ®¹o LSPB (Linear Segment with Parabolic Blend) : Phèi hîp ®a
thøc bËc 2 víi ®a thøc bËc 1.
§o¹n th¼ng
q2
q0
qf
q1
§−êng cong bËc 2
H×nh 8.2 : Quü ®¹o LSPB
TS. Ph¹m §¨ng Ph−íc
94
Robot c«ng nghiÖp
+ Quü ®¹o BBPB (Bang Bang Parabolic Blend) : lµ tr−êng hîp ®Æc biÖt
cña quü ®¹o LSPB khi ®o¹n tuyÕn tÝnh thu vÒ b»ng 0 vµ xuÊt hiÖn ®iÓm
uèn.
qf
q0
H×nh 8.2 : Quü ®¹o BBPB
NÕu cho tr−íc nhiÒu ®iÓm nót, ta cã thÓ ¸p dông nhiÒu d¹ng quü ®¹o c¬
b¶n kh¸c nhau cho mét biÕn khíp.
8.2. Quü ®¹o ®a thøc bËc 3 :
Khi thiÕt kÕ quü ®¹o robot theo ®a thøc bËc 3 qua c¸c ®iÓm nót, mçi
®o¹n quü ®¹o gi÷a hai ®iÓm nót sÏ ®−îc biÓu diÔn b»ng mét ph−¬ng tr×nh bËc
3 riªng biÖt. Quü ®¹o ®a thøc bËc 3 ®¶m b¶o sù liªn tôc cña ®¹o hµm bËc nhÊt
vµ bËc hai t¹i c¸c ®iÓm nót.
T¹i thêi ®iÓm tk ≤ t ≤ tk+1, quü ®¹o xÊp xØ ®a thøc bËc 3 cña biÕn khíp
thø i lµ qi(t) cã d¹ng :
qi(t) = ai + bi(t - tk) + ci(t - tk)2 + di(t - tk)3 (8.1)
qi(t)
qk+1
Víi c¸c rµng buéc :
qi(tk) = qk vµ q i (t k ) = q k
& &
BËc 3
qi(tk+1) = qk+1 vµ q i (t k +1 ) = q k +1 qk
& &
t
Tõ (8.1) ta thÊy : t = tk → ai = qk tk+1
tk (8.2)
LÊy ®¹o hµm cña (8.1) theo t, ta cã :
q i (t) = b i + 2c i (t − t k ) + 3d i (t − t k ) 2
&
T¹i : t = tk → b i = q k & (8.3)
T¹i t = ti+1 ta cã hai tham sè :
3(q k +1 − q k ) − (2q k + q k +1 ) δt k
& &
ci = (8.4)
δt 2
k
(q + q k ) δt k − 2(q k +1 − q k )
& &
d i = k +1 (8.5)
δt 3
k
Trong ®ã : δt k = t k +1 − t k
C¸c ph−¬ng tr×nh (8.4) vµ (8.5) nhËn ®−îc khi gi¶i (8.1) ... (8.3).
TÝnh liªn tôc cña vËn tèc lµ sù ®¶m b¶o cho quü ®¹o kh«ng gÊp khóc,
giËt côc, g©y sèc trong qu¸ tr×nh ho¹t ®éng cña robot. VËn tèc vµ gia tèc t¹i
®iÓm cuèi cña mét ®o¹n ®−êng cong bËc 3 chÝnh b»ng vËn tèc vµ gia tèc cña
®o¹n cong bËc 3 tiÕp theo.
CÇn chó ý r»ng khi thiÕt kÕ quü ®¹o trong kh«ng gian §Ò c¸t, ®Ó ®iÒu
khiÓn ®−îc robot, ë mçi thêi ®iÓm ®Òu ph¶i t×m ®−îc nghiÖm cña bµi to¸n
®éng häc ng−îc. V× vËy yªu cÇu "n·o bé" cña robot (m¸y tÝnh) ph¶i thùc hiÖn
TS. Ph¹m §¨ng Ph−íc
95
Robot c«ng nghiÖp
mét khèi l−îng c¸c phÐp tÝnh khæng lå trong mét kho¶ng thêi gian rÊt ng¾n
(vµi chôc microgi©y) ®Ó ®¶m b¶o thêi gian thùc khi robot ho¹t ®éng. NÕu ta
kh«ng t×m c¸ch c¶i biÕn thiÕt kÕ quü ®¹o th× rÊt khã ®¶m b¶o yªu cÇu nÇy.
* VÝ dô vÒ thiÕt kÕ quü ®¹o CS:
ThiÕt kÕ quü ®¹o CS (Path with Cubic segment) cña khíp thø i ®i qua
hai ®iÓm nót cã gi¸ trÞ q0 vµ qf. Víi c¸c rµng buéc q0 = 0 ; q f = 0 . & &
Tõ c¸c c«ng thøc (8.2) . . . (8.5) ta x¸c ®Þnh c¸c hÖ sè cña ®a thøc bËc 3
nh− sau :
ai = q0 ; bi = 0;
3(q f − q 0 ) - 2(q f − q 0 )
ci = di =
Vµ
(t f − t 0 ) (t f − t 0 ) 3
2
Do vËy quü ®¹o qi(t) cã d¹ng nh− sau :
3(q f − q 0 ) 2(q f − q 0 )
q i (t) = q 0 + ( t − t0 ) 2 − ( t − t0 ) 3
(t f − t 0 ) (t f − t 0 )
2 3
6(q f − q 0 ) 6(q f − q 0 )
VËn tèc lµ : q i (t) = ( t − t0 ) − ( t − t0 ) 2
&
(t f − t 0 ) (t f − t 0 )
2 3
6(q f − q 0 ) 12(q f − q 0 )
&&i (t) = − ( t − t0 )
q
Vµ gia tèc lµ :
(t f − t 0 ) 2 (t f − t 0 ) 3
Trong vÝ dô trªn, gi¶ sö thêi gian t0 = 0 vµ tf = 1 gi©y, th× :
qi(t) = q0 + 3(qf - q0) t2 - 2(qf - q0) t3
qf
q(t) Quü ®¹o
q0
t
O tf
t0
q(t)
&
Tèc ®é
t
q0 = qf = 0
& &
tf
t0
6(q f − q 0 )
q(t)
&&
(t f − t 0 ) 2 Gia tèc
t
t0 tf
6(q f − q 0 )
−
(t f − t 0 ) 2
H×nh 8.3. ThiÕt kÕ quü ®¹o CS
TS. Ph¹m §¨ng Ph−íc
96
Robot c«ng nghiÖp
Tõ c¸c ph−¬ng tr×nh quü ®¹o, ph−¬ng tr×nh vËn tèc vµ ph−¬ng tr×nh gia
tèc ta x©y dùng ®−îc c¸c biÓu ®å ®Æc tÝnh chuyÓn ®éng cña khíp thø i trªn
®o¹n quü ®¹o thiÕt kÕ.
8.3. Quü ®¹o tuyÕn tÝnh víi cung ë hai ®Çu lµ parabol (LSPB) :
Khi yªu cÇu c«ng cô g¾n trªn kh©u chÊp hµnh cuèi cña robot chuyÓn
®éng víi vËn tèc ®Òu ®Æn, ta dïng quü ®¹o LSPB.
qi(t)
Parabol
v = constant
(q0+qf)/2
Parabol t
O tf/2 tf - tb tf
tb
t0
H×nh 8.3. Quü ®¹o LSPB.
C¸c ®iÒu kiÖn liªn tôc cña quü ®¹o nÇy thÓ hiÖn ë :
q(to) = q0 ; q(tf) = qf; vµ q(t0 ) = q(t f ) = 0
& &
vµ ®iÒu kiÖn c«ng nghÖ lµ v = constant.
Quü ®¹o ®−îc chia lµm 3 ®o¹n :
a/ Trong ®o¹n 1 : 0 ≤ t ≤ tb quü ®¹o Parabol cã d¹ng :
qi(t) = α + βt + γt2 (8.6)
α = q(t0) = q0
Khi t = 0 th× (8.7)
LÊy ®¹o hµm (8.6) : q(t) = β + 2γ t
& (8.8)
β = q(to ) = 0
&
Khi t = 0 th×
T¹i thêi ®iÓm tb ta cÇn cã vËn tèc b»ng h»ng sè vËn tèc cho tr−íc v :
γ = v/2tb
Nªn khi t = tb
§Æt v/tb = a ⇒ γ = a/2 vµ quü ®¹o cã d¹ng :
( 0 ≤ t ≤ t b)
qi(t) = q0 + at2/2 (8.9)
b/ Trong ®o¹n 2 : [tb, (tf-tb)] quü ®¹o tuyÕn tÝnh cã d¹ng :
qi(t) = α0 + vt
(q + q f )
t
Do tÝnh ®èi xøng : q( f ) = 0
2 2
(q 0 + q f ) t
= α0 + v f
Suy ra
2 2
(q 0 + q f − vt f )
α0 =
VËy
2
Ph−¬ng tr×nh quü ®¹o tuyÕn tÝnh sÏ lµ :
TS. Ph¹m §¨ng Ph−íc
97
Robot c«ng nghiÖp
q f + q 0 − vt f
q i (t) = + vt (8.10)
2
Tõ ®iÒu kiÖn liªn tôc vÒ vÞ trÝ, t¹i thêi ®iÓm tb ta cã :
at 2 q f + q 0 − vt f
q0 + b = + vt b
2 2
Rót ra :
q − q f + vt f
tb = 0
v
Víi ®iÒu kiÖn tån t¹i : 0 < tb ≤ tf/2, dÉn ®Õn :
qf − q0 2(q f − q 0 )
< tf ≤
v v
§iÒu nÇy x¸c ®Þnh vËn tèc ph¶i n»m gi÷a c¸c giíi h¹n trªn, nÕu kh«ng
chuyÓn ®éng sÏ kh«ng thùc hiÖn ®−îc.
VÒ mÆt vËt lý :
NÕu tf > (qf - q0) / v vµ tf ≤ 2(qf - q0) / v qf
th× : v > (qf - q0) / tf vµ v ≤ 2(qf - q0) / tf.
θ
q0
NghÜa lµ tgθ < v ≤ tg2θ. t0 tf
c/ Trong ®o¹n 3 : (tf - tb) ≤ t ≤ tf quü ®¹o Parabol cã d¹ng :
at f2 a
q i (t) = q f − + at f t − t 2 (8.11)
2 2
Tõ c¸c ph−¬ng tr×nh (8.9)...(8.11) ta x©y dùng ®Æc tÝnh chuyÓn ®éng
theo quü ®¹o LSPB cña khíp qi nh− sau :
qi(t); q (t); q (t)
& &&
i i
qf
q0 t
tb tf
t0 tf-tb
v = const q (t)
&
i
t
tb tf-tb tf
t0
q (t)
&& t
i
tf-tb tf
tb
t0
H×nh 8.4 : §Æc tÝnh quü ®¹o LSPB
TS. Ph¹m §¨ng Ph−íc
98
Robot c«ng nghiÖp
8.4. Quü ®¹o Bang Bang Parabolic blend (BBPB) :
Nh− ®· tr×nh bµy ë trªn, ®©y lµ tr−êng hîp ®Æc biÖt cña quü ®¹o LSPB
khi ®o¹n tuyÕn tÝnh thu vÒ 0.
at 2
tf
0≤t≤
Víi : qi(t) = q0 +
2 2
q − q 0 at 2
tf
≤ t ≤ tf t-
qi(t) = 2q0 - qf +2a f
vµ víi
a 2
2
§å thÞ ®Æc tÝnh cña quü ®¹o nÇy nh− sau :
qi(t)
qf
q0 t
t0 tf/2 tf
q (t)
&
i
Vmax
t
t0 tf/2 tf
&& (t)
q
i
t
tf/2
tf
t0
H×nh 8.5. §Æc tÝnh quü ®¹o BBPB
=======================
TS. Ph¹m §¨ng Ph−íc
