Input-limited fruit and vegetable picking mechanical arm double-fixed-time second-order sliding mode control method
By designing the dual fixed time second-order sliding mode control method for the robotic arm with limited input, the input saturation and uncertainty problems during the robotic arm picking process are solved, and high-precision anti-saturation control and robustness are achieved, ensuring fruits and vegetables Efficient completion of the picking task.
Patent Information
- Application Number
- CN202411156581.8
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-22
- Publication Date
- 2024-11-08
- Estimated Expiration
- 2044-08-22
AI Technical Summary
The robotic arm has input saturation and uncertainty problems during fruit and vegetable picking, which affects the stability and accuracy of the control system, making it difficult to achieve efficient and rapid picking.
A dual fixed time second-order sliding mode control method for fruit and vegetable picking robot arm is designed, and the construction is achieved through kinematic inverse solution analysis, dynamic modeling, fixed time disturbance observer and fractional power term auxiliary system. Accurate estimation and compensation of modular errors and external interference weakens input saturation and enhances robustness.
High-precision anti-saturation control of the robotic arm system is realized, which enhances robustness, ensures real-time accuracy of trajectory tracking, and reduces energy consumption.
Smart Images

Figure CN118906058A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of agricultural robots, and in particular to a dual-fixed-time second-order sliding mode control method for a fruit and vegetable picking robot arm with limited input. Background Art
[0002] With the continuous advancement of agricultural technology, various technological achievements have been widely used in all aspects of agricultural production, especially in the field of intelligent agricultural robots. Fruit and vegetable picking is a labor-intensive job that usually requires a lot of manpower input, high labor intensity and complex working environment. The application of fruit and vegetable picking robotic arms can not only significantly improve picking efficiency and quality, but also effectively reduce labor intensity and production costs.
[0003] As a multi-input and multi-output strongly coupled nonlinear system, the robot arm has uncertainties such as modeling errors and external interference. In the process of rapid picking, the increase in the inertia force of the robot arm will reduce the stability of the system and the control accuracy. Although the traditional control method can realize the rapid motion control of the robot arm, it requires accurate modeling, and it is difficult to avoid external interference in the rapid picking action of the robot arm. Sliding mode control is known for its good robustness to external interference and uncertainty. The second-order sliding mode can suppress chattering and expand the relative order while retaining the good performance of the traditional first-order sliding mode. Compared with the arbitrary order sliding mode control design, it is simpler. Fruits and vegetables need to be picked in time after they are ripe. The picking window period of some fruits and vegetables is short. Therefore, the control system of the picking robot arm must converge in a clear and short enough time to quickly complete the picking task and reduce energy consumption. Most sliding mode control schemes use finite time convergence, while the fixed time control method can ensure that the system convergence time is bounded under any initial state, solve the problem that the trajectory tracking time is infinitely extended with the increase of initial conditions, and improve the accuracy and rapidity of control. In addition, in the actual control system of the robot arm, the inherent physical constraints of the actuator can easily lead to the control input saturation problem, affecting the dynamic control performance and stability of the system, and even causing damage to the mechanism. It is crucial to design a dual fixed-time second-order sliding mode control method for the input-constrained fruit and vegetable picking robot arm by combining the disturbance observer and the input saturation auxiliary system to achieve its high-precision anti-saturation control. Summary of the invention
[0004] The purpose of the present invention is to provide a double fixed-time second-order sliding mode control method for an input-constrained fruit and vegetable picking robot arm. Aiming at the input saturation and uncertainty problems existing in the fruit and vegetable picking robot arm system, a double fixed-time second-order sliding mode control method for an input-constrained fruit and vegetable picking robot arm is proposed to ensure the fixed-time convergence of the control system observation error and tracking error, so as to realize high-precision anti-saturation control of the input-constrained fruit and vegetable picking robot arm system.
[0005] The inventive concept of the present invention is to perform kinematic inverse analysis on a fruit and vegetable picking robot for picking tasks, obtain the kinematic forward solution and Jacobian matrix of the fruit and vegetable picking robot, and define the expected motion trajectory of the end effector of the robot. Then, a dynamic model of an input-constrained fruit and vegetable picking robot considering uncertainties such as modeling errors and interference from complex agricultural environments is established. Secondly, a fixed-time disturbance observer is designed to accurately estimate and compensate for the lumped uncertainties such as the system's modeling errors and external interference within a fixed time. A new type of fixed-time convergence auxiliary system is constructed by introducing fractional power terms containing auxiliary state quantities to compensate for input saturation. Combining the observer and the auxiliary system, a sliding surface based on the estimated value of the fixed-time disturbance observer and the fixed-time convergence auxiliary system state quantity is defined, and a dual fixed-time second-order sliding mode control method is proposed to simultaneously achieve fixed-time convergence of the observation error and tracking error of the input-constrained fruit and vegetable picking robot control system, enhance its robustness, and weaken the chattering of the sliding mode control. Finally, taking the agricultural picking robot arm as an example, simulation and prototype experimental research are carried out to verify that the designed control method can achieve real-time and accurate tracking of the target picking point trajectory. The present invention can effectively enhance the robustness of the robot arm control system for fruit and vegetable picking tasks, so as to achieve high-precision anti-saturation control of the input-constrained fruit and vegetable picking robot arm system.
[0006] In order to achieve the above-mentioned invention object, the technical solution adopted by the present invention is specifically: a double fixed-time second-order sliding mode control method for a fruit and vegetable picking robot arm with input constraints, comprising the following steps:
[0007] Step 1: Perform inverse kinematic analysis on the fruit and vegetable picking robot arm for picking tasks, obtain the kinematic forward solution and Jacobian matrix of the fruit and vegetable picking robot arm, and define the expected motion trajectory of the end effector of the robot arm;
[0008] Step 2: Establish a dynamic model of the input-constrained fruit and vegetable picking robot arm that takes into account uncertainties such as modeling errors and interference from complex agricultural environments;
[0009] Step 3: Design a fixed-time disturbance observer for the dynamic model in step 2 to accurately estimate and compensate for the lumped uncertainty D of the system modeling error and external interference within a fixed time;
[0010] Step 4: for the dynamic model in step 2, a new type of fixed-time convergence auxiliary system is constructed by introducing a fractional power term containing an auxiliary state quantity ξ to compensate for input saturation;
[0011] Step 5: Combine the disturbance observer designed in step 3 and the auxiliary system designed in step 4 to define the fixed-time disturbance observer estimate The sliding mode surface of the state quantity ξ of the auxiliary system is converged with the fixed time, and a double fixed-time second-order sliding mode control method for the input-constrained fruit and vegetable picking robot arm is designed to achieve real-time and accurate tracking of the trajectory of the target picking point;
[0012] Step 6: Taking the agricultural harvesting robot arm as an example, simulation and prototype experimental research are carried out to verify the correctness and effectiveness of the designed control method.
[0013] Furthermore, in step 2, for the fruit and vegetable picking robot arm for picking tasks, considering uncertain factors such as modeling error and interference from complex agricultural environment, the dynamic model of the input-constrained fruit and vegetable picking robot arm in the joint space is expressed as:
[0014]
[0015] In the formula, are the position, velocity and acceleration vector of each active joint of the robot arm, M(q)∈R n×n is the inertia matrix, is the Coriolis force and centrifugal force, G(q)∈R n is the gravity term, τ∈R n is the driving torque of each driving motor, and d is the disturbance term composed of system modeling error and external interference. c =(τ c1 ,τ c2 ,...,τ cn ) T Represents the control input instructions of the robot arm dynamics control system, is the difference before and after the control input saturation constraint. The actual input τ of the control system is τ c The saturation function form is
[0016]
[0017] In order to facilitate the design of the control system, M(q) is G(q) is abbreviated as M, C, G respectively. Define the lumped uncertainty D = -M -1 d, then the kinetic model (1) can be rewritten as
[0018]
[0019] Furthermore, in step 2, the input of the restricted fruit and vegetable picking robot arm has the following assumptions:
[0020] Assume that the total uncertainty of the robot system D = [D 1 ,D 2 ,...,D i ] T is bounded and first-order differentiable, and its first-order derivative is is bounded, there exists a constant satisfying
[0021] Furthermore, in step 3, a new fixed-time disturbance observer is designed to accurately estimate and compensate for the system lumped uncertainty within a fixed time.
[0022]
[0023] In the formula, λ 1 ,λ 2 ,λ 3 >0,p>1, is the estimate of L; is the estimate of the total uncertainty D of the system; 0 is the auxiliary sliding mode variable.
[0024] right Take the first-order derivative and substitute (3) into the equation to obtain
[0025]
[0026] Combined with a fixed-time disturbance observer, It can be further written as follows
[0027]
[0028] make The second-order nonlinear system
[0029]
[0030] For system (3), if the fixed-time disturbance observer is designed as (4), the estimated error of the observer converges to the origin at a fixed time, and the minimum convergence time T 1 satisfy
[0031] T 1 ≤max{[2ε 1 / 2 / λ 1 +(λ 2 (p-1)ε p-1 ) -1 ][1+(w i / W i -w i h(λ 1 ) / λ 1 ) -1 ]} (8)
[0032] in, h(λ 1 )=λ 1 -1 +(2e / wi λ 1 ) 1 / 3 , ε=(λ 1 / λ 2 ) 1 / (p+1 / 2) .
[0033] By selecting appropriate parameters, At a fixed time T 1 It converges quickly to the origin. When T ≥ T 1 When , the observer can accurately obtain the estimated value of the lumped uncertainty And the estimated time has nothing to do with the initial value of the system disturbance.
[0034] Furthermore, in step 4, in order to solve the input saturation problem and constrain the control input of the manipulator, the fractional power term of the auxiliary state quantity is introduced to construct the following new fixed time convergence auxiliary system:
[0035]
[0036] in, is bounded, and there exists A 0 Make A 0 ≥||ψ||, ξ is the auxiliary state generated by the auxiliary system, A 1 >0, A 2 >0,0<c 1 <1, c 2 >1.
[0037] Select the Lyapunov function V 1 As shown below:
[0038]
[0039] Let the Lyapunov function V 1 Stable, can get auxiliary state in fixed time T 2 Converges to zero, T 2 The form is as follows:
[0040]
[0041] Furthermore, in step 5, according to the dynamic model (3) and the auxiliary system (9), the error e is defined as 1 =qq d -ξ, for e 1 The following tracking error system of the active joint can be obtained by derivation:
[0042]
[0043] Among them, q d is the expected trajectory,
[0044] Define the estimate based on the fixed-time disturbance observer The sliding surface of the state quantity ξ of the auxiliary system with fixed time convergence
[0045]
[0046] Among them, α 1 , α 2 , α 3 >0,γ 1 >1,0<γ 2 <1.
[0047] Combined with the second-order sliding mode control, a new sliding surface is designed, which is as follows
[0048]
[0049] Among them, β 1 , β 2 , β 3 >0,θ 1 >1,0<θ 2 <1.
[0050] Design a fixed-time convergence law for σ
[0051]
[0052] Among them, k 1 , k 2 , k 3 >0,μ 1 >1,0<μ 2 <1, K 1 >0.
[0053] Furthermore, in step 5, based on the fixed-time disturbance observer designed in step 3 and the fixed-time convergence auxiliary system designed in step 4, a Lyapunov function V is constructed. 2 As shown below:
[0054]
[0055] Let the Lyapunov function V 2 The double fixed-time second-order sliding mode control law τ of the input-constrained fruit and vegetable picking robot arm is obtained. c The form is as follows
[0056]
[0057] in,
[0058]
[0059] The system will converge to σ = 0 in a fixed time, and the convergence time is T σmax When the sliding surface σ converges to 0, combined with equation (14), we can get
[0060]
[0061] At this time, the sliding surface s will converge to 0 in a fixed time, and its convergence time is T smax If s = 0, combined with formula (13), we can get
[0062]
[0063] At this time, the tracking error of the system is 1 It will converge to the vicinity of the origin in a fixed time, and the convergence time is T emax .
[0064] Furthermore, in step 5, the tracking error system (12) is considered, and the control input τ is obtained by combining the designed fixed-time disturbance observer and fixed-time convergence auxiliary system. c As shown in (17), the trajectory tracking error of the robot can converge to the vicinity of the origin within a fixed time and remain stable. The convergence time T satisfies
[0065] T≤T 1 +T 2 +T 3 (20)
[0066] Among them, T 1 As shown in formula (8), T 2 As shown in formula (11), T 3 =T σmax +T smax +T emax
[0067] T σmax =k 2 -1 [(μ 1 -1) -1 ln(1+k 2 / k 1 )+(1-μ 2 ) -1 ln(1+k 2 / k 3 )],
[0068] T smax =β 2 -1 [(θ 1 -1) -1 ln(1+β 2 / β1 )+(1-θ 2 ) -1 ln(1+β 2 / β 3 )],
[0069] T emax =α 2 -1 [(γ 1 -1) -1 ln(1+α 2 / α 1 )+(1-γ 2 ) -1 ln(1+α 2 / α 3 )].
[0070] Compared with the prior art, the present invention has the following beneficial effects:
[0071] (1) The present invention does not need to use a disturbance observer to simultaneously estimate and compensate for uncertainties such as external disturbances and saturation errors of the system, thereby reducing the computational burden of the observer. A fixed-time disturbance observer is designed to accurately estimate and compensate for lumped uncertainties such as modeling errors and external disturbances of the robotic arm system within a fixed time.
[0072] (2) The present invention constructs a new type of fixed-time convergence auxiliary system by introducing fractional power terms containing auxiliary state quantities to solve the input saturation problem of the robotic arm system.
[0073] (3) The present invention defines a sliding mode surface based on the fixed-time disturbance observer estimation value and the fixed-time convergence auxiliary system state quantity to simultaneously achieve the fixed-time convergence of the observation error and tracking error of the input-constrained fruit and vegetable picking robot control system and weaken the sliding mode control chattering. The fixed-time second-order sliding mode control method strategy is extended to a high-order picking robot control system with multiple inputs and multiple outputs.
[0074] (4) The invention theoretically proves the global fixed time convergence of the closed-loop control system, and the upper bound of the convergence time is independent of the initial state of the system, and the upper bound of the minimum convergence time of the system error is calculated. In addition, the effectiveness of the proposed control method is further verified through simulation and agricultural harvesting robot arm prototype experiments. BRIEF DESCRIPTION OF THE DRAWINGS
[0075] The accompanying drawings are used to provide further understanding of the present invention and constitute a part of the specification. They are used to explain the present invention together with the embodiments of the present invention and do not constitute a limitation of the present invention.
[0076] Figure 1 This is a flow chart of the double fixed-time second-order sliding mode control method for the input-constrained fruit and vegetable picking robot arm in the present invention.
[0077] Figure 2 This is a schematic diagram of the hardware experimental platform of the agricultural harvesting robot arm prototype system in the present invention.
[0078] Figure 3 This is an estimation curve diagram of the fixed-time disturbance observer for the lumped uncertain disturbance term in the present invention.
[0079] Figure 4 This is a trajectory tracking curve diagram of the robot arm joint 1 in a small initial state provided by an embodiment of the present invention.
[0080] Figure 5 A trajectory tracking error curve diagram of the robot arm joint 1 in a relatively small initial state provided by an embodiment of the present invention.
[0081] Figure 6 This is a trajectory tracking curve diagram of the robot arm joint 1 in a larger initial state provided by an embodiment of the present invention.
[0082] Figure 7 A trajectory tracking error curve diagram of the robot arm joint 1 in a relatively large initial state provided by an embodiment of the present invention.
[0083] Figure 8 This is a control input torque curve diagram of the input-constrained robotic arm joint 1 provided in an embodiment of the present invention.
[0084] Fig. 9 This is a curve chart of the experimental results of the joint 1 trajectory tracking error of the agricultural harvesting robot arm prototype provided in an embodiment of the present invention. DETAILED DESCRIPTION
[0085] 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 and embodiments. Of course, the specific embodiments described here are only used to explain the present invention and are not used to limit the present invention.
[0086] Example 1
[0087] See also Figures 1 to 9 Taking the agricultural harvesting robot arm as an example, the traditional control method is simulated and studied. In order to show the simulation results more clearly, this embodiment selects the position of the joint 1 of the fruit and vegetable harvesting robot arm to show the tracking performance of the traditional controller. Figure 2 The agricultural harvesting robot arm prototype shown in the figure is the research object. The following four traditional control methods are used to design and test the control system performance under the condition of input saturation. d =cos2t-sint, the model error is ΔM=[0.5,0.5,0.5,0.5,0.5,0.5] TWhen considering input limitation, the driving force range of the robot arm joint 1 is limited to [-5,5] (N·m).
[0088] In order to verify the superiority of the method of the present invention over the traditional control method in high-precision trajectory tracking, the traditional finite-time second-order sliding mode control method FT-SOSMC with an observer is recorded as controller 1. This method is based on the finite-time sliding mode control theory and designs a second-order sliding mode controller with an observer. Figure 4 and Figure 5 The tracking curve and trajectory tracking error curve of the traditional method are shown respectively when the initial angle of joint 1 is 0.25rad. Figure 6 and Figure 7 They are respectively the trajectory tracking curve and trajectory tracking error curve of the traditional method when the initial angle of joint 1 is 1.5 rad. From the analysis in the figure, it can be seen that the traditional method can achieve a good tracking effect on the reference trajectory, and the trajectory tracking error can converge within a limited time.
[0089] In order to verify that the present invention can effectively weaken the sliding mode control chattering and effectively solve the input saturation problem, the traditional second-order sliding mode control method without the introduction of a fixed-time convergence auxiliary system is recorded as controller 2, the fixed-time second-order sliding mode control method Fx-SOSMC without an observer is recorded as controller 3, and the finite-time first-order sliding mode control method FTSMC without an observer is recorded as controller 4. Figure 8 The torque curves of the control input of joint 1 under input saturation of each traditional control method are shown. Analysis shows that controller 2 obviously violates the torque constraint of [-5,5] (N·m). In addition, since controller 4 method uses the traditional first-order sliding mode control method, it has obvious sliding mode control chattering. Controller 3 method uses the second-order sliding mode control method, which effectively weakens the sliding mode control chattering. However, since it does not introduce a fixed-time disturbance observer to further reduce the burden of sliding mode control to overcome the uncertainty of the robot system, the control accuracy and ability to suppress sliding mode control of this method need to be improved under input constraints.
[0090] The simulation results show that the above traditional control methods have certain limitations when dealing with the robot arm trajectory tracking problem in complex agricultural environments, especially when dealing with input saturation and external interference, the control effect and system robustness of the traditional methods still need to be improved. These comparison results provide an important basic comparison for the effectiveness verification of the method of the present invention in subsequent embodiments.
[0091] Example 2
[0092] See also Figures 1 to 9, the technical solution provided in this embodiment is a dual fixed-time second-order sliding mode control method for an input-constrained fruit and vegetable picking robot arm, which performs kinematic inverse analysis on a fruit and vegetable picking robot arm for picking tasks, obtains the kinematic forward solution and Jacobian matrix of the fruit and vegetable picking robot arm, and defines the expected motion trajectory of the robot arm end effector. Next, a dynamic model of an input-constrained fruit and vegetable picking robot arm is established, which takes into account uncertainties such as modeling errors and interference from complex agricultural environments. Secondly, a fixed-time disturbance observer is designed to accurately estimate and compensate for the lumped uncertainties such as the system's modeling errors and external interference within a fixed time. A new type of fixed-time convergence auxiliary system is constructed by introducing fractional power terms containing auxiliary state quantities to compensate for input saturation. Combining the disturbance observer and the auxiliary system, a sliding surface based on the estimated value of the fixed-time disturbance observer and the state quantity of the fixed-time convergence auxiliary system is defined, and a dual fixed-time second-order sliding mode control method for an input-constrained fruit and vegetable picking robot arm is designed to simultaneously achieve fixed-time convergence of the observation error and tracking error of the input-constrained fruit and vegetable picking robot arm control system, enhance its robustness, and weaken the chattering of the sliding mode control. Finally, taking the agricultural picking robot arm as an example, simulation and prototype experimental research are carried out to verify that the designed control method can achieve real-time and accurate tracking of the target picking point trajectory. The present invention can effectively enhance the robustness of the robot arm control system for fruit and vegetable picking tasks, so as to achieve high-precision anti-saturation control of the input-constrained fruit and vegetable picking robot arm system.
[0093] The method flow of this embodiment is as follows Figure 1 As shown, the specific method is as follows:
[0094] 1. For the fruit and vegetable picking robot arm for picking tasks, considering uncertain factors such as modeling error and interference from complex agricultural environment, the dynamic model of the input-constrained fruit and vegetable picking robot arm in the joint space is expressed as:
[0095]
[0096] In the formula, are the position, velocity and acceleration vector of each active joint of the robot arm, M(q)∈R n×n is the inertia matrix, is the Coriolis force and centrifugal force, G(q)∈R n is the gravity term, τ∈R n is the driving torque of each driving motor, and d is the disturbance term composed of system modeling error and external interference. c =(τ c1 ,τ c2 ,...,τ cn ) T Represents the control input instructions of the robot arm dynamics control system, The difference before and after the control input saturation constraint.
[0097] In this embodiment, considering the inherent physical constraints of the robot arm actuator, the actual input τ of each joint control system is τ c The saturation function form is
[0098]
[0099] Define the lumped uncertainty D = -M -1 d, the kinetic model (1) can be rewritten as
[0100]
[0101] In this embodiment, it is assumed that the total uncertainty of the robotic arm system D = [D 1 ,D 2 ,...,D i ] T is bounded and first-order differentiable, and its first-order derivative is is bounded, there exists a constant satisfying
[0102] 2. Design a new fixed-time disturbance observer to accurately estimate and compensate for the system lumped uncertainty within a fixed time.
[0103]
[0104] In the formula, λ 1 ,λ 2 ,λ 3 >0,p>1, is the estimate of L; is the estimate of the total uncertainty D of the system; 0 is the auxiliary sliding mode variable.
[0105] For system (3), if the fixed-time disturbance observer is designed as (4), the estimated error of the observer converges to the origin at a fixed time, and the minimum convergence time T 1 satisfy
[0106] T 1 ≤max{[2ε 1 / 2 / λ 1 +(λ 2 (p-1)ε p-1 ) -1 ][1+(w i / W i -w i h(λ 1 ) / λ 1 ) -1 ]} (25)
[0107] in, h(λ 1 )=λ 1 -1 +(2e / w i λ 1 ) 1 / 3 , ε=(λ 1 / λ 2 ) 1 / (p+1 / 2) .
[0108] In this embodiment, the observer parameter λ is selected 1 =30,λ 2 =40,λ 3 =20, p=1.5.
[0109] 3. To solve the input saturation problem and constrain the control input of the robot, the fractional power term of the auxiliary state quantity is introduced to construct the following new fixed time convergence auxiliary system:
[0110]
[0111] in, is bounded, and there exists A 0 Make A 0 ≥||ψ||, ξ is the auxiliary state generated by the auxiliary system, A 1 >0, A 2 >0,0<c 1 <1, c 2 >1.
[0112] If the fixed time convergence auxiliary system is designed as formula (6), the auxiliary state is fixed at time T 2 Converges to zero
[0113]
[0114] In this embodiment, the auxiliary system parameters are selected as A 1 =0.5, A 2 =1.5, c 1 =0.55, c 1 =1.28.
[0115] 4. A sliding mode surface based on the fixed-time disturbance observer estimate and the fixed-time convergence auxiliary system state quantity is defined, and a dual fixed-time second-order sliding mode control method is proposed to simultaneously achieve the fixed-time convergence of the observation error and tracking error of the input-constrained fruit and vegetable picking robot control system, enhance its robustness, and weaken the sliding mode control jitter.
[0116] According to the dynamic model (3) and the auxiliary system (6), the error e is defined as 1 =qq d -ξ, for e 1The following tracking error system of the active joint can be obtained by derivation:
[0117]
[0118] Among them, q d is the expected trajectory,
[0119] In this embodiment, the fixed-time disturbance observer estimate is defined The sliding surface of the state quantity ξ of the auxiliary system with fixed time convergence
[0120]
[0121] Among them, α 1 , α 2 , α 3 >0,γ 1 >1,0<γ 2 <1.
[0122] Combined with the second-order sliding mode control, a new sliding surface is designed, which is as follows
[0123]
[0124] Among them, β 1 , β 2 , β 3 >0,θ 1 >1,0<θ 2 <1.
[0125] Design a fixed-time convergence law for σ
[0126]
[0127] Among them, k 1 , k 2 , k 3 >0,μ 1 >1,0<μ 2 <1, K 1 >0;
[0128] According to the equivalent control design method, the control law τ can be obtained: c The expression
[0129]
[0130] in,
[0131]
[0132] In this embodiment, the selected controller parameters are α 1 =3,α2 =3,α 3 =3,γ 1 =1.5,γ 2 =0.85, β 1 =4.2, β 2 =4.5,β 3 =4.2,θ 1 =1.5,θ 2 =0.45, k 1 =10, k 2 =5, k 3 =10,μ 1 =1.5,μ 2 =0.45, K 1 =1. The first-order sliding surface selected is
[0133] Considering the tracking error system (8), the control input τ is obtained by combining the designed fixed-time disturbance observer and fixed-time convergence auxiliary system c As shown in (12), the trajectory tracking error of the robot can converge to the vicinity of the origin within a fixed time and remain stable. The convergence time T satisfies
[0134] T≤T 1 +T 2 +T 3 (33)
[0135] Among them, T 1 As shown in formula (5), T 2 As shown in formula (7), T 3 =T σmax +T smax +T emax
[0136] T σmax =k 2 -1 [(μ 1 -1) -1 ln(1+k 2 / k 1 )+(1-μ 2 ) -1 ln(1+k 2 / k 3 )],
[0137] T smax =β 2 -1 [(θ 1 -1) -1 ln(1+β 2 / β 1 )+(1-θ 2 )-1 ln(1+β 2 / β 3 )],
[0138] T emax =α 2 -1 [(γ 1 -1) -1 ln(1+α 2 / α 1 )+(1-γ 2 ) -1 ln(1+α 2 / α 3 )].
[0139] 5. Taking the agricultural harvesting robot arm as an example, simulation and prototype experimental research are carried out to verify that the designed control method can achieve real-time and accurate tracking of the target picking point trajectory.
[0140] In order to verify the effectiveness of the dual fixed-time second-order sliding mode control method for the input-constrained fruit and vegetable picking robot arm proposed in this invention, Figure 2 The agricultural harvesting robot arm prototype shown is the controlled object. The method of the present invention is simulated and compared with the four traditional methods in Example 1 under the condition of input saturation. The model error ΔM is uniformly set to [0.5, 0.5, 0.5, 0.5, 0.5] T , τ d =cos2t-sint. When considering input limitation, the driving force range of joint 1 is limited to [-5,5] (N·m).
[0141] Figure 3 The estimated curve of the fixed-time disturbance observer designed by the present invention for the lumped uncertainty is shown. It can be seen from the analysis that, under different initial values of the disturbance, the invented observer can achieve accurate estimation of the lumped uncertainty within 0.17s, and the convergence time is independent of the initial state of the disturbance, which effectively avoids the error caused by the observation time T of the observer. 1 The problem of overestimation of system convergence time due to too long a time.
[0142] Figure 4 and Figure 5 They are the tracking curve and trajectory tracking error curve of the system when the initial angle of joint 1 is 0.25rad. Figure 6 and Figure 7 They are respectively the trajectory tracking curve and trajectory tracking error curve of the system when the initial angle of joint 1 is 1.5rad. It can be seen from the analysis that the method proposed in the present invention can achieve high-precision control, and the trajectory tracking error convergence time does not change significantly with the change of the initial state of the system, which is much shorter than the controller 1 method which can only achieve finite time convergence.
[0143] Figure 8 The torque curve of the control input of joint 1 under input saturation is shown in the analysis. The method proposed in the present invention can limit the driving force of the joint to [-5,5] (N·m) due to the introduction of a fixed-time convergence auxiliary system. However, controller 2 obviously violates the constraint condition. In addition, compared with controller 4, the switching term of this method acts on the derivative of the system control input, making the input signal of the system continuous, thereby better suppressing the sliding mode control chattering. And compared with controller 3, a fixed-time disturbance observer is introduced to further weaken the chattering. Using an agricultural harvesting robot arm prototype, motion control experiments were carried out on the traditional controller 1 and the method proposed in the present invention. Fig. 9 This is the experimental result curve of the joint 1 trajectory tracking error of the agricultural harvesting robot prototype. The experimental results show that the proposed control method has better tracking accuracy when dealing with uncertain and input saturated robot arms than the traditional finite-time second-order sliding mode control method.
[0144] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims
1. A dual fixed-time second-order sliding mode control method for an input-constrained fruit and vegetable picking robot arm, characterized in that: The steps include: Step 1: Perform inverse kinematic analysis on the fruit and vegetable picking robot arm for picking tasks, obtain the kinematic forward solution and Jacobian matrix of the fruit and vegetable picking robot arm, and define the expected motion trajectory of the end effector of the robot arm; Step 2, establish a dynamic model of the input-constrained fruit and vegetable picking robot arm considering modeling errors and the uncertainty of interference in complex agricultural environments; Step 3: Design a fixed-time disturbance observer for the dynamic model in step 2 to accurately estimate and compensate for the system modeling error and the lumped uncertainty term D of external disturbance within a fixed time; Step 4: for the dynamic model in step 2, a new type of fixed-time convergence auxiliary system is constructed by introducing a fractional power term containing an auxiliary state quantity ξ to compensate for input saturation; Step 5: Combine the disturbance observer designed in step 3 and the auxiliary system designed in step 4 to define the fixed-time disturbance observer estimate The sliding mode surface of the state quantity ξ of the auxiliary system is converged with the fixed time, and a double fixed-time second-order sliding mode control method for the input-constrained fruit and vegetable picking robot arm is designed to achieve real-time and accurate tracking of the trajectory of the target picking point; Step 6: Conduct simulation and prototype experiments on the agricultural harvesting robot arm to verify the correctness and effectiveness of the designed control method.
2. The dual fixed-time second-order sliding mode control method for input-constrained fruit and vegetable picking mechanical arm according to claim 1 is characterized in that: In step 2, for the fruit and vegetable picking robot arm for picking tasks, considering the modeling error and the uncertainty of the complex agricultural environment interference, the dynamic model of the input-constrained fruit and vegetable picking robot arm in the joint space is expressed as: In the formula, are the position, velocity and acceleration vector of each active joint of the robot arm, M(q)∈R n×n is the inertia matrix, is the Coriolis force and centrifugal force, G(q)∈R n is the gravity term, τ∈R n is the driving torque of each driving motor, d is the disturbance term composed of system modeling error and external interference, τ c =(τ c1 ,τ c2 ,...,τ cn ) T Represents the control input instructions of the robot arm dynamics control system, is the difference before and after the control input saturation constraint. The actual input τ of the control system is τ c The saturation function form is M(q), G(q) is abbreviated as M, C, G respectively, and the lumped uncertainty term D = -M -1 d, then the kinetic model (1) can be rewritten as Assume that the total uncertainty of the robot system D = [D1, D2, ..., D i ] T is bounded and first-order differentiable, and its first-order derivative is is bounded, there exists a constant satisfying (i=1,...,6).
3. The dual fixed-time second-order sliding mode control method for input-constrained fruit and vegetable picking mechanical arm according to claim 2 is characterized by: In step 3, a new fixed-time disturbance observer is designed to accurately estimate and compensate the system lumped uncertainty within a fixed time: In the formula, λ1, λ2, λ3>0, p>1, is the estimate of L; is the estimate of the system lumped uncertainty D; s0 is the auxiliary sliding mode variable; For system (3), if the fixed-time disturbance observer is designed as formula (4), the estimated error of the observer converges to the origin at a fixed time, and the minimum convergence time T1 satisfies: T1≤max{[2ε 1 / 2 / λ1+(λ2(p-1)ε p-1 ) -1 ][1+(w i / W i -w i h(λ1) / λ1) -1 ]} (5) Among them, λ1h -1 (λ1)>W i ; h(λ1)=λ1 -1 +(2e / w i λ1) 1 / 3 ,(i=1,...,6),ε=(λ1 / λ2) 1 / (p+1 / 2) 。 4. The dual fixed-time second-order sliding mode control method for input-constrained fruit and vegetable picking mechanical arm according to claim 2 is characterized in that: In step 4, in order to solve the input saturation problem and constrain the control input of the manipulator, the fractional power term of the auxiliary state quantity is introduced to construct the following new fixed time convergence auxiliary system: in, is bounded, and there exists A0 such that A0 ≥ ||ψ||, ξ is the auxiliary state generated by the auxiliary system, A1>0, A2>0, 0<c1<1, c2>1; If the fixed time convergence auxiliary system is designed as formula (6), the auxiliary state converges to zero within the fixed time T2 5. The dual fixed-time second-order sliding mode control method for input-constrained fruit and vegetable picking mechanical arm according to claim 3 or 4, characterized in that: In step 5, according to the dynamic model (3) and the auxiliary system (6), the error e1=qq is defined d -ξ, and the derivative of e1 gives the following tracking error system of the active joint: Among them, q d is the expected trajectory, Define the estimate based on the fixed-time disturbance observer The sliding surface of the state quantity ξ of the auxiliary system with fixed time convergence Among them, α1, α2, α3>0, γ1>1, 0<γ2<1; Combined with the second-order sliding mode control, a new sliding surface is designed, which is as follows Among them, β1, β2, β3>0, θ1>1, 0<θ2<1; Design a fixed-time convergence law for σ Among them, k1, k2, k3>0, μ1>1, 0<μ2<1, K1>0; According to the equivalent control design method, the control law τ can be obtained: c The expression in, Considering the tracking error system (8), the control input τ is obtained by combining the designed fixed-time disturbance observer and fixed-time convergence auxiliary system c , then the trajectory tracking error of the robot converges to the vicinity of the origin within a fixed time and remains stable, and the convergence time T satisfies T≤T1+T2+T3 (13) Where, T1 is as shown in formula (5), T2 is as shown in formula (7), T3 = T σmax +T smax +T emax T σmax =k2 -1 [(μ1-1) -1 ln(1+k2 / k1)+(1-μ2) -1 ln(1+k2 / k3)], T smax =β2 -1 [(θ1-1) -1 ln(1+β2 / β1)+(1-θ2) -1 ln(1+β2 / β3)], T emax =α2 -1 [(c1-1) -1 ln(1+α2 / α1)+(1-γ2) -1 ln(1+α2 / α3)].
Citation Information
Patent Citations
Mechanical arm input saturation fixed time trajectory tracking control method and system
CN111496792A
Flexible joint space robot rapid impedance control method based on contact torque compensation
CN115972195A
Global fixed time type backstepping second-order sliding mode control method for industrial hybrid robot
CN118131623A
Double-degree-of-freedom mechanical arm trajectory tracking method constrained by input delay state
CN118204971A
Fast compensation terminal sliding mode control method considering control input saturation
CN118426315A
