An Integrated Approach to Goal-directed Obstacle Avoidanceunder Dynamic Constraints for Dynamic Environments∗

Cyrill Stachniss Wolfram Burgard

University of Freiburg, Department of Computer Science, D-79110 Germany

Abstract

Whenever robots are installed in populated environments,they need appropriate techniques to avoid collisions withunexpected obstacles. Over the past years several reac-tive techniques have been developed that use heuristicevaluation functions to choose appropriate actions when-ever a robot encounters an unforeseen obstacle. Whereasthe majority of these approaches determines only the nextsteering command, some additionally consider sequencesof possible poses. However, they generally do not con-sider sequences of actions in the velocity space. Accord-ingly, these methods are not able to slow down the robotearly enough before it has to enter a narrow passage. Inthis paper we present a new approach that integrates pathplanning with sensor-based collision avoidance. Our al-gorithm simultaneously considers the robot’s pose andvelocities during the planning process. We employ dif-ferent strategies to deal with the huge state space that hasto be explored. Our method has been implemented andtested on real robots and in simulation runs. Extensiveexperiments demonstrate that our technique can reliablycontrol mobile robots moving at high speeds.

1 Introduction

Path planning is one of the fundamental problems in mo-bile robotics. As mentioned by Latombe [11], the ca-pability of effectively planning its motions is “eminentlynecessary since, by definition, a robot accomplishes tasksby moving in the real world.” Over the past years, a hugevariety of techniques for motion planning has been de-veloped. They can roughly be divided into map-basedapproaches such as road-map or cell-decomposition tech-niques (see [11] for an extensive overview), and reactive,sensor-based approaches [1, 4, 6, 7, 9, 10, 15].

Goal-directed path planning techniques compute pathsbased on a given map of the environment. Thereby theyassume that the environment does not change while therobot is moving. However, when robots are designed tooperate in dynamic or populated environments, this as-sumption is no longer justified. Instead the robots have

∗This work has partly been supported by the EC under contract num-ber IST-2000-29456.

to be equipped with sensors to be able to react to un-foreseen obstacles. Therefore, many researchers havedeveloped reactive, sensor-based approaches that con-trol the movements of the robot based on its sensoryinput. Typical members of this class are the curvature-velocity method [15], the dynamic window approach [4],the vector-field histogram techniques [1] and the potentialfield techniques [8]. The key idea of these approaches isto use an evaluation function to determine the best nextaction based on the current sensory input and eventu-ally given the current state of the system. The advantageof these techniques lies in their efficiency, so that newcommands can be generated at a high frequency. Onedisadvantage of the purely sensor-based approach is thesub-optimality which comes from the fact that potentiallyavailable global information is ignored. Moreover, tech-niques of this type are also incomplete, since the robotscan get stuck in U-shaped obstacles.

Therefore, several researchers have worked on extensionsto remedy these problems. For example, Kathib andChatila [7] modify the potential function to make themotion of the robot more efficient and to achieve cer-tain desirable behaviors such as wall following and track-ing. Schlegel [13] presents an approach to modify theevaluation function according to the shape of the robot,which is especially useful for transportation or manipu-lation tasks. The techniques described by Fox et al. aswell as Schmidt and Azarm [5, 14] combine the sensoryinformation with a given map of the environment, for ex-ample to deal with objects that cannot be detected withthe robot’s sensors. Ko and Simmons [9] introduced theLane-Curvature method. This approach extracts lanes outof a given map of the environment and modifies the eval-uation function so that the robot stays on the lanes. Thisleads to smoother trajectories, especially in long corri-dors.

Additionally, there are approaches to integrate path-planning techniques with sensor-based approaches. Forexample, the systems described by Burgard et al. andThrun et al. [3, 17] uses a path planning system to com-pute intermediate points lying on the optimal path. Theseintermediate points are transmitted to the reactive colli-sion avoidance system, so that the robot can no longer

get stuck in local minima of the evaluation function. Re-cently, Brock and Kathib [2] presented an elegant in-tegration of path planning and reactive collision avoid-ance. They modify the evaluation function according tothe previously planned path and this way can integrate theknowledge about globally optimal actions into a sensor-based approach.

