Tractor path tracking control method and system based on model predictive control

Abstract

The invention belongs to the technical field of path planning, and provides a tractor path tracking control method and system based on model predictive control, which utilizes a model predictive control algorithm to carry out tractor path tracking control, and optimizes weight matrix parameters in the model predictive control algorithm by introducing a particle swarm optimization algorithm. The real-time performance of a control algorithm is improved while the path tracking precision is ensured, so that the automatic driving tractor can deal with the problems of external interference and uncertainty of a field environment, and the tractor is ensured to advance according to a predefined path; meanwhile, when tractor path tracking control is carried out, vehicle speed control constraint and front wheel rotation angle control constraint are added, and by controlling and limiting the vehicle speed and the front wheel rotation angle, the stability of the tractor in the environment that the road surface is wet, slippery and uneven is improved, and the risk of damage to crops is reduced.

Description

Technical Field

[0001] The present invention belongs to the technical field of path planning, and in particular relates to a tractor path tracking control method and system based on model predictive control. Background Art

[0002] The rapid development of modern agriculture has made the precise positioning and safe obstacle avoidance technology of agricultural vehicles increasingly a key link in agricultural production. With the continuous advancement of automobile automatic navigation technology and the rise of tractor power-combined continuously variable transmission technology, the application of automatic driving in agricultural tractors has become an important trend.

[0003] Tractors are the most important agricultural power machinery, providing power for heavy-load operations such as plowing and rotary tillage, medium-load operations such as harrowing and ridging, light-load operations such as plant protection and sowing, and road transportation operations. In order to adapt to complex operating conditions, tractors have far more gears than road vehicles, and some power-shift models even have more than 80 gears, which puts high demands on the realization of unmanned tractor operations. Agricultural tractors mainly travel in fields, where road conditions are poor, the road surface is slippery and uneven, the road adhesion coefficient is low, there is uncertainty, and external interference in the field environment affects the stability of tractor automatic driving; in particular, when the tractor is automatically driving, there is a large lateral error and longitudinal error between the tracking path, which affects farmland operations. For example, when the tractor is automatically loosening the soil or applying fertilizer, if there is a lateral error, it will cause the crops on both sides of the tractor to be crushed, causing economic losses. Summary of the invention

[0004] In order to solve the above problems, the present invention proposes a tractor path tracking control method and system based on model predictive control. The present invention optimizes the weight matrix parameters in the model predictive control algorithm by introducing a particle swarm algorithm, thereby improving the real-time performance of the control algorithm while ensuring the path tracking accuracy, so that the automatic driving tractor can cope with external interference and uncertainty problems in the field environment, and ensure that the tractor moves along a predefined path; at the same time, when performing tractor path tracking control, vehicle speed control constraints and front wheel angle control constraints are added, and by limiting the vehicle speed and front wheel angle, the stability of the tractor on a slippery and uneven road surface is improved, thereby reducing the risk of damage to crops.

[0005] In order to achieve the above object, the present invention is implemented through the following technical solutions: In a first aspect, the present invention provides a tractor path tracking control method based on model predictive control, comprising: Establish tractor kinematic model; According to the tractor kinematic model, the tractor path tracking control is carried out using the model predictive control algorithm; When performing tractor path tracking control, vehicle speed control constraints and front wheel steering angle control constraints are added; and in the model predictive control algorithm, the docking elements of the state weight matrix are defined as position error and heading angle error parameters, and the particle swarm algorithm is used to optimize the position error and heading angle error parameters in the state weight matrix.

[0006] Furthermore, the tractor kinematic model is a two-degree-of-freedom kinematic model: = ; in, , Along the preset coordinate axis The first order differential of is the yaw angle of the tractor; is the first-order differential of the tractor’s yaw angle; is the tractor front wheel turning angle; is the center speed of the tractor rear axle; It is the wheelbase between the front and rear axles of the tractor.

[0007] Furthermore, the model predictive control algorithm includes a particle swarm algorithm module and a control program module. The particle swarm algorithm module is used to cyclically generate new position error and heading angle error parameters, and after sorting the performance indicators, the optimal performance indicator is selected to determine whether the termination condition is met. If the termination condition is met, the algorithm ends; otherwise, the particle swarm parameters are updated and the updated particle swarm parameters are transmitted to the control program module; the control program module recalculates the performance indicators based on the updated particle swarm parameters.

