  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.
