Subgoal Planning Algorithm for Autonomous Vehicle Guidance

Trained humans exhibit highly agile spatial skills, enabling them to operate vehicles with complex dynamics in demanding tasks and conditions. Prior work shows that humans achieve this performance by using strategies such as satisficing, learning hierarchical task structure, and using a library of motion primitive elements. A key aspect of efficient and versatile solutions is the decomposition of a task into a sequence of smaller tasks, represented by subgoals. The present work uses properties of constrained optimal control theory to define conditions that specify candidate subgoal states and enable this decomposition. The proposed subgoal algorithm uses graph search to determine a subgoal sequence that links a series of unconstrained motion guidance elements into a constrained solution trajectory. In simulation experiments, the subgoal guidance algorithm generates paths with higher performance and less computation time than an RRT* benchmark. Examples illustrate the robustness and versatility of this approach in multiple environment types.