Agricultural tracked vehicle high-precision path tracking control method based on model prediction algorithm

Abstract

The invention discloses an agricultural tracked vehicle high-precision path tracking control method based on a model prediction algorithm. The method comprises the following steps: acquiring motion parameters of an agricultural tracked vehicle; establishing a motion model in a Cartesian coordinate system according to the motion parameters; performing linearization and discretization on the motion model to obtain a discretized linear error model; a reference path sequence is obtained, a quadratic programming problem is constructed and solved based on the discretized linear error model, the target function and the system constraint, and a target control strategy is obtained; and performing tracking control on the agricultural tracked vehicle according to the target control strategy. According to the method, high-precision path tracking of the small crawler-type agricultural vehicle in the hilly area can be effectively achieved, the operation precision of the agricultural vehicle is greatly improved, and the operation quality is improved while the manual labor intensity is reduced.

Description

Technical Field

[0001] This invention belongs to the field of agricultural machinery control technology, and in particular relates to a high-precision path tracking control method for agricultural tracked vehicles based on model prediction algorithms. Background Technology

[0002] The complex and varied terrain of mountainous and hilly areas, with its frequent undulations, small plots, and uneven distribution, poses a significant challenge to mechanized operations. Furthermore, the agronomical constraints of soybean-corn strip intercropping further limit the efficient movement of medium and large-sized agricultural machinery. For example, the increased number of turns by vehicles raises the risk of crop damage; poor straight-line driving performance reduces operational efficiency; and real-time manual vehicle control increases labor intensity. Therefore, reducing crop compaction, lowering manual labor intensity, and improving vehicle operational precision have become crucial for enhancing the efficiency and quality of agricultural operations.

[0003] Current research primarily focuses on path tracking for large and medium-sized tracked vehicles in monoculture, with no studies yet addressing path tracking for small tracked vehicles in intercropping scenarios. Currently, path tracking is categorized into model-free control methods and model-based control methods. However, model-free control methods face limitations in agriculture: firstly, environmental diversity leads to high data acquisition costs and affects algorithm generalization; secondly, relying on trial-and-error parameter tuning makes it difficult to guarantee global optimality; and finally, practical experience shows that non-optimal controllers based on model-free methods perform worse than optimal controllers based on models. Model-based predictive control (MDC) can simultaneously consider multiple inputs / outputs and system states, especially under conditions where agricultural machinery typically operates at low speeds, operates in complex environments, and faces difficulties in accurate modeling. Through rolling optimization and feedback correction mechanisms, MDC can dynamically balance control performance and constraint conflicts within a finite time domain, thus demonstrating unique advantages in agricultural vehicle path tracking. However, MDC requires significant computing power, and using high-performance hardware in agriculture would greatly increase machinery prices, hindering the popularization and promotion of smart agriculture. Therefore, finding a control method that can be applied to low-performance controllers (STM32H745) while simultaneously achieving high-precision path tracking has become an urgent problem to be solved in the application of corn-soybean strip intercropping and the promotion of smart agriculture. Summary of the Invention

[0004] To address the aforementioned technical problems, this invention proposes a high-precision path tracking control method for agricultural tracked vehicles based on model prediction algorithms. This method can effectively solve the problem of high-precision path tracking for small tracked agricultural vehicles in hilly areas, greatly improve the operational accuracy of agricultural vehicles, reduce the intensity of manual labor, and improve the quality of operations.

[0005] To achieve the above objectives, this invention proposes a high-precision path tracking control method for agricultural tracked vehicles based on a model prediction algorithm, comprising:

[0006] Obtain the motion parameters of agricultural tracked vehicles;

[0007] Based on the motion parameters, a motion model is established in the Cartesian coordinate system;

[0008] The motion model is linearized and discretized to obtain a discretized linear error model;

[0009] Obtain the reference path sequence, and based on the discretized linear error model, objective function, and system constraints, construct and solve the quadratic programming problem to obtain the target control strategy;

[0010] The agricultural tracked vehicle is tracked and controlled according to the target control strategy.

[0011] Optionally, the motion parameters include: the travel speed of the agricultural tracked vehicle, the linear speed of the left and right tracks, the geometric center, the turning center, the turning radius, the track gauge, and the heading angle.

[0012] Optionally, the motion model is:

[0013]

