Motion Planning in Dynamic Environments

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

• Translating in 2D
• Minkowski Sums

Workspace
Configuration Space
4
Configuration Space

• Articulated Robots (2 Rotating DOF)
• Hard to compute explicitly

Workspace
Configuration Space
5
Probabilistic Roadmap Method

• Collision-test
• Preprocessing create roadmap
• Query find path in roadmap (multiple-shot)

Preprocessing
Query
6
Probabilistic Roadmap Method

• High-dimensional Configuration Spaces
• Probabilistically complete

6 DOF Articulated Robot
6 DOF (3T 3R) Freeflying Robot
7
Dynamic Environments

• Moving Obstacles Static Obstacles

Frogger
6 DOF Articulated Robot
8
Configuration-Time Space

• One additional dimension time
• Obstacles are stationary in CT-space

Configuration Space
Configuration-Time Space
9
Path Constraints

• Cannot go backward in time
• Maximum velocity

2D Configuration-Time Space
3D 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

Workspace
2D 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 Space
Resulting 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 Workspace
Roadmap-Time Space
23
Roadmap-based Approaches

• Discretize Roadmap-time space
• Select time step Dt
• Constrain velocity to be -vmax, 0, vmax
• 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 Robots
24 Robots
26
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
27
Prioritized Multi-Robot Planning

• Assign priorities to robots
• Plan path for robot in order of priorities
• Treat previously planned robots as moving
  obstacles

24 Robots
Problematic Case
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 Space
Configuration-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-Time Space
Configuration Space
35
Online Real-Time Planning

• t 0.5

Configuration-Time Space
Configuration Space
36
Online Real-Time Planning

• t 1

Configuration-Time Space
Configuration Space
37
Online Real-Time Planning

• t 1.5

Configuration-Time Space
Configuration Space
38
Online Real-Time Planning

• t 2

Configuration-Time Space
Configuration Space
39
Online Real-Time Planning

• t 2.5 Query! Move from s to g asap. v 1

Configuration-Time Space
Configuration Space
40
Online Real-Time Planning

• t 3 Planning in progress

Configuration-Time Space
Configuration Space
41
Online Real-Time Planning

• t 3.5 Planning in progress

Configuration-Time Space
Configuration Space
42
Online Real-Time Planning

• t 4 Planning ready!

Configuration-Time Space
Configuration Space
43
Online Real-Time Planning

• t 4.5

Configuration-Time Space
Configuration Space
44
Online Real-Time Planning

• t 5

Configuration-Time Space
Configuration Space
45
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

46
Problem!

• Planners do not guarantee to finish within a
  preset amount of time t
• 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 gt t
t gtgt 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

known
assume static
worst case
extrapolation
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 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

54
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

55
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

56
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

57
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

58
Example

• Linear extrapolation of obstacles trajectories

Configuration space
Configuration-time space
59
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

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 t

62
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

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