Whereas all these approaches have been proven to allowmobile robots to reliably navigate in dynamic environ-ments, the resulting paths sometimes are sub-optimal. Aswe will point out in more detail in this paper, one of themain reasons for this lies in the fact that the next con-trol action for the robot is chosen mainly based on itscurrent state (position, orientation and velocities), even-tually taking into account a globally planned path. Thispath, however, is only planned in the〈x, y〉 space withoutconsidering the orientation and velocities of the robot. Aswe will argue below, in certain situations the kinematicsof a robot also has to be considered during the map-basedpath planning, especially when the robot moves at a highspeed.

In this paper we present integrated path-planning and col-lision avoidance technique that takes into account thekinematics of the robot as well as the dynamic of theenvironment. Our method plans the control actions ofthe robot in the space of positions (〈x, y, θ〉) and veloc-ities (〈v, ω〉). We apply several techniques to cope withthe enormous complexity of the state space that has tobe explored. As a result our system is able to efficientlydetermine the next motion command. Our technique hasbeen implemented and tested in extensive experiments ondifferent robot systems as well as in simulation. All ex-periments show that our technique is able to reliably con-trol a robot in dynamic environments even with narrowpassages. They furthermore illustrate that our approachyields a better behavior than previous approaches.

This paper is organized as follows. In the next sectionwe will describe the motion equati8ons for synchro-driveand differential-drive robots, which build the basis for ourcollision avoidance system. Section 3 contains a discus-sion of the popular dynamic window approach. In Sec-tion 4 we will present our planning technique. Finally,in Section 5 we describe different experiments illustrat-ing the capabilities of our approach. We also perform acomparison to an alternative technique and to the optimalsolution.

2 Motion Equations for Synchro-Drive andDifferential-Drive Robots

To plan the trajectories of a robot our system is guidedby the well-known motion equations for synchro-drive ordifferential-drive robots [4]. Thereby it assumed that thetrajectory of a robot can be approximated by a sequenceof circular arcs. Accordingly, thex-coordinates of the

*

goal

c c−i 0

c

i+1cic

Figure 1: Situation in which a robot using the dynamic win-dow approach cannot reach the goal.

robot given it starts at positionx(t0) and given that it hasthe velocities〈vi, ωi〉 at timeti can be computed as:

x(tn) = x(t0) +n−1∑i=0

(F ix(ti+1))

where

F ix(t) =

− vi

ωi(sin θ(ti)− sin(θ(ti)+

ωi · (t − ti))), ωi 6= 0

vi cos(θ(ti)) · t, ωi = 0.

The corresponding equations for they-coordinate are ob-tained by replacingcos by sin and multiplying the firstline by−1.

3 The Dynamic Window Approaches

The dynamic window approach (DWA) is a popular tech-nique for reactive collision avoidance. Their key idea is touse an evaluation function that takes as input the currentstate of the system (pose and velocities) and to determinea new steering command (translational and rotational ve-locities) such that a given evaluation function is maxi-mized. The search for appropriate steering commands iscarried out within the set of admissible velocities. Amongthem are all velocities that can be reached within a certaintime frame given the maximum possible accelerations ordecelerations of the system. Furthermore, velocities thatwould inevitably lead to a collision with an obstacle arenot considered admissible. The evaluation functions typ-ically measure the progress towards the goal, the currentvelocities and the distance to obstacles [2, 4, 9, 13, 15].

Although dynamic window techniques have successfullybeen applied on a variety of robot systems, there are sit-uations in which they lead to a sub-optimal behavior. Aswe figured out in practice, this mainly comes from thefact that the existing approaches only consider the imme-diately next action or, if not, only plan in the〈x, y〉 con-figuration space. However, when the robot travels alonga corridor and has to enter a doorway, it has to decelerateearly enough to be able to actually turn into the door-way. Approaches not performing a look-ahead or tech-niques not considering the velocities during planning are

not able to detect this and therefore will fail to producethe correct motion commands.

For example, consider the situation depicted in Figure 1.The Global Dynamic Window Approach [2] tries to max-imize the following evaluation function:

Ωg(p, v, a) = α · nf1(p, v) + β · vel(v)+γ · goal(p, v, a) + δ ·∆nf1(p, v, a).