[0014] Where v represents the speed of the agricultural tracked vehicle, v L v R R represents the linear velocity of the left and right tracks of the agricultural tracked vehicle, R represents the theoretical turning radius with the geometric center of the agricultural tracked vehicle as the turning center, and L represents the track gauge of the agricultural tracked vehicle. The heading angle of the agricultural tracked vehicle is represented by Xc axis, with the positive direction of the Xc axis as 0 and counterclockwise as positive. t1 and t2 represent the sampling time points. The geometric center of the harvester is located along the X-axis in the global coordinate system. C The velocity component in the axial direction, The geometric center of the harvester is located along the Y-axis in the global coordinate system. C The velocity component in the axial direction, For the heading angle of the harvester The derivative with respect to time, i.e., the angular velocity of the turn.

[0015] Optionally, the motion model is linearized and discretized to obtain a discretized linear error model, including:

[0016] The motion model is linearized to obtain a linear error model;

[0017] The linear error model is discretized using the forward Euler method to obtain the discretized linear error model.

[0018] Optionally, a reference path sequence is obtained, and based on the discretized linear error model, objective function, and system constraints, a quadratic programming problem is constructed and solved to obtain the target control strategy, including:

[0019] Based on the discretized linear error model, update the state space equation;

[0020] Based on the updated state-space equations, a high-precision path tracking control system prediction model is constructed, wherein the high-precision path tracking control system prediction model is used to output the predicted state of the system.

[0021] Based on the prediction model of the high-precision path tracking control system, the objective function is transformed into a quadratic programming problem;

[0022] A feedback correction mechanism is established, and the controller solves the quadratic programming problem in each control cycle to obtain the target control strategy in the control time domain. The first set of results in the target control strategy is then applied to the system.

[0023] Optionally, the prediction model of the high-precision path tracking control system is:

[0024] Y(k)=Ψξ(k)+ΘΔU(k)

[0025] Among them, the predicted output matrix State transition matrix Control input matrix Control Increment Sequence ξ(k) is the extended state vector at time k, η(k+N) p ) for k+N p The system's predicted state at time 10:00. Let k be the output matrix at time k. To expand the system matrix N p Power of 1 To expand the input matrix, Δu(k+N) c -1) is for k+N c The increment of the system's input at time -1.

[0026] Optionally, the target control strategy is:

[0027] ΔU(k) * =[Δu(k) * ,Δu(k+1) * ,…,Δu(k+N c -1) * ] T

[0028] Where, ΔU(k) * For the optimal control sequence, Δu(k+N)c -1) * For k+K c The control increment at time -1, Δu(k+1) * Let Δu(k) be the control increment at time k+1. * Let be the control increment at time k, and T be the transpose.

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

[0030] This invention constructs a path tracking control method for agricultural tracked vehicles based on model predictive control, achieving high-precision and robust path tracking control, effectively improving the navigation performance and operational intelligence level of agricultural tracked vehicles. Specifically, this is reflected in the following aspects:

[0031] (1) This invention establishes a kinematic model of an agricultural tracked vehicle in the Cartesian coordinate system and performs linearization and discretization on it, which significantly reduces the computational complexity of model predictive control and improves the real-time response capability of the control system.

[0032] (2) Based on the linear error model, the present invention predicts the future state of the vehicle, constructs a new state space equation, and optimizes the control objective function through quadratic programming, thereby obtaining the optimal control sequence of the system and realizing precise control of the path tracking control process.

[0033] (3) The present invention introduces a feedback correction mechanism in the controller, which enhances the robustness of the system to disturbances and modeling errors, and further improves the stability and accuracy of path tracking.

[0034] (4) The present invention discretizes the path into a series of reference point sequences and updates them in real time as the vehicle runs, thereby improving the adaptability of the control system to dynamic paths.

[0035] (5) By introducing a joint adjustment mechanism of error weight matrix and control quantity weight matrix, the present invention can flexibly adjust the control strategy under different operating conditions, effectively improving system performance and energy consumption balance.

[0036] (6) This invention establishes a simulation platform for a path tracking control system and uses the “control variable method” to systematically analyze the influence of the prediction time domain and the control time domain on the control performance, and finally determines the optimal control parameters, thereby enhancing the generalizability and practicality of the method. Attached Figure Description

[0037] The accompanying drawings, which form part of this application, are used to provide a further understanding of this application. The illustrative embodiments and descriptions of this application are used to explain this application and do not constitute an undue limitation of this application. In the drawings:

[0038] Figure 1This is a flowchart of a high-precision path tracking control method for agricultural tracked vehicles based on a model prediction algorithm according to an embodiment of the present invention;

