Intelligent agricultural machinery autonomous operation control method and system based on dynamic adaptive LQR
The state and control weight of intelligent agricultural machinery are adjusted through dynamic adaptive LQR algorithm, which solves the error problem of traditional algorithms in multi-input variable scenarios, improves the operation accuracy and efficiency, and reduces the parameter adjustment time and the risk of system overshooting.
Patent Information
- Application Number
- CN202211029530.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-08-25
- Publication Date
- 2024-11-19
- Estimated Expiration
- 2042-08-25
AI Technical Summary
Traditional intelligent agricultural machinery control algorithms, such as PID, MPC and LQR, are difficult to effectively reduce lateral and orientation errors, and perform poorly in multi-input variable scenarios, resulting in low operational accuracy and poor efficiency.
Using the intelligent agricultural machinery autonomous operation control method based on dynamic adaptive LQR algorithm, by establishing the motion model of the intelligent agricultural machinery, dynamically adjusting the state weight matrix and control weight matrix, adjusting the weight in real time based on the current position posture information, and calculating the heading deviation control amount to perform Run control.
It improves the operating accuracy and efficiency of smart agricultural machinery, reduces tracking errors, reduces parameter adjustment time, enhances system stability and real-timeness, and avoids the frequency of system overshooting.
Smart Images

