CN-121979205-A - Path tracking control method suitable for crawler-type agricultural equipment

CN121979205ACN 121979205 ACN121979205 ACN 121979205ACN-121979205-A

Abstract

The invention discloses a path tracking control method suitable for crawler-type agricultural equipment, which comprises the steps of S1, building a navigation hardware control system on the crawler-type agricultural equipment, S2, designing a Kalman filtering algorithm to filter a course angle of the crawler-type agricultural equipment, S3, correcting equipment positioning errors caused by an attitude angle, S4, designing a fusion algorithm of Stanley and Pure Pursuit algorithms, S5, designing an enhanced SPP algorithm, S6, performing secondary optimization on a control law output by the SPP algorithm by utilizing a fuzzy PID control algorithm, S7, fitting a total control law equation by combining the control algorithms, and converting the control law into linear speeds of left and right tracks of the crawler-type agricultural equipment so as to realize steering and straight running of the crawler-type agricultural equipment. According to the invention, a complex agricultural environment is regarded as a black box system, and item-by-item optimization is performed through superposition of an algorithm with strong interpretability, so that the adaptability of the control system to the black box system is improved, and the step-by-step improvement of the equipment path tracking precision is finally realized.

Inventors

LI JUN
LI YUHAO
CAI JIAMIN
LIN KEJI
ZHU ZHONGYI
Gong Linli
YANG XINGWEI
ZENG YE
HUANG CHENGJIE

Assignees

华南农业大学

Dates

Publication Date: 20260505
Application Date: 20260106

Claims (10)

