Path Planning and Tracking Control of Tracked Agricultural Machinery Based on Improved A* and Fuzzy Control

Abstract

In order to improve the efficiency of agricultural machinery operations and reduce production costs, this article proposes a path planning algorithm based on the improved A* algorithm (IA*) and a tracking controller based on fuzzy sliding mode variable structure control (F-SMC) to meet the operation requirements of tracked agricultural machinery. Firstly, we introduce a heuristic function with variable weights, a penalty, and a fifth-order Bezier curve to make the generated path smoother. On this basis, the ant colony algorithm is introduced to further optimize the obtained path. Subsequently, based on fuzzy control theory and sliding mode variable structure control theory, we established a kinematic model for tracked agricultural machinery as the control object, designed a fuzzy sliding mode approaching law, and preprocessed it to reduce the time required for sliding mode control to reach the chosen stage. The simulation experiment of path planning shows that compared with A*, the average reduction rate of the path length for IA* is 5.51%, and the average reduction rate of the number of turning points is 39.01%. The path tracking simulation experiment shows that when the driving speed is set to 0.2 m/s, the adjustment time of the F-SMC controller is reduced by 0.99 s and 1.42 s compared to the FUZZY controller and PID controller, respectively. The variance analysis of the adjustment angle shows that the minimum variance of the F-SMC controller is 0.086, and the error converges to 0, proving that the vehicle trajectory is smoother and ultimately achieves path tracking. The field test results indicate that the path generated by the IA* algorithm can be tracked by the F-SMC controller in the actual environment. Compared to the A* algorithm and FUZZY controller, the path tracking time reduction rate of IA* and F-SMC is 29.34%, and the fuel consumption rate is reduced by 2.75%. This study is aimed at providing a feasible approach for improving the efficiency of tracked agricultural machinery operations, reducing emissions and operating costs.

1. Introduction

In the trend of land transfer, large-scale planting has created conditions for the intelligent development of agricultural machinery [1,2]. The positioning and navigation systems based on the global positioning system, Glonass, Galileo, and Beidou navigation satellite systems are increasingly being applied into precision agriculture [3,4]. The auto drive system is composed of an information perception module, positioning and attitude measurement module, path planning module, and tracking control module. While path planning and tracking control are effective supports for the implementation of autonomous driving in the field of engineering applications [5,6], as a common type of agricultural robot, tracked agricultural machinery can autonomously perceive external environmental information. It can also make decisions and plans for its own posture and path, independently controlling its own behavior during operations [7,8].

In response to the current efficiency requirements and special complex environmental characteristics faced by agricultural machinery operations, path planning technology has developed many methods with a wide range of applications [9]. For static path planning, commonly used path planning algorithms include Rapidly Exploring Random Trees (RRTs) [10], the Probabilistic Roadmap Method (PRM) [11], the Dijkstra algorithm [12], the A* algorithm [13], etc. RRT and PRM belong to the sampling-based methods, which have a stronger search ability when the spatial dimension is high or the size is large. Despite the great computational complexity, there is still significant room for planning efficiency improvements. The Dijkstra algorithm is a typical search planning algorithm, but it has drawbacks such as a large amount of search data, multiple redundant nodes, and inefficient path planning [14].

On the basis of the Dijkstra algorithm, the A* algorithm introduces an evaluation function to determine the search direction, which to some extent improves the efficiency of path planning [15,16]. When global information is known, the A* algorithm can quickly achieve path planning through node state detection and simple estimation. However, this algorithm ignores the motion constraints on the robot, resulting in too many planned path turning points and curvature jumps, which is not in line with kinematic principles [17,18]. In response to the problem of too many turning points in the traditional A* algorithm, Lao et al. [19] optimized the key point selection strategy. Simulation experiments showed that this optimization method is smoother and more efficient compared to the A* algorithm, Dijkstra algorithm, and RRT algorithm. Hu et al. [20] used the fuzzy C-means (FCM) clustering algorithm to cluster obstacles for full coverage tasks containing obstacles. Based on the clustering results, the maximum and minimum values of the horizontal and vertical coordinates of each type of obstacle were calculated; then, the Bresenham algorithm was used to segment regions along the maximum and minimum values of the obstacle boundary; then, the A* algorithm was used to find the optimal path of the subregion; and finally, the reciprocating coverage method was used to achieve subarea coverage operations. Wang et al. [21], based on the A* algorithm, first combined it with the skip search algorithm to reduce computational complexity by replacing nodes in the A* algorithm with skip points. Then, a pruning algorithm was used to prune the obtained path, further removing unnecessary nodes. Finally, they used the quadratic Bezier curve to smooth the path. And the superiority of the improved algorithm was verified through simulation experiments and experiments in actual environments.

However, the above paper only considers path planning in an ideal environment, and the working environment of agricultural machinery is complex. In highly unstructured working environments with variable soil characteristics, agricultural machinery is subject to many uncertain factors. As a result, this poses great difficulties on accurately following the established trajectory for autonomous driving [22,23]. So, it is required that the path tracking controller has certain anti-interference and adaptive capabilities to ensure a high accuracy and stability of agricultural machinery automatic driving [24,25]. At present, scholars have conducted extensive research on control systems for trajectory tracking design of ground robots, including a PID controller [26], fuzzy controller [27], sliding mode controller [28], etc. The PID controller has a simple structure and strong adaptability, so it is often used in agricultural machinery navigation control. But the main problem with the PID controller is parameter tuning for different operating conditions, as well as a difficulty in controlling overshoot and response time. Sliding mode variable structure control is widely used in nonlinear control due to its advantages such as insensitivity to disturbances, fewer adjustment parameters, and a fast response speed. Wu et al. [29] designed a fast terminal sliding mode variable structure control algorithm based on an interference observer for the kinematic model of agricultural machinery, which compensates for the observed interference values to the kinematic model. It has the advantages of fast convergence speed, no overshoot, and high tracking accuracy. However, this study focuses on wheeled agricultural machinery, which exhibits significant differences to tracked agricultural machinery tracking control. To ensure the robustness of the control and improve the path tracking performance of agricultural vehicles, it is necessary to ensure the deflection angle control of the inner and outer tracks based on scientific kinematic models. Jiao et al. [28] proposed an adaptive sliding mode control strategy based on equivalent control and switching control for tracked robots and designed an adaptive integral sliding mode switching function with variable tilt parameters based on the kinematic model of agricultural machinery and motor driving characteristics. This adaptive sliding mode control method has good anti-interference performance, but there may be chattering during the control process. The main drawback of sliding mode control is that the discontinuity of the control can lead to chattering and infinite convergence time.

