Optimization-Based Motion Planning for Autonomous Agricultural Vehicles Turning in Constrained Headlands