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

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

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)

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)

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

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

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

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

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

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

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

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

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