Path Planning in Lego Robot

Lecture 11: Navigation

Dr. Giorgos A. Demetriou Department of Computer Engineering and Computer Science

School of Engineering and Applied Sciences g.demetriou@frederick.ac.cy 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

 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 metaphors exist  Often employed due to its simplicity and

similarity to optimal control solutions

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 10

Path Planning: Overview of Algorithms

3. Graph Search  Identify a set of edges and connect them to

nodes within the free space

 Where to put the nodes?

 Overview  Solves a two-point boundary problem in the continuum  Not treated in this course

 Limitations

 Becomes very hard to solve as problem dimensionality increases  Prone to local optima

 Algorithms

 Pontryagin maximum principle  Hamilton-Jacobi-Bellman

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 11

Optimal Control based Path Planning Strategies

 Robot is treated as a point under the influence of an artificial potential field.

 Operates in the continuum  Generated robot movement is similar to

a ball rolling down the hill  Goal generates attractive force  Obstacle are repulsive forces

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 12

Potential Field Path Planning Strategies

 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)

 Set robot speed (vx, vy) proportional to the force F(q) generated by the field  The force field drives the robot to the goal  Robot is assumed to be a point mass (non-holonomics are hard to deal with)  Method produces both a plan and the corresponding control

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 13

Potential Field Path Planning: Potential Field Generation

 Parabolic function representing the Euclidean distance to the goal

 Attracting force converges linearly towards 0 (goal)

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 14

Potential Field Path Planning: Attractive Potential Field

 Should generate a barrier around all the obstacle  Strong if close to the obstacle  Not influence if fare from the obstacle

 ρ(q) minimum distance to the object  Field is positive or zero and tends to infinity as q gets closer to the object

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 15

Potential Field Path Planning: Repulsing Potential Field

 Notes:  Local minima problem exists  Problem is getting more complex if the robot is not considered as a point mass  If objects are non-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:

 Additionally a rotation potential field and a task potential field is introduced

 Rotation potential field  Force is also a function of robots

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

 Task potential field  Filters out the obstacles that should

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

 Hydrodynamics analogy  Robot is moving similar to a fluid particle following its stream

 Ensures that there are no local minima

 Boundary Conditions:  Neuman

• Equipotential lines orthogonal on object boundaries (as in image above!) • Short but dangerous paths

 Dirichlet • Equipotential lines parallel to object boundaries • Long but safe paths

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 18

Potential Field Path Planning: Using Harmonic Potentials

 Overview  Solves a least cost problem between two states on a (directed) graph  Graph structure is a discrete representation

 Limitations  State space is discretized completeness is at stake  Feasibility of paths is often not inherently encoded

 Algorithms

 (Preprocessing steps)  Breath first  Depth first  Dijkstra  A* and variants  D* and variants

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 19

Graph Search

C wikipedia.org

 Methods  Visibility graph  Voronoi diagram  Cell decomposition  ...

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 20

Graph Construction (Preprocessing Step)

 Particularly suitable for polygon-like obstacles  Shortest path length  Grow obstacles to avoid collisions

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 21

Graph Construction: Visibility Graph (1/2)

 Pros  The found path is optimal because it is the shortest length path  Implementation simple when obstacles are polygons

 Cons

 The solution path found by the visibility graph tend to take the robot as close as possible to the obstacles: the common solution is to grow obstacles by more than robot’s radius

 Number of edges and nodes increases with the number of polygons  Thus it can be inefficient in densely populated environments

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 22

Graph Construction: Visibility Graph (2/2)

 In contrast to the Visibility Graph approach, the Voronoi Diagram tends to maximize the distance between robot and obstacles

 Easily executable: Maximize the sensor readings  Works also for map-building: Move on the Voronoi edges: 1D Mapping

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 23

Graph Construction: Voronoi Diagram (1/2)

 Pros  Using range sensors like laser or sonar, a robot can navigate along the Voronoi

diagram using simple control rules

 Cons  Because the Voronoi diagram tends to keep the robot as far as possible from

obstacles, any short range sensor will be in danger of failing

 Peculiarities  When obstacles are polygons, the Voronoi map consists of straight and parabolic

segments

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 24

Graph Construction: Voronoi Diagram (2/2)

 Divide space into simple, connected regions called cells  Determine which open cells 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 sequence of wall following

movements.  Possible cell decompositions:

 Exact cell decomposition  Approximate cell decomposition:

• Fixed cell decomposition • 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)

 Enforces edge feasibility

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 29

Graph Construction: State Lattice Design (1/2)

 State lattice encodes only kinematically feasible edges

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 30

Graph Construction: State Lattice Design (2/2)

 Methods  Breath First  Depth First  Dijkstra  A* and variants  D* and variants  ...

 Discriminators  f(n) = g(n) + εh(n)  g(n‘) = g(n) + c(n,n‘)

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 31

Graph Search

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 32

Graph Search Strategies: Breadth-First Search

 Corresponds to a wavefront expansion on a 2D grid  Use of a FIFO queue

 First-found solution is optimal if all edges have equal costs  Dijkstra‘s search is an “g(n)-sorted” HEAP variation of breadth first search

 First-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

 Use of a LIFO queue  Memory efficient (fully explored subtrees can be deleted)

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 34

