A trajectory tracking control method for a life support robot system

Abstract

The present invention relates to the technical field of robot system trajectory tracking control, and discloses a trajectory tracking control method for a life support robot system, the steps of which include establishing a dynamic mathematical model, a system model and a state space model of the life support robot system; designing a disturbance observer of the system; designing a non-singular fast terminal sliding surface, and constructing a sliding mode controller of the system, including designing a tracking error system of the life support robot system, designing a non-singular fast terminal sliding surface, designing a sliding mode approaching law and constructing a sliding mode control law; the sliding mode control law transmits a control signal instruction to a system actuator to realize trajectory tracking. The present invention can solve the internal multi-state characteristics of the life support robot system, the uncertainty of model parameters in the system and the disturbance in the operating environment, and obtain relatively ideal dynamic characteristics, and has a certain robustness to changes in system parameters and external interference signals, so that the system has good stability.

Description

Technical Field

[0001] The present invention relates to the technical field of robot system trajectory tracking control, and in particular to a trajectory tracking control method for a life support robot system. Background Art

[0002] Robots are the product of the rapid development of the industrial age. Today, they have achieved rapid development and are active in various industries in different forms. The so-called life support robots are robots that respond to the social reality of the aging population in the world today and provide assistance for people's daily life and work. Compared with industrial robots, life support robots such as medical robots and sweeping robots are closer to our lives and appear around us. This particularity determines that the working environment they face will be more complex. Therefore, robots need to be able to accurately obtain relevant work information.

[0003] The control system of a life support robot is a typical multivariable, nonlinear, unstable dynamic system. In actual situations, in order to successfully move the robot to the desired position, the robot needs to perform operations such as forward, turning, or backward multiple times to finally reach the ideal position. Its control is a relatively difficult task. In addition, it is also very important to ensure the stability of the control system. Although various methods based on classical linear control theory and advanced control theory have been proposed, due to the internal multi-state characteristics of the life support robot system, the uncertainty of model parameters in the system, and the disturbance in the operating environment, the general linear control theory is no longer applicable. Summary of the invention

[0004] In view of this, the purpose of the present invention is to provide a trajectory tracking control method for a life support robot system, which can effectively solve the internal multi-state characteristics of the life support robot system, as well as the uncertainty of model parameters in the system and disturbances in the operating environment, so as to obtain more ideal dynamic characteristics, have a certain robustness to changes in system parameters and external interference signals, and make the system have good stability.

[0005] The present invention solves the above technical problems by the following technical means:

[0006] A trajectory tracking control method for a life support robot system comprises the following steps:

[0007] S1. Establish the dynamic mathematical model, system model and state space model of the life support robot system;

[0008] S2. Design a disturbance observer for the life support robot system based on a dynamic mathematical model;

[0009] S3. Based on the sliding mode theory, design a non-singular fast terminal sliding mode surface and construct a sliding mode controller for the life support robot system, including designing the tracking error system of the life support robot system, designing a non-singular fast terminal sliding mode surface, designing a sliding mode reaching law and constructing a sliding mode control law;

[0010] S4, the sliding mode control law transmits the control signal command to the system actuator to achieve trajectory tracking.

[0011] The sliding mode control of the method of the present invention is a nonlinear control method that changes the dynamics of the system by using a discontinuous control signal and forces the system to slide along a specified switching manifold. Compared with other control methods, the advantages of the present invention are its simplicity, high robustness to external disturbances and low sensitivity to changes in system parameters. The disturbance observer combines all unknown torques or forces acting on the system inside and outside into a disturbance term. This disturbance term is estimated using a disturbance observer, and the output of the disturbance observer can be used to feedforward compensate for the disturbance. Due to the feedforward nature of this compensation, the disturbance observer can provide fast, excellent tracking performance and smooth control input without the need to use a large feedback gain.

[0012] Furthermore, the method for establishing the dynamic mathematical model of the life support robot system in step S1 is:

[0013]