[0039] Figure 2 This is a schematic diagram of the "4+2" planting model according to an embodiment of the present invention;

[0040] Figure 3 This is a schematic diagram of the kinematic model of an agricultural tracked vehicle constructed using a Cartesian coordinate system according to an embodiment of the present invention.

[0041] Figure 4 This is a schematic diagram illustrating the reference point update method according to an embodiment of the present invention;

[0042] Figure 5 This is a simulation model diagram of the MPC controller according to an embodiment of the present invention;

[0043] Figure 6 This is a circular path tracing result diagram according to an embodiment of the present invention, where (a) is N. p The tracking results of the circular tracking path when fixed, (b) is N c The tracking result of a fixed circular tracking path;

[0044] Figure 7 This is a diagram showing the dual-path-shifting tracking results of an embodiment of the present invention, where (a) represents N. p The result of dual-track path tracking under fixed conditions is shown in (b), where N is... c Figure showing the results of dual-track path tracking under fixed conditions;

[0045] Figure 8 This is a schematic diagram of the combined path according to an embodiment of the present invention;

[0046] Figure 9 This is a path tracking result diagram with a row spacing of 2.4m according to an embodiment of the present invention;

[0047] Figure 10 This is a heading angle deviation diagram for three speed-based combined paths with a 2.4m line spacing according to an embodiment of the present invention. Among them, (a) is the heading angle error diagram when the line spacing is 2.4m and the speed is 0.4m / s, (b) is the heading angle error diagram when the line spacing is 2.4m and the speed is 0.8m / s, and (c) is the heading angle error diagram when the line spacing is 2.4m and the speed is 1.2m / s.

[0048] Figure 11 This is a path tracking result diagram with a row spacing of 4.4m according to an embodiment of the present invention;

[0049] Figure 12These are heading angle deviation diagrams for three speed combinations with a 4.4m line spacing in an embodiment of the present invention. (a) is the heading angle error diagram when the line spacing is 4.4m and the speed is 0.4m / s, (b) is the heading angle error diagram when the line spacing is 4.4m and the speed is 0.8m / s, and (c) is the heading angle error diagram when the line spacing is 4.4m and the speed is 1.2m / s. Detailed Implementation

[0050] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. This application will now be described in detail with reference to the accompanying drawings and embodiments.

[0051] It should be noted that the steps shown in the flowchart in the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions, and although a logical order is shown in the flowchart, in some cases the steps shown or described may be executed in a different order than that shown here.

[0052] This embodiment proposes a high-precision path tracking control method for agricultural tracked vehicles based on model prediction algorithms, such as... Figure 1 As shown, the specific steps include:

[0053] Obtain the motion parameters of agricultural tracked vehicles;

[0054] Based on the motion parameters, establish a motion model in the Cartesian coordinate system;

[0055] The motion model is linearized and discretized to obtain a discretized linear error model;

[0056] Obtain the reference path sequence, and based on the discretized linear error model, objective function, and system constraints, construct and solve the quadratic programming problem to obtain the target control strategy;

[0057] Based on the target control strategy, tracked agricultural vehicles are tracked and controlled.

[0058] Specifically, the process includes the following steps: S1. Based on the motion parameters of the agricultural tracked vehicle, including its travel speed, left and right track linear velocities, geometric center, turning center, turning radius, track gauge, and heading angle, establish a kinematic model in a Cartesian coordinate system. S2. Linearize and discretize the established kinematic model to simplify the computation of model predictive control and improve real-time performance. S3. Based on the state of the agricultural tracked vehicle over a future period using the linear error model, obtain a new state-space equation. S4. To ensure that the predicted output of the high-precision path tracking control system matches the actual output, design a control objective function and transform it into a quadratic programming problem to optimize and solve for the optimal control sequence of the system, and constrain the increment of the control quantity. S5. Establish a feedback correction mechanism so that the controller solves the above quadratic programming problem in each control cycle and obtains the optimal control sequence in the control time domain. S6. Discretize the reference path into a sequence of discrete points, updating the reference points as the agricultural tracked vehicle travels. S7. Improve system performance by adjusting the values ​​of the error weight matrix and the control quantity weight matrix. S8. Build a simulation model of the path tracking control system and find the optimal prediction time domain and control time domain values ​​using the "control variable method".

