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

68
Motion Planning with Kinematic and Dynamic
Constraints

• Jur van den Berg

69
Outline

• Kinematic Constraints
• Dynamic Constraints
• Newtonian Point Mass
• State space
• PRM?
• Discretization
• RRT
• Kinematic and Dynamic Constraints

70
Kinematic Constraints

• So far omni-directional maneuverability
• Kinematic constraints restrict motion
• Example car

71
Car-like Robot

• Configuration (x, y, q)
• Constraints
• x(t) v cos q
• y(t) v sin q
• q(t) (v / L) tan f
• f ? fmax
• L length, f steering angle,v speedt
  time (parameter)

72
Car-like Robot

• Three configuration parameters (x, y, q)
• Two inputs f steering angle,v speed
• Underactuated / non-holonomic system

73
Bounded Curvature Path Planning

• Forward moving car
• Existence among polygonal obstacles exponential
  in number of vertices
• Dubins Shortest paths without obstacles
• Consists of straight-lines and maximal turns

74
Shortest Bounded Curvature Path

• Forward moving car
• Without obstacles O(1)
• Inside convex polygon O(n log2 n)
• Among moderate obstacles O(n2 log n)
• General polygonal environment ?

75
Forward and Backward moving car

• Shortest path without obstacles O(1)
• Path existence in general polygonal environment
  
• PRM Implementation

76
More complicated kinematics

• Car pulling trailers
• Configuration(x, y, q0, q1, )
• Inputsspeed s, steering angle f
• How to connect two configurations by a feasible
  path?
• PRM?
• RRT

77
Example Needle Steering

• Movie
• Constant curvature path
• Avoiding obstacles

78
Dynamic Constraints

• So far velocity could change instantly
• Dynamic Constraints bound acceleration
• Example Newtonian point mass

• F lt Fmax
• a F / m
• v(t) a
• p(t) v(t)

79
State space

• State of robotconfiguration velocity
• (px, py, vx, vy)
• Four-dimensional state-space
• Examplevelocity tangent to path, forces in red

80
State space

• Shortest path between two states
• No obstacles
• Not a straight-line in state space!
• Because velocity must be tangent to path (p(t)
  v(t))

81
Two-Point Boundary Value Problem

• Start state (pxs, pys, vxs, vys)
• Goal state (pxg, pyg, vxg, vyg)
• Find path p 0, T ? R2, such that
• px(0) pxs, py(0) pys, px(T) pxg, py(T)
  pyg
• px(0) vxs, py(0) vys, px(T) vxg, py(T)
  vyg
• ?t t ? 0, T px(t)2 py(t)2 ? Fmax / m
• ?t t ? 0, T px(t)2 py(t)2 ? vmax
• Preferrably T is minimal among all possible paths
  p
• How to solve this problem??

82
Pontryagins minimum principle

• Complicated
• Not solvable in general
• Only for very simple cases (like Newtonian point
  mass)
• Can be used in PRM implementation

83
Planning with Dynamic Constraints

• With obstacles
• No exact solutions known
• NP-complete
• Path existence?
• Other solutions approximation algorithm

84
Approximation Discretization

• Algorithm approximates time-optimal trajectory
• Fix small time-step Dt
• Use bang-bang control, i.e, acceleration either
  maximal, or 0

85
Approximation Discretization

• Given current state (p, v)
• a ? (-1, 0, 1 amax, -1, 0, 1 amax)
• v v aDt
• p p vDt 1/2aDt2
• Result Grid in state-time space of reachable
  states
• Spacings in grid amaxDt along velocity axes,
  1/2amaxDt2 along position axes

86
Discretized Grid

• Bounded configuration space, bounded velocity
  finite grid
• Implicit Directed Acyclic Graph in grid
• Search using Dijkstra, A
• Collision-check for obstacles

87
Obstacles in State Space

• Cylindrical. Only the position matters, not the
  velocity

88
Kinodynamic Systems

• Both Kinematic and Dynamics Constraints
• Kinematic constraints
• x(t) v cos q
• y(t) v sin q
• q(t) (v / L) tan f
• f ? fmax
• Dynamic constraints
• v(t) a
• a ? amax
• v ? vmax

89
Kinodynamic Systems

• Adapted RRT

90
Traditional RRT

• For configuration spaces
• Voronoi bias

91
RRT for Kinodynamic Systems

• Dont sample in space, but (randomly) select node
  from tree.
• Randomly select a control input
• Until goal (region) is reached

92
More Examples

• Rotating Hovercraft

93
More Examples

• XWing

94
More Examples

• Car pulling trailers (complicated kinematics --
  no dynamics)

95
Problem

• Hard to reach specific goal position
• Involves two-point boundary value problem
• Therefore goal region
• Previous examples?

96
Online Kinodynamic Planning

• Partial plans. Tree is not built all the way to
  the goal
• Potentially unsafe leaf maybe in inevitable
  collision state

97
Potential Solution

• Augment state-space obstacles with regions of
  inevitable collision
• Hard to compute conservative approximation
• Can possibly be done in preprocessing

98
Kinodynamic Planning in Dynamic Environments

• Add time-dimension to state-space
• Kinodynamic RRT can be used without much
  adaptation
• Other approach decouple kinematic planning from
  dynamic planning and avoiding moving obstacles.

99
Decoupled Planning

• First plan path (or roadmap) obeying kinematic
  and geometric constraints (static obstacles)
• Then plan motion on the path (roadmap) obeying
  dynamic constraints and avoids collisions with
  moving obstacles
• Using discretization method (with extra dimension
  time)

100
(No Transcript)
101
References

• Introduction
• S. LaValle. Planning Algorithms (book)
• Bounded curvature path existence / shortest path
• S. Fortune, G. Wilfong. Planning constrained
  motion.
• J. D. Boissonat, S. Lazard. A polynomial-time
  algorithm for computing a shortest path of
  bounded curvature amidst moderate obstacles
• Sylvain Lazard. Shortest Paths of Bounded
  Curvatuere in a Convex Polygon
• PRM for Car-like robots
• P. Svestka, M. Overmars. Motion Planning for
  Car-like Robots using a Probabilistic Learning
  Approach
• Needle Steering
• http//www.haptics.me.jhu.edu/needlesteering/
• Dynamic Planning Discretization
• B. Donald, P. Xavier, J. Canny, J. Reif.
  Kinodynamic Motion Planning
• Kinodynamic RRT
• S. LaValle, J. Kuffner. Randomized Kinodynamic
  Planning
• Inevitable Collision States
• T. Fraichard, H. Asama. Inevitable Collision
  States. A step towards safer robots?
• Kinodynamic RRT in Dynamic Environments
• D. Hsu, R. Kindel, J.-C. Latombe, S. Rock.
  Randomized Kinodynamic Motion Planning with
  Moving Obstacles
• Decoupled Kinodynamic Planning in Dynamic
  Environments