Research on an Intelligent Agricultural Machinery Unmanned Driving System

Abstract

Intelligent agricultural machinery refers to machinery that can independently complete tasks in the field, which has great significance for the transformation of agricultural modernization. However, most of the existing research on intelligent agricultural machinery is limited to unilateral research on positioning, planning, and control, and has not organically combined the three to form a fully functional intelligent agricultural machinery system. Based on this, this article has developed an intelligent agricultural machinery system that integrates positioning, planning, and control. In response to the problem of large positioning errors in the large range of plane anchoring longitude and latitude, this article integrates geographic factors such as ellipsoid ratio, long and short axis radius, and altitude into coordinate transformation, and combines RTK/INS integrated inertial navigation to achieve precise positioning of the entire vehicle over a large range. In response to the problem that existing full-coverage path planning algorithms only focus on job coverage as the optimization objective and cannot achieve path optimization, this paper adopted a multi-objectivefunction-coupled full-coverage path planning algorithm that integrates three optimization objectives: job coverage, job path length, and job path quantity. This algorithm achieves optimal path planning while ensuring job coverage. As the existing pure pursuit algorithm is not suitable for the motion control of tracked mobile machinery, this paper reconstructs the existing pure pursuit algorithm based on the kinematics characteristics of tracked mobile machinery, and adds a linear interpolation module, so that the actual tracking path points of motion control are always ideal tracking path points, effectively improving the motion control accuracy and control stability. Finally, the feasibility of the intelligent agricultural machinery system was demonstrated through corresponding simulation and actual vehicle experiments. This intelligent agricultural machinery system can cooperate with various operating tools and independently complete the vast majority of agricultural production activities.

1. Introduction

With the continuous development of technology and the intensification of population aging, countries around the world are beginning to undergo agricultural modernization transformation. Agriculture 4.0 utilizes a series of emerging technologies to upgrade traditional production methods and world agricultural strategies into optimized value chains, enhancing disruptive solutions at all stages of the agricultural production chain [1]. Agricultural machinery, as a powerful driving force for agricultural modernization, has great significance in achieving intelligent agricultural machinery. Compared to traditional agricultural machinery, intelligent agricultural machinery has the advantage of effectively improving work quality and efficiency, and reducing labor costs [2,3,4,5,6,7].

The agricultural production process generally includes four stages: cultivation, planting, management, and harvesting. The key to achieving intelligent mechanized operations in these four stages is to achieve autonomous full-coverage path planning and walking of corresponding agricultural machinery. Based on this, this article takes tracked mobile machinery as the research object, committed to achieving its autonomous full-coverage path planning and walking in real fields. On the basis of achieving autonomous full-coverage path planning and walking of tracked mobile machinery, different agricultural production automation functions can be achieved with the assistance of different operation tools.

It is generally believed that unmanned agricultural machinery mainly includes three modules: vehicle positioning, full-coverage path planning, and motion control [8]. For the positioning module of unmanned agricultural machinery, European and American countries began relevant research as early as the mid-20th century [9,10,11]. At that time, there were mainly two vehicle positioning methods: cable electromagnetic induction [12,13,14] and setting markings on farmland boundaries [15,16]. Both methods required renovation of farmland, which was costly and difficult to deploy and apply on a large scale. After the 1980s, with the continuous development of technology, satellite positioning began to be applied to agricultural machinery navigation. O’Conner et al. [17,18] implemented automatic navigation of agricultural machinery based on a four-antenna GPS system, but the system did not use carrier phase difference technology, resulting in low positioning accuracy and low positioning frequency that could not meet the needs of motion control. Gerrish et al. [19] achieved the positioning of agricultural machinery vehicles based on machine vision, but machine vision positioning is susceptible to environmental factors such as lighting and weather, resulting in low positioning robustness. Pilarski et al. [20] fused the camera with RTK-GPS, which interpolated through machine vision positioning information to compensate for the limited transmission frequency of RTK-GPS. They also fused the positioning information of the two, effectively improving positioning accuracy. However, high positioning accuracy can only be achieved when environmental factors such as lighting and weather meet the requirements of machine vision positioning.

Full-coverage path planning is a crucial part of achieving intelligent agricultural machinery, and the generated path directly affects the accuracy and efficiency of agricultural machinery vehicle motion control. Jin et al. [21] divided complex farmland into several sub-regions and generated a complex farmland full-coverage path by solving the full-coverage paths of the sub-regions. However, the generated path was not the optimal path, which seriously affected the efficiency of agricultural machinery work. Fabre et al. [22] used the greedy algorithm as the objective function for farmland full-coverage path planning, which to some extent solved the problem of high repetition rate in full-coverage path planning, but did not take into account factors such as farmland coverage rate and generated path length. Yang et al. [23] also used the task repetition rate as the objective function and convolutional neural networks for full-coverage path planning of farmland. However, this method did not take into account factors such as farmland coverage rate and generated path length, and the convolutional neural network is a black box model, resulting in uncertainty in the generated target path.

