The Full-Field Path Tracking of Agricultural Machinery Based on PSO-Enhanced Fuzzy Stanley Model

Abstract

The unmanned operation of agriculture machinery in the full field of farmland is an important part of unmanned farm and smart agriculture. Although the autonomous navigation for agriculture robot has been widely studied in literature, research on the full-field path tracking problem of agriculture machinery is rare. In this paper, in order to enhance the adaptivity of path tracking algorithm, an improved fuzzy Stanley model (SM) is proposed based on particle swarm optimization (PSO), where the control gain is modified adaptively according to the tracking error, velocity and steering actuator saturation. The PSO-enhanced fuzzy SM (PSO-FSM) is verified by experiments on numerical simulation and self-driving of mobile vehicle. Simulation results indicate that the PSO-FSM achieves a better result than SM and FSM, where PSO-FSM changes the control gain adaptively under different velocities and actuator saturation conditions, and the maximum lateral errors of SM and PSO-FSM for mobile vehicle autonomous turning are 0. 32 m and 0.03 m, respectively. When the location of the mobile vehicle deviates from the expected path at 4 m in a lateral direction, the distance of the guided trajectory for the mobile vehicle to reach the expected path is no more than 5 m. A preliminary experiment is also carried out for a wheeled combine harvester working on slippery soil, and the result indicates that the maximum lateral tracking error of PSO-FSM is 0.63 m, which is acceptable for the path tracking of a combine harvester with a large operation width.

1. Introduction

Intelligent agricultural machinery is a crucial support for unmanned farm and smart agriculture, where automatic navigation is the core technology and has been widely studied [1,2]. During the special period of the outbreak of the new crown epidemic, the automatic navigation of agricultural machinery provides an important guarantee for agricultural production and food security. The path tracking of agricultural machinery controls the agricultural machinery driving along a predetermined trajectory autonomously, which is an important part of the unmanned autonomous operation of agricultural machinery in the farmland [3,4].

The widely employed path tracking control methods for agricultural machinery include PID control, pure pursuit control, fuzzy control, neural network-based control and optimal control [5,6,7,8,9], to name a few. Hu et al. [10] proposed a cascaded navigation control method for straight line path tracking, which separates the navigation control into path tracking control and steering control, and the standard deviation of the lateral error is 0.04 m. Zhang et al. [11] developed a path tracking controller by integrating an adaptive neural network estimator and a saturated auxiliary system, and compared it with the traditional methods. The controller reduces the tracking error by more than 28%. A model predictive control-based path following the controller was developed for high-speed vehicle path tracking in [12], where the fluctuation of longitudinal velocity is compensated in the predicted horizon to reduce the state prediction error. The experimental results indicate that the controller not only reduces the lateral and heading deviation effectively but also maintains the control stability under large vehicle maneuver. Li et al. [13] proposed an adaptive sliding mode control method based on non-time reference and RBF neural network to handle uncertain disturbances. The experiments demonstrate that the proposed method not only improves the path tracking performance but also eliminates the chattering phenomenon when the wheeled mobile robot suffers from uncertain disturbances. In order to overcome the disadvantages of pure pursuit in high-speed path tracking, a model predictive active yaw control implementation of pure pursuit path tracking was proposed in [14], which improves the tracking performance at high speeds by accommodating the vehicle’s steady-state lateral dynamics. Most of the abovementioned adaptive path tracking algorithms improve the tracking precision or robustness with respect to disturbances at the cost of computational complexity, which hinders their application from real-time path tracking application.

