Optimization-based motion planning for autonomous agricultural vehicles turning in constrained headlands

Headland maneuvering is a crucial part of the field operations performed by autonomous agricultural vehicles (AAVs). While motion planning for headland turning in open fields has been extensively studied and integrated into commercial autoguidance systems, the existing methods primarily address scenarios with ample headland space and thus may not work in more constrained headland geometries. Commercial orchards often contain narrow and irregularly shaped headlands, which may include static obstacles, rendering the task of planning a smooth and collision-free turning trajectory difficult. To address this challenge, we propose an optimization-based motion planning algorithm for headland turning under geometrical constraints imposed by headland geometry and obstacles. Our method models the headland and the AAV using convex polytopes as geometric primitives, and calculates optimal and collision-free turning trajectories in two stages. In the first stage, a coarse path is generated using either a classical pattern-based turning method or a directional graph-guided hybrid A* algorithm, depending on the complexity of the headland geometry. The second stage refines this coarse path by feeding it into a numerical optimizer, which considers the vehicle's kinematic, control, and collision-avoidance constraints to produce a feasible and smooth trajectory. We demonstrate the effectiveness of our algorithm by comparing it to the classical pattern-based method in various types of headlands. The results show that our optimization-based planner outperforms the classical planner in generating collision-free turning trajectories inside constrained headland spaces. Additionally, the trajectories generated by our planner respect the kinematic and control limits of the vehicle and, hence, are easier for a path-tracking controller to follow. In conclusion, our proposed approach successfully addresses complex motion planning problems in constrained headlands, making it a valuable contribution to the autonomous operation of AAVs, particularly in real-world orchard environments.