[0008] Further, the position error, heading angle error, vehicle speed and tractor front wheel turning angle are regarded as four individuals in one particle; according to the vehicle speed control constraint and the tractor front wheel turning angle constraint, as well as the preset error range value, the value range of the position error, heading angle error, vehicle speed and tractor front wheel turning angle is determined, and the combination of different individual values ​​is regarded as a different particle; within the value range, the preset position error, heading angle error, vehicle speed and tractor front wheel turning angle values ​​are regarded as the optimal particle for initialization; The sum of the position error, heading angle error and the tractor front wheel angle value is taken as the fitness; When the vehicle speed is fixed, if the current fitness of the particle is the smallest, the current position error, heading angle error, vehicle speed and tractor front wheel angle are updated to the individual best; The individual optimal values ​​of position error, heading angle error, vehicle speed and tractor front wheel turning angle are input into the control program module. If the output performance index of the program control module does not meet the termination condition, the current actual vehicle speed of the tractor and the tractor front wheel turning angle are updated into the algorithm, and the current position error and heading angle error are adjusted with the fitness unchanged as advance, and the adjusted front position error and heading angle error are continuously input into the control program module, and it is continuously judged whether the output performance index meets the termination condition; the fitness is repeatedly evaluated, and the individual optimal is updated until the maximum number of iterations is reached or the termination condition is met.

[0009] Furthermore, in the model predictive control algorithm, a vehicle speed control constraint and a front wheel steering angle control constraint are added; the vehicle speed control constraint and the front wheel steering angle control constraint are: ; ; in, is the center speed of the tractor rear axle; The expected vehicle speed, Speed ​​increment; is the vehicle speed control constraint range adjustment coefficient; The vehicle speed increment control constraint range adjustment coefficient; It is the front wheel steering angle control constraint range adjustment coefficient.

[0010] Furthermore, the vehicle speed control constraint range adjustment coefficient The initial value of is 0. If the tractor bumps more than the preset number of times within the preset time, the speed control constraint range adjustment coefficient will be increased with each increase in the preset number of times. Assign a preset negative value. At the same time, compared with the preset ridge width, the vehicle speed control constraint range adjustment coefficient is adjusted by each preset value when the preset ridge width is reduced. Assign a preset negative value; vehicle speed increment control constraint range adjustment coefficient The initial value of is 0. If the tractor bumps more than the preset number of times within the preset time, the speed increment control constraint range adjustment coefficient will be increased with each increase in the preset number of times. Assign a preset negative value. At the same time, compared with the preset ridge width, every time the preset value is reduced, the speed increment control constraint range adjustment coefficient Assign a preset negative value; front wheel steering angle control constraint range adjustment coefficient The initial value of is also 0. If the number of tractor bumps exceeds the preset number within the preset time, the front wheel steering angle control constraint range adjustment coefficient will be increased with each increase in the preset number of times. Assign a preset negative value. At the same time, compared with the preset ridge width, the front wheel steering angle control constraint range adjustment coefficient is adjusted for each preset value reduced. Assign a preset negative value.

[0011] Furthermore, the model predictive control algorithm is: ; in, Represents the system prediction output matrix; Represents the state quantity prediction parameter of the system; represents the prediction parameter of the control quantity sequence, Indicates the control increment sequence, control amount , is the center speed of the tractor rear axle, is the tractor front wheel turning angle; is the change at each moment compared to the previous moment.

[0012] In a second aspect, the present invention further provides a tractor path tracking control system based on model predictive control, comprising: The kinematic model building module is configured to: build a tractor kinematic model; The path tracking module is configured to: perform tractor path tracking control using a model predictive control algorithm according to a tractor kinematic model; When performing tractor path tracking control, vehicle speed control constraints and front wheel steering angle control constraints are added; and in the model predictive control algorithm, the docking elements of the state weight matrix are defined as position error and heading angle error parameters, and the particle swarm algorithm is used to optimize the position error and heading angle error parameters in the state weight matrix.

[0013] In a third aspect, the present invention further provides an electronic device comprising a memory, a processor, and a computer program stored in the memory and capable of running on the processor, wherein when the processor executes the program, the steps of the tractor path tracking control method based on model predictive control described in the first aspect are implemented.

[0014] In a fourth aspect, the present invention further provides a computer program product, comprising a computer program, and when the computer program is executed by a processor, the steps of the tractor path tracking control method based on model predictive control described in the first aspect are implemented.