Stanley model (SM) is a nonlinear feedback function based on lateral tracking error, which generates the steer angle command by employing the relative geometric relationship between vehicle’s pose and predefined trajectory. SM does not depend on the look-ahead distance as pure pursuit, and its lateral tracking error converges to zero exponentially [15]. The unmanned vehicle based on SM is the winner of DARPA’s second unmanned vehicle challenge, whereas its application on the automatic navigation of agricultural machinery is rarely reported. The adaptive parameter tuning is an active research field for a geometric-based path tracking controller, and there is similar problem for SM especially when vehicle maneuver or road conditions vary from time to time. The look-ahead distance of pure pursuit is adjusted in [16] by using improved particle swarm optimization (PSO), which indicates when the speed of agricultural machinery is 0.7 m/s and the driving distance exceeds 5 m, the maximum lateral error is 0.02 m. Wang et al. [17] utilized the multi-population genetic algorithm to optimize the parameters of SM controller, and results show that the path tracking performance is improved by 41.72% and 48.61% for two typical tractor turning methods (U and Ω routes), compared with SM without optimization. Amer et al. [18] proposed a PSO-optimized fuzzy supervisory system to handle the various trajectories and speed for an armored vehicle, where the parameters of the modified SM were changed adaptively based on fuzzy inference and an optimal knowledge database.

Inspired by the work of [18], this paper proposes an improved path tracking control algorithm based on the fuzzy SM (FSM) and PSO to cater for variations in the agriculture robot in terms of speed and road conditions. In this contribution, the primary controller parameters of SM are adjusted by fuzzy inference at first, and then the control gain under different velocities and actuator saturation conditions are further modified by PSO. The fuzzy algorithm is used to adjust the control gain with respect to tracking errors, which not only improves the tracking precision for the automatic turning of agricultural machinery, but also reduces the distance of guided trajectory when the initial lateral error is large. The PSO is employed to further optimize the decision-making of the front wheel steer angle and improve the adaptivity of agricultural machinery towards different vehicle speeds and actuator saturation situations. The main contributions of this paper are as follows: (1) PSO-enhanced control gain adjustment is developed to cater for varying vehicle velocities and actuator saturation conditions; (2) a cascaded path tracking algorithm based on FSM and PSO is proposed for the full-field autonomous navigation of agriculture robot; (3) the autonomous navigation experiments based on numerical simulation, a mobile vehicle and a combine harvester are employed to verify the superiority of PSO-enhanced FSM (PSO-FSM).

The structure of this paper is arranged as follows. The fuzzy Stanley model is presented in Section 2. PSO-enhanced control gain adjustment is developed in Section 3. In Section 4, the PSO-FSM is verified by using numerical simulation, path tracking of a mobile robot and an unmanned combine harvester in a slippery soil condition. Finally, Section 5 concludes the conclusions of this work.

2. Fuzzy-Based Stanley Model

In the path tracking of agricultural machinery, the bicycle model is often used for kinematic analysis, where it is assumed that the agricultural machinery is running on a smooth road, and only longitudinal pressure is generated between the tires and the ground.

As shown in Figure 1, δ is the front wheel rotation angle, θ is the heading angle of the vehicle and L is the distance between the front and rear axles of the vehicle. Kinematic analysis of this model can be summarized as

$$x^{\prime }\left(t\right)=v\sin \theta$$

$$y^{\prime }\left(t\right)=v\cos \theta$$

$${\theta }^{\prime }\left(t\right)=\frac{v\tan \delta }{L}$$

(1)

where v is the vehicle longitudinal speed; $x^{\prime }\left(t\right)$ is the vehicle speed in the x-axis direction; $y^{\prime }\left(t\right)$ is the vehicle speed in the y-axis direction; and ${\theta }^{\prime }\left(t\right)$ is the vehicle angular velocity. The simplified bicycle kinematic model will be employed in our simulation.

SM needs parameter adaptation similar to other geometric-based path tracking. Typical parameter self-adaptation methods include the intelligent search algorithm [19], neural networks algorithm [20] and fuzzy algorithm [21]. Intelligent search algorithms mainly include the genetic algorithm [22], PSO [23] and differential evolution algorithm [24]. The neural networks algorithm can be classified into BP [25], RBF [26], PNN [27] and GRNN [28] according to their excitation functions. Compared with other methods, parameter self-adaptation based on fuzzy algorithm has the advantages of low computational load and strong robustness, which is suitable for online implementation in an embedded controller [29]. In this section, the fuzzy algorithm is used to adjust the output gain of SM, which provides an algorithm basis for global intelligent optimization under varying environmental disturbances.

