Robot cng nghip

92

Chng VIII Thit k qu o robot. (Trajectory Planing) Trong cc ng dng cng nghip ca robot, ta thng gp hai trng hp sau : Trng hp 1 : Khu chp hnh cui ca robot ch cn t c v tr v hng ti cc im nt (im ta : Knot point). y chnh l phng php iu khin im (PTP). Ti , bn tay robot thc hin cc thao tc cm nm i tng hoc bung nh i tng. y l trng hp ca cc robot thc hin cng vic vn chuyn v trao i phi liu trong mt h thng t ng linh hot robot ho. Bn tay robot khng trc tip tham gia vo cc nguyn cng cng ngh nh hn, ct kim loi ... Cc im nt l mc tiu quan trng nht, cn dng ng i ti cc im nt l vn th yu. Trong trng hp ny Robot thng c lp trnh bng phng php dy hc (Teach and playback mode). Trong trng hp ny khng cn tnh ton phng trnh ng hc hoc ng hc ngc robot, chuyn ng mong mun c ghi li nh mt tp hp cc gc khp (thc t l tp hp cc gi tr m ho ca bin khp) robot thc hin li (Playback) khi lm vic. Trng hp 2 : Khu chp hnh cui ca robot phi xc nh ng i qua cc im nt theo thi gian thc. l trng hp cc tay my trc tip thc hin cc nguyn cng cng ngh nh sn, hn, ct kim loi ... Vn thit k qu o cho cc robot trong trng hp ny l rt quan trng. N quyt nh trc tip cht lng thc hin cc nguyn cng cng ngh m robot m nhn. Trong chng ny, chng ta cp n bi ton thit k qu o vi mt s qu o in hnh. Cc qu o ny khng ch c ngha trong trng hp ng dng th hai m n bao hm mt ngha chung cho mi robot, v ngay c trng hp n gin nh cc robot thuc ng dng th nht cng thc hin nhng chuyn ng qu o c bn m chng ta s nghin cu di y. 8.1. Cc khi nim v qu o robot : xc nh c ng i mong mun ca robot theo thi gian, qu o c th c tnh ton thit k trong mt h to truyn thng Oxyz (Cartesian Space) hoc thit k trong khng gian bin khp (khng gian trng vect cc to suy rng ca robot), chng hn vi robot 6 bc t do th X = [1 , 2 , 3 , 4 . 5 , 6 ] T . Thit k qu o y c hiu l xc nh qui lut chuyn ng ca cc bin khp iu khin chuyn ng ca tng khp v tng hp thnh chuyn ng chung ca robot theo mt qu o c xc nh.
TS. Phm ng Phc

Robot cng nghip

93

Qu o cn thit k nht thit phi i qua mt s im nt cho trc (t nht l im u v im cui). Ngoi cc im nt chnh, ta cn c th chn thm cc im nt ph gi l im dn hng (via point) trnh cc chng ngi vt. Khi thit k qu o trong khng gian bin khp, ti mi im nt phi xc nh gi tr ca cc bin khp bng phng php tnh ton ng hc ngc. Thi gian yu cu ca mi on qu o (gia 2 im nt) l ging nhau cho tt c cc khp v vy yu cu tt c cc khp phi t n im nt ng thi. Ngoi vic yu cu thi gian phi ging nhau cho cc khp, vic xc nh cc hm qu o ca mi bin khp khng ph thuc vo cc hm ca cc khp khc. V vy vic thit k qu o trong khng gian bin khp n gin v d tnh ton hn khi m t trong h to cc. Qu o thit k phi m bo cc iu kin lin tc (continous conditions) bao gm : + Lin tc v v tr (Position) + Lin tc v tc (Velocity) + Lin tc v gia tc (Acceleration).
x(t) x2 x1 xf-1 xo xf to t1 t2 tf-1 tf qi(t2)...
Cc im nt