Here, nf1(p, v) is the so-called navigation function,which is based onA∗-search in the〈x, y〉 space. Thetermvel(v) is a function that assesses the velocity of therobot. Far away from the goal, a high velocity means ahigh assessment. Furthermore,goal(p, v, a) is a binaryfunction which indicates that the robot is near the goal.∆nf1(p, v, a) is the grid-based gradient of the navigationfunction. Now suppose that the goal is at the very end ofthe long corridor starting in the field markedc0 and fac-ing south. Furthermore suppose the robot evaluatesΩg

when it is in cellci with i > 1. Obviously, the valuesof nf1(p, v) force the robot to stay on its horizontal path.The same holds for the gradient∆nf1(p, v). Furthermore,goal(p, v, a) is zero here, because the target location isfar away fromc0. Please note that only inc0 the valuesof nf1(p, v) and∆nf1(p, v) force the robot to turn south.Thus, the steering command in the cellsci (i > 1) of therobot is mainly governed by the termvel(v). Since thesystem seeks to maximizeΩg, the robot will accelerateas much as possible. As a result, it will be too fast tomake the necessary turn into the corridor when reachingc0. Thus, without considering the velocities the robot willfail to enter the corridor. This illustrates that it is not suf-ficient to plan in the〈x, y〉-space only. Instead one has toperform a look-ahead in the space of velocities to chooseappropriate steering commands.

4 Velocity-based Motion Planning

Our approach to integrated path planning and collisionavoidance considers the five-dimensional〈x, y, θ, v, ω〉configuration space and tries to optimize a trade-off be-tween time and collision risk. Unfortunately, planning inthe whole state space is too time-consuming and cannotcope with the real-time constraints imposed by a robotmoving quickly in dynamic environments. Our systemtherefore employs different strategies to deal with thehuge size of the space that has to be explored as wellas with the dynamics of the environment. In particular,we proceed in the following four steps explained in moredetail below:

1. Update the given (static) map according to the recentsensory input.

2. Compute a path in the〈x, y〉 space given the updatedmap.

3. Use the path generated in Step 2 to determine thesearch space to be explored in the next step. Further-more compute a heuristic to guide the search duringthis exploration.

4. Search for a partial path in the fraction ofthe 〈x, y, θ, v, ω〉 configuration space computed inStep 3.

Step 1: To represent the environment, our system usesoccupancy grid maps [12]. This representation separatesthe environment into a grid of equally spaced cells andstores in each cell〈x, y〉 the probabilityP (occx,y) thatit is occupied by an object. We assume that a map rep-resenting the static aspects of the environment is a priorigiven. To integrate sensory input into this map we ap-ply a conservative strategy. We simply set the occupancyprobability of a cell in which the beam ends to1.0 whichprevents the system from planning a path through thatcell. As soon as this cell is detected to be free again or acertain period of time has elapsed (120 secs), we reset itsoccupancy value to the original value.

Step 2: The goal of the second step is to compute apath from the current location to the target position inthe〈x, y〉-space. Our system applies the popularA∗ pro-cedure using the grid-graph induced by the occupancygrid map to find the shortest path. Thereby, the cost fortraversing a cell〈x, y〉 is proportional to its occupancyprobability P (occx,y). Cells for whichP (occx,y) ex-ceeds a threshold of0.15 are assumed to have infinitecosts. To speed up the search, the heuristic is based ona value-iteration in the〈x, y〉-space computed using thestatic map. Please note that this heuristic has to be com-puted only once for each global target location.

Step 3: The goal of this step is the computation of thesearch space to be explored in the next step. To this endwe create a channel around the path computed in the pre-vious step. In our current system we adopt the width andlength of this channel dynamically. We start with a chan-nel width of70 cm at the current location of the robot andexpand the channel along the path computed in Step 2.We stop the process as soon as the channel has reached acertain size that depends on the performance of the under-lying computer. The goal is to determine a channel thatcan be explored within the next0.25 seconds so that thesystem can issue a new motion command within this timeframe. The final location on the optimal two-dimensionalpath reached during the channel expansion will be thenext subgoal〈xg, yg〉 for the robot. We also perform adeterministic value-iteration in this channel. The result-ing value function is identical to the navigation functionnf1described above and builds an admissible heuristic fortheA∗ search carried out in the following step.

