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

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
• Translating in 2D
• Minkowski Sums
Configuration Space
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 collisionfree 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 Roadmaptime space
– Select time step Dt
– 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 space
C = 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 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
Problem!
• Planners do not guarantee to finish within a
preset amount of time t
– 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>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
extrapolation
worst case
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 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
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
•
•
•
•
•
•
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
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
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
Example
• Linear extrapolation of obstacles’
trajectories
Configuration space
Configuration-time space
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
Worst-case estimates
• Obstacles have
known maximum
velocity
• Region containing
them is a growing
disc, or a cone in CTspace
Planning amidst growing discs
• Paths avoiding the growing discs are
inherently safe, regardless of value of t
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
Motion Planning with Kinematic
and Dynamic Constraints
Jur van den Berg
Outline
•
•
•
•
•
•
•
•
Kinematic Constraints
Dynamic Constraints
Newtonian Point Mass
State space
PRM?
Discretization
RRT
Kinematic and Dynamic Constraints
Kinematic Constraints
• So far: omnidirectional
maneuverability
• Kinematic
constraints restrict
motion
• Example: car
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 = speed
t = time (parameter)
Car-like Robot
• Three configuration
parameters:
(x, y, q)
• Two inputs:
f = steering angle,
v = speed
• Underactuated / nonholonomic system
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
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: ?
Forward and Backward moving car
• Shortest path without obstacles:
O(1)
• Path existence in general
polygonal environment: …
• PRM Implementation
More complicated kinematics
• Car pulling trailers
• Configuration:
(x, y, q0, q1, …)
• Inputs:
speed s,
steering angle f
• How to connect two
configurations by a
feasible path?
• PRM?
• RRT
Example: Needle Steering
• Movie
• Constant curvature path
• Avoiding obstacles
Dynamic Constraints
• So far: velocity
could change
instantly
• Dynamic
Constraints bound
acceleration
• Example:
Newtonian point
mass
•
•
•
•
||F|| < Fmax
a=F/m
v’(t) = a
p’(t) = v(t)
State space
• State of “robot”:
configuration +
velocity
• (px, py, vx, vy)
• Four-dimensional
state-space
• Example:
velocity tangent to
path, forces in red
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))
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
– p’x(0) = vxs, p’y(0) = vys, p’x(T) = vxg, p’y(T) = vyg
– t : t  [0, T] : p’’x(t)2 + p’’y(t)2  Fmax / m
– t : t  [0, T] : p’x(t)2 + p’y(t)2  vmax
– Preferrably T is minimal among all possible
paths p
• How to solve this problem??
Pontryagin’s minimum principle
• Complicated
• Not solvable in general
• Only for very simple cases (like Newtonian
point mass)
• Can be used in PRM implementation
Planning with Dynamic Constraints
• With obstacles
• No exact
solutions known
• NP-complete
• Path existence?
• Other solutions:
approximation
algorithm
Approximation: Discretization
• Algorithm
approximates timeoptimal trajectory
• Fix small time-step
Dt
• Use “bang-bang”
control, i.e,
acceleration either
maximal, or 0
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
Discretized Grid
• Bounded configuration space, bounded
velocity: finite grid
• Implicit Directed Acyclic Graph in grid
• Search using Dijkstra, A*
• Collision-check for obstacles
Obstacles in State Space
• Cylindrical. Only the position matters, not
the velocity
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
Kinodynamic Systems
• Adapted RRT
Traditional RRT
• For configuration spaces
• Voronoi bias
RRT for Kinodynamic Systems
• Don’t sample in space, but (randomly)
select node from tree.
• Randomly select a control input
• Until goal (region) is reached
More Examples
• Rotating Hovercraft
More Examples
• XWing
More Examples
• Car pulling trailers (complicated
kinematics -- no dynamics)
Problem
• Hard to reach specific goal position
• Involves two-point boundary value
problem
• Therefore goal region
• Previous examples?
Online Kinodynamic Planning
• Partial plans. Tree
is not built all the
way to the goal
• Potentially unsafe:
leaf maybe in
inevitable
collision state
Potential Solution
• Augment state-space obstacles with regions
of inevitable collision
• Hard to compute; conservative approximation
• Can possibly be done in preprocessing
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.
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)
References
•
Introduction
–
•
Bounded curvature path existence / shortest path
–
–
–
•
T. Fraichard, H. Asama. Inevitable Collision States. A step towards safer robots?
Kinodynamic RRT in Dynamic Environments
–
•
S. LaValle, J. Kuffner. Randomized Kinodynamic Planning
Inevitable Collision States
–
•
B. Donald, P. Xavier, J. Canny, J. Reif. Kinodynamic Motion Planning
Kinodynamic RRT
–
•
http://www.haptics.me.jhu.edu/needlesteering/
Dynamic Planning: Discretization
–
•
P. Svestka, M. Overmars. Motion Planning for Car-like Robots using a Probabilistic Learning
Approach
Needle Steering
–
•
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
–
•
S. LaValle. Planning Algorithms (book)
D. Hsu, R. Kindel, J.-C. Latombe, S. Rock. Randomized Kinodynamic Motion Planning with Moving
Obstacles
Decoupled Kinodynamic Planning in Dynamic Environments
–
–
T. Fraichard. Trajectory Planning in a Dynamic Environment, a `state-time space’ approach.
J. van den Berg, M. Overmars. Kinodynamic Planning on Roadmaps in Dynamic Environments