Hnh 8.1. Tnh lin tc ca qu o robot. thit k qu o robot, ngi ta thng dng phng php xp x cc a thc bc n, cc qu o thng gp l : + Qu o CS (Cubic Segment) : Tng ng a thc bc 3; + Qu o LS (linear Segment) : Tng ng a thc bc 1; + Qu o LSPB (Linear Segment with Parabolic Blend) : Phi hp a thc bc 2 vi a thc bc 1.
q0
on thng

q2

q1

qf

ng cong bc 2

Hnh 8.2 : Qu o LSPB

TS. Phm ng Phc

Robot cng nghip

94

+ Qu o BBPB (Bang Bang Parabolic Blend) : l trng hp c bit ca qu o LSPB khi on tuyn tnh thu v bng 0 v xut hin im un.
qf q0

Hnh 8.2 : Qu o BBPB Nu cho trc nhiu im nt, ta c th p dng nhiu dng qu o c bn khc nhau cho mt bin khp. 8.2. Qu o a thc bc 3 : Khi thit k qu o robot theo a thc bc 3 qua cc im nt, mi on qu o gia hai im nt s c biu din bng mt phng trnh bc 3 ring bit. Qu o a thc bc 3 m bo s lin tc ca o hm bc nht v bc hai ti cc im nt. Ti thi im tk t tk+1, qu o xp x a thc bc 3 ca bin khp th i l qi(t) c dng : (8.1) qi(t) = ai + bi(t - tk) + ci(t - tk)2 + di(t - tk)3
qi(t)

Vi cc rng buc : & & qi(tk) = qk v q i (t k ) = q k & & qi(tk+1) = qk+1 v q i (t k +1 ) = q k +1

qk+1 qk
Bc 3

tk+1 tk (8.2) T (8.1) ta thy : t = tk ai = qk Ly o hm ca (8.1) theo t, ta c : & q i (t) = b i + 2c i (t t k ) + 3d i (t t k ) 2 & (8.3) Ti : t = tk b i = q k Ti 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 Cc phng trnh (8.4) v (8.5) nhn c khi gii (8.1) ... (8.3). Tnh lin tc ca vn tc l s m bo cho qu o khng gp khc, git cc, gy sc trong qu trnh hot ng ca robot. Vn tc v gia tc ti im cui ca mt on ng cong bc 3 chnh bng vn tc v gia tc ca on cong bc 3 tip theo. Cn ch rng khi thit k qu o trong khng gian ct, iu khin c robot, mi thi im u phi tm c nghim ca bi ton ng hc ngc. V vy yu cu "no b" ca robot (my tnh) phi thc hin
TS. Phm ng Phc

Robot cng nghip

95

mt khi lng cc php tnh khng l trong mt khong thi gian rt ngn (vi chc microgiy) m bo thi gian thc khi robot hot ng. Nu ta khng tm cch ci bin thit k qu o th rt kh m bo yu cu ny. * V d v thit k qu o CS: Thit k qu o CS (Path with Cubic segment) ca khp th i i qua & & hai im nt c gi tr q0 v qf. Vi cc rng buc q0 = 0 ; q f = 0 . T cc cng thc (8.2) . . . (8.5) ta xc nh cc h s ca a thc bc 3 nh sau : ai = q0 ; bi = 0; 3(q f q 0 ) - 2(q f q 0 ) ci = di = V 2 (t f t 0 ) (t f t 0 ) 3 Do vy qu o qi(t) c dng nh sau : 3(q f q 0 ) 2(q f q 0 ) q i (t) = q 0 + ( t t0 ) 2 ( t t0 ) 3 3 2 (t f t 0 ) (t f t 0 ) 6(q f q 0 ) 6(q f q 0 ) & ( t t0 ) ( t t0 ) 2 Vn tc l : q i (t) = 3 2 (t f t 0 ) (t f t 0 ) 6(q f q 0 ) 12(q f q 0 ) &&i (t) = q ( t t0 ) V gia tc l : (t f t 0 ) 2 (t f t 0 ) 3 Trong v d trn, gi s thi gian t0 = 0 v tf = 1 giy, th : qi(t) = q0 + 3(qf - q0) t2 - 2(qf - q0) t3
qf
q(t)

Qu o t tf

q0 O t0
& q(t)

Tc tf

