Autonomous Mobile Robots, Chapter 6 6

Planning and Navigation

Where am I going? How do I get there?

Localization "Position" Cognition

Global Map

Environment Model Path

Local Map

Perception Real World Motion Control

Environment

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2
Competencies for Navigation I
• Cognition / Reasoning :
Ø is the ability to decide what actions are required to achieve a certain
goal in a given situation (belief state).
Ø decisions ranging from what path to take to what information on the
environment to use.
• Today’s industrial robots can operate without any cognition
(reasoning) because their environment is static and very structured.
• In mobile robotics, cognition and reasoning is primarily of geometric
nature, such as picking safe path or determining where to go next.
Ø already been largely explored in literature for cases in which complete
information about the current situation and the environment exis ts (e.g.
sales man problem).

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2
Competencies for Navigation II
• However, in mobile robotics the knowledge of about the environment
and situation is usually only partially known and is uncertain.
Ø makes the task much more difficult
Ø requires multiple tasks running in parallel, some for planning (global),
some to guarantee “survival of the robot”.
• Robot control can usually be decomposed in various behaviors or
functions
Ø e.g. wall following, localization, path generation or obstacle avoidance.
• In this chapter we are concerned with path planning and navigation,
except the low lever motion control and localization.
• We can generally distinguish between (global) path planning and
(local) obstacle avoidance.

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.1
Global Path Planing
• Assumption: there exists a good enough map of the environment for
navigation.
Ø Topological or metric or a mixture between both.
• First step:
Ø Representation of the environment by a road-map (graph), cells or a
potential field. The resulting discrete locations or cells allow then to use
standard planning algorithms.
• Examples:
Ø Visibility Graph
Ø Voronoi Diagram
Ø Cell Decomposition -> Connectivity Graph
Ø Potential Field

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.1
Path Planning: Configuration Space
• State or configuration q can be described with k values qi

• What is the configuration space of a mobile robot?

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.1
Path Planning Overview
1. Road Map, Graph construction 2. Cell decomposition
Ø Identify a set of routes within the free Ø Discriminate between free and
space occupied cells

• Where to put the nodes? • Where to put the cell boundaries?

• Topology-based: • Topology- and metric-based:
Ø at distinctive locations Ø where features disappear or get visible
• Metric-based: 3. Potential Field
Ø where features disappear or get visible Ø Imposing a mathematical function over
the space
© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.1
Road-Map Path Planning: Visibility Graph

• Shortest path length

• Grow obstacles to avoid collisions
© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.1
Road-Map Path Planning: Voronoi Diagram

• Easy executable: Maximize the sensor readings

• Works also for map-building: Move on the Voronoi edges
© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.1
Road-Map Path Planning: Voronoi, Sysquake Demo

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.1
Road-Map Path Planning: Cell Decomposition
• Divide space into simple, connected regions called cells
• Determine which open sells are adjacent and construct a connectivity
graph
• Find cells in which the initial and goal configuration (state) lie and
search for a path in the connectivity graph to join them.
• From the sequence of cells found with an appropriate search algorithm,
compute a path within each cell.
Ø e.g. passing through the midpoints of cell boundaries or by sequ ence of
wall following movements.

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.1
Road-Map Path Planning: Exact Cell Decomposition

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.1
Road-Map Path Planning: Approximate Cell Decomposition

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.1
Road-Map Path Planning: Adaptive Cell Decomposition

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.1
Road-Map Path Planning: Path / Graph Search Strategies
• Wavefront Expansion NF1
(see also later)

• Breadth-First Search

• Depth-First Search

• Greedy search and A*

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.1
Potential Field Path Planning

• Robot is treated as a point under the

influence of an artificial potential field.
Ø Generated robot movement is similar to
a ball rolling down the hill
Ø Goal generates attractive force
Ø Obstacle are repulsive forces

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.1
Potential Field Path Planning: Potential Field Generation
• Generation of potential field function U(q)
Ø attracting (goal) and repulsing (obstacle) fields
Ø summing up the fields
Ø functions must be differentiable
• Generate artificial force field F(q)  ∂U 
 ∂x 