At present, commonly used motion control algorithms include motion control algorithms based on kinematic models of mobile machinery, motion control algorithms based on dynamic models of mobile machinery, and motion control algorithms independent of kinematic and dynamic models [24]. Agricultural machinery generally travels at a relatively slow speed, and the lateral force and tire slip to which it is subjected can be almost ignored. Therefore, motion control algorithms based on the kinematic model of mobile machinery are commonly used to control the entire vehicle motion of agricultural machinery [25]. O’Connor et al. [26] simplified the kinematic model of agricultural machinery to a two-wheeled vehicle model, constructed corresponding state equations, and designed a corresponding linear optimal controller. However, the control effect of this optimal controller depends on the measurement accuracy of position deviation, heading deviation, and front-wheel angle. Li et al. [27] used lateral and longitudinal errors as state variables and designed a linear quadratic regulator to control it. The control system is not affected by the speed of the agricultural machinery vehicle, but it is prone to oscillations when the steering system time constant is too large. Model predictive control is a motion control algorithm based on the kinematic or dynamic model of the entire vehicle, which determines the current optimal control quantity by predicting the future state in a certain time domain. Its control accuracy is closely related to the accuracy of the established model, and the calculation of model predictive control is relatively complex, which cannot meet the real-time requirements of the entire vehicle motion control. The University of Magdeburg in Germany designed a model predictive controller for dynamic modeling of specific agricultural machinery. However, due to the complexity of the calculation of the model predictive controller, it was unable to meet the real-time requirements of motion control. Therefore, only simulation experiments were conducted [28].

In summary, there are two main methods for positioning agricultural machinery vehicles: machine vision positioning and satellite positioning. Machine vision positioning is susceptible to environmental factors such as light and weather, and is unstable. However, satellite positioning frequency is too low and single GPS antenna positioning can only achieve sub-meter-level positioning. Based on this, this article adopts RTK/INS integrated inertial navigation fusion positioning, supplemented by a high-precision coordinate transformation algorithm, to achieve high-frequency centimeter-level positioning of the entire vehicle in the local Cartesian coordinate system. In response to the problem of existing full-coverage path algorithms only considering job coverage or job repetition rate, this paper adopted a multi-objective function-coupled full-coverage path planning algorithm [29] that integrates three optimization objectives: job coverage, job path length, and job path quantity. Mier et al. proposed this algorithm after fully analyzing the shortcomings of existing full coverage path planning. Considering the excellent performance of the path planning algorithm, this article adopts this algorithm as the path planning algorithm. This algorithm achieves the optimal path planning while ensuring job coverage. Therefore, the intelligent agricultural machinery system in this article adopts the Fields2Cover library developed by Mier et al. [30] to achieve full coverage path planning. In response to the difficulties in tuning existing PID control parameters and the poor real-time performance of model predictive control, this paper reconstructs the existing pure pursuit algorithm based on the kinematic model of tracked agricultural machinery, supplemented by a linear interpolation module, so that the path points tracked by the entire vehicle are all ideal path points, effectively improving the accuracy and stability of vehicle motion control. Based on the three major technologies, namely, fusion positioning, full-coverage path planning, and motion control, an intelligent unmanned driving system for agricultural machinery is constructed. The simulation and real vehicle test platforms are built using tracked agricultural machinery as a prototype for relevant experiments. The simulation and actual vehicle experiment results prove that the intelligent agricultural machinery unmanned driving system constructed in this paper has precise positioning, high control accuracy, and can effectively complete field full-coverage path tracking, laying a certain foundation for the development of intelligent agricultural machinery unmanned-driving-related technologies in the future.

2. RTK/INS Fusion Positioning Coordinate Transformation

Due to the susceptibility of machine vision positioning to environmental factors such as lighting and weather, this article adopts satellite positioning. Due to the fact that single line GPS can only achieve sub-meter-level positioning accuracy and low positioning frequency, this article adopts carrier phase differential GPS technology (RTK), supplemented by inertial navigation system (INS) interpolation, to achieve high-frequency and high-precision positioning.

The longitude and latitude altitude coordinates obtained from the RTK/INS integrated inertial navigation system are in the WGS-84 coordinate system and cannot be directly applied to subsequent path planning and motion control. Therefore, coordinate transformation is necessary to convert the longitude and latitude altitude coordinates into Cartesian coordinates. The commonly used coordinate transformation method is the plane vector anchoring method, which constructs two sets of vectors in the WGS-84 coordinate system and the local coordinate system defined by the vehicle for plane coordinate transformation based on the principle of the same length ratio and angle between two sets of vectors in different linear transformation coordinate systems. This method is simple and efficient, and only needs to complete anchoring through three sets of longitude and latitude altitude coordinates and their corresponding plane coordinates in the local coordinate system of the vehicle. However, this method treats the Earth’s surface as a plane and does not consider parameters such as the altitude of the vehicle and the eccentricity of the Earth’s ellipsoid, which can result in significant errors during large-scale positioning.