[0015] Compared with the prior art, the present invention has the following beneficial effects: 1. In the present invention, a model predictive control algorithm is used to perform tractor path tracking control. In the model predictive control algorithm, the docking elements of the state weight matrix are defined as position error and heading error parameters. The position error and heading error parameters in the state weight matrix are optimized using a particle swarm algorithm. By introducing the particle swarm algorithm to optimize the weight matrix parameters in the model predictive control algorithm, the real-time performance of the control algorithm is improved while ensuring the path tracking accuracy, so that the automatic driving tractor can cope with external interference and uncertainty problems in the field environment and ensure that the tractor moves along a predefined path. At the same time, when performing tractor path tracking control, vehicle speed control constraints and front wheel angle control constraints are added. By limiting the control of the vehicle speed and the front wheel angle, the stability of the tractor in a slippery and uneven road environment is improved, and the risk of damage to agricultural crops is reduced. 2. In the present invention, when determining the vehicle speed control constraint range and the front wheel angle control constraint range, an adjustment coefficient is added to ensure the stability of the vehicle form as much as possible, thereby reducing the risk of crushing crops. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] The drawings in the specification that constitute a part of this embodiment are used to provide a further understanding of this embodiment. The schematic embodiments of this embodiment and their descriptions are used to explain this embodiment and do not constitute improper limitations on this embodiment.

[0017] Figure 1 The tractor kinematic model of embodiment 1 of the present invention; Figure 2 The particle swarm algorithm of the embodiment 1 of the present invention optimizes the MPC (Model Predictive Control) weight parameter process; Figure 3 Path tracking comparison before and after MPC optimization in Example 1 of the present invention; Figure 4 Comparison of lateral displacement before and after MPC optimization in Example 1 of the present invention; Figure 5 The longitudinal displacement comparison before and after MPC optimization of Example 1 of the present invention; Figure 6 The steering angles before and after MPC optimization in Example 1 of the present invention are compared; Figure 7 This is a comparison diagram of the tracking effects of the PID and MPC controllers of Example 1 of the present invention; Figure 8 Path tracking comparison before and after the improved MPC and PID path tracking of Example 1 of the present invention; Fig. 9 Comparison of lateral displacement before and after improved MPC and PID path tracking in Example 1 of the present invention; Fig.10 The longitudinal displacement comparison before and after the improved MPC and PID path tracking of Example 1 of the present invention; Fig.11 Comparison of the front and rear steering angles of the improved MPC and PID path tracking in Example 1 of the present invention. DETAILED DESCRIPTION

[0018] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0019] It should be noted that the following detailed descriptions are exemplary and are intended to provide further explanation of the present application. Unless otherwise specified, all technical and scientific terms used herein have the same meanings as those commonly understood by those skilled in the art to which the present application belongs.

[0020] Embodiment 1: This embodiment provides a tractor path tracking control method based on model predictive control, improves the model predictive control (MPC) algorithm, and provides a weight coefficient optimization method based on a particle swarm algorithm. The traditional method of determining the weight coefficient requires first selecting a parameter and adjusting another parameter based on the result, which is inefficient and cannot guarantee the best performance, but is only approximately optimal. Therefore, this embodiment introduces a particle swarm algorithm with good search ability to solve the optimization problem, and this embodiment performs constraint solving optimization on the control input to obtain the optimal control input. The improved MPC framework enhances the robustness of the algorithm to uncertainty and external disturbances, can effectively cope with external interference in the environment, maintain the stable output of the system, and ensure the stable operation of the tractor, which is of great significance for improving the driving performance of the tractor automatic driving.

[0021] In this embodiment, a tractor kinematic model is first established, and then an MPC controller is designed based on the tractor kinematic model. Finally, two sets of comparative experiments are conducted through joint simulation experiments of Carsim and Simulink to verify the effectiveness of the control method from the aspects of path tracking, lateral displacement, longitudinal displacement, and steering angle. Specifically, it includes: S1. Establish tractor kinematic model: The tractor has a low speed when operating, generally 0.5m / s to 3.0m / s. As a low-speed moving vehicle, factors such as side slip and roll during the tractor's driving process can be ignored. The trajectory tracking controller designed using only the kinematic model has reliable control performance. To simplify the analysis, this embodiment uses a two-degree-of-freedom kinematic model, such as Figure 1 shown.

[0022] set up( )and( ) are the position coordinates of the center points of the tractor front axle and the tractor rear axle; and Along the coordinate axis and The first order differential of is the yaw angle of the tractor; is the tractor front wheel turning angle; is the center speed of the tractor rear axle; is the center speed of the vehicle's front axle; is the wheelbase of the front and rear axles of the tractor. is the rear wheel turning radius, is the instantaneous rotation center of the tractor; is the rear axle center, The center of the front axle.

[0023] At rear axle center At the rear axle center speed for:

[0024] The kinematic constraint equations of the front and rear axles are:

[0025] From the above two formulas we can get:

[0026] According to the geometric relationship between the front wheel and the rear wheel:

[0027] From the above formula, we can get the yaw angular velocity for:

[0028] At the same time, the yaw angular velocity and vehicle speed The rear wheel turning radius can be obtained and tractor front wheel angle .

