Agricultural machinery arm robust control method with anti-disturbance function

Abstract

The application relates to the technical field of agricultural equipment motion control, and discloses a robust control method for an agricultural machine arm with anti-disturbance function, which comprises the following steps: firstly, real-time collection of real angles and angular velocities of each joint, calculation of a trajectory tracking error and an angular velocity error vector; then, establishment of a mechanical arm state space model containing parameter perturbation and external disturbance; on this basis, construction of a finite-time robust sliding mode surface, further combination of an adaptive technology to construct an adaptive robust controller, and finally output of a control signal to drive each joint servo motor. When facing internal parameter perturbation and external complex farmland environment disturbance, the application can ensure that the trajectory tracking error of the mechanical arm converges to zero rapidly and stably in a limited time, and significantly improves the operation precision and global robustness of the control system.

Description

Technical Field

[0001] This invention belongs to the field of motion control of agricultural machinery equipment, specifically a robust control method for agricultural machinery robotic arms with anti-disturbance function. Background Technology

[0002] As an important component of agricultural robots, agricultural robotic arms are being applied across industries, from industrial to agricultural sectors, driving the modernization and precision of traditional agricultural technologies. They play a vital role in precision agricultural operations such as crop harvesting, seedling raising and sowing, and plant pruning.

[0003] However, agricultural robotic arms are inherently complex dynamic systems with multiple inputs and outputs, high nonlinearity, and strong coupling. Modeling these systems introduces uncertainties such as parameter perturbations, external disturbances, and unmodeled dynamics, all of which affect the control stability and performance of the robotic arms. In the complex and ever-changing farmland operating environment, robotic arms inevitably face parameter perturbations caused by load variations, external environmental disturbances such as wind loads, and the difficulty of accurate mathematical modeling. These multi-source uncertainties severely compromise the control accuracy and system stability of agricultural robotic arms.

[0004] Currently, for the motion control of agricultural machinery robotic arms, engineering mainly employs methods such as conventional PID control, traditional sliding mode control, and fuzzy control. However, under actual complex operating conditions, these methods reveal significant limitations: for example, conventional PID controllers struggle to handle strong nonlinearity and external time-varying disturbances, exhibiting poor robustness to parameter changes; while traditional sliding mode control (SMC), although possessing some robustness to internal parameter perturbations and external disturbances and capable of asymptotic convergence of system state errors, cannot guarantee that the control system reaches a steady state within a finite time, making it difficult to achieve robust control within a finite time frame.

[0005] Therefore, it is of great significance to design and develop robust control methods with anti-disturbance functions to address the problem of robust control for the stability of agricultural machinery arms. Summary of the Invention

[0006] The technical problem to be solved by the present invention is to provide a robust control method for agricultural machinery robotic arms with anti-disturbance function, which solves the problem of poor control performance of existing agricultural machinery robotic arm control systems within a limited time, so as to achieve robust control of agricultural machinery robotic arms when there are parameter perturbations and external disturbances.

[0007] To address the aforementioned technical problems, this invention provides a robust control method for an agricultural machinery robotic arm with anti-disturbance capabilities, comprising the following specific steps:

[0008] First, the state variables of each joint of the agricultural machinery robotic arm are collected, and the trajectory tracking error vector is calculated. and trajectory tracking angular velocity error vector Then, a state-space model of the agricultural machinery arm with parameter perturbation and external disturbance is established. Next, a finite-time robust sliding surface is constructed by combining the trajectory tracking error vector and the trajectory tracking angular velocity error vector through a nonlinear power weighting method. An adaptive robust controller is constructed by combining the adaptive robust compensation term generated by real-time estimation. The adaptive robust controller outputs the control signals of the servo motors of each joint to control the motion trajectory of the agricultural machinery arm.

[0009] As an improvement to the robust control method for agricultural machinery robotic arms with anti-disturbance function of the present invention:

[0010] The joint state variables include the actual angles of each joint. and true angular velocity ;

[0011] The trajectory tracking error vector of each joint From a realistic perspective and given angle difference;

[0012] The trajectory tracking angular velocity error of each joint For true angular velocity and given angular velocity difference.

[0013] As a further improvement to the robust control method for agricultural machinery robotic arms with anti-disturbance function of the present invention:

[0014] The state-space model of the agricultural machinery robotic arm is as follows:

[0015]

[0016] in, This represents the control input vector of the servo motor. This represents the positive definite inertia matrix of the agricultural machinery robotic arm. Represents the angular acceleration of the net external torque. Represents the set of uncertain terms. Given angular acceleration.

[0017] As a further improvement to the robust control method for agricultural machinery robotic arms with anti-disturbance function of the present invention:

[0018] The net external torque angular acceleration is

[0019]

[0020] in, Represents the matrices of centrifugal force and Coriolis force. Represents the gravity vector;

