Path tracking control method suitable for crawler-type agricultural equipment

Abstract

The invention discloses a path tracking control method suitable for crawler-type agricultural equipment. The method comprises the following steps: S1, building a navigation hardware control system on crawler-type equipment; s2, designing a Kalman filtering algorithm to filter the course angle of the crawler-type equipment; s3, correcting an equipment positioning error caused by the attitude angle; s4, a fusion algorithm of the Stanley algorithm and the Pure Pursuit algorithm is designed; s5, designing an enhanced SPP algorithm; s6, performing secondary optimization on the control law output by the SPP algorithm by using a fuzzy PID control algorithm; and S7, fitting a total control law equation in combination with the control algorithm, and converting the control law into the linear speed of the left and right tracks of the crawler-type equipment so as to realize steering and straight movement of the crawler-type equipment. According to the invention, a complex agricultural environment is regarded as a black box system, and item-by-item optimization is carried out by superposing an algorithm with high interpretability, so that the adaptability of a control system to the black box system is improved, and finally, the equipment path tracking precision is improved step by step.

Description

Technical Field

[0001] This invention belongs to the field of intelligent navigation technology, specifically relating to a path tracking control method suitable for tracked agricultural equipment. Background Technology

[0002] Path tracking is a crucial component of intelligent navigation technology. Current path tracking algorithms are mainly categorized into data-driven and model-driven algorithms. Data-driven algorithms do not rely on precise mathematical models but learn system patterns, establish mapping relationships, and make control decisions using large amounts of historical or real-time data. This type of algorithm is highly dependent on data and has poor interpretability, making it difficult to explain the internal physical mechanisms. In practical applications, its training process is complex and requires sophisticated equipment. Therefore, its application in actual path tracking engineering projects is not very widespread. Model-driven algorithms, on the other hand, are based on mathematical or physical models. By analyzing the physical characteristics of the system, they design control laws or estimators using control theory or optimization methods. This type of algorithm is widely used in practical engineering projects due to its strong interpretability and high stability. The most typical examples are the PurePursuit algorithm and the Stanley algorithm. In simple environments, this algorithm can achieve precise control using simple kinematic models. However, in complex agricultural scenarios, achieving accurate tracking requires considering numerous interference factors and establishing complex dynamic models.

[0003] In agricultural settings, tracked equipment is subject to numerous interfering factors when operating autonomously. The coupling effect between the tracks and the ground can cause slippage, sinking, and changes in adhesion. The equipment's own vibrations during movement can affect the positioning accuracy of the positioning system and the measurement data of attitude angles, leading to a decrease in tracking accuracy. Therefore, there is an urgent need for a path tracking algorithm that can achieve precise tracking of tracked agricultural equipment without requiring complex dynamic models. Summary of the Invention

[0004] The main objective of this invention is to overcome the shortcomings and deficiencies of the prior art and to propose a path tracking control method suitable for tracked agricultural equipment.

[0005] To achieve the above objectives, the present invention adopts the following technical solution:

[0006] A path tracking control method for tracked agricultural equipment includes the following steps:

[0007] S1. Build a navigation hardware control system on tracked equipment;

[0008] S2. Design a Kalman filter algorithm to filter the heading angle of tracked equipment;

[0009] S3. Correct the equipment positioning error caused by the attitude angle;

[0010] S4. Design a fusion algorithm of Stanley and Pure Pursuit algorithms to make the lateral control strength of the working equipment directly determined by the geometric pre-aiming scale, thereby avoiding the influence of speed changes on lateral control, eliminating instability problems under low-speed conditions, and improving the stability and interpretability of the algorithm on variable speed motion and tracked working equipment.

[0011] S5. Design an enhanced SPP algorithm to dynamically adjust the control gain and aiming distance based on the equipment speed and path curvature, ensuring the smoothness of the equipment in straight lines and the sensitivity when turning, and further improving the stability and accuracy of equipment tracking under variable speed and complex paths.

[0012] S6. Use the fuzzy PID control algorithm to perform secondary optimization on the control law output by the SPP algorithm;