[0059] More specifically, in this embodiment of the invention, a 4LZD-2 soybean combine harvester is used as the implementation object, with a maximum width of 206cm, a header width of 160cm, and a track outer spacing of 140cm. For example... Figure 2 As shown, the soybean-corn strip intercropping pattern is "4+2", that is, 4 rows of soybeans + 2 rows of corn. For this intercropping pattern, this invention provides an example implementation of a high-precision path tracking control method for agricultural tracked vehicles based on model prediction algorithms.

[0060] Furthermore, the motion parameters include: the travel speed of the agricultural tracked vehicle, the linear speed of the left and right tracks, the geometric center, the turning center, the turning radius, the track gauge, and the heading angle.

[0061] Furthermore, the kinematic model of the tracked vehicle is based on the following assumptions:

[0062] 1) The harvester's center of mass coincides with its geometric center, and the entire machine is symmetrical about the center;

[0063] 2) Ignore factors that affect driving stability, such as track slippage and vehicle tilt, when driving.

[0064] like Figure 3 As shown, the kinematic model of the tracked vehicle is established in the Cartesian coordinate system and can be described as follows:

[0065]

[0066] Where v represents the speed of the agricultural tracked vehicle, v L v R R represents the linear velocity of the left and right tracks of the agricultural tracked vehicle; L represents the theoretical turning radius with the geometric center of the agricultural tracked vehicle as the turning center; and L represents the track gauge of the agricultural tracked vehicle. The heading angle of the agricultural tracked vehicle is represented by Xc axis, with the positive direction of the Xc axis as 0 and counterclockwise as positive. t1 and t2 represent the sampling time points. The geometric center of the harvester is located along the X-axis in the global coordinate system. C The velocity component in the axial direction, The geometric center of the harvester is located along the Y-axis in the global coordinate system. C The velocity component in the axial direction, For the heading angle of the harvester The derivative with respect to time, i.e., the angular velocity of the turn.

[0067] Furthermore, the motion model is linearized and discretized to obtain a discretized linear error model, including:

[0068] Linearize the motion model to obtain a linear error model;

[0069] The forward Euler method is used to discretize the linear error model to obtain the discretized linear error model.

[0070] Specifically, the kinematic model can be considered as having u = [v L v R ] T and state variables are In a general control system, where the subscript r represents the reference quantity, the point on the reference path is represented as:

[0071]

[0072] in, χ is the derivative of the reference state quantity with respect to time. r As a reference state variable, u r For reference input;

[0073] The kinematic model established by equation (2) is a nonlinear system. In order to simplify the computation of model predictive control, improve real-time performance, and deploy it in the STM32H745 microcontroller, it needs to be transformed into a linear system. Taylor series is used to expand the nonlinear system and retain the lower-order terms.

[0074]

[0075] Subtracting equation (2) from equation (3) yields the linear error model of the tracked harvester, and the state-space equation is expressed as follows:

[0076]

[0077] In the equation, the system matrix is... System input matrix Let be the derivative of the state vector, representing the rate of change of the harvester's pose over time. Let be the derivative of the error vector, representing the rate of change of the error between the harvester's pose and the reference point over time. The reference speed for the left track of the harvester. Let L be the reference speed of the right track of the harvester, and L be the track gauge of the harvester. This is the reference heading angle for the harvester. This is the actual heading angle of the harvester. Let the column vectors be the differences between the actual state quantities and the reference state quantities of the harvester, and the column vector be the error. This is a column vector representing the difference between the actual input and the reference input of the harvester.

[0078] The linear error model is continuous. To apply this model to the design of a model predictive controller, it needs to be discretized. The forward Euler method is used for discretization, i.e.:

[0079]

[0080] In equation (5), k represents the discrete time. Let be the predicted state error vector at time k+1. Let T be the control input error vector at time k. s The sampling time.

[0081] The discretized linear error model is as follows:

[0082]

[0083] In equation (6), the discretized system matrix Discretized input matrix

[0084] Furthermore, a reference path sequence is obtained, and based on the discretized linear error model, objective function, and system constraints, a quadratic programming problem is constructed and solved to obtain the target control strategy, including:

[0085] Based on the discretized linear error model, update the state space equation;

[0086] Based on the updated state-space equations, a high-precision path tracking control system prediction model is constructed, wherein the high-precision path tracking control system prediction model is used to output the predicted state of the system.

[0087] Based on the prediction model of the high-precision path tracking control system, the objective function is transformed into a quadratic programming problem;

