lecture 9 navigation part1

8/14/2019 Lecture 9 Navigation Part1

1/38

Planning and NavigationWhere am I going? How do I get there?

?

"Position"Global Map

Perception Motion Control

Cognition

Real WorldEnvironment

Localization

PathEnvironment ModelLocal Map


2/38

Competencies for Navigation

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 whatinformation on the

environment to use.

Todays 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 exists (e.g.

sales man problem).


3/38

Competencies for Navigation

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 forplanning (global),

some to guarantee survival of the robot.

Robot control can usually be decomposed in various behaviors orfunctions

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.


4/38

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 them to usestandard planning algorithms.

Examples:

Visibility Graph

Voronoi Diagram

Cell Decomposition -> Connectivity Graph

Potential Field


5/38

Path Planning: Configuration Space

State or configuration q can be described with kvalues qi

What is the configuration space of a mobile robot?


6/38

Path Planning Overview

1. Road Map, Graph construction

Identify a set of routes within the free

space

Where to put the nodes?

Topology-based:

at distinctive locations

Metric-based:

where features disappear or get visible

2. Cell decomposition

Discriminate between free and

occupied cells

Where to put the cell boundaries? Topology- and metric-based:

where features disappear or get visible

3. Potential Field

Imposing a mathematical function over

the space


7/38

Road-Map Path Planning: Visibility Graph

Join all pairs of vertices visible

to each other

Advantages Shortest path length

Simple to implement if

environment can berepresented by polygons

Disadvantages

Slow in densely populated

environments

Too close to obstacles

o Method: Grow obstacles to

avoid collisions


8/38

Road-Map Path Planning: Voronoi Diagram

Maximize the distance between

the robot and obstacles

Advantages:Easy executable: Maximize the

sensor readings, mitigate

encoder inaccuracy

Works also for map-building:

Move on the Voronoi edges

Disadvantages:

May fail in the case of limited

range localization sensors


9/38

Road-Map Path Planning: Cell Decomposition

Basic idea: to discriminate between free cells and occupied cells

Procedure:

Divide space into simple, connected regions calledcellsDetermine which open sells are adjacent and construct a connectivity

graph

Find cells in which the initial and goal configuration (state) lie andsearch 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.

o e.g. passing through the midpoints of cell boundaries or by sequence of

wall following movements.


10/38

Road-Map Path Planning: Exact Cell Decomposition

The boundary of cells

is based on geometric

criticality

The particularposition of the robot

within each cell of

free space does not

matter

Slow in dense and

complex environment


11/38

Road-Map Path Planning: Approximate Cell Decomposition

Narrow passageways mayNarrow passageways may

be lost due to the inexactbe lost due to the inexact

nature of the tessellationnature of the tessellation


12/38

Road-Map Path Planning: Adaptive Cell Decomposition

Fixed cell decomposition

The search is linear in the

number of cells only

Fundamental cost is memory

Adaptive cell decomposition

The free space is externally

bounded by a rectangle andinternally bounded by three

polygons.

The rectangle is recursively

decomposed into smallerrectangles

Adapt to the complexity of

environment


13/38

Path Planning Algorithms

For Path planning

A* for relational graphsWavefront for operating directly on regular grids

For Interleaving Path Planning and Execution


14/38

Motivation for A*

Single Source Shortest Path algorithms are exhaustive, visiting all

edges

Cant we throw away paths when we see that they arent going to thegoal, rather than follow all branches?

This means having a mechanism to prune branches as we go, rather

than after full exploration

Algorithms which prune earlier (but correctly) are preferred overalgorithms which do it later.

Issue -> the mechanism for pruning


15/38

A* Algorithm

Similar to breadth-first: at each point in the time the planner can onlysee its node and 1 set of nodes in front

The idea is to rate the choices, choose the best one first, throw awayany choices whenever you can

f*(n) is the goodness of the path from Start to n

g*(n) is the cost of going from the Start to node n

h*(n) is the cost of going from n to the Goal

H is for heuristic function because we must have a way of guessingthe cost of n to Goal since we cant see the path between n and the Goal

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


16/38

A* Heuristic Function

G*(n) is easy: just sum up the path costs to n

h*(n) is tricky

But path planning requires an a priori map

Metric path planning requires a METRIC a priori map

Therefore, know the distance between Initial and Goal nodes, just not

the optimal way to get there

h*(n)= distance between n and Goal

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


17/38

Example: A to E

But since youre starting at A and can only look 1 node ahead, this is

what you see:

AB

D

F E

1

1

1

1

1.4

1.4

AB

D

E

1

1.4


18/38

Two choices for n: B, D

Do both

f*(B)=1+2.24=3.24

f*(D)=1.4+1.4=2.8

Cant prune, so much keep going (recurse)

Pick the most plausible path first => A-D-?-E

AB

D

E

1

1.4

1.4

2.24


19/38

A-D-?-E

stand on D

Can see 2 new nodes: F, E

f*(F)=(1.4+1)+1=3.4f*(E)=(1.4+1.4)+0=2.8

Three paths A-B-?-E >= 3.24

A-D-E = 2.8

A-D-F-?-D >=3.4

A-D-E is the winner!

Dont have to look farther because expanded the shortest first, otherscouldnt possibly do better without having negative distances,

