Post on 15-Jan-2016
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 Configuration Space
• Translating in 2D
• Minkowski Sums
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 collision-
free 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 Roadmap-time space– Select time step t– 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 spaceC = 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 time for planning
• Find path in CT beginning at (s, tw + )
• Start planning at real-world time tw
• Requirement: planning must finish before real-world time tw + has come
Problem!
• Planners do not guarantee to finish within a preset amount of time – 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 >>
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 worst caseextrapolation
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 time for planning• Plan path with start configuration-time
(s, tw + ) based on estimations acquired at time tw
• Start planning at time tw
Continuous cycle• Finish planning at time tw + • Resulting path: p0 : T C• Start executing path p0 at time tw + • Read sensor input at time tw + • Reserve time for planning• Plan path with start configuration-time
(p0(tw + 2, tw + 2) based on estimations acquired at time tw +
• Start planning at time tw +
Continuous cycle• Finish planning at time tw + 2• Resulting path: p1 : T C• Start executing path p1 at time tw + 2• Read sensor input at time tw + 2• Reserve time for planning• Plan path with start configuration-time
(p1(tw + 3, tw + 3) based on estimations acquired at time tw + 2
• Start planning at time tw + 2
Continuous cycle• Finish planning at time tw + k• Resulting path: pk – 1 : T C• Start executing path pk – 1 at time tw + k• Read sensor input at time tw + k• Reserve time for planning• Plan path with start configuration-time
(pk – 1(tw + (k + 1), tw + (k + 1)) based on estimations acquired at time tw + k
• Start planning at time tw + k
Continuous cycle
• Until the goal has been reached
• A path whose planning starts at time t is used between t + and t + 2
• Paths must be valid for at least time
• Estimates must be valid for at least 2 time
Example
• Linear extrapolation of obstacles’ trajectories
Configuration space Configuration-time space
A good value for • Large
– 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 – Small look-ahead (may be unsafe)– Hard to bias the search in the direction of the goal
• Current works– Choose 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 CT-space
Planning amidst growing discs
• Paths avoiding the growing discs are inherently safe, regardless of value of
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