Path following, real-time, embedded fuzzy control of a mobile platform Pioneer 3-DX

Transcription

1 8th WSEAS Internatonal Conference on SYSTEMS THEORY and SCIENTIFIC COMPUTATION (ISTASC 08 Rhodes, Greece, August 0-, 008 Path followng, real-tme, embedded fuzz control of a moble platform Poneer -DX A. FILIPESCU, I. SUSNEA, AL. STANCU, G. STAMATESCU Department of Automaton and Industral Informatcs Unverst Dunarea de Jos of Galat Domneasca, 47, Galat, Facult of Automaton and Computers Poltechnc Unverst of Bucharest Splaul Independente,, Bucharest, ROMANIA Abstract: - Ths paper presents a mcrocontroller mplementaton of a fuzz logc control algorthm appled to the moble platform. As moble platform, Poneer -DX has been chosen. A robotc manpulator, Poneer 5- DOF Arm s on the platform. Poneer -DX s a wheeled moble robot (WMR wth two drvng wheels and a caster wheel. The desgn and mplementaton of the fuzz controller s descrbed. Both speed and poston are acheved usng the same real-tme fuzz controller. The descrpton starts wth an eample for a crcular path followng, then presents an mplementaton of the algorthms for followng a generalzed curve path. Ke-Words: - Poneer -DX moble platform, Embedded real-tme fuzz control, Wheeled moble robot, Path followng. Wh fuzz control? Fuzz logc s now wdel used to solve some of the most mportant problems n moble robots and autonomous vehcle control, such as trajector trackng, or obstacle avodance. However, the vast majort of the estng mplementatons, count on powerful computng equpment, far beond the capabltes of a smple mcrocontroller (see, for eample, Flpescu, A.; Dugard, L.; Don, J.-M., 00 [], or Thongcha S. and Kawamura K., 000 []. Whle the theoretcal aspects of ths topc are ver well covered b the estng lterature (Susnea I, Mtescu M. [], or Ahmad I. [4], relatvel few papers descrbe practcal fuzz solutons for embedded sstems, and these are manl based on DSPs (see Baturone I, Moreno-Velo F.J., Sanchez- Solano S., Blanco V. [5]. Therefore, we have focused on mplementng advanced control algorthms on low-cost embedded devces, bult around smple mcrocontroller structures. Ths algorthm descrbed n ths paper was actuall mplemented and tested on a smple, 8- bt mcrocontroller, attached to the Poneer-DX robot (fg.. The paper s structured as follows: Secton presents the structure of the Poneer -DX moble robot and addtonal hardware. It ncludes the knematc varables assocated wth the WMR dscrete. Secton descrbes the communcaton protocol. The knematc model of the Poneer -D s presented n Secton 4. In Secton 5, the fuzz controller s presented. Closed-loop, real-tme control result are presented n Secton 6. The generalzaton of the soluton s gven n Secton 7, whle conclusons and some research drectons are presented n Secton 8. The structure of the moble robot Poneer -DX and addtonal hardware PDX s a wheeled moble robot (WMR, wth two ndependent drve wheels, plus an addtonal castor wheel for stablt, as shown n fg.. The knematc varables assocated wth the WMR are presented n fg.. An nternal mcrocontroller montors the poston nformaton provded b two encoders, and generates PWM commands, whch are amplfed and appled ndependentl to the drve wheels, as shown n fg.. The two ndependent control loops are governed b the same PID algorthm. ISSN: ISBN:

2 8th WSEAS Internatonal Conference on SYSTEMS THEORY and SCIENTIFIC COMPUTATION (ISTASC 08 Rhodes, Greece, August 0-, 008 Addtonal crcuts (not shown n fg. are related to the sonar sstem, whch allows obstacle detecton. All the communcaton wth the outsde world s performed va a seral RS communcaton nterface, accordng to a propretar protocol. Rs MCU PWM out PWM out Left motor poston feedback Left amplfer Rght amplfer Left drve Rght drve Left motor encoder Rght motor encoder Rght motor poston feedback Fg. The smplfed structure of Poneer-DX moble platform Rs Embedded controller and communcaton devce Rs Rado modem Fg. Poneer -DX WMR wth Poneer 5-DOF Arm Y b V L (, V V R Fg.. Knematc varables of the WMR moble platform For ths reason, an mplementaton of a control algorthm for ths robot must rel on an eternal devce (ether an on-board computer, or a remote PC wth wreless capabltes that communcates wth the mcrocontroller core va the RS nterface. We chose to desgn a small pece of hardware, nstalled on the WMR, consstng n a dstnct mcrocontroller wth two seral communcaton nterfaces. (Fg 4 The frst seral nterface s amed to transmt command packets to the robot, and to receve SIPs (Status Informaton Packets, whle the second RS nterface s connected to a rado modem n order to provde a wreless lnk to a remote computer, whch sends user commands, and adjusts parameter values. X Fg. 4 Block dagram of the addtonal embedded controller The control algorthm was mplemented on ths addtonal embedded controller, usng ANSI C for mamum software portablt. Remarks on the communcaton protocol Once the communcaton starts, the robot permanentl sends SIPs, ever 00 ms. Each SIP contans the current values of the coordnates (,,, as well as the most recent readngs of the sonar arra, and status varables. The control applcaton must parse the receved packets and update the assocated varables. Poneer -DX accepts a varet of command packets va the seral lne, among whch one s partcularl sgnfcant for ths applcaton. The VEL command defnes smultaneousl ndvdual values for the speeds of each drve wheel. B specfng dfferent values for the speeds of the rght wheel V R and left wheel V L, the robot s nstructed to combne translaton and rotaton n a natural wa. Havng the current coordnates, and a means to evaluate the dstance from the current poston to the target path, one can estmate the speeds of the rght and left wheel, V R and V L, so that the dstance to the target s mnmzed. ISSN: ISBN:

3 8th WSEAS Internatonal Conference on SYSTEMS THEORY and SCIENTIFIC COMPUTATION (ISTASC 08 Rhodes, Greece, August 0-, The knematc model of the moble robot Poneer -DX Assumpton: The movement of the vehcle s on a crcular trajector on a flat plane, wthout obstacles, Assumpton: The WMR moton s assumed to be pure rollng, wth no slppng. If p s a pont n a space wth n generalzed coordnates, and p s the respectve dervatve of ths vector, wth the notatons shown n fg. 5, the drect Jacoban model can be wrtten: ' r(snφ / r(snφ / ω L ' = r(cosφ / r(cosφ / ω R φ ' r / b r / b ' (snφ / (snφ / v ' = (cosφ / (cosφ / v φ' / b / b L R (5 (6 5 The fuzz controller Assume that the crcular target trajector s defned b the coordnates of the center ( C, C and the radus R. The WMR must be controlled so that the dstance d from the current poston to the center of the crcle equals R. The poston error e for the current poston (, s: e = ( + ( R (7 C C Fg. 5. WMR movng on a crcular trajector snφ 0 p' = φ + cos v 0 ω 0 ( where v s the lnear veloct of the WMR and s the angular veloct. Ths s equvalent to: ' snφ 0 v ' = cosφ 0 ω φ ' 0 ( Consderng the notatons shown n fg. : vr + vl ω R + ω L v = = r ( and the error dot e can be appromated b: e e' = (8 t provded that t s small. Remark: In case of an ellptc trajector, defned b two focal ponts ( C, C,( C, C, and the constant K (the sum of the dstances from an pont belongng to the ellpse to the focal ponts, the error s calculated wth (9: e = ( + ( + ( + ( K (9 c c c c Ths leads to three obvous domans of varaton for e(t: negatve (N, zero (Z and postve (P, whch corresponds respectvel to the stuatons when the WMR s nsde the target crcle, close to the crcumference, and outsde t. Consder the membershp functons defned n fg 6. vr vl ωr ωl ω = = r (4 b b where b s the bas of the WMR (dstance between the planes of the drve wheels, and r s the radus of the drve wheels. Substtutng ( and (4 n (: N 00% Z P DOM P Z -M 0 E M Fg. 6. Membershp functons for e(t e(t ISSN: ISBN:

4 8th WSEAS Internatonal Conference on SYSTEMS THEORY and SCIENTIFIC COMPUTATION (ISTASC 08 Rhodes, Greece, August 0-, 008 Note that, all the three membershp functons assocated wth e(t are full determned b the parameter M, and the degree of membershp (DOM to a specfc doman can be easl calculated b lnear nterpolaton. B defnng a smlar set of membershp functons for the error dot (e (t=e/t, one gets a total of 9 rules ( of the followng tpe: If the error s postve and the error dot s postve, then v R must be HIGH and v L must be LOW. The entre rule base descrbng the fuzz controller s presented n Table. Each cell of table contans a logc sentence and should be read as: If e(t s Negatve AND e (t s negatve, THEN V L must be HIGH and V R must be LOW Table. Rule base for the Fuzz controller where H,M,L desgnate the sngleton values for HIGH, MEDIUM and LOW fuzz domans of the outputs v L, v R respectvel. The truth value of the antecedent of the above sentence s mn(n(e,n(e, where N(e, N(e are the degrees of membershp of e(t and e (t to the doman N. The crsp output of the fuzz controller s a combnaton of all the rules n the rule base as follows: v error dot error e(t e (t N Z P N HL LH LM Z HL MM MH P ML MH LH K = out = K where: = z S z (0 z = mn(e, E' ( S s the correspondng sngleton value of the fuzz output, and K s the total number of rules n the rule base. E, E are the degrees of membershp of e(t and e (t to the doman correspondng to the cell. The mcrocontroller mplementaton of the above algorthm reles on two background tasks (nterrupt drven: one task receves and parses the SIPs sent b the PDX WMR and updates the global varables,, and the other uses a tmer to generate precse tme ntervals t. Then, usng (7 and (8 the MCU computes e(t and e (t for the current poston, and usng (9 generates the values of the left and rght wheel speed, v L and v R, for the net tme nterval. Fnall, the computed values of v L and v R are sent to the robot va the VEL command packet, and the tmer s restarted. Ths process contnues ndefntel, untl a STOP command s receved from the human operator va the rado lnk. 6 Real-tme fuzz control results n path followng For testng the proposed dscrete-tme sldng-mode adaptve controller Poneer -DX wth on board PC and wreless adapter has been used n crcular trajector trackng. The basc Poneer platform contans all of the components of an ntellgent moble robot for sensng and navgaton n real world envronment, ncludng batter power, drve motors and wheels, lnear encoders, and rangefndng ultrasonc sonar transducers. The moble platform s clent-server archtecture. The clent s an embedded PC wth Ethernet connecton. The robot clent software runs on the onboard PC and wreless Ethernet s used to montor and control PC operatons. The rugged P-DX s 44cm 8cm cm alumnum bod wth 6.5cm dameter drve wheels. The two motors use 8.: gear ratos and nclude 500-tck encoders. Ths dfferental drve platform s hghl holonomc and can rotate n place movng both wheels, or t can swng around a statoner wheel n a crcle of cm radus. A rear caster balances the robot. A dedcated PC software smulaton program was wrtten to test and compare the performances of the embedded mplementaton, and to adjust the control parameters. Ths software uses (6 to compute the current poston nstead of readng t from the WMR. The results of the smulaton are shown n fg. 7 and fg. 8. ISSN: ISBN:

5 8th WSEAS Internatonal Conference on SYSTEMS THEORY and SCIENTIFIC COMPUTATION (ISTASC 08 Rhodes, Greece, August 0-, 008 presented as a table of coordnate pars (,, each segment of ths curve, can be represented as a lne, havng the equaton: A+B+C=0 ( where Fg. 7 WMR epermental crcular trajector STARTING POINT (0,0 In these fgures, the contnuous lne represents the target path, and the dashed lne s the actual trajector of the WMR. Fg. 7 shows the evoluton of the WMR when the startng pont was outsde the target crcular trajector, whle fg. 8 descrbes the evoluton of the robot s nsde the crcle that defnes the target. A = ( + B = ( ( + C = ( + ( + ( Wth these notatons, the dstance from the pont ( 0, 0 to the current segment of the curve, defned b two successve ponts (,, ( +, + s determned b (4. A0 + B0 + C d = (4 A + B Snce the dstance, calculated wth the equaton (4 s alwas postve, the poston error, n terms of the fuzz algorthm descrbed above, s: e=sgn *d (5 (0,0 where sgn =+ when the moton on segment of the curve s n trgonometrc drecton, and otherwse. The epermental results dervng from the above consderatons are presented n fgure 0. Fg. 8 Epermental crcular trajector A varant of the algorthm, based on (9, produced the followng results, for an ellptcal trajector: P4(4,4 P(, P(, P(, SOLID LINE= TARGET TRAJECTORY DASHED LINE = ACTUAL TRAJECTORY (X0,Y0 Fg. 0 WMR followng an open, generalzed path Fg. 9 Epermental ellptcal trajector 7 Generalzaton of the soluton Gven a generalzed two-dmensonal curve, 8 Concluson and further research Ths research demonstrates that relatvel comple real-tme control algorthms can be mplemented on small, mcrocontroller-based, devces. Note that the fuzz parameters used n ths ISSN: ISBN:

6 8th WSEAS Internatonal Conference on SYSTEMS THEORY and SCIENTIFIC COMPUTATION (ISTASC 08 Rhodes, Greece, August 0-, 008 eperment have been arbtrarl selected, wth vrtuall no tunng, and therefore sgnfcant mprovements of the overall behavor of the WMR can be epected from applng a genetc algorthm for optmzaton. Snce the natural trajector of a WMR wth v R <>v L s an arc of crcle, comple splne calculatons can be avoded b appromatng the target path b a successon of arcs. At an gven moment, the current poston pont and the two consecutve ponts n the table that defnes the generalzed path determne a crcle, defned b the equaton (6. ( ( ( ( = 0 (6 Thus, an trajector can be appromated b a successon of arcs, each determned b the current poston and two successve ponts of the target trajector. Solvng the equaton (6 for each (, pont of the generalzed trajector to determne the coordnates of the center and the radus of the correspondng arc of crcle that appromates the trajector does not nvolve massve calculatons, and t s feasble wth an embedded devce. Ths approach could also be nterestng for solvng the problem of obstacle avodance, snce the current pont, the target pont behnd the obstacle, and a lateral ntermedar pont n an obstacle free area determne a crcle, where the smple algorthm descrbed above s easl applcable. Further research s also needed n the followng drectons: Addng a mechansm for automatc adjustment of the control parameters, usng a genetc algorthm; Implementng a sldng mode control algorthm. References: [] Flpescu, A.; Dugard, L.; Don, J.-M, Adaptve gan sldng observer based sldng controller for uncertan parameters nonlnear sstems. Applcaton to fleble jont robots. Decson and Control, 00. Proceedngs. 4nd IEEE Conference, Volume 4, Issue, 9- Dec. 00 Page(s: [] Thongcha S. and Kawamura K. Applcaton of Fuzz Control to a Sonar-Based Obstacle Avodance Moble Robot. Proceedngs of the 000 IEEE Internatonal Conference on Control Applcatons Anchorage, Alaska, USA September 5-7, 000 [] Susnea I. and Mtescu M. Mcrocontrollers n Practce, SPRINGER Verlag, 005, ISBN [4] Ahmad I. Fuzz Logc for Embedded Sstem Applcatons, Newnes, 00, ISBN [5] Baturone I, Moreno-Velo F.J, Sánchez- Solano S., Blanco V. Embedded fuzz controllers on standard DSPs., J. Ferruz, Proc. IEEE Int. Smp. on Industral Electroncs, ISIE005, Dubrovnk (Croaca, June 005. ISSN: ISBN: