Contents

Concepts in robot motion planning

Path planning methods
road maps
Mobile Robots cell decomposition
potential fields
Obstacle avoidance techniques
9) Navigation
Bibliography: Introduction to Autonomous Mobile Robots (chapter 6)
R. Siegwart, and I. Nourbakhsh, MIT Press, 2004
Handbook of Robotics (chapter 35)
B. Sicilian, and O. Khatib (Eds.), Springer, 2008
Robot Motion Planning
J.-C. Latombe, Kluwer, 1991

10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey 1 10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey 2

Concepts in path/motion planning Navigation

Tasks that may require path planning:
Objective: find a collision-free path from one pose to
Navigation: finding a collision-free path from one state to another
another.
Coverage: passing a sensor or a tool over all points in a space
Localization: using a map to determine the state of the robot Do we have a model of the environment or only the direction
Mapping: construct a representation of the environment to the goal?

Algorithm properties: If a map exists, are all obstacles included?

Optimality: does the planner find trajectories that are optimal in some
Do we need to take into account geometry, kinematics
sense (length, execution time, energy consumption)?
constraints, and/or the dynamics of the robot?
Completeness: does the planner always find a solution when one
exists? Note that most techniques make the assumption of a mass-less,
Computational complexity. holonomic, point-like robot => may require low-level motion control and
Offline / online (sensor-based: interleaving sensing, computation and a priori expansion of obstacles to be implemented robustly.
action).

10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey 3 10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey 4
 Path planning vs obstacle avoidance Approaches to path planning
Capture the connectivity of free space into
Path planning (or motion planning) a graph that is subsequently searched for
Strategic: planning the global trajectory paths:
Given a map (metric, grid-based or topological) and a goal location
Road-map: identify a set of routes within
Rather cognitive: planning of a series of actions => time consuming the free space
Obstacle avoidance Cell decomposition: discriminate between
Tactical: modulating the trajectory to avoid unforeseen, local obstacles free and occupied cells -> connectivity graph
No map or only local and sensor-based Treat robot as a particle under the
Rather reactive: no complex sensor processing => fast influence of an artificial potential field
Notes:
Obstacle avoidance is required in mobile robotics (as opposed to industrial
robotics) because environments are often not fully predictable (uncertainty
in sensors and maps, as well as presence of dynamic obstacles).
Path planning is often less complicated in mobile robotics (as opposed to
industrial robotics) because there are less DOF and a kinematic model is
often sufficient (because inertia is comparatively smaller). However, mobile
robots usually need to update their plans more frequently (because of
discrepancies between map en real environment or control errors).
10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey 5 10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey (adapted from Siegwart & Nourbakhsh, 2004, ch. 6) 6

Graph search strategies Visibility graph

Breadth-first search The challenge is to construct a set of roads that enable the robot to go
from start to goal, while minimizing the number of total roads.
Exhaustively searches the entire graph
without considering the goal until it finds it. Joining all pairs of
vertices that can see
each other (including
Depth-first search initial and goal position).
Explores as far as possible along
each branch before backtracking. Search for shortest path
length using a graph
search technique.
Best-first search (e.g. A*)
+ Complete.
Optimizes search by expanding the most promising node chosen
according to some rule. - Tend to graze obstacles
Typically used for route finding: the straight-line distance, which is => requires to grow
usually an approximation of road distance. obstacles to avoid
collisions.

10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey 7 10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey (adapted from Siegwart & Nourbakhsh, 2004, ch. 6) 8
 Voronoi diagram Cell decomposition
For each point in the free space compute its distance from the nearest
obstacle (growing circles) => results in sharp ridges.
Divide space into simple, connected regions called cells.
When the configuration space obstacles are polygons, the Voronoi
Determine which open cells are adjacent and construct a
diagram consists of straight and parabolic segments.
connectivity graph.
Tends to maximize the clearance between the robot and the obstacles.
Find cells in which the initial and goal positions lie and
+ May be straight forward to
execute when using range
search for a path in the connectivity graph to join them.
scanner (the robot maximizes From the sequence of cells found with an appropriate
the readings of local minima search algorithm, compute a trajectory within each cell, e.g.
in its sensor values). passing through the midpoints of cell boundaries or
- Far from optimal in the sense by sequence of line following movements.
of total path length.
Two families of cell decomposition methods:
- May be problematic for
exact: cells are either completely free or completely occupied;
localization with short range
approximate: not taking care of the obstacle geometry.
sensors since no obstacle may
be sensed most of the time.
10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey (adapted from Siegwart & Nourbakhsh, 2004, ch. 6) 9 10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey (adapted from Siegwart & Nourbakhsh, 2004, ch. 6) 10

Exact cell decomposition Approximate cell decomposition (1)

Underlying First example: adaptive cell decomposition:
assumption: the The rectangle is decomposed into 4 identical rectangles.
particular position of If the interior of a rectangle lies completely in free space or on an
the robot within each obstacle, it is not further decomposed. Otherwise, it is recursively
cell of free space does decomposed into 4 rectangles, and so on.
not matter.
+ Number of cells
+ Adapted to the
depend on density and
complexity of the
complexity of objects
environment
in the environment.
+ Complete.
- May not be complete
- Tend to be complex to (depending on the final
implement and is cell size).
dependent on the
density and complexity
of obstacles.
10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey (adapted from Siegwart & Nourbakhsh, 2004, ch. 6) 11 10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey (adapted from Siegwart & Nourbakhsh, 2004, ch. 6) 12
 Approximate cell decomposition (2) Grid search strategy
Wavefront expansion (also called NF1 or grassfire)
2nd example: fixed grid-size decomposition ! Numbering the cells from
Narrow passageways can be lost => may not be complete. the goal outwards.
+ Very simple path planning algorithm such as wavefront expansion ! Provides a discrete distance
can be used => computationally efficient. to the goal.
! Link cells that are adjacent
and closer to the goal.
! Using the Manhattan distance :
! Or slightly different metrics, e.g.:

10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey (adapted from Siegwart & Nourbakhsh, 2004, ch. 6) 13 10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey (adapted from Siegwart & Nourbakhsh, 2004, ch. 6) 14

Potential field Potential field generation

Robot is treated as a particle under the Generation of potential field function U(q), where q is the state variable:
influence of an artificial potential field: attracting (goal) and repulsing (obstacle) fields
Goal generates attractive force. summing up the fields
Obstacles are repulsive forces. functions must be differentiable
At every location, the direction of the Generate artificial force field F(q) & 'U #
resulting force is considered the most $ x!
F (q ) = )(U (q ) = )(U att (q ) ) (U rep (q ) = $ ''U !
promising direction of motion.
$ !
%$ 'y "!
If the robot is holonomic, the control can be: set robot speed (vx,vy) proportional
to the force F(q) generated by the field (assuming that the dynamics is
negligible).
If the robot is nonholonomic, a transformation needs to be found. For instance,
with a differential-drive, the rotation speed can be set proportional to the angle !
between F(q) and the current orientation of the robot, whereas the forward
speed can be set proportional to the amplitude of F(q) or F(q)"cos(!) (in order to
avoid moving too fast forward if the current orientation of the robot is not aligned
with F(q) ).!
10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey (adapted from Siegwart & Nourbakhsh, 2004, ch. 6) 15 10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey (adapted from Siegwart & Nourbakhsh, 2004, ch. 6) 16
 Example of attractive potential field Example of repulsing potential field
Parabolic function representing the Euclidean distance Should generate a barrier around all the known obstacles
to the goal: Strong if close to the obstacle.
No influence if far from the obstacle.

Attracting force converges linearly towards 0 (goal):

where "(q) is the minimum distance between q the object
The field is positive or zero and tends to infinity as q gets closer to
the object.

obst

10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey (adapted from Siegwart & Nourbakhsh, 2004, ch. 6) 17 10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey (adapted from Siegwart & Nourbakhsh, 2004, ch. 6) 18

Notes on potential field methods Extended potential field method

Khatib and Chatila (1995)

Additionally a rotation potential field and a task potential field

The potential field methods are usually incomplete.
can be introduced.
Local minima problem exists # can result in oscillations. Rotation potential field
! Force is also a function
Problem is getting more complex if the robot cannot be
of robots orientation w.r.t.
considered as a point mass. the obstacle.
+ Improves wall following
+ Easy to implement efficient and reasonably reliable planners. behaviour.
+ Can be used off-line to draw a complete path that is then Task potential field
followed using a low level controller. ! Filters out the obstacles
that should not influence
+ Can be used on-line to compute only the force present at the robot s motion,
current pose and control the robot accordingly. i.e. only the obstacles
in the sector in front
of the robot are
considered.
10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey (adapted from Siegwart & Nourbakhsh, 2004, ch. 6) 19 10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey (adapted from Siegwart & Nourbakhsh, 2004, ch. 6) 20
 Obstacle avoidance Bug1
The goal of obstacle avoidance algorithms (as opposed to Assuming (imprecise)
path planning) is to avoid collisions with unpredictable objects global localization, but only
or due to uncertainty in the localization process or the map. proximity sensing.

It is either based on a local map or directly uses the sensor

data. Following the obstacle to
avoid collision with it.

know
It is often implemented as an independent task
running at high frequency.

n obs
Each encountered obstacle

v(t),

tacle
Efficient obstacle avoidance methods rved is first fully circled before it
obse cle

$(t) Plan
should take into account:

s (ma
obst
a is left at the point closest to
the sensors readings the goal.

p)
the kinematics / dynamics of the robot

ed p
the overall goal direction

ath
10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey (adapted from Siegwart & Nourbakhsh, 2004, ch. 6) 21 10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey (adapted from Siegwart & Nourbakhsh, 2004, ch. 6) 22

Bug2 Direct connections from sensors to motors

Inspired from Braitenberg (1984)
Wheel speeds are weighted sums
Following the obstacle always on the of raw sensor data.
left or right side. + No (local) map required at all.
Leaving the obstacle as soon as the + Very fast (requires virtually no
line between start and goal is crossed. processing power)
+ More efficient than + Can advantageously exploit
Bug1. sensor non-linearity (e.g. IR proximity
sensors, triangulation sensors).
- Inefficient situations + A tendency towards the goal can be
still exist. superimposed. k4!
k3! k5!
k2!
- Sensitive to local minima. k1! k6!
- Sensitive to sensor response and cross-
sensitivity (e.g. ambient light with IR % %
proximity sensors).

- optiPilot video
10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey (adapted from Siegwart & Nourbakhsh, 2004, ch. 6) 23 10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey 24
 Local, map-less, potential field Local occupancy grid
Brooks (1986)
Each cell represents the confidence of the algorithm in the existence of
Treats range readings as a repulsive force vector, which will
an obstacle at that location, for example:
act on the robot (transformation from force to wheel speeds
a) For each range reading,
depends on kinematics). the cell that lies on the
If the magnitude of the sum of the repulsive forces exceeds acoustic axis and
a certain threshold, the robot stops, turns into the direction corresponds to the
measured distance d
of the resultant force vector, and moves on. is incremented,
+ Reflexive control eliminates cognition altogether. In reflexive increasing the certainty
control there is no planning and reasoning; nor are there value of the cell.
world models. Simple reflexes tie actions to perceptions, b) A histogramic pseudo-
resulting in faster response to outside stimuli. probability distribution
Figure from
is obtained by Borenstein & Koren (1991)
- In this approach, however, only one set of range readings is continuous and rapid
considered, while previous readings are lost. sampling of the
sensors while the
- No provable convergence, not complete.
robot is moving.
10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey 25 10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey (adapted from Moravec & Elfes, 1985) 26

Virtual force field (VFF) Vector field histogram (VFH)

Borenstein & Koren (1989)
Environment represented in a 2D
Each active cell exerts a virtual
occupancy grid: cell values
repulsive force toward the robot. " represents the probability that
The magnitude of this force is there is an obstacle.
proportional to the certainty value Reduction to a polar histogram
and inversely proportional to the representing the obstacle density
distance between the cell and the in each sector.
center of the vehicle. " All openings for the robot to pass
At each iteration, all virtual repulsive are found by applying a
forces are added up to yield the threshold.
resultant repulsive force Fr. " The one with lowest cost
function G is selected.
Simultaneously, a virtual attractive
force Ft of constant magnitude is Borenstein & Koren (1991)
applied to the vehicle, "pulling" it
toward the target."
The summation of Fr and Ft yields
the resulting force vector R."
Example of cost function:
10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey (adapted from Borenstein & Koren, 1989) 27 10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey (adapted from Siegwart & Nourbakhsh, 2004, ch. 6) 28
 Vector field histogram (VFH) - example Curvature velocity method (CVM)
Simmons (1996)

Notes: Adds physical constraints from the robot and the environment on the
velocity space (v,$) of the robot.
+ Smoother resulting
trajectories than VFF. Assuming that robot is traveling
on arcs (c=$/v), which is a good
Narrow passages (e.g. C!
approximation for many wheeled
doors) may be
robots including nonholonomic ones.
problematic.
Obstacles are transformed from
Local minimums can
a local occupancy grid into the
exist.
velocity space.
Reaching of the goal
The robot chooses velocity v!
can not be guaranteed B!
commands that satisfy all the C!
(not complete).
constraints and maximize an
Dynamics of the robot objective function that trades A!
is not considered. off speed, safety and goal
Borenstein et al. $&
directedness.
+ Solution corresponds directly to the commands sent to the robot.
10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey (adapted from Siegwart & Nourbakhsh, 2004, ch. 6) 29 10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey (adapted from Siegwart & Nourbakhsh, 2004, ch. 6) 30

Dynamic window approach (DWA) Integrating path planning Philippsen & Siegwart (2003)

and obstacle avoidance

Fox, Burgard et. al. (1997)

The kinematics of the robot is considered by searching a well chosen

velocity space:
! Robot is assumed to move on arcs (c=$/v) at least during one time step. Dynamic window approach with global path
! The dynamic window is the set of all tuples (v,$) that can be reached within planning
the next sample period, taking into account the acceleration capabilities of Global path generated in advance.
the robot and cycle time. v! Path adapted if obstacles are encountered.
! An objective function is
chosen to select the optimal
velocity within the reduced Optimize the following function:
dynamic window:
Max( a ! speed + b ! dist + c ! goal _ heading )
encourages
alignment to the goal
fast forward motion video
large distance to obstacle $&
DWA solves the problem in the control space using information of the vehicle dynamics,
thus it is well adapted to vehicles with slow dynamic capabilities or that work at high speed.
10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey (adapted from Siegwart & Nourbakhsh, 2004, ch. 6) 31 10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey (adapted from Siegwart & Nourbakhsh, 2004, ch. 6) 32
 CMU helicopter CMU helicopter example (cont d)
example
Scherer et al. (2007, 2008)
A path-planning algorithm generates
a smooth path around obstacles to
the next goal, but without respecting
the dynamics or the need to point in
the direction of travel. 3D occupancy (evidence) grid

The speed controller slows and accelerates the Path

vehicle based on the distance to the closest
reachable obstacle.

The priorities of the local planner are to avoid Left-right, up-down commands
obstacles and then try to follow the path from
2D vector field produced by the
the global planner. It produces
steering commands in both global planner
horizontal and vertical axes
video
using a kind of virtual force field (VFF).
10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey 33 10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey (adapted from Scherer, Singh, Chamberlain, Elgersma 2008) 34

Typical exam questions

obstacle avoidance methods

What is navigation and what matters in mobile robot

(Siegwart & Nourbakhsh, 2004, ch. 6)
Comparison of a few

navigation?
Path planning vs obstacle avoidance.
DWA
Approaches to path planning.
Visibility graph and Voronoi diagram.
CVM
Cell decomposition and search strategies.
Potential field methods.
Obstacle avoidance methods.
Map-less obstacle avoidance.
Occupancy-grid-based methods for obstacle avoidance.
Dynamic window approach for obstacle avoidance.

10-Nov-10 10-Nov-10 Mobile Robots - EPFL - J.-C. Zufferey 36