[0029]

[0030] From the above formula, the tractor kinematic model can be obtained as: =

[0031] The tractor kinematic model can be further transformed into the general form of the control system, namely: =

[0032] Among them, the state quantity , control amount .

[0033] S2. MPC algorithm modeling and improvement: S2.1. Establish tractor error model: The tractor kinematic model regards the tractor as a rigid body and mainly considers the relationship between the tractor's posture, speed and front wheel angle. The posture includes position coordinates and heading angle, and does not consider the influence of any force. Compared with the nonlinear model prediction, the linear time-varying model prediction has the advantages of small calculation amount and high control accuracy. Therefore, in this embodiment, the tractor kinematic model is first converted into a linear time-varying model. When establishing a linear time-varying model, a state space equation is established to facilitate the input of the control quantity to obtain the ideal state value. With the help of the linear time-varying model, the state of the tractor in the short future can be predicted, and the discretized linear model provides the basis for this prediction. The discrete linear model is:

[0034] in, for the moment; is the state transition matrix, the system changes from arrive changes; is the input matrix, the impact of the input on the system; for The input amount at the time.

[0035] set up is the prediction time domain of the system; Predict the state quantity of the system in the time domain; is the control time domain of the system; The control quantity of the prediction system in the time domain, is the relaxation factor weight coefficient; is the relaxation factor; is the change in each moment compared to the previous moment; The state quantity at the moment is:

[0036] in, is a vector, indicating the The post-calculation of the system status at any moment.

[0037] The resulting state space expression is:

[0038] in, For the moment About the moment predictions; A is the state transition matrix; B is the input matrix.

[0039] ,

[0040] in, is the state transfer matrix; is the input matrix; It is a zero matrix and there is no influence of external input on the system.

[0041] The state and output of the system in the prediction time domain can be expressed as:

[0042]

[0043] Combining the above formula, we can calculate the system t Prediction model at the moment Y for:

[0044]

[0045]

[0046]

[0047] in, Represents the system prediction output matrix; Represents the state quantity prediction parameter of the system; represents the prediction parameter of the control quantity sequence, Indicates the control increment sequence.

[0048] S2.2, rolling optimization: The control input sequence in the prediction time domain is obtained through rolling optimization:

[0049] According to the basic principle of predictive control, the latest measured value is used as the initial condition to predict the future dynamics of the system based on the model. An effective objective function can not only help the vehicle quickly track the desired trajectory, but also ensure the stability and safety of the vehicle during the tracking process. Therefore, it is necessary to design an optimization objective function that can comprehensively consider the system state quantity deviation and control quantity. The objective function is:

[0050] in, To control the output prediction value; To control the output reference value; and express Control input and control input increment at all times; is the relaxation factor weight coefficient; and is the system output, control input and control input increment weight matrix; is the relaxation factor.

[0051] In order to ensure the accuracy and stability of tractor trajectory tracking, it is necessary to constrain the control increment and control amount in the control process. The specific constraints set are as follows: For the constraints controlling the increment:

[0052] in, is the minimum value of the control increment; The maximum value of the control increment.

[0053] For the control quantity constraints:

[0054] in, is the minimum value of the control amount; is the maximum value of the control amount.

[0055] The MPC controller minimizes the objective function under the premise of satisfying the control constraints:

[0056] The objective function of the above formula is processed by citing the quadratic programming (QP) problem, and the optimization objective function is:

[0057] According to the designed objective function, combined with the relevant parameters of the unmanned vehicle and the experimental results of the lateral and longitudinal tracking ability test, the vehicle speed control constraints and the front wheel angle control constraints are set.

[0058] Vehicle speed control constraints:

[0059] in, The expected vehicle speed, Speed ​​increment.

[0060] Front wheel steering angle control constraints:

[0061] in, Front wheel steering angle increment.

[0062] After parameter optimization, the system convergence speed is improved and the system robustness is stronger.

[0063] S2.3, Improvements to vehicle speed control constraints and front wheel steering angle control constraints: When a tractor is operating on an uneven field, the risk of crushing crops is mainly determined by the stability of the tractor itself and the width of the ridges between operations. For example, if the tractor is more stable and the ridge width is larger, the risk of crushing crops is smaller. On the contrary, if the tractor is more unstable and the ridge width is smaller, the risk of crushing crops is greater. Based on this, when determining the vehicle speed control constraint range and the front wheel angle control constraint range, an adjustment coefficient is added to try to ensure the stability of the vehicle form and reduce the crushing of crops. Specifically: Vehicle speed control constraints:

[0064] Front wheel steering angle control constraints:

[0065] in, is the vehicle speed control constraint range adjustment coefficient; The vehicle speed increment control constraint range adjustment coefficient; It is the front wheel steering angle control constraint range adjustment coefficient.

[0066] Specifically, the vehicle speed control constraint range adjustment coefficient The initial value of is 0. If the number of bumps of the tractor exceeds the preset number within the preset time, it means that the bump degree is more serious than the preset degree. For each additional number of times, the speed control constraint range adjustment coefficient Assign a preset negative value. For example, if the number of bumps of the tractor exceeds the preset number within two minutes, it means that the bump degree is more serious than the preset degree. For every 10 times the number of bumps increases, the speed control constraint range adjustment coefficient Accumulate and assign -0.01, and improve the stability of the tractor by limiting the speed; if the number of tractor bumps within the preset time is lower than the preset number, it means that the degree of bumps is lighter than the preset degree, and the speed control constraint range adjustment coefficient is adjusted. At the same time, compared with the preset ridge width, each reduction of a preset value indicates that the ridge width is reduced, and each reduction of a preset value indicates that the vehicle speed control constraint range adjustment coefficient is Assign a preset negative value. For example, if the preset ridge width is 70cm, every 5cm reduction means that the allowed working space is smaller and the crops are more likely to be crushed. At this time, every 5cm reduction in ridge width means the speed control constraint range adjustment coefficient is Accumulate and assign -0.01, and improve the stability of the tractor by limiting the speed, and reduce the damage to the operation; the ridge width is 70cm or more, which means that the allowed working space is large, and the speed control constraint range adjustment coefficient is used at this time is 0.

[0067] Vehicle speed increment control constraint range adjustment coefficient The initial value of is also 0. If the number of tractor bumps exceeds the preset number within the preset time, it means that the bump degree is more serious than the preset degree. For each additional certain number of times, the speed increment control constraint range adjustment coefficient Assign a preset negative value. For example, if the number of bumps of the tractor exceeds the preset number within two minutes, it means that the bump degree is more serious than the preset degree. For every 10 times the number of bumps increases, the speed increment control constraint range adjustment coefficient Accumulate and assign -0.001 to improve the stability of the tractor by limiting the acceleration; the number of tractor bumps within the preset time is lower than the preset number, indicating that the degree of bumps is lighter than the preset degree. At this time, the speed increment control constraint range adjustment coefficient At the same time, compared with the preset ridge width, each reduction of a preset value indicates that the ridge width is reduced, and each reduction of a preset value indicates that the vehicle speed increment control constraint range adjustment coefficient is Assign a preset negative value. For example, if the preset ridge width is 70cm, every 5cm reduction means that the allowed working space is smaller and the crops are more likely to be crushed. At this time, every 5cm reduction in ridge width means that the vehicle speed increment controls the constraint range adjustment coefficient. Accumulate and assign -0.01, improve tractor stability by limiting acceleration and reduce damage to ridge operation; ridge width is 70cm or greater, indicating that the allowed operating space is large, at this time the vehicle speed increment controls the constraint range adjustment coefficient is 0.

[0068] Front wheel steering angle control constraint range adjustment coefficient The initial value of is also 0. If the number of bumps of the tractor exceeds the preset number within the preset time, it means that the degree of bumps is more serious than the preset degree. Assign a preset negative value. For example, if the number of bumps of the tractor exceeds the preset number within two minutes, it means that the degree of bumps is more serious than the preset degree. For every 10 times the number of bumps increases, the front wheel steering angle control constraint range adjustment coefficient Accumulate and assign -2° to improve the stability of the tractor by limiting the front wheel turning angle; if the number of tractor bumps within the preset time is lower than the preset number, it means that the degree of bumps is lighter than the preset degree, and the front wheel turning angle control constraint range adjustment coefficient is adjusted. At the same time, compared with the preset ridge width, each decrease of a preset value indicates that the ridge width is reduced, and each decrease of a preset value indicates that the front wheel steering angle control constraint range adjustment coefficient is Assign a preset negative value. For example, if the preset ridge width is 70cm, every 5cm reduction means that the allowed working space is smaller and the crops are more likely to be crushed. At this time, every 5cm reduction in ridge width means that the front wheel steering angle control constraint range adjustment coefficient Accumulate and assign -2°, improve tractor stability by limiting the front wheel rotation and reduce damage to ridge operation; ridge width is 70cm or greater, indicating that the allowed operating space is large, at this time the front wheel turning angle control constraint range adjustment coefficient is 0.