[0014] Where t is the time variable; M is the mass of the robot body, in kg; m is the mass of the load, in kg; f i (t), i = 1, 2, 3 is the control input of the robot system, in N; L is the length from the center of gravity of the robot to the wheel, in m; θ i (t), i = 1, 2, 3 are the directions of the robot's center of gravity and wheels, in rad; I0 is the moment of inertia of the robot system; x(t), y(t), θ(t) are the robot's horizontal coordinate, vertical coordinate and direction angle, respectively.

[0015] Furthermore, the method for establishing the life support robot system model in step S1 is:

[0016] Where M0 is the inertia matrix of the system, X(t) = [x(t) y(t) θ(t)] T , F(t)=[f1(t) f2(t) f3(t)] T , X(t) is the state vector of the system, F(t) is the control input of the system; D(t) = [d1(t) d2(t) d3(t)] T, D(t) is the disturbance of the system, d i (t), i=1, 2, 3 are the disturbance values ​​in x, y and direction angle respectively.

[0017] Furthermore, the method for obtaining the state space model of the life support robot system in step S1 is:

[0018]

[0019]

[0020] Furthermore, the design method of the disturbance observer of the life support robot system in step S2 is:

[0021]

[0022]

[0023] in, z(t)=[z1(t) z2(t) z3(t)] T , is the estimated disturbance, z(t) is the state vector of the disturbance observer; is the auxiliary vector of the disturbance observer, L(X) is the gain matrix of the disturbance observer, are the estimated values ​​of d1(t), d2(t), and d3(t) respectively.

[0024] Furthermore, the tracking error system design method of the life support robot system in step S3 is:

[0025] e1(t)=X(t)-X d (t),

[0026]

[0027] Where: X d (t) represents the reference trajectory, that is, X d (t) = [x dx (t) x dy (t) x dθ (t)] T ; e1(t) is the trajectory tracking error, that is, e1(t) = [e 1x (t) e 1y (t) e 1θ (t)] T ;

[0028] Among them, the tracking error dynamic equation is:

[0029]

[0030]

[0031] Furthermore, the design method of the non-singular fast terminal sliding surface s(t) of the life support robot system in step S3 is:

[0032]

[0033] Wherein: e1(t) is the trajectory tracking error, e2(t) is the first-order derivative of the trajectory tracking error e1(t); 1<γ2=(p / q)<2,γ1>γ2,p,q are positive odd numbers; α,β are the designed sliding surface s(t) parameters, and satisfy α>0,β>0.

[0034] Furthermore, the design method of the sliding mode reaching law of the life support robot system in step S3 is:

[0035]

[0036] Among them: ε, k are two parameters of the sliding mode reaching rate, satisfying ε>0, k>0.

[0037] Furthermore, the construction method of the sliding mode control law of the life support robot system in step S3 is:

[0038]

[0039] Beneficial effects of the present invention:

[0040] The present invention utilizes an intelligent control method of sliding mode control to effectively realize the anti-interference characteristics of the life support robot system and solve the problems such as the uncertainty of model parameters in the system, and can obtain relatively ideal dynamic characteristics, and has a certain robustness to system parameters and external interference, so that the system has good stability; in addition, due to the introduction of a disturbance observer, the external disturbance of the life support robot system is effectively compensated, the uncertainty in the system is suppressed, and the trajectory tracking performance is improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 It is a sliding mode control block diagram of the trajectory tracking control method of the life support robot system of the present invention.

[0042] Figure 2 4 is a trajectory tracking diagram of the life support robot according to an embodiment of the present invention.

[0043] Figure 3 It is a system control input diagram of an embodiment of the present invention.

[0044] Figure 4 is a tracking error diagram of an embodiment of the present invention.

[0045] Figure 5 It is a disturbance estimation diagram at the x position of an embodiment of the present invention.

[0046] Figure 6 It is a disturbance estimation diagram at the y position of an embodiment of the present invention.

[0047] Figure 7 It is a disturbance estimation diagram of the embodiment of the present invention in terms of direction angle.

[0048] Figure 8 3 is a diagram showing the change of the sliding surface according to an embodiment of the present invention. DETAILED DESCRIPTION

[0049] The present invention will be described in detail below with reference to the accompanying drawings:

[0050] like Figure 1-8 As shown: The trajectory tracking control method of the life support robot system of the present invention comprises the following steps:

[0051] S1. Establish a dynamic mathematical model of the life support robot system:

[0052]

[0053] Where t is the time variable; M is the mass of the robot body, in kg; m is the mass of the load, in kg; f i (t), i = 1, 2, 3 is the control input of the robot system, in N; L is the length from the center of gravity of the robot to the wheel, in m; θ i (t), i = 1, 2, 3 are the directions of the robot's center of gravity and wheels, in rad; I0 is the moment of inertia of the robot system; x(t), y(t), θ(t) are the robot's horizontal coordinate, vertical coordinate and direction angle respectively;

[0054] System model of life support robot:

[0055]

[0056] Where M0 is the inertia matrix of the system, X(t) = [x(t) y(t) θ(t)] T , F(t)=[f1(t) f2(t) f3(t)] T , X(t) is the state vector of the system, F(t) is the control input of the system; D(t) = [d1(t) d2(t) d3(t)] T , D(t) is the disturbance of the system;

[0057]

[0058]

[0059] At this point, the state space model of the life support robot system is obtained;

[0060] S2. Design a disturbance observer for the life support robot system based on the mathematical model:

[0061]

[0062]

[0063] in z(t)=[z1(t) z2(t) z3(t)] T , is the estimated disturbance, z(t) is the state vector of the disturbance observer; is the auxiliary vector of the disturbance observer, L(X) is the gain matrix of the disturbance observer, are the estimated values ​​of d1(t), d2(t), and d3(t) respectively.

[0064] S3. Based on sliding mode theory, design a non-singular fast terminal sliding surface and construct a sliding mode controller for the life support robot system;

[0065] S301, the tracking error system of the life support robot system is as follows:

[0066] e1(t)=X(t)-X d (t),

[0067]

[0068] Where: X d (t) represents the reference trajectory, that is, X d (t) = [x dx (t) x dy (t) x dθ (t)] T ; e1(t) is the trajectory tracking error, that is, e1(t) = [e 1x (t) e 1y (t) e 1θ (t)] T ;

[0069] The tracking error dynamic equation is:

[0070]

[0071]

[0072] S302. Design of non-singular fast terminal sliding surface s(t):

[0073]

[0074] Where: e1(t) is the trajectory tracking error, e2(t) is the first-order derivative of the trajectory tracking error; 1<γ2=(p / q)<2,γ1>γ2,p,q are positive odd numbers; α,β are the designed sliding surface s(t) parameters, and satisfy α>0,β>0;

[0075] S303, Design sliding mode reaching law:

[0076]

[0077] Where: ε, k are two parameters of the sliding mode approach rate, satisfying ε>0, k>0;

[0078] S304. Construct sliding mode control law:

[0079]

[0080] S4, the sliding mode control law F(t) transmits the control signal command to the system actuator to achieve trajectory tracking.

[0081] The following simulation is performed using Matlab to verify the effectiveness of the sliding mode control method in this example for controlling the life support robot system.

[0082] Life support robot system, where the system parameters are set as follows:

[0083] M=80kg, m=70kg, L=0.4m, I0=2.8kgm 2 , r0=0.2m.

[0084] The initial state of the system is: x0 = [100.25π] T . Assume the reference trajectory is as follows: x d =cos(0.1t)=0.2m,y d = sin(0.1t), The controller parameters are α=1,γ1=2,β=1,

[0085] The observer parameters are L(X) = diag{4,4,10}. The disturbance terms of the system are set to d1(t) = 0.2 sin(0.2t), d2(t) = 0.1 cos(0.2t), and d3(t) = 0.1 sin(0.2t).

[0086] The final simulation results are as follows Figure 2-8As shown, it can be seen from the simulation diagram that the controller designed in this embodiment can ensure the stability of the life support robot system. Due to the introduction of the disturbance observer, the external disturbance of the life support robot system is effectively compensated, the uncertainty in the system is suppressed, and the trajectory tracking performance is improved.

[0087] from Figure 1 It can be seen that the actual trajectory of the system is compared with the set reference trajectory. When there is a trajectory error, a sliding mode controller is designed to control the actual trajectory and realize trajectory tracking control.

