Path-Tracking Hybrid A* and Hierarchical MPC Framework for Autonomous Agricultural Vehicles

TL;DR

This work tackles cross-furrow path tracking for autonomous agricultural vehicles under nonholonomic dynamics and full-body collision constraints, by minimizing deviation from an a priori reference path. It introduces a Path-Tracking Hybrid A* planner with deviation-aware cost and heuristic functions, plus pruning and an online replanning extension for real-time obstacle avoidance, and couples it with a hierarchical MPC that uses linearized warm-starts to efficiently enforce dynamics and collision constraints. Simulations on real farm data show superior path adherence, safety, computation speed, and obstacle-avoidance capability compared with baselines, demonstrating practical viability for offline smoothing and online planning. The framework promises impact for crop protection and field efficiency and opens avenues for extending to full-field coverage and multi-vehicle coordination.

Abstract

We propose a Path-Tracking Hybrid A* planner coupled with a hierarchical Model Predictive Control (MPC) framework for path smoothing in agricultural vehicles. The goal is to minimize deviation from reference paths during cross-furrow operations, thereby optimizing operational efficiency, preventing crop and soil damage, while also enforcing curvature constraints and ensuring full-body collision avoidance. Our contributions are threefold: (1) We develop the Path-Tracking Hybrid A* algorithm to generate smooth trajectories that closely adhere to the reference trajectory, respect strict curvature constraints, and satisfy full-body collision avoidance. The adherence is achieved by designing novel cost and heuristic functions to minimize tracking errors under nonholonomic constraints. (2) We introduce an online replanning strategy as an extension that enables real-time avoidance of unforeseen obstacles, while leveraging pruning techniques to enhance computational efficiency. (3) We design a hierarchical MPC framework that ensures tight path adherence and real-time satisfaction of vehicle constraints, including nonholonomic dynamics and full-body collision avoidance. By using linearized MPC to warm-start the nonlinear solver, the framework improves the convergence of nonlinear optimization with minimal loss in accuracy. Simulations on real-world farm datasets demonstrate superior performance compared to baseline methods in safety, path adherence, computation speed, and real-time obstacle avoidance.

Figures (7)

• Figure 1: Illustration for the cost (a) and heuristic (b) for path smoothing. (a): the shaded area illustrates the deviation cost calculated by \ref{['eq:ecost']}, and the $e_{\text{cost}}$ of Path 1 is larger than that of Path 2. (b): Two nodes are extended from their parent node, and each is connected to the goal point with a Reed-and-Shepp path. Note that due to the divergent orientation of Path 2, the $e_{\text{pred}}$ of Path 2 is significantly larger than that of Path 1. Therefore, the heuristic \ref{['eq:epred']} is useful in burning the wrong heading nodes.
• Figure 2: Illustration of how to determine the half-plane separators in the predicted states. (a) If the boundary obstacle is concave, both lines of boundary segments are used as half-plane separators. (b) If the boundary obstacle or the random obstacle is convex, the half-plane separator is determined according to SDF. The feasible region is illustrated in yellow.
• Figure 3: The five scenarios of farmland and reference path illustrations. Note that we only present 88 reference paths within a total of 713 paths.
• Figure 4: Comparison of three different methods on two typical scenarios. Note that each row represents experiments of the same method in different scenarios.
• Figure 5: Average deviation degree between path-tracking Hybrid A* and the B-spline (depicted in the central box chart), and the proportion of B-spline exceeding curvature limits (shown in the lower purple bar chart).