Predictive Collision Detection for the Dynamic Window Approach

Marcell Missura1 and Maren Bennewitz1

Abstract— Foresighted navigation is an essential skill forrobots to rise from rigid factory floor installations to muchmore versatile mobile robots that partake in our every dayenvironment. The current state of the art that provides thismobility to some extent is the Dynamic Window Approachcombined with a global start-to-target path planner. However,neither the Dynamic Window Approach nor the path plannerare equipped to predict the motion of other objects in theenvironment. We propose a change in the Dynamic WindowApproach—a dynamic collision model—that is capable ofpredicting future collisions with the environment, even whenobjects are in motion. We show in our experiments that ournew way of computing the Dynamic Window Approach excelsin navigational fitness in dynamic environments while beingcomputationally efficient.

I. INTRODUCTION

Industrial robotic applications have become essential formass production by automating repetitive labor such asconstruction, sorting, or packaging. These are typically rigidfactory-floor installations where robot manipulators performstringent repetitive motions in the absence of disturbances.However, there is a rising demand for a new class of robots—mobile robots—that are able to safely locomote in humanenvironments and perform a large variety of tasks. A fewprominent examples are self-driving cars, package deliverydrones, and domestic service robots.

However, human environments are much more chaoticthan factory environments. They are vibrant with motion andthere are unforeseen events. As natural as locomotion comesto humans, be it as pedestrians or when piloting a vehicle,it turns out that the demand for efficient and collision-freelocomotion in such dynamic settings is only partially metby contemporary control software of autonomous agents.Even state-of-the-art solutions are characterized by slowupdate rates and only a static model of the environment.We claim that successful control at high velocities includingfast reaction to unforeseen events requires a high controlfrequency and predictive planning which accounts for themotion of other objects.

The state of the art constitutes of shortest path finding ina map in combination with a low-level motion controllerfor short-term collision avoidance. The work of WillowGarage [1] describes a relatively extensive test of this controlparadigm that was conducted over the course of days inan office environment. The world is represented as a three-dimensional voxel grid, which is only used for full-bodycollision checks. The actual navigation is performed in 2D.The shortest path finder is a vanilla A* algorithm in an

1Marcell Missura and Maren Bennewitz are with Humanoid Robots Lab,University of Bonn, Germany

Fig. 1: Our new version of the Dynamic Window Approach com-putes future collision points with moving objects. The red robotwith the number 10 already senses the yellow one approaching andforesightedly picks an evasive trajectory. The DWA is guided by anintermediate target (the smaller red cross) gained from a shortestpath shown with blue lines. Our hybrid world model consists of ablurred occupancy map and a polygonal representation of stationaryand moving objects.

occupancy map. An intermediate target along this path thenserves as a local goal for the Dynamic Window Approach(DWA) that computes velocity commands for the robot. Thisnavigation stack is widely available with the open sourceRobot Operation System (ROS) [2].

There is good reason for the hierarchical split betweenpath planning and collision avoidance. The DWA alone isprone to get stuck in local minima such as U shaped traps,like any motion planner that plans only a short time ahead.The A* path search, however, can quickly compute a coarsepath to the target that guides the DWA and prevents it fromgetting stuck.

The DWA has been introduced by Fox, Burgard, andThrun [3]. Circular arcs are used as a simplification toapproximate the otherwise complex trajectories of non-holonomic vehicles. After systematic sampling of such arcs,the best of them is elected according to an objective functionthat evaluates progress towards the target, but also the oc-currence of potential collisions. The selected arc then yieldsthe next velocity command that is sent to the robot, and theentire process is reiterated.

Brock and Khatib proposed the Global Dynamic WindowApproach [4] where an NF1 navigation function is used toguide the DWA. The NF1 function is basically a flood fill ofthe occupancy map that begins at the target and recursivelyincreases the value of each neighbouring cell until blockedby an obstacle. In the end, one obtains a local minimum-freegradient map that has the target as its only global minimum.The NF1 table can then be used to support the objective

(a) Dilation (b) Contour detection (c) Removal (d) Blur

Fig. 2: The steps of our assumed perception pipeline that are needed to compute grid and polygonal representations of static and dynamicobjects. The yellow object is a robotic agent in motion. a) First, an occupancy map is computed from the raw sensor data and dilatedby the radius of the robot. b) Using contour detection, polygons are computed from the occupancy map. c) Cells that belong to movingobjects are removed, but the polygons remain. d) A blur filter smoothes the inflated occupancy map.

function of the DWA. While it is a bit costly to compute,the NF1 table captures obstacles only as a static snapshotand there is no means of predicting motion. Apart from theNF1 table, a holonomic motion model was suggested in [4]that we adopted as an alternative candidate to the circulararc shaped trajectories. We introduce them later on in detailand evaluate their performance.

Ogren et al. [5] harnessed the global DWA scheme pro-posed above and proved with a few modifications in aModel Predictive Control setting safe convergence to goal.Essentially, the same NF table is used and the proof wasgiven only in a static setting where the map does not changewhile the agent is moving.

Earlier versions of navigational software relied on theforce field method as suggested by Khatib[6]. Obstacles inthe environment exert a repulsive force on the agent while thetarget serves as an attractor. While simple and effective, thismethod is also susceptible to getting stuck in local minima.An improved version that mitigates this problem has beensuggested by Rimon and Koditschek [7]. However, again astatic environment is assumed. Even if the potential fieldscould be updated in real time, the purely reactive nature ofthis approach renders foresight of motion out of reach.

Newer trends of dynamic motion planning approaches at-tempt to account for dynamically changing environments byachieving a high enough update rate such that sensed changescan be incorporated and a trajectory can be replanned. Onecategory relies on heuristic search of control actions with theA* algorithm as proposed by Likachev et al. [8] for cars andby Liu et al. [9] for quadrocopters. Computational feasibilityis achieved by discretization of the state space rather than thecontrol space based on motion primitives that would movethe vehicle between points on a lattice.

Yet another class of dynamic motion planning algorithmsattempts to refine a path to a dynamically feasible motiontrajectory on the fly. Starting with a coarse path such asthe output of an A* path planner, quintic splines [10], B-splines [11], or elastic bands [12] are used in a manner ofonline optimization. While the path to begin with is collisionfree, the refined trajectory is not necessary so, as collisionschecking of higher order curves can be time consuming.

Neither of the aforementioned methods models the mo-

tion of other objects in the environment. Even when theenvironment model is updated frequently, moving obstaclesare assumed to be stationary. This may be sufficient forobstacle avoidance at low velocities, but foresighted evasivemaneuvers are not possible.

Our method represents moving objects explicitly as poly-gons with a velocity attached. Using this velocity, predictionsof future states can be made, or more precisely, the trajecto-ries sampled by the DWA can be intersected with the edges ofa moving polygon. This way, predictive collision detectioncan take place and obstacles are avoided in a foresightedmanner. We introduce a fast numerical method that computesintersections between the circular arcs of the DWA and edgesof moving polygons, but we also investigate a holonomicmotion model that comes with a significant computationaladvantage.

In the remainder of this manuscript, first we introduceour perception pipeline and world modeling technique thatcomprises a hybrid representation of objects as grid cellsand as polygons. Then, we revisit the DWA algorithm andour reconstruction of it including the extension that allowsto compute intersections with moving obstacles using theclassic circular arc trajectories. After that, we introduce aholonomic motion model that can be used instead of thearcs and explain how collision points can be computed muchmore efficiently with it. Finally, we present experimentalresults that quantify the computational and navigationalfitness of our DWA in a dynamic multi-agent environmentwhen using each type of curve.

II. WORLD MODELING

For the motion planning concepts presented in this con-tribution to work as intended, we make the following as-sumptions. We assume that we have a dilated and blurredoccupancy map available that contains only stationary ob-jects. Furthermore, stationary and moving objects are bothrepresented as polygons as well. The processing steps of ourassumed perception pipeline, which for now only exists as amock-up in simulation, are sketched in Figure 2.

An occupancy grid can be directly obtained from sensordata by marking blocked cells that fall onto objects in thesensor range of the robot. A dilation filter can then be used to

expand clusters of blocked cells by the radius of the robot (ora little more). After the obstacles have been inflated this way,the robot can be regarded as a point. Polygons can be gainedfrom the dilated occupancy map using contour detection.We assume that by associating polygon observations overmultiple frames, we are able to measure the velocities ofthe polygons and can thus group them into two classes:stationary and moving. The cells corresponding to movingpolygons are removed from the occupancy grid. A simplebox blur filter then smoothes the occupancy grid.

Dilation, blurring, and contour detection algorithms arereadily available in the OpenCV library. The association ofpolygons over multiple frames can be handled by the Hun-garian Algorithm and their tracking, i.e., velocity estimation,can be done by Kalman filtering.

Our system exploits each part of this hybrid world repre-sentation in a specific way in order to be maximally efficient.The blurred occupancy grid is used for fast collision checkingwith stationary objects. The blurring results in careful drivingwith more clearance from the obstacles that can be sacrificedwhen having to move through narrow gaps. The polygons ofthe stationary objects are used for shortest path finding in apolygonal scene with the Minimal Construct algorithm [13],which is faster and smoother than an A* search in the grid.Finally, the moving polygons are used for dynamic collisionchecking with moving objects.

III. THE DYNAMIC WINDOW APPROACH

The core philosophy of the DWA algorithm is to sample aset of motion control parameters and to predict their outcomewhen applied to the current state of motion, assuming thecontrols stay constant for a short time. The trajectoriesresulting from the control parameters are then evaluatedaccording to an objective function that captures obstacleclearance and progress towards a target. The controls ofthe best trajectory elected by the objective function are thenpassed on to the robot to produce the next motion increment.This entire process is repeatedly executed while the robot isin motion. Figure 1 shows an illustration of robotic agentsusing the DWA in an office environment.

A. Trajectory Generation

In order to model non-holonomic motion of wheeledrobots, we regard the unicycle model. The state of theunicycle is defined by the vector s = (x, y, θ, v, ω), where(x, y) are the Cartesian coordinates, θ is the heading of therobot, v is the linear (forward) velocity, and ω is the angularvelocity. If we assume constant accelerations av and aω tobe the control parameters where av is the linear accelerationand aω is the angular acceleration, then the motion of theunicycle is described by the differential equation

xy

θvω

=

v cos θv sin θωavaω

. (1)

If we are to compute a future state at time t given the currentstate s0 and the controls (av, aω), then we need to integrateeq. (1)

s(av, aω, t) = s0+

∫ t0(v0 + avτ) cos (θ0 + ωτ + aω

2 τ2)dτ∫ t

0(v0 + avτ) sin (θ0 + ωτ + aω

2 τ2)dτ

ωt+ aω2 t

2

avtaωt

.

(2)Unfortunately, this integration is not at all easy. An exact, butalso computationally involved approximation of this integralhas been shown by [14]. The classic version of the DWA [3]circumvented this integration by sampling pairs of linear andangular velocities as controls rather than accelerations. Whenassuming constant velocities (v, ω), the type of trajectory anon-holonomic vehicle follows is a circular arc and the stateprediction function simplifies to

s(v, ω, t) = s0 +

r(sin(θ + ωt)− sin(θ))r(cos(θ + ωt)− cos(θ))

ωt00

, (3)

where r = vω is the radius of the circular arc.

B. Control SamplingWe adopt the circular arcs in our implementation, but we

sample our controls in acceleration space and generate a setof control pairs

C = {(avi , aωj )|i, j ∈ {0, ..., N − 1},

avi = −A+ i2A

N − 1,

aωj = −B + j2B

N − 1}, (4)

where A is the bound of the linear acceleration, B is thebound of the angular acceleration, and N is the number ofsamples per dimension. We set N = 7 and obtain 49 controlsin total. We then convert the acceleration pairs to a set ofvelocities

C ′ = {(vi, ωj)|i, j ∈ {0, ..., N − 1},vi = v0 + δaviT,

ωj = ω0 + δaωjT}, (5)

where (v0, ω0) is the current velocity of the robot, T isthe total time horizon of the DWA, and δ ∈ [0, 1] is atuning parameter that determines at which portion of [v0, vT ]and [ω0, ωT ] the velocities are taken. We set the DWA timeT = 0.2 and δ = 0.5.

C. Objective FunctionEach of the velocity pairs in C ′ is then evaluated with

respect to the objective function

F (v, ω) = α gridclearance(v, ω)

+ β polygonclearance(v, ω)

+ γ progress(v, ω). (6)

Our objective function F consists of three parts, includingtwo types of clearance functions. The grid clearance function

gridclearance(v, ω) =−max{grid(s (v, ω, tk)) |

k ∈ {1, ...,K}, tk = kT

K} (7)

captures the proximity of static obstacles that are representedby our blurred occupancy map. It is computed by predictingthe location of the controlled agent using eq. (3) at timestk sampled from the interval [0, T ] and evaluating the occu-pancy grid in these points. Since we dilate the occupancygrid, obstacles are relatively thick and a low number ofsamples is sufficient. In our implementation, we use onlyK = 2 samples—one in the middle of the trajectory and oneat the end point. The evaluation of the occupancy map withthe grid() function amounts to a table lookup that returns ablurred occupancy value in the [0, 1] range.

The polygonal clearance function polygonclearance()captures potential collisions with moving objects that arerepresented as polygons with a velocity attached. It returnsa clearance indicator tc

T ∈ [0, 1] that is computed from thecollision time tc ∈ [0, T ]. The collision time indicates at whattime in the future the first collision with any polygon edgewill occur. If no collision occurs at all, or the first collisionoccurs after the DWA time (tc > T ), tc is set to T . Thedetails of the polygonal clearance function are explained insection III-D after the presentation of the objective function.

The progress indicator

progress(v, ω) = 1− |s (v, ω, T )− g|argmax(vi,ωj)∈C′ |s (vi, ωj , T )− g|

(8)computes the normalized Euclidean distance between the endpoint of the trajectory and a target location g that is gainedfrom the path planner.

Each component of the objective function is a normalizedvalue in the [0, 1] range. Thus, it is easy to weight themwith the parameters α, β, and γ in eq. (6). We set α = 0.8,β = 1.0, and γ = 0.5. Finally, the acceleration pair (a, b) isreturned that produced the velocities (v, ω) that maximizesthe objective function (6).

Note that our objective function is different from the oneoriginally suggested in [3]. We do not use the heading orthe velocity of the robot to measure progress, but use theEuclidean distance to an intermediate target instead. Our gridclearance is essentially the same as in [3], except that we takethe maximum of only two samples. The component for thegeometric clearance is new.

Furthermore, we omit the computation of the admissibil-ity of the trajectories. In the classic version, inadmissibletrajectories are discarded before being evaluated by theobjective function. We argue that it is harmful to discardinadmissible trajectories, because cases may occur wherechoosing an inadmissible trajectory is the only option. Forexample, if an object suddenly appears in front of the agentthat has not been sensed before—this could be a childchasing a ball that just rolled out onto the street—and a

collision seems unavoidable, it is still better to aim for thebest inadmissible trajectory that maximizes the time untilcollision than the controller not knowing what to do at all.Instead of the admissibility check, we simply differentiatebetween trajectories that are in collision, i.e., tc < T , andones that are not. We elect the best candidate according toF only among the trajectories that are not in collision. Ifthere is no such trajectory, then we choose the trajectorywith the largest polygon clearance tc. Naturally, not havingto compute the admissibility benefits the computation time.

Having chosen the best velocity (vi, ωj) ∈ C ′, we re-turn the associated acceleration (avi , aωj

) ∈ C. This is asignifcant difference between our version and the classicDWA. Accelerations are closer to physical feasibility thansetting velocities directly and can thus control a robot moresmoothly.

D. Dynamic Collision Checking

Fig. 3: Dynamic collision check between a moving polygon edge(p, q) with velocity vp and a circular trajectory around the centerc. The robot is currently at the position (x, y).

The polygonal clearance function polygonclearance()used in eq. (6) computes the time tc of a future collisonbetween the agent traveling along the circular arc definedby the unicycle state s = (x, y, θ, v, ω) and a set of movingpolygons. To intersect the motion of the unicycle with thepolygons, an intersection has to be computed with everyedge of every polygon in the set to find the earliest one.Since we are using only the polygons that represent movingobjects in the sensory range of the robot, the number ofedges is typically very small and does not toll an excessivecomputational burden.

Let (p, q) = (px, py, qx, qy) be a polygon edge definedby its end points and vp = (vpx , vpy ) the velocity of thepolygon. To compute a dynamic intersection with this edge,we first rectify the task by centering the coordinate system onthe center of the circular arc, rotating around the center suchthat the edge is horizontal, and normalizing by the radius ofthe arc. The center of the arc is given by

c =

(xy

)+R

(θ +

π

2

)(r0

), (9)

where R is a rotation matrix and r = vω is the radius of the

arc. Let ψ = arctan(qy−pyqx−px

)be the angle of the polygon

edge with respect to the x-axis. Then the rectified edge and

its velocity are

p′ =1

rR (−ψ) (p− c), (10)

q′ =1

rR (−ψ) (q − c), (11)

v′p =1

rR (−ψ) vp. (12)

Furthermore, we need the angle φ = θ−ψ− sgn(r)π2 of theposition of the robot around the arc center c with respect tothe horizontal. This is illustrated in Figure 3. Then, the timetc of collision between the edge and the robot is the root ofthe equation

sin(φ+ ωtc)− v′py tc − p′y = 0. (13)

Unfortunately, we cannot solve for the root algebraically.We have to use a numerical method instead. We found thatthree iterations of the Illinois algorithm yield good results.Once tc is found, and if tc < T , we need to check if p′x <cos(φ + ωtc) − v′pxtc < q′x to make sure that the collisionpoint is between the left and right boundaries of the rectifiededge, assuming p′x < q′x.

Before the numerical method is run, a bounding box testcan be applied using the boolean function

B(p′, q′, v′p) = (p′y > 1 ∧ p′y + v′pyT > 1

∨ p′y < −1 ∧ p′y + v′pyT < −1∨ p′x > 1 ∧ p′x + v′pxT > 1

∨ q′x < −1 ∧ q′x + v′pxT < −1), (14)

assuming p′x < q′x, in order to quickly discard cases thatcannot possibly intersect. In all discarded cases, also iftc > T or of the collision point is out of bounds, we settc = T .

IV. HOLONOMIC MODEL

The need for a numerical method to intersect circulararcs with moving edges is unsatisfying. Thus, we consider acomputationally efficient model—a holonomic one [15].

A. Trajectory Generation

Holonomic motion in a 2D plane is characterized by thedifferential equation

xyvxvy

=

vxvyaxay

, (15)

where the state of motion is s = (x, y, vx, vy) with (x, y) theCartesian coordinates in the plane and (vx, vy) the respectivevelocities along the x and y axes. This system does nothave an explicit orientation, even though the direction ofthe motion θ = arctan(

vyvx) does implicitly give a heading.

The control parameters are the accelerations (ax, ay). This

time, we can easily integrate the equation of motion (15) andobtain the state prediction function

s(ax, ay, t) =

x+ vxt+

12axt

2

y + vyt+12ayt

2

vx + axtvy + ayt

. (16)

B. Control Sampling

The sampling of controls for the holonomic model in aDWA fashion becomes

C = {(axi, ayj )|i, j ∈ {0, ..., N − 1},

axi= −A+ i

2A

N − 1,

ayj = −A+ j2A

N − 1}, (17)

where A is the bound of the linear acceleration.