[0069] S2.3. Optimize MPC weight coefficient based on particle swarm algorithm: Usually, the selection of matrix parameters depends on experience and experiments. Determine one parameter by experiment first, and adjust another parameter according to the result. This method is often inefficient and cannot guarantee the best performance, but is only approximately optimal. Therefore, this embodiment introduces a particle swarm algorithm with good search ability to solve the optimization problem. This embodiment uses a particle swarm algorithm to optimize and solve the weight coefficient in the model predictive control algorithm.

[0070] The optimal control quantity of each iteration process is calculated by the weight matrix Q and matrix R Decision Matrix Q and matrix R As the most important parameter in the control algorithm, it determines the performance of the control system; Q and R They are two quantities that constrain each other and have no definite values.

[0071] In this embodiment, there are two control variables, the front wheel angle and the speed, in the MPC model, so the control weight matrix is ​​set to the unit matrix. Q The diagonal parameters in represent the weights of various errors, and in the vehicle tracking process we focus on position error and heading angle error, so the matrix is ​​set to:

[0072] Among them, the position error a and heading angle error b These are the two parameters that need to be optimized. The particle swarm algorithm is used to optimize these two parameters in the state weight matrix Q. The specific parameter optimization algorithm flow chart is as follows: Figure 2 shown.

[0073] Define the matrix R as the control input weight, which includes the vehicle speed and tractor front wheel angle There are two parameters to be optimized.

[0074] Depend on Figure 2 As shown, the algorithm can be divided into two modules. The first is the particle swarm algorithm module, which is mainly responsible for cyclically generating new position errors. a and heading angle error b The parameter values, as well as the sorting of performance indicators to select the optimal performance indicator to see if it meets the termination condition. Secondly, the MPC control program module mainly adds the part of updating particle parameters and calculating performance indicators, and uses the MPC objective function as the performance indicator of the particle swarm algorithm.

[0075] Position error a and heading angle error b , vehicle speed and tractor front wheel angle There are two parameters to be optimized.

[0076] S2.3.1. Initializing particles: position error a , heading angle error b , vehicle speed and tractor front wheel angle As four individuals in one particle. Optionally, the position error is determined based on the vehicle speed control constraint and the tractor front wheel angle constraint, as well as the preset error range value. a , heading angle error b , vehicle speed and tractor front wheel angle The value range of different individual values ​​is regarded as a different particle; within the value range, the preset positioning error a , heading angle error b , vehicle speed and tractor front wheel angle The value is used as the optimal particle for initialization.

[0077] S2.3.2, Evaluate fitness: Calculate the fitness value of each particle, using the position error a , heading angle error b and tractor front wheel angle Minimum is the objective function, which determines the fitness value of each particle. Optionally, the position error a , heading angle error b , tractor front wheel angle The sum of the values ​​is taken as the fitness.

[0078] S2.3.3. Update individual best and global best: At speed In the case of fixed, if the current fitness of the particle is the smallest, the current position error a , heading angle errorb , vehicle speed and tractor front wheel angle Updated to individual best.

[0079] S2.3.4, Iteration: Position error a , heading angle error b , vehicle speed and tractor front wheel angle The individual optimal value is input into the control program module. If the output performance index of the program control module does not meet the termination condition, the actual speed of the tractor is and tractor front wheel angle Update to the algorithm, with the fitness unchanged as advance, and the current position error a and heading angle error b Make adjustments and continue to adjust the front position error a and heading angle error b Input to the control program module, and continue to determine whether the output performance index meets the termination condition. Repeat the fitness evaluation and update the individual best until the maximum number of iterations is reached or the termination condition is met. Of course, in some embodiments, after iterating to a preset number of times, the iteration termination time can be shortened by appropriately adjusting the fitness.

[0080] S3. Simulation analysis and verification: S3.1. Tracking comparison experiment before and after MPC improvement: The tractor mainly travels in the field, so the road conditions are not good, the road surface is slippery and uneven, and the road adhesion coefficient is low, ranging from 0.2 to 0.6. In the simulation, in order to simulate the real environment, the general soil-road adhesion coefficient is set to 0.4, and the plant protection and pesticide application task is completed at a speed of 3.6 km / h. The tractor-related physical parameters involved in trajectory tracking control are shown in Table 1.

[0081] Table 1 Tractor physical parameters

[0082] In order to verify the effectiveness of the MPC algorithm in the trajectory tracking control of unmanned tractors, MATLAB / Simulink and Carsim were used for joint simulation. The tractor model, road model, and path trajectory were set through Carsim. Under the same hardware and software environment, other parameters remained unchanged. The weight parameters selected by experience and the weight parameters obtained by particle swarm optimization were input respectively, and finally the reference trajectory was tracked respectively. The simulation results of the experimental method and particle swarm optimization method parameter tracking effect are shown in the figure. Figure 3 , Figure 4 , Figure 5 and Figure 6 shown.