1. The path tracking control method suitable for the crawler-type agricultural equipment is characterized by comprising the following steps of: s1, building a navigation hardware control system on crawler equipment; S2, designing a Kalman filtering algorithm to filter the course angle of the crawler equipment; s3, correcting equipment positioning errors caused by attitude angles; s4, designing a fusion algorithm of Stanley and Pure burst algorithms, so that the transverse control intensity of the operation equipment is directly determined by a geometric pre-aiming scale, thereby avoiding the influence of speed change on transverse control, eliminating the instability problem under a low-speed working condition, and improving the stability and the interpretability of the algorithm on variable-speed motion and crawler-type operation equipment; S5, designing an enhanced SPP algorithm, dynamically adjusting control gain and pretightening distance according to equipment speed and path curvature, guaranteeing the stability of equipment in straight running and the sensitivity in over-bending, and further improving the stability and precision of equipment tracking under variable speed and complex paths; 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 by combining the control algorithm, and converting the control law into the linear speeds of the left and right tracks of the crawler equipment so as to realize steering and straight running of the crawler equipment.
2. The path tracking control method for crawler-type agricultural equipment according to claim 1, wherein step S1 specifically comprises: Selecting crawler-type operation equipment with a differential motion function, and installing a navigation hardware control system, wherein the navigation hardware control system comprises a positioning system and a decision system; the positioning system comprises a GNSS-RTK and an inertial navigation module, and is used for detecting the change of the attitude angle of the equipment and providing the coordinate information of the equipment; The decision system is an industrial personal computer and is used for receiving and processing the sensor data and issuing a control instruction according to the processing result.
3. The path tracking control method for crawler-type agricultural equipment according to claim 1, wherein step S2 specifically comprises: S21, setting a heading angle difference delta ψk measured by the GNSS-RTK and the inertial navigation module, an angular speed w k of the equipment at a specific moment and a zero offset b k of the inertial navigation system as state variables, taking the angular speed measured by the inertial navigation module as control input, determining relevant process noise, and determining a state equation by combining the variable information, wherein the state equation is expressed as follows: ; wherein x k is the current state vector, x k-1 is the state vector of the last moment, F is the state transition matrix, w k-1 is the process noise, and w ψ,k ,w w,k ,w b,k is the noise of the corresponding state variables respectively; S22, constructing an observation equation according to the measured value z k , the observation matrix H and the related measurement noise v k , wherein the observation equation is expressed as follows: ; s23, constructing a prediction equation based on the state equation, in the prediction stage, carrying out prior estimation on the state variable according to the system state transition model, and synchronously updating the corresponding state covariance matrix; And S24, constructing an update equation based on the observation equation, and calculating a Kalman gain based on the observation model and the measurement information in the update stage to correct the predicted state so as to obtain posterior estimation of the state.
4. The path tracking control method for crawler-type agricultural equipment according to claim 1, wherein step S3 specifically comprises: S31, acquiring attitude angle information of agricultural equipment through a positioning system, wherein the attitude angle information comprises a yaw angle psi, a pitch angle theta and a roll angle phi; S32, when the attitude angle of the agricultural equipment changes, the space offset between the positioning sensor and the equipment positioning center is subjected to rotation mapping in the global coordinate system, so that the coordinate value of the equipment positioning center in the global coordinate system generates errors, and the errors are corrected according to the attitude angle, and an attitude-position error coupling correction equation is as follows: ; ; The method comprises the steps of (x, y, z) measuring equipment position information of a positioning system in a global coordinate system, (a, b, h) measuring the position offset of equipment positioning points relative to an origin of a vehicle body coordinate system, (E -1 (psi, theta, phi) is a rotation transformation matrix from the vehicle body coordinate system to the global coordinate system, and (x a , y a , z a ) is the position information of the origin of the vehicle in the global coordinate system after calibration.
5. The path tracking control method for crawler-type agricultural equipment according to claim 1, wherein step S4 specifically comprises: S41, changing a speed item in the Stanley algorithm into a dynamic pretightening distance based on the Stanley algorithm and combining the pretightening characteristic of the Pure burst algorithm, wherein an optimized control equation is shown as follows: ; Wherein delta is the control output of the algorithm; The heading angle error, e y is a transverse error, d is a dynamic pretightening distance, k a is a comprehensive control gain, and the gain coefficients including a basic gain coefficient k 0 , a speed gain coefficient k v and a path curvature k k ;k a are expressed as: ; where k is the curvature of the local path in front of the device.
6. The path tracking control method for crawler-type agricultural equipment according to claim 1, wherein step S5 specifically comprises: Because the crawler-type agricultural equipment is affected by a plurality of interference factors in the actual operation process, a center distance error and a course angle error exist, wherein the center distance error is a fixed space offset between a positioning sensor and a positioning center defined by the equipment; for the center distance error, setting crawler-type agricultural equipment to different speeds, measuring the constant positioning point error of the equipment through experiments, and then taking the average value of the measured errors as a compensation value e yb to compensate into a control law; for course angle error compensation, if an unavoidable steady-state error still exists after the center distance error compensation, the secondary compensation is performed by the following course angle error compensation formula: ; Wherein, the K j is a related gain coefficient, and final parameters can be determined through experimental tests in the later period; For the calculated error correction value; In order to realize the dynamic self-adaptive adjustment of the pretightening distance, the curvature of the speed and the reference path and the corresponding gain coefficient are introduced into the dynamic adjustment of the pretightening distance, so that the control equation is as follows: ; the control law expression of the optimization determination algorithm is as follows: 。
7. The path-following control method for crawler-type agricultural equipment as set forth in claim 6, wherein step S5 further includes: the gain coefficient in the control law is experimentally measured in the actual application scene by using a step-by-step test method, and comprises the following steps: setting the speed and curvature gain parameters to 0; After determining the range of the pretightening distance, respectively testing the most suitable pretightening distance under different speeds and different curvatures, and then integrating the data to determine the most suitable speed gain parameter and curvature gain parameter; By the method, the change of the pretightening distance along with the speed of the crawler-type agricultural equipment and the curvature of the reference path in the tracking process of the equipment is ensured, and the pretightening distance is adaptively adjusted.
8. A path-following control method for tracked agricultural equipment according to claim 1, wherein step S6 comprises: S61, constructing an algorithm control architecture of fuzzy PID, and setting three basic parameters of the PID algorithm on line by utilizing a fuzzy rule, namely a proportional coefficient k p , an integral coefficient k i and a differential coefficient k d ; S62, establishing a discrete PID control algorithm, wherein the discrete PID control algorithm is shown as the following formula: ; wherein, w (k) is the control output of the current moment, e (k) is the error of the current moment; S63, obtaining basic parameters of a PID control algorithm through experimental tests of a critical oscillation method, wherein the basic parameters comprise a proportional coefficient k p0 , an integral coefficient k i0 and a differential coefficient k d0 ; S64, taking the error and the error change rate as input parameters, obtaining an effective interval of the error and the error change rate through experiments, and determining a domain, calculating a scale factor according to a set standardized interval [ -1,1], and quantizing the input parameters by using the scale factor; S65, dividing the fuzzy variable into 7 layers { NB, NM, NS, ZO, PS, PM, PB }, and calculating a membership value of the input parameter through a triangle membership function; s66, establishing a fuzzy rule control table by adopting a hierarchical regulation strategy, and carrying out online adjustment on three basic parameters of the PID; s67, calculating the triggering strength of the rule by utilizing multiplication according to the 7 multiplied by 7 fuzzy rule; S68, according to the specific parameter values required by the PID control algorithm under various operation conditions, respectively representing the adjustment amounts of three basic parameters of the PID control algorithm by adopting independent constant conclusion matrixes; S69, performing deblurring calculation by adopting a zero-order Sugeno weighted average method; S610, dynamically limiting the input quantity by adopting a saturation function; s611, integrating the above steps to obtain a detailed control equation, which is expressed as: ; Wherein alpha p ,α i ,α d is the variable quantity of the corresponding parameter after being adjusted by the fuzzy control rule, and k po ,k io ,k do is the basic parameter of the PID control algorithm respectively.
9. The path tracking control method for crawler-type agricultural equipment according to claim 8, wherein in step S66, on-line adjustment is performed on three basic parameters of PID, and the following regulation rules are adopted as the relevant regulation rules: When a larger error occurs, the actions of k p and k i are improved so as to quickly eliminate the deviation, when the error is in a middle range, k p and k i are stabilized in a proper range, the response speed is ensured, and meanwhile, k d is improved to inhibit overshoot, and when the error is in a smaller range, the action of k p is reduced, and proper k d and k i are maintained so as to realize accurate control.
10. The path tracking control method for crawler-type agricultural equipment according to claim 6, wherein step S7 specifically comprises: calculating the specific steering angle required by the crawler agricultural equipment in the calculation period through the control law expression in the step S5 Performing secondary optimization through a fuzzy PID control algorithm; Combining the calculated steering angle, and converting the steering angle into the linear speeds of left and right tracks of the crawler-type agricultural equipment through a differential motion model, so as to realize the steering of the equipment; The navigation hardware control system adopts a closed-loop control architecture based on course feedback control, and carries out closed-loop adjustment by using course information acquired by a positioning sensor, so that the real-time control of the running direction of the crawler-type agricultural equipment is realized.

Description

Path tracking control method suitable for crawler-type agricultural equipment Technical Field The invention belongs to the technical field of intelligent navigation, and particularly relates to a path tracking control method suitable for crawler-type agricultural equipment. Background Path tracking is one of the key parts in intelligent navigation technology. The current path tracking algorithm is mainly divided into a data-driven algorithm and a model-driven algorithm. The data driving algorithm does not depend on an accurate mathematical model, learns a system rule through a large amount of historical data or real-time data, establishes a mapping relation and performs decision control. This type of algorithm has strong dependence on data and poor interpretability, and it is difficult to interpret the internal physical mechanism. In practical application, the training process is complex, and the requirement on running equipment is high. The application in the actual engineering project of path tracking is not very extensive. The model driven algorithm is an algorithm for designing a control law or an estimator by using a control theory or an optimization method through analyzing the physical characteristics of a system based on an exponential model or a physical model. The algorithm has wide application in practical engineering projects due to the characteristics of strong interpretability, high stability and the like. The most typical of these are the Pure burst algorithm and the Stanley algorithm. Under a simple environment, the algorithm can rely on a simple kinematic model, so that accurate control can be realized. However, in a complex agricultural scene, to achieve a precise tracking effect, the algorithm needs to take numerous interference factors into consideration to build a complex dynamic model. In agricultural settings, tracked equipment is subject to numerous interference factors in the case of autonomous operation. The coupling effect between the track and the ground can cause slippage, subsidence and variations in adhesion of the device. The shaking of the equipment in the moving process can influence the positioning precision of the positioning system and the measurement data of the attitude angle, so that the tracking precision is reduced. Therefore, there is a need for a path tracking algorithm that can achieve accurate tracking of tracked agricultural equipment without the need for complex kinetic models. Disclosure of Invention The invention mainly aims to overcome the defects and shortcomings of the prior art and provides a path tracking control method suitable for crawler-type agricultural equipment. In order to achieve the above purpose, the present invention adopts the following technical scheme: a path tracking control method suitable for crawler-type agricultural equipment, comprising the steps of: s1, building a navigation hardware control system on crawler equipment; S2, designing a Kalman filtering algorithm to filter the course angle of the crawler equipment; s3, correcting equipment positioning errors caused by attitude angles; s4, designing a fusion algorithm of Stanley and Pure burst algorithms, so that the transverse control intensity of the operation equipment is directly determined by a geometric pre-aiming scale, thereby avoiding the influence of speed change on transverse control, eliminating the instability problem under a low-speed working condition, and improving the stability and the interpretability of the algorithm on variable-speed motion and crawler-type operation equipment; S5, designing an enhanced SPP algorithm, dynamically adjusting control gain and pretightening distance according to equipment speed and path curvature, guaranteeing the stability of equipment in straight running and the sensitivity in over-bending, and further improving the stability and precision of equipment tracking under variable speed and complex paths; 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 by combining the control algorithm, and converting the control law into the linear speeds of the left and right tracks of the crawler equipment so as to realize steering and straight running of the crawler equipment. Compared with the prior art, the invention has the following advantages and beneficial effects: 1. According to the invention, a complex agricultural environment is regarded as a black box system, and item-by-item optimization is performed through superposition of an algorithm with strong interpretability, so that the adaptability of the control system to the black box system is improved, and the step-by-step improvement of the equipment path tracking precision is finally realized. 2. The method for optimizing the course angle data by utilizing the Kalman filtering algorithm disclosed by the invention is used for carrying out fusion optimization on the data