Intelligent agricultural machine trajectory tracking control method based on adaptive model prediction

Abstract

The invention discloses an intelligent agricultural machine trajectory tracking control method based on adaptive model prediction. The method comprises the following steps: establishing a discrete linear state space equation based on an agricultural machine kinematics tracking error model; a fuzzy controller is designed by taking the lateral error and the vehicle speed as input, adaptive adjustment is performed on a prediction time domain, a current optimal prediction time domain is calculated, and the trajectory tracking precision is improved; inputting the optimal prediction time domain into a prediction control model of the controller, designing an objective function according to the model, and converting a solving process into a constrained quadratic programming problem; the target function of the model prediction controller is solved, a first variable of an optimal solution is obtained, the variable serves as the control quantity increment at the current moment, and the control input of the agricultural machine is adjusted. Aiming at the problem that a fixed prediction time domain model prediction control algorithm is difficult to meet the path tracking accuracy requirement during trajectory tracking control, the trajectory tracking accuracy and the site adaptability in the agricultural machinery operation process are improved to the maximum extent.

Description

Technical Field

[0001] The present invention relates to the technical field of agricultural machinery trajectory tracking control, and in particular to an intelligent agricultural machinery trajectory tracking control method based on adaptive model prediction. Background Art

[0002] With the continuous improvement of the level of agricultural mechanization, agricultural machinery automation technology has been widely used in agricultural production, especially in the field of precision agriculture. However, although agricultural machinery autonomous driving technology has made significant progress in improving operation efficiency, how to accurately track the driving trajectory of agricultural machinery, especially in complex field environments, is still a major challenge facing the current agricultural mechanization field. Traditional model predictive control methods usually rely on a fixed prediction time domain, which lacks sufficient adaptability in complex agricultural environments, resulting in insufficient control accuracy and response speed, and it is difficult to meet the needs of efficient and accurate operations. Therefore, how to overcome the limitations of traditional model predictive control methods in complex environments, improve their adaptability and accuracy, and achieve more efficient trajectory tracking and operation optimization, is a key issue that needs to be urgently solved in the current agricultural automation field. Summary of the invention

[0003] In order to overcome the deficiencies in the prior art, the present invention provides an intelligent agricultural machinery trajectory tracking control method based on adaptive model prediction, which improves the agricultural machinery trajectory tracking control effect to the greatest extent.

[0004] In order to achieve the above object, the method comprises the following steps:

[0005] Step S1, establishing a discrete linear state space equation based on the agricultural machinery kinematic tracking error model;

[0006] Step S2, designing a fuzzy controller with the lateral error and vehicle speed as input, adaptively adjusting the prediction time domain, calculating the current optimal prediction time domain, and improving the trajectory tracking accuracy;

[0007] Step S3, inputting the optimal prediction time domain into the predictive control model of the controller, designing the objective function according to the model, and converting the solution process into a constrained quadratic programming problem;

[0008] Step S4, solving the objective function of the model predictive controller to obtain the first variable of the optimal solution, which is used as the control variable increment at the current moment to adjust the control input of the agricultural machinery.

[0009] As a further solution of the present invention: the specific steps of step S1 include:

[0010] Establish kinematic model of agricultural machinery:

[0011]

[0012] Among them, x and y are the coordinates of the agricultural machinery on the X-axis and Y-axis respectively. is the heading angle of the agricultural machinery, l is the wheelbase of the agricultural machinery, and δ is the front wheel turning angle.

[0013] The established nonlinear kinematic model of agricultural machinery needs a lot of computing resources to predict the future state of agricultural machinery. Therefore, it is necessary to linearize the model. r ,u r ) is expanded using Taylor series:

[0014]

[0015] The linear time-varying agricultural machinery error model is obtained as follows:

[0016]

[0017] The controller is usually solved in the discrete time domain, while the dynamic behavior of agricultural machinery is usually continuous. In order to perform predictive control, it needs to be discretized so that the system can adapt to the digital computing environment. The above formula is discretized by forward Euler, and we get:

[0018]

[0019] After sorting, the discrete linear state space equation based on the agricultural machinery kinematic tracking error model is obtained:

[0020]

[0021] As a further solution of the present invention: the specific steps of step S2 include:

[0022] The lateral error indicates the degree of deviation of the vehicle from the target trajectory. If the lateral error is large, it means that the vehicle is far away from the target trajectory, and the system needs longer time to adjust to restore to the desired trajectory. Therefore, the larger the lateral error, the longer the prediction time domain is usually required to correct the path. When the lateral error is small, the vehicle is closer to the target trajectory, and a shorter prediction time domain can be used to maintain smooth trajectory tracking. When the vehicle speed is low, the prediction time domain parameters are selected in a smaller range; when the vehicle speed is high, the prediction time domain parameter values ​​are selected in a larger range to ensure the normal and good operation of the vehicle controller. In summary, the lateral error e is selected. Y and vehicle speed v as the input of the fuzzy controller, the prediction time domain N of the model predictive control P As the output of the fuzzy controller.

[0023] Fuzzy adaptive control design is divided into three stages: fuzzification, fuzzy reasoning and defuzzification.

[0024] Considering the actual working needs of agricultural machinery, the basic domain of lateral error is determined to be [0, 12] (unit: m), the driving speed of agricultural machinery is generally 6-8 km / h, and its domain range is [0, 2.5] (unit: m / s), and the domain range of the prediction time domain is N P =[5,35]. The lateral error of agricultural machinery, operating speed and prediction time domain are all set to 7 fuzzy subsets, namely {SS (very small), HS (small), S (small), M (medium), B (large), HB (very large), SB (very large)}. In terms of membership function selection: Gaussian membership functions are used at both ends of the input and output domains to make the control more stable, and triangular membership functions are used in the middle area of ​​the input and output domains to ensure higher accuracy and sensitivity.

[0025] The fuzzy rule base contains fuzzy rules that consider the relationship between input and output. These rules are constructed based on the impact of tracking effect and actual control experience, as shown in Table 1:

[0026] Table 1 Prediction time domain fuzzy rules

[0027]

[0028] The two input quantities of agricultural machinery lateral error and operating speed are subjected to fuzzy reasoning to obtain fuzzy output. The centroid method is selected to defuzzify and obtain the accurate output prediction time domain.

[0029] As a further solution of the present invention: the specific steps of step S3 include:

[0030] Define the output equation:

[0031]

[0032] Construct a new state vector:

[0033]

[0034] The new state space expression is:

[0035]

[0036] Among them, N u Represents the number of control quantities, 0 represents a matrix of a certain dimension, The representative dimension is N u The identity matrix of .

[0037] So the new output equation is:

[0038]

[0039] Among them, N x Represents the number of state quantities, The representative dimension is N x The identity matrix of .

[0040] According to the prediction time domain provided by the adaptive fuzzy control module, N P And the original control time domain is N C , the predicted output of the system can be calculated:

[0041] Y=ψξ(k)+ΘΔU

[0042] in,

[0043]

[0044] The objective function is a key part of model predictive control. It is used to evaluate the effects of different control strategies and provide a basis for the optimization of the controller. The expression of the objective function is selected as:

[0045]

[0046] Among them, Y r is the expected value, Q Q is the state quantity weight coefficient matrix, R R is the control quantity weight coefficient matrix, ε is the relaxation factor, and ρ is the relaxation factor weight coefficient.

[0047] Transform the objective function into and simplify it into the standard form of quadratic programming to obtain:

[0048]

[0049] Where H = Θ T Q Q Θ+R R , g = E T Q Q Θ

[0050] In practical applications, the control inputs and states of agricultural machinery are usually subject to some physical limitations, so it is necessary to add these constraints during the optimization process.

[0051]