Graph Search Strategies: Depth-First Search

 Similar to Dijkstra‘s algorithm, A* also uses a HEAP (but “f(n)-sorted”)  A* uses a heuristic function h(n) (often euclidean distance)

f(n) = g(n) + εh(n)

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 35

Graph Search Strategies: A* Search

 Similar to A* search, except that the search begins from the goal outward f(n) = g(n) + εh(n)

 First pass is identical to A*  Subsequent passes reuse information from previous searches

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 36

Graph Search Strategies: D* Search

 Most popular version is the rapidly exploring random tree (RRT)  Well suited for high-dimensional search spaces  Often produces highly suboptimal solutions

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

 The goal of obstacle avoidance algorithms is to avoid collisions with obstacles

 They are usually based on a local map  Often implemented as a more or less

independent task  However, efficient obstacle avoidance

should be optimal with respect to  The overall goal  The actual speed and kinematics of the

robot  The on-boards sensors  The actual and future risk of collision

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 39

Obstacle Avoidance (Local Path Planning)

 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  Advantages

 No global map required

 Completeness guaranteed

 Disadvantages  Solutions are often

highly suboptimal

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 40

Obstacle Avoidance: Bug1

 Following the obstacle always on the left or right side  Leaving the obstacle if the direct connection between start and goal is crossed

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 41

Obstacle Avoidance: Bug2

 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  The steering direction is computed in two steps:

• 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)

 Accounts also in a very simplified way for vehicle kinematics  Robot moving on arcs or straight

lines  Obstacles blocking a given

direction also blocks all the trajectories (arcs) going through this direction

 Obstacles are enlarged so that all kinematically blocked trajectories are properly taken into account

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 43

Obstacle Avoidance: Vector Field Histogram+ (VFH+)

 Limitations:  Limitation if narrow areas (e.g. doors) have to be passed  Local minima might not be avoided  Reaching of the goal can not be guaranteed  Dynamics of the robot not really considered

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 44

Obstacle Avoidance: Limitations of VFH

 The kinematics of the robot are considered via search in velocity space:  Circular trajectories : The dynamic window approach considers only circular

trajectories uniquely determined by pairs (v,ω) of translational and rotational velocities.

 Admissible velocities : A pair (v, ω) is considered admissible, if the robot is able to stop before it reaches the closest obstacle on the corresponding curvature. (b: breakage)

 Dynamic window : The dynamic window restricts the 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

Obstacle Avoidance: Dynamic Window Approach

 Resulting search space  The area Vr is defined as the intersection of the restricted areas, namely,

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 46

Obstacle Avoidance: Dynamic Window Approach

 Maximizing the objective function  In order to incorporate the criteria target heading, and velocity, the maximum of the

objective function, G(v, ω), is computed over Vr.

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 47

Dynamic Window Approach

 Global approach:  This is done by adding a minima-free function (e.g. NF1 wave-propagation) to the

objective function O presented above.  Occupancy grid is updated from range measurements

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

 Pure serial decomposition (e.g. sense-think-act)

 Pure parallel decomposition (e.g. via behaviors)

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 51

Control decomposition

 Executive Layer  Activation of behaviors  Failure recognition  Re-initiating the planner

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 52

General Tiered Architecture

 Global planning is generally not real-time capable  Planner is triggered when needed: e.g. blockage, failure

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 53

A Three-Tiered Episodic Planning Architecture

 All integrated. No temporal decoupling between planner and executive layer → see case study

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 54

An integrated planning and execution architecture

Planning and Navigation IV: Case Studies

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 55

 6 DOF position estimation based on information filter sensor fusion

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 56

Localization –Position Estimation

 Autonomous navigation based on extended D* and traversability maps

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 57

Planning in Mixed Environments

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 58

Navigation in Dynamic Environments

 Direct Creation of Feasible Trajectories  Design of a n-dimensional State Lattice

 Guarantees on Solution Optimality

 Deterministic search

 Global and Local Planning  Unified Framework: Design of a Single Planner

 Low Exectution Time (i.e. <0.1[s])

 Planner becomes (MP-) Controller

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 59

Way Forward: Planning Prerequisites

 A common Approach to Motion Planning  Generation of a global path on a 2D grid

• Path smoothing or employment of a local planner • Control onto the path

 Deficiencies  Optimality guarantees (if any) are lost  Fusion of multiple disparate approaches

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 60

Dynamic Environments (1/2)

 Dynamic Environments: often Dealt with via Frequent Replanning

 Potential Solution  Addition of motion prediction and interaction into the planning stage  Use of lattice graphs

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 61

Dynamic Environments (2/2)

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 62

Graph Search on a State Lattice

 State lattice encodes only kinematically feasible edges

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 63

4D State Lattice

 Planning in 4D: (x, y, heading, velocity)

Dr. Giorgos A. Demetriou ACSC 414 - Robotics 64

Segway RMP in Narrow Environments