Transcript Document

Motion Planning in Dynamic
Environments
Jur van den Berg
Outline
•
•
•
•
•
•
•
•
•
Recap – Configuration Spaces, PRM
Moving obstacles
Configuration-time space
Time constraints
Exact methods
PRM?
RRT
Roadmap based
Multi-Robot Motion Planning
Configuration Space
• Static environment
• Dimension = #DOF
Workspace
• Translating in 2D
• Minkowski Sums
Configuration Space
Configuration Space
• Articulated Robots (2 Rotating DOF)
• Hard to compute explicitly
Workspace
Configuration Space
Probabilistic Roadmap Method
• Collision-test
• Preprocessing: create roadmap
• Query: find path in roadmap (multiple-shot)
Preprocessing
Query
Probabilistic Roadmap Method
• High-dimensional Configuration Spaces
• Probabilistically complete
6 DOF Articulated Robot
6 DOF (3T + 3R) Freeflying Robot
Dynamic Environments
• Moving Obstacles + Static Obstacles
Frogger
6 DOF Articulated Robot
Configuration-Time Space
• One additional dimension: time
• Obstacles are stationary in CT-space
Configuration Space
Configuration-Time Space
Path Constraints
• Cannot go backward in time
• Maximum velocity
2D Configuration-Time Space
3D Configuration-Time Space
Goal Specification
• Specific configuration and moment in time
• Specific configuration, as fast as possible
g = (x, y, t)
g = (x, y)
Exact Algorithms
• Cell decomposition (2D Translation + time)
• Piecewise linear motions of obstacles
• No bound on velocity of robot
Exact Algorithms
•
•
•
•
•
•
Asteroid Avoidance (2D Translation + time)
Constant linear motions of obstacles
Constant number of obstacles
Bound on velocity of robot
Time-minimal path
Polynomial time algorithm
Exact Algorithms
• Robots with more degrees of freedom
• Rotating obstacles
• At least exponential running time
• General problem PSPACE-hard
Other Approaches
• Path-velocity decomposition
• First: plan path in configuration space
• Then: tune velocity along path
Workspace
2D Configuration-Time Space
Path-Velocity Decomposition
• Reduces problem to 2D
• Cell decomposition, visibility graph
Cell decomposition
(Adapted) Visibility Graph
Probabilistic Approaches
• PRM?
Probabilistic Approaches
• PRM?
• Directed Edges
Probabilistic Approaches
•
•
•
•
PRM?
Directed Edges
Transitory Configuration Space
Multiple-shot paradigm does not hold
Probabilistic Approaches
• Repetitive dynamic environments
• Specific class of problems
• Period is least common multiple of obstacles
Roadmap in Configuration-Time Space
Resulting Path
Probabilistic Approaches
• (Rapid Random Trees) RRT
• Single-shot
• Build tree oriented along time-axis
Probabilistic Approaches
• Advantages
– Any dimensional configuration-spaces
– Any behavior of obstacles
– Only requirement: is robot configured at c collisionfree at time t ?
• Disadvantages
– Narrow passages
– All effort in query phase
Roadmap-based Approaches
• Roadmap-velocity decomposition
• First: build roadmap in configuration space
• Then: find trajectory on roadmap avoiding
moving obstacles
Roadmap in Workspace
Roadmap-Time Space
Roadmap-based Approaches
• Discretize Roadmaptime space
– Select time step Dt
– Constrain velocity to
be {-vmax, 0, vmax}
• Find shortest path
using A*
Roadmap-based Approaches
Multi-Robot Motion Planning
• Environment: static obstacles (possibly
dynamic obstacles as well)
• N robots with start and goal position
12 Robots
24 Robots
Composite Configuration Space
• Configuration space
C = C1  C2  …  CN
• Dimension is sum of
DOFs of all robots
• Very high-dimensional
• Cylindrical obstacles
Composite Configuration Space
3 Robots, 1 DOF each
Prioritized Multi-Robot Planning
• Assign priorities to robots
• Plan path for robot in order of priorities
• Treat previously planned robots as
moving obstacles
Problematic Case
24 Robots
Next Class
• Unknown obstacle motions
• Online planning
• Continuous “push” by real-world time
References
•
Introduction
– M. de Berg, M. van Kreveld, M. Overmars, O. Schwarzkopf. Computational Geometry,
Algorithms and Applications (book)
– S. LaValle. Planning Algorithms (book – freely available on-line)
– J. van den Berg. Path Planning in Dynamic Environments (PhD thesis)
•
Exact Planning with Moving Obstacles
– J. Reif, M. Sharir. Motion Planning in the Presence of Moving Obstacles
– K. Fujimura, H. Samet. Planning a Time-Minimal Motion among Moving Obstacles
•
Path-Velocity Decomposition
– K. Kant, S. Zucker. Toward Efficient Trajectory Planning: the Path-Velocity
Decomposition
– K. Fujimura. Time-Minimum Routes in Time-Dependent Networks
•
Probabilistic Approaches
– D. Hsu, R. Kindel, J.-C. Latombe, S. Rock. Randomized Kinodynamic Motion Planning
with Moving Obstacles
– J. van den Berg, M. Overmars. Path Planning in Repetitive Environments
•
Roadmap-Based Approaches
– J. van den Berg, M. Overmars. Roadmap-Based Motion Planning in Dynamic
Environments
•
Prioritized Multi-Robot Motion Planning
– J. van den Berg, M. Overmars. Prioritized Motion Planning for Multiple Robots
Motion Planning in Dynamic
Environments #2
Jur van den Berg
Outline
•
•
•
•
•
•
•
Recap Configuration-Time space
Offline vs. Online (real-time)
Anytime (partial) planning
Known vs. Unknown Trajectories
Continuous cycle of sensing and planning
Static vs. Dynamic
Estimating future trajectories
Configuration-Time Space
• Natural space for planning problem
• Time as additional dimension
Configuration Space
Configuration-Time Space
Offline vs. Online Planning
• Offline: “enough” time for planning
• Online: plan while the world is changing
– Real world time and time modeled in CT
space are related
• Applications offline:
– Multi-robot motion planning
• Applications online:
– Real-time vehicle navigation
Online Real-Time Planning
• t=0
Configuration Space
Configuration-Time Space
Online Real-Time Planning
• t = 0.5
Configuration Space
Configuration-Time Space
Online Real-Time Planning
• t=1
Configuration Space
Configuration-Time Space
Online Real-Time Planning
• t = 1.5
Configuration Space
Configuration-Time Space
Online Real-Time Planning
• t=2
Configuration Space
Configuration-Time Space
Online Real-Time Planning
• t = 2.5
Query! Move from s to g asap. v = 1
Configuration Space
Configuration-Time Space
Online Real-Time Planning
• t=3
Planning in progress…
Configuration Space
Configuration-Time Space
Online Real-Time Planning
• t = 3.5
Planning in progress…
Configuration Space
Configuration-Time Space
Online Real-Time Planning
• t=4
Planning ready!
Configuration Space
Configuration-Time Space
Online Real-Time Planning
• t = 4.5
Configuration Space
Configuration-Time Space
Online Real-Time Planning
• t=5
Configuration Space
Configuration-Time Space
Online Planning
•
•
•
•
•
•
Planning takes time!
If query (s, g) is posed at real-world time tw
Reserve t time for planning
Find path in CT beginning at (s, tw + t)
Start planning at real-world time tw
Requirement: planning must finish before
real-world time tw + t has come
Problem!
• Planners do not guarantee to finish within a
preset amount of time t
– RRT
– Roadmap-based
Anytime or Partial Planning
•
•
•
•
Not necessarily plan all the way to the goal
Returns “best” initial path when time runs out
Continue planning while executing path
Planning must always be “ahead” of execution
t=t
t>t
t >> t
Issue
• How to choose a good value for t
– Large t: large latency
– Small t: risky – small look-ahead
Example
• (James Kuffner, CMU)
Unknown Trajectories
• Obstacles have unknown future trajectories
• Available information:
– Past trajectories
– Current observations (velocity)
Trajectory Estimation
• Estimate future trajectory based on past
trajectory and current observation
known
assume static
extrapolation
worst case
Trajectory Estimation
• Estimated trajectories are less valuable
further away in the future
• Continuously updating estimations
Continuous cycle of sensing and
planning
•
•
•
•
Receive query (s, g) at time tw
Read sensor input at time tw
Reserve t time for planning
Plan path with start configuration-time
(s, tw + t) based on estimations acquired
at time tw
• Start planning at time tw
Continuous cycle
Finish planning at time tw + t
Resulting path: p0 : T  C
Start executing path p0 at time tw + t
Read sensor input at time tw + t
Reserve t time for planning
Plan path with start configuration-time
(p0(tw + 2t), tw + 2t) based on estimations
acquired at time tw + t
• Start planning at time tw + t
•
•
•
•
•
•
Continuous cycle
•
•
•
•
•
•
Finish planning at time tw + 2t
Resulting path: p1 : T  C
Start executing path p1 at time tw + 2t
Read sensor input at time tw + 2t
Reserve t time for planning
Plan path with start configuration-time
(p1(tw + 3t), tw + 3t) based on estimations
acquired at time tw + 2t
• Start planning at time tw + 2t
Continuous cycle
•
•
•
•
•
•
Finish planning at time tw + kt
Resulting path: pk – 1 : T  C
Start executing path pk – 1 at time tw + kt
Read sensor input at time tw + kt
Reserve t time for planning
Plan path with start configuration-time
(pk – 1(tw + (k + 1)t), tw + (k + 1)t) based on
estimations acquired at time tw + kt
• Start planning at time tw + kt
Continuous cycle
• Until the goal has been reached
• A path whose planning starts at time t is
used between t + t and t + 2t
• Paths must be valid for at least t time
• Estimates must be valid for at least 2t time
Example
• Linear extrapolation of obstacles’
trajectories
Configuration space
Configuration-time space
A good value for t
• Large t
–
–
–
–
Large look-ahead
Heavy reliance on estimates
Slow reaction on changes in environment (may be unsafe)
Spends much time on (portions of) path that will not be
used
– Important decisions in near-future based on unreliable
estimates of far future
• Small t
– Small look-ahead (may be unsafe)
– Hard to bias the search in the direction of the goal
• Current works
– Choose t based on “dynamicity” of the environment
Worst-case estimates
• Obstacles have
known maximum
velocity
• Region containing
them is a growing
disc, or a cone in CTspace
Planning amidst growing discs
• Paths avoiding the growing discs are
inherently safe, regardless of value of t
Planning amidst growing discs
• Space fills up quickly: continuous
replanning
• Robot must move faster than obstacles
• O(n3 log n) algorithm – very fast in practice
Advanced trajectory estimation
• Based on statistical data
Obstacle avoidance
• Highly reactive
• No planning (no look-ahead)
• Velocity Obstacles
Obstacle avoidance
• Example
Next Class
• Kinodynamic Motion Planning
• State Space
• Planning in Dynamic Environments with
Kinodynamic Constraints
References
•
Introduction
– J. van den Berg. Path Planning in Dynamic Environments (PhD thesis)
•
Online Motion Planning in Unknown Environments
– S. Petty, T. Fraichard. Safe Motion Planning in Dynamic Environments
– D. Hsu, R. Kindel, J.-C. Latombe, S. Rock. Randomized Kinodynamic Motion Planning
with Moving Obstacles
– J. van den Berg, D. Ferguson, J. Kuffner. Anytime Path Planning and Replanning in
Dynamic Environments
•
Planning among Growing Discs
– J. van den Berg, M. Overmars. Planning the Shortest Safe Path amidst Unpredictably
Moving Obstacles
•
Trajectory Estimation
– D. Vasquez, F. Large, T. Fraichard, C. Laugier. Moving obstacles’ motion prediction for
autonomous navigation.
•
Velocity Obstacles
– P. Fiorini, Z. Shiller. Motion Planning in Dynamic Environments using Velocity
Obstacles
– F. Large, S. Sckhavat, Z. Shiller, C. Laugier. Using non-linear velocity obstacles to plan
motions in a dynamic environment