Step 4: In the final step we compute a path fromthe robot’s current state〈x, y, θ, v, ω〉 to the location〈xg, yg〉. Thereby we explore the five-dimensional con-

channel

current location -

goal

?

subgoal

?

5d-path

?

2d-path6

Figure 2: Trajectories and corresponding channel gen-erated in one cycle.

figuration space〈x, y, θ, v, ω〉 bounded by the channelcomputed in step 3. To determine appropriate values forθg, vg andωg we distinguish two situations. If〈xg, yg〉corresponds to the global target location we allow an ar-bitrary orientation forθg. In this case the velocitiesvg

andωg, however, have to be zero. If〈xg, yg〉 differs fromthe global target, we allow arbitrary values forvg but setωg to zero andθg to the direction between〈xg, yg〉 andthe next position on the path computed in step 2. Againwe applyA∗ to find the optimal sequence of motion com-mands. As heuristic we use the value function computedin the previous step. The discretization of the state spacetypically is 10 cm for the position,π/16 degrees for theheading,π/16 degrees per second for the rotational ve-locity, and10 cm/sec for the translational velocity. Tocompute the successor state of a given state, we use themotion equations described in Section 2:

〈x1, y1, θ1, v1, ω1〉〈v2,ω2〉−→ 〈x2, y2, θ2, v2, ω2〉.

Thereby, the maximum changes in the velocities are lim-ited by the accelerations of the robot’s motors. As a re-sult, we obtain a sequence of velocity commands〈v, ω〉the robot must execute to reach〈xg, yg, θg, vg, ωg〉.Figure 2 shows some of the data structures generated dur-ing Steps 1 to 4. The solid line corresponds to the optimalpath planned in the two-dimensional configuration space.In this situation the robot starts in the lower corridor andhas to travel to the left end of the upper corridor. Alsoshown is the channel computed in Step 3 and the resultingintermediate goal location〈xg, yg〉. The dotted line cor-responds to the trajectory resulting from the full planningin the five-dimensional configuration space in Step 4.

Our current implementation is highly efficient [16]. Ituses lookup-tables to perform most of the geometric op-erations. This includes the computation of cells coveredby the robot and the computation of successor states.Our current system is able to explore channels of 2 mlength and 70 cm width within 0.25 seconds on a standard800 MHz Pentium III computer. Whenever the channelis too large to be explored, we use the dynamic win-dow technique to quickly generate admissible velocities.We then reduce the size of the channel for the next timeframe. If, however, the channel turns out to be too small

Figure 3: Pioneer 1 robot Ludwig driving around an unex-pected obstacle.

since sufficient time remains in Step 4, we increase thesize of the channel appropriately. This way, our approachdynamically adapts itself to the performance of the un-derlying processor and to the complexity of the planningproblems. Please note, that our approach in principle isable to compute the optimal path from the robot’s cur-rent location to the target position given the map and thecurrent sensory input. If enough computational resourcesare available, the channel can cover the whole configu-ration space so that the system can compute the optimalsequence of steering commands.

5 Experimental Results

Our path planning method has been implemented and ex-tensively tested on our mobile robots Albert and Ludwig.Whereas Albert is an RWI B21r robot, Ludwig is a Pio-neer 1 system. Both robots are equipped with SICK laserrange finders that are used to detect dynamic obstacles.Additionally, we carried out a series of simulation runsto compare our system to the dynamic window techniquedescribed in [4].

5.1 Collision avoidance in Dynamic Environments

The first experiment was carried out using both robots inour office environment at the University of Freiburg. Totest the capabilities of our system to deal with unexpectedobstacles we installed several objects in the corridor andchanged their positions frequently. Additionally, peoplewere walking in the environment. In both experiments,during which the robots traveled over 300 m with aver-age speeds of over 30 cm/s, we did not observe a singlecollision. Figure 3 shows a typical situation during theseexperiments. Here Ludwig is moving around an unex-pected obstacle in the corridor. We also performed exten-sive simulation experiments with an overall path lengthof 20 km. The simulator we used realizes the full func-tionality and behavior of the a mobile platform includ-ing the ability to set different accelerations, velocities etc.During all experiments we found that the generated pathswere very smooth and that the overall behavior was quiteefficient.

