Hierarchical Trajectory Planning of an Autonomous Car Based on the Integration of a Sampling and an Optimization Method
principles of robot motion : theory , algorithms , and implementations [ book review ] .