[0013] S7. By combining the above control algorithms to fit the overall control law equation, the control law is converted into the linear velocity of the left and right tracks of the tracked equipment, thereby realizing the turning and straight-line movement of the tracked equipment.

[0014] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0015] 1. This invention treats the complex agricultural environment as a black box system and optimizes it item by item by superimposing highly interpretable algorithms, thereby improving the adaptability of the control system to the black box system and ultimately improving the accuracy of equipment path tracking step by step.

[0016] 2. This invention utilizes a Kalman filter algorithm to optimize heading angle data by fusing and optimizing data calculated by GNSS-RTK and an inertial navigation module (INS). GNSS-RTK provides real-time heading angle data, but interference can lead to unstable heading angles or even signal loss. Therefore, an inertial navigation module is used to temporarily compensate for the GNSS-RTK signal. The inertial navigation module can only provide accurate deflection angle data in the short term, so GNSS-RTK is needed to correct it. Through the cooperation of both, accurate and stable heading angle data is provided to the device.

[0017] 3. The core tracking algorithm of this invention integrates the lateral adjustment function of the Stanley algorithm and the curvature prediction function of the Pure Pursuit algorithm. Based on the Stanley algorithm, the algorithm modifies its velocity term to dynamically predict the aiming distance, thereby achieving the fusion of the two algorithms. Compared with the traditional Stanley algorithm, the optimized algorithm avoids jitter in low-speed conditions and can predict the steering direction in advance in high-speed conditions, making it safer and ensuring that the system has better robustness and predictability.

[0018] 4. This invention utilizes a fuzzy PID control algorithm to perform secondary optimization and adjustment of the control law calculated by SPP, thereby improving the adaptability of the control system, enhancing its anti-interference capability, and improving the stability of the control system. Attached Figure Description

[0019] Figure 1 This is a schematic diagram illustrating the operating principle of the present invention.

[0020] Figure 2 This is a schematic diagram illustrating the operating principle of the navigation hardware control system in this invention.

[0021] Figure 3 This is a flowchart illustrating the operation of the method according to an embodiment of the present invention. Detailed Implementation

[0022] The present invention will be further described in detail below with reference to the embodiments and accompanying drawings, but the embodiments of the present invention are not limited thereto.

[0023] like Figure 1 As shown, a path tracking control method suitable for tracked agricultural equipment includes the following steps:

[0024] S1. Build a navigation hardware control system on the tracked equipment; specifically:

[0025] Select a tracked work equipment with differential motion capability and install a navigation hardware control system, which includes a positioning system and a decision-making system; such as Figure 2 The diagram shown illustrates the operating principle of the navigation hardware control system.

[0026] The positioning system includes GNSS-RTK (Global Navigation Satellite System - Real-time Dynamic Positioning) and an inertial navigation module, which are used to detect changes in the device's attitude angles and provide the device's coordinate information;

[0027] The decision-making system is an industrial control computer used to receive and process sensor data and issue control commands based on the processing results.

[0028] S2. Design a Kalman filter algorithm to filter the heading angle of tracked equipment; specifically including:

[0029] S21. The heading angle difference δ measured by GNSS-RTK and inertial navigation module ψk The angular velocity w of the device at a specific moment k and the zero bias b of the inertial navigation system k Let this be the state variable; using the angular velocity measured by the inertial navigation module as the control input, and determining the relevant process noise, the state equation is determined based on the above variable information, and is expressed as:

[0030] ;

[0031] Where, x k It is the current state vector; x k-1 It is the state vector of the previous time step; F is the state transition matrix; w k-1 It is process noise; w ψ,k w w,k w b,k These are the noises corresponding to the state variables;

[0032] S22. Based on the measured value z k and the observation matrix H and the associated measurement noise v k The observation equation is constructed as follows:

[0033] ;

[0034] S23. Construct prediction equations based on state equations; In the prediction stage, perform prior estimation of state variables based on the system state transition model, and update the corresponding state covariance matrix synchronously.

[0035] S24. Construct an update equation based on the observation equation; in the update stage, calculate the Kalman gain based on the observation model and measurement information, correct the predicted state, and thus obtain the posterior estimate of the state.