Figure CN115407656B_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the technical field of intelligent agricultural machinery, and in particular relates to an autonomous operation control method and system of intelligent agricultural machinery based on an LQR algorithm. Background Art
[0002] China is a major agricultural country, and the agricultural industry is one of the main economic industries in our country. However, affected by the global epidemic, food security and agricultural development have received increasing attention from academia and the public. To ensure food security and promote the stable development of agriculture, it is necessary to combine the entire agricultural production process, start from the constraints, improve efficiency, and save labor. Therefore, the use of unmanned agricultural machinery (abbreviated as: intelligent agricultural machinery) for agricultural operation management has become a hot research topic. During the operation of intelligent agricultural machinery, decision-making and control algorithms are of paramount importance.
[0003] Intelligent agricultural machinery completes specific tasks by sensing the environment, positioning, planning paths, and real-time tracking. Mature control decision-making methods can greatly improve the operation quality of intelligent agricultural machinery and improve operation efficiency. Traditional control algorithms include PID, MPC, LQR and other control algorithms. The traditional PID algorithm combines the three links of proportion, integration, and differentiation for control. However, the algorithm is regulated as a single input variable, and is insufficient in the regulation of multiple input variables such as lateral error and heading error. When faced with multiple input variables, control algorithms such as LQR and MPC perform better.
[0004] The linear quadratic regulator (LQR) is based on the state space expression in modern control theory. The objective function is designed through the optimal control theory to solve the state feedback controller, and the feedback control is uniquely determined by the Q and R matrices of the LQR algorithm. Moreover, the algorithm is easy to form the optimal control, which facilitates the agricultural machinery to achieve the task stably, accurately and quickly.
[0005] Model Predictive Control (MPC), like the LQR algorithm, is also a multivariable control strategy. The MPC algorithm consists of three steps, including predicting future dynamics, solving optimization problems, repeatedly solving problems through rolling correction, feedback correction, and calculating the best action value at the current moment. Through the current state value, predictive calculations are performed to find control instructions for multiple future moments, and only the control instructions at the most recent moment are taken. When the control instructions are calculated again, predictions are performed again to calculate the control instructions at future moments and perform rolling optimization. A large amount of calculations are required to accurately control situations where the input variables have a large rate of change.
[0006] The basic technical requirements for the control decision of intelligent agricultural machinery in agricultural scenarios are: (1) reduce the lateral and directional errors between the target operation path and (2) ensure the operation quality and efficiency. In addition, with the improvement of the positioning accuracy of unmanned driving in farmland scenarios, the control decision technology of intelligent agricultural machinery also needs to meet the corresponding performance indicators. For example, while meeting the operation speed, the operation accuracy must be around 3 cm, which poses a challenge to the weight parameters and curve tracking effect of balancing the lateral error and directional error. The traditional PID algorithm can only handle the problem of a single input variable and cannot be well applied to the scene of agricultural machinery tracking that requires the regulation of multiple input variables. In addition, when applying the PID algorithm, it is necessary to track the preview point beyond the distant point, and the angle change is small, which makes it difficult to perform single variable regulation. The target point is easily lost at the turning moment. The MPC algorithm can accurately complete the tracking and predictive control of the agricultural machinery state through prediction under the condition of obtaining accurate modeling of the agricultural machinery. However, when the agricultural machinery is tracking and controlling the operation, the change rate of the lateral error is small, and the demand for predicting the state at future moments is low. In addition, since the operation environment cannot be accurately obtained, the prediction result cannot be combined with the environment, requiring a lot of calculation, and the real-time performance is poor when running on low-cost equipment. Although the LQR algorithm also requires an accurate model, it only calculates the optimal action value at the current state moment. It can ensure low computational complexity when predicting the optimal control value at multiple moments. It is easy to implement and low-cost. It is suitable for situations where the lateral error changes little when intelligent agricultural machinery tracks a straight line. Summary of the invention
[0007] In view of the problems of low operation precision and poor effect of intelligent agricultural machinery in the prior art, the present invention proposes an autonomous operation control method of intelligent agricultural machinery based on a dynamic adaptive LQR algorithm, including: establishing a motion model of the intelligent agricultural machinery, generating an objective function of the motion model based on the LQR algorithm, and establishing a dynamic adjustment model of a state weight matrix and a control weight matrix about the objective function; during the working process of the intelligent agricultural machinery, obtaining the position and posture information of the intelligent agricultural machinery at the current moment, and adjusting the state weight matrix and the control weight matrix according to the position and posture information with the dynamic adjustment model; solving the heading deviation control amount at the current moment with the adjusted objective function, and controlling the operation of the intelligent agricultural machinery with the heading deviation control amount.
[0008] The method for controlling the autonomous operation of intelligent agricultural machinery of the present invention, wherein the state weight matrix Q=diag[q1,q2,q3], the control weight matrix R=[γ1]; the dynamic adjustment model includes:
[0009]
[0010]
[0011]
[0012] γ1=k v ·v
[0013] q1 is the deviation weight value relative to the X direction in the world coordinate system, q2 is the deviation weight value relative to the Y direction in the world coordinate system, q3 is the weight value relative to the heading deviation in the world coordinate system, and γ1 represents the output limit of the intelligent agricultural machinery control action; Q ψ is the dynamic compensation value, k ey is the horizontal weight adjustment coefficient, k ψ is the weight adjustment coefficient, e y is the lateral error between the intelligent agricultural machine and the nearest preview point at the current moment, e ψ is the heading angle error between the intelligent agricultural machine and the nearest preview point at the current moment, e yth is the upper limit of the lateral error between the intelligent agricultural machine and the nearest preview point at the current moment, e ytl is the lower limit of the lateral error of the compensation weight, e ψth is the upper limit of the orientation error, q 1_max is the preset maximum value of q1, q 3_max is the preset maximum value of q3, k v is the output weight adjustment coefficient, and v is the expected speed of the intelligent agricultural machinery.
[0014] The autonomous operation control method of the intelligent agricultural machinery of the present invention establishes the Ackerman motion model of the intelligent agricultural machinery through the relationship between the state change, speed and angle of the intelligent agricultural machinery in the world coordinate system:
[0015]
[0016] The state quantity of the intelligent agricultural machine at sampling time T is obtained by sorting out the Ackerman motion model. Control quantity u=[v,ω], the kinematic equation of the intelligent agricultural machinery is:
[0017]
[0018] in, is the heading angle of the intelligent agricultural machine, ω is the yaw angular velocity of the intelligent agricultural machine, is the instantaneous change of the intelligent agricultural machinery in the X direction of the world coordinate system, is the instantaneous change of the intelligent agricultural machinery in the Y direction of the world coordinate system, r is the turning radius of the intelligent agricultural machinery at the current speed and yaw angular velocity, δ is the front wheel steering angle of the intelligent agricultural machinery, and l is the axis length of the intelligent agricultural machinery.
[0019] The intelligent agricultural machinery autonomous operation control method of the present invention, wherein the objective function
[0020] The present invention also proposes an intelligent agricultural machinery autonomous operation control system based on dynamic adaptive LQR, including: a model construction module, used to establish a motion model of the intelligent agricultural machinery, and generate an objective function of the motion model based on the LQR algorithm, and establish a dynamic adjustment model of a state weight matrix and a control weight matrix about the objective function; a weight adjustment module, used to obtain the position and posture information of the intelligent agricultural machinery at the current moment during the working process of the intelligent agricultural machinery, and adjust the state weight matrix and the control weight matrix according to the position and posture information with the dynamic adjustment model; an operation control module, used to solve the heading deviation control amount at the current moment with the adjusted objective function, and perform operation control of the intelligent agricultural machinery with the heading deviation control amount.
[0021] The intelligent agricultural machinery autonomous operation control system of the present invention, wherein the state weight matrix Q=diag[q1,q2,q3], the control weight matrix R=[γ1]; the dynamic adjustment model includes:
[0022]
[0023]
[0024]
[0025] γ1=k v ·v r
[0026] q1 is the deviation weight value relative to the X direction in the world coordinate system, q2 is the deviation weight value relative to the Y direction in the world coordinate system, q3 is the weight value relative to the heading deviation in the world coordinate system, and γ1 represents the output limit of the intelligent agricultural machinery control action; Q ψ is the dynamic compensation value, k ey is the horizontal weight adjustment coefficient, k ψ is the weight adjustment coefficient, e y is the lateral error between the intelligent agricultural machine and the nearest preview point at the current moment, e ψ is the heading angle error between the intelligent agricultural machine and the nearest preview point at the current moment, e yth is the upper limit of the lateral error between the intelligent agricultural machine and the nearest preview point at the current moment, e ytl is the lower limit of the lateral error of the compensation weight, e ψth is the upper limit of the orientation error, q 1_max is the preset maximum value of q1, q 3_max is the preset maximum value of q3, k v is the output weight adjustment coefficient, and v is the expected speed of the intelligent agricultural machinery.
[0027] In the intelligent agricultural machinery autonomous operation control system of the present invention, in the model building module, the Ackerman motion model of the intelligent agricultural machinery is established through the relationship between the state change, speed and angle of the intelligent agricultural machinery in the world coordinate system:
[0028]
[0029] The state quantity of the intelligent agricultural machine at sampling time T is obtained by sorting out the Ackerman motion model. Control quantity u=[v,ω], the kinematic equation of the intelligent agricultural machinery is:
[0030]
[0031] in, is the heading angle of the intelligent agricultural machine, ω is the yaw angular velocity of the intelligent agricultural machine, is the instantaneous change of the intelligent agricultural machinery in the X direction of the world coordinate system, is the instantaneous change of the intelligent agricultural machinery in the Y direction of the world coordinate system, r is the turning radius of the intelligent agricultural machinery at the current speed and yaw angular velocity, δ is the front wheel steering angle of the intelligent agricultural machinery, and l is the axis length of the intelligent agricultural machinery.
[0032] The intelligent agricultural machinery autonomous operation control system of the present invention, wherein the objective function
[0033] The present invention also proposes a computer-readable storage medium storing computer-executable instructions, characterized in that when the computer-executable instructions are executed, the intelligent agricultural machinery autonomous operation control method based on the LQR algorithm as described above is implemented.
[0034] The present invention also proposes a data processing device, comprising the computer-readable storage medium as described above. When the processor of the data processing device calls and executes the computer-executable instructions in the computer-readable storage medium, it performs autonomous operation control of intelligent agricultural machinery based on the dynamic adaptive LQR algorithm. BRIEF DESCRIPTION OF THE DRAWINGS
[0035] Figure 1 It is a flow chart of the autonomous operation control method of intelligent agricultural machinery based on LQR algorithm of the present invention.
[0036] Figure 2 It is a schematic diagram of the intelligent agricultural machinery autonomous operation control system based on the LQR algorithm of the present invention.
[0037] Figure 3 It is a schematic diagram of a data processing device of the present invention. DETAILED DESCRIPTION
[0038] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings. It should be understood that the specific implementation method described herein is only used to explain the present invention and is not used to limit the present invention.
[0039] When the inventor used the LQR algorithm to study agricultural machinery decision-making and control, he found that when traditional PID control agricultural machinery makes agricultural machinery control decisions, due to the use of a single input variable, a pre-preview point is required to form an effective tracking yaw angle, thereby adjusting the output command; the preview point tracking is often lost when turning, and since the adjustment effects of each link affect each other, a lot of time is required for adjustment, and the tracking accuracy is not ideal, the parameters of the integral link are difficult to control, and steady-state errors are prone to occur; in addition, when the LQR algorithm makes intelligent agricultural machinery control decisions, due to the parameter tuning of the Q and R matrices, the adjustment time is long, and the effect is often difficult to achieve the expected result.
[0040] After studying the LQR algorithm, the inventor found that the LQR algorithm can be used as a control strategy for agricultural machinery operations to solve the above defects, with better tracking effect and less likely to lose the preview point when turning. The feedback matrix solved by the objective function established by the optimal control theory can better control the agricultural machinery to track the target point steadily and quickly; according to the lateral error e y and heading error e ψ Based on the changes in the speed, a mathematical model is established to dynamically adjust the state weight values q1, q2, q3 in the Q matrix in the LQR algorithm. Then, the speed and the angle output weight γ1 in the R matrix in the LQR algorithm are modeled. According to the speed, the output limit on the angle is increased to optimize the tracking effect.
[0041] Based on the above problems, the present invention analyzes the kinematic state space expression of the intelligent agricultural machinery model, establishes the optimal objective function, migrates the LQR algorithm and applies it to the operation path tracking decision of intelligent agricultural machinery, and establishes a mathematical model of the Q and R matrices in the LQR algorithm according to the test data, and performs dynamic parameter correction to enable it to meet actual application requirements.
[0042] When adjusting the state weight and output weight of the LQR algorithm, a dynamic parameter correction mathematical model was established by obtaining data through a large number of tests on intelligent agricultural machinery. The dynamic parameter correction mathematical model was based on the lateral distance e between the intelligent agricultural machinery and the target point at the current moment. y 、Course difference angle e ψ The size of the LQR algorithm is dynamically adjusted, and the R matrix of the LQR algorithm is adjusted according to the longitudinal speed at the current moment. This reduces the tracking error to a certain extent, reduces the time required for adjusting parameters, and improves the operation accuracy.
[0043] The present invention provides a theoretical explanation and algorithm derivation for the application of LQR algorithm as intelligent agricultural machinery decision control. Through kinematic model analysis, objective function establishment, LQR algorithm derivation and solution, the LQR algorithm is transferred to agricultural machinery operation decision-making to improve the operation accuracy of intelligent agricultural machinery, and the application model algorithm enters the field of intelligent agricultural machinery operation, providing an available algorithm decision for complex agricultural machinery operation environments. In addition, a data model is established according to data, and the weight parameters of the state matrix and the output matrix in the LQR algorithm are dynamically adjusted to reduce the time consumed by adjusting many matrix parameters, and the operation accuracy is improved by real-time adjustment, which reduces the time required for parameter adjustment to a certain extent, reduces the test pressure, improves the stability of agricultural machinery entry, improves the lateral accuracy during tracking operation, improves the stability of the system, and reduces the frequency of system overshoot.
[0044] The present invention utilizes the lateral difference e between the agricultural machine and the target point y and the heading angle difference e ψ Two dynamic variables update the parameter values q1, q2, q3 of the LQR state Q matrix in real time, adjust the parameter γ1 of the output matrix R according to the speed of different states, better balance the angle command output by the algorithm, and reduce the overshoot problem of the system.
[0045] Figure 1 1 is a flow chart of the autonomous operation control method of intelligent agricultural machinery based on the LQR algorithm of the present invention. As shown in 1, the autonomous operation control method of intelligent agricultural machinery based on the LQR algorithm of the present invention specifically includes:
[0046] Step S1, dividing the operation blocks of the intelligent agricultural machinery and planning the operation path of the intelligent agricultural machinery;
[0047] Step S2, determining the current coordinates and heading angle information of the agricultural machine according to the GPS data;
[0048] Step S3, searching for the target point information closest to the current agricultural machine in the planned path, and calculating the lateral difference and the heading angle difference.
[0049] Step S4, determining whether the current lateral difference is greater than 30 cm; if the current lateral difference is less than or equal to 30 cm, executing step S5; if the current lateral difference is greater than 30 cm, executing step S7;
[0050] Step S5, dynamically updating the weight of the LQR algorithm according to the established mathematical model;
[0051] Step S6, solving the LQR high-speed operation instruction, reloading the agricultural machine speed, and performing high-speed operation;
[0052] Step S7, loading fixed LQR algorithm weights;
[0053] Step S8, solving the LQR low-speed operation instruction, reloading the agricultural machine speed, and executing low-speed line entry;
[0054] Step S9, the bottom layer of the agricultural machinery executes the action instruction;
[0055] Step S10, determining whether the final target point has been reached, if the final target point has been reached, then the current intelligent agricultural machinery autonomous operation control is terminated, if the final target point has not been reached, then executing step S2 again.
[0056] The overall algorithm analysis of the present invention is as follows Figure 2 Specifically, it includes:
[0057] According to the kinematic model, the state space expression of intelligent agricultural machinery is established, the cost function is constructed through optimal control, the feedback gain is calculated using the algebraic Riccati equation, and the optimal control decision is calculated based on the current state error and the weights of the Q and R matrices.
[0058] 1.1) Intelligent agricultural machinery is an Ackerman model, and the relationship equation between the state change, speed and angle of the intelligent agricultural machinery in the world coordinate system is established:
[0059]
[0060] is the instantaneous change of the intelligent agricultural machinery in the X direction of the world coordinate system, is the instantaneous change of the intelligent agricultural machinery in the Y direction of the world coordinate system, v is the expected speed of the intelligent agricultural machinery, is the heading angle of the intelligent agricultural machinery, is the change of the instantaneous heading angle of the agricultural machinery.
[0061]
[0062] ω is the yaw rate of the agricultural machinery, r is the turning radius of the intelligent agricultural machinery at the current expected speed and yaw rate, δ is the front wheel steering angle, and l is the axis length of the intelligent agricultural machinery.
[0063] Arrange the state quantity as The control quantity is u = [v, ω], then the kinematic model of the vehicle can be converted into the following form:
[0064]
[0065] After obtaining the kinematic equation of the agricultural machinery, the control system is expressed as:
[0066]
[0067] Expand (4) by Taylor and discretize it, ignore the high-order terms, and simplify to get the A and B matrices:
[0068]
[0069]
[0070]
[0071] V is the set speed value, ψ r The reference heading angle, t is the sampling time, l is the axis length of the agricultural machine, and δ is the reference front wheel turning angle.
[0072] 1.2) Full State Controller
[0073] Assume that the linear control system of agricultural machinery is:
[0074]
[0075] is the state at the next moment, and A and B are the state space matrices A and B of the motion model of the agricultural machinery.
[0076] In order to achieve the desired state, a state feedback controller needs to be set up:
[0077] u=-Kχ (9)
[0078] Substituting (5) into (4) we get:
[0079]
[0080] 1.3) Objective Function
[0081] The objective function can ensure that the intelligent agricultural machinery can quickly and smoothly track the desired trajectory. Therefore, it is necessary to optimize the deviation of the system state quantity and the control quantity. Through the optimal control principle, the objective function of the following form is constructed:
[0082]
[0083] χ is the state variable, u is the control variable, Q is the state weight matrix, and R is the control weight matrix.
[0084] Substituting u=-Kx into the cost formula (7), we obtain:
[0085]
[0086] Assume there exists a constant matrix P such that:
[0087]
[0088] Expand the left side of (9) by differential equation:
[0089]
[0090] Substituting (6) into (10), we can get:
[0091]
[0092] From formula (11), we can see that the solution becomes a quadratic optimization problem.
[0093] Replace A in formula (11) c =A-BK to replace:
[0094] A T P+PA+Q+K T RK-K T B T P-PBK=0 (16)
[0095] In (12), K is assumed to be K = R -1 B T P, then the formula is rearranged as:
[0096] A T P+PA+Q-PBR -1 B T P=0 (17)
[0097] In formula (13), A, B, Q, and R are known quantities, and the Riccati equation is used to solve it:
[0098] And bring the solved P into K=R -1 B T P calculates the feedback matrix K; and obtains the optimal control amount u=-Kχ at that moment through the feedback matrix.
[0099] According to the test data, a mathematical model is established to dynamically adjust the parameters of the Q and R matrices. As a controller of agricultural machinery, the Q and R weight matrices of LQR will have an important impact on the performance of the entire controller. In order to further improve the tracking accuracy of the target path and reduce the adjustment time, the invention adjusts the parameters and state errors in the Q and R matrices in real time and designs a dynamic Q and R matrix.
[0100] Based on the agricultural machinery state model in this paper, the weighted matrix can be described as:
[0101] Q=diag[q1,q2,q3], R=[γ1] (18)
[0102] Among them, each element in the matrix Q represents the corresponding state importance, that is, q1 represents the deviation weight value of the control system in the X direction in the world coordinate system, q2 represents the deviation weight value of the control system in the Y direction in the world coordinate system, q3 represents the weight value of the control system for the heading deviation in the world coordinate system, and γ1 represents the output limit of the control action.
[0103] When approaching the expected trajectory, calculate the lateral error e between the agricultural machine and the nearest target point y The heading angle error e ψ , establish a mathematical model (19) to adjust the weight values of q1, q2, and q3 in real time, and optimize the output angle control command. Since the invention only uses LQR to make angle control decisions, in order to ensure the uniformity of agricultural operations, the speed is a uniform speed value, and the variable speed of the algorithm is not applied. When the agricultural machinery approaches the expected trajectory, the heading angle weight q3 will cooperate with q1 and q2 to complete the intelligent agricultural machinery tracking decision. However, if the weight parameter is too large, it will cause the system to overshoot, and if it is too small, it will prolong the system convergence time. In response to the above problems, dynamic parameter correction is designed. When the error parameter is too large, the corresponding weight parameter will increase. When the error parameter is too small, the corresponding error is small, and the parameter will decrease accordingly to avoid overshoot.
[0104]
[0105] Where: e y is the lateral error between the agricultural machine and the nearest preview point at the current moment, e ψ is the heading angle error between the agricultural machine and the nearest preview point at the current moment, e yth is the upper limit of the lateral error, is the lower limit of the lateral error of the compensation weight, and e ψth is the upper limit of the orientation error, k ey is the horizontal weight adjustment coefficient, k ψ is the adjustment factor towards the weight.
[0106] When the lateral error is greater than e ytl When the current lateral difference is calculated, a dynamic compensation value Q is calculated. ψ , used to balance the difference weight q3 of the current heading angle, to prevent the heading weight from being adjusted too much and delaying system convergence. And set the upper limit value q of the state weight 1_max ,q 3_max , to prevent excessive weight values caused by excessive current state differences, causing system overshoot and reduced stability.
[0107] γ1=k v *v (20)
[0108] Where: k v is the output weight adjustment coefficient.
[0109] The R matrix needs to be dynamically adjusted according to the current speed value. When the intelligent agricultural machinery operates at different operating speeds, even if the current speed has been adjusted as a variable in the state matrix, the system overshoot problem still occurs due to the excessive speed. It is necessary to adjust γ1 according to the speed value and limit the output angle according to the speed, thereby reducing the frequency of system overshoot problems.
[0110] Figure 3 Schematic diagram of the data processing device of the present invention. Figure 3 As shown, an embodiment of the present invention further provides a computer-readable storage medium, and a data processing device. The computer-readable storage medium of the present invention stores computer-executable instructions, and when the computer-executable instructions are executed by the processor of the data processing device, the above-mentioned agricultural machinery autonomous operation control method is implemented. A person of ordinary skill in the art can understand that all or part of the steps in the above-mentioned method can be completed by instructing related hardware (such as a processor, FPGA, ASIC, etc.) through a program, and the program can be stored in a readable storage medium, such as a read-only memory, a disk or an optical disk, etc. All or part of the steps in the above-mentioned embodiment can also be implemented using one or more integrated circuits. Accordingly, each module in the above-mentioned embodiment can be implemented in the form of hardware, such as implementing its corresponding functions through an integrated circuit, or can be implemented in the form of a software function module, such as implementing its corresponding functions through a processor executing a program / instruction stored in a memory. The embodiment of the present invention is not limited to any specific form of combination of hardware and software.
[0111] This invention introduces an improved dynamically adaptive LQR algorithm into the control algorithm of intelligent agricultural machinery, which improves the operation accuracy while ensuring the operation efficiency. By establishing a dynamic correction relationship between the lateral error and the heading angle error and the state matrix parameters, and a dynamic correction relationship between the different operation speeds of the agricultural machinery and the output matrix parameters, the state matrix and the output matrix of the intelligent agricultural machinery are corrected in real time according to the error value and the speed value. This not only improves the operation accuracy of the agricultural machinery, but also reduces the time required for multi-parameter adjustment of the Q and R matrices of the LQR algorithm, and reduces the problems of system overshoot and slow system convergence.
[0112] The above implementation modes are only used to illustrate the present invention, but not to limit the present invention. Ordinary technicians in the relevant technical field can make various changes and modifications without departing from the spirit and scope of the present invention. Therefore, all equivalent technical solutions also belong to the scope of the present invention. The patent protection scope of the present invention should be defined by the claims.
Claims
1. A method for autonomous operation control of intelligent agricultural machinery based on dynamic adaptive LQR, characterized in that: include: A motion model of intelligent agricultural machinery is established, and an objective function of the motion model is generated based on the LQR algorithm, and a dynamic adjustment model of a state weight matrix and a control weight matrix of the objective function is established; the state weight matrix Q = diag [q1, q2, q3], and the control weight matrix R = [γ1]; the dynamic adjustment model includes: γ1=k v ·v q1 is the deviation weight value relative to the X direction in the world coordinate system, q2 is the deviation weight value relative to the Y direction in the world coordinate system, q3 is the weight value relative to the heading deviation in the world coordinate system, and γ1 represents the output limit of the intelligent agricultural machinery control action; Q ψ is the dynamic compensation value, k ey is the horizontal weight adjustment coefficient, k ψ is the weight adjustment coefficient, e y is the lateral error between the intelligent agricultural machine and the nearest preview point at the current moment, e ψ is the heading angle error between the intelligent agricultural machine and the nearest preview point at the current moment, e yth is the upper limit of the lateral error between the intelligent agricultural machine and the nearest preview point at the current moment, e ytl is the lower limit of the lateral error of the compensation weight, e ψth is the upper limit of the orientation error, q 1_max is the preset maximum value of q1, q 3_max is the preset maximum value of q3, k v is the output weight adjustment coefficient, v is the expected speed of the intelligent agricultural machinery; During the operation of the intelligent agricultural machine, the position and posture information of the intelligent agricultural machine at the current moment is obtained, and the state weight matrix and the control weight matrix are adjusted by the dynamic adjustment model according to the position and posture information; The heading deviation control amount at the current moment is calculated using the adjusted objective function, and the operation of the intelligent agricultural machinery is controlled using the heading deviation control amount.
2. The method for controlling the autonomous operation of an intelligent agricultural machine according to claim 1, characterized in that: Based on the relationship between the state change, speed and angle of the intelligent agricultural machinery in the world coordinate system, the Ackerman motion model of the intelligent agricultural machinery is established: The state quantity of the intelligent agricultural machinery at a certain sampling time is obtained through the Ackerman motion model. Control quantity u=[v,ω], the kinematic equation of the intelligent agricultural machinery is: in, is the heading angle of the intelligent agricultural machine, ω is the yaw angular velocity of the intelligent agricultural machine, is the instantaneous change of the intelligent agricultural machinery in the X direction of the world coordinate system, is the instantaneous change of the intelligent agricultural machinery in the Y direction of the world coordinate system, r is the turning radius of the intelligent agricultural machinery at the current expected speed and yaw angular velocity, δ is the front wheel steering angle of the intelligent agricultural machinery, and l is the axis length of the intelligent agricultural machinery.
3. The method for controlling the autonomous operation of an intelligent agricultural machine according to claim 2, characterized in that: The objective function:
4. An intelligent agricultural machinery autonomous operation control system based on dynamic adaptive LQR, characterized in that: include: The model building module is used to establish a motion model of the intelligent agricultural machinery, generate the objective function of the motion model based on the LQR algorithm, and establish a dynamic adjustment model of the state weight matrix and the control weight matrix of the objective function; the state weight matrix Q = diag [q1, q2, q3], the control weight matrix R = [γ1]; the dynamic adjustment model includes: γ1=k v ·v r q1 is the deviation weight value relative to the X direction in the world coordinate system, q2 is the deviation weight value relative to the Y direction in the world coordinate system, q3 is the weight value relative to the heading deviation in the world coordinate system, and γ1 represents the output limit of the intelligent agricultural machinery control action; Q ψ is the dynamic compensation value, k ey is the horizontal weight adjustment coefficient, k ψ is the weight adjustment coefficient, e y is the lateral error between the intelligent agricultural machine and the nearest preview point at the current moment, e ψ is the heading angle error between the intelligent agricultural machine and the nearest preview point at the current moment, e yth is the upper limit of the lateral error between the intelligent agricultural machine and the nearest preview point at the current moment, e ytl is the lower limit of the lateral error of the compensation weight, e ψth is the upper limit of the orientation error, q 1_max is the preset maximum value of q1, q 3_max is the preset maximum value of q3, k v is the output weight adjustment coefficient, v is the expected speed of the intelligent agricultural machinery; The weight adjustment module is used to obtain the position and posture information of the intelligent agricultural machine at the current moment during the working process of the intelligent agricultural machine, and adjust the state weight matrix and the control weight matrix according to the position and posture information using the dynamic adjustment model; The operation control module is used to calculate the heading deviation control amount at the current moment with the adjusted objective function, and to control the operation of the intelligent agricultural machinery with the heading deviation control amount.
5. The intelligent agricultural machinery autonomous operation control system according to claim 4, characterized in that: In the model building module, the Ackerman motion model of the intelligent agricultural machinery is established through the relationship between the state change, speed and angle of the intelligent agricultural machinery in the world coordinate system: The state quantity of the intelligent agricultural machinery at a certain sampling time is obtained through the Ackerman motion model. Control quantity u=[v,ω], the kinematic equation of the intelligent agricultural machinery is: in, is the heading angle of the intelligent agricultural machine, ω is the yaw angular velocity of the intelligent agricultural machine, is the instantaneous change of the intelligent agricultural machinery in the X direction of the world coordinate system, is the instantaneous change of the intelligent agricultural machinery in the Y direction of the world coordinate system, r is the turning radius of the intelligent agricultural machinery at the current speed and yaw angular velocity, δ is the front wheel steering angle of the intelligent agricultural machinery, and l is the axis length of the intelligent agricultural machinery.
6. The intelligent agricultural machinery autonomous operation control system according to claim 5, characterized in that: The objective function:
7. A computer-readable storage medium storing computer-executable instructions, characterized in that: When the computer executable instructions are executed, the intelligent agricultural machinery autonomous operation control method based on dynamic adaptive LQR as described in any one of claims 1 to 3 is implemented.
8. A data processing device, comprising the computer-readable storage medium as described in claim 7, when the processor of the data processing device calls and executes the computer-executable instructions in the computer-readable storage medium, it performs autonomous operation control of intelligent agricultural machinery based on dynamic adaptive LQR.
