Motion Planning in Dynamic Environments Jur van den Berg.

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

• Translating in 2D

• Minkowski Sums

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?


• PRM?

• Directed Edges


• PRM?

• Directed Edges

• Transitory Configuration Space

• Multiple-shot paradigm does not hold


• Repetitive dynamic environments

• Specific class of problems

• Period is least common multiple of obstacles

Roadmap in Configuration-Time Space Resulting Path


• (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 Workspace Roadmap-Time Space


• Discretize Roadmap-time space– Select time step t– Constrain velocity to

be {-vmax, 0, vmax}

• Find shortest path using A*


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

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


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



• t = 0.5



• t = 1



• t = 1.5



• t = 2



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



• t = 3 Planning in progress…



• t = 3.5 Planning in progress…



• t = 4 Planning ready!



• t = 4.5



• t = 5


Online Planning

• Planning takes time!

• If query (s, g) is posed at real-world time tw

• Reserve time for planning

• Find path in CT beginning at (s, tw + )

• Start planning at real-world time tw

• Requirement: planning must finish before real-world time tw + 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

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

• Read sensor input at time tw

• Reserve time for planning• Plan path with start configuration-time

(s, tw + ) based on estimations acquired at time tw

• Start planning at time tw

Continuous cycle• Finish planning at time tw + • Resulting path: p0 : T C• Start executing path p0 at time tw + • Read sensor input at time tw + • Reserve time for planning• Plan path with start configuration-time

(p0(tw + 2, tw + 2) based on estimations acquired at time tw +

• Start planning at time tw +

Continuous cycle• Finish planning at time tw + 2• Resulting path: p1 : T C• Start executing path p1 at time tw + 2• Read sensor input at time tw + 2• Reserve time for planning• Plan path with start configuration-time

(p1(tw + 3, tw + 3) based on estimations acquired at time tw + 2

• Start planning at time tw + 2

Continuous cycle• Finish planning at time tw + k• Resulting path: pk – 1 : T C• Start executing path pk – 1 at time tw + k• Read sensor input at time tw + k• Reserve time for planning• Plan path with start configuration-time

(pk – 1(tw + (k + 1), tw + (k + 1)) based on estimations acquired at time tw + k

• Start planning at time tw + 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 space Configuration-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(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

Date post:	15-Jan-2016
Category:	Documents
View:	216 times
Download:	0 times

Motion Planning in Dynamic Environments Jur van den Berg.

Documents