[0083] Figure 3 It can be seen from the MPC tracking algorithm after particle swarm optimization parameters can make the tractor return to the reference path more quickly. Figure 4 and Figure 5 From the path tracking error diagram, we can see that the particle swarm optimization method can reduce the overall horizontal and vertical path tracking errors of MPC. Figure 6 From the steering angle comparison chart, it can be seen that the improved controller can converge faster than the unimproved one.

[0084] S3.2. Tracking comparison experiment of improved MPC and PID: Since the driving road of a tractor is different from that of an ordinary car, it needs to travel along the planned path according to different operating tasks. In an orchard, a tractor generally performs agricultural activities such as land preparation, spraying, and fertilizing. The model in Carsim is combined with the MPC controller designed by Simulink, and a comparative experiment on path tracking is carried out with the PID method.

[0085] like Figure 7 As shown in Figure 1, it is described that the MPC controller tracks the tractor path better than the PID controller. When the tractor is controlled by MPC, the tractor can track the given driving route well during the actual driving process.

[0086] from Figure 8 From the above, the improved MPC path tracking performance is better than PID, and the improved MPC algorithm makes the tractor closer to the desired trajectory. Fig. 9 and Fig.10 It can be seen that the improved MPC can more effectively reduce the deviation of lateral and longitudinal displacements. Under the action of the improved MPC controller, the accurate trajectory tracking task can be effectively completed. Fig.11 It can be seen that the front wheel steering angle controlled by the two methods has always been kept within a reasonable range of physical limitations of the steering structure, but the control tracking effect of the improved MPC is better.

[0087] In summary, from the perspective of trajectory tracking, both the MPC control algorithm and the PID algorithm can achieve good tracking effects, but the tracking accuracy of the MPC algorithm is higher, and the steering angle control outputs of the two algorithms are also controlled within the constraint range. The improved MPC algorithm has better tracking effects.

[0088] Embodiment 2: This embodiment provides a tractor path tracking control system based on model predictive control, including: The kinematic model building module is configured to: build a tractor kinematic model; The path tracking module is configured to: perform tractor path tracking control using a model predictive control algorithm according to a tractor kinematic model; When performing tractor path tracking control, vehicle speed control constraints and front wheel steering angle control constraints are added; and in the model predictive control algorithm, the docking elements of the state weight matrix are defined as position error and heading angle error parameters, and the particle swarm algorithm is used to optimize the position error and heading angle error parameters in the state weight matrix.

[0089] The working method of the system is the same as the tractor path tracking control method based on model predictive control in Example 1, and will not be repeated here.

[0090] Embodiment 3: This embodiment provides an electronic device, including a memory, a processor, and a computer program stored in the memory and capable of running on the processor. When the processor executes the program, the steps of the tractor path tracking control method based on model predictive control described in Example 1 are implemented.

[0091] Embodiment 4: This embodiment provides a computer program product, which includes a computer program. When the computer program is executed by a processor, the steps of the tractor path tracking control method based on model predictive control described in Example 1 are implemented.

[0092] The above description is only a preferred embodiment of the present embodiment and is not intended to limit the present embodiment. For those skilled in the art, the present embodiment may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present embodiment shall be included in the protection scope of the present embodiment.

Claims

1. A tractor path tracking control method based on model predictive control, characterized in that: include: Establish tractor kinematic model; According to the tractor kinematic model, the tractor path tracking control is carried out using the model predictive control algorithm; When performing tractor path tracking control, vehicle speed control constraints and front wheel steering angle control constraints are added; and in the model predictive control algorithm, the docking elements of the state weight matrix are defined as position error and heading angle error parameters, and the particle swarm algorithm is used to optimize the position error and heading angle error parameters in the state weight matrix.

2. The tractor path tracking control method based on model predictive control according to claim 1, characterized in that: The tractor kinematic model is a two-degree-of-freedom kinematic model: = ； in, , Along the preset coordinate axis The first order differential of ; is the yaw angle of the tractor; is the first-order differential of the tractor’s yaw angle; is the tractor front wheel turning angle; is the center speed of the tractor rear axle; It is the wheelbase between the front and rear axles of the tractor.

3. The tractor path tracking control method based on model predictive control according to claim 1, characterized in that: The model predictive control algorithm includes a particle swarm algorithm module and a control program module. The particle swarm algorithm module is used to cyclically generate new position error and heading angle error parameters, and after sorting the performance indicators, the optimal performance indicator is selected to determine whether the termination condition is met. If the termination condition is met, the algorithm ends; otherwise, the particle swarm parameters are updated and the updated particle swarm parameters are transmitted to the control program module. The control program module recalculates the performance index according to the updated particle swarm parameters.

