Planning for on-road driving through incrementally created graphs

A general-purpose hierarchical planning framework that allows for cost-optimal, logic-constrained plans is presented. This framework is applied to planning an on-road path for an autonomous vehicle traveling amongst moving and stationary objects. Through this application, the ability to implement both hard and soft constraints, as well as cope with dynamic environments and user objectives are demonstrated.