To sum up, in response to the shortcomings of the traditional A* algorithm, such as multiple turning points and discontinuous sliding mode control, this paper introduces the Bezier curve, variable weight penalty function, and penalty functions into the traditional A* algorithm. Then, a path tracking control law was designed based on sliding mode control theory, and a fuzzy sliding mode variable structure controller was designed using fuzzy mathematics principles to improve the anti-interference ability and path-tracking accuracy of tracked agricultural machinery in complex environments. The content of the remaining chapters of this article is as follows: Section 2 introduces the design concept of the IA*; Section 3 constructs the mathematical model for tracked agricultural machinery; Section 4 designs the F-SMC tracking controller; Section 5 uses simulation experiments to verify the performance of the IA* and F-SMC; Section 6 validates the practicality of the IA* algorithm and F-SMC in real field environments; and Section 7 summarizes the content of this article and introduces our future research directions. Due to the excessive number of terms involved in this article, in order to facilitate browsing, we have added a text box below to introduce their meanings.

2. Design of the IA* Algorithm

2.1. Path Smoothing Method

The A* algorithm is a heuristic search algorithm that is used to find the shortest path in a static environment [30]. The evaluation function of this algorithm is defined as follows:

$$F(n)=G(n)+H(n)$$

(1)

Compared to other path planning methods, the A* algorithm has the advantages of low computational complexity and high accuracy. However, as indicated by Equation (1), the path generated by the A* algorithm is generally a sequence of scattered points, resulting in many turning points and sudden changes in curvature. This path may not necessarily conform to the kinematic principles of the agricultural machinery. Therefore, parametric curves such as Dubins curves [31] and clothoids [32] are used in path planning, as they require less computation. On the other hand, these curves are not practical because they cannot generate closed paths [33]. Moreover, most curve-based path planning techniques rely on the interpolation of reference points, with the resulting curve passing through all reference points. This approach often results in longer paths compared to the Bezier curve, since these curves only pass through the first and last control points [34]. The Bezier curve is used in various fields, such as face recognition [35], medical imaging [36], computer graphics and animation [37], path planning for robots [38,39,40], etc. The fifth-order Bezier curve is formed for the path of the wheeled mobile robot with the constraints of speed, acceleration, and jerk [41]. To address this issue, we use a fifth-order Bezier curve to smooth the path generated by the traditional A* algorithm. The Bezier curve is capable of describing complex polygons [42]. First, we connect the start and end points of the path to form a polygon. Then, we approximate this polygon using the Bezier formula to obtain the parameters of the Bezier curve (as shown in Figure 1). The points that determine the direction of the curve are called control points, and the polygon formed by connecting these points is called the control polygon.

By applying a fifth-order Bezier curve to smooth the path generated by the A* algorithm, the expression of the fifth-order Bezier curve is shown as follows:

$$P(u)={(1-u)}^5{P}_0+5{(1-u)}^{4}u{P}_1+10{(1-u)}^3{u}^2{P}_2+10{(1-u)}^3{P}_3+5{(1-u)}^4{P}_4+u^5{P}_5$$

(2)

Converting Equation (2) into matrix form, we can determine that

$$P=\left(\begin{array}{c}\begin{array}{c}{u}_b^5\\ u_b^4\\ u_b^3\end{array}\\ \begin{array}{c}{u}_b^2\\ ub\\ 1\end{array}\end{array}\right)\times \left(\begin{array}{cc}\begin{array}{ccc}-1& 5& -10\\ 5& -20& 30\\ -10& 30& -30\end{array}& \begin{array}{ccc}10& -5& 1\\ -20& 5& 0\\ 10& 0& 0\end{array}\\ \begin{array}{ccc}10& -20& 10\\ -5& 5& 0\\ 1& 0& 0\end{array}& \begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\end{array}\right)\times \left(\begin{array}{c}\begin{array}{c}P0\\ P1\\ P2\end{array}\\ \begin{array}{c}P3\\ P4\\ P5\end{array}\end{array}\right)$$

(3)

where the constant matrix in the middle represents the coefficient matrix of the Bezier curve.

Based on the properties of the Bezier curve, it can be determined that it has a second-order derivative. Therefore, the curvature k of the Bezier curve at any point can be expressed as follows:

$$k(u_b)=\frac{\left|P^{\prime }(u_b)\times P^{″}(u_b)\right|}{{\left|P^{\prime }(u_b)\right|}^2}$$

(4)

As the path planned by the A* algorithm consists of nodes, we assume that the planned path is

$$P_{\mathrm{path}}=(P_x,P_y)$$

(5)

By using P_path as the control points of the fifth-order Bezier curve and calculating the corresponding Bezier curve, path smoothing can be achieved. Therefore, based on Equation (4), the optimized path curvature C can be calculated as

