Programming Robots in ROS Slides adapted from CMU, Harvard ...
Transcript of Programming Robots in ROS Slides adapted from CMU, Harvard ...
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
2) follow obstacles until you can
head toward the goal again
3) continue
assume a left-turning robot
OK ?
The turning direction might be decided beforehand…
“Bug 0” algorithm
Bug Zapper
1) head toward goal
2) follow obstacles until you can
head toward the goal again
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
1) head toward goal on the m-line
2) if an obstacle is in the way,
follow it until you encounter the
m-line again.
“Bug 2” Algorithm
A better bug?
1) head toward goal on the m-line
2) if an obstacle is in the way,
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?
1) head toward goal on the m-line
2) if an obstacle is in the way,
follow it until you encounter the m-
line again closer to the goal.
3) Leave the obstacle and continue
toward the goal
Goal
Start
“Bug 2” Algorithm
Better or worse than Bug1?
• Bug Algorithms
• Artificial Potential Fields
• Wavefront Planning
• Visibility/Voronoi Graphs
• Configuration-Space (C-Space)
• Quad-Trees
• Probabilistic Road Maps
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
Adapted from slides by Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds
Attractive/Repulsive Potential Field
– Uatt is the “attractive” potential --- move to the goal
– Urep is the “repulsive” potential --- avoid obstacles
Adapted from slides by Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds
Artificial Potential Field Methods:
Attractive Potential
)(
)()(
goal
attatt
qk
qUqF
Conical Potential
Quadratic Potential
Adapted from slides by Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds
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?
Adapted from slides by Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds
The Repulsive Potential
Adapted from slides by Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds
Repulsive Potential
Adapted from slides by Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds
Total Potential Function
+ =
)()()( repatt qUqUqU
)()( qUqF
Adapted from slides by Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds
Potential Fields
• Bug Algorithms
• Artificial Potential Fields
• Wavefront Planning
• Visibility/Voronoi Graphs
• Configuration-Space (C-Space)
• Quad-Trees
• Probabilistic Road Maps
Outline
Adapted from slides by Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds
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
Adapted from slides by Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds
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
Adapted from slides by Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds
The Wavefront Planner: Setup
Adapted from slides by Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds
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
Adapted from slides by Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds
The Wavefront in Action (Part 2)
• 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
Adapted from slides by Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds
The Wavefront in Action (Part 3)
• Repeat again...
Adapted from slides by Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds
The Wavefront in Action (Part 4)
• And again...
Adapted from slides by Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds
The Wavefront in Action (Part 5)
• And again until...
Adapted from slides by Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds
The Wavefront in Action (Done)
• You’re done
– Remember, 0’s should only remain if unreachable regions exist
Adapted from slides by Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds
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
• Artificial Potential Fields
• Wavefront Planning
• Visibility/Voronoi Graphs
• Configuration-Space (C-Space)
• Quad-Trees
• Probabilistic Road Maps
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
Quadtree ExampleQuadtree Example
Space Representation Equivalent quadtree
Free nodeGray node
NW childNE SW
SE
Quadtree ExampleQuadtree Example
Space Representation Equivalent quadtree
Obstacle Node
G
S(G)
Quadtree ExampleQuadtree Example
Space Representation Equivalent quadtree
Each of these steps are examples of
pruned quadtrees, or the space at
different resolutions
Quadtree ExampleQuadtree Example
Space Representation Equivalent quadtree
Quadtree ExampleQuadtree Example
Space Representation Equivalent quadtree
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
Probabilistic Road Maps
[Kavraki, Svetska, Latombe,Overmars, 95]
q ∈ Rn describes configurationQfree is the set of configurations without collision
29/61
Probabilistic Road Maps
[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
Probabilistic Road Maps
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
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