Based on this, this article adopts a new coordinate transformation method, as shown in Figure 1. This transformation method takes into account parameters such as the current altitude of the entire vehicle, the eccentricity of the Earth’s ellipsoid, and the radius of the Earth’s major and minor axes. The longitude and latitude coordinates of the WGS-84 coordinate system are converted into Cartesian coordinates in the Earth-Centered Earth-Fixed coordinate system (ECEF), and then the Cartesian coordinates in the ECEF coordinate system are converted into Cartesian coordinates in the local Cartesian coordinates coordinate system (ENU) to achieve vehicle positioning. The derivation of this method is as follows.

Calculate the eccentricity of the Earth ellipsoid and the curvature radius of the reference ellipsoid corresponding to the current vehicle position, as shown in Equations (1) and (2):

$$e^2=\frac{a^2-b^2}{a^2}$$

(1)

$$N=\frac{a}{\sqrt{1-e^2{sin}^2\left(lat\right)}}$$

(2)

among them, e is the eccentricity of the Earth, a is the long radius of the reference Earth, b is the short radius of the reference Earth, N is the curvature radius of the reference Earth corresponding to the current position of the entire vehicle, and $lat$ is the latitude information in the longitude and latitude altitude coordinates of the current vehicle located in the WGS-84 coordinate system.

By using the eccentricity of the Earth ellipsoid and the curvature radius of the reference ellipsoid corresponding to the current vehicle position, the longitude and latitude coordinates of the WGS-84 coordinate system where the current vehicle is located are converted into Cartesian coordinates in the ECEF coordinate system, as shown in Equation (3).