[0021] The lumped uncertainty term is:

[0022]

[0023] in, , and These represent parameter perturbations of the agricultural machinery robotic arm; Represents the external disturbance vector; at the same time ,in, It is an unknown normal quantity.

[0024] As a further improvement to the robust control method for agricultural machinery robotic arms with anti-disturbance function of the present invention:

[0025] The finite-time robust sliding surface is:

[0026]

[0027] in, This represents known normal quantities. and It is a positive odd number and , Representing Bernoulli numbers, This indicates the number of items to be summed.

[0028] As a further improvement to the robust control method for agricultural machinery robotic arms with anti-disturbance function of the present invention:

[0029] The adaptive robust controller is:

[0030]

[0031] in, This represents known normal quantities. For compensating control items, This is an adaptive robust compensation term.

[0032] As a further improvement to the robust control method for agricultural machinery robotic arms with anti-disturbance function of the present invention:

[0033] The compensation control item is specifically as follows:

[0034]

[0035] The adaptive robust compensation term is specifically as follows:

[0036]

[0037] in, This is an estimated value.

[0038] As a further improvement to the robust control method for agricultural machinery robotic arms with anti-disturbance function of the present invention:

[0039] The estimated value Update according to the following adaptive algorithm:

[0040]

[0041] in, These are known normal values.

[0042] The beneficial effects of this invention are mainly reflected in:

[0043] 1. This invention designs a novel sliding surface that can ensure the convergence of system errors within a finite time, ensuring that the trajectory tracking error and angular velocity error of the agricultural machinery arm converge to zero within a finite time; at the same time, it exhibits faster tracking speed and no overshoot in the transient phase of the response, effectively improving the smoothness of the robotic arm's motion trajectory.

[0044] 2. The present invention incorporates adaptive technology into the designed controller. This technology reduces the dependence on the upper bound of parameter perturbation and external disturbance information, and can perform online estimation and dynamic compensation of the upper bound of uncertainty of the robot arm's parameter perturbation and external disturbance in real time, thereby enhancing the system's adaptability.

[0045] 3. This invention designs a finite-time robust controller based on sliding mode control, fully considering the highly nonlinear dynamics of the agricultural machinery robotic arm, including high centrifugal force, Coriolis force, and gravity. Simulation experiments demonstrate that this invention can output a smooth and stable control signal, accelerating the convergence speed, thereby improving the reliability and accuracy of the agricultural machinery robotic arm in farmland environments. Attached Figure Description

[0046] The specific embodiments of the present invention will be further described in detail below with reference to the accompanying drawings.

[0047] Figure 1 This is a flowchart illustrating a robust control method for an agricultural machinery robotic arm with anti-disturbance function according to the present invention.

[0048] Figure 2 The motion trajectory and expected trajectory curve of joint 1 of the agricultural machinery robotic arm under parameter perturbation and external disturbance;

[0049] Figure 3 The curves show the motion trajectory and expected trajectory of joint 2 of the agricultural machinery arm under parameter perturbation and external disturbance. Detailed Implementation

[0050] The present invention will be further described below with reference to specific embodiments, but the scope of protection of the present invention is not limited thereto:

[0051] Example 1: A robust control method for agricultural machinery robotic arms with anti-disturbance function

[0052] The agricultural machinery robotic arm system mainly consists of a multi-joint mechanical structure, a servo drive unit, and a sensing module. The servo drive unit includes servo motors for driving each joint, and the sensing module includes sensors integrated into each joint to collect joint state variables in real time. These joint state variables include the actual angles of each joint of the agricultural machinery robotic arm. and true angular velocity Then, the robust control method for agricultural machinery robotic arms with anti-disturbance function of the present invention obtains the control signals of the servo motors of each joint, realizing precise control of the robotic arm's movement. The robust control method for agricultural machinery robotic arms of the present invention is specifically optimized for parameter perturbations and external disturbances existing in the system. Under such disturbance conditions, it can still ensure that the actual movement trajectory of the agricultural machinery robotic arm quickly and stably tracks the desired trajectory, and that the trajectory tracking error converges to zero within a finite time, thereby improving the movement performance and operational reliability of the agricultural machinery robotic arm in complex farmland environments. The specific process of the robust control method for agricultural machinery robotic arms with anti-disturbance function of the present invention is as follows: Figure 1 As shown, it includes the following steps:

[0053] Step 1: Define the trajectory tracking error vector and trajectory tracking angular velocity error vector for each joint of the agricultural machinery robotic arm.

[0054] Real-time collected data on the true angles of each joint of the agricultural machinery robotic arm and true angular velocity Calculate the error vector for each joint:

[0055] 1) Determine the actual angles of each joint of the agricultural machinery robotic arm. and given angle The difference is used as the trajectory tracking error vector for each joint controlled by the agricultural machinery robotic arm. ;