F ( q) = −∇U (q ) = −∇U att ( q) − ∇U rep (q ) =  ∂U 
 
 ∂y 
• Set robot speed (vx, vy) proportional to the force F(q) generated by the
field
Ø the force field drives the robot to the goal
Ø if robot is assumed to be a point mass

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.1
Potential Field Path Planning: Attractive Potential Field
• Parabolic function representing the Euclidean distance to
the goal

• Attracting force converges linearly towards 0 (goal)

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.1
Potential Field Path Planning: Repulsing Potential Field
• Should generate a barrier around all the obstacle
Ø strong if close to the obstacle
Ø not influence if fare from the obstacle

Ø : minimum distance to the object

Ø Field is positive or zero and tends to infinity as q gets closer to the object

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.1
Potential Field Path Planning: Sysquake Demo

• Notes:
Ø Local minima problem exists
Ø problem is getting more complex if the robot is not considered as a point
mass
Ø If objects are convex there exists situations where several minimal
distances exist → can result in oscillations

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.1
Potential Field Path Planning: Extended Potential Field Method
Khatib and Chatila

• Additionally a rotation potential field and a task potential field in

introduced
• Rotation potential field
Ø force is also a function
of robots orientation to
the obstacle
• Task potential field
Ø Filters out the obstacles
that should not influence
the robots movements,
i.e. only the obstacles
in the sector Z in front
of the robot are
considered

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.1
Potential Field Path Planning: Potential Field using a Dyn. Model
Monatana et at.

• Forces in the polar plane

Ø no time consuming transformations

• Robot modeled thoroughly

Ø potential field forces directly acting on the model
Ø filters the movement -> smooth

• Local minima
Ø set a new goal point

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.1
Potential Field Path Planning: Using Harmonic Potentials
• Hydrodynamics analogy
Ø robot is moving similar to a fluid particle following its stream
• Ensures that there are no local minima

• Note:
Ø Complicated, only simulation shown

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.2
Obstacle Avoidance (Local Path Planning)
• The goal of the obstacle avoidance algorithms is to avoid collisions
with obstacles
• It is usually based on local map
• Often implemented as a more or less independent task
• However, efficient obstacle avoidance
should be optimal with respect to

know
Ø the overall goal

n obs
Ø the actual speed and kinematics of the robot

v(t),

tacle
Ø the on boards sensors rved

ω(t) Pla
obse le

s (ma
Ø the actual and future risk of collision obst
ac

p)
ned
path
• Example: Alice

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.2
Obstacle Avoidance: Bug1
• Following along the obstacle to avoid it
• Each encountered obstacle is once fully circled before it is left at the
point closest to the goal

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.2
Obstacle Avoidance: Bug2
Ø Following the obstacle always on the left or right side
Ø Leaving the obstacle if the direct connection between
start and goal is crossed

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.2
Obstacle Avoidance: Vector Field Histogram (VFH)
Borenstein et al.
• Environment represented in a grid (2 DOF)
Ø cell values equivalent to the probability that there is an obstacle
• Reduction in different steps to a 1 DOF histogram
Ø calculation of steering direction
Ø all openings for the robot to pass are found
Ø the one with lowest cost function G is selected

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.2
Obstacle Avoidance: Vector Field Histogram + (VFH+)
Borenstein et al.

• Accounts also in a very simplified way

for the moving trajectories (dynamics)
Ø robot moving on arcs
Ø obstacles blocking a given direction
also blocks all the trajectories
(arcs) going through this direction

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.2
Obstacle Avoidance: Video VFH
Borenstein et al.

• Notes:
Ø Limitation if narrow areas
(e.g. doors) have to be
passed
Ø Local minimum might not
be avoided
Ø Reaching of the goal can
not be guaranteed
Ø Dynamics of the robot not
really considered

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.2
Obstacle Avoidance: The Bubble Band Concept
Khatib and Chatila
• Bubble = Maximum free space which can be reached without any ris k
of collision
Ø generated using the distance to the object and a simplified mode l of the
robot
Ø bubbles are used to form a band of bubbles which connects the start
point with the goal point

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.2