violations of laws of geometry

AB

D

E

1

1.4

1.4

F1

1


20/38

Class Exercise

Find the best path from A to E

D

F E

1

1

1

1

1.4

1.4

AB

C

Notes:Notes:A* is hard to use for path planning where there are factors otheA* is hard to use for path planning where there are factors other thanr than

distance to consider in generating the path, such as terrain condistance to consider in generating the path, such as terrain conditionsditions


21/38

Wavefront Planners

Well suited for grid map

Consider the Cspace to be a conductive material with heat radiating out

from the initial node to the goal node.Eventually the heat will spread and reach the goal if there is a way

Can combine terrain types into theCan combine terrain types into the

planning by assigning differentplanning by assigning different

levels of conductivity to the cellslevels of conductivity to the cells


22/38

Trulla Algorithm

obstacleobstacle

UndesirableUndesirable

terrainterrain

OpenOpen

areaarea


23/38

Interleaving Path Planning and Reactive Execution

Graph-based planners generate apath and sub-paths or sub-segments

What happens if blocked? What happens if avoid an obstacle andactually is now closer to the next sub-goal?

Causes Sub-goal obsession

When does the robot think it has reached (x,y)? What about encodererror?

Need a Termination condition, usually +/- width of robot


24/38

Trulla Example


25/38

Two Example Approaches

If compute all possible paths in advance, not a problem

D* ( by Tony Stentz)

Run A* over all possible pairs of nodes Solution to sub-goal obsession: continuously update the map and

dynamically repair the A* paths affected by the changes in the map ->

continuously replanning

Event-driven scheme to trigger replanning

Trulla (by Ken Hughes)

By-product of wave propagation style is path to everywhere

Using dot-product of the path vector and the actual vector as an

affordance to trigger replanning


26/38

Two Example Approaches (continued)

Both algorithms dont handle the situation where the real world is

actually friendlier, which means an obstacle thought to be there really

isnt.

The robot could achieve a significant savings in navigation by

opportunistically going through the gap

6.2.1


27/38

Potential Field Path Planning

Robot is treated as apoint under the

influence of an artificial potential field.

Generated robot movement is similar toa ball rolling down the hill

Goal generates attractive force

Obstacle are repulsive forces

6.2.1


28/38

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)

Set robot speed (vx, vy) proportional to the force F(q) generated by thefield

the force field drives the robot to the goal

if robot is assumed to be a point mass

===

y

Ux

U

qUqUqUqF repatt )()()()(

6.2.1


29/38

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

Field is positive or zero andtends to infinity as q gets closer to the object


30/38

Path Planning for Cye Mobile Robot (Batavia and Nourbakhsh)

Cye is a 2-wheeled differential drive robot

Primary mode of navigation is dead-reckoning

Challenges The robot must maintain adequate

distance from alls and other obstacles

There is uncertainty associated with the

creation of the map, and there can bedynamic obstacles

The robot should ideally use exploredareas, but should be willing to traverse

unexplored areas if doing so results in asignificant savings in path length

Path generation time must be short

The use of check points must be

incorporated into the path planner tominimize ded-reckoning error


31/38


Path planner

Grid based potential field

Assume that the robot is a holonomic platform

The field is constructed in two stages

o First stage: each cell contains a value denoting the traversability of

that point

distance to the nearest obstacle and terrain type

o Second stage: a potential field is created in which each world pointis the distance to the goal, nonlinearly weighted by the corresponding

value in the traversability grid

wavefront transformation

This field is used to create a path from start to goalwhich minimizes the path length plus the cost of

traversability

Check points to correct the localization errors


32/38


Avoiding local minima

Using unexplored areas to shorten path length


33/38

Exploration

How to cover an unknown area efficiently?

Random search

Avoid-past behavior combined with the random potential field

(visited cell as a repulsive field)

Move-to-unknown-area behavior (use the centroid of unknown area as agoal)

The above behavior-oriented approaches are simple and easy toimplement, however, they are inefficient when presented with two ormore unexplored areas

Can explore reactively (move to open area), but wed like to create

maps Two major methods

Frontier-based

Generalized Voronoi Graph (GVG)


34/38

Frontier Based Exploration

Robot senses environment

Borders of low certainty form

frontiers

Rate the frontiers

Centroid

Utility of exploring (big?Close?)

Move robot to the centroid and

repeat (continuously localize and map

as you go)

Open spaceOpen space

Occupied spaceOccupied space

FrontierFrontier

RobotRobot

currentcurrent

positionposition


35/38

GVG (Generalized Voronoi Graph)

Essentially, the robot tries to move ahead but stay in the middlEssentially, the robot tries to move ahead but stay in the middle or at ae or at a

tangent to surrounding objects. This path is a GVG edge.tangent to surrounding objects. This path is a GVG edge.

dead enddead end

If encounter branches inIf encounter branches inthe GVG edges, choosethe GVG edges, choose

one at random to followone at random to follow

Starting pointStarting point


36/38

Reaches deadend at 9, backtracks


37/38

Goes back and catches missing areas


38/38

Discussion of Exploration

Both methods work OK indoors, not so clear on utility outdoors

GVG

Susceptible to noise, hard to recover nodes

Frontier

Have to rate the frontiers so dont trash

lecture 9 navigation part1

Documents