C. Objective Function

The objective function remains the same as equa-tion (6), except that F (ax, ay) now evaluates holonomicacceleration inputs. The state prediction function that thegridclearance() and progress() components use is nowequation (16) instead of equation (3).

D. Dynamic Collision Checking

The most intriguing part of the holonomic model is theeasiness of dynamic collision checking, i.e., the implemen-tation of the polygonclearance() function. Again, an inter-section has to be computed for every polygonal edge to findthe earliest time of collision tc. Let (p, q) = (px, py, qx, qy)be a polygon edge and vp = (vpx , vpy ) the velocity of thepolygon. In the holonomic case, we can subtract vp from thevelocity of the robot in order to regard the polygon edge asstationary and set

v′x = vx − vpx (18)v′y = vy − vpy . (19)

We then express the edge in a normal form

y = ax+ b. (20)

by setting

a =qy − pyqx − px

, (21)

b = py − apx. (22)

Then, we use x(t) and y(t) from equation (16), with sub-tracted vp, in (20) and obtain

y + v′ytc +1

2ayt

2c = a

(x+ v′xtc +

1

2axt

2c

)+ b. (23)

Finally, solving for tc yields

tc =(v′y − av′x)(ay − aax)

±

√(v′y − av′x)2

(ay − aax)2− 2(y − ax− b)

(ay − aax).

(24)

From the two possible solutions, we are interested in thesmaller positive one. If one exists and tc < T , we checkif px < x+ v′xtc +

12axt

2c < qx, assuming px < qx. If the

boundary conditions are true, we identified a collision timetc. Otherwise we set tc = T and assume no collision.

E. Conversion

In order to use a holonomic model to control a non-holonomic vehicle, a conversion method is needed. First, thenon-holonomic input state snh = (xnh, ynh, θ, v, ω) needsto be converted to a holonomic one sh = (xh, yh, vx, vy) bysetting

xh = xnh

yh = ynh

vx = cos(θ) v

vy = sin(θ) v. (25)

Then, sh can be used to execute the holonomic DWA. Thisyields the acceleration (ax, ay) that maximizes the objectivefunction. However, this is a holonomic acceleration and it isnot directly applicable to control a non-holonomic vehicle. Itneeds to be converted to a reasonable non-holonomic controlsignal (av, aω). Exploiting the fact that the non-holonomicvelocity v = ‖(vx, vy)‖ is the equivalent of the norm of theholonomic velocity, and the non-holonomic acceleration avis the first derivative of v with respect to time, we compute

av =d

dt

√(vx + axt)2, (vy + ayt)2. (26)

The angular acceleration aω is a little bit more tricky. If thenon-holonomic heading θ = arctan(vy, vy) is the directionof the holonomic motion, then the angular velocity shouldbe its first derivative with respect to time

ωtentative =d

dtarctan(vy + ayt, vx + axt). (27)

However, since (ax, ay) is the output of the DWA, thisderivative is already not continuous, and we would haveto take the second derivate of the heading in order toobtain aω . Instead of attempting to take the first derivativeof a fluctuating quantity, we obtain aω by using a simplecontroller

aω =ddt arctan(vy + ayt, vx + axt)− ω

σ, (28)

where σ = 0.01 s is the time period of the main controlloop and ω is the current angular velocity of the non-holonomic vehicle. Finally, we bound the angular accelera-tion −B < aω < B. This way, we obtain a controller thatsteers as fast as it can towards the angular velocity theDWA commands, but maintains continuous curvature. In oursimulation this works quite nicely.

V. EXPERIMENTAL RESULTS

We have performed extensive simulation experiments inorder to test our implementation of the DWA. We usedtwo different maps that we filled successively with more

Fig. 4: The maps that were used in our experiments. Left: voidmap. Right: office map.

and more moving agents. The maps are shown in Figure 4.The map on the left is void space with two targets. Agentsare commanded to alternate between the two targets andinevitably get in each others’ way. This map isolates thecapability of the controllers to deal with moving obstacles.The map on the right is an office environment characterizedby narrow spaces. Each room contains a potential target.Agents are commanded to drive to a succession of randomtargets. With a growing number of active agents, the spacegets quickly congested. The accompanying video materialgives a visual impression of these experiments.

