Efficient and Safe Trajectory Planning for Autonomous Agricultural Vehicle Headland Turning in Cluttered Orchard Environments

Peng Wei, Chen Peng*, Wenwu Lu, Yuankai Zhu, Stavros Vougioukas, Zhenghao Fei, Zhikang Ge
Zhejiang University and University of California, Davis
IEEE ROBOTICS AND AUTOMATION LETTERS (RA-L)-2025
*Indicates corresponding author
Paper Code arXiv

Illustration of an electric tractor equipped with a KMS sprayer executing a turning trajectory in a headland.

Abstract

Autonomous agricultural vehicles (AAVs), including field robots and autonomous tractors, are becoming essential in modern farming by improving efficiency and reducing labor costs. A critical task in AAV operations is headland turning between crop rows. This task is challenging in orchards with limited headland space, irregular boundaries, operational constraints, and static obstacles. While traditional trajectory planning methods work well in arable farming, they often fail in cluttered orchard environments. This letter presents a novel trajectory planner that enhances the safety and efficiency of AAV headland maneuvers, leveraging advancements in autonomous driving. Our approach includes an efficient front-end algorithm and a highperformance back-end optimization. Applied to vehicles with various implements, it outperforms state-of-the-art methods in both standard and challenging orchard fields. This work bridges agricultural and autonomous driving technologies, facilitating a broader adoption of AAVs in complex orchards.

Covering-circle collision detection

Process of determining optimal covering circles for vehicle and implement. The black areas represent trees, and the gray areas represent map inflation.

Multiple safe corridors generation

Illustration of (a) geometric representations of obstacles and the AAV; (b) results from the front-end search using covering circles; (c) the parameterized initial trajectory with multiple segments, piece points, and constraint points; and (d) the optimized trajectory following the back-end refinement.

Results in Simulations

Results on a Physical Robot

Results on a tricycle robot carrying a spray boom: (a) without obstacles, and (b) with obstacles in the headland space. The geometric representations of the environment and AAV are shown, with planned trajectories as black dashed lines and actual trajectories color-coded by velocity.

Video Presentation

BibTeX

@ARTICLE{10854653,
  author={Wei, Peng and Peng, Chen and Lu, Wenwu and Zhu, Yuankai and Vougioukas, Stavros and Fei, Zhenghao and Ge, Zhikang},
  journal={IEEE Robotics and Automation Letters}, 
  title={Efficient and Safe Trajectory Planning for Autonomous Agricultural Vehicle Headland Turning in Cluttered Orchard Environments}, 
  year={2025},
  volume={10},
  number={3},
  pages={2574-2581},
  keywords={Trajectory;Turning;Trajectory planning;Collision avoidance;Planning;Optimization;Geometry;Crops;Autonomous vehicles;Vehicle dynamics;Agricultural automation;motion and path planning;collision avoidance;optimization and optimal control;agricultural autonomous vehicle},
  doi={10.1109/LRA.2025.3534056}
}