motion planning for dynamic environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · icra 2012...
TRANSCRIPT
![Page 1: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/1.jpg)
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 1 / 64
Motion Planning for Dynamic Environments
Part IV - Dynamic Environments: Methods
Steven M. LaValle
University of Illinois
![Page 2: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/2.jpg)
Solution to Homework 3
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 2 / 64
The chicken follows an interesting curve, depending on λ.
Rajeev Sharma, IEEE TRA 1992
![Page 3: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/3.jpg)
Methods
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 3 / 64
Families:
� Completely predictable environments
� Sensor feedback and collision avoidance
� Planning under bounded motion uncertainty
� Dynamic programming over cost maps
� Information spaces that tolerate uncertainty
![Page 4: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/4.jpg)
Completely predictable
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 4 / 64
![Page 5: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/5.jpg)
Recall Configuration-Time Space
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 5 / 64
Cfree(t1) Cfree(t2) Cfree(t3)
t3t2t1
xt
yt
qG
t
At each time slice t ∈ T , we must avoid
Cobs(t) = {q ∈ C | A(q) ∩ O(t) 6= ∅}
![Page 6: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/6.jpg)
Solutions for Completely Predictable Environments
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 6 / 64
� Sampling-based methods
� Combinatorial methods
� Handling robot speed bounds
� Velocity tuning method
![Page 7: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/7.jpg)
Extending Combinatorial Methods
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64
Transitivity issue:
C2
C3
C1
q
t
In ordinary path planning, if C1 and C2 are adjacent and C2 and C3
adjacent, then a path exists from C1 to C3.
However, for dynamic environments it might require time travel.
![Page 8: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/8.jpg)
Extending Sampling-Based Methods
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 8 / 64
Most approaches depend on a metric ρ : C × C → [0,∞).Must extend into X to ensure that time only increases.
To extend across Z = C × T :
ρZ(x, x′) =
0 if q = q′
∞ if q 6= q′ and t′ ≤ t
ρ(q, q′) otherwise.
� Sampling-based RRTs extend across Z using ρZ .
Bidirectional is a bit more complicated.
� Sampling-based roadmaps (including PRMs) extend to produce
directed roadmaps.
![Page 9: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/9.jpg)
Bounded Speed
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 9 / 64
Robot velocity: v = (x, y)Speed bound: |v| ≤ b for some constant b > 0
The velocity v at every point in Z must point within a cone at all times:
(
x(t+∆t)− x(t))2
+(
y(t+∆t)− y(t))2
≤ b2(∆t)2.
t
y
Warning: PSPACE-hard in general.
![Page 10: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/10.jpg)
Velocity Tuning
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 10 / 64
O(t)
A
t
1
0
s
Workspace State space
Compute a collision-free path: τ : [0, 1] → Cfree.
Design a timing function (or time scaling): σ : T → [0, 1].This produces a composition φ = τ ◦ σ, which maps from T to Cfree via
[0, 1].
![Page 11: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/11.jpg)
Velocity Tuning
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 11 / 64
Because it is a 2D problem, many methods can be used.
Simple grid search: BFS, DFS, Dijkstra, A∗, ...
It is more elegant and efficient to use combinatorial methods.
t
1
0
s
For example, trapezoidal decomposition.
![Page 12: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/12.jpg)
Sensor Feedback
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 12 / 64
![Page 13: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/13.jpg)
Velocity Obstacles
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 13 / 64
B
A
vA
vB
� Two rigid bodies A and B moving in R2.
� They have constant velocities vA and vB .
� If vB is constant, what values of vA cause collision?
![Page 14: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/14.jpg)
Velocity Obstacles
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 14 / 64
λ(p, v) = {p+ tv | t ≥ 0}
V OAB(vB) = {vA | λ(pA, vA − vB) ∩ Cobs 6= ∅}
Here, Cobs = B ⊖A (Minkowski difference).
Fiorini, Shiller, 1998.
![Page 15: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/15.jpg)
Reciprocal Velocity Obstacles
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 15 / 64
What is both bodies react? Oscillation possible.
Suppose that all bodies follow the same strategy.
This can be taken into account for a great advantage.
RV OAB(vB) = {v′A | 2v′A − vA ∈ V OA
b (vB)}
Choose v′A as the average of its current velocity and a velocity that lies
outside the velocity obstacle.
van den Berg, Lin, Manocha, 2008
![Page 16: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/16.jpg)
Recriprocal Velocity Obstacles
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 16 / 64
A computed result:
Try it at the next ICRA coffee break...
![Page 17: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/17.jpg)
Other Sensor Feedback Strategies
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 17 / 64
� Potential fields, Khatib, 1980
� Vector field histogram, Borenstein, Koren, 1991
� Dynamic window approach, Fox, Burgard, Thrun, 1997
� Nearness diagram, Minguez, Montano, 2004
Many more...
![Page 18: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/18.jpg)
Bounded Uncertainty
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 18 / 64
![Page 19: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/19.jpg)
Time-Minimal Trajectories
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 19 / 64
� Point (or disc) robot moves at constant speed.
� A finite set of point (or disc) obstacles.
� Obstacles have omnidirectional speed bound.
� Problem: Compute time-optimal collision-free trajectory.
van den Berg, Overmars, 2008
![Page 20: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/20.jpg)
Time-Minimal Trajectories
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 20 / 64
A computed example, shown through configuration-time space:
Can solve problems O(n3 lg n) time.
It is related to shortest-path graphs in the plane (bitangents).
Recently improved to O(n2 lgn) by Maheshwari et al.
![Page 21: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/21.jpg)
Dynamic Programming
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 21 / 64
![Page 22: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/22.jpg)
Cost Maps
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 22 / 64
Instead of a crisp Cobs and Cfree, a cost could be associated with each q
(or each neighborhood).
![Page 23: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/23.jpg)
Value Iteration Methods
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 23 / 64
Let X be any state space.
We can make a state-time space by Z = X × T .
Let U be an action set.
There are K + 1 stages (1, 2, . . . ,K + 1) along the time axis.
Let x′ = f(x, u) be a state transition equation.
Let L denote a stage-additive cost functional,
L =K∑
k=1
l(xk, uk) + lK+1(xK+1).
The task or goal can be expressed in terms of L.
![Page 24: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/24.jpg)
Value Iteration Methods
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 24 / 64
A feedback plan is represented as π : X → U
Let G∗
k(xk) denote the optimal cost to go from xk at stage k (optimized
over all possible π).
xk
Stage k + 1
Stage k
Possible next states
Bellman’s dynamic programming equation:
G∗
k(xk) = minuk∈U(xk)
{
l(xk, uk) +G∗
k+1(xk+1)}
![Page 25: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/25.jpg)
Value Iteration Methods
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 25 / 64
Bellman’s dynamic programming equation:
G∗
k(xk) = minuk∈U(xk)
{
l(xk, uk) +G∗
k+1(xk+1)}
.
Algorithm:
� Initially, G∗
K+1 is known (from lK+1(xK+1)).
� Compute G∗
K from G∗
K+1.
� Compute G∗
K−1 from G∗
K .
�
...
![Page 26: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/26.jpg)
Value Iteration Methods
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 26 / 64
But X and U are usually continuous spaces.
A finite subset of U can be sampled in Bellman’s equation.
Interpolation (this is the 1D case) over X :
G∗
k+1(x) ≈ αG∗
k+1(si) + (1− α)G∗
k+1(si+1)
![Page 27: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/27.jpg)
Value Iteration Methods
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 27 / 64
Stochastic version not difficult.
Let p(xk+1|xk, uk) be a probabilistic state transition equation.
Bellman’s equation becomes:
G∗
k(xk) = minuk∈U(xk)
{
l(xk, uk) +∑
xk+1
G∗
k+1(xk+1)p(xk+1|xk, uk)}
.
Optimizes the expected cost-to-go.
In the stationary case, there are Dijkstra-like versions.
See Planning Algorithms: Sections 2.3.2, 8.5.5, 10.6
![Page 28: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/28.jpg)
Applying to Dynamic Environments
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 28 / 64
Recall the hybrid system formulation.
m = 4
m = 1 m = 2
m = 3
m = 4
m = 3
m = 2
m = 1
C
C
C
C
Modes Layers
Doors may open or close according to a Markov chain.
![Page 29: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/29.jpg)
Applying to Dynamic Environments
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 29 / 64
The optimal cost-to-go and feedback plan.0 20 40 60 80 100
XG
0 20 40 60 80 100
XG
Cost-to-go, open mode Cost-to-go, closed mode
XG XG
Vector field, open mode Vector field, closed mode
![Page 30: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/30.jpg)
Maintaining Visibility
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 30 / 64
ObserverTarget
Visibility Region
Trajectory: known or unknown
� A robot must follow a moving target with a camera.
� How to move the robot to maintain visibility as much as possible?
� Optimize the total robot motion.
� Predictable and partially predictable target cases
LaValle, Gonzalez-Banos, Becker, Latombe, 1997
![Page 31: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/31.jpg)
Maintaining Visibility
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 31 / 64
Optimal robot trajectories computed using value iteration:
For unpredictable target, move robot to maximize the target’s minimum
time to escape.
![Page 32: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/32.jpg)
D-Star
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 32 / 64
D-Star: Stentz, 1994
D-Star Lite: Koenig, Likhachev, 2002
Consider A∗ search on a weighted grid graph.
Execution of the plan causes new information to be learned.
Enhance A∗ to allow edge costs to increase or decrease.
![Page 33: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/33.jpg)
D-Star
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 33 / 64
Let
rhs(q) = minq′∈Succ(q)
{c(q, q′) + g(q′)}
For the optimal cost-to-go function, Bellman’s equation should be satisfied
everywhere:
g(q) = rhs(q)
(Also, g(qG) = 0.)
If it is not, then fix it!
![Page 34: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/34.jpg)
D-Star
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 34 / 64
Let h(q, q′) be a heuristic underestimate of the optimal cost from q to q′.
Keep search queue sorted by key value:
min(g(q), rhs(s)) + h(qI , s)
If vertices have equal key value, then select one with smallest
min(g(q), rhs(s)).
When edges costs change, affected nodes are placed on the search
queue.
Iterations continue until all affected nodes are fixed, and Bellman is happy
again.
![Page 35: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/35.jpg)
Dynamic Replanning
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 35 / 64
![Page 36: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/36.jpg)
Sliding Window
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 36 / 64
Consider the following loop:
1. Plan a sequence of actions
2. Take the first action
3. Receive new information from sensors
4. Go to 1
![Page 37: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/37.jpg)
Sliding Window
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 37 / 64
That was the usual sense-plan-act loop.
Related ideas:
� Receding horizon control
� Model predictive control
� Dynamic replanning
� Partial motion planning
� Anytime planning
![Page 38: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/38.jpg)
Partial Motion Planning
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 38 / 64
� Construct a partial plan toward the goal within allotted time.
� Compute Xric (inevitable collision states).
� Ensure that paths are safe by avoiding Xric.
� While executing, construct the next partial plan.
Fraichard, Asama, 2004; Petti, Fraichard, 2005
![Page 39: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/39.jpg)
Partial Motion Planning
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 39 / 64
Probabilistic RRTs
� Use partial planning paradigm.
� Build a probabilistic “cost map” that biases RRT growth into lower
collision probabilities.
� Use HMM prediction models learned from other moving bodies.
Fulgenzi, Spalanzani, and Laugier, 2009
![Page 40: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/40.jpg)
Replanning From Scratch
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 40 / 64
Kuffner, 2004
Run A∗ or Dijkstra but with reduced neighborhood structure.
Computation times around 10ms.
![Page 41: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/41.jpg)
Replanning
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 41 / 64
A few other replanning works:
� Leven, Hutchinson, 2002
� Jaillet, Simeon, 2004
� Kallmann, Bargmann, Mataric 2004
� Vannoy, Xiao 2006
� Bekris, Kavraki, 2007
� Nabbe, Hebert, 2007
� Bekris, 2010
![Page 42: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/42.jpg)
Anytime Algorithms
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 42 / 64
Appearing throughout compute science, an any-time algorithm has
properties:
� May be terminated at any time
� The solution it produces gradually improves over time
This seems ideally suited for on-line planning and execution.
![Page 43: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/43.jpg)
Anytime RRTs
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 43 / 64
Ferguson, Stentz, 2006
![Page 44: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/44.jpg)
Anytime RRTs
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 44 / 64
![Page 45: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/45.jpg)
Anytime RRTs
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 45 / 64
![Page 46: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/46.jpg)
Anytime RRTs
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 46 / 64
![Page 47: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/47.jpg)
Anytime RRTs
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 47 / 64
![Page 48: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/48.jpg)
RRT*
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 48 / 64
� Grow RRT in the usual way
� When a new vertex xnew is added, try to connect to other RRT
vertices within radius ρ.
� Among all paths to the root from xnew, add a new RRT edge only
for the shortest one.
� If possible to reduce cost for other vertices within radius ρ by
connecting to xnew, then disconnect them from their parents and
connect them through xnew.
� The radius ρ is prescribed through careful percolation theory
analysis (related to dispersion).
� RRT* yields asymptotically optimal paths through Cfree.
Karaman, Frazzoli, IJRR 2011
![Page 49: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/49.jpg)
RRT*
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 49 / 64
![Page 50: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/50.jpg)
Anytime D*
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 50 / 64
Backwards A*:
� Sort queue by: g(q) + h(qI , q)
� g(q) is the optimal cost-to-come from qG.
� h(qI , q) is the guaranteed underestimate of the optimal cost from
qI to q.
![Page 51: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/51.jpg)
Anytime D*
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 51 / 64
Anytime A*:
� Sort queue by: g(q) + γh(qI , q)
� γ ≥ 1 is an inflation factor
� It causes non-optimality, but no worse than a factor of γ.
� Approach: Generate a quick solution for large γ, and then gradually
decrease it.
![Page 52: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/52.jpg)
Anytime D*
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 52 / 64
Anytime D*:
� Use g(q) + γh(qI , q) in D* lite
� Optimality factor for computed paths remains γ.
� Likhachev, Ferguson, Gordon, Stentz, Thrun, 2005
![Page 53: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/53.jpg)
Anytime D*
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 53 / 64
Example:
![Page 54: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/54.jpg)
Information Spaces
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 54 / 64
![Page 55: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/55.jpg)
Visibility-Based Pursuit-Evasion
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 55 / 64
Recall simple model: Evader moves on a continuous path.
An exact cell decomposition method can solve it.
Guibas et al. 1999
![Page 56: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/56.jpg)
Visibility-Based Pursuit-Evasion
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 56 / 64
Identify all unique situations that can occur:
An information state is identified by (x, S) in which
x = the position of the pursuer
S = set of possible evader positions
The set of all information states forms an information space.
Many closed-path motions retain the same information state.
![Page 57: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/57.jpg)
Visibility-Based Pursuit-Evasion
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 57 / 64
Environment Inflections
Bitangents Cell Decomposition
![Page 58: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/58.jpg)
Imperfect State Information
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 58 / 64
Two types of imperfect state information:
1. Environment: Obstacles, cost map, moving body configurations
2. Robot: The localization problem
These generally force plan feedback to occur over an information space:
π : I → U
![Page 59: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/59.jpg)
Imperfect State Information
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 59 / 64
What does ι ∈ I look like? Possibilities:
� A partial map with robot localized
� A full map with a pdf over robot configurations
� A topological map with robot localized
In the most general setting, we may obtain either a set
F (uk−1, yk) ⊆ X
or a pdf
p(xk | uk−1, yk)
over whatever X state space is needed.
The state x ∈ X may encode robot configuration, map, other bodies.
![Page 60: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/60.jpg)
Planning in the Probabilistic Information Space
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 60 / 64
State: x ∈ X encodes configuration and velocities of robot and bodies.
Stochastic transition law: p(xk+1|xk, uk)Disturbed sensor mapping: p(yk|xk)
� Receding horizon approach
� Partially closed loop: Estimate future sensor readings
� Compute information feedback strategies
DuToit, Burdick, 2012
![Page 61: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/61.jpg)
Planning in the Probabilistic Information Space
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 61 / 64
There are many other approaches to planning in belief space:
� Roy, Burgard, Fox, Thrun, 1998
� Pineau, Gordon, 2005
� Kurniawati, Hsu, Lee, 2008
� Prentice, Roy, 2009
� Hauser, 2010
� Platt, Kaelbling, Lozano-Perez, Tedrake, 2012
This list is very incomplete...
![Page 62: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/62.jpg)
Forward Projections
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 62 / 64
C
T
C
T
Static Predictable
C
T
C
T
Bounded Uncertainty Probabilistic Uncertainty
![Page 63: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/63.jpg)
Summary of Part IV
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 63 / 64
� Model: predictable, bounded uncertainty, probabilistic
� Sensor feedback vs. dynamic replanning vs. computing optimal
strategy
� The power of dynamic programming
� In which information space should the robot live?
� There are NP-hard problems everywhere. We have yet to really
understand what makes some problems simpler.
� Which method to use? Need demo, robust experimental system,
theoretical guarantees?
![Page 64: Motion Planning for Dynamic Environmentsmsl.cs.uiuc.edu/~lavalle/icra12/slides4.pdf · ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 7 / 64 Transitivity issue: C2 C3 C1 q](https://reader034.vdocuments.us/reader034/viewer/2022050206/5f58f455fb9cf54a2b7f6d14/html5/thumbnails/64.jpg)
Homework 4: Solve During This Century
Completely predictable
Sensor Feedback
Bounded Uncertainty
Dynamic Programming
Dynamic Replanning
Information Spaces
ICRA 2012 Tutorial - Motion Planning - 14 May 2012 – 64 / 64
Let A be a rigid, polygonal (or semi-algebraic) robot.
Let µ(A) denote the area of A.
1
1
A
What is the largest robot, in terms of µ(A), that can fit through the
corridor?