DYNAMIC MODEL BASED PREDICTIVE CONTROL FOR MOBILE ROBOTS

Transcription

1 XII Rnón rjo n Procsmnto l Informcón y Control, 6 l 8 octr 27 DYNAMIC MODEL BASED PREDICIVE CONROL FOR MOBILE ROBOS Anrés Rosls, Mgl Pñ, Gstvo Scgl, Vcnt Mt, Frnno Scsco Insttto Atomátc (INAU. Unvrs Nconl Sn Jn Av. Lrtor Sn Mrtín 9 (ost. J54ARL. Sn Jn, Argntn -ml:{rosls,gscgl,vmt,frnno,slwnsk}@nt.nsj..r Astrct A nonlnr prctv controllr (NMPC s vlop to control ncycl-lk mol root for trjctory trckng. Dynmc mol of PIONNER 3-DX mol root s s, whr trnl forcs n whls slng hv n consr. Rstrctons on control ctons n systm stts r lso consr. Smltons rslts t oth trckng n rglton (postonng r shown, ths rslts show th goo prformnc of vlop controllr. Fnlly, ths ppr shows tht th controllr cn mplmnt n rl-tm y sng n nlyss of compt tms of NMPC lgorthm. Ky wors mol root, prctv nonlnr control (NMPC, trjctory trckng, ynmc mol I. INRODUCION h trjctory trckng control for mol roots s fnmntl prolm, whch hs n nvstgt hstvly y th scntfc commnty. Svrl pprs l ot th sgn of control lws for mol root wth ts ynmc mol, for nstnc n trjctory trckng (D l Crz t l., 26; Dong t l., 25; Algl t l., 24; Yng t l., 999; Zhng t l., 998. On of frst nvstgton rslts for ths prolm ws l n Knym t l. (99, whr thor ss th Lypnov thory to sgn th trckng controllr. Nvrthlss, ths n othrs controllrs o not tk n cont th rstrctons n th control sgnls cs t s hr tsk to mplmnt. Nonlnr prctv control (NMPC s most frqntly control-optmzton tchnq s n nstry. hs mthoology hs n sgn to l wth optmzton prolms wth rstrctons. NMPC s n optmzton lgorthm onln to prct otpt systm s on crrnt stts n mol systm, n lso, to fn control ftr n opn loop y sng nmrc optmzton n to pply th frst control sgnl of ths optmz ftr to systm. In mol s prctv control to s th rcng horzon, th stlty nlyss s on of th mn prolms (Pñ, 22. Prvos works hv monstrt tht fnt rcng horzon cn grnt NMPC stlty vn for nonlnr systms (Cmcho t l., 998; Pñ, 22, lthogh ths strtgy s consr hvy to ts compttonl ffort n prctc. h NMPC stlty nlyss wth fnt rcng horzon hs n st n Myn t l. (2 n Fonts (2. Rcntly, th qlts of prctv control hv n plor n ppl n rootcs, woks lk Dongng t l. (26, Hjr t l. (25, Künh t l. (25 n Rmírz t l. (999 show ffrnt pprochs n strtgs n MPC wth goo rslts for trckng control n rglton. hs ppr ppls NMPC to ynmc mol of mol root n mks n nlyss y compts tms for optmzton lgorthm so tht to l to mplmnt th controllr n prmntl works. h ppr s orgnz s follows: Scton II scrs th NMPC lgorthm n th progrmmng schms s. Scton III shows th ynmc mol of mol root. Scton IV scrs th smlton rslts. Conclsons n ftr works r tl n lst Scton. II. PREDICIVE NONLINEAR CONROL A gnrl trmnstc nonlnr mol t scrt tm for systm cn prss lk, ( k + = ( ( k, ( k ( k = ( ( k f v ( y h (2 whr, (k, v(k n y(k r th stt vctor, sgnl control n otpt systm rspctvly. h most of MPC mthos r s on common schm (Rmírz t l., 999. A cost fnctonl J s fn, whch s oftn qrtc fncton wth th sm of th norms of ftr trjctory trckng rrors, ( k+ k = ( k+ k ( k+ y y (3 prct ovr prcton horzon N pls th sm of th norms of prct ncrmnts n th control cton, ovr control horzon N, N N 2 2 δ ( λ ( Q = = Δ v( k + k = v( k + k v ( k + k J = k+ k + Δ v k+ k (4 R

2 XII Rnón rjo n Procsmnto l Informcón y Control, 6 l 8 octr 27 whr, δ n λ r pnlty sqncs slly chosn s constnts, y (k+ s th sr otpt n th notton y(k+ k mns tht y(k+ s compt wth known nformton t nstnt k. Ftr systm otpts y(k+ k for =,, N, r prct y sng procss mol snc th prvos otpts n npts t nstnt k, n snc th ftr prct control ctons v(k+ k for =,, N -, whch r prtn to compt. Also, 2 = Q n Q > (Pñ, 22. Q On ths wy J cn prss lk fncton tht only pns on ftr control ctons. h prctv control ojctv s to otn sqnc of ftr control ctons [v(k, v(k+ k,, v(k+n - k], so tht prct otpts y(k+ k, sng th systm mol, r so nr of rfrnc y (k+ k s t s possl, long th prcton horzon. hs s otn y mns of mnmzton of J rspct to control vrls. Aftr otnng ths sqnc, rcng horzon strtgy s s, whch ntls pplyng only th frst control cton v(k compt. hs procss s rpt vry smplng tm. Whn nonlnr mols r s, MPC pns to fn solton for nonlnr progrmmng prolm n vry smplng stp. o solv tht prolm s ncssry ong th optmzton n solvng th systm mol. Both prolms cn mplmnt y two ffrnt wys: sqntl or smltnosly (Pñ, 22. A. Sqntl optmzton lgorthm In th sqntl mplmntton, solton t vry trton for th optmzton rotn s fon. Controls r th cson vrls, whch gos nto lgorthm n ths compts th mol solton. hn, ths solton s s to vlt th ojctv fncton n th compt vl s gvn to th optmzton progrm. h optmzton vrl s, ( k ( k k ( k N k L (5 z = v v + v + Frst, fnctonl mst solv th systm mol wth vctor z vls n wth crrnt stt (k ppl N tms th Eq. (. On ths wy, th vctor sqnc ( k + k ( k + 2 k L ( k + N k s got, whrs wth Eq. (2 th otpt vls sqnc y( k + k y( k + 2 k L y( k + N k s otn. hn, th cost fnctonl (Eq. (4 s vlt wth ths vls. h cost fnctonl J pns on prct otpts, whch r s on stt vctor s wll n ths vctor s fncton of control ctons (optmzton vrls. hrfor, rslts to otn th fnctonl grnt, otpts hv to rv rspct control ctons from k to (k+n -. hs s complct n not lwys hs solton. hrfor, th sqntl rsolton hs not grnt nformton n t mst otn y sng nmrc ffrntton, whch s compttonlly ngtv, cs ths gnrts grtst clcls cost n convrgnc prolms. B. Smltnos optmzton lgorthm Unlk sqntl solton, smltnos solton n optmzton ncls stts n controls of mol lk cson vrls. Mol qtons r to th optmzton prolm lk rstrcton qtons. hn, th optmzton vrl rslts, ( k ( k k L ( k N k ( k ( k+ k L ( k+ N k z = v v v n othr wors, th stts n control ctons r consr lk optmzton vrls. h mnson of ths vctor s (N + pn, whch s ggr thn sqntl pproch mnson (pn, whr n p r szs of stt vctor n control npt, rspctvly. hs ntls consrl ncrs n th optmzton vrl sz n rlton to sqntl pproch. Mol qtons ppr lk qlty rstrctons s follows n Eq. (6, ( k k ( ( k, ( k ( k 2 k ( ( k, ( k + = f v + = f + v + R = M f v ( k + N k = ( ( k + N, ( k + N In ths pproch, th grnt otnng y nlytc wy s smplst, so tht, th optmzton lgorthm s l to ncorport n n plct wy. By sng th fnctonl n Eq. (4 rslts Eq. (7. h qlty rstrctons grnt s sprs mtr (Eq. (8. ( ( k+ k ( k+ ( ( k+ k ( k+ 2δG Q h y k+ 2δ G Q h y k+ 2 M J 2δ N ( ( k N k ( k N k N = G Q h + y + + 2λRΔv( k 2λ2RΔ v( k+ 2λ2RΔ v( k+ 2λ3RΔ v( k+ 2 M 2λN h G = RΔ v( k+ N ( ( k, v( k ( k ( k k= (6 (6 (7 I F + L I L M M O M M L I F( k+ N (8 R = L I Fv ( k L Fv ( k + L M M O M M L Fv ( k+ N

3 XII Rnón rjo n Procsmnto l Informcón y Control, 6 l 8 octr 27 f whr, ( ( k, v( k f F =, ( ( k, v( k F =, k+ vk+ ( k v ( k k= k= I s th ntty mtr wth mnson n p s nll mtr wth p mnson. In smll prolms wth fw stts n short prcton horzon, sqntl pproch s proly mor ffctv (Pñ, 22. Gnrlly, n g prolms smltnos pproch s mor rost, cs t s lss prol tht t fls. In sqntl pproch th ton of rstrctons n stts or otpts s mor complct. In ton to mol rstrctons, control ctons rstrctons, sty stt rstrctons, tc, cn. III. DYNAMIC MODEL OF MOBILE ROBO o l to s MPC strtgy s ncssry hvng mol root mol. hs mol wll s to prct ftr poston n orntton of controll systm. A ncycl-lk mol root prmntlly vlt y sng PIONNER 3-DX roots n D l Crz t l. (26 wll s. A rf mol s show n Fg. n s prsnt n Eq. (9. h mol wth mor tls cn sn on D l Crz t l. (26. h root poston s fn y h= [, y], ths pont s loct stnc from rr l cntr of th root, n ū r th longtnl n s sps of mss cntr, ω s th nglr sp n ψ s th orntton ngl of th root, G s th grvty cntr, B s th s ln cntr of th whls, E s th work tool plcng pont n C s th cstor whl plcng pont. cosψ ωsn ψ & sn ψ+ ωcosψ y & ω c ψ& = θ3 2 θ4 + ω c ω & θ θ θ ω& θ5 θ6 ω ω θ 2 θ2 θ2 (9 whr, th mol root prmtrs, vlts n D l Crz t l. (26 r, θ =.2489 θ 2 =.2424 θ 3 =.9363 θ = θ = θ = Eqton (9 cn wrot n compct wy s follows, whr, ( t = [ y ψ ] systm n ( = [ ω ] & ( t = f( ( t + g( v( t ( ω s th stt vctor of v t c c s th control on. Dynmc mol of mol root cn scrtz y sng ny nmrc ppromton pproch, for mpl Elr, n othr wors, k+ = k + o f ( k + g ( v k ( whr, o s th smplng tm n th vctor of ntl contons s ( t = [ y ψ ω ]. Eqton (2 s th otpt systm, ( k = ( ( k = ( k y h C (2 whr, C s mtr o mnson (o s th stt vctor sz. In ths ppr th otpt qton s gvn y, ū E C y ( k = ( k y( k ψ( k (3 y B ω G Fgr : Mol root ncycl-lk mol n ts prmtrs From mol n Fg., th ynmc mol of mol root s otnng (D l Crz t l., 26, h ψ IV. SIMULAION AND RESULS h mml solt vls of lnr n nglr sps of mol root s n smltons r.5[m/s] n.745[r/s], rspctvly. A smplng tm o =.[s] ws s for ll trjctors n th prcton horzon s ws N = N = 7. Frthrmor, mtr Q = g[,,.5], mtr R = I n prmtrs δ = 28 n λ =.8. Lnr rfrnc sp ws,25[m/s] for oth rfrnc trjctors. Rfrnc s vls for stt ψ, wr compt y sng Eq. (4, =tn y& ψ & (4

4 XII Rnón rjo n Procsmnto l Informcón y Control, 6 l 8 octr 27 Frst rfrnc trjctory ppl to th systm ws n ght-shp crv fn y, Vloc Lnl [m/s] c = sn ( 2, y = k cos( k t k r k t ω ω.5 rl + whr, k, k ω R n n ths cs k = n k ω =.. h ntl contons wr ( t = [ ] In Fg. 2 t s shown th trjctory scrs y mol root to follow th frst rfrnc, mmm rror n ths cs ws 3[mm]. Vloc Anglr [r/s] w c w rl Poscon n Y [m] ncl fnl s rl Poscon n X [m] Fgr 2: Eght-shp trjctory for mol root Fgr 3 shows -y mol root poston n qrtc rror grphc, ths rror tns to zro. Poscon [m] Error,y [m] rr rr y mpo [s] y y Fgr 3: -y mol root stts ( n qrtc rror ( mpo [s] Fgr 4: Control ctons ( c y ( w c mpo l Algortmo [s] mpo l Algortmo [s] Proos Mstro [no] Fgr 5: Algorthm cton tm rng smlton ( sqntl (97% < o, ( smltnos (72% < o Scon trjctory ws ppl to chck th postonng control of systm, ( ω t π 2rcos r + 4, = r 2, Poscon n Y [m] ( rsn 2ωr t+ π 4, t < 5π y = r, t 5π Fgr 4 shows control ctons of systm compr wth rl lnr n nglr sps of mol root. Fgrs 5( n 5( show th cton tm of optmzton lgorthm for ch smplng tm n th smltons. Sqntl lgorthm s shown n Fg. 5(, whr 97% of cton tms r smllr thn o, whrs smltnos lgorthm s shown n Fg. 5(, whr 72% of cton tms r smllr thn o ncl fnl s rl Poscon n X [m] Fgr 6: Postonng trjctory for mol root

5 XII Rnón rjo n Procsmnto l Informcón y Control, 6 l 8 octr 27 + whr, r R. In ths cs r =.4[m] n ω r =.[r/s], wth ntl contons: ( t = [ π ] In Fg. 6, t s shown th trjctory scrs y mol root for frst trckng pth n nt postonng n spcfc plc. mpo l Algortmo [s] Poscon [m] y y mpo l Algortmo [s] Error,y [m].5 rr rr y mpo [s] Fgr 7: -y mol root stts ( n qrtc rror ( From Fg. 7, t cn notc th -y poston of mol root n th qrtc rror grphc, ths rror tns to zro. Vloc Lnl [m/s] Vloc Anglr [r/s] mpo [s] Fgr 8: Control ctons ( c y ( w c c rl w c w rl Fgr 8 shows control ctons of systm compr wth rl lnr n nglr sps of mol root. It s osrv tht t th momnt whn mol root rrvs to sr poston, t s sty thr wth nll sp. From Fgs. 9( n 9(, t s shown th cton tm of optmzton lgorthm for ch smplng tm n th smltons. Sqntl lgorthm s shown n Fg. 9(, whr 93% of cton tms r smllr thn o, whrs smltnos lgorthm s shown n Fg. 9(, whr 8% of cton tms r smllr thn o Proos Mstro [no] Fgr 9: Algorthm cton tm rng smlton ( sqntl (93% < o, ( smltnos (8% < o V. CONCLUSIONS h prolm to l mol root throgh prvos compt trjctors hs n solv y sng NMPC strtgy lk nvgton lgorthm. Dynmc holonomc mol of ncycl-lk mol root hs n s; ths mol ws vlt y D l Crz t l. (26. A goo control systm prformnc, y mns NMPC hs n chv, oth postonng n trjctory trckng. Rslts wr otn y sng th NMPC sqntl pproch, cs compt tms wth ths lgorthm wr ttr thn th NMPC smltnos on, s t s osrv n th smltons. Propos controllr s l to mplmnt prmntlly s t s shown n th nlyss of rslts. By sng soptml schm, goo rslts hv n chv; ths provs tht wth spcfc softwr for ths prolm, t s possl to rch ttr rslts. Smltons show tht th most of NMPC compt tms, r low th rngs ccpt y mol root (o =.[s], ths smltons wr on y sng MALAB Optmzton oolo. Frthrmor, prlmnry compt tms r hghst to th lgorthm strts from stnt ntl contons to th prolm optmm. Ftr works wll ntl mor rost nlyss consrng nonzro ncrtn vctor n, n ton, prmntl pplctons wll m n PIONNER mol roots wth collson vonc tchnqs. ACKNOWLEDGEMENS hs work ws prtlly fn y th Consjo Nconl Invstgcons Cntífcs y écncs (CONICE - Ntonl Concl for Scntfc Rsrch, Argntn n y th Srvco Almán Intrcmo Acémco (DAAD Dtschr Akmschr Astsch Dnts.

6 XII Rnón rjo n Procsmnto l Informcón y Control, 6 l 8 octr 27 REFERENCES Algl A. y Why, Dynmc Molng n Aptv rcton Control for Mol Roots, Annl Confrnc IEEE IES, pp , (24. Cmcho E. y Borons C., Mol Prctv Control n th Procss Instry, Sprngr-Vrlg, (998. D l Crz C. y Crll R., Lnlzcón con Rlmntcón l Molo Dnámco n Root Móvl y Control Sgmnto ryctor, AADECA, (26. Dong W. y Khnrt K., Rost Aptv Control of Nonholonomc Mol Root wth Prmtr n Nonprmtr Uncrtnts, IEEE rnsctons on Rootcs, pp , (25. Dongng G. y Hoshng H., Rcng Horzon rckng Control of Whl mol Roots, Control Systms chnology, vol. 4, pp , (26. F.A.C.C. Fonts, A Gnrl Frmwork to Dsgn Stlzng Nonlnr Mol Prctv Controllrs, Sst. Control Ltt., pp , (2. Hjr R., om R., Bochr P. y Dmr D., Fnt Horzon Nonlnr Prctv Control y th ylor Appromton: Applcton to Root rckng rjctory, Int. J. Appl. Mth. Comp. Sc., vol. 5, pp , (25. Knym Y., Kmr Y., Myzk F. y Nogch., A Stl rckng Control Mtho for n Atonomos Mol Root, Proc. IEEE ICRA, pp , (99. Künh F., Goms J. y Fttr W., Mol Root rjctory rckng sng Mol Prctv Control, II IEEE LARS, (25. Myn D., Rwlngs J., Ro C. y Scokrt P., Constrn Mol Prctv Control: Stlty n Optmlty, Atomtc, pp , (2. Pñ M., Control so n Molos Borrosos, ss Doctoro INAU UNSJ, (22. Rmírz D., Lmón-Mrro D., Gómz-Ortg J. y E. Cmcho, Aplccón l Control Prctvo so n Molo No Lnl l Nvgcón n Root Móvl tlzno Algortmos Gnétcos, Métoos Nmércos n Ingnrí, (999. Scch H., Control Vhíclos Atogos con Rlmntcón Snsorl, ss Mstrí INAU UNSJ, (998. Yng J. y Km H., Slng Mo Control for ryctory rckng of Nonholonomc Whl Mol Roots, IEEE rnsctons on Rootcs n Atomton, pp , (999. Zhng Y., Hong D., Chng J. y Vlnsky S., Dynmc Mol Bs Rost rckng Control of Dffrntlly Str Whl Mol Root, Procngs of th Amrcn Control Confrnc, pp , (998.