On each map, we successively added one more agent aftera half an hour of simulation up to a total of ten agents inthe map at the same time. After every half an hour witha fixed number agents, we record the number of collisionsthat occured between any two agents, the number of targetsreached in total by any agent, and the average runtime ofthe DWA controllers. The entire process has been repeatedfor four different controllers: our new implementation of theDWA using arcs and then the holonomic model, and a classicversion of the controllers with each type of curve wheremoving objects are not explicitly modeled, but only appearas cells in the occupancy map. The results are shown inFigure 5.

In the left column of Figure 5, it can be clearly seenthat both new controllers perform well, much better thanthe classic controllers. Up to approximately 6 or 7 agentsin the map, the total number of goals reached increaseswhile the number of collisions remains relatively small.Adding more agents to the map results in a decrease ofperformance. The classic controllers seem paralyzed startingfrom a number of 4 agents. They block each others’ way anddon’t make any progress. This is the reason why we see solittle collisions with the classic controllers on this map. Thearc-based controller performs a bit better than the holonomicone, even though its execution is much more costly. Theincrease in runtime clearly stems from having to process thenumeric intersection with the increasing number of movingpolygons. We observe the same effect on both maps. Even so,the runtime remains under one millisecond at all times. Thepredictive holonomic DWA and both classic ones computein a fraction of a millisecond.

All controllers perform perfectly fine when driving only

0

500

1000

1500

2000

2500

3000

1 2 3 4 5 6 7 8 9 10

Goa

ls

#Agents

Void Goals

Arc PredictiveHolo Predictive

Arc ClassicHolo Classic

0

500

1000

1500

2000

2500

3000

3500

4000

1 2 3 4 5 6 7 8 9 10

Goa

ls

#Agents

Office Goals

Arc PredictiveHolo Predictive

Arc ClassicHolo Classic

0

500

1000

1500

2000

2500

3000

1 2 3 4 5 6 7 8 9 10

Col

lisio

ns

#Agents

Void Collisions

Arc PredictiveHolo Predictive

Arc ClassicHolo Classic

0

500

1000

1500

2000

2500

3000

3500

1 2 3 4 5 6 7 8 9 10

Col

lisio

ns

#Agents

Office Collisions

Arc PredictiveHolo Predictive

Arc ClassicHolo Classic

0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

1 2 3 4 5 6 7 8 9 10

Run

time

[ms]

#Agents

Void Runtimes

Arc PredictiveHolo Predictive

Arc ClassicHolo Classic

0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

1 2 3 4 5 6 7 8 9 10

Run

time

[ms]

#Agents

Office Runtimes

Arc PredictiveHolo Predictive

Arc ClassicHolo Classic

Fig. 5: Plots of the data recorded during our experiments. The left column shows the results of the experiments in the void map. Theright column contains the plots of the experiments performed in the office map.

one agent in the office map. They accelerate the robot upto 15 m/s when having the opportunity to do so. Neitherof the controllers ever collides with the walls. Collisionsare always due to errors in interaction between multiplerobots. When increasing the number of agents, the classiccontrollers produce significantly more collisions while thenew controllers can fairly reliably handle up to six or sevenagents. At this point, the classic controllers lack performanceand they reach much fewer targets. The predictive controllerscontinue to perform, even though the number of collisionsincreases. Again, the arc controller performs better than theholonomic one at the cost of an order of magnitude slowerexecution. According to our observation, the main issue arecongestions. Eventually, many robots gather in the sameplace and block each others’ way. Neither of them is thenable to make much progress. The arc controller is wigglingthe robot in such situations, as it has the action of turning inplace available, even when a robot is blocked from all sides.The wiggling introduces random impulses to the congestedgroup and often helps to dissolve the crowd. The holonomiccontroller shows no such wiggly behavior. It nearly freezeswhen the robot is blocked and recovers only by chance.