[0036] S3. Correct the equipment positioning error caused by the attitude angle; specifically including:

[0037] S31. Obtain the attitude angle information of agricultural equipment through the positioning system, including yaw angle ψ, pitch angle θ and roll angle φ;

[0038] S32. When the attitude angle of agricultural equipment changes, the spatial offset between the positioning sensor and the equipment's positioning center undergoes rotational mapping in the global coordinate system, resulting in errors in the coordinate values ​​of the equipment's positioning center in the global coordinate system. This is corrected based on the attitude angle, and the attitude-position error coupling correction equation is:

[0039] ;

[0040] ;

[0041] Where (x, y, z) represents the device position information measured by the positioning system in the global coordinate system; (a, b, h) represents the position offset of the device positioning point relative to the origin of the vehicle coordinate system; E -1 (ψ, θ, φ) is the rotation transformation matrix from the vehicle coordinate system to the global coordinate system; (x a , y a , z a This refers to the position information of the vehicle origin in the global coordinate system after calibration.

[0042] S4. Design a fusion algorithm combining the Stanley and Pure Pursuit algorithms, so that the lateral control strength of the working equipment is directly determined by the geometric pre-aiming scale. This avoids the influence of speed changes on lateral control, eliminates instability problems under low-speed conditions, and improves the stability and interpretability of the algorithm on variable-speed motion and tracked working equipment; specifically including:

[0043] S41. Based on the Stanley algorithm and combined with the pre-aiming characteristics of the Pure Pursuit algorithm, the velocity term in the Stanley algorithm is replaced with a dynamic pre-aiming distance. The optimized control equations are as follows:

[0044] ;

[0045] Where δ is the control output of the algorithm; For heading angle error; e y d represents the lateral error; d represents the dynamic aiming distance; k represents the lateral error. a This is a comprehensive control gain, including the base gain coefficient k0 and the speed gain coefficient k. v and the gain coefficient k of path curvature k ;k a Represented as:

[0046] ;

[0047] Where k is the curvature of the local path in front of the device.

[0048] S5. Design an enhanced SPP algorithm to dynamically adjust the control gain and pre-aiming distance based on the device speed and path curvature, ensuring the smoothness of the device's straight-line movement and its sensitivity when cornering, further improving the stability and accuracy of device tracking under variable speed and complex paths; specifically including:

[0049] Due to numerous interference factors during actual operation, tracked agricultural equipment suffers from center distance error and heading angle error. Center distance error is the fixed spatial offset between the positioning sensor and the positioning center defined by the equipment. Heading angle error is the difference between the heading angle estimated by the positioning sensor and the actual heading angle of the equipment.

[0050] To address the center distance error, the tracked agricultural equipment was set to different speeds, and the constant positioning point error of the equipment was measured experimentally. The average value of the measured errors was then used as the compensation value e. yb The compensation is added to the control law;

[0051] For heading angle error compensation, if the system still has an uneliminable steady-state error after center distance error compensation, secondary compensation is performed using the following heading angle error compensation formula:

[0052] ;

[0053] in, The steady-state error is given by v, where v is the speed of the device; k is the speed of the device. j These are the relevant gain coefficients, and the final parameters can be determined through experimental testing later. This is the calculated error correction value;

[0054] To achieve dynamic adaptive adjustment of the aiming distance, the velocity, the curvature of the reference path, and the corresponding gain coefficient are incorporated into the dynamic adjustment of the aiming distance. Therefore, the control equation is:

[0055] ;

[0056] Based on the above optimizations, the control law expression for the algorithm is determined as follows:

[0057] .

[0058] Step S5 also includes:

[0059] The gain coefficient in the control law is experimentally measured in practical applications using a step-by-step testing method, including:

[0060] Set the velocity and curvature gain parameters to 0;

[0061] The SPP algorithm is embedded into the test equipment, and the maximum and minimum aiming distances suitable for tracked equipment are determined by using a step-by-step testing method. After determining the range of aiming distances, the most suitable aiming distances at different speeds and curvatures are tested respectively. Then, the data are integrated to determine the most suitable speed gain parameters and curvature gain parameters.

[0062] By using the above methods, it is ensured that during the tracking process, the aiming distance is adaptively adjusted according to the changes in the speed of the tracked agricultural equipment and the curvature of the reference path.

[0063] S6. Utilize a fuzzy PID control algorithm to perform secondary optimization on the control law output by the SPP algorithm; including:

[0064] S61. Construct the algorithm control architecture of fuzzy PID; use fuzzy rules to tune the three basic parameters of the PID algorithm online, including the proportional coefficient k. p Integral coefficient k i and the differential coefficient k d ;

[0065] S62. Establish a discrete-form PID control algorithm, as shown in the following formula:

[0066] ;

[0067] Where w(k) is the control output at the current time; e(k) is the error at the current time; and ec(k) is the error rate of change.

[0068] S63. Obtain the basic parameters of the PID control algorithm, including the proportional coefficient k, through experimental testing using the critical oscillation method. p0 Integral coefficient k i0 and the differential coefficient k d0 ;

[0069] S64. Using error and error rate of change as input parameters; obtaining the effective intervals of error and error rate of change through experiments to determine the universe of discourse; calculating the scaling factor based on the set standardization interval [-1,1], and using the scaling factor to quantify the input parameters;

[0070] S65. Using fuzzy language, divide the fuzzy variables into 7 levels {NB, NM, NS, ZO, PS, PM, PB}, and calculate the membership values ​​of the input parameters using the triangular membership function.

[0071] S66. A fuzzy rule control table is established using a hierarchical control strategy to adjust the three basic parameters of the PID online.

[0072] S67. Based on the 7×7 fuzzy rule, calculate the trigger strength of the rule using multiplication;

[0073] S68. Based on the experimentally measured values ​​of the parameters required by the PID control algorithm under various operating conditions, independent constant conclusion matrices are used to represent the adjustment amounts of the three basic parameters of the PID control algorithm.

[0074] S69. Use the zero-order Sugeno weighted average method for fuzzy calculation;

[0075] S610. Use a saturation function to dynamically limit the input quantity;

[0076] S611. Integrating the above steps, we obtain the detailed governing equations, expressed as follows:

[0077] ;

[0078] Where, α p α i α d k represents the change in the corresponding parameter after adjustment by the fuzzy control rules. po k io k do These are the basic parameters of the PID control algorithm.

[0079] In step S66, the three basic parameters of the PID are adjusted online, and the relevant control rules adopt the following control principles:

[0080] When a large error occurs, increase k. p and k i The purpose of this is to quickly eliminate deviations; when the error is within a moderate range, k p and k i To keep the stability within an appropriate range, ensuring response speed while improving k d To suppress overshoot; when the error is within a small range, decrease k. p The role of, and maintaining appropriate k d and k i This achieves precise control. Detailed control rules are shown in Table 1 below.

[0081] Table 1 Fuzzy Rule Control Table

[0082]

[0083] S7. Combining the above control algorithms, fit the overall control law equation and convert the control law into the linear velocities of the left and right tracks of the tracked equipment, thereby realizing the steering and straight-line movement of the tracked equipment; specifically including:

[0084] The specific steering angle required by the tracked agricultural equipment within the calculation cycle is calculated using the control law expression in step S5. Then, it undergoes secondary optimization through a fuzzy PID control algorithm;

[0085] By combining the calculated steering angle, the differential motion model is used to convert the steering angle into the linear velocity of the left and right tracks of the tracked agricultural equipment, thereby realizing the steering of the equipment.

[0086] The navigation hardware control system adopts a closed-loop control architecture based on heading feedback control. It uses heading information obtained from positioning sensors to perform closed-loop adjustment, thereby achieving real-time control of the travel direction of the tracked agricultural equipment.

[0087] Example; This example provides a path tracking control method suitable for tracked agricultural equipment, such as Figure 3 The diagram shown is a flowchart of the method's operation. The method includes the following steps:

[0088] A1. Select a tracked work equipment with differential speed function, and build a navigation and control hardware system according to step S1 of this invention.

[0089] A2. Adjust the key parameters of the algorithm according to the actual situation of the tracked equipment in steps S2-S7 of this invention.

[0090] A3. Using the C++ programming language, rewrite the designed control algorithm. After rewriting, embed the C++ code into the industrial control computer.

[0091] A4. Select the work site; use a remote control to operate the equipment or a handheld path recording device to record the reference path, and save the reference path as a BAT file.

[0092] A5. Save the recorded reference path file in the specified working directory.

[0093] A6. After completing steps A1-A5, power on the device (power on the navigation system and mobile system, etc.).

[0094] A7. After powering on, the device will enter an initialization state, waiting for the remote control to select between automatic and manual operation modes. In manual mode, the device can be operated manually using the remote control; in automatic mode, the device searches for a reference path.

[0095] A8. After the device finds the reference path, it reads and parses the reference path and executes the path tracking control algorithm.

[0096] A9. Before executing the tracking control algorithm, the device relies on GNSS-RTK and an inertial navigation module (INS) for self-positioning to determine its specific location in the global coordinate system. GNSS-RTK and the inertial navigation module complement each other during sensor-based positioning. When GNSS-RTK signals are unavailable, the inertial navigation module is used for short-term positioning.

[0097] A10. After the sensor positioning provides positioning data during equipment operation, the heading angle data is filtered using a designed Kalman filter algorithm.

[0098] A11. The system determines whether there is an attitude angle based on the sensor positioning data, and uses the attitude-position error coupling correction equation in step S3 of the present invention to compensate the positioning point of the device.

[0099] A12. Calculate the specific steering angle of the equipment by using the current speed of the equipment and the curvature change of the local reference path in front of the equipment. The calculation method adopts steps S4 and S5 of the present invention.

[0100] A13. After calculating the control law required for steering, the fuzzy PID control algorithm performs secondary adjustment and optimization of the steering angle based on the adjustment error and the error change law; the calculation steps are as shown in step S6 of this invention.

[0101] A14. The final control command is converted into the linear velocity of the left and right tracks of the tracked equipment through the differential motion model and input to the microcontroller.

[0102] A15. After the chassis drive system executes the command, the sensor positioning system provides feedback on the execution result to start the next control cycle. The control cycle will end only when the system switches to manual mode.

[0103] This invention utilizes a heading feedback control mechanism to construct a closed-loop control system for real-time heading correction. It integrates the lateral error adjustment of the Stanley algorithm and the curvature prediction function of the Pure Pursuit algorithm to design the core control algorithm. A secondary optimization of the control law is performed using an offline parameter optimization mechanism and a fuzzy PID algorithm, further improving the tracking accuracy and adaptability of the control system. All possible scenarios that may occur during equipment operation are organized using a fuzzy rule table to cope with various emergencies and improve the system's robustness.

[0104] It should also be noted that, in this specification, terms such as "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0105] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A path tracking control method suitable for tracked agricultural equipment, characterized in that, Includes the following steps: S1. Build a navigation hardware control system on tracked equipment; S2. Design a Kalman filter algorithm to filter the heading angle of tracked equipment; S3. Correct the equipment positioning error caused by the attitude angle; S4. Design a fusion algorithm of Stanley and Pure Pursuit algorithms to make the lateral control strength of the working equipment directly determined by the geometric pre-aiming scale, thereby avoiding the influence of speed changes on lateral control, eliminating instability problems under low-speed conditions, and improving the stability and interpretability of the algorithm on variable speed motion and tracked working equipment. S5. Design an enhanced SPP algorithm to dynamically adjust the control gain and aiming distance based on the equipment speed and path curvature, ensuring the smoothness of the equipment in straight lines and the sensitivity when turning, and further improving the stability and accuracy of equipment tracking under variable speed and complex paths. S6. Use the fuzzy PID control algorithm to perform secondary optimization on the control law output by the SPP algorithm; S7. By combining the above control algorithms to fit the overall control law equation, the control law is converted into the linear velocity of the left and right tracks of the tracked equipment, thereby realizing the turning and straight-line movement of the tracked equipment.

2. The path tracking control method for tracked agricultural equipment according to claim 1, characterized in that, Step S1 is as follows: Select a tracked work equipment with differential motion function and install a navigation hardware control system, which includes a positioning system and a decision-making system. The positioning system includes a GNSS-RTK and an inertial navigation module, which are used to detect changes in the device's attitude angles and provide the device's coordinate information; The decision-making system is an industrial control computer used to receive and process sensor data and issue control commands based on the processing results.

3. The path tracking control method for tracked agricultural equipment according to claim 1, characterized in that, Step S2 specifically includes: S21. The heading angle difference δ measured by GNSS-RTK and inertial navigation module ψk The angular velocity w of the device at a specific moment k and the zero bias b of the inertial navigation system k Let this be the state variable; using the angular velocity measured by the inertial navigation module as the control input, and determining the relevant process noise, the state equation is determined based on the above variable information, and is expressed as: ； Where, x k It is the current state vector; x k-1 It is the state vector of the previous time step; F is the state transition matrix; w k-1 It is process noise; w ψ,k w w,k w b,k These are the noises corresponding to the state variables; S22. Based on the measured value z k and the observation matrix H and the associated measurement noise v k The observation equation is constructed as follows: ； S23. Construct prediction equations based on state equations; In the prediction stage, perform prior estimation of state variables based on the system state transition model, and update the corresponding state covariance matrix synchronously. S24. Construct an update equation based on the observation equation; in the update stage, calculate the Kalman gain based on the observation model and measurement information, correct the predicted state, and thus obtain the posterior estimate of the state.

4. The path tracking control method for tracked agricultural equipment according to claim 1, characterized in that, Step S3 specifically includes: S31. Obtain the attitude angle information of agricultural equipment through the positioning system, including yaw angle ψ, pitch angle θ and roll angle φ; S32. When the attitude angle of agricultural equipment changes, the spatial offset between the positioning sensor and the equipment's positioning center undergoes rotational mapping in the global coordinate system, resulting in errors in the coordinate values ​​of the equipment's positioning center in the global coordinate system. This is corrected based on the attitude angle, and the attitude-position error coupling correction equation is: ； ； Where (x, y, z) represents the device position information measured by the positioning system in the global coordinate system; (a, b, h) represents the position offset of the device positioning point relative to the origin of the vehicle coordinate system; E -1 (ψ, θ, φ) is the rotation transformation matrix from the vehicle coordinate system to the global coordinate system; (x a , y a , z a This refers to the position information of the vehicle origin in the global coordinate system after calibration.

5. A path tracking control method for tracked agricultural equipment according to claim 1, characterized in that, Step S4 specifically includes: S41. Based on the Stanley algorithm and combined with the pre-aiming characteristics of the Pure Pursuit algorithm, the velocity term in the Stanley algorithm is replaced with a dynamic pre-aiming distance. The optimized control equations are as follows: ； Where δ is the control output of the algorithm; For heading angle error; e y d represents the lateral error; d represents the dynamic aiming distance; k represents the lateral error. a This is a comprehensive control gain, including the base gain coefficient k0 and the speed gain coefficient k. v and the gain coefficient k of path curvature k ;k a Represented as: ； Where k is the curvature of the local path in front of the device.

6. The path tracking control method for tracked agricultural equipment according to claim 1, characterized in that, Step S5 specifically includes: Due to numerous interference factors during actual operation, tracked agricultural equipment suffers from center distance error and heading angle error. Center distance error is the fixed spatial offset between the positioning sensor and the positioning center defined by the equipment. Heading angle error is the difference between the heading angle estimated by the positioning sensor and the actual heading angle of the equipment. To address the center distance error, the tracked agricultural equipment was set to different speeds, and the constant positioning point error of the equipment was measured experimentally. The average value of the measured errors was then used as the compensation value e. yb The compensation is added to the control law; For heading angle error compensation, if an uneliminable steady-state error still exists after center distance error compensation, secondary compensation is performed using the following heading angle error compensation formula: ； in, The steady-state error is given by v, where v is the speed of the device; k is the speed of the device. j These are the relevant gain coefficients, and the final parameters can be determined through experimental testing later. This is the calculated error correction value; To achieve dynamic adaptive adjustment of the aiming distance, the velocity, the curvature of the reference path, and the corresponding gain coefficient are incorporated into the dynamic adjustment of the aiming distance. Therefore, the control equation is: ； Based on the above optimizations, the control law expression for the algorithm is determined as follows: 。 7. A path tracking control method for tracked agricultural equipment according to claim 6, characterized in that, Step S5 also includes: The gain coefficient in the control law is experimentally measured in practical applications using a step-by-step testing method, including: Set the velocity and curvature gain parameters to 0; The SPP algorithm is embedded into the test equipment, and the maximum and minimum aiming distances suitable for tracked equipment are determined by using a step-by-step testing method. After determining the range of aiming distances, the most suitable aiming distances at different speeds and curvatures are tested respectively. Then, the data are integrated to determine the most suitable speed gain parameters and curvature gain parameters. By using the above methods, it is ensured that during the tracking process, the aiming distance is adaptively adjusted according to the changes in the speed of the tracked agricultural equipment and the curvature of the reference path.

8. A path tracking control method for tracked agricultural equipment according to claim 1, characterized in that, Step S6 includes: S61. Construct the algorithm control architecture of fuzzy PID; use fuzzy rules to tune the three basic parameters of the PID algorithm online, including the proportional coefficient k. p Integral coefficient k i and the differential coefficient k d ; S62. Establish a discrete-form PID control algorithm, as shown in the following formula: ； Where w(k) is the control output at the current time; e(k) is the error at the current time; S63. Obtain the basic parameters of the PID control algorithm, including the proportional coefficient k, through experimental testing using the critical oscillation method. p0 Integral coefficient k i0 and the differential coefficient k d0 ; S64. Using error and error rate of change as input parameters; obtaining the effective intervals of error and error rate of change through experiments to determine the universe of discourse; calculating the scaling factor based on the set standardization interval [-1,1], and using the scaling factor to quantify the input parameters; S65. Using fuzzy language, divide the fuzzy variables into 7 levels {NB, NM, NS, ZO, PS, PM, PB}, and calculate the membership values ​​of the input parameters using the triangular membership function. S66. A fuzzy rule control table is established using a hierarchical control strategy to adjust the three basic parameters of the PID online. S67. Based on the 7×7 fuzzy rule, calculate the trigger strength of the rule using multiplication; S68. Based on the experimentally measured values ​​of the parameters required by the PID control algorithm under various operating conditions, independent constant conclusion matrices are used to represent the adjustment amounts of the three basic parameters of the PID control algorithm. S69. Use the zero-order Sugeno weighted average method for fuzzy calculation; S610. Use a saturation function to dynamically limit the input quantity; S611. Integrating the above steps, we obtain the detailed governing equations, expressed as follows: ； Where, α p α i α d k represents the change in the corresponding parameter after adjustment by the fuzzy control rules. po k io k do These are the basic parameters of the PID control algorithm.

9. A path tracking control method for tracked agricultural equipment according to claim 8, characterized in that, In step S66, the three basic parameters of the PID are adjusted online, and the relevant control rules adopt the following control principles: When a large error occurs, increase k. p and k i The purpose of this is to quickly eliminate deviations; when the error is within a moderate range, k p and k i To keep the stability within an appropriate range, ensuring response speed while improving k d To suppress overshoot; when the error is within a small range, decrease k. p The role of, and maintaining appropriate k d and k i This allows for precise control.

10. A path tracking control method for tracked agricultural equipment according to claim 6, characterized in that, Step S7 specifically includes: The specific steering angle required by the tracked agricultural equipment within the calculation cycle is calculated using the control law expression in step S5. Then, it undergoes secondary optimization through a fuzzy PID control algorithm; By combining the calculated steering angle, the differential motion model is used to convert the steering angle into the linear velocity of the left and right tracks of the tracked agricultural equipment, thereby realizing the steering of the equipment. The navigation hardware control system adopts a closed-loop control architecture based on heading feedback control. It uses heading information obtained from positioning sensors to perform closed-loop adjustment, thereby achieving real-time control of the travel direction of the tracked agricultural equipment.

Images