$$\left\{\begin{array}{c}X=(N+alt)cos\left(lat\right)cos\left(lon\right)\hfill \\ Y=(N+alt)cos\left(lat\right)sin\left(lon\right)\hfill \\ Z=(N(1-e^2)+alt)sin\left(lat\right)\hfill \end{array}\right.$$

(3)

among them, $(X,Y,Z)$ is the Cartesian coordinates of the current vehicle located in the ECEF coordinate system, $(lon,lat,alt)$ is the longitude and latitude altitude coordinates of the current vehicle located in the WGS-84 coordinate system, N is the reference Earth curvature radius corresponding to the current vehicle position, and e is the Earth eccentricity.

Select the origin of the ENU coordinate system, measure the corresponding longitude and latitude altitude coordinates using RTK/INS integrated inertial navigation, and use Equations (1)–(3) to convert the longitude and latitude altitude coordinates corresponding to the origin of the ENU coordinate system to Cartesian coordinates under the ECEF coordinate system, as shown in Equations (4) and (5).

$$N_0=\frac{a}{\sqrt{1-e^2{sin}^2\left(la{t}_0\right)}}$$

(4)

$$\left\{\begin{array}{c}{X}_0=(N_0+al{t}_0)cos\left(la{t}_0\right)cos\left(lo{n}_0\right)\hfill \\ Y_0=(N_0+al{t}_0)cos\left(la{t}_0\right)sin\left(lo{n}_0\right)\hfill \\ Z_0=(N_0(1-e^2)+al{t}_0)sin\left(la{t}_0\right)\hfill \end{array}\right.$$

(5)

among them, $N_0$ is the reference Earth curvature radius corresponding to the ENU coordinate system origin, $(X_0,Y_0,Z_0)$ is the Cartesian coordinates of the ECEF coordinate system where the ENU coordinate system origin is located, and $(lo{n}_0,la{t}_0,al{t}_0)$ is the longitude and latitude altitude coordinates of the WGS-84 coordinate system where the ENU coordinate system origin is located.

Subtract the Cartesian coordinates of the current vehicle in the ECEF coordinate system from the Cartesian coordinates of the ENU coordinate system origin in the ECEF coordinate system, and use the coordinate difference to solve the coordinates of the current vehicle in the ENU coordinate system, as shown in Equations (6) and (8).

$$\left(\begin{array}{c}\Delta X\hfill \\ \Delta Y\hfill \\ \Delta Z\hfill \end{array}\right)=\left(\begin{array}{c}X\hfill \\ Y\hfill \\ Z\hfill \end{array}\right)-\left(\begin{array}{c}{X}_0\hfill \\ Y_0\hfill \\ Z_0\hfill \end{array}\right)$$

(6)

$$\left(\begin{array}{c}{X}_{ENU}\hfill \\ Y_{ENU}\hfill \\ Z_{ENU}\hfill \end{array}\right)=S·\left(\begin{array}{c}\Delta X\hfill \\ \Delta Y\hfill \\ \Delta Z\hfill \end{array}\right)$$

(7)

$$S=\left(\begin{array}{ccc}-sin\left(lo{n}_0\right)& cos\left(lo{n}_0\right)& 0\\ -sin\left(la{t}_0\right)cos\left(lo{n}_0\right)& -sin\left(la{t}_0\right)sin\left(lo{n}_0\right)& cos\left(la{t}_0\right)\\ cos\left(la{t}_0\right)cos\left(lo{n}_0\right)& cos\left(la{t}_0\right)sin\left(lo{n}_0\right)& sin\left(la{t}_0\right)\end{array}\right)$$

(8)

among them, $(\Delta X,\Delta Y,\Delta Z)$ is the difference between the coordinates of the current vehicle located in the ECEF coordinate system and the coordinates of the ENU coordinate system origin located in the ECEF coordinate system, $(X,Y,Z)$ is the Cartesian coordinates of the current vehicle located in the ECEF coordinate system, $(X_0,Y_0,Z_0)$ is the Cartesian coordinates of the ENU coordinate system origin located in the ECEF coordinate system, $(X_{ENU},Y_{ENU},Z_{ENU})$ are the coordinates of the current vehicle located in the ENU coordinate system, and S is the coordinate transformation matrix from the ECEF coordinate system to the ENU coordinate system, $(lo{n}_0,la{t}_0,al{t}_0)$ is the longitude and latitude altitude coordinates of the ENU coordinate system origin located in the WGS-84 coordinate system.

3. A Fully Covered Path Planning Algorithm with Multi-Objective Function Coupling Using the Fields2Cover Library

In response to the problems of existing full-coverage path planning algorithms that only consider job coverage or job repetition rate as optimization objectives, i.e., they have a single optimization objective and a non-optimal path generation, this paper adopted a multi-objective function-coupled full-coverage path planning algorithm [29], which takes parameters such as job coverage, global path length, and number of paths as coupling optimization objectives to generate the global optimal path while meeting job coverage, effectively improving operational efficiency and saving energy costs for agricultural machinery. This algorithm was proposed by Mier et al. and is currently an advanced full-coverage path planning algorithm. The full-coverage path planning part of the intelligent agricultural machinery system in this article adopts the Fields2Cover library developed by Mier et al. [30], which can meet the full-coverage path planning needs of most existing farmland and has strong practicality. The core principles of this algorithm will be explained below.

3.1. Agricultural Machinery Operation Area and Parallel Path Generation

The technical route of the multi-objective function-coupled full-coverage path planning algorithm adopted in this article is as follows. Firstly, the RTK/INS integrated inertial navigation is used to dot the actual farmland boundary, obtain the longitude and latitude altitude coordinates corresponding to each vertex of the actual farmland boundary, and generate the corresponding farmland boundary in the ENU coordinate system, as shown in Figure 2. On the basis of the corresponding farmland boundary, a certain width is shrunk inward to generate the agricultural machinery operation area, as shown in Figure 3. The contraction width is generally set as the turning radius of the agricultural machinery.

Several parallel paths are planned within the generated agricultural machinery operation area to achieve full-coverage path planning and the operation of agricultural machinery within the operation area. To ensure the optimal coverage and operation path of agricultural machinery in the operation area, this algorithm sets three optimization objective functions.

Firstly, the objective function for generating the number of parallel paths within the work area is to minimize the number of parallel paths generated while ensuring job coverage, in order to improve the walking and working efficiency of agricultural machinery. The number of parallel paths generated is related to the area and shape of the agricultural machinery operation area, as well as the width of the agricultural machinery itself. Moreover, since the minimum number of parallel paths corresponding to square farmland is the largest when the area of the agricultural machinery operation area is fixed, the objective function satisfies the following relationship [29]:

$$0\le N_{\theta }\le \frac{\sqrt{S_w}}{W_r}$$

(9)

among them, $N_{\theta }$ is the number of parallel paths generated at the angle of $\theta$, $S_w$ is the area of square farmland operation area, and $W_r$ is the width of agricultural machinery operation.

The second optimization objective function set in this algorithm is the job coverage objective function, which aims to maximize the job coverage of agricultural machinery traveling along the planned path, as shown in Equation (10) [29].

$$S_{cov}=\frac{S_w\bigcap \left\{{\bigcup }_i{S}^i\right\}}{S_w}$$

(10)

where $S_{cov}$ is the coverage rate of agricultural machinery operation, $S_w$ is the area of agricultural machinery operation area, and $S^i$ is the area of agricultural machinery operation along the i-th parallel path. The third optimization objective function set in this algorithm is the total length of the operation path. The function is to minimize the total length of the generated fully covered path while ensuring the coverage rate of agricultural machinery operations, as shown in Equation (11) [29].

$$\sum _i^M{L}^i=\sum _i^M\sum _j^{L_p^i-1}∥L_{p=j+1}^i-L_{p=j}^i∥$$

(11)

among them, ${\displaystyle \sum _i^M}{L}^i$ is the total length of all parallel paths, M is the number of parallel paths, $L_p^i$ is the number of path points for the i-th parallel path, $L_{p=j}^i$ is the j-th path point on the i-th parallel path, and $∥x∥$ is the Euclidean norm.

When planning parallel paths, the above three objective functions work together to ensure that the planned global coverage path maximizes job coverage and path optimality.

3.2. Parallel Path Sorting and Curve Generation

In order to adapt agricultural machinery to different scenarios, this algorithm sets up four parallel path-sorting methods, namely, boustrophedon sorting, snake sorting, spiral sorting, and custom sorting, as shown in Figure 4. The boustrophedon sorting method is widely used in full-coverage path planning. It adopts a reciprocating and circuitous covering method. After driving along the first parallel path, it immediately follows the second parallel path, and so on, until the entire vehicle runs along all parallel paths. The disadvantage of this method is that the distance between adjacent driving paths is too short, resulting in sharp curves that affect the control effect of the entire vehicle. Unlike the sequential traversal of boustrophedon sorting, snake sorting skips one parallel path each time, travels to the last parallel path, and then returns to driving along other parallel paths that have not been driven. The advantage of this method is that the distance between adjacent driving paths is large, and the generated curves are relatively smooth.

During the crop harvesting process, due to the limited capacity of the harvester, it is necessary to unload the crops into the truck after harvesting a portion of the area. In this case, spiral sorting can be used to avoid the truck from driving to the uncut area and damaging the crops. The key point of spiral sorting is that the operation area can be divided into several sub-areas, and spiral driving can be carried out in the sub-areas. After the current sub-area is covered, it can be moved to the next sub-area. Custom sorting is mainly to meet the personalized needs of different users, and users can independently define parallel path sorting according to their actual needs.

After arranging the parallel paths in order, corresponding curves can be generated between adjacent parallel paths with sequence numbers. In this algorithm, there are three methods for generating curves: the straight line method, the Dubins curve [31], and the Reeds–Shepp curve [32]. The straight line method directly connects the endpoint of the current parallel path with the starting point of the adjacent parallel path with a straight line. The Dubins curve method and the Reeds–Shepp curve method use the Dubins curve and the Reeds–Shepp curve to connect the endpoint of the current parallel path with the starting point of the adjacent parallel path with the next ordinal, respectively. It is worth noting that the curves generated by the Dubins curve method only require the vehicle to have forward function to complete tracking, while the curves generated by the Reeds–Shepp curve method require the vehicle to have both forward and backward functions to complete the corresponding curve tracking.

4. Improved Pure Pursuit Algorithm Based on Linear Interpolation

In response to the difficulties in parameter tuning of existing PID algorithms and the poor real-time performance of model predictive control, this paper reconstructs the existing pure pursuit algorithm based on the kinematic model of tracked agricultural machinery chassis, and designs a linear interpolation algorithm on this basis. This algorithm can make the target points of each tracking of the entire vehicle ideal, effectively improving control accuracy and smoothness. The technical details are as follows.

Figure 5 is the schematic diagram of an improved pure pursuit algorithm proposed in this article for the kinematic model characteristics of tracked agricultural machinery chassis. Among them, R is the arc radius of the tracked agricultural machinery chassis tracking path, $\alpha$ is the angle between the agricultural machinery vehicle body and the tracking target point, e is the lateral deviation between the agricultural machinery vehicle and the tracking target point, $l_d$ is the preview distance, b is the half-value of the left and right track spacing of the tracked agricultural machinery chassis, and $(x_p,y_p)$ are the coordinates of the tracking target point.

The basic principle of the improved pure pursuit algorithm proposed in this article is as follows: Firstly, the current pose of the entire vehicle is obtained through the RTK/INS integrated inertial navigation system combined with the coordinate conversion algorithm proposed in this article. The tracking target point of the entire vehicle is determined based on the given preview distance and the given global tracking path. After determining the tracking target point of the entire vehicle, a circular arc is constructed between the vehicle and the tracking target point as the local path for the vehicle to travel to the tracking target point. Through geometric relationships, the local path arc radius R of the entire vehicle traveling to the tracking target point can be solved. Given the tracking speed $v_c$ of the entire vehicle, the angular velocity w of the entire vehicle around the center of the tracked local path can be calculated. Based on the angular velocity, the velocities $v_L$ and $v_R$ of the left and right tracks of the tracked agricultural machinery chassis can be further calculated. By strictly calculating and controlling the speed of the left and right tracks, the entire vehicle can track the local path until it reaches the tracking target point, and then search for the next tracking target point. Repeat the above steps until the entire vehicle reaches the given global tracking path endpoint.

The relevant derivation is shown in Equations (12)–(16). It is worth noting that the chassis of tracked agricultural machinery belongs to differential mobile machinery, which is different from the Ackermann steering model that achieves turning by changing the front wheel angle. It controls the differential speed of the left and right tracks to achieve the steering of the entire vehicle. When the speed of the left and right tracks is the same, the entire vehicle goes straight; when the speed of the left track is greater than that of the right track, the entire vehicle turns right; Otherwise, turn left.

$$\frac{l_d}{sin\left(2\alpha \right)}=\frac{R}{sin(\frac{\pi }{2}-\alpha )}$$

(12)

$$k=\frac{1}{R}=\frac{2sin\alpha }{l_d}=\frac{2e}{l_d^2}$$

(13)

$$w=\frac{v_c}{R}=k{v}_c=\frac{2e{v}_c}{l_d^2}$$

(14)

$$v_L=w(R-b)=\frac{2e{v}_c(R-b)}{l_d^2}$$

(15)

$$v_R=w(R+b)=\frac{2e{v}_c(R+b)}{l_d^2}$$

(16)

Due to the fact that the pure pursuit algorithm tracks a path consisting of several discrete path points, given the preview distance, when the path points are too sparse, the distance between the path points tracked by the entire vehicle and the entire vehicle cannot be strictly limited to the given preview distance, which will seriously affect the accuracy of vehicle motion control. By increasing the number of path points in the tracking path, this problem can be effectively solved. However, having too many path points can lead to unnecessary computational waste on the onboard computing platform. At the same time, having too many path points can also lead to the vehicle’s trajectory being repeatedly planned, resulting in the vehicle’s motion control oscillating repeatedly and greatly reducing the smoothness of motion control. Based on this, this article proposes a linear interpolation method, as shown in Figure 6.

After obtaining the current tracking path point of the entire vehicle, a straight line equation is constructed using the current tracking path point of the entire vehicle and the previous tracking path point of the entire vehicle, as shown in Equation (17).

$$\left\{\begin{array}{c}Ax+By+C=0\hfill \\ A=y_2-y_1\hfill \\ B=-x_2-x_1\hfill \\ C=(-1)\ast (y_2-y_1)x_1+(x_2-x_1)y_1\hfill \end{array}\right.$$

(17)

among them, A, B, C are the coefficients of the linear equation, $(x_1,y_1)$ are the coordinates of the last tracking path point on the entire vehicle, and $(x_2,y_2)$ are the coordinates of the current tracking path point on the entire vehicle.

After solving the linear equation, calculate the distance between the current vehicle and the linear equation, as shown in Equation (18).

$$d=\frac{\left|A{x}_p+B{y}_p+C\right|}{\sqrt{A^2+B^2}}$$

(18)

among them, $(x_p,y_p)$ is the current position of the entire vehicle, and A, B, C are the coefficients of the linear equation.

As shown in Figure 6, after solving the distance between the current vehicle and the above linear equation, a vehicle tracking target search circle is constructed with the current position of the vehicle as the center of the circle and the preview distance as the radius, as shown in Equation (19).

$$\left\{\begin{array}{c}{\left(x-x_p\right)}^2+{\left(y-y_p\right)}^2=r^2\hfill \\ Ax+By+C=0\hfill \end{array}\right.$$

(19)

among them, $(x_p,y_p)$ is the current position of the entire vehicle, r is the preview distance, and, A, B, C are the coefficients of the linear equation mentioned above.

When $d>r$, the entire vehicle tracking target search circle did not intersect with the above linear equation, and at this time, the tracking target point interpolation could not be performed; when $d<r$, the search circle for the vehicle tracking target intersected with the above linear equation, and there were two solutions. The point closest to the current tracking path point of the vehicle was selected as the vehicle tracking target point; when $d=r$, the search circle for the vehicle tracking target was tangent to the linear equation mentioned above, and there was a unique solution; that is, the vehicle tracking target point. After solving the tracking target point of the entire vehicle, the speed of the left and right tracks can be calculated to control the entire vehicle to move towards the tracking target point. Repeat the above steps until the global path tracking is completed.

5. Building and Related Experiments Based on Gazebo Simulation Platform

To verify the performance of the positioning, planning, motion control, and other algorithms in this article, a tracked intelligent agricultural machinery simulation platform was built based on Gazebo, as shown in Figure 7.

This simulation test platform is equipped with various sensors such as binocular cameras, LiDAR, and GPS, which can perform prior verification on various algorithms of intelligent agricultural machinery systems. The advantage of using Gazebo to build a simulation platform is that it can effectively reduce project research and development costs, shorten research and development cycles, and maximize the safety of experimental personnel.

To conduct a prior verification of the performance of the positioning, planning, and motion control algorithms in this article, corresponding simulation scenarios were constructed for simulation experiments, as shown in Figure 8.

5.1. Simulation Performance Verification of RTK/INS Fusion Positioning Coordinate Transformation Algorithm

Firstly, the vehicle GPS is used to dot the testing area to obtain the longitude and latitude altitude coordinates of each vertex at the boundary of the testing area. The coordinate transformation algorithm proposed in this article converts the longitude and latitude altitude coordinates of each vertex into the corresponding Cartesian coordinates in the ENU coordinate system. Then, the multi-target function-coupled full-coverage path planning algorithm adopted in this article is used for the full-coverage path planning of the testing area, as shown in Figure 9.

In the agricultural production process, the boustrophedon method is commonly used for full-coverage path planning. At the same time, in order to more intuitively observe the error of the RTK/INS fusion positioning coordinate transformation algorithm proposed in this article and the trajectory tracking error of the improved pure pursuit algorithm, the boustrophedon full-coverage path is selected as the global tracking path.

To avoid the impact of vehicle GPS observation noise on the simulation test results of coordinate transformation algorithms, the position observation noise parameter of the vehicle GPS is set to 0; that is, the longitude and latitude altitude coordinates observed by the vehicle GPS are absolute true values. The coordinate transformation algorithm proposed in this article is used to convert the real-time obtained vehicle longitude and latitude altitude coordinates into the corresponding Cartesian coordinates under the ENU coordinate system, forming the vehicle positioning trajectory. As it is a simulation experiment, the true position of the entire vehicle in the simulated world coordinate system can be obtained in real-time by subscribing to the odometer topic, generating a reference trajectory. Due to the absolute truth of the longitude and latitude altitude coordinates observed by the vehicle GPS and the true position of the vehicle subscribed through the topic in the simulated world coordinate system, the error between the positioning trajectory and the reference trajectory calculated by the vehicle through the coordinate transformation algorithm is the true transformation error of the coordinate transformation algorithm proposed in this article.

Use the evo evaluation tool to convert the positioning trajectory into a reference trajectory coordinate system, and perform absolute pose error (APE) and relative pose error (RPE) evaluations. The results are shown in Figure 10 and Figure 11.

From the analysis of Figure 10 and Figure 11, it can be concluded that the average APE value of the positioning trajectory relative to the reference trajectory, i.e., the reference true value, is 0.066 m, and the root mean square error is 0.07 m; the average RPE value is 0.02 m, with a root mean square error of 0.026 m, which can effectively meet the positioning accuracy requirements of agricultural machinery autonomous navigation systems.

5.2. Simulation Performance Verification of Improved Pure Pursuit Algorithm Based on Linear Interpolation

On the basis of determining that the RTK/INS fusion positioning coordinate transformation algorithm proposed in this article can ensure the accuracy of vehicle positioning, the error between the vehicle’s travel trajectory and the set boustrophedon full-coverage path is analyzed to verify the performance of the improved pure pursuit algorithm proposed in this article. The tracking speed of the vehicle is set to 1 m/s, and the preview distance is 1 m. Figure 12 shows the comparison between the set path and the driving trajectory during the simulation test process.

From the analysis in Figure 12, it can be seen that the improved pure pursuit algorithm proposed in this article has good tracking performance in the straight line section and poor tracking performance in the curve section. Due to the fact that during the operation of agricultural machinery, bends are usually completed in non operating areas, and the linear tracking part generally affects the operation effect. Therefore, this article mainly discusses the trajectory tracking error of each linear segment, as shown in Figure 13.

From the analysis in Figure 13, it can be seen that the trajectory tracking error of the entire vehicle in the straight section is relatively small. The maximum tracking error in the X-axis direction is 0.048 m, with an average error of 0.029 m, and the maximum error in the Y-axis direction is 0.049 m, with an average error of 0.031 m. The simulation test results show that the improved pure pursuit algorithm proposed in this paper has high control accuracy and can be applied to agricultural machinery motion control.

6. Construction and Testing of Intelligent Agricultural Machinery Real Vehicle Test Platform

To further validate the performance of the positioning, planning, and motion control algorithms in this article, an intelligent agricultural machinery real vehicle test platform was built based on an electric tracked chassis, as shown in Figure 14. This real vehicle testing platform is equipped with sensors such as binocular cameras, LiDAR, RTK/INS integrated navigation, etc. The perception information of each sensor is received through the onboard computing platform, which can be used for the real vehicle verification of various algorithms in the field of intelligent agricultural machinery. The main equipment parameters of the actual vehicle test platform are shown in

Table 1. Main equipment parameters of the actual vehicle test platform.

Device Name	Equipment Model	Equipment Parameters
IMU	N100	Output frequency: 400 Hz Serial port baud rate: 921,600 Pitch/roll accuracy: 0.05° number of axles: 9 axes
RTK/INS	CHCNAV CGI-610	Attitude accuracy: 0.1° Positioning accuracy: 1 cm Output frequency: 100 Hz Initialization time: 1 min
Lidar	Velodyne16	Measurement range: 100 m Measurement accuracy: ±3 cm Vertical measurement angle: 30° Horizontal measurement angle: 360° Measurement frequency: 5–20 Hz
On-board computer	TW-T906	CPU: 8-core Arm architecture GPU: 1792 core Navidia Ampere architecture Computational power: 200 TOPS

.

As shown in Figure 15, an open field was selected to validate the full-coverage path planning algorithm and improved pure pursuit algorithm proposed in this paper on a real vehicle.

Firstly, the RTK/INS integrated inertial navigation system is used to plot points at the four corners of the test site edge, obtaining the corresponding latitude and longitude altitude coordinates. The coordinates are converted into the corresponding ENU coordinates through coordinate transformation algorithms. Finally, the full-coverage path planning algorithm adopted in this article is used to generate the full-coverage path within the test area, as shown in Figure 16.

Similar to the simulation experiment, in order to observe the trajectory tracking error of the improved pure pursuit algorithm proposed in this article more intuitively, the boustrophedon full-coverage path is selected as the global tracking path. The improved pure pursuit algorithm proposed in this article is used for trajectory tracking, with a vehicle tracking speed of 1 m/s and a preview distance of 1 m. The tracking effect is shown in Figure 17 and Figure 18.

From the analysis in Figure 18, it can be seen that the improved pure pursuit algorithm proposed in this paper has good tracking performance in the straight line section and poor tracking performance in the curve section, which is consistent with the simulation results. Similarly, in the process of agricultural machinery operation, curves are usually completed in non operating areas, and the linear tracking part generally affects the operation effect. Therefore, this article mainly discusses the trajectory tracking error of each linear segment, as shown in Figure 19.

According to the analysis in Figure 19, consistent with the simulation test results, the trajectory tracking error of the entire vehicle in the straight section is relatively small. The maximum tracking error in the X-axis direction is 0.049 m, with an average error of 0.016 m, and the maximum error in the Y-axis direction is 0.129 m, with an average error of 0.017 m. This can effectively meet the walking accuracy requirements of agricultural machinery operations.

7. Conclusions

This article creates an intelligent agricultural machinery system that integrates positioning, planning, and motion control. Based on RTK/INS integrated inertial navigation and self-designed coordinate transformation algorithm, high-precision vehicle positioning is achieved. The vehicle positioning error is within 10 cm, which can effectively meet the requirements of agricultural machinery intelligent systems for vehicle positioning accuracy. By utilizing multiple objective functions and coupling mechanisms, high coverage and full-coverage path planning are achieved, effectively improving the efficiency of agricultural machinery operations.

Finally, this article improves the existing pure pursuit algorithm by adding a linear interpolation module to effectively improve the accuracy of vehicle motion control, achieving high fitting vehicle motion control in straight segments. In the simulation test, the maximum control error of the entire vehicle in the X-axis and Y-axis directions does not exceed 5 cm, and the average error is around 3 cm. In the actual vehicle test, the maximum control error of the entire vehicle in the X-axis and Y-axis directions does not exceed 13 cm, and the average error does not exceed 2 cm, indicating precise control and high reliability. This has laid a certain foundation for the development of unmanned agricultural machinery systems in the future.

The intelligent agricultural machinery system proposed in this article is suitable for autonomous full-coverage path planning and walking of various tracked agricultural machinery, and can independently complete various agricultural production activities in conjunction with various operating equipment, effectively improving agricultural production efficiency and reducing labor costs. At the same time, the improved pure pursuit algorithm in this article can also be modified based on various agricultural machinery kinematic models, making it suitable for autonomous full-coverage path planning and walking of various agricultural machinery.

Based on the existing foundation of this article, subsequent research can focus on the following aspects. Firstly, the full-coverage path planning adopted in this article is only applicable to relatively regular land. In the future, we can consider combining the cell segmentation method to achieve full-coverage path planning for irregular farmland. Secondly, the improved pure pursuit algorithm proposed in this article is only applicable to the motion control of medium- and low-speed agricultural machinery. In the future, we can consider introducing dynamic motion control methods to make this algorithm suitable for high-speed agricultural machinery motion control. Thirdly, this article only achieves single-machine intelligence, and can be combined with intelligent interconnected systems such as drones and 5G communication to achieve multi-machine collaborative work in the future.