[0056] 2) The actual angular velocities of each joint of the agricultural machinery arm. and given angular velocity The difference is used as the trajectory tracking angular velocity error vector of each joint controlled by the agricultural machinery robotic arm. Then there is .

[0057] Step 2: Based on the analysis and definition in Step 1, establish a state-space model of the agricultural machinery arm with parameter perturbations and external disturbances, specifically as follows:

[0058] (1)

[0059] in, , , Representing the The trajectory tracking error of each joint Representing the The trajectory tracking angular velocity error of each joint; The matrix represents the positive definite inertia of the agricultural machinery robotic arm, and the elements in the matrix are the nominal values ​​of the system. This represents the control input vector of the servo motors at each joint of the agricultural machinery robotic arm; This represents a given angular acceleration vector.

[0060] The net external torque angular acceleration during system operation is ,in, This matrix represents the centrifugal force and Coriolis force of the agricultural machinery robotic arm during operation, with the elements in the matrix being the system's nominal values. This represents the gravity vector of the agricultural machinery arm, with elements representing standard gravitational acceleration.

[0061] The lumped uncertainty term of the system is ,in, , , These represent parameter perturbations of the agricultural machinery robotic arm; This represents the external disturbance vector. Simultaneously... ,in, It is an unknown normal quantity.

[0062] Step 3: Construct a finite-time robust sliding surface

[0063] Based on the trajectory tracking error vector and trajectory tracking angular velocity error vector of the agricultural machinery arm obtained in step 1, a finite-time robust sliding mode surface vector is constructed using a nonlinear power-weighted method. By appropriately selecting the power-law parameters and weighting coefficients, the finite-time robust sliding mode surface vector satisfies the finite-time stability condition while also considering convergence speed and control smoothness, thus ensuring that the error converges within a finite time. A finite-time robust sliding mode surface vector is designed for the agricultural machinery arm control system. Specifically:

[0064] (2)

[0065] in, Indicates known normal quantities; and It is a positive odd number and ; Represent Bernoulli numbers; This represents the number of items to be summed, i.e. The summation index is used for the series expansion. The upper limit of the summation in formula (2) is infinite. However, in actual engineering applications and subsequent simulation experiments, in order to balance control accuracy and reduce the computational complexity of the controller and ensure the real-time performance of the system, the infinite series is truncated and the upper limit of the summation is taken as 5 (i.e., only the first 5 terms of the series are taken).

[0066] Step 4: Build an adaptive robust controller

[0067] Combining finite-time robust sliding surface and adaptive techniques, an online adaptive estimation mechanism is established to estimate the upper bound of uncertainty, addressing parameter perturbations and external disturbances in the system. Real-time updates of estimated parameters, and finite-time robust sliding surface and estimated value An adaptive robust compensation term is introduced to generate an adaptive robust controller. The adaptive robust controller of this invention can maintain system stability under unknown disturbance conditions and improve trajectory tracking performance. The designed adaptive robust controller is as follows:

[0068] (3)

[0069] in, Indicates known normal quantities; For compensating control items, For adaptive robust compensation terms;

[0070] (4)

[0071] (5)

[0072] in, For parameters The estimated value, and the estimated value Update according to the following adaptive algorithm:

[0073] (6)

[0074] Among them, parameters These are known normal values.

[0075] Under the action of the adaptive robust controller of the present invention, the system's state convergence time There exists an upper limit that satisfies the following condition:

[0076] (7)

[0077] Step 5: Robust control of agricultural machinery robotic arms with disturbance resistance capabilities in the presence of parameter perturbations and external disturbances.

[0078] The control signals of each joint output by the adaptive robust controller in step 4 are sent to the servo motors of the corresponding joints to control the motion trajectory of the agricultural machinery arm. Under the condition of parameter perturbation and external disturbance in the system, the trajectory tracking error of the agricultural machinery arm can be converged to zero within a finite time, thereby improving the motion control performance of the agricultural machinery arm.

[0079] Experiment 1

