Situational Planning for the MIT DARPA Challenge Vehicle

Thomas Coffee

Image Credit: David Moore et al.

Problem StatementInputs• 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 SearchDonald 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 TreesLaValle 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 CriteriaTest 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

• …?