Programming Robots in ROS Slides adapted from CMU, …web.ics.purdue.edu/~rvoyles/Classes/ROSprogramming/Lectures/PathPlanning.pdfPath Planning Problem Given an initial configuration

Programming Robots in ROS

Slides adapted from CMU, Harvard and TU Stuttgart

Path Planning ProblemPath Planning Problem

Given an initial configuration Given an initial configuration q_startq_startand a goal configuration and a goal configuration q_goalq_goal, we , we must generate the must generate the bestbest continuous path continuous path of legal configurations between these of legal configurations between these two points, if such a path exists.two points, if such a path exists.

Illustration of Basic DefinitionsIllustration of Basic Definitions

q_start

q_goal

obstacles

borderLegal

configurations

Optimal path

Feedback control, path finding, trajectory optim.

goalstart

path finding

trajectory optimization

feedback control

• Feedback Control: E.g., qt+1 = qt + J](y∗ − φ(qt))

• Trajectory Optimization: argminq0:T f(q0:T )

• Path Finding: Find some q0:T with only valid configurations

6/61

Control, path finding, trajectory optimization

• Combining methods:1) Path Finding

goalstart

path finding

trajectory optimization

feedback control

2) Trajectory Optimization (“smoothing”)3) Feedback Control

• Many problems can be solved with only feedback control (though notoptimally)

• Many more problems can be solved locally optimal with only trajectoryoptimization

• Tricky problems need path finding: global search for valid paths

7/61

• Reactive (Purely Local)

• Visual Homing, Wall Following

• Bug-Based Algorithms

• Sense Direction and Distance to Goal

• Metric Path Planning

• Path Represented as a Series of Waypoints

• Assumes Distance-Based Map or Graph

Types of Navigation Approaches

• Bug Algorithms

• Artificial Potential Fields

• Wavefront Planning

• Visibility/Voronoi Graphs

• Configuration-Space (C-Space)

• Quad-Trees

• Probabilistic Road Maps

Outline

Buginner Strategy

1) head toward goal

2) follow obstacles until you can

head toward the goal again

3) continue

path ?

“Bug 0” algorithm • known direction to goal

• otherwise local sensing

walls/obstacles & encoders

Goal

Start

Buginner Strategy

1) head toward goal



3) continue

assume a left-turning robot

OK ?

The turning direction might be decided beforehand…

“Bug 0” algorithm

Bug Zapper

1) head toward goal



3) continue

What map will foil Bug 0 ? “Bug 0” algorithm

start

goal

A better bug?

Call the line from the starting

point to the goal the m-line

1) head toward goal on the m-line

“Bug 2” Algorithm

A better bug?

Call the line from the starting

point to the goal the m-line


2) if an obstacle is in the way,

follow it until you encounter the

m-line again.


A better bug?



follow it until you encounter the

m-line again.

3) Leave the obstacle and continue

toward the goal

OK ?

m-line“Bug 2” Algorithm

A better bug?



follow it until you encounter the m-

line again closer to the goal.

3) Leave the obstacle and continue

toward the goal

Goal

Start


Better or worse than Bug1?

• Bug Algorithms





• Quad-Trees


Outline

Adapted from slides by Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds

The Basic Idea

• A really simple idea:

– Suppose the goal is a point g 2

– Suppose the robot is a point r 2

– Think of a “spring” drawing the robot toward

the goal and away from obstacles:

– Can also think of like and opposite charges


Attractive/Repulsive Potential Field

– Uatt is the “attractive” potential --- move to the goal

– Urep is the “repulsive” potential --- avoid obstacles


Artificial Potential Field Methods:

