ar13 5 path planning navigation - graz university of ... · advanced robotics – path planning and...

Gerald Steinbauer

Institute for Software Technology

1

Advanced Robotics – Path Planning and Navigation

Advanced RoboticsPath Planning & Navigation

Gerald SteinbauerInstitute for Software Technology

Gerald Steinbauer


2


Agenda

• Motivation• Basic Definitions• Configuration Space• Global Planning• Local Planning – Obstacle Avoidance• ROS Navigation Stack

Gerald Steinbauer


3


Literature

Choset, Lynch, Hutchinson, Kantor, Burgard, Kavraki and Thrun. Principle of Robot Motion – Theory, Algorithms and Implementation. MIT Press. 2005.

Gerald Steinbauer


4


Motivation

Gerald Steinbauer


5


Piano Mover’s Problem

• given• a set of obstacles• the initial position of a robot• the final position of a robot

• goal• find a path that• moves the robot from the initial to final

position• avoids the obstacles (at all times)

Gerald Steinbauer


6


Challenges in the Real World

• physical Laws, e.g. inertia, acceleration• uncertainty, e.g. maps, observations• geometric constraints, e.g. shape of a robot• dynamic environment, e.g. a moving crowd

Gerald Steinbauer


7


Properties of Algorithms

• completeness• does the algorithm find a solution in finite time if one exists• resolution complete• probabilistic complete

• optimality• does the algorithm find the optimal solution(s)

• complexity• what are the computational and memory demands

• off-line vs. on-Line• pre-computing ?

• sensor-based• does the algorithm integrate a sensing step

Gerald Steinbauer


8


Bug Algorithm

• earliest and most simple• sensor-based navigation• biologically inspired

• robot is able to detect obstacles and the direction to the goal

• basic idea• follow the straight line to the goal• if hitting an obstacle circle the obstacle clockwise• continue from the closest free point

Gerald Steinbauer


9


Bug Algorithm

Gerald Steinbauer


10


Bug Algorithm• uses exhaustive

search• complete for finite

number of hits• path-length

n

iiendstartbugendstart pqqdLqqd

1