[0052] In the formula, ΔU min and ΔU max are the minimum and maximum values ​​of the control increment, U min and U max are the minimum and maximum values ​​of the control amount respectively.

[0053] As a further solution of the present invention: the specific steps of step S4 include:

[0054] The core of model predictive control is to generate a series of control actions by predicting and optimizing the future state of the system. Since the controller usually re-solves the objective function at each moment, it will optimize the control strategy according to the current state to better adjust the control input. The first optimized solution can be directly applied to the control system as the control quantity increment to achieve real-time adjustment of the system state.

[0055] In each control cycle, according to the constraints, a quadratic planner is used to solve the objective function and calculate a series of optimal control input increments in the control domain:

[0056] ΔU(k)=[Δu(k), Δu(k+1),…, Δu(k+N C -1)]

[0057] Apply the first element in the above equation to the system, and the actual system control input u(k) = u(k-1) + Δu(k). At the next moment, recalculate the optimal control input to achieve continuous control of the system.

[0058] Compared with the prior art, the intelligent agricultural machinery trajectory tracking control method based on adaptive model prediction provided by the present invention can improve the agricultural machinery trajectory tracking accuracy by adopting the above-mentioned technical scheme, and can obtain a relatively accurate operation path, thereby effectively reducing labor intensity, improving operation quality, and improving overall work efficiency, providing technical reference and support for future research and application. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Figure 1 A system diagram of the intelligent agricultural machinery trajectory tracking control method based on adaptive model prediction provided by the present invention;

[0060] Figure 2 It is a schematic diagram of the kinematic model of the agricultural machinery of the present invention;

[0061] Figure 3 It is a schematic diagram of the influence of the prediction time domain on the path tracking control in the model predictive control of the present invention;

[0062] Figure 4 Schematic diagram of the adaptive fuzzy controller of the present invention. DETAILED DESCRIPTION

[0063] The specific implementation of the present invention is further described in detail below in conjunction with the drawings and examples.

[0064] like Figure 1 As shown in the overall flow chart, in an embodiment of the present invention, a method for intelligent agricultural machinery trajectory tracking control based on adaptive model prediction includes the following specific steps:

[0065] Step S1, establishing a discrete linear state space equation based on the agricultural machinery kinematic tracking error model.

[0066] In this embodiment, Figure 2 As shown, the kinematic model of agricultural machinery is established:

[0067]

[0068] Among them, x and y are the coordinates of the agricultural machinery on the X-axis and Y-axis respectively. is the heading angle of the agricultural machinery, l is the wheelbase of the agricultural machinery, and δ is the front wheel turning angle.

[0069] The established nonlinear kinematic model of agricultural machinery needs a lot of computing resources to predict the future state of agricultural machinery. Therefore, it is necessary to linearize the model. r ,u r ) is expanded using Taylor series:

[0070]

[0071] The linear time-varying agricultural machinery error model is obtained as follows:

[0072]

[0073] The controller is usually solved in the discrete time domain, while the dynamic behavior of agricultural machinery is usually continuous. In order to perform predictive control, it needs to be discretized so that the system can adapt to the digital computing environment. The above formula is discretized by forward Euler, and we get:

[0074]

[0075] After sorting, the discrete linear state space equation based on the agricultural machinery kinematic tracking error model is obtained:

[0076]

[0077] Step S2: design a fuzzy controller with the lateral error and vehicle speed as input, adaptively adjust the prediction time domain, calculate the current optimal prediction time domain, and improve the trajectory tracking accuracy.

[0078] In this embodiment, Figure 3 As shown in the figure, when other parameters remain unchanged, the prediction step size N of the model predictive controller is P The larger the N is, the more the system can predict the path in the future and obtain more information about the status of the agricultural machinery. PIf N is too large, the error weight of the agricultural machinery farther away from the current position will increase, thereby reducing the path tracking accuracy near the current position. In addition, this will also increase the computational burden of the model predictive control algorithm and affect the real-time performance of the system. On the contrary, if N P If it is too small, the future state information obtained will be relatively limited. Especially when the system control amount is constrained, the agricultural machinery may not be able to adjust its direction in time, which will seriously affect the path tracking accuracy and system stability, and may even cause path tracking failure.