Figure 4: Typical trajectories taken by Albert when entering aroom using the DWA (left) and our approach (right).

Figure 5: Albert tries to enter a doorway blocked by severalobstacles using the DWA (left) and our technique (right).

5.2 Comparison to the Dynamic Window Approach

We also performed several experiments to compare ourtechnique to the collision avoidance system that we suc-cessfully employed over the past years. This system usesthe dynamic window approach technique combined witha 2d path planner [17]. Figure 4 shows the outcome ofone such experiment. Here the robot had to travel alongthe corridor of our office environment and had to enter therightmost office in the north. The left image shows thetrajectory generated by the dynamic window approach.The right image contains the trajectory generated by ouralgorithm. Since the DWA chooses too high speeds inthe corridor, it is not able to directly enter the room whenit reaches the doorway. Rather it first stops, turns backand then enters the room. Our module, in contrast, slowsthe robot down before it reaches the doorway area so thatAlbert is able to directly enter the room. With our sys-tem the robot completed the whole run in 34.5 secondsdriving 11.86 m, which corresponds to an average speedof 34.3 cm/sec. The dynamic window technique, how-ever, required 48.5 seconds and traveled 12.31 m, whichresults in an average speed of 25.4 cm/s. The maximumspeed of the system was set to 40 cm/sec in both runs.

Another experiment carried out with Albert is depictedin Figure 5. Here we installed several objects in front ofa doorway to increase the difficulty of entering the cor-responding room. As can be seen in the left image, thedynamic window technique again is too fast to make thenecessary sharp turn into the narrow passage. Our tech-nique, however, slows the robot down early enough sothat it can enter the passage immediately. Figure 6 de-picts a sequence of images showing Albert executing the

Figure 6: Albert traveling along the path shown in the rightimage of Figure 5.

0

20

40

60

80

100

120

140

time

[sec

s]

our technique, straight line pathDWA, straight line path

our technique, entering a roomDWA, entering a room

Figure 7: Average time needed to reach the goal location us-ing our planning module and using the DWA.

steering commands generated by our system.1

At this time we would like to mention that the evalu-ation function used by the dynamic window techniquesin fact can be tuned to optimize the behavior of a robotin specific situations like the ones given in the experi-ments described above. However, this generally reducesthe performance of the system in other situations. In theexperiments described here we therefore used parametersfor the DWA that we generally used on our system sincethey have shown to yield the best overall performance ina wide range of situations.

5.3 Simulation Experiments

To get a quantitative assessment of the performance ofour approach we performed a series of simulation exper-iments. In the first task the robot had to travel along astraight corridor without any unexpected obstacles. Thesecond scenario was similar to that shown in Figure 4.The robot had to move along a corridor and had to entera room. Figure 7 shows for 50 different runs the averagetime the robot needed to reach the target location. The er-ror bars, which are not visible in the first three columns,indicate theα = 0.05 significance level. As can be seenfrom the figure, both collision avoidance strategies showthe same performance if the robot stays in the corridor.However, our approach requires significantly less timeto complete the second task. Furthermore, our method

1More pictures and complete videos are available athttp://www.informatik.uni-freiburg.de/˜burgard/publications.html

145

150

155

160

165

170

175

w=70,l=150 w=90,l=250 w=110,l=500 optimal solution

driv

ing

time

[sec

s]driving time using different channel sizes

Figure 8: Average time needed to reach a goal location fordifferent sizes (w=width andl=length).

yields only a small variance of the overall completiontime. The DWA, in contrast, shows a quite high vari-ance, which comes from the necessary retries to enter theroom.

Figure 8 shows the time needed to complete a series ofnavigation tasks using channels of different size. The lastcolumn is the time needed by the optimal solution, i.e., ifthe whole state space is explored. As can be seen, the re-sults obtained with channels of500 cm length and110 cmwidth are as good as the optimal solution.

In a further simulation experiment we investigated theperformance of our method in situations like that shownin Figure 1. In contrast to the DWA, our method deceler-ates the robot early enough so that the system is able toturn into the corridor immediately [16].

6 Conclusion

