A two-stage optimal motion planner for autonomous agricultural vehicles

This paper presents a two-stage motion planning algorithm which can compute low-cost motions for autonomous agricultural vehicles, for a given cost function defined over the entire path (e.g., shortest path, maximum clearance, etc.). In the first stage, the algorithm utilizes randomized motion planning to explore the space of possible motions and computes a feasible sub-optimal trajectory. In the second stage, the optimization of the stage-1 motion is formulated within the optimal control framework and function-space gradient descent is used to minimize the cost of the entire motion. The numerical results suggest that the two-stage motion planner can compute optimal or quasi-optimal motions in free space very quickly. In the presence of obstacles however, the execution time increases significantly. Furthermore, kino-dynamic, or dynamic motion models seem to be necessary in order to produce smooth motion trajectories.