Field-validated VIO-MPC fusion for autonomous headland turning in GNSS-denied orchards

Autonomous headland turning in GNSS-denied orchard environments remains a critical challenge for agricultural robots, as traditional GNSS-based systems suffer from signal degradation under dense foliage, while LiDAR-based approaches struggle with sparse data and dynamic terrain. Existing controllers often exhibit oscillations during sharp turns, compromising efficiency and risking crop damage. To address these limitations, this study proposes a novel vision-inertial navigation framework integrating VIO and MPC for robust headland turning. The key innovation lies in the synergistic combination of low-cost VIO for reliable localization under canopy occlusion and MPC for constraint-aware trajectory tracking, enabling precise navigation without GNSS dependency. The VIO module utilizes a ZED 2i camera to generate 3D environmental landmarks, which are enhanced by cubic spline regression for noise reduction and reference path planning. The MPC controller optimizes trajectory tracking with predictive constraints, minimizing lateral deviations and oscillations. Simulations demonstrated rapid convergence under 0.5 m initial pose errors, outperforming Pure Pursuit and Stanley methods in heading stability. Field tests in banana orchards achieved 0.014 m average radial error and 1.8° heading error at maximum constraint of 0.6 m/s, with VIO localization latency of 60 ms per image. The methodology provides a foundational solution for scalable precision farming applications, including spraying and harvesting robots, with future extensions targeting multi-sensor fusion and adaptive path planning for heterogeneous orchard architectures.