$$C(u_b)=\raisebox{1ex}{$P_x^{\prime }(u_b)P_y^{″}(u_b)-P_y^{\prime }(u_b)P_x^{″}(u_b)$}\!\left/ \!\raisebox{-1ex}{${‖P_x^{\prime }(u_b)+P_y^{\prime }(u_b)‖}^{3/2}$}\right.$$

(6)

2.2. Introduction of Heuristic Functions and Penalty Functions with Variable Weights

In the traditional A* algorithm, the heuristic function H(n) in Equation (1) is calculated using either Euclidean distance (Equation (7)) or Manhattan distance (Equation (8)).

$$H(n)=\sqrt{{(x_t-x_n)}^2+{(y_t-y_n)}^2}$$

(7)

$$H(n)=\left|x_t-x_n\right|+\left|y_t-y_n\right|$$

(8)

From these equations, it can be observed that both Euclidean distance and Manhattan distance cannot quickly respond to deviations in search direction when there are multiple nodes. Based on the idea of Manhattan distance, this paper uses the difference in distance between the current node and the target node. This serves as a guiding indicator to assign different weights for searching the target node quickly and accurately (as shown in Figure 2). The new evaluation cost calculation method is as follows:

$$H(n)=\left\{\begin{array}{c}\alpha \left|x_t-x_n\right|+\beta \left|y_t-y_n\right|\\ \alpha \left|y_t-y_n\right|+\beta \left|x_t-x_n\right|\end{array}\right., \alpha >\beta$$

(9)

where α and β are constant coefficients.

Due to the presence of obstacles in the environment, the A* algorithm may not strictly follow the shortest distance principle when expanding nodes during path planning. When the environment is complex, A* tends to deviate from the theoretically shortest path during the search. To search faster towards the vicinity of the shortest path, we introduce a penalty function q(n) in the evaluation function. The new evaluation function is as follows:

$$F(n)=G(n)+H(n)+q(n)$$

(10)

When a node deviates from the theoretically shortest path during the search process, a penalty is imposed on that node. The penalty function is as follows:

$$ax+by+c=0$$

(11)

$$q(n)=\gamma \frac{\left|x_n+y_n+c\right|}{\sqrt{a^2+b^2}}$$

(12)

Equation (11) represents the straight line between the start node and the target node, and x and y represent the independent and dependent variables of the equation, respectively. Equation (12) represents the distance between the current node and the theoretically shortest path, and x₀ and y₀ represent the coordinates of the current node.

2.3. Combination with the Ant Colony Algorithm

The ant colony algorithm (ACA) is a biomimetic intelligent algorithm that utilizes its own secreted “pheromones” to transmit information during the foraging process. With the accumulation of search times, the shorter the path, the more ants pass by, and the more pheromones remain on the path [43,44]. The basic principle of the ACA is similar to finding the optimal path for mobile robots. Therefore, it is widely used in path planning. The ACA generally adopts distributed computing, positive feedback parallelism, and heuristic search systems, which have advantages such as strong robustness and easy integration with other algorithms [45,46]. In order to further improve job performance, we use the ant colony algorithm to further optimize the A* algorithm.

Assuming there are ρ points, the agricultural machinery covers them in sequential order $X=(x_1,x_2,\cdots ,x_{\rho })$, and the distance is denoted as V(X). We transform solving the shortest path problem into the TSP (traveling salesman problem), which is to find a sequence of points X that minimizes the distance function V(X). The mathematical expression is as follows:

$$\min V(X)={\displaystyle \sum _{i=1}^{\rho -1}D(x_i},x_{i+1})+D(x_{\rho },x_1)$$

(13)

$$D=\left[\begin{array}{cc}\begin{array}{ccc}\begin{array}{c}\begin{array}{c}0\\ d_{21}\end{array}\\ \begin{array}{c}{d}_{31}\\ ⋮\end{array}\\ \begin{array}{c}{d}_{\rho -1,1}\\ d_{\rho ,1}\end{array}\end{array}& \begin{array}{c}\begin{array}{c}{d}_{12}\\ 0\\ d_{32}\end{array}\\ \begin{array}{c}\\ d_{\rho -1,2}\\ d_{\rho ,2}\end{array}\end{array}& \begin{array}{c}\begin{array}{c}{d}_{13}\\ d_{23}\\ 0\end{array}\\ \begin{array}{c}\\ d_{\rho -1,3}\\ d_{\rho ,3}\end{array}\end{array}\end{array}& \begin{array}{ccc}\begin{array}{c}\begin{array}{c}\dots \\ \dots \end{array}\\ \begin{array}{c}\dots \\ \end{array}\\ \begin{array}{c}\dots \\ \dots \end{array}\end{array}& \begin{array}{c}\begin{array}{c}{d}_{1,\rho -1}\\ d_{2,\rho -1}\\ d_{3,\rho -1}\end{array}\\ \begin{array}{c}\\ 0\\ d_{\rho ,\rho -1}\end{array}\end{array}& \begin{array}{c}\begin{array}{c}{d}_{1,\rho }\\ d_{2,\rho }\\ d_{3,\rho }\end{array}\\ \begin{array}{c}\\ d_{\rho -1,\rho }\\ 0\end{array}\end{array}\end{array}\end{array}\right]$$

(14)

Based on the distance matrix D, we use the ACA to find a sequence of points X that minimizes the distance function V(X). Given the number of ants m in the ant colony and the distance matrix D, we use a taboo table t_k to record the points that are visited by ant k. After one cycle, the taboo table is cleared, and the ants can make another free choice. Each ant determines its state transition probability during movement based on the distribution of pheromones and heuristic information:

$$p_{ij}^k=\left\{\begin{array}{c}{\tau }_{ij}^{\alpha }(t){\eta }_{ij}^{\beta }(t)/{\displaystyle \sum _{s\in ak}{\tau }_{is}^{\alpha }(t){\eta }_{is}^{\beta }(t),\begin{array}{cc}& j\in a_k\end{array}}\\ \begin{array}{ccc}\begin{array}{cc}& \end{array}& & \end{array}0,\begin{array}{cc}\begin{array}{cccc}& & & \end{array}& \mathrm{Others}\end{array}\end{array}\right.$$

(15)

$${\eta }_{ij}(t)=1/d_{ij}$$

(16)

The ants select the next node according to Equations (15) and (16). The length of the movement path is denoted as L_k (k = 1, 2, …, m). The shortest path L_min and the coverage order are recorded, and then $\Delta {\tau }_{ij}$ is reset and the pheromones are updated:

$${\tau }_{ij}(t+1)=(1-\rho 1){\tau }_{ij}(t)+\Delta {\tau }_{ij}(t)$$

(17)

$$\Delta {\tau }_{ij}(t)={\displaystyle \sum _{k=1}^m\Delta {\tau }_{ij}^k}(t)$$

(18)

$$\Delta {\tau }_{ij}^k(t)=\left\{\begin{array}{ll}Q/L_k,& \mathrm{The} k_{\mathrm{th}} \mathrm{ant} \mathrm{passes} \mathrm{through} (i, j)\\ 0,& \mathrm{The} k_{\mathrm{th}} \mathrm{ant} \mathrm{did} \mathrm{not} \mathrm{pass} \mathrm{through} (i, j)\end{array}\right.$$

(19)

when the maximum number of iterations is reached, the algorithm exits the loop, and the optimal coverage path X at that time is output as the solution.

The IA* pseudo code is as follows (Algorithm 1): generate_control_points and generate_bezier_curves are two specific functions that need to be implemented. The former is responsible for generating control points for the fifth-order Bezier curve based on the current node, while the latter uses these control points to generate Bezier curves and ensures that these curves do not collide with any obstacles.

Algorithm 1 IA*

FUCTION IA*(start, goal, obstacles, m, ρ1, α, β) openSet = PriorityQueue() closedSet = Set() cameFrom = Dictionary() gScore = Dictionary() fScore = Dictionary() pheromone = initialize_pheromone(start, goal) openSet.add(start) gScore[start] = 0 fScore[start] = heuristic(start, goal) while openSet is not empty:    current = openSet.extract_min()    if current is goal:       return reconstruct_path(cameFrom, current)    openSet.remove(current)    closedSet.add(current)    controlPoints = generate_control_points(current)    neighbors = generate_bezier_curves(controlPoints, obstacles) for each ant in m:    current = start    path = [current]    while current is not goal:       neighbors = generate_bezier_curves(current, obstacles)       nextNode = choose_next_node(current, neighbors, pheromone, α, β)       path.append(nextNode)       update_pheromone(pheromone, path, ρ1)       current = nextNode    if is_path_better(path):       openSet.clear()       closedSet.clear()       cameFrom.clear()       gScore.clear()       fScore.clear()       openSet.add(start)       gScore[start] = 0       fScore[start] = heuristic(start, goal)       update_pheromone_with_new_path(pheromone, path)    return reconstruct_path(cameFrom, goal) function choose_next_node(current, neighbors, pheromone, α, β)    probabilities = calculate_probabilities(neighbors, pheromone, α, β)    return select_node_by_roulette_wheel(probabilities) function update_pheromone(pheromone, path, ρ1)    for each edge in path       pheromone[edge] *= (1 − ρ1)       pheromone[edge] += pheromone_deposit function is_path_better(path)    return betterPathFound

This chapter designs the IA* algorithm. Firstly, we introduce a heuristic function with variable weights in the traditional A* algorithm, using the difference in distance between the current node and the target node as the guiding indicator. By assigning different weights, it is guided to quickly and accurately search for the target node. In order to quickly search for nodes around the shortest route, we introduce a penalty function in the evaluation function. We introduce a fifth-order Bezier curve to make the generated path smoother based on the Manhattan distance concept. The purpose is to overcome the shortcomings of the A* algorithm having too many turning points and not complying with the kinematic principles of tracked agricultural machinery. On this basis, the ant colony algorithm is introduced to further optimize the obtained path.

3. Tracked Agricultural Machinery Model

The soil characteristics in the working environment of the agricultural machinery are complex. The design of the tracking controller is based on the geometric relationship between the tracked agricultural machinery body and the desired path. The tracked agricultural machinery generally adopts differential steering, which means that steering is achieved by the speed difference of the tracks, allowing the agricultural machinery to maneuver flexibly. During steering, there is a phenomenon of rotation in the inner track [28]. Based on the above analysis, we have established the kinematic model as shown in Figure 3.

As shown in Figure 3, xOy represents the Cartesian coordinate system. The line on which AB lies (denoted as H) is the desired path; CD represents the inner track, with C being the front point of the inner track. EF represents the outer track. C′D′ and E′F′ represent the tracks before adjustment. M is the centroid of mass of the fuselage; O_L and O_R are the centers of the inner and outer tracks, respectively. φ represents the angle between the current velocity direction and the desired path, which is the desired value of the heading angle. From the geometric relationship in Figure 3, we have that

$$\left\{\begin{array}{l}\left|{\alpha }_R\right|=\left|{\beta }_R\right|=\arctan \frac{L_1}{2R+L_2}\\ \left|{\alpha }_L\right|=\left|{\beta }_L\right|=\arctan \frac{L_1}{2R-L_2}\end{array}\right.$$

(20)

From Equation (20), we know that the deviation angle of the tracks is determined by R, L₁, and L₂.

Assuming that the linear velocity of the agricultural machinery is V, we establish the kinematic equation:

$$\left\{\begin{array}{l}\stackrel{•}{x}=\stackrel{•}{l_1\sin \phi =}\stackrel{•}{l_1}\sin \phi +l_1\cos \phi =V\sin \phi \\ \stackrel{•}{\mathrm{y}}=\stackrel{•}{l_1\cos \phi =}\stackrel{•}{l_1}\sin \phi -l_1\cos \phi =V\cos \phi \end{array}\right.$$

(21)

The real-time pose of the fuselage is represented as follows:

$$\begin{array}{l}{x}_t=x_M+{\displaystyle {\int }_{t0}^t}{\stackrel{.}{x}}_{t}dt\\ y_t=y_M+{\displaystyle {\int }_{t0}^t{\stackrel{.}{y}}_{t}dt}\\ {\phi }_d={\phi }_0+{\displaystyle {\int }_{t0}^t{\stackrel{.}{\phi }}_{d}dt}\end{array}$$

(22)

As the inner and outer tracks of the agricultural machinery move in opposite directions during turning, we assume that the deviation angles of C and E are x₁ and x₂ in radians, respectively. The state equations are as follows:

$$\left\{\begin{array}{l}\stackrel{•}{x_1}=\stackrel{•}{{\theta }_r}=\frac{2V\sin {\alpha }_L}{L_2}=\frac{2V{L}_1}{\sqrt{L_2^4-4L_2^{3}R+(4R^2+L_1)L_2^2}}\\ \stackrel{•}{x_2}=\stackrel{•}{{\theta }_f}=\frac{2V\cos {\alpha }_R}{L_1}=\frac{2V{L}_1}{\sqrt{L_2^4+4L_2^{3}R+(4R^2+L_1)L_2^2}}\end{array}\right.$$

(23)

The linear velocity of the fuselage should be the vector sum of the linear velocities of the inner and outer tracks. According to the physical relationship between linear velocity and angular velocity, we have that

$${\dot{x}}_1=v_1=\frac{{\theta }_r}{\Delta t}·\frac{L_1}{2}$$

(24)

$${\dot{x}}_2=v_2=\frac{{\theta }_f}{\Delta t}·\frac{L_1}{2}$$

(25)

$$V=\frac{({\theta }_r+{\theta }_f)L_1^2}{4\Delta t}$$

(26)

Based on the second law of Newton, we can derive that

$$V=\frac{d\sqrt{X_t^2+Y_t^2}}{dt}=({\dot{x}}_1+{\dot{x}}_2)\frac{L_1}{2}$$

(27)

This chapter constructs a kinematic model for tracked agricultural machinery and then designs the expected deflection angle polynomials for the inner and outer tracks, with time as the variable.

4. Design of F-SMC

4.1. Design of Position Control Law

To ensure a safe and stable tracking path of agricultural machinery, a control system is designed with the input of the inner and outer track deviation angles as follows:

$$\dot{x}=f(x,t)+Bu(t)$$

(28)

$$x={(x_1,x_2)}^{\mathrm{T}}$$

(29)

From Figure 3, it can be seen that the adjustment angle of the fuselage center of mass is the vector sum of the inner and outer track deviation angles. The control equations for the inner and outer tracks are as follows:

$$\left\{\begin{array}{c}{\theta }_R={\theta }_r+{\displaystyle \underset{t0}{\overset{t}{\int }}\epsilon \mathrm{dt}}\\ {\theta }_F={\theta }_f+{\displaystyle \underset{t0}{\overset{t}{\int }}\kappa \mathrm{dt}}\end{array}\right.$$

(30)

The error tracking equation is

$$\left\{\begin{array}{c}{\dot{x}}_e=V\cos \phi -{\dot{x}}_M\\ {\dot{y}}_e=V\sin \phi -{\dot{y}}_M\end{array}\right.$$

(31)

where s₁ = x_e = x_t − x_M, s₂ = y_e = y_t − y_M. In this case, we assume that $V_{\cos \phi }$ = k₁u₁ and $V_{\sin \phi }$ = k₂u₂. From this, we can obtain the desired trajectory as

$$\phi =\arctan \frac{k_1{u}_1}{k_2{u}_2}=\arctan \frac{x_e-x_M}{y_e-y_M}$$

(32)

When $\phi$ = ${\phi }_d$, the tracking control is completed, and the agricultural machinery reaches the desired position.

4.2. Design of Posture Control Law

After designing the position control law, we further design the attitude control law to achieve tracking of the desired angles. We design the sliding mode function s₃ = $\phi$_e= $\phi$ − $\phi$_d, from which it can be obtained that

$${\dot{s}}_3={\dot{\phi }}_e=\omega -{\dot{\phi }}_d$$

(33)

Based on this, the attitude control law is

$$\omega ={\dot{\phi }}_d-k_3\cdot s_3-{\eta }_3·\mathrm{sgn}{s}_3$$

(34)

where k₃ > 0, ${\eta }_3>0$ (both k₃ and ${\eta }_3$ are constant coefficients).

From this, we can obtain that

$${\dot{s}}_3=-k_3{s}_3-{\eta }_3\mathrm{sgn}{s}_3$$

(35)

Taking the Lyapunov function $V=\frac{1}{2}{s}_3^2$, we have that

$$\dot{V}=s_3·{\dot{s}}_3=-k_3{s}_3^2-{\eta }_3\left|s_3\right|\le -k_3{s}_3^2$$

(36)

From this, we can obtain that

$$\dot{V}\le -2k_{3}V$$

(37)

From this, it can be seen that $\phi$_e converges to 0, i.e., $\phi$ − $\phi$_d → 0. Therefore, the deviation linear velocity of the tracks converges, proving that the agricultural machinery can achieve attitude adjustment and path tracking.

4.3. Fuzzy Approximation Sliding Mode Attitude Control

In order to improve the performance of the control system, we design a fuzzy control based on the attitude control law to improve the speed, with the control system converging to the origin. In this paper, parameters $\epsilon$ and $\kappa$ are designed. We take the derivative of the vector sum of the adjusted angular velocity of the inner and outer tracks, which is the attitude angular velocity of the tracked machinery. The fuzzy exponential approach law of $\omega$ is set as follows:

$$\dot{s}(x)={\dot{\phi }}_d-{\eta }_1\epsilon -{\eta }_2\kappa s$$

(38)

where $\epsilon$ and $\kappa$ are determined by fuzzy mathematical methods; the constant coefficients ${\eta }_1$ and ${\eta }_2$ are selected based on the motion characteristics of the tracked agricultural machinery. The fuzzy approach is used to normalize the switching function s. The input of the fuzzy sliding mode variable structure controller is defined as

$$s_n=\lambda s$$

(39)

where n is 1, 2, 3; $\epsilon$ and $\kappa$ are the output of the fuzzy controller. We define the linguistic values of s_n, $\epsilon$, and $\kappa$ as {$\ddot{A_{-3}}$, $\ddot{A_{-2}}$, $\ddot{A_{-1}}$, $\ddot{A_0}$, $\ddot{A_1}$, $\ddot{A_2}$, $\ddot{A_3}$}, {$\ddot{B_{-3}}$, $\ddot{B_{-2}}$, $\ddot{B_{-1}}$, $\ddot{B_0}$, $\ddot{B_1}$, $\ddot{B_2}$, $\ddot{B_3}$}, and {$\ddot{C_1}$, $\ddot{C_2}$, $\ddot{C_3}$}, respectively. The pseudo code for the fuzzy control rules is as follows (Algorithm 2):

Algorithm 2 Fuzzy control rules

$R\epsilon$: if sn is $\ddot{B}$ then $\epsilon$ is

$\ddot{B_{-i}}$ (i = −3, −2, −1, 0, 1, 2, 3)

Input sn is $u=-{\left(CB\right)}^{-1}(-{\eta }_1\epsilon +-{\eta }_{2}ks+C\widehat{f})$ then

Output $\epsilon$ is $\ddot{C_3}$

Rk: if sn is $\ddot{A}$−3,−2,−1 then $\kappa$ is $\ddot{C_3}$

if sn is $\ddot{A_0}$ then $\kappa$ is $\ddot{C_2}$

if sn is $\ddot{A}$1,2,3 then $\kappa$ is $\ddot{C_1}$

Input sn is $\ddot{A}$ then

Output $\kappa$ is $\ddot{C}$

According to the control rules, the maximum value principle is used to convert the fuzzy controller outputs $\epsilon$ and $\kappa$ into precise control variables. The calculation method is as follows:

$$\left\{\begin{array}{l}\epsilon ={\displaystyle \underset{-3/4}{\overset{3/4}{\int }}\epsilon \stackrel{\sim }{B}}(\epsilon )d\epsilon /{\displaystyle \underset{3/4}{\overset{-3/4}{\int }}\epsilon \stackrel{\sim }{B}}(\epsilon )d\epsilon \\ \kappa ={\displaystyle \underset{0}{\overset{3/4}{\int }}\kappa \stackrel{\sim }{C}}(\kappa )d\kappa /{\displaystyle \underset{0}{\overset{3/4}{\int }}\stackrel{\sim }{C}}(\kappa )d\kappa \end{array}\right.$$

(40)

Therefore, substituting $\epsilon and \kappa$ into the fuzzy approximation law, we obtain the following:

$$u=-{(CB)}^{-1}(-{\eta }_1\epsilon +{\eta }_2\kappa s+C\hat{f})$$

(41)

This chapter introduced the design of the F-SMC tracking controller. Based on fuzzy control ideas and sliding mode variable structure control theory, we designed a fuzzy sliding mode variable structure path tracking controller. We established a kinematic model using tracked agricultural machinery as the control object and designed the expected deflection angle polynomial of the inner and outer tracks with time as the variable. We designed a sliding mode function using the expected deflection angle and derived the attitude control law for angular velocity, as well as proved that the sliding mode function converges to 0 through the Lyapunov theorem. Next, we designed a fuzzy sliding mode reaching law and preprocessed it to reduce the time for sliding mode control to reach the next stage.

5. Simulation Test

5.1. Path Planning Simulation Experiment

In order to explore the values of $\alpha , \beta$, and $\gamma$, we first conducted a preliminary experiment. We set $\alpha$:$\beta$ = {2:1, 3:1, 4:1, 5:1, 3:2, 4:3, 5:2, 5:3, 5:4}. The results show that the algorithm has the best performance when $\alpha$:$\beta$ = 5:4. Next, we set $\alpha$:$\beta$ = 5:4, $\gamma$ = {0.1, 0.2,…, 0.9}. The results show that when $\gamma$ = 0.3, the algorithm has the best performance. Therefore, our final determined values are $\alpha$ = 1,$\beta$ = 0.8, and $\gamma$ = 0.3.

To verify the performance of the IA* algorithm, we compare the A* algorithm with it. We designed three grid environments (denoted as scenes 1~3). They simulate transported agricultural machinery for small warehouses, large warehouses, and standard orchards, respectively. The configuration environment of the computer is MATLAB R2022a, Intel Core i7-12700H CPU, 2.30 GHz main frequency, 32 GB memory, and Windows 11 operating system. Among them, the black grid represents obstacles (when the obstacle is less than one grid, expand the obstacle to fill the grid), the white grid represents feasible areas, the green grid represents the starting point, and the yellow grid represents the ending point, with a resolution of 1 m × 1 m. The simulation results are shown in Figure 4, Figure 5 and Figure 6.

In order to visually compare the A* algorithm with the IA* algorithm, we recorded their planned path length, runtime, and number of turning points. The results are shown in

Table 1. Comparison of path planning simulation test data.

Scene	Algorithm	Length [m]	Reduction Rate [%]	Runtime [s]	Number of Turning Points [pc]	Reduction Rate [%]
1	A*	48.96	5.58	1.647	8	62.50
	IA*	46.23		1.648	3
2	A*	95.10	5.36	2.173	11	36.36
	IA*	90.00		2.176	7
3	A*	33.28	5.59	1.660	11	18.18
	IA*	31.42		1.661	9

.

From Figure 4, Figure 5 and Figure 6 and Table 1, it can be seen that both the A* algorithm and the IA* algorithm can plan a path from the starting point to the ending point, but there are differences in path length, calculation time, and path continuity. In all three scenarios, the path length calculated by the IA* algorithm was smaller than A*, with reduction values of 2.73 m, 5.1 m, and 1.86 m, respectively. The reduction rates were 5.58%, 5.36%, and 5.59%, with an average reduction rate of 5.51%. The IA* algorithm has a computing time that is 0.001 s, 0.003 s, and 0.001 s longer than the A* algorithm, which is far less than the shortest time that human vision can perceive. In terms of turning points, IA* has reduced the number of turning points by five, four, and two, respectively, compared to A*, with a reduction rate of 62.50%, 36.36%, and 18.18% and an average reduction rate of 39.01%. From the figures, it can be seen that the two curves planned by the A* algorithm only have equal coordinate values at their coincident points. It is indicated that the zero-order parameters are continuous. This leads to the need for agricultural machinery to first reduce its speed to 0 and then turn before moving forward when encountering a turning point during the forward-moving process. The path planned by the IA* algorithm not only satisfies the continuity of zero-order parameters at the intersection point of two curves, but also satisfies the continuity of first-order and second-order parameters. Therefore, agricultural machinery can maintain a constant speed or acceleration when turning. Based on the data, it can be seen that the IA* algorithm always outperforms the A* algorithm in path length and smoothness planning, which can reduce the time and energy consumption of agricultural machinery during path tracking.

5.2. Path Tracking Simulation Experiment

In this paper, MATLAB/SIMULINK is used as the simulation platform to verify the performance of the F-SMC controller. The tracking control of the U-turn path for a tracked agricultural machinery is conducted under the condition of no disturbance (the parameters of the control system are shown in

Table 2. F-SMC control system parameters.

Parameter	V [m/s]	L1 [m]	L2 [m]	${\mathit{\theta }}_{\mathit{r}}$ [°]	${\mathit{\theta }}_{\mathit{f}}$ [°]	Start	End	k1	k2	$\mathit{\epsilon }$	$\mathit{\kappa }$
Data	0.50	1.04	0.68	0	0	(1, −10)	(1, 10)	1	1	1	1

). We compare the fuzzy controller (FUZZY), PID controller, and the proposed F-SMC controller in this paper. Among them, the parameters required by the FUZZY controller are the same as those of the F-SMC. We referred to reference [47] for the parameters for the PID controller. We designed the control system structure diagram of F-SMC (as shown in Figure 7). According to Figure 7, we establish a corresponding controller model in SIMULINK that combines the fuzzy control front-end and sliding mode variable structure control at the back end, as shown in Figure 8. In Figure 8, X12 (the expected deflection angle of the inner and outer tracks) is input to S1 (the input terminal of the F-SMC controller). After being processed by the FUZZY, the output parameters are combined to the S2 (position controller) to calculate the thd (position parameter). The thd is input to the S3 (differential controller) for differential processing and output as dthd. After being processed by the S4 (attitude controller), the ut (preadjusted deflection angle) is output. After comparing with the S5 (kinematic model of the controlled object, a tracked lawn mower), the feedback signal is output to FUZZY. The F-SMC controller integrates the S1 and S5 to output phi (the actual adjustment angle). The F-SMC controller operates continuously during the time period t.

Figure 9 shows the comparison of path tracking. From Figure 9, it can be seen that the tracked agricultural machinery meets greater difficulty in path tracking in the curved section, specifically from coordinates (25, −10) to (32, 0). To further compare the control accuracy of each controller, we sampled the deviation angles of each controller in this section, as shown in Figure 10 and

Table 3. Deviation angle sampling error comparison table.

Controller	Sample Point [pc]	Total Error [°]	Average [°]	Variance [°]
F-SMC	700	−41.313	−0.059	0.086
Fuzzy	700	−60.201	−0.086	0.094
PID	700	−194.60	−0.278	0.276

. To explore the dispersion of each sampling point further, we set φ_e to 0 and plotted a boxplot, as shown in Figure 11.

From Figure 9 and Figure 10, compared to the FUZZY and PID controllers, it can be observed that the F-SMC controller has the smallest deviation, with the closest tracking path to the desired value. In Figure 10, within the sampling range of [200, 600], the F-SMC controller requires the fewest number of samples to adjust the deviation angle back to 0, with an average of 192 samples. The FUZZY and PID controllers require 291 and 334 samples, respectively. This indicates that during the path tracking process, the F-SMC controller can quickly calculate the deviation between the actual path and the desired path, and it can complete the task using fewer samples, improving the efficiency of path tracking and reducing the response time. From Figure 11, it can be seen that the total adjustment time for turning is 1.92 s, 2.91 s, and 3.34 s for the F-SMC, FUZZY, and PID controllers, respectively. The F-SMC controller has the shortest response time, resulting in efficiency improvements of 34.02% and 42.51% compared to the FUZZY and PID controllers, respectively.

In this chapter, we validated the superiority of IA* and F-SMC compared to other classic methods through simulation experiments on computers.

6. Field Experiment

6.1. Materials and Methods

To further verify the feasibility of IA* and F-SMC in real environments, we selected the G33 lawn mower produced by Wuxi Kalman Navigation Technology Co., Ltd. (Wuxi, China) for path planning and tracking experiments (as shown in Figure 12). The mechanical and structural parameters of G33 are shown in

Table 4. Field test results.

Parameter	Size [m]	L1 [m]	L2 [m]	R [m]	V1 [m/s]	V2 [m/s]	$\mathit{\omega }$ [rad/s]
Data	1.07 × 0.98 × 0.44	0.60	0.75	2	1.5	1.5	0.2

. The experimental environment is the Shunnong Fruit Modern Agricultural Park in Shunping County, Baoding City, Hebei Province.

Due to the influence of various factors such as equipment performance, orchard road conditions, and signal quality during the experimental process, the actual walking trajectory of the lawn mower deviates greatly from the theoretical calculation. Therefore, the length of the path in this section is not used as an indicator to evaluate the efficiency of the operation. In addition to path length, the total operation time (T) and mowing oil consumption (G) are also intuitive efficiency evaluation indicators. Among them, T is timed by a timer, and G is calculated as follows:

$$G=\frac{H_1-H_2}{H_3}\times 100\%$$

(42)

In order to observe the chattering suppression effect of F-SMC, we randomly selected 500 sampling points during the experiment. The vibration frequency of these sampling points was measured using a vibration sensor.

6.2. Result and Analysis

We conducted a field experiment on 2 August 2023, with the results shown in Figure 13 and Figure 14. We used the radar carried by the lawn mower to create a map of the environment, and the mapping process was completed using the rviz platform in the ubunto system. In order to make the comparison effect obvious, we used the FUZZY controller to track the path generated by A* (blue line in Figure 13) and used the F-SMC controller to track the path generated by IA* (yellow line in Figure 13). Figure 14 shows the vibration suppression effect of FUZZY and F-SMC. In order to compare the vibration suppression effects of the two controllers more intuitively, we have drawn a comparison chart of vibration suppression errors (as shown in Figure 15). The experimental results are shown in

Table 5. Field test results.

Method	T [s]	Reduction Rate [%]	G [%]	Number of Sampling Points [pc]	Mean Vibration Frequency [Hz]
A* and FUZZY	893	29.34	6.30	500	36.76
IA* and F-SMC	631		3.55	500	16.93

.

From Figure 13 and Table 5, it can be seen that the IA* algorithm is practical with F-SMC, and the paths generated by IA* can normally be tracked by F-SMC. Compared to A* and FUZZY, IA* and F-SMC has a path tracking time reduction rate of 29.34% and a fuel consumption reduction rate of 2.75%. From Figure 14 and Figure 15, it can be seen that compared with the FUZZY controller, $\epsilon$ and $\kappa$ can improve the accuracy of the ${\theta }_R$ and ${\theta }_F$, reduce the computational workload of the controller’s back end, shorten the adjustment time of F-SMC, and accelerate the tracking of the desired path of tracked agricultural machinery. When using the FUZZY controller to track the path, the average vibration frequency of the tracked agricultural machine is 16.93 Hz. When using the F-SMC controller to track the path, the average vibration frequency of the tracked agricultural machine is 36.76 Hz. F-SMC has a better vibration suppression effect.

In this chapter, we transplanted IA* and F-SMC to the control system of the G33 lawn mower, verifying their practicality in real-world environments.

7. Conclusions

In order to improve the efficiency of agricultural operations to reduce the cost of agricultural products, this paper proposes an IA* path planning algorithm. Meanwhile, it recommends an F-SMC path tracking controller that is characterized by tracked agricultural machinery operations.

• We introduce a heuristic function with variable weights in the traditional A* algorithm, taking the difference in distance between the current node and the target node as the guiding indicator. By assigning different weights, we guide it to quickly and accurately search for the target node. In order to quickly search for nodes that are near the shortest route, we introduce a penalty function in the evaluation function. In order to overcome the shortcomings of the A* algorithm, which has too many turning points that do not comply with the kinematic principles of tracked agricultural machinery, and based on the Manhattan distance concept, we introduced a fifth-order Bezier to make the generated path smoother. On this basis, the ACA is introduced to further optimize the obtained path.
• Based on fuzzy control theory and sliding mode variable structure control theory, we have designed an F-SMC controller. We established a kinematic model using tracked agricultural machinery as the control object and designed the expected deflection angle polynomial of the inner and outer tracks with time as the variable. The sliding mode function is designed using the expected deflection angle, and the attitude control law for angular velocity is derived. The convergence of the sliding mode function to 0 is proved through the Lyapunov theorem. Next, we designed a fuzzy sliding mode reaching law and preprocessed it to reduce the time required for the sliding mode control to reach the chosen stage.
• We conducted simulation experiments and field experiments. The simulation test results show that compared with A*, the average reduction rate of the path length for IA* is 5.51%, and the average reduction rate of the number of turning points is 39.01%. Compared with the FUZZY controller and PID controller, the adjustment time of F-SMC is reduced by 0.99 s and 1.42 s, respectively. The field test results show that compared to the A* algorithm and FUZZY controller, the path tracking time reduction rate of IA* and F-SMC is 29.34%, and the fuel consumption rate is reduced by 2.75%.

The IA* and F-SMC proposed in this article belong to path planning and path tracking technology, but the functions of the algorithm are limited. The application scenarios of agricultural machinery are complex. Highly intelligent agricultural machinery should have real-time modeling technology to cope with different scenarios. Therefore, we will carry out environmental modeling work in the future.