2.1. Stanley Path Tracking Algorithm

The schematic diagram of SM is given in Figure 2, where the control input of the expected front wheel angle of the SM consists of two parts, i.e., corresponding to the lateral deviation and the heading deviation, respectively:

$$\delta \left(t\right)={\delta }_e\left(t\right)+{\delta }_{\theta }\left(t\right)$$

(2)

where $\delta \left(t\right)$ is the expected angle; ${\delta }_e\left(t\right)$ is the expected angle due to lateral deviation; and ${\delta }_{\theta }\left(t\right)$ is the expected angle due to heading deviation.

By only considering the influence of lateral deviation, the larger the lateral deviation, the larger the expected front wheel steering angle. Suppose the expected path and the tangent line intersects at a point, that in front of the closest point with respect to front wheel by d, and according to the geometric relationship the following nonlinear scaling function can be obtained

$${\delta }_e\left(t\right)=\mathit{arctan}\frac{e\left(t\right)}{d\left(t\right)}=\mathit{arctan}\frac{ke\left(t\right)}{v\left(t\right)}$$

(3)

where $k$ is the gain coefficient; $e\left(t\right)$ is the lateral deviation; and $v\left(t\right)$ is the driving speed.

If only the influence of heading deviation is taken into consideration, the front wheel deflection angle is consistent with the tangent direction of the given path. The expected steering angle of the front wheel is equal to the angle between the vehicle heading and the tangent direction of the nearest path point, that is

$${\delta }_{\theta }\left(t\right)={\theta }_e\left(t\right)$$

(4)

where ${\theta }_e\left(t\right)$ is the heading deviation.

By taking both the two aspects into consideration, the expected rotation angle function of the front wheel is obtained as follows:

$$\delta \left(t\right)={\theta }_e\left(t\right)+\mathit{arctan}\frac{ke\left(t\right)}{v\left(t\right)}$$

(5)

2.2. Parameter Self-Adaptation Based on Fuzzy Algorithm

It is notable in Equation (4) that the influence of the lateral deviation on the wheel steering angle is weighted by the gain coefficient of SM. The larger gain coefficient can reduce lateral deviation efficiently and enable the agricultural machinery to converge to the expected path quickly, whereas a large gain coefficient will lead to tracking error fluctuation. The small gain coefficient can make the agricultural machinery run smoothly and thus reduce the steady-state tracking error when the algorithm converges, but the controller converges slowly, especially when a large initial lateral error is given. Therefore, the time-varying gain coefficient should be selected according to different lateral deviations, especially for the full-field path tracking of agriculture machinery, which has stringent requirements on the distance of guided trajectory. What is more, the effect of the heading deviation and lateral deviation work on wheel steering angle is directional. When the lateral deviation and the heading deviation have the same direction of action on the wheel angle, the gain coefficient is decreased appropriately to make the controller stable. Conversely, when the lateral deviation and the heading deviation work on the wheel steering angle in opposite directions, the gain coefficient would be increased appropriately to speed up the algorithm’s convergence speed.

The fuzzy reasoning rules are initially formulated by taking the actual conditions of agricultural machinery into consideration, where the membership function is constructed with the lateral deviation and heading deviation as input variables, and the steering angle of the front wheel as output whose maximum value is limited to 35 degrees. The maximum value and standard deviation of the lateral deviation are employed as observations, and a trial-and-error procedure is performed to find the optimal gain coefficient under different inputs.

The universe for the lateral deviation is [−3 m, 3 m], which includes right large, right medium, right small, zero, left small, left medium, left large, respectively, corresponding to NP, NM, NS, ZO, PS, PM and PB, as it is shown in Figure 3a. The universe of the heading deviation is [−30°, 30°], as it is shown in Figure 3b, which includes right large, right medium, right small, zero, left small, left medium and left large, respectively, corresponding to NP, NM, NS, ZO, PS, PM and PB. As it is shown in Figure 3c, the universe of gain coefficients is [0, 1.2], and the gain coefficients include zero, small, medium and large, respectively, corresponding to ZO, PS, PM and PB.