VI. CONCLUSIONS

We have introduced a new type of predictive DWA con-troller that explicitly models moving objects and takes theirmotion into account by predicting future collisions. Wecould demonstrate in our experiment that if we take awaythe predictiveness, the controllers perform much worse. Wecompared two distinct types of motion models—arcs anda holonomic model—and found interesting conclusions. Thearc-based controller has an advantage over the holonomic onedue to an unintended wiggly behavior by which it managesto free itself from congestions. However, this advantagecomes at a loss of top computational performance thatallows motion planning at higher frequencies than it hasbeen possible so far. In future work we intend to reduce thenumber of collisions and to research ways of dealing withcongestions.

REFERENCES

[1] E. Marder-Eppstein, E. Berger, T. Foote, B.P. Gerkey, and K. Konolige.The office marathon: Robust navigation in an indoor office environ-ment. In Proc. of the IEEE Int. Conf. on Robotics & Automation(ICRA), 2010.

[2] M. Quigley, B. Gerkey, K. Conley, J. Faust, T. Foote, J. Leibs,E. Berger, R. Wheeler, and A. Ng. ROS: an open-source robotoperating system. In ICRA Workshop on Open Source Robotics, 2009.

[3] D. Fox, W. Burgard, and S. Thrun. The dynamic window approach tocollision avoidance. IEEE Robotics Automation Magazine, Mar 1997.

[4] Oliver Brock and Oussama Khatib. High-speed navigation using theglobal dynamic window approach. In Proc. of the IEEE Int. Conf. onRobotics & Automation (ICRA), pages 341–346, 1999.

[5] P. Ogren and N. E. Leonard. A convergent dynamic window approachto obstacle avoidance. IEEE Transactions on Robotics, 21(2):188–195,April 2005.

[6] O. Khatib. Real-time obstacle avoidance for manipulators and mobilerobots. In Proceedings. 1985 IEEE International Conference onRobotics and Automation, volume 2, pages 500–505, March 1985.

[7] E. Rimon and D. E. Koditschek. Exact robot navigation using artificialpotential functions. IEEE Transactions on Robotics and Automation,8(5):501–518, Oct 1992.

[8] Maxim Likhachev and Dave Ferguson. Planning long dynamically fea-sible maneuvers for autonomous vehicles. The International Journalof Robotics Research, 28(8):933–945, 2009.

[9] Sikang Liu, Nikolay Atanasov, Kartik Mohta, and Vijay Kumar.Search-based motion planning for quadrotors using linear quadraticminimum time control. CoRR, abs/1709.05401, 2017.

[10] B. Lau, C. Sprunk, and W. Burgard. Kinodynamic motion planning

for mobile robots using splines. In Proc. of the IEEE/RSJ Int. Conf.on Intelligent Robots & Systems (IROS), 2009.

[11] Vladyslav C. Usenko, Lukas von Stumberg, Andrej Pangercic, andDaniel Cremers. Real-time trajectory replanning for mavs usinguniform b-splines and 3d circular buffer. CoRR, abs/1703.01416, 2017.

[12] C. Rosmann, F. Hoffmann, and T. Bertram. Kinodynamic trajectoryoptimization and control for car-like robots. In Proc. of the IEEE/RSJInt. Conf. on Intelligent Robots & Systems (IROS), pages 5681–5686,Sept 2017.

[13] M. Missura, D. D. Lee, and M. Bennewitz. Minimal construct:Efficient shortest path finding for mobile robots in polygonal maps.In Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots & Systems(IROS), 2018 (to appear).

[14] M. Missura and S. Behnke. Efficient kinodynamic trajectory genera-tion for wheeled robots. In Proc. of the IEEE Int. Conf. on Robotics& Automation (ICRA), 2011.

[15] M. Missura, D. D. Lee, O. von Stryk, , and M. Bennewitz. Thesynchronized holonomic model: A framework for efficient motiongeneration. In Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots

& Systems (IROS), 2017).