1. Motion Planning in Dynamic Environments Jur van den Berg

2. Outline Recap – Configuration Spaces, PRM Moving obstacles Configuration-time space Time constraints Exact methods PRM? RRT Roadmap based Multi-Robot Motion Planning

3. Configuration Space Static environment Dimension = #DOF WorkspaceConfiguration Space Translating in 2D Minkowski Sums

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

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

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

7. Dynamic Environments Moving Obstacles + Static Obstacles Frogger6 DOF Articulated Robot

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

9. Path Constraints Cannot go backward in time Maximum velocity 2D Configuration-Time Space3D Configuration-Time Space

10. Goal Specification Specific configuration and moment in time Specific configuration, as fast as possible g = (x, y, t)g = (x, y)

11. Exact Algorithms Cell decomposition (2D Translation + time) Piecewise linear motions of obstacles No bound on velocity of robot

12. 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

13. Exact Algorithms Robots with more degrees of freedom Rotating obstacles At least exponential running time General problem PSPACE-hard

14. Other Approaches Path-velocity decomposition First: plan path in configuration space Then: tune velocity along path Workspace2D Configuration-Time Space

15. Path-Velocity Decomposition Reduces problem to 2D Cell decomposition, visibility graph Cell decomposition(Adapted) Visibility Graph

16. Probabilistic Approaches PRM?

17. Probabilistic Approaches PRM? Directed Edges

18. Probabilistic Approaches PRM? Directed Edges Transitory Configuration Space Multiple-shot paradigm does not hold

19. Probabilistic Approaches Repetitive dynamic environments Specific class of problems Period is least common multiple of obstacles Roadmap in Configuration-Time SpaceResulting Path

20. Probabilistic Approaches (Rapid Random Trees) RRT Single-shot Build tree oriented along time-axis

21. 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

22. 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

23. 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*

24. Roadmap-based Approaches

25. Multi-Robot Motion Planning Environment: static obstacles (possibly dynamic obstacles as well) N robots with start and goal position 12 Robots24 Robots

26. 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

27. 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

28. Next Class Unknown obstacle motions Online planning Continuous “push” by real-world time

29. 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

30. Motion Planning in Dynamic Environments #2 Jur van den Berg

31. 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

32. Configuration-Time Space Natural space for planning problem Time as additional dimension Configuration SpaceConfiguration-Time Space

33. 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

34. Online Real-Time Planning t = 0 Configuration Space Configuration-Time Space

35. Online Real-Time Planning t = 0.5 Configuration Space Configuration-Time Space

36. Online Real-Time Planning t = 1 Configuration Space Configuration-Time Space

37. Online Real-Time Planning t = 1.5 Configuration Space Configuration-Time Space

38. Online Real-Time Planning t = 2 Configuration Space Configuration-Time Space

39. Online Real-Time Planning t = 2.5 Query! Move from s to g asap. v = 1 Configuration Space Configuration-Time Space

40. Online Real-Time Planning t = 3 Planning in progress… Configuration Space Configuration-Time Space

41. Online Real-Time Planning t = 3.5 Planning in progress… Configuration Space Configuration-Time Space

42. Online Real-Time Planning t = 4 Planning ready! Configuration Space Configuration-Time Space

43. Online Real-Time Planning t = 4.5 Configuration Space Configuration-Time Space

44. Online Real-Time Planning t = 5 Configuration Space Configuration-Time Space

45. 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

46. Problem! Planners do not guarantee to finish within a preset amount of time  –RRT –Roadmap-based

47. 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 >> 

48. Issue How to choose a good value for t –Large t: large latency –Small t: risky – small look-ahead

49. Example (James Kuffner, CMU)

50. Unknown Trajectories Obstacles have unknown future trajectories Available information: –Past trajectories –Current observations (velocity)

51. Trajectory Estimation Estimate future trajectory based on past trajectory and current observation knownassume staticworst caseextrapolation

52. Trajectory Estimation Estimated trajectories are less valuable further away in the future Continuously updating estimations

53. 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

54. 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 + 

55. 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 

56. 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 

57. 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

58. Example Linear extrapolation of obstacles’ trajectories Configuration spaceConfiguration-time space

59. 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

60. Worst-case estimates Obstacles have known maximum velocity Region containing them is a growing disc, or a cone in CT- space

61. Planning amidst growing discs Paths avoiding the growing discs are inherently safe, regardless of value of 

62. 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

63. Advanced trajectory estimation Based on statistical data

64. Obstacle avoidance Highly reactive No planning (no look-ahead) Velocity Obstacles

65. Obstacle avoidance Example

66. Next Class Kinodynamic Motion Planning State Space Planning in Dynamic Environments with Kinodynamic Constraints

67. 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