A Vision Navigation Method for Agricultural Machines Based on a Combination of an Improved MPC Algorithm and SMC

Abstract

Vision navigation systems provide significant advantages in agricultural scenarios such as pesticide spraying, weeding, and harvesting by interpreting crop row structures in real-time to establish guidance lines. However, the delay introduced by image processing causes the path and pose information relied upon by the controller to lag behind the actual vehicle state. In this study, a hierarchical delay-compensated cooperative control framework (HDC-CC) was designed to synergize Model Predictive Control (MPC) and Sliding Mode Control (SMC), combining predictive optimization with robust stability enforcement for agricultural navigation. An upper-layer MPC module incorporated a novel delay state observer that compensated for visual latency by forward-predicting vehicle states using a 3-DoF dynamics model, generating optimized front-wheel steering angles under actuator constraints. Concurrently, a lower-layer SMC module ensured dynamic stability by computing additional yaw moments via adaptive sliding surfaces, with torque distribution optimized through quadratic programming. Under varying adhesion conditions tests demonstrated error reductions of 74.72% on high-adhesion road and 56.19% on low-adhesion surfaces. In Gazebo simulations of unstructured farmland environments, the proposed framework achieved an average path tracking error of only 0.091 m. The approach effectively overcame vision-controller mismatches through predictive compensation and hierarchical coordination, providing a robust solution for vision autonomous agricultural machinery navigation in various row-crop operations.

1. Introduction

In agricultural pest control operations, unmanned agricultural machinery has emerged as a pivotal tool for enhancing production efficiency and reducing labor costs, primarily due to its advanced autonomous navigation technology. It is extensively utilized in various agricultural tasks, including weeding, pesticide application, crop monitoring, and harvesting [1,2,3]. Currently, mainstream navigation systems predominantly employ a fusion positioning approach that integrates the Global Navigation Satellite System (GNSS) with an Inertial Measurement Unit (IMU) [4]. However, the intricate nature of field environments often leads to weak GNSS signals, while the cumulative error associated with the IMU can substantially increase during prolonged operations. When these two systems fail to coordinate effectively, it may result in trajectory deviations or even operational disruptions [5].

Vision-based navigation technology exhibits superior environmental adaptability through real-time analysis of crop row line features, rendering it particularly suitable for operational scenarios characterized by severe signal obstruction [6,7]. However, the temporal matching issue between dynamic perception and motion control in visual systems can significantly impact path tracking accuracy, as it is susceptible to mechanical motion status and image processing delays. Unlike autonomous passenger vehicles that travel on regular roads, agricultural operations need extremely high precision when conducted between crop rows to avoid damaging precious crops during close-range operations. Agricultural vehicles operate at a lower speed and with significant variations, which means that a perception delay of just a few hundred milliseconds could lead to significant and corrective spatial deviations. Therefore, compensating for the visual delay is a necessary condition for achieving stable and undamaged autonomous agricultural operations. To improve the autonomous operational accuracy of agricultural machinery and achieve dynamic coordination between visual navigation and motion control, it is crucial to develop stable and reliable path tracking algorithms and control strategies.

The core of path tracking lies in the application of control technology, which encompasses a variety of strategies including proportional-integral-derivative (PID) control, adaptive control, SMC, linear quadratic regulator (LQR), MPC and machine learning methods. Among these, the strong robustness of SMC and the optimization capabilities of MPC render their combined architecture promising for addressing uncertainties in complex systems. Zhang et al. designed an adaptive model predictive controller for tracking unmanned aerial vehicles, demonstrating the robust stability and control accuracy of the associated control algorithm [8]. Dong et al. combined sliding mode control with MPC to enhance vehicle trajectory tracking performance on slippery roads [9].Qi et al. proposed a sliding mode model predictive controller integrated with thrust allocation (TA) and utilized sliding mode control to ensure robust stability [10]. Teng et al. introduced an improved MPC motion stability controller that combines sliding mode variable structure active steering to enhance trajectory tracking stability of vehicles in extreme scenarios [11]. These studies underscore the potential of MPC-SMC fusion architectures in improving robustness and tracking performance. However, in agricultural machinery visual navigation scenarios, existing fusion schemes still fall short in synergistic optimization of significant perception delays and control responses.

Meanwhile, a large number of studies have focused on tracking algorithms using MPC and its nonlinear MPC [12,13,14]. Mehraban et al. incorporated fuzzy logic into the MPC framework to adaptively generate controller weighting factors [15]. Yin et al. combined extended Kalman filtering with robust tube-based model predictive control to enhance tracking performance [16]. Considering the unique characteristics of agricultural machinery, Soitinaho et al. designed a path tracking and obstacle avoidance method based on nonlinear model predictive control specifically for front-wheel-drive tractors. The speed and steering actuators were simulated as first-order systems without time delay, achieving precise path tracking and obstacle avoidance by optimizing the cost function [17]. However, the increased algorithmic complexity poses real-time bottlenecks for agricultural machinery embedded platforms, leading to two distinct optimization approaches: Zhou et al. proposed an adaptive learning model predictive control strategy that utilizes Bayesian optimization to dynamically adjust control rules and reduce computational burden [18]; Wang et al. employed a particle swarm optimization algorithm to achieve adaptive optimization of the prediction range [19]. Both approaches break through computational efficiency constraints from data-driven and time-domain optimization perspectives, respectively. To further enhance the robustness of path tracking algorithms in various environments, Zhao et al. integrated an adaptive Extended Kalman Filter (EKF) with model mismatch estimation for state compensation, achieving unbiased path following [20]. Zhu et al. proposed a fault-tolerant MPC method based on a time-varying intermediate estimator to mitigate the effects of actuator faults [21]. In the field of collaborative optimization of visual navigation and path tracking, Chris et al. proposed a learning-based nonlinear model predictive control approach, providing insights for the collaborative optimization of visual perception and control models [22].While their work provides valuable insights, it does not explicitly tackle the fundamental challenge of synchronizing visual perception latency with the motion response in dynamic operational scenarios.

In summary, although existing vehicle path tracking research has made significant strides in enhancing the accuracy of kinematic models and robust control mechanisms, there remains a notable lack of focus on the development of pure vision-based navigation path tracking algorithms that incorporate robust control mechanisms. This is particularly true in complex farmland environments characterized by considerable perception delays [23].

To address this gap, we propose a predictive control architecture termed HDC-CC. The overall structure framework is illustrated in Figure 1, which integrates visual delay compensation and motion control co-optimization. The core contributions of this work are as follows: (1) To tackle the significant delay issues encountered in agricultural machinery vision systems within farmland environments, we introduce a delay compensation algorithm that based on historical compensation errors. By synergizing a delay state observer with the dynamic characteristics of agricultural machinery, we can real-time predict path drift and integrate a real-time compensation correction module at the forefront of the MPC controller. This approach effectively mitigates path tracking deviations caused by delays in image acquisition and processing; (2) We construct a hierarchical coupling framework that interlinks MPC and SMC, fostering a profound synergy between path tracking and dynamic stability through a bidirectional constraint transmission mechanism. The MPC layer generates precise front wheel steering angle commands based on forward-looking optimization, while the SMC layer continuously monitors vehicle stability boundaries in real time and utilizes the sliding surface to swiftly respond and generate active yaw moments, thereby enhancing the system’s robust adaptability to unstructured farmland environments.