[0079] like Figure 4 As shown, the fuzzy adaptive control design is divided into three stages: fuzzification, fuzzy reasoning, and defuzzification. The lateral error indicates the degree of deviation of the vehicle from the target trajectory. If the lateral error is large, it means that the vehicle is farther away from the target trajectory, and the system needs a longer time to adjust to restore to the desired trajectory. Therefore, the larger the lateral error, the longer the prediction time domain is usually required to correct the path. When the lateral error is small, the vehicle is closer to the target trajectory, and a shorter prediction time domain can be used to maintain smooth trajectory tracking. When the vehicle speed is low, the prediction time domain parameters are selected in a smaller range; when the vehicle speed is high, the prediction time domain parameter values ​​are selected in a larger range to ensure the normal and good operation of the vehicle controller. In summary, the lateral error e is selected. Y and vehicle speed v as the input of the fuzzy controller, the prediction time domain N of the model predictive control P As the output of the fuzzy controller.

[0080] Considering the actual working needs of agricultural machinery, the basic domain of lateral error is determined to be [0, 12] (unit: m), the driving speed of agricultural machinery is generally 6-8 km / h, and its domain range is [0, 2.5] (unit: m / s), and the domain range of the prediction time domain is N P =[5,35]. The lateral error of agricultural machinery, operating speed and prediction time domain are all set to 7 fuzzy subsets, namely {SS (very small), HS (small), S (small), M (medium), B (large), HB (very large), SB (very large)}. In terms of membership function selection: Gaussian membership functions are used at both ends of the input and output domains to make the control more stable, and triangular membership functions are used in the middle area of ​​the input and output domains to ensure higher accuracy and sensitivity.

[0081] The fuzzy rule base contains fuzzy rules that consider the relationship between input and output. These rules are constructed based on the impact of tracking effect and actual control experience, as shown in Table 1:

[0082] Table 1 Prediction time domain fuzzy rules

[0083]

[0084]

[0085] The two input quantities of agricultural machinery lateral error and operating speed are subjected to fuzzy reasoning to obtain fuzzy output. The centroid method is selected to defuzzify and obtain the accurate output prediction time domain.

[0086] Step S3: input the optimal prediction time domain into the predictive control model of the controller, design the objective function according to the model, and transform the solution process into a constrained quadratic programming problem.

[0087] In this embodiment, the output equation is defined as:

[0088]

[0089] Construct a new state vector:

[0090]

[0091] The new state space expression is:

[0092]

[0093] Among them, N u Represents the number of control quantities, 0 represents a matrix of a certain dimension, The representative dimension is N u The identity matrix of .

[0094] So the new output equation is:

[0095]

[0096] Among them, N x Represents the number of state quantities, IN x The representative dimension is N x The identity matrix of .

[0097] According to the prediction time domain provided by the adaptive fuzzy control module, N P And the original control time domain is N C , the predicted output of the system can be calculated:

[0098] Y=ψξ(k)+ΘΔU

[0099] in,

[0100]

[0101] The objective function is a key part of model predictive control. It is used to evaluate the effects of different control strategies and provide a basis for the optimization of the controller. The expression of the objective function is selected as:

[0102]

[0103] Among them, Y r is the expected value, Q Q is the state quantity weight coefficient matrix, R R is the control quantity weight coefficient matrix, ε is the relaxation factor, and ρ is the relaxation factor weight coefficient.

[0104] Transform the objective function into and simplify it into the standard form of quadratic programming to obtain:

[0105]

[0106] Where H = Θ T Q Q Θ+R R , g = E T Q Q Θ

[0107] In practical applications, the control inputs and states of agricultural machinery are usually subject to some physical limitations, so it is necessary to add these constraints during the optimization process.