The specific rules of the fuzzy inference table are designed as follows: the smaller the heading deviation and the larger the lateral deviation, then the larger the gain coefficient; if the heading deviation and the lateral deviation work on the wheel angle in the same direction, then the larger the heading deviation, the smaller the gain coefficient; if the heading deviation and lateral deviation work on the wheel angle in opposite directions, then the larger the heading deviation, the larger the gain coefficient; if the heading deviation is large, and the heading deviation and lateral deviation work on the front wheel angle in the same direction, then a smaller gain coefficient is selected; and if the heading deviation is large, and the heading deviation and lateral deviation work on the steering angle of the front wheel in opposite directions, then a larger gain coefficient is selected. Finally, after verification by experiment test, a total of 49 fuzzy rules are summarized in

Table 1. Rule base for fuzzy logic control.

${\mathit{\theta }}_{\mathit{e}}$	$\mathit{e}$
	NB	NM	NS	ZO	PS	PM	PB
NB	PS	PS	PS	PM	PB	PB	PB
NM	PM	PS	PS	PS	PM	PB	PB
NS	PM	PM	PS	PS	PM	PM	PB
ZO	PM	PM	PS	PS	PS	PM	PM
PS	PB	PM	PM	PS	PS	PM	PM
PM	PB	PB	PM	PS	PS	PS	PM
PB	PB	PB	PB	PM	PS	PS	PS

, where $e$ stands for the lateral deviation and ${\theta }_e$ represents the heading deviation.

The typical defuzzification methods include maximum subordination principle, center of gravity method and weighted average method [30]. The center of gravity method is widely used in industry application because of its smooth output, so the center of gravity method is chosen in our work for defuzzification, which is formulated as

$$u_0=\frac{{{\displaystyle \sum }}_{k=1}^m{u}_k{\mu }_k\left(u_k\right)}{{{\displaystyle \sum }}_{k=1}^m{\mu }_k\left(u_k\right)}$$

(6)

where ${\mu }_0$ is the output coefficient; $m$ is the number of levels; ${\mu }_k$ is the coefficients for each level; and ${\mu }_k\left(u_k\right)$ is the degree of membership.

3. PSO-Enhanced Fuzzy Stanley Model

3.1. Principle of PSO Optimization

PSO is a heuristic swarm intelligence algorithm, which stems from the research on the predation behavior of bird flocks [31]. In order to find the place where most food is located, the flocks adjust their search directions by using their own experience and communication among the flocks. Because of its fast convergence speed, easy online implementation and simple principle, it is widely used in the fields of fuzzy control system design and other high-dimensional optimization problems [32]. In the application of intelligent agricultural machinery for full-field unmanned operation, the various operation speeds, soil conditions and actuator saturation of steering control system will have a great effect on the accuracy and stability of path tracking. Therefore, in order to improve the adaptively of fuzzy-based SM, the PSO algorithm is employed to further optimize the expected steering angle.

The steering control system of agricultural machinery is a closed-loop PID control system including an electric steering wheel, a mechanical steering mechanism and angle sensor fixed on the front wheel. It is easy to understand that when the driving speed is high, the steering wheel control input should be changed slowly and a small steering angle should be given. In order to handle the time-varying speed, PSO is employed to optimize the output gain of FSM. Define a control gain coefficient α, and the control gain of FSM is multiplied by the coefficient

$${\delta }_{PSO}\left(t\right)=\alpha \cdot \delta \left(t\right)$$

(7)

where ${\delta }_{PSO}\left(t\right)$ is the desired front wheel angle after utilizing PSO.

When the speed of the agricultural machinery is high, the value of the coefficient α should be reduced to improve the stability of the control. What is more, the steering control systems of the wheeled mobile vehicle and combine harvester involved in our work are actually a first-order system with time delay, and the varying parameters of the closed-loop PID will have a great effect on the navigation control decision. When the mobile robot is subject to its steady-state stage of path tracking, i.e., at the end of a straight line tracking, the actuator saturation of the steering control system will make it difficult to respond to the large, expected input angle in a timely manner. However, by multiplying the expected steering angle by a coefficient α, the response speed of the steering control system can be improved, and thus the accuracy of the path tracking during the path transition period can be ensured.

