Situational Planning for the MIT DARPA Challenge Vehicle

1. Situational Planning for the MIT DARPA Challenge Vehicle Thomas Coffee Image Credit: David Moore et al.

2. Problem Statement Inputs • Vehicle path: position-space waypoint sequence ← Mission Planner • Vehicle state: state-space configuration ← Perceptual State Estimator • Obstruction environment: position-space regions, current velocities, and types (lanes, static obstacles, vehicles, unknowns) ← Perceptual State Estimator • Law constraints: lane corridors and speed limits ← RNDF • [ Sensor model … ← ??? ] Output (~10 Hz) • Status: reports success/failure of each constraint on vehicle trajectory plan • Vehicle trajectory: high-resolution state-space curve • Open-space-realizable by vehicle control system • Avoids input obstacles’ current-velocity-bounded subspace • Avoids input unknown regions’ zero-velocity subspace • Consistent with law constraints • Prioritizes inconsistent constraints by type: static > vehicle > unknown > lane > law • [ Attempts to achieve sensor coverage of unknown regions ] • Time estimate to first waypoint in sequence

3. Overview of Past Approaches • Exact solutions • Dynamic programming state-space grid search • Holonomic configuration-space planning with steering control for admissible path fitting • Adaptive exploration and searching with maximally spaced landmarks (Ariadne’s Clew) • Probabilistic roadmaps with learning by adaptive sampling query by graph search • Rapidly exploring random trees (bidirectional)

4. Exact Solutions Limited to special cases: • Canny J, Rege A, Reif J (1990) • Point masses only • < 2-D position space • Static bounds on velocity and acceleration • Souères P, Boissonnat JD (1998) • Specific to forward-backward simple car geometry and dynamics • Does not handle obstacles • Constructs distance-optimal solution (time-optimal only with decoupled steering and accelerator dynamics)

5. State-Space Grid Search Donald B, Xavier P, Canny J, Reif J (1993) • Advantages • Provably polynomial running time (first such algorithm) • Provably near-optimal (1 + ε), can trade run time vs. ε • Uses bang-control steps often corresponding to optimal paths • Disadvantages • Handles only simple magnitude bounds on state variables • Does not scale well to larger dof problems

6. Holonomic + Steering Control Variety of holonomic planning techniques with domain-specific steering control • Advantages • Holonomic/non-holonomic decision problems equivalent for small-time controllable systems • Fast path planning in lower-dimensional spaces • Disadvantages • Path planning separated from vehicle dynamics (Lozano-Perez configuration space): inefficient use of resources • Paths found may be topologically distant from dynamically optimal paths • Incomplete for non-small-time controllable systems • Steering control must be specialized for each application

7. Ariadne’s Clew Bessière P, Ahuactzin J-M, Talbi E-G, Mazer E (1993) • Advantages • Landmarks adaptively sample widely over the configuration space • Fast optimization step based on genetic algorithms • Signficantly faster still with massively parallel implementation • Disadvantages • Produces rather suboptimal path results regardless of c-space difficulty • Extremely messy implementation with many free parameters

8. Probabilistic Roadmaps Kavraki LE, Svestka P, Latombe J-C, Overmars MH (1996) • Advantages • Roadmaps adaptively sample widely over the configuration space • Fast roadmap forest expansion based on semi-complete planning • Learning and query phases resize appropriately to resource constraints • Disadvantages • Produces somewhat suboptimal path results regardless of c-space difficulty • Somewhat messy implementation with many free parameters • Requires additional smoothing to fully respect dynamic constraints

9. Rapidly Exploring Random Trees LaValle SM, Kuffner JJ (2001) • Advantages • Fast adaptive sampling of configuration space using Voronoi randomization bias • Scales well to higher-dimensional configuration spaces • Disadvantages • Produces somewhat suboptimal path results regardless of c-space difficulty • Bidirectional approach requires additional smoothing at tree intersection

10. Baseline Approach • Deterministic (single) tree exploration in unobstructed configuration spacetime • Tree maintenance/expansion based on D* • Heuristic using modified Reeds-Shepp metric • Node expansion using “hard” and “level” steer, accelerator/brake for optimality • Reverse gear dynamics included by default • Moderate aggressiveness: model dynamic obstacles as bounded by current velocity

11. Justification for Baseline Approach • Simple essential configuration space (3-D), hence high-dof performance not required • Highly dynamic obstacle map, hence building up map information less valuable • D* approach provides a candidate path regardless of search completion • Computational resources expended only on strong candidate paths • Node expansion strategy can be tailored to obstacle and law constraints to produce near-optimal paths • Guaranteed approximate optimality, can be traded vs. node expansion parameters

12. Test Plan & Success Criteria Test Plan • Software testing with simulated splinter and vehicle dynamics • Hardware testing on splinter with added dynamic constraint layer • Hardware testing on DGC vehicle? Success Criteria • Consistent path generation meeting constraints for reasonable driving environments • Behavioral appropriateness of paths generated • Sufficient plan frequency to maintain intended course and avoid dynamic obstacles in non-emergency scenarios

13. Project Timeline • Nov 17: Initial software implementation • Nov 24: Simulation testing complete • Dec 01: Splinter testing complete • Dec 08: Initial DGC vehicle testing complete • Dec 13: Final code/documentation delivered • …?