[0088] A feedback correction mechanism is established, and the controller solves the quadratic programming problem in each control cycle to obtain the target control strategy in the control time domain. The first set of results from the target control strategy is then applied to the system.

[0089] Specifically, based on this linear error model, the state of the harvester over a future period is predicted, and... The new state-space equations are obtained:

[0090]

[0091] In equation (7), the extended system matrix Extended input matrix Output matrix I represents the identity matrix, n=3 represents the state dimension, and m=2 represents the control variable dimension.

[0092] According to the state-space equation expression, when the prediction time domain length is N... p At that time, it can be known that in k+N p The system's predicted state at time t is:

[0093]

[0094] When the prediction time domain of the path tracking controller is N p The control time domain is N c And N p >N c At that time, the system prediction model can be obtained as follows:

[0095] Y(k)=Ψξ(k)+ΘΔU(k) ​​(9)

[0096] Among them, the predicted output matrix State transition matrix Control input matrix Control Increment Sequence ξ(k) is the extended state vector at time k, η(k+N) p ) for k+N p The system's predicted state at time 10:00. Let k be the output matrix at time k. For prediction of time domain length N p Extended system matrix at time, To expand the input matrix, Δu(k+N) c -1) is for k+N c The increment of the system's input at time -1.

[0097] More specifically, to ensure that the system's predicted output matches the actual output and that the harvester quickly and smoothly tracks the reference path, an objective function needs to be designed and transformed into a quadratic programming problem to optimize and solve for the optimal control sequence of the system. Simultaneously, to prevent abrupt changes in the system's control input that could affect its stability, constraints need to be placed on the increment of the control input. The objective function is designed as follows:

[0098]

[0099] In equation (10), n ref The reference output is represented by Q and R, the weight matrices are represented by ρ, the weight coefficients are represented by ε, and the relaxation factor is represented by ε to prevent the objective function from having no feasible solution. The first term on the right represents the system's tracking accuracy of the reference path, and the second term reflects the smoothness of the system's tracking process, ensuring that the system accurately and smoothly tracks the reference path.

[0100] Considering that the linear speed of the harvester track is within a finite range, the control quantity calculated by the controller cannot differ too much from the actual speed. Therefore, when designing the objective function, it is also necessary to consider the upper and lower limits of the control quantity, the upper and lower limits of the control quantity increment, and the constraint of the actual speed.

[0101] The upper and lower limits of the control quantity are expressed as follows:

[0102] u min (k+i)≤u(k+i)≤u max (k+i)

[0103] i = 0, 1, ..., N c -1 (11)

[0104] In equation (11), u min (k+i)=0,u max (k+i) = 2 represents the minimum and maximum values ​​of the control quantity, respectively.

[0105] The constraint on the control increment is expressed as:

[0106] Δu min (k+i)≤Δu(k+i)≤Δu max (k+i)

[0107] i = 0, 1, ..., N c -1 (12)

[0108] In equation (12), to prevent the control quantity from becoming too large and causing a sudden change in the control quantity, the minimum value of the control quantity increment is designed to be Δu. min (k+i) = -0.25, with a maximum value of Δu. max (k+i)=0.25.

[0109] The constraint of actual speed on the control variable is expressed as follows:

[0110] -0.1≤s×u(k+i)-v ture ≤0.1

[0111] i = 0, 1, ..., N c -1 (13)

[0112] In equation (13), v ture The actual speed of the harvester is collected by the navigation system.

[0113] In the objective function, the solution result is the control increment in the control time domain. Therefore, the constraints are required to appear in the form of control increment or its transformation. The above constraints are then transformed.

[0114] It is easy to see that the following relation exists:

[0115] u(k+i)=u(k+i-1)+Δu(k+i) (14)

[0116] make:

[0117]

[0118] In equations (15) and (16), 1 Nc×1 Indicates the number of rows is N c The column vector, u(k-1), represents the actual control quantity at the previous time step, I m This represents an identity matrix of dimension m, where m = 2.

[0119] The constraints on the control quantity can then be reformulated as follows:

[0120] U min ≤A Nc ×ΔU(k)+U k ≤U max (17)

[0121] In equation (17), U min U represents the set of minimum values ​​of the control quantity within the control time domain. max This represents the set of maximum values ​​of control variables within the control time domain.

[0122] The constraint of actual speed on the control variable can be re-expressed as:

[0123]

[0124] In equation (18),

[0125] Expanding the objective function of the expression into a standard quadratic form and combining it with the constraints, we transform it into the following quadratic programming problem:

[0126] minJ(ξ(k),u(k-1),ΔU(k))=

[0127] st

[0128] [ΔU(k) T ,ε] T H k [ΔU(k) T ,ε]+G k [ΔU(k) T ,ε]

[0129] U min ≤A Nc ×ΔU(k)+U k ≤U max (19)

[0130] In equation (19), the Hessian matrix Linear term vector G k =[2E(k)] T QΘ0).

[0131]

[0132] ΔU min ≤ΔU(k)≤ΔU max (twenty one)

[0133] The controller solves a quadratic programming problem in each control cycle to obtain the initial control sequence in the control time domain:

[0134] ΔU(k) * =[Δu(k) * ,Δu(k+1) * ,…,Δu(k+N c -1) * ] T (twenty two)

[0135] Where, ΔU(k) * For the optimal control sequence, Δu(k+N) c -1) * For k+N c The control increment at time -1, Δu(k+1) * Let Δu(k) be the control increment at time k+1. * Let be the control increment at time k, and T be the transpose.

[0136] The first element of this control sequence is used as the incremental input of the actual control quantity of the system at the current moment, that is:

[0137] u(k) = u(k-1) + Δu(k) * (twenty three)

[0138] When the system enters the next control cycle, the current state variables of the system are used as the initial values ​​to re-predict the output for a period of time in the future. A new optimal control sequence is obtained through optimization. The first element of the control sequence is applied to the system, and the above process is repeated until the system stops, thereby enabling the harvester to track the reference path.

[0139] Reference point update:

[0140] In practical path tracking control systems, the reference path is a sequence of discrete points, and these reference points need to be updated as the harvester moves. The methods for updating reference points are as follows: Figure 4 As shown, P0P1…P n It is a discrete point sequence of the reference path, where point Q(x,y) represents the current position of the harvester, and d i This indicates the distance from the harvester's current position to the current reference point P. i The distance, d i-1 This indicates the distance from the harvester's current position to the previous reference point P. i-1 The distance when d i-1 >d i At that time, the current reference point P i Updated to P i+1 Repeat the above process until the reference point is updated to P. n .

[0141] Determine the weight matrix:

[0142] The values ​​of the error weight matrix Q and the control quantity weight matrix R are relative. When the value of Q is much greater than R, the system is more sensitive to tracking errors; conversely, when the value of Q is much less than R, the system is more sensitive to control quantities. For the soybean harvesting scenario in soybean-corn strip intercropping, where the requirements for tracking errors are even higher, this method sets the value of Q to be much greater than the value of R.

[0143] In practical engineering applications, either Q or R is usually kept constant, and the performance of the system is improved by adjusting the value of the other weight matrix. Furthermore, to facilitate calculation and reduce computational load, Q and R are typically set as diagonal matrices, let:

[0144]

[0145] In the formula, m represents the dimension of the control quantity, and N... c To control the length of the time domain.

[0146]

[0147] In the formula, n is the dimension of the state variable, N p To predict the length of the time domain.

[0148] Determine the time-domain parameters:

[0149] Based on the above path tracking method, a simulation model of the path tracking control system was built in Matlab / Simulink, as follows: Figure 5 As shown. The optimal prediction time domain N is found using the "controlled variable method". p and control time domain N c The value of .

[0150] Considering the harvester's relatively low operating speed and small speed variation, N is set to reduce the computational scale and improve real-time performance. c =3, N p =15.

[0151] Simulation tests were conducted using double-track and circular paths to determine the optimal N. p and N c The expression for a double lane change route is as follows:

[0152]

[0153] In the formula, Y r X r , These are the reference coordinates and reference heading angle during the harvester's movement, respectively. Other parameters are: d n1 =3.6, d n2 =3.6, d m1 =25,d m2 =25, r1=0.096(X) r -60)-1.2, r2=0.096(X r -120)-1.2.

[0154] The circular path expression is as follows:

[0155]

[0156] In equation (27), Y r X r , These represent the reference coordinates and reference heading angle during the harvester's movement, respectively. X0 and Y0 are the coordinates of the center of the circular path, r is the radius, and v is the reference heading angle. ref For reference speed.

[0157] During the simulation, the harvester's initial position was (0,0), the initial heading angle was 0, the fixed reference speed was 0.8 m / s, and the simulation step size was 0.2 s. Different N values ​​were set. p N c The length is used to test the tracking performance of the controller.