3.2. Implemention of PSO-Enhanced FSM

The fitness function of PSO is also named as the objective function. The selection of fitness function has a significant effect on the convergence speed and steady-state accuracy of the PSO-FSM. The integral time absolute error (ITAE) is frequently applied as the performance metric in the automatic control community, which calculates the integral of the product of the time and absolute error value for some predefined time interval [33]. In this paper, the inverse of the sum of ITAE of lateral deviation and heading deviation is used as the fitness function, and the calculation formula is as follows

$$f={\omega }_1\mathsf{\Delta }t{\displaystyle \sum }_{j=1}^N{t}_j\left|e\left(j\right)\right|+{\omega }_2\mathsf{\Delta }t{\displaystyle \sum }_{j=1}^N{t}_j\left|{\theta }_e\left(j\right)\right|$$

(8)

$$F=\frac{1}{f}$$

(9)

where $f$ is the sum of ITAE of lateral deviation and heading deviation, ${\omega }_1$ and ${\omega }_2$ are the weights which correspond to ITAE, $\mathsf{\Delta }t$ is the integration interval, and $e\left(j\right)$ and ${\theta }_e\left(j\right)$ correspond to the lateral deviation and heading deviation at time $j\mathsf{\Delta }t$. N is the number of control periods involved in the calculation of the f value, and F is the fitness value employed for PSO-FSM.

The PSO simulates birds in a flock by designing a massless particle where only the speed and position are adjustable, and the flowchart of PSO is shown in Figure 4. Firstly, each particle searches for the optimal solution individually in the search space, then, its position is recorded as the local optimal position. Secondly, each particle shares its optimal position with other particles in the entire flock to find the temporary global optimal position of the current flock. Finally, each particle in the flock adjusts its attributes continuously according to the optimal position of the individual and the current flock until the global optimal solution is found. The specific procedures are as follows:

(1)

Initialize the particle flock. Parameters such as the inertia coefficient $\omega$ and learning factor $c_1$, $c_2$ are determined according to the operation of agricultural machinery.

(2)

Calculate the fitness of each particle in the flock and update the optimal position of each particle, then calculate the optimal position of the current flock. Adjust the speed and position of each particle accordingly. The calculation formulas are as follows:

$$v_i\left(k+1\right)=\omega v_i\left(k\right)+c_1\xi \left(p_{ibest}\left(k\right)-x_i\left(k\right)\right)+c_2\eta \left(p_{gbest}\left(k\right)-x_i\left(k\right)\right)$$

(10)

$$x_i\left(k+1\right)=x_i\left(k\right)+v_i\left(k+1\right)$$

(11)

where $v_i\left(k\right)$ and $x_i\left(k\right)$ are the velocity and position of particle i in the k-th iteration, $p_{ibest}$ is the optimal position of particle i, $p_{gbest}$ is the optimal location of the entire flock and ξ and η are random numbers between 0 and 1.

(3)

The particles are continuously updated until the maximum number of iterations is reached or the required convergence accuracy is achieved. Then, the optimal position of the flock is the value of the optimization coefficient α.

Finally, the input and output block diagram of PSO-FSM can be summarized in Figure 5, where $e$ represents the lateral deviation, ${\theta }_e$ represents the heading deviation and ${\delta }_{\alpha }$ represents the expected front wheel angle after PSO.

4. Results and Discussion

4.1. Numerical Simulation

In this section, the path tracking algorithms including SM, FSM and the proposed PSO-FSM are verified in autonomous turning under different scenarios using numerical simulation. A typical U turning trajectory is stimulated to verify the superiority of PSO-FSM. The flowchart of the numerical simulation is shown in Figure 6, where the vehicle kinematic model defined by (1) is employed to update the vehicle state, and the particle flock size is 20, the inertia coefficient ω = 0.5, the learning factors c₁ = 1, c₂ = 2, the maximum number of iterations K = 200 and the termination threshold J = 0.8. These parameters not only reduce the occurrence of large errors but also prevents the overshoot of the system output.

