autonomous mobile robotsweb.eecs.utk.edu/~leparker/courses/cs494-529-fall11/... · 2011. 10....

Autonomous Mobile Robots

Autonomous Systems LabZürich

Navigation

Where am I?

Where am I going?

How do I get there?

"Position"

Global Map

Perception Motion Control

Cognition

Real WorldEnvironment

Localization

PathEnvironment Model

Local Map

?

© R. Siegwart, ETH Zurich - ASL

6 - Planning and Navigation6

2 Required Competences for Navigation

Navigation is composed of localization, mapping and motion planning

Previous Lectures:

Vehicle kinematics

Different forms of sensors

Localization

Next Lecture:

Simulatneous localization and mapping (SLAM)

Today:

Different forms of motion planning inside the cognition loop

"Position"

Global Map


Cognition


Localization


Local Map


Motion Planning in Action

We have come a long way since Shakey!


3

C Willow Garage C Stanford



4 Outline of this Lecture

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



5 The Planning Problem (1/2)

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

Coffee

room

Corridor

Bill’s

offic

e

Topological Map

Coarse Grid Map

(for reference only)

Coffee

room

Corridor

Bill’s

offic

e

Topological Map

Coarse Grid Map

(for reference only)



6 The Planning Problem (2/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)



7 Work Space (Map) Configuration Space

State or configuration q can be described with k values qi

What is the configuration space of a mobile robot?

Work Space Configuration Space: the dimension of this

space is equal to the Degrees of Freedom (DoF) of the robot


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.


8 Configuration Space for a Mobile Robot



Planning and Navigation I:

Global Path Planning

Where am I?

Where am I going?

How do I get there?

"Position"

Global Map


Cognition


Localization


Local Map

?



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?

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

Source: http://mitocw.udsm.ac.tz



11 Optimal Control based Path Planning Strategies

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

Source: http://mitocw.udsm.ac.tz



12 Potential Field 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

Khatib

C Khatib



13 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 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

y

Ux

U