2. Vehicle Model

The typical operating trajectory of an agricultural tractor in the field necessitates traveling in a straight line within a single working line, executing a turn at the end of the row, and subsequently entering the next working line. Throughout this operational process, high-performance straight-line trajectory tracking is critical to the effectiveness of tillage and constitutes the majority of the operation. Concurrently, high-precision curve trajectory tracking, particularly at the end of the row, is of paramount importance, as it not only ensures that the tractor smoothly and accurately transitions into the next working line but also significantly mitigates the risk of repeated trampling in the field caused by improper operation.

To address the demand for high-precision, robust operational trajectory tracking in complex and variable farmland environments, an accurate vehicle model is essential as the foundation for the path tracking control system [24,25]. Vehicle models are primarily categorized into kinematic models and dynamic models: kinematic models describe vehicle pose changes based on geometric relationships, providing the advantage of simplified calculations; dynamic models incorporate parameters such as mass, moment of inertia, and tire mechanics to more accurately characterize a vehicle’s dynamic response characteristics on unstructured terrain, particularly demonstrating superior predictive capability for complex conditions such as side slip and load transfer. For agricultural machinery operating at moderate speeds with significant lateral dynamics influences, full vehicle models with higher degrees of freedom offer superior fidelity but introduce prohibitive computational complexity for real-time model predictive control applications. Conversely, purely kinematic models lack the necessary dynamic coupling representation for accurate path tracking under varying soil conditions. The 2-DoF and 3-DoF dynamic models thus represent an optimal compromise, capturing essential lateral-yaw-roll couplings while maintaining computational tractability for embedded implementation. Given the pronounced disturbance characteristics of uneven road surfaces and time-varying soil parameters in agricultural operational environments, the development of a dynamic vehicle model is a prerequisite for achieving reliable path tracking. Among these, two-degree-of-freedom and three-degree-of-freedom vehicle models are commonly selected due to their favorable balance between model accuracy and computational complexity.

2.1. 2-DoF Vehicle Dynamics Model

The two-degree-of-freedom model of the vehicle is predicated on several idealized assumptions:

• Constant Longitudinal Motion: The vehicle maintains a constant speed, resulting in zero translational acceleration along the x-axis (longitudinal direction);
• Planar Ground Motion: Disregarding the effects of the suspension system, the vehicle body is constrained to perform only planar motion parallel to the ground, meaning that vertical motion, pitch motion, and roll motion are all restricted to zero;
• Simplified Steering Input: The dynamics of the steering system are not considered; instead, the steering angle of the front wheels is directly utilized as the model input.

Based on these assumptions, vehicle motion is simplified to a single-track model that incorporates only lateral translational motion (along the y-axis) and yaw motion (rotation about the z-axis). While these simplifications ignore suspension dynamics and terrain-induced roll/pitch effects, which may reduce model fidelity on highly uneven terrain, they provide a sufficient dynamics representation for designing the upper-layer path tracking controller during typical steady-state field operations. Furthermore, the model presumes that the lateral stiffness of the left and right wheels on the same axle is identical, allowing a single wheel to effectively represent the entire axle. The motion and force diagram of the single-track model is illustrated in Figure 2.

Figure 2a illustrates the change in the vehicle’s state from time t to $t+\Delta t$. From the geometric relationship, the rate of change of velocity along the x and y axes of the ground coordinate system is described by Equation (1), where $\omega$ represents the yaw angular velocity, $v_x$ and $v_y$ represent the longitudinal and lateral velocities respectively.

