A method of path tracking and control for an agricultural machine

Abstract

The Stanley tracking control method based on particle swarm multi-objective optimization provided in the application uses a particle swarm multi-objective optimization algorithm to optimize the gain coefficient in the Stanley method on the basis of the Stanley geometric model. Compared with the linear model control method and the optimal control method, the method can reduce the dependence on the mathematical model of the agricultural machine, and effectively cope with the complex and changeable orchard environment. Compared with the fuzzy control method and the sliding mode variable structure control method, the method can effectively improve the path tracking accuracy of the agricultural machine. Compared with other tracking and control methods, the method can reduce the time for the agricultural machine to transition from the online phase to the online operation phase, prevent the agricultural machine from skidding during the process of turning at the head of the road with large curvature, and improve the tracking and control accuracy of the curved road.

Description

Technical Field

[0001] This invention relates to the field of agricultural machinery navigation control, and more specifically to an improved Stanley control method for agricultural machinery path tracking. Background Technology

[0002] my country has a vast territory with complex and varied terrain and diverse climates. Statistics show that in 2022, my country's total orchard planting area reached 184.15 million mu (approximately 12.9 million hectares), with a total fruit output exceeding 300 million tons. my country ranks first in the world in both fruit tree planting area and output, becoming the world's largest fruit-producing country. However, correspondingly, the average comprehensive mechanization rate of my country's orchard planting industry is less than 30%, and the level of mechanization cannot meet the needs of modern fruit industry development. Against the backdrop of a declining labor force and the need for orchard industry upgrading in my country, equipping traditional machinery with automatic navigation devices to assist manual operations can save a significant amount of labor and improve operational efficiency.

[0003] From a positioning technology perspective, my country's BeiDou Navigation Satellite System (hereinafter referred to as BeiDou) technology has reached centimeter-level accuracy, meeting the precision requirements of orchard agricultural machinery (hereinafter referred to as agricultural machinery). From a navigation control technology perspective, due to the rapid development of autonomous driving technology, various navigation control methods have been developed. These include linear model control and optimal control methods based on classical control theory, and fuzzy control and sliding mode variable structure control methods based on nonlinear control theory.

[0004] Control methods based on classical control theory require the establishment of kinematic or dynamic models of the vehicle. However, in orchard environments, due to the complex terrain, including hills and mountains, it is difficult to accurately describe the motion state using mathematical models, often resulting in control effects that fall short of expectations. While control methods based on nonlinear control theory do not rely on mathematical models, they suffer from significant tracking errors during the initial setup and turning phases of the agricultural machinery. Therefore, it is essential to research a control method that is independent of the agricultural machinery's mathematical model yet possesses high tracking accuracy. Summary of the Invention

[0005] To overcome the technical problems of existing path tracking and control methods' dependence on agricultural machinery mathematical models and poor tracking performance under sharp turns, this invention provides a Stanley tracking control method based on particle swarm optimization (PSO) for use in the field of orchard agricultural machinery automatic driving. This method can reduce the time spent on agricultural machinery during the online process and improve tracking accuracy during the operation phase.

[0006] To achieve the above objectives, the present invention provides the following solution: a path tracking and control method for agricultural machinery, comprising the following steps:

[0007] Step 1: Record the expected path;

[0008] Step 2: Preprocess satellite and front wheel angle sensor data for the current motion cycle;

[0009] Step 3: Based on the expected path data collected in Step 1 and the agricultural machinery motion status data in Step 2, and combined with the Stanley geometric model, obtain the lateral deviation and heading angle deviation of the agricultural machinery in the current motion cycle; finally, considering the influence of lateral deviation and heading angle deviation on the expected turning angle, derive the expression for the expected turning angle.

[0010] Step 4: Based on the relevant data obtained in Step 2 and Step 3, obtain the parameters to be optimized in the Stanley tracking method through the PSO optimization algorithm, and calculate the expected steering angle of the front wheels;

[0011] Step 5: Perform lateral control on the agricultural machinery and track the expected path in real time.