[0158] The testing method is as follows: first fix N. p Unchanged, adjusted up and down N c Length, perform multiple simulations and record N when the tracking effect is good. c Subsequently, N, which showed better tracking performance, was fixed. c Unchanged, adjusted up and down N p The length of N was used to perform multiple simulations, and the N values ​​when the tracking effect was good were recorded. p The N recorded in the two separate instances. p N c Length is the parameter combination that results in good tracking performance for the controller.

[0159] Figure 6 (a)-(b) show the results at different N p N c Tracking performance under a circular path. When N p When N is fixed at 15, c =3 has the worst tracking performance, N c =4 provides the best tracking performance. When N = 4, the tracking performance is the best. c When N is fixed at 4, p =10 has the worst tracking performance, N p =25 provides the best tracking performance.

[0160] Figure 7 (a)-(b) show the results at different N p N c The tracking effect under a dual-track path is shown. The designed path tracking controller can track a dual-track path, but when the parameters are not suitable, slight oscillations occur at the end of the path, with a maximum amplitude of approximately 1.5 cm. From the magnified view, when N... c Fixed at 4, N p The tracking effect is best at 15:00, at which point a slight oscillation occurs and then stabilizes, with the error being less than 0.5cm after stabilization.

[0161] In summary, N p =25, N c =4 and N p =15, N c =4 is optimal for both circular and double-line-shifting paths. Considering the lower hardware processor performance in actual deployments of the path tracking controller, N is chosen to be optimal without affecting accuracy. p N cThe value of should be as small as possible; meanwhile, simulation results of circular paths show that when N p When the values ​​are 15, 20, and 25, the tracking error difference between them is about 0.5 cm, which is much smaller than the measurement error of the navigation system in real-world conditions. Therefore, this method ultimately determines the prediction time domain N of the path tracking controller. p The length is 15, and the control time domain is N. c The length is 4.

[0162] Path tracking simulation experiment:

[0163] After determining the weight matrix and time domain length, to verify the performance of the path tracking controller designed in this method, the tracking accuracy of the controller was verified under different vehicle speeds and with the same control parameters. Taking the corn-soybean strip intercropping "4+2" planting pattern as an example, a combined path was designed consisting of a straight path with a row spacing of 2.4m connected by a circular arc turning path to simulate the harvester's "S-shaped path" operation in the field. The combined path is as follows: Figure 8 As shown.

[0164] The initial position of the harvester was set to (0,0), the initial heading angle to 0°, the control period to 200ms, the prediction time domain to 15, and the control time domain to 4. Simulations were then performed at speeds of 0.4m / s, 0.8m / s, and 1.2m / s, respectively. The results are as follows. Figure 7 As shown in (a)-(b).

[0165] Figure 9 The combined path tracking results at different speeds show that the path tracking controller achieves smooth tracking of the combined path at three speeds. As can be seen from the magnified view, there is a lateral deviation adjustment distance of about 2m after line break. After the system corrects the deviation, smooth tracking of the straight line is achieved.

[0166] Figure 10 (a)-(c) show the heading angle deviation at different speeds. Simulation results show that the heading angle has two abrupt changes at the turn, after which the heading angle deviation stabilizes at around 0°, and the heading angle deviation increases with increasing speed during the turn.

[0167] Table 1

[0168]

[0169]

[0170] Table 2

[0171]

[0172] Table 1 above shows the path tracking position error with a row spacing of 2.4m, and Table 2 shows the path tracking heading angle error with a row spacing of 2.4m. From the results, it can be seen that when the radius of the U-turn path is constant, the tracking error and heading deviation increase with increasing speed. Therefore, to verify the performance of the path tracking controller under large turning radius conditions, simulations were conducted with the turning radius increased while maintaining constant speed. The simulation results for a turning radius of 2.2m and a row spacing of 4.4m are as follows: Figure 9 As shown.

[0173] Figure 11 The simulation results show the tracking effect of a combined path with a radius of 2.2m at different speeds. The simulation results show that the path tracking controller achieves smooth tracking of the combined path at three speeds. From the enlarged view, it can be seen that there is a lateral deviation adjustment distance of about 1.5m after line break. After the system corrects the deviation, smooth tracking of the straight line is achieved. However, the tracking effect decreases as the speed increases.