5.1),(),(

Gerald Steinbauer


11


Configuration Space

• to ensure that we have no collision at any time we have to represent the robot’s position

• moreover we have to represent the position of eachpoint of the robot

• we use the concept of configuration, e.g. necessary for robot arms

• the space of all possible configurations is the configuration space or C-space• q … single configuration• Q … entire configuration space

Gerald Steinbauer


12


Example C-Space

• circular robot with radius r on the plane• configuration can be represented as position (x,y)

y

x

Gerald Steinbauer


13


Another Example C-Space

Configuration Space

Work Space

Gerald Steinbauer


14


Obstacle in the Configuration Space

• let q be a point in the configuration space Q• path planning revised: find a mapping c:[0,1]→Q so

that no configuration along the path intersects an obstacle

• a configuration space obstacle QOi is a set of configurations q at which the robot intersects WOithat is:

• the free configuration space

• a free path is a mapping: c:[0,1]→Qfree

• a semi-free path is a mapping: c:[0,1] →cl(Qfree)

})(|{ ii WOqRQqQO

)(\ i

ifree QOQQ

Gerald Steinbauer


15


Example Obstacle Space

© Ken Goldberg

Gerald Steinbauer


16


Building Blocks for Ground Robots

Global Planner

Local Planner

Trajectory Generation

Goal

Intermediate Goals

Path

Velocity Commands

Gerald Steinbauer


17


Global Planning

• given• a representation of the environment including known (static)

obstacles, e.g. a map• a current position (configuration)• a goal position (configuration)

• goal• find a free path from the current position (configuration) to the goal

position (configuration)

• some assumptions• holonomic motion• point robot• static environment• no dynamic

Gerald Steinbauer


18


Topological Representations

• represent the environment as graphs or roadmaps• graph comprises

• nodes – configurations• edges – (free) connections between configurations or roads

• either given or constructed (e.g., cell decomposition)• planning = search for shortest path in graph

Gerald Steinbauer


19


Geometrical Representations

• represent the environment by its geometry• represents the workspace of the robot• uses geometric primitives to represent the environment• compact representation

Gerald Steinbauer


20


Grid-Based Representations

• represent the environment as grid map• grid cells are either free or occupied• may represent some uncertainty• represents the workspace of the robot• memory demand depends on the resolution

Gerald Steinbauer


21


Cell Decomposition

• basic Idea: convert representation into a graph and do motion planning on that graph

• divide the free configuration space Qfree into simple connected regions named cells

• generate a connectivity graph with connected regions• locate “goal” and “start” cells• search for a path trough the graph• follow the path

Gerald Steinbauer


22


Trapezoid Cell Decomposition

• assumes polygonalenvironment

• decompose into trapezoids or triangles

• sort vertices in one direction

• iterate all vertices vi

• extend vertex vi up and/or down by two line segments until hitting an obstacle

Gerald Steinbauer


23


Trapezoid Cell Decomposition – Run Time

• for vertices• sorting the vertices needs ∙ time• intersection done in a naïve way, we need time• if we use a scan line we need only ∙• four different events and an edge list

• both edges of a vertex are on the left → remove both from list• both edges of a vertex are on the right → add both to list• if the upper edge is on the left and the lower edge on the right →

add upper edge and remove lower edge• if the upper edge is on the right and the lower edge on the left →

add lower edge and remove upper edge

• perform intersection check only on edges in the list

Gerald Steinbauer


24


Occupancy Grid

• occupancy grids are examples of an approximate cell decomposition – regular grid of cells

• easy to implement• widely used• may require post planning smoothing

Gerald Steinbauer


25


Occupancy Grid – Alternative Representations

• resolution is a limiting issue• too fine a grid leads to long search time• too coarse a grid misses paths

• quad-tree• adaptable cell size• tree structure

Gerald Steinbauer


26


A* Search• most popular algorithm to find paths in graphs• estimated costs function for a node n

• f(n)=g(n)+h(n)• g(n) … costs from start so far• h(n) … estimated costs to goal from n

• expand node n with lowest f(n) first• two node lists: open (new), close (fully expand)• if a successor node already closed – ignore node• if successor n is open and g’(n) < g(n) then update

f(n) and mark n as open• continue until goal reached or no more open nodes• h(n) has to underestimate the real costs

Gerald Steinbauer


27


A* Search Example

h(n) … Euclidian distance to goal

find the shortest path from Saabrücken to Würzburg

Gerald Steinbauer


28


LPN Algorithm

• a method to find minimum cost paths based on linear programming

• work well on grid maps, extenion of the wave-front algorithm

• given• path as points in sample space P={p1,p2,…,pn}• costs for traversing P

• navigation function represents the steepest gradient for a path Pkstarting at point k

• goal• a path P with minimum costs

1

11

1

),()()(k

iii

k

ii ppApIPF

)(min kPk PFNk

Gerald Steinbauer


29


LPN Algorithm• initially assign 0 to goal and to all other points• obstacles contribute to I(p)• goal point is active point• expand path to neighbors p and calculate F’(p)• if F’(p) < F(p) update F(p) and set p as active point• continue for all active points until no one remains

Gerald Steinbauer


30


LPN ExampleI=0

C=I=20C=

I=5C=

I=0C=

I=0C=0

I=50C=

I=10C=

I=0C=

I=0C=

I=0C=

I=0C=

I=0C=

A=10

A=10 A=10A=10

A=10A=10

A=10 A=10A=10

A=1

0

A=1

0

A=1

0

A=1

0

A=1

0

A=1

0

A=1

0

A=1

0

Gerald Steinbauer


31


Sampling-Based Approaches

• so far planner rely on explicit representations of • for complex and high-dimensional spaces this can be

impractical• basic idea: sample collision-free configuration and

connect them to get a path• multi-query planner: the planner once generate a

general data structure by sampling which can be used for several path planning problems, e.g. probabilistic road maps

• single-query planner: sampling is done for everyproblem, e.g. rapidly-exploring random trees

Gerald Steinbauer


32


Sampling-Based Approaches

multi-query planner

single-query planner

Gerald Steinbauer


33


Rapidly-Exploring Random Trees (RRT)

• basic idea• given a start configuration and a goal configuration • build a tree based on connected samples from rooted in • try to reach

• what do we need• configuration space • start configuration and a goal configuration • collision detector: : → ,• incremental simulator: given compute ∆• distance metric: : → 0,∞

Gerald Steinbauer


34


Rapidly-Exploring Random Trees (RRT)build_rrt( )

.init( )for =1 to K do

=random_config()extend( , )

return

extend( , )=nearest_config( , )

if new_config( , , , ).add_vertec( ).add_edge( , , )

if = thenreturn reached

elsereturn advanced

return traspped

Gerald Steinbauer


35



Gerald Steinbauer


36



• basically not goal-orientated• apply some bias

• choose a new sample randomly with probability 1 and as randomly with probability

• grow two trees from and and merge them

• the sampling strategy and the increment control the exploration of the configuration space, e.g. density of the tree

• RRT is probabilistic complete

Gerald Steinbauer


37


Potential Field

• models the robot as a charged particle in a potential field U(x)

• the goal has an attractive force• obstacles have a repulsive force • gradient of the potential field

provides a force• robot rolls down a surface towards the goal

• easy to implement• hard to find proper parameter• local minima

)()( xUxF

Gerald Steinbauer


38


OMPL: The Open Motion Planning Library

• open source sample-based motion planning library• by Rice University• provides a number of ready to use planner but also

templates to implement new ones• OMPL contains implementations of PRM, RRT, EST,

SBL, KPIECE, …• OMPL supports various state (configuration) spaces• OMPL is a stand-alone library but closely integrated

in ROS• OMPL is the standard library for arm motion planning

in ROS

Gerald Steinbauer


39


OMPL.app

interactive GUI for motion planning problems to be solved by OMPL

Gerald Steinbauer


40


Potential Field Example

Gerald Steinbauer


41


Obstacle Avoidance

• performs local planning• in general the environment is not fully modeled• the environment changes dynamically• obstacle avoidance may

• modify or replan a globally generated plan• move the robot towards a goal without a global plan (e.g., Bug

algorithm)

• it relies on• information about the goal or sub-goal on the global path• local context information (localization)• recent sensor information (local map and obstacles)

Gerald Steinbauer


42


Vector Field Histogram• generate “obstacle forces”

which increase near obstacles• generate a discrete polar

histogram • use a threshold to get a binary

diagram• check for a valley the robot

can pass• select the solution closest to

the target• distinguish two cases (wide,

narrow)

Gerald Steinbauer


43


Idea Local Planning

fff

ti

ti+1

Gerald Steinbauer


44


Dynamic Window Approach

• the approach assumes a differential drive robot –moving along arcs

• considers aspects of the dynamic• works in the velocity space (v,)• the approach uses

• the actual velocity of the robot • a dynamic window of reachable velocities in the next cycle – given

by the dynamic of the robot

• calculate admissible velocities – allow to stop before hitting an obstacle

• maximize an objective function),(),(),(),( vdistcvvelocitybvheadingavO

Gerald Steinbauer


45


Dynamic Window Approach

]},[],[|),{( tttvvtvvvvv aaaad

• heading(v,): -Target• distance(v,): closets obstacle• velocity(v,): proportional to v

}),(2),(2|),{( bba vdistvvdistvvv

Gerald Steinbauer


46


Global Dynamic Window Approach

• incorporate some features of global planning• add a minimum-free function NF1 (wave-propagation)• use a occupancy grid around robot• update occupancy grid by range measurements

• extend the objective function of the DWA to stick at the greatest gradient of the navigation function• avoid local minima

Gerald Steinbauer


47


Combining Local and Global Methods

• neither global nor local methods are enough• global methods fail to address local variations,

uncertainty, dynamics, …• local methods get trapped in local minima

• solution: combine both methods

• several methods come in a global X version such as global DWA

Gerald Steinbauer


48


(Rough) Frequency Range

• large span in frequencies in navigation

• system architecture have to support this variety

Path Planning (< 0.1 Hz)

Path Deformation (~ 5 Hz)

Obstacle Avoidance (~ 10 Hz)

Actuator Control (~ 150 Hz)

Gerald Steinbauer


49


ROS Navigation Stack

• ROS provides a full navigation stack• global planning – A* and LPN• local planning – Dynamic Window Approach or Trajectory Rollout

• supports differential drive and omni-directional robots• works with and without global localization and map• can work with 2d and/or 3d maps and sensors

• for usage and parameter look to the tutorial

Gerald Steinbauer


50


ROS Navigation Stack Overview

Gerald Steinbauer


51


Some Challenges

• the navigation stack is trimmed towards challenging indoor office-like environments• narrow passages or doors• difficult 3d structures like tables or wider bases• rich variety of objects in the environment• unexplored areas like blind corners

Gerald Steinbauer


52


Cost Maps

• cost maps are used to generate a path or do achieve obstacle avoidance

• cost maps are 2d• global cost map

• initialized by the global map• updated by sensor information• map coordinate frame

• local cost map• rolling window – centered around

the robot• cares about local obstacles• updated by sensor information• odometry coordinate frame

Gerald Steinbauer


53


2d versus 3d

• the navigation stack is able to work with laser scans (2d) and point cloudes (3d)

• laser scans• are filtered for larger geometric objects like lines – remaining points

are classified as obstacle• ego-hits are removed

• point clouds• are converted into a 3d occupancy grid• unknown or occluded areas are treaded as forbidden areas• for navigation the 3d occupancy grid is projected to a 2d one

Gerald Steinbauer


54


Thank you!

ar13 5 path planning navigation - graz university of ... · advanced robotics – path planning and...

Documents