In this paper we presented an integrated approach tosensor-based collision avoidance and path planning formobile robots in dynamic environments. Our systemplans in the full〈x, y, θ, v, ω〉 configuration space andtherefore takes into account the kinematics of the robot.The key advantage of our approach is that it also performsa lookahead in the velocity space. Accordingly the robotcan decelerate early enough which is highly important es-pecially in narrow environments.

Our algorithm includes several techniques to deal withthe complexity of the induced search problem during re-planning. The overall system is highly efficient and canbe run on a standard PC. It automatically adapts itself tothe performance of the underlying processor and to thecomplexity of the search problem.

The approach has been implemented and tested on differ-ent robotic systems. In all experiments our technique wasable to generate safe trajectories. We compared our ap-proach to the popular DWA technique. The experimentsdemonstrate that our algorithm yields more efficient tra-jectories which are often close to the optimal ones.

References[1] J. Borenstein and Y. Koren. The vector field histogram - fast

obstacle avoidance for mobile robots.IEEE Transactionson Robotics and Automation, 7(3):278–288, 1991.

[2] O. Brock and O. Khatib. High-speed naviagation using theglobal dynamic window approach. InProc. of the IEEEInt. Conf. on Robotics & Automation (ICRA), 1999.

[3] W. Burgard, A.B. Cremers, D. Fox, D. Hahnel, G. Lake-meyer, D. Schulz, W. Steiner, and S. Thrun. Experienceswith an interactive museum tour-guide robot.Artificial In-telligence, 114(1-2), 2000.

[4] D. Fox, W. Burgard, and S. Thrun. The dynamic windowapproach to collision avoidance.IEEE Robotics & Automa-tion Magazine, 4(1), 1997.

[5] D. Fox, W. Burgard, and S. Thrun. A hybrid collisionavoidance method for mobile robots. InProc. of the IEEEInt. Conf. on Robotics & Automation (ICRA), 1998.

[6] H. Hu and M. Brady. A bayesian approach to real-time ob-stacle avoidance for a mobile robot. InAutonomous Robots,volume 1. Kluwer Academic Publishers, Boston, 1994.

[7] M. Khatib and R. Chatila. An extended potential field ap-proach for mobile robot sensor-based motions. InProc.Int. Conf. on Intelligent Autonomous Systems (IAS’4), 1995.

[8] O. Khatib. Real-time obstacle avoidance for robot ma-nipulator and mobile robots.The International Journal ofRobotics Research, 5(1):90–98, 1986.

[9] N. Y. Ko and R. Simmons. The lane-curvature methodfor local obstacle avoidanc. InProc. of the IEEE/RSJInt. Conf. on Intelligent Robots and Systems (IROS), 1998.

[10] K. Konolige. A gradient method for realtime robot control.In Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robotsand Systems (IROS), 2000.

[11] J.-C. Latombe.Robot Motion Planning. Kluwer AcademicPublishers, 1991.

[12] H.P. Moravec and A.E. Elfes. High resolution maps fromwide angle sonar. InProc. IEEE Int. Conf. Robotics andAutomation, pages 116–121, 1985.

[13] C. Schlegel. Fast local obstacle avoidance under kinematicand dynamic constraints for a mobile robot. InProc. ofthe IEEE/RSJ Int. Conf. on Intelligent Robots and Systems(IROS), 1998.

[14] K. Schmidt and K. Azarm. Mobile robot navigation in a dy-namic world using an unsteady diffusion equation strategy.In Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robotsand Systems (IROS), 1992.

[15] R. Simmons. The curvature-velocity method for localobstacle avoidance. InProc. of the IEEE Int. Conf. onRobotics & Automation (ICRA), 1996.

[16] C. Stachniss. Zielgerichtete Kollisionsvermeidung fur mo-bile Roboter in dynamischen Umgebungen. Master’s thesis,University of Freiburg, Department of Computer Science,2002. In German, to appear.

[17] S. Thrun, A. Bucken, W. Burgard, D. Fox, T. Frohlinghaus,D. Hennig, T. Hofmann, M. Krell, and T. Schimdt. Maplearning and high-speed navigation in RHINO. In D. Ko-rtenkamp, R.P. Bonasso, and R. Murphy, editors,AI-basedMobile Robots: Case studies of successful robot systems.MIT Press, Cambridge, MA, 1998.