Figure 7a shows the test result when the vehicle runs at 1 m/s with small time delay of steering control system, which is an ideal work condition for agricultural machinery. It can be seen that both FSM and PSO-FSM achieve better performance than SM, especially when the curvature is variable, whereas PSO-FSM does not outperform FSM, obviously because the gain coefficient α does not have a direct modification on the expected front wheel angle. Figure 7b shows the results in the high-speed operation scenario, where the vehicle speed is increased to 3 m/s with other conditions unchanged. It is notable that the PSO-FSM achieves the best accuracy among the involved algorithms, and both SM and FSM have an obvious performance degradation. Once a low driving speed is given, e.g., 1 m/s, the actuator saturation situation and state delay of the control signal would have a great effect on the steering control system, which suits the actual operation scenario. By importing the control input limitation to the steering control system, i.e., scale the expected steering angle every control period by multiplying the random coefficient between 0 and 1, the steering actuator saturation can be stimulated. The corresponding result is shown in Figure 7c, which indicates when the vehicle starts turning with obvious curvature variation, PSO-FSM not only improves the response speed of steering control system, but also achieves better stability once the autonomous turning is finished. The detailed simulation results are listed in

Table 2. Path tracking result under different scenarios.

Velocity (m/s)	Steering Angle Scaling	Algorithm	MAE (cm)	RMSE (cm)
1	1	SM	1.1	36.1
1	1	FSM	0.4	13.6
1	1	PSO-FSM	0.3	12.4
3	1	SM	9.2	93.7
3	1	FSM	1.8	28.7
3	1	PSO-FSM	0.9	15.6
1	0~1	SM	20.9	48.1
1	0~1	FSM	5.4	16.2
1	0~1	PSO-FSM	0.3	12.5

, where the mean absolute error (MAE) and root mean square error (RMSE) are taken as the performance metrics. As we can see, PSO-FSM achieves a much better result than FSM and SM, especially when the scenarios of high-speed or random actuator saturation appear.

4.2. Mobile Vehicle Autonomous Navigation Test

In order to verify the effectiveness of the proposed PSO-SM, a real-time autonomous navigation test is performed by employing a mobile vehicle, as shown in Figure 8. The dual-antenna RTK position system is applied to provide the position, velocity, heading and pitch information of the mobile vehicle. The base station receiver is OEM719 and the mobile station receiver is OEM718D (provided by NovAtel Inc., Calgary, AB, Canada), where the positioning accuracy is 1 cm + 1 ppm and heading accuracy is 0.08° with a 2 m antenna baseline. The update frequency of the dual-antenna RTK is 5 Hz. The motor driver RMDS405 supports 30 A current (provided by Shenzhen RoboModule Technology Co. Ltd., Shenzhen, China), and the encoder is Omron E6B2 with a resolution up to 3600 P/R.

The navigation decision system NavLight (with Version 4.07) is built by the intelligent agricultural machinery team of Jiangsu University (Zhenjiang, China), which includes a full-field path plan and navigation signal processing and the cross-platform navigation software based on Qt works well by using multi-thread coding technology. First, predefined path information is generated utilizing the position information of the farmland. Second, the real-time heading and position information is sent to navigation PC and then lateral deviation and heading deviation are calculated and sent to the embedded controller. Finally, the controller calculates the desired steering angle based on the navigation deviation and sends the control information to the motor driver via the CAN bus at a frequency of 5 Hz. The motor driver converts the control command into voltage and receives feedback from the encoder to form a closed-loop control system, which can control the motor at 100 Hz.