[0080] The robust control method for agricultural machinery manipulators with anti-disturbance function (hereinafter referred to as ANFCC) described in Example 1 was used to simulate and verify the present invention in a two-joint manipulator. To describe the parameter perturbation of the agricultural machinery manipulator, a certain model of two-degree-of-freedom agricultural machinery manipulator was used as an example. The actual mass and moment of inertia of the two joints were set to 1.2 times their nominal values, and the nominal masses of joint 1 and joint 2 were 0.5 kg and 1.5 kg, respectively; the nominal moments of inertia of joint 1 and joint 2 were both... The rod lengths of joint 1 and joint 2 are 1m and 0.8m respectively; the external time-varying disturbance is... and The specific parameters of the controller of this invention are as follows: , , , , , , To verify the effectiveness of the robust control method of this invention, this invention combines traditional sliding mode control... ( The simulation results (using trajectory tracking error as a benchmark for comparison) include the actual motion trajectory of the two joints of the agricultural machinery arm and the given expected trajectory curve. The simulation results of the two joints are as follows: Figures 2 to 3 As shown.

[0081] Depend on Figure 2 and Figure 3 It is known that, under the condition of parameter perturbations and external disturbances in the control system of agricultural machinery robotic arms, the adaptive robust controller of this invention can enable the trajectory tracking error of the agricultural machinery robotic arm to converge to zero and remain stable within a finite time, thus improving the trajectory tracking performance. Compared with the traditional sliding mode control (SMC) method, when the initial position deviation is large, this invention (ANFCC, blue line) achieves zero steady-state error tracking of the desired trajectory (Desired, black line) in about 2.5 seconds, while the traditional sliding mode control (SMC, red line) has a significant lag, and it takes about 5 seconds to complete the tracking convergence. The trajectory tracking effect of the agricultural machinery robotic arm based on this invention (ANFCC) is better, and the system error convergence speed is significantly faster. At the same time, in In the transient response phase of seconds, the ANFCC curve is steeper than the SMC curve and has no overshoot, proving that the finite-time sliding surface proposed in this invention has a stronger error convergence driving force.

[0082] In summary, the agricultural machinery arm control system constructed based on the robust control method for agricultural machinery arms with anti-disturbance function described in this invention has strong robustness when facing parameter perturbations and external disturbances. At the same time, the system error can converge to zero within a finite time and has a better convergence speed than traditional sliding mode control.

[0083] Finally, it should be noted that the above examples are merely some specific embodiments of the present invention. Obviously, the present invention is not limited to the above embodiments, and many variations can be applied to robotic arm systems with different degrees of freedom. All variations that can be directly derived or conceived by those skilled in the art from the disclosure of this invention should be considered within the scope of protection of this invention.

Claims

1. A robust control method for an agricultural machinery robotic arm with anti-disturbance function, characterized in that... The process includes the following: First, the state variables of each joint of the agricultural machinery robotic arm are collected, and the trajectory tracking error vector is calculated. and trajectory tracking angular velocity error vector ; Then, a state-space model of the agricultural machinery arm with parameter perturbation and external disturbance is established. Next, a finite-time robust sliding surface is constructed by combining the trajectory tracking error vector and the trajectory tracking angular velocity error vector through a nonlinear power weighting method. An adaptive robust controller is constructed by combining the adaptive robust compensation term generated by real-time estimation. The adaptive robust controller outputs the control signals of the servo motors of each joint to control the motion trajectory of the agricultural machinery arm.

2. The robust control method for an agricultural machinery robotic arm with anti-disturbance function according to claim 1, characterized in that: The joint state variables include the actual angles of each joint. and true angular velocity ; The trajectory tracking error vector of each joint From a realistic perspective and given angle difference; The trajectory tracking angular velocity error of each joint For true angular velocity and given angular velocity difference.

3. The robust control method for an agricultural machinery robotic arm with anti-disturbance function according to claim 2, characterized in that: The state-space model of the agricultural machinery robotic arm is as follows: in, This represents the control input vector of the servo motor. This represents the positive definite inertia matrix of the agricultural machinery robotic arm. Represents the angular acceleration of the net external torque. Represents the set of uncertain terms. Given angular acceleration.

4. A robust control method for an agricultural machinery robotic arm with anti-disturbance function according to claim 3, characterized in that: The net external torque angular acceleration is in, Represents the matrices of centrifugal force and Coriolis force. Represents the gravity vector; The lumped uncertainty term is: in, , and These represent parameter perturbations of the agricultural machinery robotic arm; Represents the external disturbance vector; at the same time ,in, It is an unknown normal quantity.

5. A robust control method for an agricultural machinery robotic arm with anti-disturbance function according to claim 4, characterized in that: The finite-time robust sliding surface is: in, This represents known normal quantities. and It is a positive odd number and , Representing Bernoulli numbers, This indicates the number of items to be summed.

6. A robust control method for an agricultural machinery robotic arm with anti-disturbance function according to claim 5, characterized in that: The adaptive robust controller is: in, This represents known normal quantities. For compensating control items, This is an adaptive robust compensation term.

7. A robust control method for an agricultural machinery robotic arm with anti-disturbance function according to claim 6, characterized in that: The compensation control item is specifically as follows: The adaptive robust compensation term is specifically as follows: in, This is an estimated value.

8. A robust control method for an agricultural machinery robotic arm with anti-disturbance function according to claim 7, characterized in that: The estimated value Update according to the following adaptive algorithm: in, These are known normal values.

Images