[0174] Figure 12 (a)-(c) show the heading angle deviation at different speeds. Simulation results, as shown in Tables 3 (path tracking position error with a row spacing of 4.4m) and 4 (path tracking heading angle error with a row spacing of 4.4m), indicate that although abrupt changes in heading angle still occur at turns, the changes between the two abrupt changes are smoother, and the peak value is smaller. At a speed of 1.2m / s, there are fluctuations in heading angle deviation on straight sections, but these fluctuations are almost negligible, indicating that the path tracking controller achieves smooth tracking of the combined path.

[0175] Table 3

[0176]

[0177]

[0178] Table 4

[0179]

[0180] The above are merely preferred embodiments of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A high-precision path tracking control method for agricultural tracked vehicles based on model prediction algorithms, characterized in that, include: Obtain the motion parameters of agricultural tracked vehicles; Based on the motion parameters, a motion model is established in the Cartesian coordinate system; The motion model is linearized and discretized to obtain a discretized linear error model; Obtain the reference path sequence, and based on the discretized linear error model, objective function, and system constraints, construct and solve the quadratic programming problem to obtain the target control strategy; The agricultural tracked vehicle is tracked and controlled according to the target control strategy.

2. The high-precision path tracking control method for agricultural tracked vehicles based on model prediction algorithm according to claim 1, characterized in that, The motion parameters include: the travel speed of the agricultural tracked vehicle, the linear velocity of the left and right tracks, the geometric center, the turning center, the turning radius, the track gauge, and the heading angle.

3. The high-precision path tracking control method for agricultural tracked vehicles based on model prediction algorithm according to claim 1, characterized in that, The motion model is as follows: Where v represents the speed of the agricultural tracked vehicle, v L v R R represents the linear velocity of the left and right tracks of the agricultural tracked vehicle, R represents the theoretical turning radius with the geometric center of the agricultural tracked vehicle as the turning center, and L represents the track gauge of the agricultural tracked vehicle. The heading angle of the agricultural tracked vehicle is represented by Xc axis, with the positive direction of the Xc axis as 0 and counterclockwise as positive. t1 and t2 represent the sampling time points. The geometric center of the harvester is located along the X-axis in the global coordinate system. C The velocity component in the axial direction, The geometric center of the harvester is located along the Y-axis in the global coordinate system. C The velocity component in the axial direction, For the heading angle of the harvester The derivative with respect to time, i.e., the angular velocity of the turn.

4. The high-precision path tracking control method for agricultural tracked vehicles based on model prediction algorithm according to claim 1, characterized in that, The motion model is linearized and discretized to obtain a discretized linear error model, including: The motion model is linearized to obtain a linear error model; The linear error model is discretized using the forward Euler method to obtain the discretized linear error model.

5. The high-precision path tracking control method for agricultural tracked vehicles based on model prediction algorithm according to claim 1, characterized in that, Obtain the reference path sequence, and based on the discretized linear error model, objective function, and system constraints, construct and solve the quadratic programming problem to obtain the target control strategy, including: Based on the discretized linear error model, update the state space equation; Based on the updated state-space equations, a high-precision path tracking control system prediction model is constructed, wherein the high-precision path tracking control system prediction model is used to output the predicted state of the system. Based on the prediction model of the high-precision path tracking control system, the objective function is transformed into a quadratic programming problem; A feedback correction mechanism is established, and the controller solves the quadratic programming problem in each control cycle to obtain the target control strategy in the control time domain. The first set of results in the target control strategy is then applied to the system.

6. The high-precision path tracking control method for agricultural tracked vehicles based on model prediction algorithm according to claim 5, characterized in that, The prediction model for the high-precision path tracking control system is as follows: Y(k)=Ψξ(k)+ΘΔU(k) Among them, the predicted output matrix State transition matrix Control input matrix Control Increment Sequence ξ(k) is the extended state vector at time k, η(k+N) p ) for k+N p The system's predicted state at time 10:

00. Let k be the output matrix at time k. To expand the system matrix N p Power of 1 To expand the input matrix, Δu(k+N) c -1) is for k+N c The increment of the system's input at time -1.

7. A high-precision path tracking control method for agricultural tracked vehicles based on a model prediction algorithm according to claim 6, characterized in that, The target control strategy is as follows: ΔU(k) * =[Δu(k) * ,Δu(k+1) * ,…,Δu(k+N c -1) * ] T Where, ΔU(k) * For the optimal control sequence, Δu(k+N) c -1) * For k+N c The control increment at time -1, Δu(k+1) * Let Δu(k) be the control increment at time k+1. * Let be the control increment at time k, and T be the transpose.

Images