& & q0 = qf = 0

t0
&& q(t)

6(q f q 0 ) (t f t 0 ) 2
t0

Gia tc t tf

6(q f q 0 ) (t f t 0 ) 2

Hnh 8.3. Thit k qu o CS

TS. Phm ng Phc

Robot cng nghip

96

T cc phng trnh qu o, phng trnh vn tc v phng trnh gia tc ta xy dng c cc biu c tnh chuyn ng ca khp th i trn on qu o thit k. 8.3. Qu o tuyn tnh vi cung hai u l parabol (LSPB) : Khi yu cu cng c gn trn khu chp hnh cui ca robot chuyn ng vi vn tc u n, ta dng qu o LSPB.
qi(t)
Parabol

v = constant (q0+qf)/2
Parabol O

t t0 tb tf/2 tf - tb tf

Hnh 8.3. Qu o LSPB. Cc iu kin lin tc ca qu o ny th hin : & & q(to) = q0 ; q(tf) = qf; v q(t0 ) = q(t f ) = 0 v iu kin cng ngh l v = constant. Qu o c chia lm 3 on : a/ Trong on 1 : 0 t tb qu o Parabol c dng : (8.6) qi(t) = + t + t2 = q(t0) = q0 (8.7) Khi t = 0 th & (8.8) Ly o hm (8.6) : q(t) = + 2 t & Khi t = 0 th = q(to ) = 0 Ti thi im tb ta cn c vn tc bng hng s vn tc cho trc v : = v/2tb Nn khi t = tb t v/tb = a = a/2 v qu o c dng : (0 t tb) (8.9) qi(t) = q0 + at2/2 b/ Trong on 2 : [tb, (tf-tb)] qu o tuyn tnh c dng : qi(t) = 0 + vt t (q + q f ) Do tnh i xng : q( f ) = 0 2 2 (q 0 + q f ) t Suy ra = 0 + v f 2 2 (q 0 + q f vt f ) 0 = Vy 2 Phng trnh qu o tuyn tnh s l :
TS. Phm ng Phc

Robot cng nghip

97

q f + q 0 vt f + vt 2 T iu kin lin tc v v tr, ti thi im tb ta c : at 2 q f + q 0 vt f q0 + b = + vt b 2 2 Rt ra : q q f + vt f tb = 0 v Vi iu kin tn ti : 0 < tb tf/2, dn n : q i (t) =

(8.10)

qf q0 2(q f q 0 ) < tf v v iu ny xc nh vn tc phi nm gia cc gii hn trn, nu khng chuyn ng s khng thc hin c. V mt vt l : Nu tf > (qf - q0) / v v tf 2(qf - q0) / v qf th : v > (qf - q0) / tf v v 2(qf - q0) / tf.
Ngha l tg < v tg2.
q0

t0

tf

c/ Trong on 3 : (tf - tb) t tf qu o Parabol c dng : at f2 a q i (t) = q f + at f t t 2 (8.11) 2 2 T cc phng trnh (8.9)...(8.11) ta xy dng c tnh chuyn ng theo qu o LSPB ca khp qi nh sau :
qf q0 t0 tb tf-tb v = const tf & && qi(t); q (t); q (t) i i

& q (t) i tf && q (t) i t

t0

tb

tf-tb

t0

tb

tf-tb

tf

Hnh 8.4 : c tnh qu o LSPB

TS. Phm ng Phc

Robot cng nghip

98

8.4. Qu o Bang Bang Parabolic blend (BBPB) : Nh trnh by trn, y l trng hp c bit ca qu o LSPB khi on tuyn tnh thu v 0. tf at 2 qi(t) = q0 + Vi : 0t 2 2 q q 0 at 2 tf t tf qi(t) = 2q0 - qf +2a f v vi t2 a 2 th c tnh ca qu o ny nh sau :
qi(t) qf q0 t0
& q (t) i

t tf/2 Vmax t tf

t0
&& (t) q i

tf/2

tf t tf

t0

tf/2

Hnh 8.5. c tnh qu o BBPB

=======================

TS. Phm ng Phc