qUqUqUqF repatt )()()()(



14 Potential Field Path Planning: Attractive Potential Field

Parabolic function representing the Euclidean distance

to the goal

Attracting force converges linearly towards 0 (goal)

goalgoal qq

)(

)()(

goalatt

attatt

qqk

qUqF

2

2

)(2

1

)(2

1)(

goalatt

goalattatt

qqk

qkqU



15 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

obst.



16 Potential Field Path Planning:

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



17 Potential Field Path Planning: Extended Potential Field Method

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

consideredKhatib and Chatila



18 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

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



19 Graph Search

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

C w

ikip

edia

.org



20 Graph Construction (Preprocessing Step)

Methods

Visibility graph

Voronoi diagram

Cell decomposition

...



21 Graph Construction: Visibility Graph (1/2)

Particularly suitable for polygon-like obstacles

Shortest path length

Grow obstacles to avoid collisions



22 Graph Construction: Visibility Graph (2/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



23 Graph Construction: Voronoi Diagram (1/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



24 Graph Construction: Voronoi Diagram (2/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



25 Graph Construction: Cell Decomposition (1/4)

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



26 Graph Construction: Exact Cell Decomposition (2/4)



27 Graph Construction: Approximate Cell Decomposition (3/4)



28 Graph Construction: Adaptive Cell Decomposition (4/4)


Graph Construction: State Lattice Design (1/2)

6 - Planning and Navigation

Online:

Incremental Graph

Constr.

Offline:

Lattice Gen.

Offline:

Motion Model

6

29

Enforces edge feasibility


Graph Construction: State Lattice Design (2/2)

State lattice encodes only kinematically feasible edges


Martin Rufli

6

30



31 Graph Search

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



32 Graph Search Strategies: Breadth-First Search

B C D

A

E

B C D

A

FE

B C D

A

GFE

B C D

A

F

G

G H I

FE

B C D

A

F

G

ID

G H I

FE

B C D

A

F

G

I KD

G H I

FE

B C D

A

H

F

G

I K CD

G H I

FE

B C D

A

H

F

G

I K CD L

A

G H I

FE

B C D

A

H

F

G

I K CD L

LA

G H I

FE

B C D

A

H

F

G

I K CD L

L LA

G H I

FE

B C D

A

H

F

G

I K CD L

L L AA

G H I

FE

B C D

A

H

K

F

G

I K CD L

L L AA

G H I

FE

B C D

A

H

F

G

I K CD L

L L AA

L

G H I

FE

B C D

A

A=initial

B

C

D

F

G

H

I

K

L=goal

E

H

F

G

I K CD L

G H I

FE

B C D

A

First path found!= optimal

G

G H

FE

B C D

A



33 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

Fig. 1: NF1: put in each cell its L1-distance from the goal position (used also in local path planning)



34 Graph Search Strategies: Depth-First Search

B C D

A

E

B C D

A

FE

B C D

A

G H

FE

B C D

A

ID

G H

FE

B C D

A

D

A

G H

FE

B C D

A

I

I KD

LA

G H

FE

B C D

A

ID

LA

G H

FE

B C D

A

First path found!

NOT optimal

I KD

L LA

G H

FE

B C D

A

G

I KD

L LA

G H

FE

B C D

A

F

G

I KD

L LA

G H I

FE

B C D

A

H

F

G

I K CD

L LA

G H I

FE

B C D

A

H

F

G

I K CD

L L AA

G H I

FE

B C D

A

H

K

F

G

I K CD

L L AA

G H I

FE

B C D

A

H

K

F

G

I K CD

L L AA

L

G H I

FE

B C D

A

H

K

F

G

I K CD L

L L AA

L

G H I

FE

B C D

A

A=initial

B

C

D

F

G

H

I

K

L=goal

E

Use of a LIFO queue

Memory efficient (fully explored subtrees can be deleted)



35 Graph Search Strategies: A* 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)



36 Graph Search Strategies: D* 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

C M. Likhachev



37 Graph Search Strategies: Randomized Search

Most popular version is the rapidly exploring random tree (RRT)

Well suited for high-dimensional search spaces

Often produces highly suboptimal solutions

C S.

LaValle


Lecture Evaluation


38



Planning and Navigation II:

Obstacle Avoidance

Where am I?

Where am I going?

How do I get there?

"Position"

Global Map


Cognition


Localization


Local Map

?



40 Obstacle Avoidance (Local Path Planning)

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

Example: Alice



41 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

Advantages

No global map required

Completeness guaranteed

Disadvantages

Solutions are often

highly suboptimal



42 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



43 Obstacle Avoidance: Vector Field Histogram (VFH)

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

Bore

nst

ein

et a

l.

target_direction = alignment of the robot path with the goal

wheel_orientation = difference between the new direction and the currrent wheel orientation

previous_direction = difference between the previously selected direction and the new direction



44 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

Borenstein et al.



45

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

Borenstein et al.

Obstacle Avoidance: Limitations of VFH



47 Obstacle Avoidance: Dynamic Window Approach

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

, , ,d a a a aV v v v v t v v t t t



48 Obstacle Avoidance: Dynamic Window Approach

Resulting search space

The area Vr is defined as the intersection of the restricted areas,

namely,

r s a dV V V V

C D. Fox

Vs: static limits in velocityVd: dynamic limits in change in velocityVa: limits due to nearby obstacles



49 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.

, ( , ) ( , ) ( , )G v heading v dist v velocity v heading = measure of progress toward the goal

dist = Distance to the closest obstacle in trajectory

velocity = Forward velocity of the robot, encouraging fast movements

C D. Fox



50 Obstacle Avoidance: Global 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



Planning and Navigation III:

Architectures

Where am I?

Where am I going?

How do I get there?

"Position"

Global Map


Cognition


Localization


Local Map

?



52 Basic architectural example


CognitionLocalization

Real World

Environment

Per

cepti

on

to

Act

ion

Ob

sta

cle

Avo

ida

nce

Posi

tio

n

Fee

db

ack

Path

Environment

Model

Local Map

Local Map

PositionPosition

Local Map



53 Control decomposition

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

Pure parallel decomposition (e.g. via behaviors)



54 General Tiered Architecture

Executive Layer

activation of behaviors

failure recognition

re-initiating the planner



55 A Three-Tiered Episodic Planning Architecture.

Global planning is generally not real-time capable

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



56 An integrated planning and execution architecture

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



Planning and Navigation IV:

Case Studies

Where am I?

Where am I going?

How do I get there?

"Position"

Global Map


Cognition


Localization


Local Map

?


Differential GPS

Inertial measurement unit

Optical gyro

Wheel encoders

Localization – Position Estimation

6 DOF position estimation based on information filter sensor fusion


58



60 Planning in Mixed Environments

Autonomous navigation

based on extended D*

and traversability maps


Navigation in Dynamic Environments


61


Way Forward: Planning Prerequisites

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.


Dynamic Environments (1/2)


Martin Rufli

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

6

3

M. Rufli and R. Siegwart: Deformable Lattice Graphs

6

63

Deficiencies

Optimality guarantees (if any) are lost

Fusion of multiple disparate approaches


Dynamic Environments (2/2)


3210

Dynamic Environments: often Dealt with via Frequent Replanning

64

quasi-static dynamic

Potential Solution

Addition of motion prediction and interaction into the planning stage

Use of lattice graphs

6

64


Graph Search on a State Lattice


Martin Rufli

6

65


4D State Lattice

State lattice encodes only kinematically feasible edges


Martin Rufli

6

66


Segway RMP in Narrow Environments

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


Martin Rufli

6

67

autonomous mobile robotsweb.eecs.utk.edu/~leparker/courses/cs494-529-fall11/... · 2011. 10....

Documents