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 WorkspaceConfiguration Space Translating in 2D Minkowski Sums

Configuration Space Articulated Robots (2 Rotating DOF) Hard to compute explicitly WorkspaceConfiguration Space

Probabilistic Roadmap Method Collision-test Preprocessing: create roadmap Query: find path in roadmap (multiple-shot) PreprocessingQuery

Probabilistic Roadmap Method High-dimensional Configuration Spaces Probabilistically complete 6 DOF Articulated Robot6 DOF (3T + 3R) Freeflying Robot

Dynamic Environments Moving Obstacles + Static Obstacles Frogger6 DOF Articulated Robot

Configuration-Time Space One additional dimension: time Obstacles are stationary in CT-space Configuration SpaceConfiguration-Time Space

Path Constraints Cannot go backward in time Maximum velocity 2D Configuration-Time Space3D 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 Workspace2D 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 SpaceResulting 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 WorkspaceRoadmap-Time Space

Roadmap-based Approaches Discretize Roadmap- time space –Select time step  t –Constrain velocity to be {-v max, 0, v max } 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 Robots24 Robots

Composite Configuration Space Configuration space C = C 1  C 2  …  C N 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 SpaceConfiguration-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 t w Reserve  time for planning Find path in CT beginning at (s, t w +  ) Start planning at real-world time t w Requirement: planning must finish before real-world time t w +  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 knownassume staticworst 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 t w Read sensor input at time t w Reserve  time for planning Plan path with start configuration-time (s, t w +  ) based on estimations acquired at time t w Start planning at time t w

Continuous cycle Finish planning at time t w +  Resulting path: p 0 : T  C Start executing path p 0 at time t w +  Read sensor input at time t w +  Reserve  time for planning Plan path with start configuration-time (p 0 (t w + 2 , t w + 2  ) based on estimations acquired at time t w +  Start planning at time t w + 

Continuous cycle Finish planning at time t w + 2  Resulting path: p 1 : T  C Start executing path p 1 at time t w + 2  Read sensor input at time t w + 2  Reserve  time for planning Plan path with start configuration-time (p 1 (t w + 3 , t w + 3  ) based on estimations acquired at time t w + 2  Start planning at time t w + 2 

Continuous cycle Finish planning at time t w + k  Resulting path: p k – 1 : T  C Start executing path p k – 1 at time t w + k  Read sensor input at time t w + k  Reserve  time for planning Plan path with start configuration-time (p k – 1 (t w + (k + 1) , t w + (k + 1)  ) based on estimations acquired at time t w + k  Start planning at time t w + 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 spaceConfiguration-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(n 3 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