4. The tractor path tracking control method based on model predictive control according to claim 3, characterized in that: The position error, heading angle error, vehicle speed and tractor front wheel turning angle are regarded as four individuals in one particle; according to the vehicle speed control constraint and the tractor front wheel turning angle constraint, as well as the preset error range value, the value range of the position error, heading angle error, vehicle speed and tractor front wheel turning angle is determined, and the combination of different individual values ​​is regarded as a different particle; within the value range, the preset position error, heading angle error, vehicle speed and tractor front wheel turning angle values ​​are regarded as the optimal particle for initialization; The sum of the position error, heading angle error and the tractor front wheel angle value is taken as the fitness; When the vehicle speed is fixed, if the current fitness of the particle is the smallest, the current position error, heading angle error, vehicle speed and tractor front wheel angle are updated to the individual best; The individual optimal values ​​of position error, heading angle error, vehicle speed and tractor front wheel turning angle are input into the control program module. If the output performance index of the program control module does not meet the termination condition, the current actual vehicle speed of the tractor and the tractor front wheel turning angle are updated into the algorithm, and the current position error and heading angle error are adjusted with the fitness unchanged as advance, and the adjusted front position error and heading angle error are continuously input into the control program module, and it is continuously judged whether the output performance index meets the termination condition; the fitness is repeatedly evaluated, and the individual optimal is updated until the maximum number of iterations is reached or the termination condition is met.

5. The tractor path tracking control method based on model predictive control according to claim 1, characterized in that: In the model predictive control algorithm, vehicle speed control constraints and front wheel steering angle control constraints are added; the vehicle speed control constraints and front wheel steering angle control constraints are: ； ； in, is the center speed of the tractor rear axle; The expected vehicle speed, Speed ​​increment; is the vehicle speed control constraint range adjustment coefficient; The vehicle speed increment control constraint range adjustment coefficient; It is the front wheel steering angle control constraint range adjustment coefficient.

6. The tractor path tracking control method based on model predictive control according to claim 5, characterized in that: Vehicle speed control constraint range adjustment coefficient The initial value of is 0. If the tractor bumps more than the preset number of times within the preset time, the speed control constraint range adjustment coefficient will be increased with each increase in the preset number of times. Assign a preset negative value. At the same time, compared with the preset ridge width, the vehicle speed control constraint range adjustment coefficient is adjusted by each preset value when the preset ridge width is reduced. Assign a preset negative value; vehicle speed increment control constraint range adjustment coefficient The initial value of is 0. If the tractor bumps more than the preset number of times within the preset time, the speed increment control constraint range adjustment coefficient will be increased with each increase in the preset number of times. Assign a preset negative value. At the same time, compared with the preset ridge width, every time the preset value is reduced, the speed increment control constraint range adjustment coefficient Assign a preset negative value; front wheel steering angle control constraint range adjustment coefficient The initial value of is also 0. If the number of tractor bumps exceeds the preset number within the preset time, the front wheel steering angle control constraint range adjustment coefficient will be increased with each increase in the preset number of times. Assign a preset negative value. At the same time, compared with the preset ridge width, the front wheel steering angle control constraint range adjustment coefficient is adjusted for each preset value reduced. Assign a preset negative value.

7. The tractor path tracking control method based on model predictive control according to claim 1, characterized in that: The model predictive control algorithm is: ； in, Represents the system prediction output matrix; Represents the state quantity prediction parameter of the system; represents the prediction parameter of the control quantity sequence, Indicates the control increment sequence, control amount , is the center speed of the tractor rear axle, is the tractor front wheel turning angle; is the change at each moment compared to the previous moment.

8. A tractor path tracking control system based on model predictive control, characterized in that: include: The kinematic model building module is configured to: build a tractor kinematic model; The path tracking module is configured to: perform tractor path tracking control using a model predictive control algorithm according to a tractor kinematic model; When performing tractor path tracking control, vehicle speed control constraints and front wheel steering angle control constraints are added; and in the model predictive control algorithm, the docking elements of the state weight matrix are defined as position error and heading angle error parameters, and the particle swarm algorithm is used to optimize the position error and heading angle error parameters in the state weight matrix.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and capable of running on the processor, characterized in that: When the processor executes the program, the steps of the tractor path tracking control method based on model predictive control as described in any one of claims 1 to 7 are implemented.

10. A computer program product, characterized in that The computer program product comprises a computer program, and when the computer program is executed by a processor, the steps of the tractor path tracking control method based on model predictive control as claimed in any one of claims 1 to 7 are implemented.

Images