[0108]

[0109] In the formula, ΔU min and ΔU max are the minimum and maximum values ​​of the control increment, U min and U max are the minimum and maximum values ​​of the control amount respectively.

[0110] Step S4, solving the objective function of the model predictive controller to obtain the first variable of the optimal solution, which is used as the control variable increment at the current moment to adjust the control input of the agricultural machinery.

[0111] In this embodiment, the core of model predictive control is to generate a series of control actions by predicting and optimizing the future state of the system. Since the controller usually re-solves the objective function at each moment, it optimizes the control strategy according to the current state to better adjust the control input. The first optimized solution can be directly applied to the control system as the control quantity increment to achieve real-time adjustment of the system state.

[0112] In each control cycle, according to the constraints, a quadratic planner is used to solve the objective function and calculate a series of optimal control input increments in the control domain:

[0113] ΔU(k)=[Δu(k), Δu(k+1),…, Δu(k+N C 1)]

[0114] Apply the first element in the above equation to the system, and the actual system control input u(k) = u(k-1) + Δu(k). At the next moment, recalculate the optimal control input to achieve continuous control of the system.

[0115] The preferred embodiments of the present invention are described in detail above in conjunction with the accompanying drawings. However, the present invention is not limited to the specific details in the above embodiments. Within the technical concept of the present invention, the technical solution of the present invention can be subjected to a variety of simple modifications, and these simple modifications all belong to the protection scope of the present invention. It should also be noted that the various specific technical features described in the above specific embodiments can be combined in any suitable manner without contradiction. In order to avoid unnecessary repetition, the present invention will not further describe various possible combinations. In addition, the various different embodiments of the present invention can also be arbitrarily combined, as long as they do not violate the concept of the present invention, they should also be regarded as the contents disclosed by the present invention.

Claims

1. Intelligent agricultural machinery trajectory tracking control method based on adaptive model prediction, characterized in that: The following steps are involved: Step S1, establishing a discrete linear state space equation based on the kinematic tracking error model of the agricultural machinery; Step S2, designing a fuzzy controller with the lateral error and vehicle speed as input, adaptively adjusting the prediction time domain, calculating the current optimal prediction time domain, and improving the trajectory tracking accuracy; Step S3, inputting the optimal prediction time domain into the predictive control model of the controller, designing the objective function according to the model, and converting the solution process into a constrained quadratic programming problem; Step S4, solving the objective function of the model predictive controller to obtain the first variable of the optimal solution, which is used as the control variable increment at the current moment to adjust the control input of the agricultural machinery.

2. The intelligent agricultural machinery trajectory tracking control method based on adaptive model prediction according to claim 1 is characterized in that: The step S1 specifically includes the following steps: Establish kinematic model of agricultural machinery: Among them, x and y are the coordinates of the agricultural machinery on the X-axis and Y-axis respectively. is the heading angle of the agricultural machinery, l is the wheelbase of the agricultural machinery, and δ is the front wheel turning angle; The established nonlinear kinematic model of agricultural machinery predicts the future state of agricultural machinery, which requires a lot of computing resources. Therefore, it is necessary to linearize the model and transform the kinematic equation at the reference trajectory point (X r ,u r ) is expanded using Taylor series: The linear time-varying agricultural machinery error model is obtained as follows: The controller is usually solved in the discrete time domain, while the dynamic behavior of agricultural machinery is usually continuous. In order to perform predictive control, it needs to be discretized so that the system can adapt to the digital computing environment. The above formula is discretized by forward Euler, and we get: After sorting, the discrete linear state space equation based on the agricultural machinery kinematic tracking error model is obtained:

3. The intelligent agricultural machinery trajectory tracking control method based on adaptive model prediction according to claim 1 is characterized in that: The step S2 specifically includes the following steps: The lateral error indicates the degree of deviation of the vehicle from the target trajectory. If the lateral error is large, it means that the vehicle is far away from the target trajectory, and the system needs longer time to adjust to restore to the expected trajectory. Therefore, the larger the lateral error, the longer the prediction time domain is usually required to correct the path; when the lateral error is small, the vehicle is close to the target trajectory, and a shorter prediction time domain can be used to maintain a stable trajectory tracking. When the vehicle speed is low, the prediction time domain parameters are selected in a smaller range; when the vehicle speed is high, the prediction time domain parameter values ​​are selected in a larger range to ensure the normal and good operation of the vehicle controller. In summary, the lateral error e is selected. Y and vehicle speed v as the input of the fuzzy controller, the prediction time domain N of the model predictive control P As the output of the fuzzy controller.

4. The intelligent agricultural machinery trajectory tracking control method based on adaptive model prediction according to claim 1 is characterized in that: The specific steps of step S3 include: Define the output equation: Construct a new state vector: The new state space expression is: Among them, N u Represents the number of control quantities, 0 represents a matrix of a certain dimension, The representative dimension is N u The identity matrix of So the new output equation is: Among them, N x Represents the number of state quantities, The representative dimension is N x The identity matrix of According to the prediction time domain provided by the adaptive fuzzy control module, N P And the original control time domain is N C , the predicted output of the system can be calculated: Y=ψξ(k)+ΘΔU in, The objective function is a key part of model predictive control. It is used to evaluate the effects of different control strategies and provide a basis for the optimization of the controller. The expression of the objective function is selected as: Among them, Y r is the expected value, Q Q is the state quantity weight coefficient matrix, R R is the control quantity weight coefficient matrix, ε is the relaxation factor, and ρ is the relaxation factor weight coefficient; Transform the objective function into, simplify it into the standard form of quadratic programming, and get: where H = Θ T Q Q Θ + R R , g = E T Q Q Θ In practical applications, the control inputs and states of agricultural machinery are usually subject to some physical limitations, so it is necessary to add these constraints during the optimization process: In the formula, ΔU min and ΔU max are the minimum and maximum values ​​of the control increment, U min and U max are the minimum and maximum values ​​of the control quantity respectively.

5. The intelligent agricultural machinery trajectory tracking control method based on adaptive model prediction according to claim 1 is characterized in that: The specific steps of step S4 include: The core of model predictive control is to generate a series of control actions by predicting and optimizing the future state of the system. Since the controller usually re-solves the objective function at each moment, it will optimize the control strategy according to the current state to better adjust the control input. The first optimized solution can be directly applied to the control system as the control quantity increment to achieve real-time adjustment of the system state. In each control cycle, according to the constraints, the quadratic planner is used to solve the objective function and calculate a series of optimal control input increments in the control time domain: ΔU(k)=[Δu(k),Δu(k+1),…,Δu(k+N C -1)] Apply the first element in the above formula to the system, and the actual system control input u(k)=u(k-1)+Δu(k). At the next moment, recalculate the optimal control input to achieve continuous control of the system.

6. The intelligent agricultural machinery trajectory tracking control method based on adaptive model prediction according to claim 3 is characterized in that: The fuzzy adaptive control design in step S2 is divided into three stages: fuzzification, fuzzy reasoning and defuzzification; Considering the actual working needs of agricultural machinery, the basic domain of lateral error is determined to be [0, 12] (unit: m), the driving speed of agricultural machinery is generally 6-8 km / h, and its domain range is [0, 2.5] (unit: m / s), and the domain range of the prediction time domain is N P =[5,35]; The lateral error of agricultural machinery, operating speed and prediction time domain are all set to 7 fuzzy subsets, namely {SS (super small), HS (small), S (small), M (medium), B (large), HB (very large), SB (super large)}; In terms of membership function selection: Gaussian membership functions are used at both ends of the input and output domains to make the control more stable, and triangular membership functions are used in the middle area of ​​the input and output domains to ensure higher accuracy and sensitivity.

Images