$$\left\{\begin{array}{c}{a}_x={\displaystyle \frac{\Delta v_x-v_y\Delta \theta }{\Delta t}}={\dot{v}}_x-v_y\omega \hfill \\ a_y={\displaystyle \frac{\Delta v_y+v_x\Delta \theta }{\Delta t}}={\dot{v}}_y+v_x\omega \hfill \end{array}\right.$$

(1)

Figure 2b depicts a simplified vehicle subjected solely to lateral forces acting on the front and rear axles. When the lateral acceleration of the vehicle is limited to 0.4 g or below, the lateral characteristics of the tires remain within the linear range, indicating that the lateral force $F_{\alpha }$ and lateral stiffness $C_{\alpha }$ are equivalent [26]. Based on Newton’s second law and Euler’s law of rotation, let the vehicle mass be denoted as m, $I_z$ as the moment of inertia of the vehicle about the z-axis, a as the distance from the front axle to the center of mass, and b as the distance from the rear axle to the center of mass. The differential equations describing the dynamics of this two-degree-of-freedom system are established as shown in Equation (2).

$$\left\{\begin{array}{c}2C_{\alpha f}{\alpha }_{f}cos\delta +2C_{\alpha r}{\alpha }_r=m{a}_y\hfill \\ 2C_{\alpha f}{\alpha }_{f}cos\delta ·a-2C_{\alpha r}{\alpha }_r·b=I_z\dot{\omega }\hfill \end{array}\right.$$

(2)

When $\theta$ is small, the calculations for the heading angle $\epsilon$ and the center of mass lateral deviation angle $\beta$ are presented in Equations (3) and (4).

$$\epsilon =tan\epsilon ={\displaystyle \frac{v_y+\omega a}{v_x}}$$

(3)

$$\beta =tan\beta ={\displaystyle \frac{v_y}{v_x}}$$

(4)

Thus, let $v_y=v$ and $v_x=u$ (constant), and calculate the lateral deviation angles of the front and rear wheels using Equations (5) and (6).

$$\begin{array}{cc}\hfill {\alpha }_f& =-(\delta -\epsilon )=-\left(\delta -{\displaystyle \frac{v+\omega a}{u}}\right)=-\left(\delta -{\displaystyle \frac{u\beta +\omega a}{u}}\right)=\beta +{\displaystyle \frac{a\omega }{u}}-\delta \hfill \end{array}$$

(5)

$$\begin{array}{cc}\hfill {\alpha }_r& ={\displaystyle \frac{v-\omega b}{u}}={\displaystyle \frac{u\beta -\omega b}{u}}=\beta -{\displaystyle \frac{\omega b}{u}}\hfill \end{array}$$

(6)

During agricultural straight-line tracking and gentle turning maneuvers, the vehicle exhibits sufficiently small yaw rates and sideslip angles, which confine the lateral dynamics to the linear region of tire forces. Under these conditions of small angular velocities, $cos\delta$ = 1. By substituting Equations (5) and (6) into Equation (2), let $k_1=2C_{\alpha f}$ and $k_2=2C_{\alpha r}$, further linearizing the model as shown in Equation (7).

$$\left\{\begin{array}{c}{k}_1\left(\beta +{\displaystyle \frac{a\omega }{u}}-\delta \right)+k_2\left(\beta -{\displaystyle \frac{\omega b}{u}}\right)=m(\dot{v}+u\omega )\hfill \\ k_1\left(\beta +{\displaystyle \frac{a\omega }{u}}-\delta \right)a-k_2\left(\beta -{\displaystyle \frac{\omega b}{u}}\right)b=I_z\dot{\omega }\hfill \end{array}\right.$$

(7)

The simplification yields:

$$\begin{array}{cc}\hfill \dot{\beta }& ={\displaystyle \frac{k_1\left(\beta +\frac{a\omega }{u}-\delta \right)+k_2\left(\beta -\frac{\omega b}{u}\right)-mu\omega }{mu}}=\omega \left({\displaystyle \frac{a{k}_1-b{k}_2}{m{u}^2}}-1\right)+\beta {\displaystyle \frac{k_1+k_2}{mu}}-\delta {\displaystyle \frac{k_1}{mu}}\hfill \end{array}$$

(8)

$$\begin{array}{cc}\hfill \dot{\omega }& ={\displaystyle \frac{k_1\left(\beta +\frac{a\omega }{u}-\delta \right)a-k_2\left(\beta -\frac{\omega b}{u}\right)b}{I_z}}=\omega {\displaystyle \frac{a^2{k}_1+b^2{k}_2}{u{I}_z}}+{\displaystyle \frac{a{k}_1-b{k}_2}{I_z}}\beta -{\displaystyle \frac{a{k}_1}{I_z}}\delta \hfill \end{array}$$

(9)

2.2. 3-DoF Vehicle Dynamics Model

The three-degree-of-freedom model and the two-degree-of-freedom model differ in their capacity to comprehensively describe the vehicle’s motion state. The three-degree-of-freedom model, in addition to accounting for the vehicle’s lateral displacement and heading angle, incorporates an additional roll degree of freedom that characterizes the vehicle’s rotational motion around the vertical axis. This inclusion allows for a more accurate representation of the lateral dynamic coupling effects observed during the steering maneuvers of agricultural machinery. The model is constructed based on the previously stated assumptions 2 and 3. Under the specified force conditions, it is simplified into a three-degree-of-freedom model encompassing longitudinal, lateral, and yaw motions. The vehicle dynamics equations are presented in Equation (10):

$$\left\{\begin{array}{c}m({\dot{v}}_x-v_y\omega )=2F_{xf}cos\delta -2F_{yf}sin\delta +2F_{xr}\hfill \\ m({\dot{v}}_y+v_x\omega )=2F_{xf}sin\delta +2F_{yf}cos\delta +2F_{yr}\hfill \\ I_z\dot{\omega }=a(2F_{xf}sin\delta +2F_{yf}cos\delta )-2b{F}_{yr}+\Delta M\hfill \end{array}\right.$$

(10)

where $F_{xf}$ and $F_{xr}$ denote the longitudinal forces acting on the front and rear tires of the vehicle, respectively, while $F_{yf}$ and $F_{yr}$ represent the lateral forces acting on the front and rear tires. By employing a linear tire model, the lateral force Equation (11) and longitudinal force Equation (12) for the tires can be derived as follows:

$$\left\{\begin{array}{c}{F}_{xf}=C_{lf}{S}_f\hfill \\ F_{xr}=C_{lr}{S}_r\hfill \end{array}\right.$$

(11)

$$\left\{\begin{array}{c}{F}_{yf}=C_{cf}{\alpha }_f=C_{cf}\left({\displaystyle \frac{v_y+a\omega }{v_x}}-\delta \right)\hfill \\ F_{yr}=C_{cr}{\alpha }_r=C_{cr}\left({\displaystyle \frac{v_y-b\omega }{v_x}}\right)\hfill \end{array}\right.$$

(12)

where $C_{lf}$ and $C_{lr}$ signify the longitudinal stiffnesses; $C_{cf}$ and $C_{cr}$ indicate the lateral stiffnesses; $s_f$ and $s_r$ denote the slip rates; and ${\alpha }_f$ and ${\alpha }_r$ represent the lateral angles. Substituting Equations (11) and (12) into (10) and rearranging the terms yields the following results:

$$\begin{array}{cc}\hfill {\dot{v}}_x& =v_y\omega +{\displaystyle \frac{2}{m}}\left[C_{lf}{S}_f+C_{cf}\left({\displaystyle \frac{v_y+a\omega }{v_x}}-\delta \right)\delta +C_{lr}{S}_r\right]\hfill \end{array}$$

(13)

$$\begin{array}{cc}\hfill {\dot{v}}_y& =-v_x\omega +{\displaystyle \frac{2}{m}}\left[C_{cf}\left({\displaystyle \frac{v_y+a\omega }{v_x}}-\delta \right)+C_{cr}{\displaystyle \frac{v_y-b\omega }{v_x}}\right]\hfill \end{array}$$

(14)

$$\begin{array}{cc}\hfill \dot{\omega }& ={\displaystyle \frac{2}{I_z}}\left[a{C}_{cf}\left({\displaystyle \frac{v_y+a\omega }{v_x}}-\delta \right)-b{C}_{cr}{\displaystyle \frac{v_y-b\omega }{v_x}}\right]+{\displaystyle \frac{\Delta M}{I_z}}\hfill \end{array}$$

(15)

Using Equations (8), (9), and (13)–(15) to construct the state-space representation, both 2-DOF and 3-DOF single-track models were implemented in MATLAB (version R2021b) and Simulink (version R2021b), with their simulation results compared. With initial values of $v_y$ and $\omega$ set to 0 and $v_x$ initialized at 5 m/s, simulations were conducted for the sinusoidal angle input scenario. The front wheel steering input was defined as $\delta ={\displaystyle \frac{10\pi }{180}}sin(0.8\pi ·\mathrm{time})$, reflecting the typical frequency of human steering wheel rotation, approximately 0.3 to 0.5 Hz. As depicted in Figure 3, the comparison curves illustrate that both models exhibit a high degree of consistency in the trends of lateral velocity and yaw angular velocity, with their waveform characteristics and phase relationships generally aligning.This observation suggests that both models are grounded in the same theoretical framework regarding fundamental dynamic characteristics. Analysis of the tire slip angle response confirms that the 3-DOF model accurately characterizes centrifugal load transfer dynamics. Notably, the front wheel slip angle increases from $3.12°$ to $6.12°$, while the slip angle rises from $1.82°$ to $3.88°$ during center-of-gravity shifts, accurately quantifying tire load transfer. This characteristic is critical for predicting unique tire wear patterns and adhesion boundary changes in agricultural field operations. During a $30°$ sharp turn at 5 m/s, the 3-DOF model predicts a maximum roll angle of $4.86°$. By including roll dynamics, the model captures roll-tire coupling phenomena through integrated tire-ground interaction mechanics. Consequently, the 3-DOF model demonstrates high fidelity for typical agricultural scenarios involving low-speed, high-steer maneuvers.

3. MPC Path Tracking Controller Design

In path tracking control for visual autonomous driving systems, the visual perception module plays a crucial role in environmental perception and the extraction of crop row center lines. However, field visual navigation line extraction algorithms that rely on deep learning (as referenced in [27,28,29,30]) require between 100 to 500 ms for image processing, which results in vehicle posture information being significantly delayed relative to the actual state. To mitigate the reduction in control performance caused by the inherent time delay in visual perception, this paper proposes an MPC architecture that incorporates a visual delay compensation strategy. The fundamental concept of this architecture is to leverage the vehicle dynamics model and real-time motion state to construct a state prediction compensator. Specifically, the system first acquires the visual pose observation values, which are subject to inherent delays, while simultaneously recording the corresponding control input sequence within a historical time window. Subsequently, utilizing the vehicle dynamics model, forward integration calculations are performed to dynamically advance the delayed visual observations and adjust them in real-time to the present moment, thereby yielding an accurate estimate of the current system state. The compensated state estimate is employed directly as the initial state of the MPC prediction model, allowing the controller to optimize the future control sequence in each optimization cycle based on real-time state information.

3.1. Delay State Observer Design

The visual system output at time t, $X_{vision\left(t\right)}$, corresponds to the vehicle state at time $t-\tau$. To address this latency, a time-delay state observer is designed that estimates the current state by integrating historical control inputs $U_{history}$ over $[t-\tau ,t]$ with the delayed vision output, as formulated in Equation (16):

$$\widehat{X}\left(t\right)=\Psi (X_{vision}\left(t\right),U_{history})$$

(16)

where $\widehat{X}\left(t\right)$ denotes the estimated state at time t, and $\Psi (·)$ represents the forward integration operator based on the vehicle dynamics model. Within $t-\tau$, control inputs are known while state evolution depends on instantaneous states. The visual delay time $\tau$ is primarily determined by the image processing pipeline, including image acquisition, feature extraction, and navigation line computation. In practical implementations, $\tau$ can be empirically measured as the average processing latency of the vision system. For the purpose of delay compensation, $\tau$ is treated as a known system parameter. The proposed delay state observer does not require real-time estimation of $\tau$, but rather focuses on compensating for its effects through model-based forward prediction. For delay compensation, integration initiates from the vision-measured state at $[t-\tau ,t]$, propagating forward to current time t via numerical integration. The continuous-time forward integration is mathematically expressed in Equation (17):

$$\widehat{X}\left(t\right)=X(t-\tau )+{\int }_{t-\tau }^{t}f(X\left(\xi \right),U\left(\xi \right))d\xi$$

(17)

Here, $f(·)$ denotes the vehicle dynamics model.The forward integration is implemented numerically using the explicit Euler method due to its computational efficiency and suitability for real-time control applications. Combining Equations (10)–(15), the simplified vehicle dynamics model in state-space form is expressed as Equation (18):

$$\left\{\begin{array}{c}\dot{\chi }=f(\chi ,u)\hfill \\ y=C\chi \hfill \end{array}\right.$$

(18)

where the state vector $\chi ={[v_x,v_y,\omega ,\phi ,X,Y]}^T$, control input $u=\delta$, and output vector $y={[\phi ,Y]}^T$, $\mathit{c}=\left[\begin{array}{cccccc}0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$

Given a sampling time $T_s$, the delay steps are calculated as $k_{\tau }=\lfloor \tau /T_s\rfloor$. For implementation in discrete-time systems, numerical integration of Equation (17) proceeds as follows:

$${\widehat{X}}_k=X_{k-k_{\tau }}+\sum _{i=0}^{k_{\tau }-1}f(X_{k-k_{\tau }+i},U_{k-k_{\tau }+i})T_s$$

(19)

The delayed state $X_{k-k_{\tau }}$ obtained from the vision system at time step $k-k_{\tau }$, serves as the integration initial condition. Forward integration is then performed from $t_{k-k_{\tau }}$ to $t_k$ using the dynamics model $f(·)$ and recorded control inputs $U_{k-k_{\tau }+i}$. After executing $k_{\tau }$ integration steps, the current state estimate ${\widehat{X}}_k$ at time $t_k$ is obtained.

3.2. Prediction Model Construction

To perform prediction and control based on the compensated state, the compensated estimate ${\widehat{X}}_k$ is treated as the current state $X_k$. An extended state vector is constructed by augmenting this state with the previous control input, as shown in Equation (20):

$${\xi }_k=\left[\begin{array}{c}{X}_k\\ u_{k-1}\end{array}\right]$$

(20)

where $X_k\in R^n$ and $u_{k-1}\in R^m$. Utilizing a 3-DOF agricultural vehicle dynamics model, a prediction model that accounts for the visual delay period is established through linearization and discretization, which describes the evolution law of the extended state under the control increment $\Delta u\left(k\right)=u\left(k\right)-u(k-1)$. The prediction model is presented in Equations (21) and (22):

$${\xi }_{k+1}={\overline{A}}_k{\xi }_k+{\overline{B}}_k\Delta u_k+{\overline{d}}_k$$

(21)

$$\eta \left(k\right)=C\xi \left(k\right)={\left[\begin{array}{cc}\phi \left(k\right)& Y\left(k\right)\end{array}\right]}^T$$

(22)

where ${\overline{A}}_k=\left[\begin{array}{cc}{A}_k& B_k\\ 0& I\end{array}\right]$, ${\overline{B}}_k=\left[\begin{array}{c}{B}_k\\ I\end{array}\right]$, $A_k\in R^{n\times n}$ and $B_k\in R^{n\times m}$ are state and control matrices from the linearized-discrete dynamics at ${\xi }_k$. I is the identity matrix, and ${\overline{d}}_k\in R^{n\times m}$ represents the discretization error and linearization truncation error.

The MPC controller predicts the future output of the system by iterating the system Equations (20)–(22). Over prediction horizon $N_p$ and the control horizon $N_c$, the vectorized output prediction equation is provided by Equation (23):

$$Y\left(k\right)=\underset{\mathrm{Initial}\mathrm{State}\mathrm{Influence}}{\underbrace{\Psi \left(k\right)\xi \left(k\right)}}+\underset{\mathrm{Control}\mathrm{Action}\mathrm{Effects}}{\underbrace{\Theta \left(k\right)\Delta U\left(k\right)}}+\underset{\mathrm{Model}\mathrm{Error}\mathrm{Effects}}{\underbrace{\Gamma \left(k\right)\Omega \left(k\right)}}$$

(23)

where,

$$\Psi \left(k\right)=\left[\begin{array}{c}C{\overline{A}}_k\\ C{\overline{A}}_k^2\\ ⋮\\ C{\overline{A}}_k^{N_p}\end{array}\right],\Theta \left(k\right)=\left[\begin{array}{ccc}C{\overline{B}}_k& 0& \dots \\ C{\overline{A}}_k{\overline{B}}_k& C{\overline{B}}_k& \dots \\ ⋮& ⋮& \ddots \\ C{\overline{A}}_k^{N_p-1}{\overline{B}}_k& C{\overline{A}}_k^{N_p-2}{\overline{B}}_k& \dots \end{array}\right],\Gamma \left(k\right)=\left[\begin{array}{cccc}{\displaystyle \frac{1}{{\overline{A}}_k}}& 0& 0& 0\\ 1& 1& 0& 0\\ ⋮& ⋮& \ddots & ⋮\\ {\overline{A}}_k^{N_p-1}& {\overline{A}}_k^{N_p-2}& \dots & 1\end{array}\right].$$

$Y\left(k\right)$ is the output sequence, $\Delta U\left(k\right)$ is the control increment sequence, and $\Omega \left(k\right)$ is the error sequence.

3.3. Objective Function

Building upon the prediction model framework, this study presents a refined objective function design for model predictive control. The parameters for the MPC controller as listed in

Table 1. MPC Controller Parameters.

Parameter	Symbol	Value
Prediction Horizon	$N_p$	20
Control Horizon	$N_c$	10
State Weight Matrix	Q	$\mathrm{diag}(10,100)$
Control Weight Matrix	R	1
Compensation Error Weight Matrix	S	$\mathrm{diag}(50,10)$
Compensation History Steps	M	5
Forgetting Factor	$\lambda$	0.8
Sampling Time	$T_s$	0.02 s

.

The function is designed to minimize the error between the predicted state variables and the desired values in the time domain while constraining the controller output. Traditional MPC formulations exhibit significant limitations in vision-based navigation systems due to inherent visual processing latency, compromising real-time performance and robustness in dynamic environments. To address this, a modified objective function retains conventional tracking error and control penalty terms while incorporating a novel delay compensation error penalty, establishing an MPC path-tracking controller suitable for visual delay scenarios, as shown in Equation (24).

$$J\left(k\right)=\sum _{j=1}^{N_p}{∥{\eta }_{ref}(k+j)-\eta (k+j)∥}_Q^2+\sum _{j=0}^{N_c-1}{∥\Delta u(k+j)∥}_R^2+{∥e_{comp}∥}_s^2+\rho {\epsilon }_s^2$$

(24)

Here, ${\epsilon }_s$ represents a slack variable, and $\rho$ serves as its penalty weight for managing constraint conflict. The error compensation term accounts for the cumulative error generated during the forward integral compensation process. When optimizing at timestep k, the compensation error from the most recent M steps is employed to construct the penalty term. By weighting the historical compensation errors, the controller is compelled to consider past compensation accuracy when optimizing future control actions, thus suppressing the cumulative drift caused by visual delay. The historical compensation error penalty term is defined as illustrated in Equation (25).

$${∥e_{comp}∥}_s^2=\sum _{m=0}^{M-1}{\lambda }^m{e}_{\mathrm{comp}}{(k-m)}^{T}S{e}_{\mathrm{comp}}(k-m)$$

(25)

where, $e_{\mathrm{comp}}(k-m)$ denotes the historical compensation error from m steps prior, $\lambda$ represents the error weight, and S is a positive definite diagonal weight matrix prioritizing positional tracking accuracy. To facilitate the solution process, Equation (24) is reformulated into standard quadratic programming form based on the prediction Equations (21) and (22). The resulting quadratic programming problem in Equation (26) is solved using the quadprog solver from MATLAB’s Optimization Toolbox.

$$\underset{\Delta U\left(k\right),\epsilon }{min}J\left(k\right)={\displaystyle \frac{1}{2}}\Delta U{\left(k\right)}^T\mathbf{H}\Delta U\left(k\right)+{\mathbf{f}}^T\Delta U\left(k\right)+\rho {\epsilon }_s^2$$

(26)

s.t.

• $\begin{array}{ccc}& \Delta U_{min}\le \Delta U\left(k\right)\le \Delta U_{max}\hfill & \\ & U_{min}\le {\mathbf{C}}_{\mathbf{p}}u(k-1)+\mathbf{D}\Delta U\left(k\right)\le U_{max}\hfill & \\ & {\eta }_{min}-{\epsilon }_s\mathbf{E}\le \Psi \xi \left(k\right)+\Theta \Delta U\left(k\right)\le {\eta }_{max}+{\epsilon }_s\mathbf{E}\hfill & \\ & {\epsilon }_s\ge 0\hfill \end{array}$

During the optimization at a single time step k, the historical errors $e_{\mathrm{comp}}(k-m)$ for $m=0,\dots ,M-1$ are already known and fixed values. Therefore, the entire term $\parallel e_{\mathrm{comp}}{\parallel }_s^2$ is a scalar constant during the minimization of $J\left(k\right)$ with respect to the future control sequence $\Delta U\left(k\right)$. Since adding a constant to an objective function does not change the optimal solution $\Delta U*\left(k\right)$, this term is omitted when constructing the standard Quadratic Programming (QP) problem in Equation (26). However, this term plays a vital role across consecutive control steps: the compensation error $e_{\mathrm{comp}}\left(k\right)$ is updated at every time step based on the difference between the predicted state and the newly available (but delayed) vision measurement. This creates a feedback mechanism on the quality of the delay compensation, influencing the controller’s behavior over time by shifting the state prediction baseline $\widehat{X}\left(k\right)$.

The objective function incorporates the following constraints: steering angle $-0.52\mathrm{rad}\le \delta \le 0.52\mathrm{rad}$; steering angle change rate $-0.26\mathrm{rad}\le \Delta \delta \le 0.26\mathrm{rad}$. Additionally, path tracking boundary constraints are imposed to ensure that the agricultural machinery does not deviate excessively from the predefined path during operation, with constraints of lateral deviation $e_y\le 0.1\mathrm{m}$ and heading angle deviation $e_{\phi }\le 10°$. Given the earlier assumption that the tires operate within the linear region, the tire force calculation employs an approximate expression. To ensure that the lateral force of the tire is linearly related to the lateral deviation angle and that the tire force remains within the adhesion limit, restrictions are applied to the slip angles of the tire and center of mass: $-5°\le {\alpha }_r\le 5°$, $-2.5°\le {\alpha }_f\le 2.5°$, and $-2°\le \beta \le 2°$, the controller sampling time is $T_s=0.02$ s.

The controller implements the first element $U_0$ of the optimized control sequence U as the vehicle’s actuation command. This rolling horizon optimization process repeats each control cycle, enabling continuous trajectory tracking. The objective function design achieves precise path following while compensating for vision-induced cumulative errors, enhancing system robustness and control smoothness in dynamic environments.

4. Stability Controller Design

Within the MPC path tracking framework, which incorporates visual delay compensation, the vehicle acquires environmental information via the visual perception system and generates a reference path. The MPC controller computes the front wheel steering angle $\delta$, through a rolling optimization process. However, under typical agricultural conditions, such as navigating rough terrain or experiencing localized variations in adhesion, path tracking may lead to vehicle instability due to uneven ground or interference from crop residues. To address this limitation, a stability controller is implemented as a lower-layer actuator, forming a coordinated control architecture with the predictive controller. Following visual delay compensation, the corrected path information feeds into the MPC module. Its output steering command $\delta$ then directly inputs to the stability controller for execution.

4.1. Calculation of Expected Yaw Angular Velocity

The core input parameters for the stability controller include: Yaw rate reference value generated by MPC ${\omega }_{mpc}$; Longitudinal vehicle speed $v_x$ (m/s); Front wheel steering angle $\delta$; Road adhesion coefficient $\mu$. Based on vehicle dynamic stability conditions, the yaw rate safety boundary under current operating conditions is calculated through Equation (27), g is the acceleration due to gravity ($9.81{\mathrm{m}/\mathrm{s}}^2$).

$$|{\omega }_{max}|={\displaystyle \frac{\mu g}{max(v_x,v_{\min })}}$$

(27)

A safeguard mechanism sets a minimum speed threshold $v_{\min }=0.5\mathrm{m}/\mathrm{s}$ to prevent boundary overflow during low-speed operation. A dual-layer decision logic generates the final desired value per Equation (28), where $\mathrm{sgn}\left(\delta \right)$ retains the steering direction sign from MPC.

$${\omega }_d=\left\{\begin{array}{cc}{\omega }_{mpc}\hfill & \mathrm{if}|{\omega }_{mpc}|\le {\omega }_{max}\hfill \\ \mathrm{sgn}\left(\delta \right)·{\omega }_{max}\hfill & \mathrm{otherwise}\hfill \end{array}\right.$$

(28)

This architecture prioritizes vehicle dynamic stability. When $\omega$ approaches the safety boundary, stability intervention is triggered, prompting the MPC controller to temporarily relax path-tracking accuracy. This achieves a dynamic trade-off between safety and tracking precision.

4.2. Additional Yaw Moment Calculation

The required additional yaw moment $\Delta M$ is computed via a sliding mode control algorithm based on the deviation between the desired yaw rate ${\omega }_d$ and the actual yaw rate $\omega$. Defining the yaw rate tracking error as $e_{\omega }=\omega -{\omega }_d$, the sliding surface function is designed as Equation (29). When the system state reaches the sliding surface, $s_{\omega }=0$.

$$s_{\omega }=c_{\omega }{e}_{\omega }+{\dot{e}}_{\omega }$$

(29)

Here, $c_{\omega }$ represents a positive weighting coefficient balancing error magnitude and error change rate. To ensure robustness during the approach phase and reduce chattering, a constant-velocity reaching law is adopted, as shown in Equation (30):

$$\dot{s_{\omega }}=-\epsilon ·\mathrm{sgn}\left(s_{\omega }\right)-K{s}_{\omega },\epsilon >0$$

(30)

To rigorously demonstrate the stability of the designed sliding mode control law and verify that the sliding condition is satisfied, a Lyapunov stability analysis is conducted. Consider the following candidate Lyapunov function:

$$V={\displaystyle \frac{1}{2}}{s}_{\omega }^2$$

(31)

This function is positive definite, as $V>0$ for all $s_{\omega }\ne 0$, and $V=0$ only when $s_{\omega }=0$. The time derivative of V is obtained as:

$$\dot{V}=s_{\omega }{\dot{s}}_{\omega }$$

(32)

Substituting the reaching law, ${\dot{s}}_{\omega }=-\eta ·\mathrm{sgn}\left(s_{\omega }\right)$ from Equations (30)–(32) yields:

$$\dot{V}=s_{\omega }·(-\eta ·\mathrm{sgn}\left(s_{\omega }\right))$$

For $s_{\omega }\ne 0$, the sign function can be expressed as $\mathrm{sgn}\left(s_{\omega }\right)=s_{\omega }/\left|s_{\omega }\right|$. Therefore, the derivative simplifies to:

$$\dot{V}=-\eta ·s_{\omega }·{\displaystyle \frac{s_{\omega }}{|s_{\omega }|}}=-\eta ·\left|s_{\omega }\right|$$

(33)

Given that the parameter $\eta$ is strictly positive ($\eta >0$), it follows from Equation (33) that $\dot{V}\le 0$ for all $s_{\omega }$, with equality holding only if $s_{\omega }=0$. Specifically, $\dot{V}<0$ for any $s_{\omega }\ne 0$. This satisfies the reaching condition of sliding mode motion, guaranteeing that the system state will be driven from any initial point in the phase space to the sliding manifold $s_{\omega }=0$ within a finite time. Once the system state reaches the manifold, its dynamics are governed by the equation $s_{\omega }=0$ defined in Equation (29), ensuring that the tracking error $e_{\omega }$ converges to zero exponentially.

Combining Equations (15), (29) and (30), the additional yaw moment derivation proceeds as follows:

$$f\left(\omega \right)={\displaystyle \frac{2}{I_z}}\left[a{c}_{cf}\left({\displaystyle \frac{v_y+a\omega }{v_x}}-\delta \right)-b{c}_{cr}{\displaystyle \frac{v_y-b\omega }{v_x}}\right]$$

$$\dot{\omega }={\displaystyle \frac{\partial f}{\partial t}}+{\displaystyle \frac{\Delta M}{I_z}}\approx {\displaystyle \frac{\Delta M}{I_z}}$$

$$\Delta M=I_z\left[-\epsilon \mathrm{sgn}\left(s_{\omega }\right)-K{s}_{\omega }-c_{\omega }(\dot{\omega }-{\dot{\omega }}_d)+{\dot{\omega }}_d\right]-I_{z}f\left(\omega \right)$$

$$\Delta M=I_z\left(-\epsilon ·\mathrm{sgn}\left(s\right)-Ks-c_{\omega }{\dot{e}}_{\omega }+{\dot{\omega }}_d\right)-\left[2a{c}_{cf}\left(\delta -{\displaystyle \frac{v_y+a\omega }{v_x}}\right)-2b{c}_{cr}{\displaystyle \frac{v_y-b\omega }{v_x}}\right]$$

This dynamically compensates for unstable moments generated by front-wheel steering inputs, ensuring rapid tracking of ${\omega }_d$ by the actual yaw rate.

4.3. Torque Distribution Optimization

After obtaining the additional yaw torque $\Delta M$, it is essential to convert it into execution commands for the four-wheel hub motors through the optimized distribution of drive torque. Considering the uneven vertical load distribution $F_{zij}$ on each wheel and the sudden changes in local road adhesion coefficient ${\mu }_{ij}$ in agricultural environments, an optimization problem is formulated to minimize the tire force utilization ratio. Since tire lateral forces are coupled with driving forces and are relatively small compared to driving forces, only driving force distribution is considered. The objective function is formulated as shown in Equation (34).

$$minJ=min\sum _{ij=fl,fr,rl,rr}{\displaystyle \frac{F_{xij}^2}{{\left({\mu }_{ij}{F}_{zij}\right)}^2}}$$

(34)

This objective function minimizes the sum of squared ratios of each tire’s longitudinal force $F_{xij}$ to its adhesion limit ${\mu }_{ij}{F}_{zij}$.

The torque allocation must strictly satisfy vehicle-level dynamic requirements from the upper-layer controller, enforced through two key equality constraints (35) and (36). Longitudinal Force Equilibrium: The sum of longitudinal tire force projections along the vehicle coordinates system’s x-axis must equal the force required to achieve the target longitudinal acceleration $a_x$.

$$(F_{xfl}+F_{xfr})cos{\delta }_f+F_{xrl}+F_{xrr}=m{a}_x$$

(35)

Yaw Moment Equilibrium: The moment generated by the differential longitudinal forces between left and right tires must equal the additional yaw moment $\Delta M$ computed by the upper controller.

$${\displaystyle \frac{B_f}{2}}(F_{xfr}-F_{xfl})cos{\delta }_f+{\displaystyle \frac{B_r}{2}}(F_{xrr}-F_{xrl})=\Delta M$$

(36)

Here, $B_f$ and $B_r$ denote the front and rear axle track widths, respectively. This constraint produces the required $\Delta M$ through driven wheel torque differentiation. Each hub motor’s torque capacity is bounded, directly constraining tire longitudinal forces per Equation (37).

$$|F_{xij}|\le {\displaystyle \frac{T_{max}}{R_{eff}}}$$

(37)

$T_{max}$ represents the maximum brake torque deliverable by each hub motor, while $R_{eff}$ is the effective wheel radius. This constitutes a quadratic programming problem with linear constraints. The MATLAB fmincon solver computes optimal longitudinal forces $F_{xij}$ for all four tires. The torque command for each hub motor is derived as $T_{ij}=F_{xij}{R}_{eff}$. The motor controller executes precise torque tracking based on these commands. The presented torque distribution strategy is agnostic to the actuator implementation. For tractors with standard differential braking systems, the optimized yaw moment $\Delta M$ is naturally executed as a differential braking force between the left and right wheels.

5. Experimental Results and Analysis

5.1. Effectiveness of Delay Compensation

The study validates the delay compensation module’s efficacy using a visual delay time delay time $\tau$ to 0.4 s. Tracking performance of the original MPC was compared with that of the MPC enhanced by the visual delay compensation module across three typical trajectories: circular, S-curve, and straight, as illustrated in Figure 4. In the trajectory fitting space matching comparison shown in Figure 3a, the original MPC path without delay compensation exhibits significant fluctuations, with noticeable jittering occurring in regions of curvature change along the circular trajectory and at the turning points of the S-curve trajectory. This phenomenon is primarily attributed to the delay-dynamic coupling effect. When the controller generates control commands based on historical state information, the delay hinders real-time responsiveness to curvature changes, resulting in a mismatch between the system state at the execution moment and the predicted state, thereby causing the accumulation of tracking errors. Although the fluctuation amplitude in the straight-line trajectory is minimal, the noise induced by the delay is amplified under integral action, leading to slight oscillations in both speed and attitude angle. As depicted in Figure 3b, the improved MPC algorithm significantly reduces the average tracking error. The tracking error for the circular path decreased from 0.46 m to 0.111 m (75.87% reduction), for the S-curve path from 0.564 m to 0.136 m (75.89% reduction), and for the straight path from 0.236 m to 0.051 m (95.58% reduction). After the introduction of visual delay compensation, the MPC incorporates the delay time into the system dynamics equations, constructs an extended state space model, and enables the controller to pre-compensate for state drift within the $\tau$ time frame, effectively mitigating the adverse effects of visual delay on the control system.

To verify the effectiveness of the proposed delay-aware method, it was compared with the baseline MPC and the simple forward prediction (SFP) method. The SFP method uses the same vehicle dynamics model for forward prediction but without the historical-error regularization. The results in

Table 2. Performance comparison of different delay compensation methods.

Controller	Circular Path	S-Curve Path	Straight Path	Average
	Error (m)	Error (m)	Error (m)	Error (m)
Baseline MPC (no compensation)	0.460	0.564	0.236	0.420
Simple Forward Prediction (SFP)	0.185	0.231	0.098	0.171
Proposed Delay State Observer	0.111	0.136	0.051	0.099

demonstrate that the proposed delay state observer with historical-error regularization significantly outperforms both the baseline MPC and the SFP method across all tested trajectories. It attains a minimal average tracking error of merely 0.099 m, representing a substantial 42.1% enhancement over the SFP method at 0.171 m. This advantage is consistently demonstrated on each specific path, with errors as low as 0.111 m for the circular path, 0.136 m for the S-shaped path, and 0.051 m for the straight-line path. This pronounced contrast underscores the critical role of the historical-error regularization mechanism, as it actively compensates for the estimation errors accumulated over time.

To further systematically verify the robustness of the delay compensation module, tests were conducted on the improved algorithm under a delay scenario ranging from 0–1 s. Three typical trajectories circular, S-curve, and straight were selected, and the average tracking error was calculated at 50 random delay time points. The results were compared with the performance of the original MPC, as illustrated in Figure 5.

Figure 5a depicts the trend of tracking error as a function of delay time. As shown in the figure, the tracking error of the original MPC increases non-linearly with increasing delay time, indicating that the predicted trajectory based on the delayed state is spatially mismatched with the actual vehicle orientation. Consequently, the front wheel steering angle command consistently lags behind the actual required direction. This issue is particularly pronounced in the curvature transition regions of the circular and S- curve trajectories, where the misjudgment of steering timing caused by delayed observations significantly amplifies the amplitude of yaw angle oscillations, leading to increased trajectory tracking deviations. The MPC with the integrated delay compensation module effectively addresses the spatial mismatch issue through a real-time state synchronization mechanism. The core of this approach lies in dynamic predictive compensation, which ensures that control commands closely align with the vehicle’s current actual pose, thereby maintaining near-zero delay error levels within the range of $\tau \le 0.8$ s. When $\tau$ exceeds 0.8 s, the improved MPC begins to exhibit performance degradation; however, the magnitude of this degradation is significantly smaller than that of the original MPC. This degradation phenomenon highlights the physical limits of the compensation mechanism: when the delay time is excessively long, the accumulated uncertainty of the prediction model substantially diminishes the effectiveness of compensation.

Figure 5b quantitatively compares error distributions between baseline MPC and compensated MPC across three trajectory types using box plots. Overall, complex trajectories, such as circular and S-curve paths, demonstrate more pronounced compensation effects compared to simple straight-line trajectories. Straight-line trajectories exhibit a significantly smaller median error reduction due to their inherent kinematic smoothness, which minimizes baseline control errors and thus constrains compensation potential. In the case of circular trajectories, 82% of the difference data points exceed the median value, confirming that the compensation module maintains stable gain characteristics during continuous curved motion. The median difference value for S-curve trajectories is 0.513, which surpasses the circular trajectory’s value of 0.491 and the straight line’s value of 0.349. Furthermore, S-curve trajectories exhibit the most concentrated distribution characteristics, with an interquartile range (IQR) of 0.301, which is lower than the circular trajectory’s IQR of 0.405 and the straight line’s IQR of 0.412. These results indicate that the MPC algorithm, enhanced with the fusion delay compensation module, retains robust performance across the entire delay range (0–1 s) and under various motion conditions.

5.2. Robustness Analysis of the Cooperative Controller

To verify the effectiveness of the proposed coordinated path-tracking and stability control strategy, a co-simulation platform was developed using CarSim (version 2019.1) and Simulink. Vehicle path tracking simulation analysis was conducted, with CarSim providing the vehicle dynamics model and the designed controller along with the tire force distribution method implemented in Simulink. Figure 6 illustrates the double-line shift scenario and the simulation framework in Simulink. The experiment aimed to track a reference path composed of straight and curved segments. The controlled vehicle was a medium-sized agricultural tractor, configured with key parameters including a mass $m=3190$ kg, yaw inertia $I_z=11,903\mathrm{kg}·{\mathrm{m}}^2$, wheelbase $L=2.15\mathrm{m}$, a front wheel tread of 1.505 m, a rear wheel tread of 1.635 m, front and rear cornering stiffnesses $C_{\alpha f}=73\mathrm{kN}/\mathrm{rad}$ and $C_{\alpha r}=152\mathrm{kN}/\mathrm{rad}$, and tread widths of $1.505\mathrm{m}$ (front) and $1.635\mathrm{m}$(rear), as also detailed in Figure 6. It operated at a speed of 5 km/h on road surfaces with a friction coefficient of 0.7 for dry hard soil and 0.4 for wet soft soil.

The simulation results presented in Figure 7 ($\mu$ = 0.7) and Figure 8 ($\mu$ = 0.4) illustrate effective trajectory tracking performance under varying road surface adhesion conditions. In high adhesion scenarios ($\mu$ = 0.7), the average path tracking error decreased from 44.7 mm to 11.3 mm. When the vehicle enters a high curvature zone, the real-time stability compensation mechanism of the SMC layer effectively mitigates path deviation. Compared to the original MPC, the cooperative controller exhibits a significant enhancement in yaw control performance, with the average yaw angle error reduced by 73.24% (from 0.1566° to 0.0419°), alongside a reduction in the amplitude of yaw angular velocity fluctuations. This suggests that the controller continuously monitors variations in the vehicle’s yaw rate and, upon nearing the safety boundary, activates sliding mode compensation control to generate an active yaw torque. This torque is distributed through differential braking of the drive wheels to counteract yaw dynamics nonlinear errors. Concurrently, the vehicle’s center of gravity lateral deviation angle and front wheel steering angle remain within safety limits, thereby validating the dual-layer collaborative mechanism of the MPC and SMC layers in maintaining state boundary stability.

For low-friction road surfaces ($\mu$ = 0.4), the cooperative controller achieved an average error reduction of 56.19% (from 81.2 mm to 35.6 mm), demonstrating superior adaptability to environmental conditions. In contrast to high-friction scenarios, low-friction surfaces generate smaller lateral forces, which result in longer convergence times for yaw rate and minor fluctuations. Nevertheless, the architecture maintains the error magnitude within a strict boundary of 36 mm, representing a 56% improvement over the baseline MPC system, thereby ensuring robust control under low-friction conditions. Regarding stability control, the reduction in the center of gravity lateral deviation angle reached 76.19% (from 0.0147° to 0.0035°), reflecting an enhancement of 1.19 percentage points over high-adhesion road surfaces (75.00%). This indicates that the SMC layer actively reinforces stability control weights through dynamic optimization of domain constraints when adhesion is insufficient. Although front wheel steering angle changes are constrained by the physical limitations of the road surface, direct yaw torque control based on drive wheel differential braking can effectively compensate for degradation in steering performance, thereby validating the robustness of the sliding mode compensation mechanism under low $\mu$ conditions.

5.3. Co-Simulation Validation

To verify the path tracking performance of the proposed algorithm for autonomous navigation in agricultural fields, a virtual farm environment was constructed using the Gazebo simulation platform (Figure 9a). This environment simulates essential farm features, including structured crop rows and soil texture. The test platform is equipped with a forward-facing virtual RGB camera, which captures image data in real time via the perception node implemented in Python (version 3.8). In the perception pipeline, the system first employs the ExG segmentation algorithm to accurately extract seedling row regions, followed by robust least squares fitting to generate the desired reference heading. To comprehensively evaluate the real-time performance of the perception system, precise timing measurements were conducted for each processing stage: the ExG vegetation segmentation stage requires an average processing time of 45 ms, while the subsequent robust least squares fitting stage averages 18 ms, resulting in a total processing cycle of 63 ms for the entire perception pipeline. The perception node employs the ExG segmentation algorithm to extract the seedling row region and generates the desired reference heading through robust least squares fitting. The control layer compiles the HDC-CC core algorithm, implemented in Simulink, into an independent ROS node utilizing the ROS Toolbox. This controller node interacts with both the vehicle dynamics model and the perception node in Gazebo in real time through predefined ROS Topic interfaces.

Tracking tests were conducted along a typical serpentine working path, as illustrated in Figure 9b. In an unstructured farmland environment, the overall average tracking error was measured at only 0.091 m, which demonstrates the exceptional robustness and adaptability of the HDC-CC architecture. The maximum error recorded was 0.297 m, occurring in the initial turning region, while subsequent turning errors exhibited a significant decreasing trend (0.231 m in the second turning region and 0.215 m in the third turning region), thereby validating the effectiveness of the delayed state observer. The proposed mechanism effectively mitigates the accumulation of historical errors by specifically compensating for temporal misalignment. The system optimizes control strategies by continuously accumulating environmental information in real-time. Notably, the delay visual error penalty term within the objective function effectively suppresses the cumulative impact of historical errors, allowing the system to persistently learn and enhance accuracy under continuous turning conditions. The test results indicate that the predictive control architecture, which integrates delay compensation and motion control co-optimization, effectively addresses the path tracking challenges encountered by visually autonomous navigation agricultural machinery in unstructured farmland environments, thereby providing a reliable technical foundation for high-precision autonomous operations.

To evaluate the practical deployment potential of the HDC-CC framework, a detailed computational resource profiling was conducted on Gazebo environment. The resource consumption of each core module is meticulously detailed in

Table 3. Computational resource profile of the HDC-CC modules.

Component	CPU Utilization (%)	Time (ms)	Memory (MB)
Delay State Observer	4.1	5.8	38
MPC Path Tracking	25.7	33.3	108
SMC Stability Controller	3.0	3.5	15
Quadratic Programming	2.8	3.2	12
ROS Communication Overhead	6.5	7.9	48
Total (HDC-CC Framework)	42.1	53.7	221

. The Model Predictive Control module, as the core algorithmic component, demonstrates the most significant computational load with a single-cycle execution time of 33.3 ms and 25.7% CPU utilization, which aligns with the computational characteristics of solving optimization problems online. The Delay State Observer and Sliding Mode Stability Controller exhibit relatively low computational demands, with execution times of 5.8 ms and 3.5 ms respectively, indicating their suitability for implementation on embedded controllers with limited computational resources. Regarding memory usage, the MPC module requires 108 MB, while the total memory footprint of the framework is 221 MB, which falls within acceptable limits for modern agricultural embedded processors. Comprehensive analysis demonstrates the HDC-CC framework’s real-time implementation potential on mainstream agricultural embedded platforms, providing a critical baseline for evaluating computational demands prior to deployment.

6. Conclusions

This study presents a hierarchical delay-compensated cooperative control (HDC-CC) method for vision-based navigation of agricultural machinery, addressing the critical need for precision in field operations such as targeted spraying and weeding. This method constructs a real-time pose prediction compensator based on the forward integral of vehicle dynamics, effectively overcoming inherent delays in image processing. Experimental results demonstrate that under a visual delay of 0.4 s, the proposed method reduces the S-curve path tracking error from 0.564 m to 0.136 m, while maintaining near-zero error levels within a 0.8 s delay range. In dual-line shifting tests, the system achieves a tracking accuracy of 0.0113 m on high-friction surfaces and sustains a control error of 0.0356 m on low-friction surfaces, improving robustness by 56% over MPC. By incorporating historical-error regularization, the average tracking error is reduced by 42.1% compared to the SFP method, confirming the effectiveness of accumulated error compensation. In the Gazebo farmland simulation, an average tracking error of 0.091 m for the snake-shaped path verifies that the proposed method mitigates control mismatch caused by visual latency through state prediction and dynamic synergy optimization. This provides a reliable technical foundation for enhancing operational accuracy and cost efficiency in precision agriculture.

Despite these promising outcomes, it is important to note that the current validation is based on simulation experiments. Future work will include real-world implementation on a dedicated agricultural robotic platform to evaluate performance under unpredictable field conditions. Additionally, optimizing the computational load of the MPC and SMC modules for deployment on embedded systems will be a critical step toward practical application.