In order to verify the adaptability of PSO-FSM to different path curvatures and speeds of agricultural machinery, the full-field working path for the combine harvester is designed. As it is shown in Figure 9, a hybrid path is designed for the combine harvester of large operating width, where the outer circle is composed of autonomous turning path and the arrows indicate the driving direction. If half of the remaining field width is less than the turning radius of the combine harvester, the turning at the field head is carried out by manual driving, which is more efficient than autonomous turning with back-up control. The combine harvester performs harvesting operations in sequence. When the remaining field width is smaller than the size required for the harvester to carry out autonomous turning, the navigation PC sends a signal to the controller before the end of last straight line for autonomous turning, e.g., straight line 4 in Figure 9, and then automatically switches to the manual-assisted driving mode.

The full-field path tracking of the combine harvester can be divided into the guiding stage, straight line and curve path tracking stage. The lateral tracking error of SM for the straight line is small, and the fixed output gain can achieve acceptable result, thus the guiding and curve path tracking stages are our main focuses in this work. The initial parameter setting of SM and PSO-FSM is the same as the numerical simulation to make a fair comparison. The average speeds of the mobile vehicle for the straight line and autonomous turning stages are 2.5 m/s and 0.8 m/s, respectively, i.e., the vehicle reduces its speed automatically before starting the curve path tracking. The path tracking trajectories of different algorithms for one U turning are shown in Figure 10, where both the guiding trajectory and curve path tracking result are included. It is notable in Figure 10a that PSO-FSM reduces the guiding distance of the mobile vehicle when the initial lateral deviation is set as 4 m, where the guiding distance is less than 5 m. In Figure 10b, because of the time-varying tracking error and steering actuator saturation, the performance of SM is degraded a lot compared with the straight line tracking. It is important to note that PSO-FSM outperforms SM which coincides with the numerical simulation, i.e., SM cannot achieve stability and precision path tracking by use fixed gain coefficient for time-varying curvature and disturbance. PSO and fuzzy inference improve SM significantly by employing a cascaded optimization structure to change the gain coefficient adaptively. The parameter variation in PSO-FSM is shown in Figure 11, where Figure 11a is the gain coefficient k of the path tracking test based on PSO-FSM. It is notable that k is updated adaptively according to the deviation change, and its value is reduced quickly when the path tracking of the curve segment is finished. Figure 11b is the expected front wheel steering angle calculated by the embedded controller. It is notable that when the curvature is changed, the expected steering angle changed accordingly, which presents different results with respect to the output gain coefficient variation.

The tracking error of the different algorithms is shown in Figure 12, where the lateral errors of the straight line and curve segment are given. Notice that PSO-FSM reduces the guiding distance of the autonomous vehicle in Figure 12a, whereas once the path tracking is stable, i.e., after the vehicle runs after about 5 s, there is no obvious difference between SM and PSO-FSM. Both the algorithms achieve a maximum lateral tracking error less than 3 cm with a standard deviation less than 1 cm. However, the steady-state error of SM in the curve segment is improved significantly by PSO-FSM, where the maximum lateral error is reduced from 32 cm to 3 cm. In order to verify the full-field unmanned driving ability of the proposed algorithm, the full-field path tracking result of the mobile vehicle is shown in Figure 13, and the detail tracking errors are listed in

Table 3. Full-field path tracking error of the mobile vehicle.

Algorithm	Guiding Distance (m)	Maximum Error (cm)	MAE (cm)	RMSE (cm)
SM	12	32	7.4	11.4
PSO-FSM	5	3	1.6	2.7

. As we can see, PSO-FSM not only improves the guiding performance of the autonomous vehicle but also reduces the tracking error significantly. The maximum tracking error of PSO-FSM is less than 3 cm in the full-field path tracking test, which results from the fact that the uncertain disturbances of the mobile vehicle path tracking on a dry pavement is well handled by the cascaded path tracking algorithm.

When analyzing the experiment data, we notice that there is an obvious steady-state error in the straight line segment, which may result from the mechanical clearance of the steering control system and misalignment error between GNSS baseline and the forward direction of the mobile vehicle. In the application of large agricultural machinery, the error coming from the mechanical clearance of the steering control system is negligible and the misalignment error can be further compensated by field calibration [34].

4.3. Combine Harvester Field Test