[0012] Furthermore, in step 1, the expected path is recorded. Specifically, after setting the operation path, start point, and end point in a fixed area, the machine is then driven manually to complete the pre-set path from the origin in the orchard. The data from the Beidou satellite receiver and the front wheel angle sensor during the movement of the agricultural machinery are recorded.

[0013] Furthermore, the satellite and front wheel angle sensor data mentioned in step 2 include the latitude and longitude L(t), speed υ(t), heading angle δ(t), and left front wheel steering angle α within the current motion cycle of the agricultural machinery. L (t), Right front wheel steering angle α R (t), left front wheel yaw rate β L (t), Right front wheel angular velocity β R (t);

[0014] Furthermore, in step 2, the Universal Transverse Mercator (UTM) projection method is used to convert the latitude and longitude L(t) position into a planar position P. r (X r ,Y r Then, the heading angle Y(t) is subjected to average value filtering.

[0015] Furthermore, the front wheel angle sensor data mentioned in step 2 is processed by combining the angle and angular velocity data of the left and right front wheels using linear weighting coefficients. The resulting formula for the front wheel angle sensor is as follows:

[0016]

[0017] In the formula, α is the front wheel angle, β is the front wheel angular velocity, k1 is the nonlinear weighting coefficient of the left front wheel data, and k2 is the nonlinear weighting coefficient of the right front wheel data.

[0018] The values ​​of k1 and k2 are determined using a Gaussian weighting function. When the front wheels of the agricultural machinery are to the left of the vehicle's direction of travel, the linear weighting coefficient k1 has a larger weight, and when the front wheels are to the right of the vehicle's direction of travel, the linear weighting coefficient k2 has a larger weight. This increases the consistency between the final calculated front wheel angular velocity and front wheel angular velocity data and the vehicle's yaw angle and yaw rate data.

[0019] Furthermore, the expected path data set of the agricultural machinery in step 3 can be represented as: {P0(X0,Y0),P1(X1,Y1),P...} k (X k ,Y k ),...P n (X n ,Y n The lateral deviation represents the minimum distance between the current position of the agricultural machinery and all the points in the expected path set, while the heading angle deviation represents the angle between the tangent at the nearest point in the expected path and the current heading line of the agricultural machinery.

[0020] Furthermore, the relevant data in step 4 includes the speed υ(t) of the agricultural machinery during its current motion cycle, the lateral deviation e(t), and the heading angle deviation δ. e (t). The obtained position data, velocity data, and the center angular velocity of the front wheel of the agricultural machinery are input into the PSO optimization algorithm. The algorithm is used to perform continuous iterative calculations to finally determine the optimal value of the parameter to be optimized. The optimal value of the parameter to be optimized is used as the gain coefficient in step 3 to calculate the expected steering angle of the front wheel.

[0021] Furthermore, in step 5, the manual steering wheel on the agricultural machinery is replaced with an electronically controlled steering wheel, thereby achieving lateral control of the agricultural machinery based on the calculated desired front wheel angle.

[0022] The beneficial effects of this invention are as follows:

[0023] The Stanley tracking control method based on particle swarm optimization described in this invention, building upon the Stanley tracking control method based on the Stanley geometric model, proposes using a particle swarm optimization algorithm to optimize the gain coefficients in the Stanley method. Compared to linear model control and optimal control methods, this method reduces the dependence on the mathematical model of agricultural machinery and effectively copes with the complex and ever-changing orchard environment. Compared to fuzzy control and sliding mode variable structure control methods, this method can effectively improve the path tracking accuracy of agricultural machinery. Compared to other tracking and control methods, this method can reduce the time for agricultural machinery to transition from the online stage to the online operation stage, prevent sideslip during large-curvature turn-arounds at the edge of the field, and improve cornering tracking and control accuracy. Attached Figure Description

[0024] Figure 1 This is a flowchart of the path tracking and control method based on agricultural machinery according to the present invention;

[0025] Figure 2 This is a schematic diagram of the Stanley tracking model of the present invention;

[0026] Figure 3 This is a flowchart of the PSO optimization algorithm of the present invention. Detailed Implementation

[0027] To enhance understanding of the present invention, the embodiments will be described in detail below with reference to the accompanying drawings.

[0028] Example 1: See Figure 1 The flowchart of the embodiments of the present invention is as follows: Figure 1 As shown. A path tracking and control method for agricultural machinery, the method comprising the following steps:

[0029] Step 1: Record the expected path.

[0030] Specifically, it involves the following steps:

[0031] 1.1 First, based on the needs of orchard operations and with the opinions of orchard workers, set a fixed operating path and operating origin. Then, using manual driving, run the pre-set path through the orchard from the origin, and record the Beidou satellite receiver data and front wheel angle sensor data during the movement of the agricultural machinery in CSV format files respectively;

[0032] 1.2 Read the CSV file from step 1.1 and extract the latitude and longitude data L(t) and heading angle data Y(t) of the agricultural machinery. Specifically, since the Beidou satellite receiver transmits data in GPRMC string format, the latitude and longitude data and heading angle data can be obtained by extracting characters bit by bit, and then the obtained characters can be converted into numerical values.

[0033] 1.3. The latitude and longitude data L(t) are converted into plane coordinate data L using the Universal Transverse Mercator (UTM) projection method. H (t), where the latitude offset is set to 0;

[0034] 1.4 Considering the widespread turbulence in orchard environments, it is necessary to perform average filtering on the heading angle data Y(t). Specifically, the average of the five most recent heading angle data is taken. The specific filtering principle is: when |Y(t)-Y - If |t| < 2°, then the data Y(t) is considered reliable, i.e., Y(t) = Y - (t);

[0035] 1.5 Finally, the preprocessed planar coordinates L from steps 1.3 and 1.4 will be... H The heading angle Y(t) and the heading angle Y(t) are stored in a file. When the agricultural machinery automatic navigation device performs positioning and navigation work, the file is loaded into the system in real time.

[0036] Step 2: Preprocess the satellite and front wheel angle sensor data for the current motion cycle, as follows:

[0037] The sensor data for the agricultural machinery during its current motion cycle includes data from the BeiDou satellite receiver and data from the front wheel angle sensor. The processing method for the BeiDou satellite receiver data is the same as in steps 1.2 and 1.3 above. For the front wheel angle sensor data, the angular velocity and angle data from the two angle sensors on the left and right front wheels during the current cycle are read, and then the average of the two sensor data is taken as the forward θ(t) = θ. e (t)+θ H The deflection angle β(t) and deflection angular velocity γ(t) of the axis center.

[0038] Step 3: Obtain the lateral deviation and heading angle deviation of the agricultural machinery.

[0039] like Figure 2 As shown, a coordinate system is established using the XOY coordinate system. Where P... r (X r ,Y rLet P(t) represent the current position of the agricultural machinery, and P(t) represent the desired path dataset. From P... r (X r ,Y r Establish a geometric relationship between P(t) and P(t), and find P. r (X r ,Y r The distance between P and all points along the recording path. Specifically, P r (X r ,Y r ) to any point P k (X k ,Y k The distance d) rk The calculation formula is:

[0040]

[0041] In the formula P o P r Let P be a vector pointing from the starting point of the desired path to the current position of the agricultural machinery. r P k Let P be a vector pointing from the current position of the agricultural machinery to any point k on the desired path. o P k This represents a vector that points from the starting point of the desired path to any point k on the desired path.

[0042] Choose the point with the shortest distance as P. m (t).

[0043] like Figure 2 As shown, the control input for the desired front wheel steering angle of the Stanley model consists of two parts: lateral deviation and heading angle deviation. Its output formula is as follows:

[0044] θ(t)=θ e (t)+θ H (t) (3)

[0045] In the formula, θ(t) is the desired rotation angle, θ e (t) represents the desired rotation angle caused by the lateral deviation, θ H (t) represents the desired turning angle caused by the heading angle deviation.

[0046] If we consider the effect of lateral deviation on the expected turning angle alone, we can assume that the nearest point P on the given path is at a distance d(t) in the front wheel direction of the expected trajectory of the agricultural machinery. m The tangents at point (t) intersect, then according to geometric principles:

[0047]

[0048] In the formula, e(t) is the lateral deviation, Q is the gain coefficient, and ν(t) is the agricultural machinery speed.

[0049] If we consider only the effect of heading angle deviation on the desired steering angle, and the front wheel steering angle is in the same direction as the tangent of the expected path, then the desired front wheel steering angle is equal to the angle between the vehicle's heading angle and the tangent of the nearest path point. Therefore, according to geometric principles:

[0050] θ H (t)=δ e (t) (5)

[0051] In the formula δ e (t) represents the heading angle deviation.

[0052] Finally, considering the combined effects of lateral deviation and heading angle deviation on the desired turning angle, the expression for the desired turning angle is derived as follows:

[0053]

[0054] Step 4: Optimize the gain coefficient in the Stanley method using the PSO optimization algorithm.

[0055] To improve the tracking accuracy of the Stanley tracking method and adapt to the widespread bumps and sideslip problems in the complex environment of orchards, the PSO algorithm is used to optimize the gain coefficient Q in the Stanley tracking method, enabling the gain coefficient to adapt to unexpected situations such as vehicle speed and sideslip during the tracking process. The specific steps are as follows:

[0056] 4.1. Taking the gain coefficient Q of the Stanley tracking method as a particle, the state of this particle in N-dimensional space can be represented by its position P. k and speed V k This can be represented as:

[0057]

[0058] In the formula, k ranges from 1 to N, where N represents the total number of particles in the particle swarm.

[0059] The fitness function in the PSO algorithm determines the optimization effect of the algorithm. Combining the design principles of the fitness function and the control indicators for agricultural machinery navigation control, and taking into account path tracking accuracy, steering continuity and agricultural machinery stability, the following fitness function is designed:

[0060]

[0061] In the formula, J1 is the position error index, J2 is the heading error index, and J3 is the angle continuity index. and The weighting coefficients for J1, J2, and J3 are respectively, where:

[0062]

[0063]

[0064]

[0065] In the formula, P(t) is the actual trajectory of the agricultural machinery, P s (t) is the expected path, E^ is the standard threshold for path tracking error, and v x Let y(t) be the vehicle speed in the direction of travel, y(t) be the lateral velocity of the center of gravity of the agricultural machinery, y^ be the standard deviation threshold of the lateral velocity of the center of gravity of the agricultural machinery, β'(t) be the yaw velocity of the agricultural machinery, and β^ be the standard threshold of the yaw velocity of the agricultural machinery.

[0066] The magnitudes of these three weighting coefficients are determined by the importance coefficients of each weight. According to the requirements of agricultural machinery navigation control, The importance decreases sequentially, and then the relative importance coefficient k of adjacent indicators is given by the expert scoring system. i Finally, the formula for calculating the weighting coefficient is obtained:

[0067]

[0068] During PSO optimization, the state update formula for this particle in N-dimensional space is as follows:

[0069]

[0070] In the formula X i For the local optimal position of the particle, G i c1 and c2 are adaptive factors, representing the global optimal position of the particle. Let be the inertia factor, which decreases linearly with the number of iterations. The specific expression is as follows:

[0071]

[0072] In the formula and The weights N are the maximum and minimum inertia factors, respectively. i N represents the current iteration number. all This represents the total number of iterations to date.

[0073] Thus, the gain coefficient Q in Stanley's algorithm can be obtained through the PSO algorithm.

[0074] Step 5: Obtain and output the desired front wheel steering angle.

[0075] Substituting the gain coefficient Q calculated in step 4 into equation (5), the desired front wheel steering angle is finally calculated. Since there are nonlinear characteristics such as gap and saturation between the steering angle of the electronically controlled steering wheel and the front wheel steering angle, a PID control method is adopted here. The calculated desired front wheel steering angle is input into the PID controller, and finally the angular velocity of the electronically controlled steering wheel is output, thereby achieving the accuracy and speed of agricultural machinery steering.

[0076] It should be noted that the above embodiments are not intended to limit the scope of protection of the present invention. Equivalent transformations or substitutions made based on the above technical solutions all fall within the scope of protection of the claims of the present invention.

Claims

1. A path tracking and control method for agricultural machinery, characterized in that, The method includes the following steps: Step 1: Record the expected path; Step 2: Preprocess satellite and front wheel angle sensor data for the current motion cycle; Step 3: Based on the expected path data collected in Step 1 and the agricultural machinery motion status data in Step 2, and combined with the Stanley geometric model, obtain the lateral deviation and heading angle deviation of the agricultural machinery in the current motion cycle; finally, taking into account the influence of the lateral deviation and heading angle deviation on the expected turning angle, derive the expression for the expected turning angle. Step 4: Based on the relevant data obtained in Step 2 and Step 3, obtain the gain coefficient in the Stanley tracking method through the PSO optimization algorithm, and calculate the expected steering angle of the front wheels; In step 5, the agricultural machinery is laterally controlled and the expected path is tracked in real time. In step 3, The Stanley tracking control method based on particle swarm optimization (PSO) is adopted to obtain the expected steering angle of the front wheels. Specifically, the lateral deviation and heading angle deviation of the agricultural machinery in the current motion cycle are obtained through the Stanley geometric model, and the steering angle expression of the Stanley geometric model is derived. Then, the gain coefficient in the Stanley tracking method is obtained by using the particle swarm optimization algorithm. Finally, the steering angle of the front wheels is obtained by substituting it into the steering angle expression of the Stanley geometric model.

2. The path tracking and control method for agricultural machinery according to claim 1, characterized in that, Step 1 is detailed as follows: Step 1.1 First, based on the needs of orchard operations and with reference to the opinions of orchard workers, set a fixed operation path and operation origin. Then, use manual driving to run the pre-set path from the origin in the orchard, and record the Beidou satellite receiver data and front wheel angle sensor data during the movement of the agricultural machinery in CSV format files respectively. Step 1.2: Read the CSV file from Step 1.1 and extract the latitude and longitude data of the agricultural machinery. and heading angle data Specifically, since the data transmitted by the Beidou satellite receiver is in the format of GPRMC zero character string, the latitude and longitude data and heading angle data are obtained by extracting characters bit by bit, and then the obtained characters are converted into numerical values. Step 1.3: Use the Universal Transverse Mercator (UTM) projection method to project the latitude and longitude data. Convert to planar coordinate data The latitude offset is set to 0. Step 1.4: Assess the heading angle data. A mean-value filtering process is performed; specifically, the average value of the five most recent heading angle data is taken. The specific filtering principle is: when Then the data is considered to be... It is reliable, that is ; Step 1.5: Finally, the preprocessed planar coordinates from steps 1.3 and 1.4 will be... and heading angle The file is stored in the system and loaded into the system in real time when the agricultural machinery's automatic navigation device performs positioning and navigation.

3. The path tracking and control method for agricultural machinery according to claim 1, characterized in that, In step 2, the sensor data is preprocessed, specifically: latitude and longitude coordinates are converted to planar coordinates using transverse Mercator projection; the heading angle is filtered using an average value filtering algorithm; and the angle and angular velocity data of the left and right front wheels are synthesized using nonlinear weighting coefficients. The resulting formula for the front wheel angle sensor is as follows: （1） In the formula For the front wheel angle, The angular velocity of the front wheel is... The non-linear weighting coefficients for the left front wheel data are... The non-linear weighting coefficients for the right front wheel data are... in and The magnitudes are determined using a Gaussian weighting function. When the agricultural machinery's front wheels are to the left of the vehicle's forward direction, the nonlinear weighting coefficients... The nonlinear weighting coefficient and the linear weighting coefficient have a relatively large weighting when the front wheels of the agricultural machinery are to the right of the vehicle's direction of travel. This gives it a larger weight, which increases the consistency between the final calculated front wheel angle and front wheel angular velocity data and the vehicle's yaw angle and yaw rate data.

4. The path tracking and control method for agricultural machinery according to claim 1, characterized in that, Step 3: Obtain the lateral deviation and heading angle deviation of the agricultural machinery, as detailed below. by Establish a coordinate system, where, This is the current location of the agricultural machinery. For the desired path dataset, from arrive Establish geometric relationships between them and find The distance between all points on the recording path, specifically... to any point distance The calculation formula is: (2) In the formula This represents a vector pointing from the starting point of the desired path to the current position of the agricultural machinery. Let represent the vector pointing from the current position of the agricultural machinery to any point k on the desired path. Let represent the vector pointing from the starting point of the desired path to any point k on the desired path. Take the point with the shortest distance as , The control input for the desired front wheel steering angle of the Stanley model consists of two parts: lateral deviation and heading angle deviation. Its output formula is as follows: (3) In the formula For the desired turning point, The expected rotation angle caused by lateral deviation. The desired turning angle caused by the heading angle deviation. If we consider the effect of lateral deviation on the expected turning angle alone, we can assume that the point on the given path is the closest point at a distance d(t) in the front wheel direction of the expected trajectory of the agricultural machinery. If the tangents at a point intersect, then according to geometric principles: (4) In the formula Where Q is the lateral deviation and Q is the gain coefficient. For the speed of agricultural machinery, If we consider only the effect of heading angle deviation on the desired steering angle, and the front wheel steering angle is in the same direction as the tangent of the expected path, then the desired front wheel steering angle is equal to the angle between the vehicle's heading angle and the tangent of the nearest path point. Therefore, according to geometric principles: (5) In the formula For heading angle deviation, Finally, considering the combined effects of lateral deviation and heading angle deviation on the desired turning angle, the expression for the desired turning angle is derived as follows: (6)。 5. The path tracking and control method for agricultural machinery according to claim 1, characterized in that, Step 4: Optimize the gain coefficient Q in the Stanley method using the PSO optimization algorithm, as detailed below: The gain coefficient Q of the Stanley tracking method is considered as a particle, and the available positions of this particle in N-dimensional space are... and speed This can be represented as: (7) In the formula, k ranges from 1 to N, where N represents the total number of particles in the particle swarm. The fitness function in the PSO algorithm determines the optimization effect of the algorithm. Combining the design principles of the fitness function and the control indicators for agricultural machinery navigation control, and taking into account path tracking accuracy, steering continuity and agricultural machinery stability, the following fitness function is designed: (8) In the formula For position error index, For heading error index, As an indicator of angular continuity, , and They are respectively , and The weighting coefficients are: (9) (10) (11) In equation (9) This represents the nth cycle of motion. It is the actual trajectory of the agricultural machinery. This is the expected path. This is the standard threshold for path tracking error; In formula (10) The speed of the agricultural machinery in the direction of travel. The side slip angular velocity of the agricultural machinery's center of gravity. The threshold value for the standard deviation of the lateral slip angular velocity of the agricultural machinery's center of gravity; In formula (11) The yaw rate of the agricultural machinery. The standard threshold for the yaw rate of agricultural machinery. The magnitudes of these three weighting coefficients are determined by the importance coefficients of each weight, according to the requirements of agricultural machinery navigation control. The importance of each indicator decreases in that order, and then the relative importance coefficients of adjacent indicators are given through an expert scoring system. Finally, the formula for calculating the weighting coefficient is obtained: (12) During PSO optimization, the state update formula for this particle in N-dimensional space is as follows: (13) In the formula This represents the local optimal position of the particle. This is the global optimal position for the particle. and As an adaptive factor, Let be the inertia factor, which decreases linearly with the number of iterations. The specific expression is as follows: (14) In the formula and These are the weights for the maximum and minimum inertia factors, respectively. This represents the current iteration number. This represents the current total number of iterations. Thus, the gain coefficient Q in the Stanley method can be obtained through the PSO algorithm.

Images