Agricultural robot robust control method and system based on high-order all-drive theory

Abstract

The invention discloses an agricultural robot robust control method and system based on a high-order all-drive theory, and the method comprises the steps: determining a kinematic model of the course deviation and transverse position deviation of an agricultural robot, and constructing a kinetic model of the agricultural robot; converting the dynamical model of the agricultural robot into a high-order all-drive system model of the agricultural robot, and constructing a linear control system model considering external disturbance; constructing an improved nonlinear disturbance observer for disturbance estimation to obtain an external disturbance estimation value of the agricultural robot; and according to the improved nonlinear disturbance observer, constructing a robust feedback controller to perform operation control on the agricultural robot. According to the invention, an under-actuated control system can be converted into an all-drive control system, and meanwhile, the influence of external unknown disturbance is considered, so that the robustness of the controller is enhanced. The agricultural robot robust control method and system based on the high-order all-drive theory can be widely applied to the technical field of agricultural robot control.

Description

Technical Field

[0001] The present invention relates to the technical field of agricultural robot control, and in particular to a robust control method and system for agricultural robots based on the high-order fully actuated theory. Background Art

[0002] The autonomous operation technology of agricultural robots can effectively improve the operation efficiency and quality of agricultural machinery, avoid missing operations or repeated operations, and reduce the dependence on agricultural machine operators. For complex terrain areas, when designing a path tracking controller, the impacts of these complex conditions on the autonomous operation effect need to be fully considered. The control system of agricultural machinery should achieve both lateral position tracking and heading tracking, making this control system a underactuated system with one input and two outputs in terms of physical meaning, which will increase the difficulty of controller design and reduce the control effect; considering that there are many parameters in the dynamic model of agricultural machinery that are difficult to accurately measure, and terrain changes will bring unknown disturbances to the control system, a suitable disturbance observer needs to be designed.

[0003] Related technologies have conducted extensive research on the control methods of agricultural robots and contributed various control methods, mainly including PID method, pure pursuit method, model predictive control method, adaptive control, neural network control, etc. However, current methods are mostly based on pure kinematic models or underactuated control system models, lacking effective means to convert them into fully actuated system models; moreover, current disturbance observers usually require three-dimensional measurement information of position, velocity, and acceleration, with high requirements for the measurement system.

[0004] In summary, the technical problems existing in related technologies need to be improved. Summary of the Invention

[0005] In order to solve the above technical problems, the purpose of the present invention is to provide a robust control method and system for agricultural robots based on the high-order fully actuated theory, which can convert an underactuated control system into a fully actuated control system, and at the same time consider the influence of external unknown disturbances, thereby enhancing the robustness of the controller.

[0006] The first technical solution adopted by the present invention is: a robust control method for agricultural robots based on the high-order fully actuated theory, including the following steps:

[0007] Determine the kinematic models of the heading deviation and lateral position deviation of the agricultural robot according to the geometric structure of the agricultural robot, set the input and output of the kinematic models, and construct the dynamic model of the agricultural robot;

[0008] Convert the dynamic model of the agricultural robot into a high-order fully actuated system model of the agricultural robot, and construct a linear control system model considering external disturbances;

[0009] Based on the principle of reducing the state deviation of the agricultural robot, an improved nonlinear disturbance observer is constructed to estimate the disturbance of the linear control system model considering external disturbances, and the estimated value of the external disturbance of the agricultural robot is obtained;

[0010] According to the improved nonlinear disturbance observer, a robust feedback controller is constructed and combined with the estimated value of the external disturbance of the agricultural robot to control the operation of the agricultural robot, so as to realize the real-time tracking of the path of the agricultural robot.

[0011] Furthermore, the step of determining the kinematic model of the heading deviation and lateral position deviation of the agricultural robot according to the geometric structure of the agricultural robot, setting the input and output of the kinematic model, and constructing the dynamic model of the agricultural robot specifically includes:

[0012] According to the geometric structure of the agricultural robot, judge the distance and direction between the forward path of the agricultural robot and the center line of the target path, and obtain the target steering angle of the agricultural robot;

[0013] The agricultural robot plans and walks according to the target steering angle, and constructs a kinematic model of the heading deviation and lateral position deviation of the agricultural robot;