Obstacle Avoidance: Basic Curvature Velocity Methods (CVM)

Simmons et al.

• Adding physical constraints from the robot and the environment on the
velocity space (v, ω) of the robot
Ø Assumption that robot is traveling on arcs (c= ω / v)
Ø Acceleration constraints:
Ø Obstacle constraints: Obstacles are transformed in velocity spac e
Ø Objective function to select the optimal speed

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.2

Obstacle Avoidance: Lane Curvature Velocity Methods (CVM)

Simmons et al.

• Improvement of basic CVM

Ø Not only arcs are considered
Ø lanes are calculated trading off lane length and width to the closest
obstacles
Ø Lane with best properties is chosen using an objective function

• Note:
Ø Better performance to pass narrow areas (e.g. doors)
Ø Problem with local minima persists

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.2

Obstacle Avoidance: Dynamic Window Approach

Fox and Burgard, Brock and Khatib
• The kinematics of the robot is considered by searching a well ch osen velocity space
Ø velocity space -> some sort of configuration space
Ø robot is assumed to move on arcs
Ø ensures that the robot comes to stop before hitting an obstacle
Ø objective function is chosen to select the optimal velocity

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.2
Obstacle Avoidance: Global Dynamic Window Approach
• Global approach:
Ø This is done by adding a minima-free function named NF1 (wave-
propagation) to the objective function O presented above.
Ø Occupancy grid is updated from range measurements

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.2
Obstacle Avoidance: The Schlegel Approach
• Some sort of a variation of the dynamic window approch
Ø takes into account the shape of the robot
Ø Cartesian grid and motion of circular arcs
Ø NF1 planner
Ø real time performance achieved
by use of precalculated table

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.2
Obstacle Avoidance: The EPFL-ASL approach
• Dynamic window approach with global path planing
Ø Global path generated in advance
Ø Path adapted if obstacles are encountered
Ø dynamic window considering also the shape of the robot
Ø real-time because only max speed is calculated
• Selection (Objective) Function:
Max( a ⋅ speed + b ⋅ dist + c ⋅ goal _ heading )
Ø speed = v / vmax
Ø dist = L / Lmax
Ø goal_heading = 1- (α - ωT) / π

• Matlab-Demo
Ø start Matlab
α
Ø cd demoJan (or cd E:\demo\demoJan) Intermediate goal
Ø demoX

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.2
Obstacle Avoidance: Other approaches

• Behavior based
Ø difficult to introduce a precise task
Ø reachability of goal not provable

• Fuzzy, Neuro-Fuzzy
Ø learning required
Ø difficult to generalize

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.2

Comparison
Obstacle Avoidance

D
um
tocA
barntce

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.2

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.2.2

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.3.3
Generic temporal decomposition

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.3.3
4-level temporal decomposition

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.3.3
Control decomposition
• Pure serial decomposition

• Pure parallel decomposition

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.3.4
Sample Environment

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.3.4
Our basic architectural example

Localization Position Cognition

Position
Local Map
Local Map

Perception to

Feedback
Avoidance
Position
Obstacle
Action

Path
Environment Model
Local Map

Real World
Environment
Perception Motion Control

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.3.4
General Tiered Architecture

• Executive Layer
Ø activation of behaviors
Ø failure recognition
Ø re-initiating the planner

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.3.4
A Tow-Tiered Architecture for Off-Line Planning

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.3.4
A Three-Tiered Episodic Planning Architecture.

• Planner is triggered when needed: e.g. blockage, failure

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.3.4
An integrated planning and execution architecture

• All integrated, no temporal between planner and executive layer

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.3.4
Example: The RoboX Architecture

© R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 6 6.3.4
Example: RoboX @ EXPO.02

© R. Siegwart, I. Nourbakhsh