Lecture 11: Navigation Dr. Giorgos A. Demetriou Department of Computer Engineering and Computer Science School of Engineering and Applied Sciences [email protected] http://staff.fit.ac.cy/com.dg All lectures are based on the Lectures developed at ETH by Roland Siegwart, Margarita Chli and Martin Rufli mailto:[email protected] http://staff.fit.ac.cy/com.dg  Navigation is composed of localization, mapping and motion planning Dr. Giorgos A. Demetriou ACSC 414 - Robotics 2 Required Competences for Navigation  We have come a long way since Shakey! Dr. Giorgos A. Demetriou ACSC 414 - Robotics 3 Motion Planning in Action  Motion Planning  State-space and obstacle representation • Work space • Configuration space  Global motion planning • Optimal control (not treated) • Deterministic graph search • Potential fields • Probabilistic / random approaches  Local collision avoidance  BUG  VFH  DWA  ...  Glimpses into state of the art methods  Dynamic environments  Interaction Dr. Giorgos A. Demetriou ACSC 414 - Robotics 4 Outline of this Lecture  The problem: find a path in the work space (physical space) from an initial position to a goal position avoiding all collisions with obstacles  Assumption: there exists a good enough map of the environment for navigation.  Topological  Metric  Hybrid methods Dr. Giorgos A. Demetriou ACSC 414 - Robotics 5 The Planning Problem (1/2)  We can generally distinguish between  (global) path planning and  (local) obstacle avoidance.  First step:  Transformation of the map into a representation useful for planning  This step is planner-dependent  Second step:  Plan a path on the transformed map  Third step:  Send motion commands to controller  This step is planner-dependent (e.g. Model based feed forward, path following) Dr. Giorgos A. Demetriou ACSC 414 - Robotics 6 The Planning Problem (2/2)  State or configuration q can be described with k values qi Dr. Giorgos A. Demetriou ACSC 414 - Robotics 7 Work Space (Map) → Configuration Space  Mobile robots operating on a flat ground have 3 DoF: (x, y, θ)  For simplification, in path planning mobile roboticists often assume that the robot is holonomic and that it is a point. In this way the configuration space is reduced to 2D (x,y)  Because we have reduced each robot to a point, we have to inflate each obstacle by the size of the robot radius to compensate. Dr. Giorgos A. Demetriou ACSC 414 - Robotics 8 Configuration Space for a Mobile Robot Planning and Navigation I: Global Path Planning Dr. Giorgos A. Demetriou ACSC 414 - Robotics 9 1. Optimal Control  Solves for the truly optimal solution  Becomes intractable for even moderately complex and/or nonconvex problems 2. Potential Field  Imposes a mathematical function over the state/configuration space  Many physical metap ...

1. Lecture 11:
Navigation
Dr. Giorgos A. Demetriou
Department of Computer Engineering and Computer Science
School of Engineering and Applied Sciences
[email protected]
http://staff.fit.ac.cy/com.dg
All lectures are based on the Lectures developed at ETH by
Roland Siegwart, Margarita Chli and Martin Rufli
mailto:[email protected]
http://staff.fit.ac.cy/com.dg
planning
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 2
Required Competences for Navigation

2. Dr. Giorgos A. Demetriou ACSC 414 - Robotics 3
Motion Planning in Action
-space and obstacle representation
• Work space
• Configuration space
• Optimal control (not treated)
• Deterministic graph search
• Potential fields
• Probabilistic / random approaches
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 4

3. Outline of this Lecture
from an initial
position to a goal position avoiding all collisions with obstacles
environment for navigation.
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 5
The Planning Problem (1/2)
planning
-dependent
a path on the transformed map

4. -dependent (e.g. Model based feed
forward, path following)
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 6
The Planning Problem (2/2)
e or configuration q can be described with k values qi
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 7
Work Space (Map) → Configuration Space
θ)
mobile roboticists often
assume that the
robot is holonomic and that it is a point. In this way the
configuration space is
reduced to 2D (x,y)
inflate each obstacle
by the size of the robot radius to compensate.
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 8

5. Configuration Space for a Mobile Robot
Planning and Navigation I: Global Path Planning
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 9
1. Optimal Control
truly optimal solution
complex and/or nonconvex problems
2. Potential Field
state/configuration space
ue to its simplicity and
similarity to optimal control solutions
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 10
Path Planning: Overview of Algorithms

6. 3. Graph Search
nodes within the free space
-point boundary problem in the continuum
increases
Algorithms
-Jacobi-Bellman
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 11
Optimal Control based Path Planning Strategies

7. influence of an artificial potential field.
a ball rolling down the hill
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 12
Potential Field Path Planning Strategies
roportional to the force F(q)
generated by the field
-holonomics are
hard to deal with)

8. Dr. Giorgos A. Demetriou ACSC 414 - Robotics 13
Potential Field Path Planning: Potential Field Generation
to the goal
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 14
Potential Field Path Planning: Attractive Potential Field
the object
to the object

9. Dr. Giorgos A. Demetriou ACSC 414 - Robotics 15
Potential Field Path Planning: Repulsing Potential Field
m is getting more complex if the robot is not
considered as a point mass
-convex there exists situations where
several minimal distances exist
→ can result in oscillations
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 16
Potential Field Path Planning:
and a task potential field is introduced
orientation relative to the obstacles.
This is done using a gain factor that
reduces the repulsive force when
obstacles are parallel to robot’s
direction of travel

