ECTOR FIELD PATH PLANNING AND CONTROL OF AN AUTONOMOUS ROBOT IN A DYNAMIC ENIRONMENT J.C. Wolf, P. Robso, J.M. Davies* School of Computg, Commuicatios ad Electroics, *School of Mathematics ad Statistics, Uiversity of Plymouth, Plymouth, PL4 8AA, Devo, U.K. ABSTRACT Potetial field methods are discussed as possible solutios to obstacle avoidace for mobile robots. A ew solutio to well kow problems associated with potetial fields is preseted. The desig of a geeral cotrol system, usg frequecy doma aalysis of the put sigal, that matches the path plag method to a robot s dyamics, is discussed. Prelimary experimetal results are preseted ad suggestio for further work proposed.. INTRODUCTION A variety of path plag methods for mobile robots have bee developed over the past two decades [-4,7]. The potetial field method is oe of the more popular methods used robot path plag due to its mathematical simplicity, elegace ad its suitability for dyamic eviromets. Oe of the origal works o potetial field methods is by Khatib [], who applied the method to maipulators as well as mobile robots. Khatib describes the idea as follows: The maipulator moves a field of forces. The positio to be reached is a attractive pole for the ed effector ad obstacles are repulsive surfaces for the maipulator parts. This paper troduces a theory which tries to resolve the problems idetified [] ad [3] particularly with regard to the oscillatio problems. Oe part of the theory volves a improvemet the path predictio ad a secod part of the theory volves makg a improvemet the trackg error arisg from the dyamics of the system.. POTENTIAL AND FORCE FIELD METHODS Mobile robots ad obstacles ca be treated as sgle pots provided their dimesios are small compariso to the workspace. These pots act as repulsive ad attractive poles a force field... Geeral Theory Cosider a dimesioal Cartesia coordate system (x,y) the plae of the football pitch, where x ad y axes are alog two perpedicular sides of the rectagular football pitch as show Figure. y O T O O3 Figure. Diagram showg a cotrolled robot, 3 obstacles ad a target positio. I geeral the resultat force actg o a cotrolled robot at ay positio the force field ca be expressed as the sum of repulsive forces from obstacles ad the attractive force towards the target as show equatio () x p = T + Oi () i= where is the umber of obstacles with a close rage of the robot,, i =.., are the repulsive forces actg o Oi
the robot due to the i th obstacle, T is the attractive force towards the target ad p is the resultat force actg o the robot. It should be oted that the magitudes of the repulsive forces are chose to be depedat o the distaces of the obstacle from the robot. However the magitude of the target force ca be set to a costat or set to a fuctio of the distace of the target from the robot []. I our paper we have chose the target force to be a costat. p = T + T9i () i= To simplify the otatio oly oe obstacle will be cosidered where the repulsive force is deoted by T9. Previously developed potetial field methods have force vectors which origate a directio perpedicular to a obstacle surface with a magitude that reduces with creasg distace from the obstacle. This approach is similar to that used electric or gravitatioal potetial eergy field theory. However, as poted out by Y.Kore ad J.Boreste [], this approach is too simplistic ad these authors have idetified the followg ma problems:. Trap situatios due to local mima (cyclic behaviour). No passage betwee closely spaced obstacles 3. Oscillatios the presece of obstacles 4. Oscillatios arrow passages A further problem metioed [] ad also [3] is that high speed robots deviate from the directio path of the force field. This is a importat problem if robots are required to move at a high speed, as the case of robot football. The root of the problem lies that the robot dyamics caot give rise to stataeous path chages ad therefore caot follow the geerated vector field. Some of these problems are associated with a speed restrictio beg required o paths which have a small radius of curvature... The preseted vector field theory Most theories use equatio () to compute the robot path which results a path of mimum potetial eergy. I the preseted theory, a modificatio to equatio () is proposed which each obstacle repulsive force,, T9 i acts a directio which is perpedicular to the target uit vector T as show Figure. Equatio () is ow modified to give the resultat force actg o the robot ad is give by Figure. ector field forces actg o cotrolled robot Let x = RO represet the distace betwee the robot ad the obstacle. Let α be the agle betwee vectors p as show Figure. The magitude of T9 where T9 a RO ad is to be defed by the formula = k G (x, µ, σ) (3) a ( x µ ( α) ) G (x, µ, σ) = e σ is a o-ormalised Gaussia distributio fuctio, µ (α) is a mea legth which is depedat o the agle α, σ is a costat variace ad k is a costat scalg factor. A Gaussia distributio was chose this theory sce this fuctio gave a more gradual build up of the force as the robot approaches a obstacle. Some itial experimets, ot described here, have show that this fuctio leads to better path stability, mimum oscillatios ad less trackg error whe compared to other types of fuctios such as the force verse power law. The Gaussia fuctio used equatio (3) has the property which a maximum value occurs whe the distace x = µ ( α) ad has a zero value whe x teds to fity. For reasos further explaed sectio.3, a
fuctio µ (α) is troduced which is defed equatio (4). This fuctio allows the robot path to be optimised after the robot has traversed a particular obstacle. r µ ( α) = (4) α + e τ where τ is a costat agle ad r represets a maximum value for µ at a agle α =. This fuctio was suitably chose to represet a mootoically decreasg fuctio of α ad to give rise to a strog fluece o the path directio whe α is small (i.e. whe the robot is travellg towards the obstacle) ad a egligible fluece o the path directio whe α becomes large (i.e. whe the robot has traversed the obstacle). A plot of µ (α) is show Figure 3 for the case whe r = 6 ad τ = 3. 6 5 4 µ(α ) 3 4 6 8 4 6 8 Agle α (Degrees) Figure 3. Plot of µ (α) as a fuctio of α.3 Calibratio Procedure For the case of the Gaussia distributio, a value of σ is chose to give a reasoable magitude of the repulsive force whe x = µ + σ. i.e. whe x lies at oe stadard deviatio away from the mea of the Guassia distributio. This calibratio costat will be depedet o the dimesios of the obstacle. For the fuctio µ (α), the costat τ is chose to give a offset value of approximately.r whe o α = 9. This correspods to a value of o τ 3. Figure 4 shows a typical path of the robot whe a obstacle is placed betwee a start pot ad the target. I order to demostrate the fluece of the fuctio µ (α) o the robot the path for the case whe µ = is also preseted. It should be oted that differet k values were used for the two paths so that both paths arrive at the same pot the workspace whe the robot has begu to traverse the obstacle. It ca clearly be see Figure 4 that the robot does ot follow the most direct route to the target, after traversg the object, uless the fuctio µ (α) is take to accout the theory. The more direct route has less curvature ad therefore the robot ca atta a higher speed ad therefore result a lower approach time. y (ches) 5 4 3 No Zero mu Zero mu Obstacle Target 5 5 5 x (ches) Figure 4. The path of a robot startg at (,) ad avoidg a obstacle o the way to target at (5,) with τ = 3, σ =, r = The system described so far represets the ectorfield Geerator block show Figure 5. A descriptio of the remag part of the complete block diagram appears the followg sectio. 3.. Cotrol System 3. PATH PLANNER The desired headg agle of the robot at ay pot the workspace is deoted by θ ad is equal to the directio agle of the resultat force. i.e. θ = p (5) It should be oted that θ is the put to a cotrol system that aligs the robot. However the magitude of p is ot used the cotrol system. I a complex dyamic eviromet with may obstacles, this put is somewhat upredictable. Therefore the desiged cotrol system eeds to lower the robot s speed to the required level order for the cotrol system to follow θ with mimum trackg error.
Figure 5. Complete block diagram of the Robot Cotrol System I order to detect a mismatch the demaded trajectory to the robots capabilities, a simulatio program executes the robots path before actual robot movemet is set to take place. Durg this simulatio period the cotroller put θ (t) is measured ad stored to memory. The measured sigal is the trasformed to the frequecy doma which provides formatio about the required badwidth of the path. The trasform is defed equatio (6). Θ (jω) = F{ (t) θ } (6) where F deotes the Fourier trasform. By comparg Θ (jω) to the vehicle badwidth G( jω) it ca be determed if the vehicle badwidth is exceeded. I this case the demaded speed of the robot must be reduced. The curret headg agle of the robot is subtracted from the demaded headg agle geerated to produce the agular velocity ω. The cotroller aims to cotuously le up the robot with the vector-field. The magitude of ` the forward velocity (defed equatio (8)) is also computed ad is used as a put to the cotroller. To avoid skiddg, e.g. as a result of a large cetripetal force, the value of must be costraed to a upper limit. (This upper limit is depedet upo a umber of factors cludg the coefficiet of frictio of the tyres ad robot mass). This esures that the ormal acceleratio does ot exceed a kow limit. For circular motio, the ward acceleratio is related to the agular velocity ω ad the velocity v by the formula a = vω. O usg this formula, ca be expressed as a cost = (7) ω The forward velocity is defed equatio (8). max if > max ` = (8) otherwise where max is a kow maximum target approach velocity ad is flueced by the path frequecy spectrum error. For a two-wheel differetial drive robot [6] the left had ad right had robot wheel velocities L ad R respectively are the computed usg the formulas Lm Rm ` = M ω L / where matrix M = (9) L / The trasfer-fuctio block G(s), which appears Figure 5, relates the requested wheel velocity (put) to the curret wheel velocity (output). This fuctio represets the wheel velocity of the robot. A discrete versio of G(s) is used the simulatio part of the path plag. 3. Robot Model A real velocity-cotrolled robot football player ca be modelled approximately by a first or secod order lear system combed with a o-lear rate-limiter ad a pure time delay. The time delay is due to the time required for radio trasmissio ad visio system processg. The ratelimiter limits the acceleratio of the robot sce a real robot ca oly accelerate with a torque which is proportioal to
the maximum curret of the circuit. The robot acceleratio is also limited order to prevet forward wheel slip ad to coserve battery power. A block diagram of the real model is show Figure 6 below. sl K e ( + st )( + st ) Figure 6. Real Model Block Diagram I the SimuroSot Middle league simulator, the trasfer fuctio G(s) has bee determed by a step respose experimet ad ca be modelled by the trasfer fuctio TD s K e G(s) = () + T s where T is the delay the simulator, D T is the system time costat, ad K is a amplificatio factor 4. EXPERIMENTS AND RESULTS I the limited time available it has ot bee possible to coduct a full rage of experimets order to test ad verify the theory developed here. Notwithstadg these restrictios a series of practical tests have produced ecouragg results. The costats K, TD, ad T, required equatio (), were obtaed by curve fittg the data show Figure 7. The values of these costats are show below. T = frames D T = 6.7 samples K =.88 to match the output to ches/frame. The put rage to the SimuroSot system has bee scaled to lie the rage betwee - ad. The trasfer fuctio G(s) is coverted to a discrete trasform G(z) with a samplg time beg set to frame. The same step respose experimet was carried out with a real robot football system as show Figure 8. Usg curve fittg techiques, Figure 8 ca therefore be used to determe the costats appearg Figure 6. elocity(pixels/frame) 4 8 6 4 3 4 5 Frame Number 4. Determatio of Robot Models The step respose experimet, referred to sectio 3., of the robot velocity at each time frame is show Figure 7. Figure 8. Step-respose of a real football robot elocity uits pixels/frame (frame-rate = 5Hz, 768 pixels to.m) otg the large dead-time at the begg from the frame grabber. elocity (Iches/Frame).8.6.4..8.6.4. 5 5 5 3 35 4 45 5 55 6 Frame Number Iitial Slope Le m easured A compariso betwee the simulated path ad the real robot path is show Figure 9. The differece betwee the two paths is due to the accuracy of the model used equatio (). However the simulatio is accurate eough to predict the arrival time. Figure 7. Step-respose of a SimuroSot robot. elocity uit ches/frame. (fps = 6Hz)
6. REFERENCES [] Oussama Khatib, Real-Time Obstacle Avoidace for Maipulator ad Mobile Robots The Iteratioal Joural of Robotics Research, MIT Press, Cambridge USA, pp 9-98, Sprg 986 [] Y. Kore, J. Boreste, Potetial Field Methods ad Their Iheret Limitatios for Mobile Robot Navigatio, IEEE Coferece o Robotics ad Automatio, Sacrameto Califoria, pp 398-44, April 99 Figure 9. Compariso betwee the simulated path ad the real executed path. 5. CONCLUSION The disadvatages of traditioal vector field avigatio have bee idetified. A modificatio to vector field avigatio, which improves robot avigatio ad obstacle avoidace behaviour, has bee demostrated. This ew variatio uses a repulsive force perpedicular to the target le of sight ad characterised by a Gaussia distributio, to esure collisio is avoided. I this case, the robot is guided o a smooth path aroud the obstacle. This eables higher operatioal speeds to be achieved. A geeric cotroller which helps optimise the ew vector field method has bee desiged. By comparg the badwidth of the demad sigal to the badwidth of the robot, the cotroller esures that over ambitious commads, e.g. a tight, high speed tur which would cause the robot to skid, are avoided. The cotroller is desiged to automatically limit put commads so that they are with the dyamic capabilities of the robot. I the ear future it is hoped to use a Mirosot robot to successfully demostrate the ew vector field method workg uiso with the geeric cotroller. Potetial time delays durg operatio are idetified as a possible source of cocer. [3] Prahlad adakkepat, Tog Heg Lee ad Liu X, Applicatio of Evolutioary Artificial Potetial Field Robot Soccer System, Natioal Uiversity of Sgapore, Sgapore [4] M. Khatib, R. Chatila, A Exteded Potetial Field Approach for Mobile Robot Sesor-Based Motios, Proceedgs of the Itelliget Autoomous Systems IAS-4, IOS Press, Karlsruhe Germay, pp. 49-496,March 995 [5] Ma-Wook Ha, Peter Kopacek, Neural Networks for Cotrol of soccer robots, Istitute for Hadlg Devices ad Robotics, iea Uiversity of Techology, iea, Austria. Published the ISIE, Cholula, Puebla, Mexico [6] Phillip J. McKerrow, Itroductio to Robotics, Addiso-Wessley, Sydey, Australia, pp. 4-4, 99 [7] Rolad Siegward, Illah R. Nourbakhsh, Itroductio to Autoomous Mobile Robots, MIT Press, Cambridge, Massachusetts, Lodo, Eglad, pp. 67-7, 4