multi-robot motion planning #2

Multi-Robot Motion Planning #2

Jur van den Berg

Outline

• Recap: Composite Configuration Space• Prioritized Planning• Planning in Dynamic Environments• Application: Traffic Reconstruction• Reciprocal Velocity Obstacles

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

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)

Possible 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

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

Roadmap-based Approaches• Discretize Roadmap-

time space– Select time step t– Constrain velocity to be

{-vmax, 0, vmax}

• Find shortest path using A*

Roadmap-based Approaches

Prioritized Multi-Robot Planning• Instead of planning in Nd-dimensional

composite configuration space, plan N times in (d+1)-dimensional configuration-time space

• Finding a path is not guaranteed

12 Robots 24 Robots

Application: Traffic Reconstruction

• Sensors A and B along a highway• For each car: time, velocity and lane at

position A and B• What happened in between?

Approach• Create roadmap encoding car’s kinematic

constraints

• Plan trajectory between start and goal on roadmap encoding car’s dynamic constraints

• Plan in order of time at point A, and avoid previously planned cars

Video

• Link

References

• Erdmann, Lozano-Perez. On Multiple Moving Objects• Kant, Zucker. Toward Efficient Trajectory Planning: the

Path-Velocity Decomposition• Van den Berg, Overmars. Prioritized Motion Planning

for Multiple Robots• Hsu, Kindel, Latombe, Rock. Randomized Kinodynamic

Motion Planning with Moving Obstacles• Van den Berg, Overmars. Roadmap-Based Motion

Planning in Dynamic Environments• Van den Berg, Sewall, Lin, Manocha. Virtualized Traffic:

Reconstructing Traffic Flows from Discrete Spatio-Temporal Data