Track tracking control method of life support robot systemTechnical Field
The invention relates to the technical field of robot system track tracking control, in particular to a track tracking control method of a life support robot system.
Background
Robots are the product of rapid development in the industrial age, and have been developed rapidly so far, and are active in various industries in different forms. The life support robot is a robot which is proposed in response to the social reality of the aging of the global population of the current generation and provides assistance for the daily life and work of people. Compared with industrial robots, such as medical robots and floor sweeping robots, life support robots are closer to our lives and appear at our sides, and the specificity determines that the working environment facing the robot is more complex, so that the robot is required to accurately acquire relevant working information.
The control system of the life support robot is a typical multivariable, nonlinear, unstable dynamic system. In practice, in order to successfully operate the robot to a desired position, the robot is required to perform operations such as forward, steering or backward multiple times, and finally to reach the desired position, and control thereof is a difficult task. In addition, it is very important to ensure stability of the control system, and although methods based on various classical linear control theory and advanced control theory have been proposed at present, general linear control theory is not applicable any more due to internal multi-state characteristics of the life support robot system, and problems of uncertainty of model parameters and disturbance in the operation environment in the system.
Disclosure of Invention
In view of the above, the present invention aims to provide a trajectory tracking control method for a life support robot system, which can effectively solve the problems of internal multi-state characteristics of the life support robot system, uncertainty of model parameters in the system, disturbance in an operating environment, etc., obtain ideal dynamic characteristics, and have certain robustness to system parameters and external disturbance signal changes, so that the system has good stability.
The invention solves the technical problems by the following technical means:
A track following control method of a life support robot system includes the following steps:
S1, establishing a dynamic mathematical model, a system model and a state space model of a life support robot system;
s2, designing a disturbance observer of the life support robot system based on a dynamic mathematical model;
S3, designing a nonsingular rapid terminal sliding mode surface based on a sliding mode theory, and constructing a sliding mode controller of the life support robot system, wherein the sliding mode controller comprises a tracking error system for designing the life support robot system, a nonsingular rapid terminal sliding mode surface, a sliding mode approaching law and a sliding mode control law;
s4, transmitting a control signal instruction to a system executor by the sliding mode control law to realize track tracking.
The sliding mode control of the method of the present invention is a nonlinear control method that alters the dynamics of the system by using a discontinuous control signal and forces the system to slide along a prescribed switching pattern. The advantages of the invention compared to other control methods are its simplicity, high robustness to external disturbances and low sensitivity to system parameter variations. The disturbance observer groups all unknown torques or forces acting on the inside and outside of the system together into a disturbance term. This disturbance term is estimated using a disturbance observer whose output can be used to feed forward compensate for the disturbance. Due to the feedforward nature of such compensation, the disturbance observer can provide fast, excellent tracking performance and smooth control input without the need to use a large feedback gain.
Further, the method for establishing the dynamic mathematical model of the life support robot system in the step S1 is as follows:
Wherein t is a time variable, M is a mass of the robot body, M is a mass of the load, fi (t), i=1, 2,3 is a control input of the robot system, L is a length from a center of gravity of the robot to a wheel, M is a unit of thetai (t), i=1, 2,3 is a direction of the center of gravity of the robot and the wheel, I0 is a rotational inertia of the robot system, x (t), y (t), and theta (t) are an abscissa, an ordinate, and a direction angle of the robot, respectively.
Further, in the step S1, the method for establishing the life support robot system model includes:
Where M0 is the inertial 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 variable of the system, Di (t), i=1, 2,3 are the disturbance values in X, y and direction angles, respectively.
Further, the method for obtaining the state space model of the life support robot system in the step S1 is as follows:
further, in the step S2, the method for designing the disturbance observer of the life support robot system includes:
wherein,z(t)=[z1(t) z2(t) z3(t)]T,Is the estimated disturbance quantity, 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,Respectively, is an estimate of d1(t),d2(t),d3 (t).
Further, in the step S3, the method for designing the tracking error system of the life support robot system includes:
e1(t)＝X(t)-Xd(t),
Wherein Xd (t) represents a reference track, namely Xd(t)＝[xdx(t) xdy(t) xdθ(t)]T;e1 (t) is a track tracking error, namely e1(t)＝[e1x(t) e1y(t) e1θ(t)]T;
wherein, the tracking error dynamic equation is:
further, in the step S3, the design method of the nonsingular rapid terminal sliding mode surface S (t) of the life support robot system is as follows:
Wherein e1 (t) is a track tracking error, e2 (t) is a first derivative of a track tracking error e1 (t), 1< gamma2＝(p/q)＜2,γ1＞γ2, p, q are positive odd numbers, and alpha and beta are designed sliding mode surface s (t) parameters and satisfy alpha >0 and beta >0.
Further, in the step S3, the design method of the sliding mode approach law of the life support robot system is as follows:
wherein epsilon, k are two parameters of sliding mode approach rate, and epsilon >0 and k >0 are satisfied.
Further, in the step S3, the method for constructing the sliding mode control law of the life support robot system includes:
The invention has the beneficial effects that:
the intelligent control method based on sliding mode control effectively achieves the anti-interference characteristic of the life support robot system, solves the problems of uncertainty of model parameters and the like in the system, can obtain ideal dynamic characteristics, has certain robustness to system parameters and external interference, enables the system to have good stability, and further, due to the introduction of a disturbance observer, effectively compensates external disturbance of the life support robot system, suppresses uncertainty in the system and improves track tracking performance.
Drawings
Fig. 1 is a sliding mode control block diagram of a trajectory tracking control method of a life support robot system of the present invention.
Fig. 2 is a diagram illustrating a trajectory tracking of a life support robot according to an embodiment of the present invention.
Fig. 3 is a system control input diagram of an embodiment of the present invention.
Fig. 4 is a tracking error diagram of an embodiment of the present invention.
FIG. 5 is a plot of disturbance estimation at the x-position for an embodiment of the present invention.
FIG. 6 is a plot of disturbance estimation at the y-position for an embodiment of the present invention.
FIG. 7 is a plot of disturbance estimates in directional angles for an embodiment of the present invention.
Fig. 8 is a variation of the slide surface of an embodiment of the present invention.
Detailed Description
The invention will be described in detail below with reference to the attached drawings:
As shown in fig. 1 to 8, the trajectory tracking control method of the life support robot system of the present invention comprises the steps of:
S1, establishing a dynamic mathematical model of the life support robot system:
wherein t is a time variable, M is a mass of a robot body, M is a mass of a load, fi (t), i=1, 2,3 is a control input of the robot system, N is a length from a center of gravity of the robot to a wheel, M is a unit of thetai (t), i=1, 2,3 is a direction of the center of gravity of the robot and the wheel, rad is a unit of rotational inertia of the robot system, x (t), y (t), and theta (t) are an abscissa, an ordinate, and a direction angle of the robot, respectively;
system model of life support robot:
wherein M0 is the inertial 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 quantity of the system;
so far, a state space model of the life support robot system is obtained;
s2, designing a disturbance observer of the life support robot system based on a mathematical model:
Wherein the method comprises the steps ofz(t)=[z1(t) z2(t) z3(t)]T,Is the estimated disturbance quantity, 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,Respectively, is an estimate of d1(t),d2(t),d3 (t).
S3, designing a nonsingular rapid terminal sliding mode surface based on a sliding mode theory, and constructing a sliding mode controller of the life support robot system;
s301, a tracking error system of the life support robot system is as follows:
e1(t)＝X(t)-Xd(t),
Wherein Xd (t) represents a reference track, namely Xd(t)＝[xdx(t) xdy(t) xdθ(t)]T;e1 (t) is a track tracking error, namely e1(t)＝[e1x(t) e1y(t) e1θ(t)]T;
The tracking error dynamic equation is
S302, designing a nonsingular rapid terminal sliding die surface S (t):
wherein e1 (t) is a track tracking error, e2 (t) is a first derivative of the track tracking error, 1< gamma2＝(p/q)＜2,γ1＞γ2, p, q are positive odd numbers, alpha and beta are designed sliding mode surface s (t) parameters, and alpha >0, beta >0 are satisfied;
s303, designing a sliding mode approach law:
Wherein epsilon, k are two parameters of sliding mode approach rate, and epsilon >0 and k >0 are satisfied;
s304, constructing a sliding mode control law:
s4, the sliding mode control law F (t) transmits a control signal instruction to a system executor to realize track tracking.
Next, simulation was performed by using Matlab to verify the effectiveness of the sliding mode control method in this example for the control of the life support robot system.
A life support robotic system wherein system parameters are set as follows:
M=80kg,m=70kg,L=0.4m,I0＝2.8kgm2,r0=0.2m。
The initial state of the system is x0＝[100.25π]T. Let the reference trajectory be xd＝cos(0.1t)＝0.2m,yd =sin (0.1 t),The controller parameters are α=1, γ1 =2, β=1,
The observer parameter is L (X) =diag {4,4,10}. The disturbance term of the system is set to d1(t)＝0.2 sin(0.2t),d2(t)＝0.1 cos(0.2t),d3 (t) =0.1 sin (0.2 t).
Finally, simulation results are shown in fig. 2-8, and as can be seen from the simulation diagrams, the controller designed by the embodiment can ensure the stability of the life support robot system, effectively compensates external disturbance of the life support robot system due to the introduction of the disturbance observer, suppresses uncertainty in the system, and improves the track tracking performance.
It can be seen from fig. 1 that the actual track of the system is compared with the set reference track, and when a track error exists, a sliding mode controller is designed to control the actual track, so as to realize track tracking control.
2-8, It can be seen that the sliding mode controller designed in the embodiment can enable the system state of the life support robot to track to a desired state after being disturbed by the outside, so as to realize track tracking control.
The above embodiments are only for illustrating the technical solution of the present invention and not for limiting the same, and although the present invention has been described in detail with reference to the preferred embodiments, it should be understood by those skilled in the art that modifications and equivalents may be made thereto without departing from the spirit and scope of the technical solution of the present invention, which is intended to be covered by the scope of the claims of the present invention. The technology, shape, and construction parts of the present invention, which are not described in detail, are known in the art.