[0014] Perform a driving force analysis on the kinematic model of the heading deviation and lateral position deviation of the agricultural robot, and establish a preliminary dynamic model of the agricultural robot;

[0015] Based on the dynamic model of the agricultural robot, set the tire steering angle of the agricultural robot as the input and the heading angle as the output, and construct the dynamic model of the agricultural robot.

[0016] [[ID=2']]Furthermore, the expression of the dynamic model of the agricultural robot is specifically as follows:

[0017]

[0018] In the above formula, represents the heading angular acceleration, represents the heading angular velocity, μ represents the road surface friction coefficient, C f 、C r respectively represent the cornering stiffness of the front and rear tires of the agricultural robot, l f and l r respectively represent the distances from the front and rear axles of the agricultural robot to the center of mass, J z represents the moment of inertia, v x represents the longitudinal velocity of the agricultural robot, δ r represents the rear wheel steering angle of the agricultural robot.

[0019] Further, the step of converting the dynamic model of the agricultural robot into a high-order all-wheel drive system model of the agricultural robot and constructing a linear control system model considering external disturbances specifically includes:

[0020] Taking the second derivative of the lateral position deviation in the kinematic model of the heading deviation and lateral position deviation of the agricultural robot to obtain a simplified kinematic model of the heading deviation and lateral position deviation of the agricultural robot;

[0021] Substituting the kinematic model of the heading deviation and lateral position deviation of the agricultural robot and the dynamic model of the agricultural robot into the simplified kinematic model of the heading deviation and lateral position deviation of the agricultural robot to eliminate the heading deviation, and constructing a high-order all-wheel drive system model of the agricultural robot;

[0022] Based on the high-order all-wheel drive system model of the agricultural robot, constructing a control system model and a feedback controller considering model parameter uncertainty and external disturbances according to the external disturbance factors in the operation process of the agricultural robot;

[0023] Embedding the feedback controller into the control system model considering model parameter uncertainty and external disturbances to obtain a linear control system model considering external disturbances.

[0024] Further, the expression of the high-order all-wheel drive system model of the agricultural robot is specifically as follows:

[0025]

[0026] In the above formula, represents the second derivative of the lateral position deviation, v x represents the longitudinal speed, represents the heading angular velocity, represents the expected heading angular velocity of the preset point, L represents the forward viewing distance, represents the heading angular acceleration, μ represents the road surface friction coefficient, C f 、C r respectively represent the cornering stiffness of the front and rear tires of the agricultural robot, l f and l r respectively represent the distances from the front and rear axles of the agricultural robot to the center of mass, J z represents the moment of inertia, δ r represents the rear wheel steering angle of the agricultural robot.

[0027] Further, the step of constructing an improved non-linear disturbance observer based on the principle of reducing the state deviation of the agricultural robot to estimate the disturbance of the linear control system model considering external disturbances and obtaining the external disturbance estimation value of the agricultural robot specifically includes:

[0028] Based on the principle of reducing the state deviation of the agricultural robot, a non - linear disturbance observer is constructed;

[0029] Delete the terms related to acceleration in the non - linear disturbance observer and perform an integration operation to obtain a preliminary improved non - linear disturbance observer;

[0030] Define an auxiliary variable and embed it into the preliminary improved non - linear disturbance observer to construct an improved non - linear disturbance observer;

[0031] Use the improved non - linear disturbance observer to estimate the model parameter uncertainty and external disturbance of the linear control system model considering external disturbances, and obtain the external disturbance estimation value of the agricultural robot.

[0032] Furthermore, the expression of the improved non - linear disturbance observer is specifically as follows:

[0033]

[0034] In the above formula, represents the disturbance estimation value, represents the disturbance compensation function, ζ represents the auxiliary variable, represents the first - order derivative of the auxiliary variable, represents the disturbance compensation term, u represents the reference input, x1 represents the state variable of the agricultural robot system, represents the first - order derivative of the state variable of the agricultural robot system.

[0035] Furthermore, the step of constructing a robust feedback controller according to the improved non - linear disturbance observer and combining the external disturbance estimation value of the agricultural robot to control the operation of the agricultural robot to achieve real - time tracking of the agricultural robot path specifically includes:

[0036] Construct a robust feedback controller according to the improved non - linear disturbance observer;

[0037] Based on the dynamic model of the agricultural robot, use the robust feedback controller and combine the external disturbance estimation value of the agricultural robot to control the operation of the agricultural robot to achieve real - time tracking of the agricultural robot path.

[0038] Furthermore, the expression of the robust feedback controller is specifically as follows:

[0039]

[0040] In the above formula, υ represents the reference input, represents the disturbance estimation value, k1, k2 represent the controller gain coefficients, x1 represents the state variable of the agricultural robot system, x2 represents the first - order derivative of the state variable.

[0041] The second technical solution adopted by the present invention is: a robust control system for an agricultural robot based on the high-order fully actuated theory, including:

[0042] A first module, configured to determine a kinematic model of the heading deviation and lateral position deviation of the agricultural robot according to the geometric structure of the agricultural robot, set the input and output of the kinematic model, and construct a dynamic model of the agricultural robot;

[0043] A second module, configured to convert the dynamic model of the agricultural robot into a high-order fully actuated system model of the agricultural robot, and construct a linear control system model considering external disturbances;

[0044] A third module, configured to construct an improved nonlinear disturbance observer based on the principle of reducing the state deviation of the agricultural robot to perform disturbance estimation on the linear control system model considering external disturbances, and obtain an external disturbance estimation value of the agricultural robot;

[0045] A fourth module, configured to construct a robust feedback controller according to the improved nonlinear disturbance observer and combine the external disturbance estimation value of the agricultural robot to perform operation control on the agricultural robot, so as to realize real-time tracking of the path of the agricultural robot.

[0046] The beneficial effects of the method and system of the present invention are: by determining the kinematic model of the heading deviation and lateral position deviation of the agricultural robot according to the geometric structure of the agricultural robot, setting the input and output of the kinematic model, and constructing the dynamic model of the agricultural robot, that is, obtaining an underactuated control system model, and then using the elimination method to convert the low-order underactuated system into a high-order fully actuated system, which is beneficial to obtaining a linear control system model for subsequent disturbance observer and robust controller design. Further, based on the principle of reducing the state deviation of the agricultural robot, an improved nonlinear disturbance observer is constructed to perform disturbance estimation on the linear control system model considering external disturbances, and an external disturbance estimation value of the agricultural robot is obtained, which can realize the simultaneous estimation of the uncertain system model and unknown external disturbances without additional measurement information. Finally, according to the improved nonlinear disturbance observer, a robust feedback controller is constructed and combined with the external disturbance estimation value of the agricultural robot to perform operation control on the agricultural robot, so as to realize real-time tracking of the path of the agricultural robot and improve the tracking stability of the system. Description of the Drawings

[0047] Figure 1 is a flowchart of the steps of a robust control method for an agricultural robot based on the high-order fully actuated theory of the present invention;

[0048] Figure 2 is a structural block diagram of a robust control system for an agricultural robot based on the high-order fully actuated theory of the present invention;

[0049] Figure 3 It is a schematic diagram of the geometric structure of the agricultural robot provided by a specific embodiment of the present invention;

[0050] Figure 4 It is a schematic diagram of the path tracking effect of the agricultural robot provided by a specific embodiment of the present invention. Specific embodiments

[0051] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. For the step numbers in the following embodiments, they are only set for the convenience of elaboration and explanation, and no limitation is imposed on the order between steps. The execution order of each step in the embodiments can be adaptively adjusted according to the understanding of those skilled in the art.

[0052] Referring to Figure 1 , the present invention provides a robust control method for an agricultural robot based on the high-order full drive theory, and the method includes the following steps:

[0053] S100. Determine the kinematic models of the heading deviation and lateral position deviation of the agricultural robot according to the geometric structure of the agricultural robot, set the input and output of the kinematic models, and construct the dynamic model of the agricultural robot;

[0054] Specifically, according to the geometric structure of the agricultural robot, judge the distance and direction between the forward path of the agricultural robot and the center line of the target path, and obtain the target steering angle of the agricultural robot; the agricultural robot performs path planning and walking according to the target steering angle, and constructs the kinematic models of the heading deviation and lateral position deviation of the agricultural robot; perform a driving force analysis on the kinematic models of the heading deviation and lateral position deviation of the agricultural robot to establish a preliminary dynamic model of the agricultural robot; based on the dynamic model of the agricultural robot, set the tire steering angle of the agricultural robot as the input and the heading angle as the output, and construct the dynamic model of the agricultural robot.

[0055] In this embodiment, as Figure 3 shown, when the agricultural robot operates along the target path, assuming that the vehicle can be as predictive as a farmer, it can judge the distance and direction between it and the center line of the target path according to the path point in front of the vehicle driving direction, and then generate a suitable target steering angle so that it can walk along the desired path. According to this rule, the kinematic models of the heading deviation and lateral position deviation can be established, and their expressions are:

[0056]

[0057] In the above formula, Δψ represents the heading deviation, ψ represents the current heading angle, ψ r represents the desired heading angle of the preset point, e represents the lateral position deviation, v x represents the longitudinal speed, vy v represents the longitudinal speed, L represents the forward viewing distance, and w represents the yaw angular velocity of the vehicle.

[0058] Based on the driving force analysis of the agricultural machinery, a dynamic model of the agricultural machinery is established, and its expression is:

[0059]

[0060] In the above formula, J z represents the moment of inertia, l f and l r respectively represent the distances from the front and rear axles to the center of mass, F yf , F yr respectively represent the lateral forces on the front and rear tires, represents the heading angular acceleration.

[0061] Among them, the lateral force of the tire can be approximately expressed as:

[0062]

[0063] Among them, is the side slip angle of the front tire, is the side slip force of the rear tire. C f , C r respectively represent the side slip stiffness of the front and rear tires, μ is the road surface friction coefficient. δ r is the steering angle of the rear wheel of the agricultural machinery, that is, the control input to be designed later.

[0064] Substituting the approximate expression of the lateral force of the tire into the expression of the dynamic model of the agricultural machinery, the dynamic equation of the heading angle can be obtained, and its expression is:

[0065]

[0066] In the above formula, represents the heading angular acceleration, represents the heading angular velocity, μ represents the road surface friction coefficient, C f , C r respectively represent the side slip stiffness of the front and rear tires of the agricultural robot, l f and l r respectively represent the distances from the front and rear axles to the center of mass of the agricultural robot, J z represents the moment of inertia, v x represents the longitudinal speed of the agricultural robot, δ r represents the steering angle of the rear wheel of the agricultural robot.

[0067] S200. Convert the dynamic model of the agricultural robot into a high-order all-wheel drive system model of the agricultural robot, and construct a linear control system model considering external disturbances;

[0068] Specifically, the second-order derivative of the lateral position deviation in the kinematic model of the heading deviation and lateral position deviation of the agricultural robot is taken to obtain a simplified kinematic model of the heading deviation and lateral position deviation of the agricultural robot; the kinematic model of the heading deviation and lateral position deviation of the agricultural robot and the dynamic model of the agricultural robot are substituted into the simplified kinematic model of the heading deviation and lateral position deviation of the agricultural robot to eliminate the heading deviation, and a high-order fully actuated system model of the agricultural robot is constructed; based on the high-order fully actuated system model of the agricultural robot, a control system model and a feedback controller considering model parameter uncertainty and external disturbances are constructed according to the external disturbance factors in the operation process of the agricultural robot; the feedback controller is embedded into the control system model considering model parameter uncertainty and external disturbances to obtain a linear control system model considering external disturbances.

[0069] In this embodiment, it can be seen from the expression of the dynamic equation of the heading angle that the control input δ r can only directly control the heading deviation and cannot directly control the lateral position deviation, belonging to an underactuated system. Therefore, this step transforms the system into a fully actuated control system to facilitate the design and implementation of the controller.

[0070] Considering that when the agricultural robot is operating, it can usually be approximated as traveling at a low speed and at a constant speed. Therefore, the kinematic model can be simplified according to the small-angle assumption (sin(Δψ)≈Δψ, cos(Δψ)≈1). Taking the second-order derivative of the lateral position deviation in the kinematic model of the heading deviation and lateral position deviation gives:

[0071]

[0072] In the above formula, is the second derivative of the lateral position deviation, that is, the lateral acceleration deviation, v x is the longitudinal speed, represents the heading angular velocity deviation, L represents the forward viewing distance, represents the heading angular acceleration.

[0073] To eliminate the variable of the heading deviation, the expressions of the kinematic model of the heading deviation and lateral position deviation and the expression of the dynamic equation of the heading angle are substituted into the expression of the lateral position deviation after the second-order derivative, and the expression of the high-order fully actuated system model of the agricultural robot is specifically as follows:

[0074]

[0075] In the above formula, represents the second derivative of the lateral position deviation, v x represents the longitudinal speed, represents the heading angular velocity, denotes the expected heading angular velocity of the preset point, and L denotes the forward viewing distance. denotes the heading angular acceleration, μ denotes the road surface friction coefficient, and C f 、C r respectively denote the cornering stiffness of the front and rear tires of the agricultural robot, l f and l r respectively denote the distances from the front and rear axles of the agricultural robot to the center of mass, J z denotes the moment of inertia, and δ r denotes the rear wheel steering angle of the agricultural robot.

[0076] Therefore, the low-order underactuated system of the kinematic model of the original heading deviation and lateral position deviation is converted into a high-order fully actuated system model of the agricultural robot.

[0077] Considering that some model parameters of the agricultural robot control system (such as the moment of inertia J z , the cornering stiffness C f 、C r ) are difficult to measure accurately, some model parameters (such as μ is the road surface friction coefficient) will change, and it will also be affected by external disturbances (such as uneven ground) during the operation. Therefore, a control system model considering model parameter uncertainty and external disturbances is established, and its expression is:

[0078]

[0079] In the above formula, the state variable of the system is x1 = e, the control variable is u = δ r , is the control input coefficient, d is the sum of the model parameter uncertainty part and external disturbances, which is unknown and unmeasurable. is the measurable part.

[0080] Construct a feedback controller as follows: According to the control system model considering model parameter uncertainty and external disturbances, the expression of this feedback controller can be designed as:

[0081] u = M -1 (υ - f(·))

[0082] where υ represents the reference input.

[0083] Substitute the expression of the feedback controller into the expression of the control system model considering model parameter uncertainty and external disturbances, and we can get:

[0084]

[0085] It can be seen that this system is a linear closed-loop control system, which is convenient for the subsequent design of disturbance observers and robust controllers.

[0086] S300. Based on the principle of reducing the state deviation of the agricultural robot, an improved nonlinear disturbance observer is constructed to estimate the disturbance of the linear control system model considering external disturbances, and the external disturbance estimation value of the agricultural robot is obtained;

[0087] Specifically, based on the principle of reducing the state deviation of the agricultural robot, a nonlinear disturbance observer is constructed; the terms related to acceleration in the nonlinear disturbance observer are deleted and integrated to obtain a preliminary improved nonlinear disturbance observer; an auxiliary variable is defined and embedded into the preliminary improved nonlinear disturbance observer to construct an improved nonlinear disturbance observer; the improved nonlinear disturbance observer is used to estimate the model parameter uncertainty and external disturbance of the linear control system model considering external disturbances, and the external disturbance estimation value of the agricultural robot is obtained.

[0088] In this embodiment, to eliminate the influence of model uncertainty and external disturbances on the control system, a nonlinear disturbance observer can be designed according to the principle of reducing the system state deviation. The formula of this disturbance observer is:

[0089]

[0090] Among them, is the disturbance estimation value, is the disturbance compensation function.

[0091] As can be seen from the above formula, this disturbance observer is related to the second derivative of the state variable (i.e., the acceleration information ), and it can be seen from the subsequent controller that this acceleration information is not required in the controller design process. Therefore, it is possible to consider reducing the measurement information and the sensor cost, and further improve the disturbance observer based on the disturbance observer formula.

[0092] First, subtract the terms related to acceleration (i.e., ) from both sides of the disturbance observer formula, and we can get

[0093]

[0094] Integrating both sides of the above formula, the expression of the preliminary improved nonlinear disturbance observer can be obtained as:

[0095]

[0096] Among them, is the disturbance compensation term.

[0097] To avoid the integral operation and increase the operation difficulty, an auxiliary variable can be defined, and its derivative can be obtained as its expression:

[0098]

[0099] Based on the expression of the auxiliary variable, we can design an improved nonlinear disturbance observer as follows:

[0100]

[0101] In this disturbance estimator, can be appropriately selected according to the Lyapunov stability theorem, and it can also be proved that this observer is convergent.

[0102] S400. According to the improved nonlinear disturbance observer, construct a robust feedback controller and combine the estimated value of the external disturbance of the agricultural robot to control the operation of the agricultural robot, so as to realize the real-time tracking of the path of the agricultural robot.

[0103] Specifically, according to the improved nonlinear disturbance observer, construct a robust feedback controller; based on the dynamic model of the agricultural robot, use the robust feedback controller and combine the estimated value of the external disturbance of the agricultural robot to control the operation of the agricultural robot, so as to realize the real-time tracking of the path of the agricultural robot.

[0104] In this embodiment, in order to enable the agricultural robot to operate according to the required path, the designed controller is:

[0105]

[0106] In the above formula, k1 and k2 are controller gain coefficients.

[0107] For the dynamic model of the agricultural robot, adopt the improved nonlinear disturbance estimator formula and design the robust controller formula, which can enable the agricultural robot to drive and operate according to the desired trajectory, and according to the Lyapunov stability theorem, it can be demonstrated that the controller of this system is stable.

[0108] In summary, in the embodiments of the present invention, a kinematic model of lateral deviation and heading deviation is established according to the geometric structure of the agricultural robot, with the rear wheel steering angle as the input and the heading angle as the output, and a dynamic model of the agricultural robot is established. This combined model physically belongs to an underactuated control system with one input and two outputs; based on the theory of high-order fully actuated systems, the physically underactuated model is converted into a fully actuated system model in the mathematical sense through an elimination method, which is convenient for subsequent controller design; an improved non-linear disturbance observer is constructed, which can realize the real-time estimation of unknown disturbances and model parameter uncertainties without the need for acceleration measurement information; a robust feedback controller is designed, which enables the agricultural robot to autonomously operate along the desired path. By constructing the kinematic model and dynamic model of the agricultural robot, an underactuated control system model is obtained, and the low-order underactuated system is converted into a high-order fully actuated system by using the elimination method. Converting the underactuated system model into a fully actuated system model is beneficial to obtaining a linear control system model for subsequent disturbance observer and robust controller design. An improved non-linear disturbance observer is designed to estimate unknown disturbances in real time. Among them, the designed non-linear disturbance observer can realize the simultaneous estimation of the uncertain system model and unknown external disturbances without the need for additional measurement information and is used in the feedback control loop of the controller.

[0109] As Figure 4 shown, to verify the effectiveness of the proposed controller, a simulation experiment was carried out, and the proposed controller was compared and analyzed with existing relatively new controllers (ECA controller, EEMB controller, ILO controller). In this experiment, the initial position deviation was set to -2 m, the expected value was 0 m, and the initial angle and expected angle were 0°. From the first three subgraphs (position tracking, angle tracking, and control force), it can be seen that compared with the other three controllers, the proposed controller can achieve position tracking and angle tracking with less fluctuation and has a fast convergence time. According to the fourth subgraph (disturbance value), it can be known that the disturbance observer designed in the present invention can effectively and real-time estimate the external unknown disturbance with high accuracy.

[0110] Referring Figure 2 , a robust control system for an agricultural robot based on high-order fully actuated theory, includes:

[0111] The first module 201 is used to determine the kinematic model of the heading deviation and lateral position deviation of the agricultural robot according to the geometric structure of the agricultural robot, set the input and output of the kinematic model, and construct the dynamic model of the agricultural robot;

[0112] The second module 202 is used to convert the dynamic model of the agricultural robot into a high-order fully actuated system model of the agricultural robot and construct a linear control system model considering external disturbances;

[0113] The third module 203 is used to construct an improved non-linear disturbance observer based on the principle of reducing the state deviation of the agricultural robot to estimate the disturbance of the linear control system model considering external disturbances, and obtain the estimated value of the external disturbance of the agricultural robot;

[0114] The fourth module 204 is used to construct a robust feedback controller according to the improved non-linear disturbance observer and combine the estimated value of the external disturbance of the agricultural robot to control the operation of the agricultural robot, so as to realize the real-time tracking of the path of the agricultural robot.

[0115] The content in the above method embodiments is applicable to the system embodiments. The functions specifically implemented by the system embodiments are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those of the above method embodiments.

[0116] The above is a specific description of the preferred embodiment of the present invention. However, the present invention is not limited to the described embodiment. Those skilled in the art can make various equivalent deformations or substitutions without departing from the spirit of the present invention, and these equivalent deformations or substitutions are all included in the scope defined by the claims of this application.

Claims

1. A robust control method for an agricultural robot based on the high-order full-drive theory, characterized in that, It includes the following steps: Determine the kinematic models of the heading deviation and lateral position deviation of the agricultural robot according to the geometric structure of the agricultural robot, set the input and output of the kinematic models, and construct the dynamic model of the agricultural robot; Convert the dynamic model of the agricultural robot into a high-order all-wheel drive system model of the agricultural robot, and construct a linear control system model considering external disturbances; Based on the principle of reducing the state deviation of the agricultural robot, construct an improved nonlinear disturbance observer to estimate the disturbance of the linear control system model considering external disturbances, and obtain the external disturbance estimation value of the agricultural robot; According to the improved nonlinear disturbance observer, construct a robust feedback controller and combine it with the external disturbance estimation value of the agricultural robot to control the operation of the agricultural robot, and realize the real-time tracking of the agricultural robot path.

2. The robust control method of an agricultural robot based on the high-order full-drive theory according to claim 1, wherein, The step of determining the kinematic models of the heading deviation and lateral position deviation of the agricultural robot according to the geometric structure of the agricultural robot, setting the input and output of the kinematic models, and constructing the dynamic model of the agricultural robot specifically includes: According to the geometric structure of the agricultural robot, judge the distance and direction between the forward path of the agricultural robot and the center line of the target path, and obtain the target steering angle of the agricultural robot; The agricultural robot plans and walks according to the target steering angle, and constructs the kinematic models of the heading deviation and lateral position deviation of the agricultural robot; Conduct a driving force analysis on the kinematic models of the heading deviation and lateral position deviation of the agricultural robot, and establish a preliminary dynamic model of the agricultural robot; Based on the dynamic model of the agricultural robot, set the tire steering angle of the agricultural robot as the input and the heading angle as the output, and construct the dynamic model of the agricultural robot.

3. The robust control method for an agricultural robot based on the high-order full-drive theory according to claim 2, wherein, The expression of the dynamic model of the agricultural robot is specifically as follows: In the above formula, represents the yaw angular acceleration, represents the yaw angular velocity, μ represents the road surface friction coefficient, C f and C r respectively represent the cornering stiffness of the front and rear tires of the agricultural robot, l f and l r respectively represent the distances from the front and rear axles of the agricultural robot to the center of mass, J z represents the moment of inertia, v x represents the longitudinal velocity of the agricultural robot, δ r represents the rear-wheel steering angle of the agricultural robot.

4. The robust control method for an agricultural robot based on the high-order full-drive theory according to claim 3, characterized in that, The step of converting the dynamic model of the agricultural robot into a high-order all-wheel drive system model of the agricultural robot, and constructing a linear control system model considering external disturbances specifically includes: Take the second derivative of the lateral position deviation in the kinematic models of the heading deviation and lateral position deviation of the agricultural robot to obtain the simplified kinematic models of the heading deviation and lateral position deviation of the agricultural robot; Substitute the kinematic models of the heading deviation and lateral position deviation of the agricultural robot and the dynamic model of the agricultural robot into the simplified kinematic models of the heading deviation and lateral position deviation of the agricultural robot to eliminate the heading deviation, and construct a high-order all-wheel drive system model of the agricultural robot; Based on the high-order all-wheel drive system model of the agricultural robot, construct a control system model and a feedback controller considering model parameter uncertainty and external disturbances according to the external disturbance factors in the operation process of the agricultural robot; Embed the feedback controller into the control system model considering model parameter uncertainty and external disturbances to obtain a linear control system model considering external disturbances.

5. The robust control method of an agricultural robot based on the high-order full-drive theory according to claim 4, characterized in that, The expression of the high-order all-wheel drive system model of the agricultural robot is specifically as follows: In the above formula, represents the second derivative of the lateral position deviation, v x represents the longitudinal velocity, represents the yaw angular velocity, represents the desired yaw angular velocity of the preset point, L represents the forward viewing distance, represents the yaw angular acceleration, μ represents the road surface friction coefficient, C f 、C r respectively represent the cornering stiffness of the front and rear tires of the agricultural robot, l f and l r respectively represent the distances from the front and rear axles of the agricultural robot to the center of mass, J z represents the moment of inertia, δ r represents the rear wheel steering angle of the agricultural robot.

6. The robust control method of an agricultural robot based on the high-order full-drive theory according to claim 5, characterized in that, The step of constructing an improved nonlinear disturbance observer based on the principle of reducing the state deviation of the agricultural robot to estimate the disturbance of the linear control system model considering external disturbances and obtaining the external disturbance estimation value of the agricultural robot specifically includes: Based on the principle of reducing the state deviation of the agricultural robot, a nonlinear disturbance observer is constructed; The terms related to acceleration in the nonlinear disturbance observer are deleted and integrated to obtain a preliminary improved nonlinear disturbance observer; An auxiliary variable is defined and embedded into the preliminary improved nonlinear disturbance observer to construct an improved nonlinear disturbance observer; The improved nonlinear disturbance observer is used to estimate the model parameter uncertainty and external disturbance of the linear control system model considering external disturbances, and the external disturbance estimation value of the agricultural robot is obtained.

7. The robust control method for an agricultural robot based on the high-order full-drive theory according to claim 6, wherein, The expression of the improved nonlinear disturbance observer is specifically as follows: In the above formula, represents the disturbance estimated value, represents the disturbance compensation function, ζ represents the auxiliary variable, represents the first derivative of the auxiliary variable, represents the disturbance compensation term, υ represents the reference input, x1 represents the state variable of the agricultural robot system, represents the first derivative of the state variable of the agricultural robot system.

8. The robust control method of an agricultural robot based on the high-order full-drive theory according to claim 7, characterized in that, The step of constructing a robust feedback controller according to the improved nonlinear disturbance observer and combining the external disturbance estimation value of the agricultural robot to control the operation of the agricultural robot to achieve real-time tracking of the agricultural robot path specifically includes: Construct a robust feedback controller according to the improved nonlinear disturbance observer; Based on the dynamic model of the agricultural robot, the robust feedback controller is used to control the operation of the agricultural robot in combination with the external disturbance estimation value of the agricultural robot to achieve real-time tracking of the agricultural robot path.

9. The robust control method of an agricultural robot based on the high-order full-drive theory according to claim 8, characterized in that, The expression of the robust feedback controller is specifically as follows: In the above formula, υ represents the reference input, represents the disturbance estimation value, k1 and k2 represent the controller gain coefficients, x1 represents the state variable of the agricultural robot system, and x2 represents the first derivative of the state variable.

10. A robust control system for an agricultural robot based on the high-order full-drive theory, characterized in that, It includes the following modules: The first module is used to determine the kinematic models of the heading deviation and lateral position deviation of the agricultural robot according to the geometric structure of the agricultural robot, set the input and output of the kinematic models, and construct the dynamic model of the agricultural robot; The second module is used to convert the dynamic model of the agricultural robot into a high-order fully actuated system model of the agricultural robot and construct a linear control system model considering external disturbances; The third module is used to construct an improved nonlinear disturbance observer based on the principle of reducing the state deviation of the agricultural robot to estimate the disturbance of the linear control system model considering external disturbances, and obtain the external disturbance estimation value of the agricultural robot; The fourth module is used to construct a robust feedback controller according to the improved nonlinear disturbance observer and control the operation of the agricultural robot in combination with the external disturbance estimation value of the agricultural robot to achieve real-time tracking of the agricultural robot path.

Images