Attractive Potential

)(

)()(

goal

attatt

qk

qUqF

Conical Potential

Quadratic Potential


Artificial Potential Field Methods:

Attractive Potential

Combined Potential

In some cases, it may be desirable to have distance

functions that grow more slowly to avoid huge velocities

far from the goal

one idea is to use the quadratic potential near the

goal (< d*) and the conic farther away

One minor issue: what?


The Repulsive Potential


Repulsive Potential


Total Potential Function

+ =

)()()( repatt qUqUqU

)()( qUqF


Potential Fields

• Bug Algorithms





• Quad-Trees


Outline


Computing Distance: Use a Grid

• use a discrete version of space and work from there

– The Brushfire algorithm is one way to do this

• need to define a grid on space

• need to define connectivity (4/8)

• obstacles start with a 1 in grid; free space is zero

4 8


The Wave-front Planner

• Apply the brushfire algorithm starting from the goal

• Label the goal pixel 2 and add all zero neighbors to L

– While L

• dequeue the top element of L, t

• set d(t) to 1+mint’ N(t),d(t) > 1 d(t’)

• Enqueue all t’ N(t) with d(t)=0 to L (at the end)

• The result is now a distance for every cell

– gradient descent is again a matter of moving to the neighbor with the

lowest distance value


The Wavefront Planner: Setup


The Wavefront in Action (Part 1)

• Starting with the goal, set all adjacent cells with “0” to the current cell + 1

– 4-Point Connectivity or 8-Point Connectivity?– Your Choice. We’ll use 8-Point Connectivity in our example



• Now repeat with the modified cells

– This will be repeated until no 0’s are adjacent to cells with values >= 2

• 0’s will only remain when regions are unreachable



• Repeat again...



• And again...



• And again until...


The Wavefront in Action (Done)

• You’re done

– Remember, 0’s should only remain if unreachable regions exist


The Wavefront, Now What?

• To find the shortest path, according to your metric, simply always move toward a cell with a

lower number– The numbers generated by the Wavefront planner are roughly proportional to their distance from the goal

Two

possible

shortest

paths

shown

• Bug Algorithms





• Quad-Trees


Outline

Illustration of Basic DefinitionsIllustration of Basic Definitions

q_start

q_goal

obstacles

borderLegal

configurations

Optimal path

Visibility Graph ExampleVisibility Graph Example

q_start

q_goal

VoronoiVoronoi Graph ExampleGraph Example

h

q_start

q_goal

CC--Space Transform StepsSpace Transform StepsFor a mobile robot that For a mobile robot that onlyonly translates translates

Choose point on the robot.Choose point on the robot.Grow obstacle by translating robot around the edge of Grow obstacle by translating robot around the edge of the obstacle and tracing line made by the reference the obstacle and tracing line made by the reference point of the robot.point of the robot.Represent the robot only by the reference point.Represent the robot only by the reference point.Legal configurations now consist of all nonLegal configurations now consist of all non--obstacle obstacle points.points.There is a trick to make this process easier.There is a trick to make this process easier.

Robots that can rotate as well are usually Robots that can rotate as well are usually represented by a circle.represented by a circle.Once the obstacles have been grown, you can Once the obstacles have been grown, you can plan a path!plan a path!

Cell DecompositionCell DecompositionLast approach: Divide CLast approach: Divide C--space into convex space into convex polygons and plan between legal cellspolygons and plan between legal cells

WeWe’’ll use gridsll use grids……but algorithm extends to but algorithm extends to general convex polygons of different sizesgeneral convex polygons of different sizes

Algorithm:Algorithm:1.1. Start with CStart with C--space mapspace map

2.2. Divide map into polygonsDivide map into polygons

3.3. Mark cells containing obstacles as occupiedMark cells containing obstacles as occupied

4.4. Search for path to goal using unoccupied cellsSearch for path to goal using unoccupied cells

Cell Decomposition ExampleCell Decomposition Example

h

q_start

q_goal

Occupied

Free

Cell Decomposition in PracticeCell Decomposition in Practice

Multiple ways to subMultiple ways to sub--divide Cdivide C--spacespaceGrids, QuadGrids, Quad--treestrees

Resolution is a limiting issueResolution is a limiting issueToo fine a grid leads to long search timeToo fine a grid leads to long search time

Too coarse a grid misses pathsToo coarse a grid misses paths

May require post planning smoothingMay require post planning smoothing

Requires a search algorithmRequires a search algorithmE.g. A* and D*E.g. A* and D*

QuadQuad--Tree ExampleTree Example

Quadtree ExampleQuadtree Example

Space Representation Equivalent quadtree

Russell Gayle, The University of North Carolina, Chapel Hill



Free nodeGray node

NW childNE SW

SE



Obstacle Node

G

S(G)



Each of these steps are examples of

pruned quadtrees, or the space at

different resolutions





Complete quadtree

QuadtreeQuadtree--Based Path PlanningBased Path PlanningPreprocessingPreprocessing

Step 1Step 1Grow the obstacles by radius of the robotGrow the obstacles by radius of the robot’’s cross section s cross section Convert the result into a quadtreeConvert the result into a quadtree

Step 2Step 2Compute a distance transform of the free nodes (from the Compute a distance transform of the free nodes (from the center of the region represented by a node to the nearest center of the region represented by a node to the nearest obstacle)obstacle)

Given start and goal pointsGiven start and goal pointsDetermine the nodes S and G which contains these pointsDetermine the nodes S and G which contains these pointsCompute the minimum cost path from S to G through free Compute the minimum cost path from S to G through free nodes using the A* graph searchnodes using the A* graph search

SearchSearchOnce you have your graph you need to Once you have your graph you need to search for the best pathsearch for the best path

Several search methods can be used:Several search methods can be used:A* is the most popular (we will discuss this A* is the most popular (we will discuss this briefly on Monday)briefly on Monday)

Other options include random search, depth Other options include random search, depth first search, breadth first search, etc.first search, breadth first search, etc.

Good search techniques are important for a Good search techniques are important for a variety of reasons variety of reasons –– you will learn more you will learn more about them in 15about them in 15--211 and other CS classes211 and other CS classes

Dijkstra Algorithm

• Is efficient in discrete domains– Given start and goal node in an arbitrary graph– Incrementally label nodes with their distance-from-start

• Produces optimal (shortest) paths

• Applying this to continuous domains requires discretization– Grid-like discretization in high-dimensions is daunting! (curse ofdimensionality)– What are other ways to “discretize” space more efficiently?

27/61

Sample-based Path Finding

28/61

Probabilistic Road Maps

[Kavraki, Svetska, Latombe,Overmars, 95]

q ∈ Rn describes configurationQfree is the set of configurations without collision

29/61


[Kavraki, Svetska, Latombe,Overmars, 95]

q ∈ Rn describes configurationQfree is the set of configurations without collision

29/61


[Kavraki, Svetska, Latombe,Overmars, 95]Probabilistic Road Map– generates a graph G = (V,E) of configurations– such that configurations along each edges are ∈ Qfree 30/61


Given the graph, use (e.g.) Dijkstra to find path from qstart to qgoal.

31/61

Probabilistic Road Maps – generation

Input: number n of samples, number k number of nearest neighborsOutput: PRM G = (V,E)

1: initialize V = ∅, E = ∅2: while |V | < n do // find n collision free points qi3: q ← random sample from Q

4: if q ∈ Qfree then V ← V ∪ {q}5: end while6: for all q ∈ V do // check if near points can be connected7: Nq ← k nearest neighbors of q in V8: for all q′ ∈ Nq do9: if path(q, q′) ∈ Qfree then E ← E ∪ {(q, q′)}

10: end for11: end for

where path(q, q′) is a local planner (easiest: straight line)

32/61

Local Planner

tests collisions up to a specified resolution δ

33/61

Problem: Narrow Passages

The smaller the gap (clearance %) the more unlikely to sample suchpoints.

34/61

PRM theory(for uniform sampling in d-dim space)

• Let a, b ∈ Qfree and γ a path in Qfree connecting a and b.

Then the probability that PRM found the path after n samples is

P (PRM-success |n) ≥ 1− 2|γ|%

e−σ%dn

σ = |B1|2d|Qfree|

% = clearance of γ (distance to obstacles)(roughly: the exponential term are “volume ratios”)

• This result is called probabilistic complete (one can achieve anyprobability with high enough n)

• For a given success probability, n needs to be exponential in d

35/61

Other PRM sampling strategies

illustration from O. Brock’s lecture

Gaussian: q1 ∼ U; q2 ∼ N(q1, σ); if q1 ∈ Qfree and q2 6∈ Qfree, add q1 (or vice versa).

Bridge: q1 ∼ U; q2 ∼ N(q1, σ); q3 = (q1 + q2)/2; if q1, q2 6∈ Qfree and q3 ∈ Qfree, add q3.

• Sampling strategy can be made more intelligence: “utility-basedsampling”

• Connection sampling(once earlier sampling has produced connected components)

36/61

Other PRM sampling strategies

illustration from O. Brock’s lecture

Gaussian: q1 ∼ U; q2 ∼ N(q1, σ); if q1 ∈ Qfree and q2 6∈ Qfree, add q1 (or vice versa).

Bridge: q1 ∼ U; q2 ∼ N(q1, σ); q3 = (q1 + q2)/2; if q1, q2 6∈ Qfree and q3 ∈ Qfree, add q3.

• Sampling strategy can be made more intelligence: “utility-basedsampling”

• Connection sampling(once earlier sampling has produced connected components) 36/61

Probabilistic Roadmaps – conclusions

• Pros:– Algorithmically very simple– Highly explorative– Allows probabilistic performance guarantees– Good to answer many queries in an unchanged environment

• Cons:– Precomputation of exhaustive roadmap takes a long time

(but not necessary for “Lazy PRMs”)

37/61

Rapidly Exploring Random Trees2 motivations:

• Single Query path finding: Focus computational efforts on paths forspecific (qstart, qgoal)

• Use actually controllable DoFs to incrementally explore the searchspace: control-based path finding.

(Ensures that RRTs can be extended to handling differentialconstraints.)

38/61

n = 1

39/61

n = 100

39/61

n = 300

39/61

n = 600

39/61

n = 1000

39/61

n = 2000

39/61

Rapidly Exploring Random Trees

Simplest RRT with straight line local planner and step size α

Input: qstart, number n of nodes, stepsize αOutput: tree T = (V,E)

1: initialize V = {qstart}, E = ∅2: for i = 0 : n do3: qtarget ← random sample from Q

4: qnear ← nearest neighbor of qtarget in V5: qnew ← qnear +

α|qtarget−qnear|

(qtarget − qnear)

6: if qnew ∈ Qfree then V ← V ∪ {qnew}, E ← E ∪ {(qnear, qnew)}7: end for

40/61

Rapidly Exploring Random Trees

RRT growing directedly towards the goal

Input: qstart, qgoal, number n of nodes, stepsize α, βOutput: tree T = (V,E)

1: initialize V = {qstart}, E = ∅2: for i = 0 : n do3: if rand(0, 1) < β then qtarget ← qgoal4: else qtarget ← random sample from Q

5: qnear ← nearest neighbor of qtarget in V6: qnew ← qnear +

α|qtarget−qnear|

(qtarget − qnear)

7: if qnew ∈ Qfree then V ← V ∪ {qnew}, E ← E ∪ {(qnear, qnew)}8: end for

41/61

n = 1

42/61

n = 100

42/61

n = 200

42/61

n = 300

42/61

n = 400

42/61

n = 500

42/61

Bi-directional search

• grow two trees starting from qstart and qgoal

let one tree grow towards the other(e.g., “choose qnew of T1 as qtarget of T2”)

43/61

Summary: RRTs

• Pros (shared with PRMs):– Algorithmically very simple– Highly explorative– Allows probabilistic performance guarantees

• Pros (beyond PRMs):– Focus computation on single query (qstart, qgoal) problem– Trees from multiple queries can be merged to a roadmap– Can be extended to differential constraints (nonholonomic systems)

• To keep in mind (shared with PRMs):– The metric (for nearest neighbor selection) is sometimes critical– The local planner may be non-trivial

44/61

ReferencesSteven M. LaValle: Planning Algorithms,http://planning.cs.uiuc.edu/.

Choset et. al.: Principles of Motion Planning, MIT Press.

Latombe’s “motion planning” lecture, http://robotics.stanford.edu/~latombe/cs326/2007/schedule.htm

45/61

http://planning.cs.uiuc.edu/

http://robotics.stanford.edu/~latombe/cs326/2007/schedule.htm

http://robotics.stanford.edu/~latombe/cs326/2007/schedule.htm

Non-holonomic systems

46/61

Non-holonomic systems

• We define a nonholonomic system as one with differentialconstraints:

dim(ut) < dim(xt)

⇒ Not all degrees of freedom are directly controllable

• Dynamic systems are an example!

• General definition of a differential constraint:For any given state x, let Ux be the tangent space that is generated bycontrols:

Ux = {x : x = f(x, u), u ∈ U}(non-holonomic ⇐⇒ dim(Ux) < dim(x))

The elements of Ux are elements of Tx subject to additional differentialconstraints.

47/61

Car example

x = v cos θ

y = v sin θ

θ = (v/L) tanϕ

|ϕ| < Φ

State q =

x

y

θ

Controls u =

vϕ

Sytem equation

x

y

θ

=

v cos θ

v sin θ

(v/L) tanϕ

48/61

Car example

• The car is a non-holonomic system: not all DoFs are controlled,dim(u) < dim(q)

We have the differential constraint q:

x sin θ − y cos θ = 0

“A car cannot move directly lateral.”

• Analogy to dynamic systems: Just like a car cannot instantly move sidewards,a dynamic system cannot instantly change its position q: the current change inposition is constrained by the current velocity q.

49/61

Path finding with a non-holonomic systemCould a car follow this trajectory?

This is generated with a PRM in the state space q = (x, y, θ) ignoringthe differntial constraint.

50/61

Path finding with a non-holonomic systemThis is a solution we would like to have:

The path respects differential constraints.Each step in the path corresponds to setting certain controls. 51/61

Control-based sampling to grow a tree

• Control-based sampling: fulfils none of the nice exploration propertiesof RRTs, but fulfils the differential constraints:

1) Select a q ∈ T from tree of current configurations

2) Pick control vector u at random

3) Integrate equation of motion over short duration (picked at randomor not)

4) If the motion is collision-free, add the endpoint to the tree

52/61

Control-based sampling for the car

1) Select a q ∈ T2) Pick v, φ, and τ3) Integrate motion from q

4) Add result if collision-free

53/61

J. Barraquand and J.C. Latombe. Nonholonomic Multibody Robots:

Controllability and Motion Planning in the Presence of Obstacles. Algorithmica,

10:121-155, 1993.

car parking54/61

car parking

55/61

parking with only left-steering

56/61

Documents

Programming Robots in ROS Slides adapted from CMU, …web.ics.purdue.edu/~rvoyles/Classes/ROSprogramming/Lectures/PathPlanning.pdfPath Planning Problem Given an initial configuration