In order to further verify the fitness of the PSO-FSM algorithm, a field test was performed based on a wheeled combine harvester C230 (provided by John Deere, Moline city, IL, USA) as shown in Figure 14. In the design of the autonomous navigation system of C230, the same dual-antenna RTK position system was employed as the wheeled mobile vehicle, and steering angle sensor, electric steering wheel were installed to set up the close-loop control of self-driving. Electric control ignition, electric control shift, electric control accelerator and electric control clutch were designed to realize unmanned combine harvester. The C230 harvester has an operating width of 5 m and a turning radius of about 8 m. Its navigation system configuration and operation during harvesting operation is consistent with that of the mobile vehicle experiment, and the path planning scheme shown in Figure 7 is employed for the full-field path tracking test.

The whole control system includes an electric steering wheel, angle sensor, electric throttle and control unit related to grain unloading and threshing. The electric steering wheel is Shanghai Lianshi EMS2 steering drive unit, whose working current goes up to 10 A. The advantages of EMS2 include high torque, high precision, IP65 dustproof and waterproof, which is suitable for the harsh operating environment of a combine harvester. The angle sensor is DWQCAB-V-CH from Beijing Tianhaike Company, with a linearity of 0.02%FS and angular resolution of 0.022°. The angle sensor is installed directly above the steering vertical axis of the harvester and is directly connected to the center steering vertical axis. Because of the large operating width of C230, the average operating speeds for a straight line and automatic turn are about 0.8 m/s and 0.6 m/s, respectively, which is almost the same as manual driving operating. Limited by the large turning radius and reliability of manual–automatic gear switching, only the straight line harvesting operation and automatic turning at the outer circle path were performed in the field test.

The result of the unmanned operation of the combine harvester in the farmland is shown in Figure 15, where both the straight line and curved path tracking errors are smaller than 0.5 m if the path tracking algorithm is stable. However, the maximum lateral tracking error of the harvester is 0.63 m due to the harsh field soil environment, such as side slip resulting from field ridge. However, the tracking accuracy is acceptable for the C230 automatic navigation because of its large operation width. Compared with the mobile vehicle test result, we can conclude that the harsh soil condition and state delay of the steering control system have a complicated effect on the parameter tuning of SM, especially for bigger and heavier combine harvesters. Because heading information is only employed to correct the positioning error in this test, the lever-arm error coming from pitch and roll is not compensated, which can be seen from the field ridge-induced tracking error. Furthermore, the lever-arm of the GNSS antenna installed in C230 is obtained manually by measuring tape, and more precise lever-arm and high-frequency attitude information would improve the dual-antenna RTK position, which needs further verification and is also part of our future work.

5. Conclusions

In order to cater for the variation factors of the path tracking algorithm for autonomous agriculture machinery, the PSO-enhanced fuzzy Stanley model (PSO-FSM) is developed and verified. The internal factors of the path tracking control system are handled by using fuzzy algorithm design, and the time-varying external factors, such as actuator saturation and varying speed, are taken into consideration by PSO. Experiments based on numerical simulation and autonomous navigation vehicle are firstly performed, and the result demonstrates that PSO-FSM outperforms FSM and SM in terms of guiding distance under large initial error and path tracking precision under automatic turning. The field test of autonomous navigation vehicle indicates that the full-field tracking errors of PSO-FSM including straight line and curve path tracking are less than 3 cm. Then, a preliminary experiment on harsh farmland soil is performed, and the PSO-FSM achieves maximum tracking error at 0.63 m.

In the future, the dual-antenna RTK positioning system should be enhanced by fusing inertial navigation system, which not only provides the position and attitude at high update frequency, but also enables precise lever-arm identification and reliable seamless navigation. In addition, the calculation of PSO optimalization is based on the fitness functions (8) and (9), if more tracking deviations are provided, the optimal result would be better, especially when the curvature and environment disturbances change randomly. With the increase in the number of control periods, the computational complexity is increased too, and thus running the optimalization on a FPGA chip to speed up the calculation of PSO is also part of our future work.