[0088] Comprehensive analysis Figure 2-8 It can be seen that the sliding mode controller designed in the embodiment can make the state of the life support robot system track to the expected state after being disturbed by the external environment, and realize trajectory tracking control.

[0089] The above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit the present invention. Although the present invention is described in detail with reference to the preferred embodiments, it should be understood by those skilled in the art that the technical solutions of the present invention may be modified or replaced by equivalents without departing from the purpose and scope of the technical solutions of the present invention, which should be included in the scope of the claims of the present invention. The techniques, shapes, and structural parts not described in detail in the present invention are all known technologies.

Claims

1. A trajectory tracking control method for a life support robot system, characterized in that: The following steps are involved: S1. Establish the dynamic mathematical model, system model and state space model of the life support robot system; S2. Design a disturbance observer for the life support robot system based on a dynamic mathematical model; S3. Based on the sliding mode theory, design a non-singular fast terminal sliding mode surface and construct a sliding mode controller for the life support robot system, including designing the tracking error system of the life support robot system, designing a non-singular fast terminal sliding mode surface, designing a sliding mode reaching law and constructing a sliding mode control law; S4, the sliding mode control law transmits the control signal command to the system actuator to achieve trajectory tracking; The design method of the disturbance observer of the life support robot system in step S2 is: in, z(t)=[z1(t)z2(t)z3(t)] T , is the estimated disturbance, z(t) is the state vector of the disturbance observer; is the auxiliary vector of the disturbance observer, L(X) is the gain matrix of the disturbance observer, are the estimated values ​​of d1(t), d2(t), and d3(t) respectively; The tracking error system design method of the life support robot system in step S3 is: e1(t)=X(t)-X d (t), Where: X d (t) represents the reference trajectory, that is, X d (t) = [x dx (t) x dy (t) x dθ (t)] T ; e1(t) is the trajectory tracking error, that is, e1(t) = [e 1x (t)e 1y (t)e 1θ (t)] T ; e2(t) is the derivative of the trajectory tracking error, that is, e2(t) = [e 2x (t)e 2y (t)e 2θ (t)] T ; Among them, the tracking error dynamic equation is: The design method of the non-singular fast terminal sliding surface s(t) of the life support robot system in step S3 is: Where: e1(t) is the trajectory tracking error, e2(t) is the first-order derivative of the trajectory tracking error e1(t); the following conditions must be met during design: 1<γ2=(p / q)<2, γ1>γ2, p, q are positive odd numbers; α, β are the parameters of the designed sliding surface s(t), and α>0, β>0; The design method of the sliding mode reaching law of the life support robot system in step S3 is: Where: ε, k are two parameters of the sliding mode reaching law, satisfying ε>0, k>0; The construction method of the sliding mode control law of the life support robot system in step S3 is:

2. The trajectory tracking control method of a life support robot system according to claim 1, characterized in that: The method for establishing the dynamic mathematical model of the life support robot system in step S1 is: Where t is the time variable; M is the mass of the robot body, in kg; m is the mass of the load, in kg; f i (t), i = 1, 2, 3 is the control input of the robot system, in N; L is the length from the center of gravity of the robot to the wheel, in m; θ i (t), i = 1, 2, 3 are the directions of the robot's center of gravity and wheels, in rad; I0 is the moment of inertia of the robot system; x(t), y(t), θ(t) are the robot's horizontal coordinate, vertical coordinate and direction angle, respectively.

3. The trajectory tracking control method of a life support robot system according to claim 2, characterized in that: The method for establishing the life support robot system model in step S1 is: Where M0 is the inertia matrix of the system, X(t) = [x(t)y(t)θ(t)] T , F(t)=[f1(t)f2(t)f3(t)] T , X(t) is the state vector of the system, B(t) is the parameter matrix of the controller, and F(t) is the control input of the system; D(t) = [d1(t) d2(t) d3(t)] T , D(t) is the disturbance of the system, d i (t), i=1, 2, 3 are the disturbance values ​​in x, y and direction angle respectively.

4. The trajectory tracking control method of a life support robot system according to claim 3, characterized in that: The method for obtaining the state space model of the life support robot system in step S1 is:

Images