10. not influence the robots movements,
i.e. only the obstacles in the sector in
front of the robot are considered
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 17
Potential Field Path Planning: Extended Potential Field Method
stream
re no local minima
• Equipotential lines orthogonal on object boundaries (as in
image above!)
• Short but dangerous paths
• Equipotential lines parallel to object boundaries
• Long but safe paths
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 18

11. Potential Field Path Planning: Using Harmonic Potentials
graph
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 19
Graph Search
C wikipedia.org

12. Dr. Giorgos A. Demetriou ACSC 414 - Robotics 20
Graph Construction (Preprocessing Step)
-like obstacles
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 21
Graph Construction: Visibility Graph (1/2)
ause it is the shortest length
path
the robot as close as
possible to the obstacles: the common solution is to grow
obstacles by more than
robot’s radius
polygons

13. Dr. Giorgos A. Demetriou ACSC 414 - Robotics 22
Graph Construction: Visibility Graph (2/2)
Diagram tends to
maximize the distance between robot and obstacles
-building: Move on the Voronoi edges: 1D
Mapping
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 23
Graph Construction: Voronoi Diagram (1/2)
along the Voronoi
diagram using simple control rules
oi diagram tends to keep the robot as far as
possible from
obstacles, any short range sensor will be in danger of failing

14. straight and parabolic
segments
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 24
Graph Construction: Voronoi Diagram (2/2)
connectivity graph
d goal configuration (state)
lie and search for a
path in the connectivity graph to join them.
algorithm, compute
a path within each cell.
daries or by
sequence of wall following
movements.
• Fixed cell decomposition

15. • Adaptive cell decomposition
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 25
Graph Construction: Cell Decomposition (1/4)
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 26
Graph Construction: Exact Cell Decomposition (2/4)
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 27
Graph Construction: Approximate Cell Decomposition (3/4)
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 28
Graph Construction: Adaptive Cell Decomposition (4/4)
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 29
Graph Construction: State Lattice Design (1/2)

16. Dr. Giorgos A. Demetriou ACSC 414 - Robotics 30
Graph Construction: State Lattice Design (2/2)
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 31
Graph Search
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 32
Graph Search Strategies: Breadth-First Search
ds to a wavefront expansion on a 2D grid

17. -found solution is optimal if all edges have equal costs
-sorted” HEAP variation of
breadth first search
-found solution is guaranteed to be optimal no matter the
cell cost
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 33
Graph Search Strategies: Breadth-First Search
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 34
Graph Search Strategies: Depth-First Search
“f(n)-sorted”)
f(n) = g(n) + εh(n)
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 35
Graph Search Strategies: A* Search

18. goal outward
f(n) = g(n) + εh(n)
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 36
Graph Search Strategies: D* Search
(RRT)
-dimensional search spaces
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 37
Graph Search Strategies: Randomized Search
Planning and Navigation II: Obstacle Avoidance
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 38
is to avoid collisions with obstacles

19. independent task
should be optimal with respect to
cs of the
robot
-boards sensors
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 39
Obstacle Avoidance (Local Path Planning)
is once fully circled before it is
left at the point closest
to the goal
required
guaranteed

20. Solution
s are often
highly suboptimal
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 40
Obstacle Avoidance: Bug1
and goal is crossed
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 41
Obstacle Avoidance: Bug2

21. obstacle
• All openings for the robot to pass are found
• The one with lowest cost function G is selected
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 42
Obstacle Avoidance: Vector Field Histogram (VFH)
way for vehicle kinematics
ng on arcs or straight
lines
direction also blocks all the
trajectories (arcs) going through

22. this direction
kinematically blocked trajectories
are properly taken into account
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 43
Obstacle Avoidance: Vector Field Histogram+ (VFH+)
anteed
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 44
Obstacle Avoidance: Limitations of VFH

23. velocity space:
dynamic window approach
considers only circular
trajectories uniquely determined by pairs (v,ω) of translational
and rotational
velocities.
admissible, if the robot is able to
stop before it reaches the closest obstacle on the corresponding
curvature.
(b: breakage)
admissible velocities to those
that can be reached within a short time interval given the
limited accelerations of the
robot
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 45

24. Obstacle Avoidance: Dynamic Window Approach
areas, namely,
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 46
Obstacle Avoidance: Dynamic Window Approach
velocity, the maximum of the
objective function, G(v, ω), is computed over Vr.
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 47
Dynamic Window Approach

25. -free function (e.g. NF1
wave-propagation) to the
objective function O presented above.
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 48
Obstacle Avoidance: GlobalDynamic Window Approach
Planning and Navigation III: Architectures
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 49
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 50
Basic architectural example

26. -think-act)
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 51
Control decomposition
viors
-initiating the planner
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 52

27. General Tiered Architecture
-time capable
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 53
A Three-Tiered Episodic Planning Architecture
executive layer →
see case study
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 54
An integrated planning and execution architecture

28. Planning and Navigation IV: Case Studies
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 55
fusion
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 56
Localization –Position Estimation
traversability maps
Dr. Giorgos A. Demetriou ACSC 414 - Robotics 57
Planning in Mixed Environments

29. Dr. Giorgos A. Demetriou ACSC 414 - Robotics 58
Navigation in Dynamic Environments
-dimensional State Lattice