Self-adaptive finite time convergence sliding mode control method of robot
Technical Field
The invention belongs to the technical field of control, and particularly relates to a self-adaptive finite time convergence sliding mode control method of a robot, which is a robust control method suitable for a robot containing uncertainty.
Background
General robot systems include parallel robots, series robots, and hybrid robots. The ultimate goal of robot system control is to enable the generalized coordinate vector q of the robot to reach a steady state or track the command signal qd(ii) a To achieve this, it is necessary to obtain the actual generalized coordinate vector q of the robot system and the actual generalized velocity vector q of the robot systemAnd then calculating the value of the main power or moment required by the driving robot according to a specific control method, and further driving the robot to finish the required operation action.
The robot system belongs to a typical complex multi-input multi-output nonlinear system, and the influence of uncertainty is inevitable due to the simplification of a dynamic modeling process of the robot system, and comprises the following steps: friction, external disturbances, noise, system parameter fluctuations, faults, unmodeled dynamics, and the like. Partial uncertainties of the robot system have been analyzed in the prior art, but analysis considering the effects of faults is relatively poor. The applicant has previously conducted a detailed analysis of the relevant uncertainties of parallel robotic systems (see the relevant documents MengQiang, Zhang Tao, Gao Xiang, et al, "Adaptive sizing mode fault-tolerant control of uncategorized Stewart platform on line Multi-body dynamics", IEEE/ASME Transactions on mechanics, 2014: 19(3): 882-894; QiangMeng, Tao Zhang, just-yan Song, "Adaptive sizing control for injecting of uncategorized Stewart platform on position equations," pivoting).
The above document adopts a Kane method to establish a kinetic equation of a parallel robot system as shown in formula (1.1):
wherein M (q) is a parallel robot system mass n × n dimensional matrix expressed as M (q) ∈ Rn×n(hereinafter, referred to broadly as, e.g.. ∈ R)n×nThe representation matrix belongs to a matrix of n × n dimensions);is a Coriolis/centripetal term coefficient vector of the parallel robot system G (q) ∈ Rn×1Is the gravity term vector of the parallel robot system, tau ∈ Rn×1For main power/moment of parallel robot system, q ∈ Rn×1The generalized coordinate vector is a generalized coordinate vector of the parallel robot system;is a generalized velocity vector of the parallel robot system;representing a generalized acceleration vector of the parallel robot system; n represents the dimension of the parallel robot system and is a positive integer.
(considering that the mechanical system may have friction, external interference, noise, system parameter fluctuation, fault, unmodeled dynamics, etc., and the kinetic equation thereof may not be completely accurate, therefore, equation (1.1) is modified to the parallel robot kinetic equation considering uncertainty as equation (1.2):
wherein M is0∈Rn×n,C0∈Rn×1,G0∈Rn×1Respectively representing calculable items in the mass matrix, Coriolis/centripetal term coefficient vector and gravity term vector of the parallel robot system, and Δ M ∈ Rn×n,ΔC∈Rn×1,ΔG∈Rn×1Respectively representing the uncertainty items (delta M, delta C, delta G) of the mass matrix, the Coriolis/centripetal term coefficient vector and the gravity term vector of the parallel robot system, and generally calculating the uncertainty items M0,C0,G0Within 40%);a∈Rn×nrepresentative of multiplicative failure (a,iIs determined according to the fault degree of the actuator, and the effective value range is [0,1 ]],a,i1 means that the ith actuator is not faulty,a,i0 denotes complete failure of the ith actuator, 0 <a,i< 1 indicates that the ith actuator partially failed);f∈Rn×1for additive failure: (f,iIs determined according to the degree of actuator failure, and the effective range is smaller than the maximum force or moment of the actuator); the uncertainty items delta M, delta C and delta G are generated because all friction, external interference, noise, system parameter fluctuation and the like in an actual physical system can cause uncertainty items to a parallel robot system dynamic equation, and in the modeling analysis process, only main dynamic characteristics of the system are generally considered for simplifying calculation, so that uncertainty items which are dynamically introduced without modeling can exist.aAndfis mainly caused byDue to a failure of a system actuator);
the formula (1.2) is further arranged into a formula (1.3)
Wherein,total∈Rn×1the total uncertainty is as in formula (1.4):
the constraint of robot uncertainty items (relevant documents can be seen: Liu jin beautiful jade, sliding mode variable structure control MATLAB simulation, Qing Hua university Press: 386-
Wherein ρ ∈ Rn×1Is a rigid robot uncertainty; b1、b2、b3Is a positive number (b)1、b2、b3The value of (a) is determined according to a total uncertainty term rho existing in the system; b satisfying constraint equation (1.5) at different times1、b2、b3May vary, so that in practical applications b1、b2、b3Is generally unknown, and can estimate an approximate range of values at most); the norm operation is taken as | left | |; (*)2Squaring;
for an n-dimensional robot system, the characteristics of the directions of the degrees of freedom may be greatly different, so that the formula (1.5) is conservative in estimating the uncertainty of the system, and it is difficult to meet the requirements of the robot system on control accuracy and torque smoothness of the robot system with different characteristics of the degrees of freedom.
Finite time convergence (Terminal) sliding mode control is widely used in the industrial field due to its characteristic of finite time convergence. At present, the conventional Terminal sliding mode control has also been popularized to the robot field (relevant documents can be seen: Liu jin jade. sliding mode variable structure control MATLAB simulation. Qinghua university Press: 386-. The Liujin jade teaching teaches a finite time convergence (Terminal) sliding mode control method designed for a robot, and the finite time convergence (Terminal) sliding mode control method is as follows (2.1) - (2.2):
determining slip form surface S ∈ Rn×1As shown in formula (2.1):
wherein, q-qd∈Rn×1Representing a generalized coordinate error vector, q and qdActual generalized coordinate vectors and expected generalized coordinate vectors;the derivative of (e.g. in formula:the derivative of the representation); p and q are set values, positive odd numbers are taken and satisfyΛ=diag[λ1,…λn]Is a diagonal array with adjustable parameters; lambda [ alpha ]1,…λnTaking a positive number as a set value; diag [ x, y ]]Representing a diagonal matrix composed of elements x, y;
calculating the value of the main power/moment required for driving the robot as shown in formula (2.2):
in the formula, tau ∈ Rn×1Is calculated robot main power/moment;a generalized acceleration vector expected for the robotic system;
disclosure of Invention
The invention aims to overcome the defects of the prior art and provides a self-adaptive finite time convergence (Terminal) sliding mode control method of a robot. The method introduces a self-adaptive update rate on the basis of the traditional finite time convergence (Terminal) sliding mode control, and greatly reduces the flutter problem of the sliding mode control while realizing high-precision control; the method can be used for solving the possible influence of different dynamic characteristics in each degree of freedom direction of a robot system by determining an n-dimensional sliding mode surface and providing an n-dimensional self-adaptive updating rate; the proposed n-dimensional adaptive update rate introduces a function increasing along with time at the initial stage of action, and is used for solving the problems of torque saturation and flutter of the robot system at the starting stage; several dynamic compensation schemes for ensuring real-time control in practical engineering are provided.
The invention provides a self-adaptive finite time convergence sliding mode control method of a robot, which is characterized by being suitable for robot systems of parallel robots, series robots and series-parallel robots, and comprising the following steps of:
1) establishing a robot dynamics equation considering uncertainty:
a traditional dynamics analysis method is adopted to establish a dynamics equation of the robot system as shown in formula (1):
wherein M (q) ∈ Rn×nIs a robot system quality matrix;is a robot system Coriolis/centripetal term coefficient vector G (q) ∈ Rn×1Is the gravity term vector of the robot system, tau ∈ Rn×1For main power/moment of robot system, q ∈ Rn ×1Is a generalized coordinate vector of a robot system;is a generalized velocity vector of the robot system;representing a generalized acceleration vector of the robot system; n represents the dimension of the robot system and is a positive integer;
and (3) introducing an uncertainty term into the formula (1), and modifying the robot dynamics equation considering the uncertainty into a formula (2):
wherein M is0∈Rn×n,C0∈Rn×1,G0∈Rn×1Respectively representing calculable items in the robot system mass matrix, Coriolis/centripetal term coefficient vector and gravity term vector, and Δ M ∈ Rn×n,ΔC∈Rn×1,ΔG∈Rn×1Respectively representing uncertain items of a robot system quality matrix, a Coriolis/centripetal item coefficient vector and a gravity item vector;a∈Rn×nrepresentative of multiplicative faults;f∈Rn×1is an additive failure;
the formula (2) is further organized into a formula (3):
wherein,total∈Rn×1the total uncertainty is as in formula (4):
taking into account the dynamics of the robot system, the total uncertaintytotalSatisfies the constraint formula (5):
wherein,total,i∈ R denotes an n-dimensional vectortotalThe ith element (∈ R indicates that is a real number, for example, in formula (5):total,i∈ R representstotal,iIs a real number; (*)iThe i-th element representing (); b1,i、b2,i、b3,iIs a positive number (b)1,i、b2,i、b3,iIs based on the total uncertainty of the systemtotal,iA decision); | is an absolute value operation; (*)2Squaring;
2) determining slip form surface S ∈ Rn×1As shown in formula (6):
wherein, q-qd∈Rn×1Representing a generalized coordinate error vector, q and qdActual generalized coordinate vectors and expected generalized coordinate vectors;the derivative of is expressed; p and q are set values, positive odd numbers are taken and satisfyΛ=diag[λ1,…λn]Is a diagonal array with adjustable parameters; lambda [ alpha ]1,…λnTaking a positive number as a set value; diag [ x, y ]]Representing a diagonal matrix composed of elements x, y;
3) introducing an adaptive update rate in n dimensionsAs shown in formulas (7) to (11):
equation (7) consists of two parts:the function is a function which is increased and bounded along with time and is used for solving the problems of torque saturation and flutter in the starting stage of the robot system;is used to adaptively estimate the total uncertainty of the robot systemtotal,iThe upper bound value of (d);
wherein,is an adaptive estimated value; d1,i∈R,d2,i∈R,d3,i∈ R is the set value of the ith degree of freedom direction and takes positive number, arctan is the arctangent function, ciSetting the value of the ith degree of freedom direction, and taking a positive number; pi is the circumference ratio; t is a time variable;calculating a matrix; i; (*)-1Represents the inverse of the matrix (;the derivative of is expressed;
4) calculating the value of the main power/moment required for driving the robot:
according to the actual generalized coordinate vector q of the robot system and the actual generalized velocity vector of the robot systemValue of (a), calculating a calculable term M in online robot dynamics0,C0,G0A value of (d); then, calculating the value of the main power/moment required by the driving robot according to the robot dynamic equation, the constraint, the sliding mode surface and the self-adaptive update rate considering the uncertainty in the steps 1) to 3) as shown in the formula (12):
in the formula, τantsmc∈Rn×1Is calculated robot main power/moment;the generalized acceleration vector expected for the robot system, ξ∈ R is a set value, taking a small positive number;)TIs a transpose of the matrix/vector (#);
the robot system is driven by main power/moment (12), so that the generalized coordinate vector q of the robot is converged to a steady state or tracking command signal q within limited timed。
The invention provides a second self-adaptive finite time convergence sliding mode control method of a robot, which is characterized by being suitable for robot systems of parallel robots, series robots and series-parallel robots, and comprising the following steps of:
1) establishing a robot dynamics equation considering uncertainty:
a traditional dynamics analysis method is adopted to establish a dynamics equation of the robot system as shown in formula (1):
wherein M (q) ∈ Rn×nIs a robot system quality matrix;is a robot system Coriolis/centripetal term coefficient vector G (q) ∈ Rn×1Is the gravity term vector of the robot system, tau ∈ Rn×1For main power/moment of robot system, q ∈ Rn ×1Is a generalized coordinate vector of a robot system;is a generalized velocity vector of the robot system;representing a generalized acceleration vector of the robot system; n represents the dimension of the robot system and is a positive integer;
and (3) introducing an uncertainty term into the formula (1), and modifying the robot dynamics equation considering the uncertainty into a formula (2):
wherein M is0∈Rn×n,C0∈Rn×1,G0∈Rn×1Respectively representing calculable items in the robot system mass matrix, Coriolis/centripetal term coefficient vector and gravity term vector, and Δ M ∈ Rn×n,ΔC∈Rn×1,ΔG∈Rn×1Respectively representing uncertain items of a robot system quality matrix, a Coriolis/centripetal item coefficient vector and a gravity item vector;a∈Rn×nrepresentative of multiplicative faults;f∈Rn×1is an additive failure;
the formula (2) is further organized into a formula (3):
wherein,total∈Rn×1the total uncertainty is as in formula (4):
taking into account the dynamics of the robot system, the total uncertaintytotalSatisfies the constraint formula (5):
wherein,total,i∈ R denotes an n-dimensional vectortotalThe ith element (∈ R indicates that is a real number, for example, in formula (5):total,i∈ R representstotal,iIs a real number; (*)iThe i-th element representing (); b1,i、b2,i、b3,iIs a positive number (b)1,i、b2,i、b3,iIs based on the total uncertainty of the systemtotal,iA decision); | is an absolute value operation; (*)2Squaring;
2) determining slip form surface S ∈ Rn×1As shown in formula (6):
wherein, q-qd∈Rn×1Representing a generalized coordinate error vector, q and qdActual generalized coordinate vectors and expected generalized coordinate vectors;the derivative of is expressed; p and q are set values,take positive odd number and satisfyΛ=diag[λ1,…λn]Is a diagonal array with adjustable parameters; lambda [ alpha ]1,…λnTaking a positive number as a set value; diag [ x, y ]]Representing a diagonal matrix composed of elements x, y;
3) introducing an adaptive update rate in n dimensionsAs shown in formulas (7) to (11):
equation (7) consists of two parts:the function is a function which is increased and bounded along with time and is used for solving the problems of torque saturation and flutter in the starting stage of the robot system;is used to adaptively estimate the total uncertainty of the robot systemtotal,iThe upper bound value of (d);
is an adaptive estimated value; d1,i∈R,d2,i∈R,d3,i∈ R is the set value of the ith degree of freedom direction and takes positive number, arctan is the arctangent function, ciSetting the value of the ith degree of freedom direction, and taking a positive number; pi is the circumference ratio; t is a time variable;calculating a matrix; i; (*)-1Represents the inverse of the matrix (;the derivative of is expressed; m0(tN-1) Is shown at tN-1A calculable item of the quality matrix of the robot system at the moment; t is tN-1Calculating a set value of the moment for system dynamics; t is tN-1The middle subscript N is a positive integer and takes a value of(when N is 1, tN-1=t0Indicating the initial time); t is tfThe moment when the robot system stops moving; INT (×) denotes a rounding operation, i.e. taking the largest integer part not exceeding the real number (×); Δ t ═ tN-tN-1Taking delta t not less than t as a set value of interval timec;
4) Calculating the value of the main power/moment required for driving the robot:
according to the actual generalized coordinate vector q of the robot system and the actual generalized velocity vector of the robot systemThe value of (d) is a constant time interval (t is an interval time Δ t)N-tN-1) Calculating a calculable term (M) in robot dynamics0(tN-1),C0(tN-1),G0(tN-1) A value of); then, calculating the value of the main power/moment required for driving the robot according to the robot dynamic equation, the constraint, the sliding mode surface and the self-adaptive update rate considering the uncertainty in the steps 1) -3) as shown in the formula (12)':
when t isN-1+tc≤t<tN+tcTime of flight
In the formula, C0(tN-1),G0(tN-1) Are respectively shown at tN-1Calculable items in the Coriolis/centripetal item coefficient vector and the gravity item vector of the time robot system; t is tcRepresenting the time required for calculating a calculable item in a robot system quality matrix, a Coriolis/centripetal item coefficient vector and a gravity item vector (the specific numerical value of the time is related to the dynamics analysis precision and a hardware platform); t is tNCalculating a set value of the moment for system dynamics; tau isantsmc∈Rn×1Is calculated robot main power/moment;the generalized acceleration vector expected for the robot system, ξ∈ R is a set value, taking a small positive number;)TIs a transpose of the matrix/vector (#);
the robot system is driven by main power/moment (12)' to ensure that the generalized coordinate vector q of the robot converges to a steady state or tracks a command signal q within a limited timed。
The invention provides a third self-adaptive finite time convergence sliding mode control method of a robot, which is characterized by being suitable for robot systems of parallel robots, series robots and series-parallel robots, and comprising the following steps of:
1) establishing a robot dynamics equation considering uncertainty:
a traditional dynamics analysis method is adopted to establish a dynamics equation of the robot system as shown in formula (1):
wherein M (q) ∈ Rn×nIs a robot system quality matrix;is a robot system Coriolis/centripetal term coefficient vector G (q) ∈ Rn×1Is the gravity term vector of the robot system, tau ∈ Rn×1For main power/moment of robot system, q ∈ Rn ×1Is a generalized coordinate vector of a robot system;is a generalized velocity vector of the robot system;representing a generalized acceleration vector of the robot system; n represents the dimension of the robot system and is a positive integer;
and (3) introducing an uncertainty term into the formula (1), and modifying the robot dynamics equation considering the uncertainty into a formula (2):
wherein M is0∈Rn×n,C0∈Rn×1,G0∈Rn×1Respectively representing calculable items in the robot system mass matrix, Coriolis/centripetal term coefficient vector and gravity term vector, and Δ M ∈ Rn×n,ΔC∈Rn×1,ΔG∈Rn×1Respectively representing uncertain items of a robot system quality matrix, a Coriolis/centripetal item coefficient vector and a gravity item vector;a∈Rn×nrepresentative of multiplicative faults;f∈Rn×1is an additive failure;
the formula (2) is further organized into a formula (3):
wherein,total∈Rn×1the total uncertainty is as in formula (4):
taking into account the dynamics of the robot system, the total uncertaintytotalSatisfies the constraint formula (5):
wherein,total,i∈ R denotes an n-dimensional vectortotalThe ith element (∈ R indicates that is a real number, for example, in formula (5):total,i∈ R representstotal,iIs a real number; (*)iThe i-th element representing (); b1,i、b2,i、b3,iIs a positive number (b)1,i、b2,i、b3,iIs based on the total uncertainty of the systemtotal,iA decision); | is an absolute value operation; (*)2Squaring;
2) determining slip form surface S ∈ Rn×1As shown in formula (6):
wherein, q-qd∈Rn×1Representing a vector of errors in a generalized coordinate,q and qdActual generalized coordinate vectors and expected generalized coordinate vectors;the derivative of is expressed; p and q are set values, positive odd numbers are taken and satisfyΛ=diag[λ1,…λn]Is a diagonal array with adjustable parameters; lambda [ alpha ]1,…λnTaking a positive number as a set value; diag [ x, y ]]Representing a diagonal matrix composed of elements x, y;
3) introducing an adaptive update rate in n dimensionsAs shown in formulas (7) to (11):
equation (7) consists of two parts:the function is a function which is increased and bounded along with time and is used for solving the problems of torque saturation and flutter in the starting stage of the robot system;is used to adaptively estimate the total uncertainty of the robot systemtotal,iThe upper bound value of (d);
wherein,is an adaptive estimated value; d1,i∈R,d2,i∈R,d3,i∈ R is the set value of the ith degree of freedom direction and takes positive number, arctan is the arctangent function, ciSetting the value of the ith degree of freedom direction, and taking a positive number; pi is the circumference ratio; t is a time variable;calculating a matrix; i; (*)-1Represents the inverse of the matrix (;the derivative of is expressed; m0,dIndicating robot system quality at respective moments of offline computationA calculable term of the matrix;
4) calculating the value of the main power/moment required for driving the robot:
according to the generalized coordinate vector q expected by the robot systemdAnd the desired generalized velocity vector of the robotic systemFirst off-line, the calculable term (M) in the robot dynamics is calculated0,d,C0,d,G0,d) And storing the result, and directly calling (M) at the corresponding moment in the actual movement0,d,C0,d,G0,d) A value of (d); then, calculating the value of the main power/moment required for driving the robot according to the robot dynamic equation, the constraint, the sliding mode surface and the self-adaptive update rate considering the uncertainty in the steps 1) -3) as shown in the formula (12) ":
in the formula, C0,d,G0,dRespectively representing calculable items in the Coriolis/centripetal item coefficient vector and the gravity item vector of the robot system at corresponding moments of off-line calculation; tau isantsmc∈Rn×1Is calculated robot main power/moment;the generalized acceleration vector expected for the robot system, ξ∈ R is a set value, taking a small positive number;)TIs a transpose of the matrix/vector (#);
the robot system is driven by main power/moment (12)', so that the generalized coordinate vector q of the robot can be converged to a steady state or tracking command signal q within limited timed。
Compared with the prior art, the invention has the characteristics and beneficial effects that:
firstly, the invention considers the uncertainty of the robot, especially after the fault influence, the uncertainty of the robot can change greatly with the time. On the basis of the traditional finite time convergence (Terminal) sliding mode control, the self-adaptive update rate is introduced, so that the flutter problem of the sliding mode control is greatly reduced while high-precision control is realized;
the second step is as follows: the method can be used for solving the possible influence of different dynamic characteristics in each degree of freedom direction of a robot system by determining an n-dimensional sliding mode surface and providing an n-dimensional self-adaptive updating rate;
thirdly, the proposed n-dimensional self-adaptive update rate introduces a function which is gradually increased along with time and is bounded at the initial action stage, so as to solve the problems of torque saturation and flutter of the robot system at the starting stage;
and fourthly, providing a plurality of dynamic compensation schemes for ensuring real-time control in practical engineering.
The invention expands the related research results to the field of common robots and is suitable for robot systems with structural types such as parallel robots, serial robots, hybrid robots and the like.
Drawings
FIG. 1 is a block diagram of a flow chart of an implementation of the method of the present invention;
FIG. 2 is a simplified diagram of a conventional 2-branched robot for an embodiment of the present method.
Fig. 3 is a tracking error curve of the joint 1 of the present embodiment under the action of a conventional finite time convergence sliding mode control method "C1", a conventional finite time convergence sliding mode control method "C2" and an adaptive finite time convergence sliding mode control method "C3" according to the present invention;
fig. 4 is a tracking error curve of the joint 2 of the present embodiment under the action of a conventional finite time convergence sliding mode control method "C1", a conventional finite time convergence sliding mode control method "C2" and an adaptive finite time convergence sliding mode control method "C3" according to the present invention;
FIG. 5 is a control input at joint 1 and at joint 2 for a conventional finite time convergence sliding mode control method "C1";
FIG. 6 is a control input at joint 1 and at joint 2 for a conventional finite time convergence sliding mode control method "C2";
fig. 7 shows the control inputs of the adaptive finite time convergence sliding mode control method "C3" at the joint 1 and the joint 2 according to the present invention.
Detailed Description
The invention is further explained below with reference to the drawings and the embodiments.
The invention provides a self-adaptive finite time convergence sliding mode control method of a robot, which is characterized by being suitable for robot systems of parallel robots, series robots and series-parallel robots, and comprising the following steps of:
1) establishing a robot dynamics equation considering uncertainty:
the method comprises the following steps of establishing a kinetic equation of the robot system by adopting a traditional kinetic analysis method (the traditional kinetic analysis method adopts any one of a Kane method, a Newton-Eulerian method, a Newton-Lagrange method, a virtual work principle and the like) as shown in the formula (1):
wherein M (q) ∈ Rn×nIs a robot system quality matrix;is a robot system Coriolis/centripetal term coefficient vector G (q) ∈ Rn×1Is the gravity term vector of the robot system, tau ∈ Rn×1For main power/moment of robot system, q ∈ Rn ×1Is a generalized coordinate vector of a robot system;is a generalized velocity vector of the robot system;representing a generalized acceleration vector of the robot system; n represents the dimension of the robot system and is a positive integer;
(considering that the mechanical system may have friction, external interference, noise, system parameter fluctuation, fault, unmodeled dynamics and other influences, and the kinetic equation of the mechanical system cannot be completely accurate, therefore, the equation (1) is introduced into an uncertainty term, and the robot kinetic equation modified to consider the uncertainty is the equation (2):
wherein M is0∈Rn×n,C0∈Rn×1,G0∈Rn×1Respectively representing calculable items in the robot system mass matrix, Coriolis/centripetal term coefficient vector and gravity term vector, and Δ M ∈ Rn×n,ΔC∈Rn×1,ΔG∈Rn×1Respectively representing the mass matrix of the robot system, the Coriolis/centripetal term coefficient vector and the uncertainty items of the gravity term vector (delta M, delta C and delta G represent the uncertainty of the system);a∈Rn×nrepresentative of multiplicative failure (a,iIs determined according to the degree of actuator failure);f∈Rn×1for additive failure: (f,iIs determined according to the degree of actuator failure);
(uncertain items Delta M, Delta C and Delta G are generated because the dynamics equation of the robot system has uncertain items caused by friction, external interference, noise, system parameter fluctuation and the like in an actual physical system, and in the modeling analysis process, only the calculation is generally considered for simplifying the calculationThe main dynamics of the system are taken into account and therefore there can be uncertainties introduced by unmodeled dynamics.aAndfis mainly caused by a failure of the system actuator);
the formula (2) is further organized into a formula (3):
wherein,total∈Rn×1the total uncertainty is as in formula (4):
taking into account the dynamics of the robot system, the total uncertaintytotalSatisfy the constraint formula (5)
Wherein,total,i∈ R denotes an n-dimensional vectortotalThe ith element (∈ R indicates that is a real number, for example, in formula (5):total,i∈ R representstotal,iIs a real number; (*)iThe i-th element representing (); b1,i、b2,i、b3,iIs a positive number (b)1,i、b2,i、b3,iIs based on the total uncertainty of the systemtotal,iA decision); | is an absolute value operation; (*)2To obtainSquaring;
(formula (5) for the description of the uncertainty term of the robot system, the influence of different dynamic characteristics possibly existing in each degree of freedom direction of the robot system is fully considered, and the conservatism of the uncertainty estimation of the robot system is greatly reduced;
2) determining slip form surface S ∈ Rn×1As shown in formula (6):
wherein, q-qd∈Rn×1Representing a generalized coordinate error vector, q and qdActual generalized coordinate vectors and expected generalized coordinate vectors;the derivative of (e.g. in formula:the derivative of the representation); p and q are set values, positive odd numbers are taken and satisfyQ is adjusted according to actual conditions to realize the stability of the robot as a value principle), Λ ═ diag [ lambda ]1,…λn]Is a diagonal array with adjustable parameters; lambda [ alpha ]1,…λnTaking a positive number (lambda) as the set value1,…λnNeeds to be adjusted according to actual conditions so as toThe robot stability can be realized as a value-taking principle); diag [ x, y ]]Representing a diagonal matrix composed of elements x, y;
3) introducing an adaptive update rate in n dimensionsAs shown in formulas (7) to (11):
(in order to realize high-precision control and reduce the flutter problem of sliding mode control, the invention introduces self-adaptive update rate on the basis of traditional finite time convergence (Terminal) sliding mode control; in order to solve the influence of different dynamic characteristics possibly existing in each degree of freedom direction of a robot system, the self-adaptive update rate and a sliding mode surface are designed to be n-dimensional; in order to solve the problems of moment saturation and flutter of the robot system in a starting stage, the self-adaptive update rate of the n-dimensional also introduces a function which is gradually increased along with time and is bounded in the starting stage.)
Equation (7) consists of two parts:the function is a function which is increased and bounded along with time and is used for solving the problems of torque saturation and flutter in the starting stage of the robot system;is used to adaptively estimate the total uncertainty of the robot systemtotal,iThe upper bound value of (d);
wherein,is an adaptive estimated value; d1,i∈R,d2,i∈R,d3,i∈ R is the setting value of the ith degree of freedom direction and takes the positive number (d)1,i,d2,i,d3,iAdjustment is carried out according to actual conditions so as to ensure that the robot system is stable and the control effect is satisfactory; arctan (×) is an arctangent function; c. CiIs a set value of the ith degree of freedom direction, and takes a positive number (c)iThe adjustment is carried out according to the actual situation so as to reduce the problems of moment saturation and flutter in the starting stage of the robot system; pi is the circumference ratio; t is a time variable;calculating a matrix; i; (*)-1The inverse of the matrix (#) (e.g. in formula:representation matrix M0The inverse of (d);the derivative of (e.g. in formula:to representDerivative of (d);
4) calculating the value of the main power/moment required for driving the robot:
according to the actual generalized coordinate vector q of the robot system and the actual generalized velocity vector of the robot systemValue of (a), calculating a calculable term M in online robot dynamics0,C0,G0A value of (d); then, calculating the value of the main power/moment required by the driving robot according to the robot dynamic equation, the constraint, the sliding mode surface and the self-adaptive update rate considering the uncertainty in the steps 1) to 3) as shown in the formula (12):
in the formula, τantsmc∈Rn×1Is calculated robot main power/moment;the generalized acceleration vector expected by the robot system, ξ∈ R as a set value, and taking a smaller positive number (ξ as a value belonging to the prior art and needing to be adjusted according to actual conditions to reduce the flutter of the main power/moment as a value principle) as well as a method for controlling the robot systemTTo get a momentTranspose of matrix/vector (#) (e.g. in formula: s)TAs a transpose of the orientation quantity s);
the robot system is driven by main power/moment (12), so that the generalized coordinate vector q of the robot is converged to a steady state or tracking command signal q within limited timed。
In the formulas (8) and (12), the calculable term M in the robot system mass matrix, the Coriolis/centripetal term coefficient vector and the gravity term vector0,C0,G0There are many implementation forms, and it is most important to consider the real-time performance and the change of motion trajectory of the robot dynamics calculation. The adopted method at least comprises the following steps:
the second method for realizing the steps 3) to 4) comprises the following steps:
the method is suitable for the condition (t) that the system dynamics can not be calculated in real time and the motion trail is not changed muchcIs larger and is more than or equal to t at the interval time delta tcCalculable item M in internal robot system quality matrix, Coriolis/centripetal item coefficient vector and gravity item vector0,C0,G0Is not greatly changed), specifically:
according to the actual generalized coordinate vector q of the robot system and the actual generalized velocity vector of the robot systemThe value of (d) is a constant time interval (t is an interval time Δ t)N-tN-1) Calculating a calculable term (M) in robot dynamics0(tN-1),C0(tN-1),G0(tN-1) A value of); then, calculating the value of the main power/moment required for driving the robot according to the robot dynamics equation, the constraint, the sliding mode surface and the adaptive update rate considering the uncertainty in the steps 1) to 3) as shown in the formulas (8) 'to (12)' (except for (8) 'and (12)', the other formulas are the same as the first method):
when t isN-1+tc≤t<tN+tcTime of flight
In the formula,calculating a matrix; i; (*)-1Represents the inverse of the matrix (;the derivative of is expressed; t is tN-1And tNCalculating a set value of the moment for system dynamics; t is tN-1And tNThe middle subscript N is a positive integer and takes a value of(when N is 1, tN-1=t0Indicating the initial time); t is tfThe moment when the robot system stops moving; INT (×) denotes a rounding operation, i.e. taking the largest integer part not exceeding the real number (×); Δ t ═ tN-tN-1Is a set value of interval time (taking delta t to be more than or equal to t)cAnd a calculable item M in the robot system mass matrix, the Coriolis/centripetal item coefficient vector and the gravity item vector in the interval time delta t0,C0,G0Is not much varied); m0(tN-1),C0(tN-1),G0(tN-1) Are respectively shown at tN-1Calculable items in the mass matrix, the Coriolis/centripetal item coefficient vector and the gravity item vector of the robot system at the moment; t is tcRepresenting the time required for calculating a calculable item in a robot system mass matrix, a Coriolis/centripetal item coefficient vector and a gravity item vector, wherein specific numerical values of the time are related to the dynamics analysis precision and a hardware platform; t represents the current time;
if the system dynamics can not be calculated in real time and the motion trail changes greatly (t)cIs larger and is more than or equal to t at the interval time delta tcInner robotComputable term M in system quality matrix, Coriolis/centripetal term coefficient vector and gravity term vector0,C0,G0Is greatly changed), the third method of the present invention for realizing the above steps 3) -4) is:
according to the generalized coordinate vector q expected by the robot systemdAnd the desired generalized velocity vector of the robotic systemFirst off-line, the calculable term (M) in the robot dynamics is calculated0,d,C0,d,G0,d) And storing the result, and directly calling (M) at the corresponding moment in the actual movement0,d,C0,d,G0,d) A value of (d); then, calculating the value of the main power/moment required for driving the robot according to the robot dynamics equation, the constraint, the sliding mode surface and the adaptive update rate considering the uncertainty in the steps 1) to 3), as shown in the formulas (8) to (12) (except for the formulas (8) to (12), the other formulas are the same as the first method):
in the formula, M0,d,C0,d,G0,dRespectively representing the calculable items in the robot system quality matrix, the Coriolis/centripetal item coefficient vector and the gravity item vector at the corresponding moment of off-line calculation.
Examples
The method of the embodiment takes a conventional 2-branch arm (typical serial robot system) as a control object, and the 2-branch structure is shown in fig. 2, wherein r is1,r2,J1,J2,m1,m2,q1,q2Respectively represents the length of the branch chain 1, the length of the branch chain 2, the inertia of the branch chain 1, the inertia of the branch chain 2, the mass of the branch chain 1, the mass of the branch chain 2, the rotation angle of the branch chain 1 and the rotation angle of the branch chain 2.
Given its principal structural parameter r1,r2,J1,J2,m1And m21m,0.8m,5kgm,5kgm,0.5kg and 1.5kg, respectively. Assume a desired motion trajectory qd,rob=[qd,rob,1,qd,rob,2]TAnd an initial attitude q0,robAre respectively as
qd,rob,1=a1sin(ω1t)+a2cos(ω2t)+a3sin(ω3t)+a4cos(ω4t)+a5sin(ω5t)+a6cos(ω6t) (3.1)
qd,rob,2=b1sin(ω1t)+b2cos(ω2t)+b3sin(ω3t)+b4cos(ω4t)+b5sin(ω5t)+b6cos(ω6t) (3.2)
q0,rob=[1.5,-1.5]T(3.3)
Wherein (a)1~a6),(b1~b6) And (ω)1~ω6) Set to (1,1,0.01,1,0.01,0.001), (-1, -1, -0.01, -1, -0.01, -0.001), and (0.1,0.2,0.4,0.8,1.6,3.2), respectively; sin (, denotes the sine value of (); cos represents the cosine value of (#);
the self-adaptive finite time convergence sliding-mode control method for the mechanical arm comprises the following steps: 1) establishing a robot dynamics equation considering uncertainty:
a Newton-Euler method is adopted to establish a kinetic equation of the mechanical arm as shown in formula (3.4):
wherein,
qrob=[q1,q2]T
M11=(m1+m2)r1 2+m2r2 2+2m2r1r2cos(q2)+J1
M12=M21=m2r2 2+m2r1r2cos(q2)
M22=m2r2 2+J2
(3.5)
G1=((m1+m2)r1cos(q2)+m2r2cos(q1+q2))g
G2=m2r2cos(q1+q2)g
in the formula, MijRepresentation matrix Mrob(qrob) The (i, j) th element of (a); ciAnd GiThen respectively representAnd Grob(qrob) The ith element of (1). Mrob(qrob)∈R2×2Is the quality matrix of the mechanical arm;is the robot arm Coriolis/centripetal neck coefficient vector; grob(qrob)∈R2×1Is the mechanical arm gravity term vector; tau isrob∈R2×1The main power/moment of the mechanical arm; q. q.srob∈R2×1Is a generalized coordinate vector of the mechanical arm;is the generalized velocity vector of the mechanical arm;representing a generalized acceleration vector of the mechanical arm; the dimension of the mechanical arm is 2; g is the acceleration of gravity;
and (3) introducing an uncertainty term into the formula (3.4), and modifying the mechanical arm dynamics equation considering the uncertainty into the formula (3.6):
wherein M is0,rob∈R2×2,C0,rob∈R2×1,G0,rob∈R2×1Respectively represent the calculable items in the robot mass matrix, the coriolis/centripetal coefficient vector and the gravity term vector (the robot dynamics parameters of the calculable/calculable value in the embodiment are: in the formula,is a real m1,m2,J1,J2An estimable/calculable value of; m0,rob,C0,rob,G0,robCan be formed by1,r2,And the actual movement locus qrobCalculated by substituting the formula (3.5); Δ Mrob∈R2×2,ΔCrob∈R2×1,ΔGrob∈R2×1Respectively representing uncertain items of a mechanical arm mass matrix, a Coriolis/centripetal item coefficient vector and a gravity item vector; (in this example Mrob(qrob)、Grob(qrob) Can be determined from the main structural parameter r1,r2,J1,J2,m1,m2And the actual movement locus qrobSubstituting the formula (3.5) to obtain the calculation result; Δ Mrob,ΔCrob,ΔGrobMay be respectively formed byrob=Mrob(qrob)-M0,rob,ΔGrob=Grob(qrob)-G0,robCalculated);a,rob∈R2×2representative of multiplicative failure (a,rob,iIs determined according to the fault degree of the actuator, and the effective value range is [0,1 ]]The value of the embodiment isa,rob,i=1);f,rob∈R2×1For additive failure: (f,rob,iAccording to the size ofThe degree of the fault of the line device is determined, and the value of the embodiment isf,rob,i=0);
The formula (3.6) is further arranged into a formula (3.7)
Wherein,total,rob∈R2×1the total uncertainty is as in formula (3.8):
taking into account the dynamics of the arm, this total uncertaintytotal,robSatisfy the constraint formula (3.9)
Wherein,total,rob,i∈ R denotes a 2-dimensional vectortotal,robThe ith element of (1); b1,rob,i、b2,rob,i、b3,rob,iCan be estimated as b is more than or equal to 11,rob,i≤10、8≤b2,rob,i≤80、3≤b3,rob,iB is less than or equal to 30 (at different time, satisfying constraint formula (3.9))1,rob,i、b2,rob,i、b3,rob,iMay vary, so that in practical applications b1,rob,i、b2,rob,i、b3,rob,iIs generally unknown, and can estimate an approximate range of values at most); | is an absolute value operation; (*)2Squaring;
2) determining slip form surface Srob∈Rn×1As shown in formula (3.10):
wherein,rob=qrob-qd,rob∈R2×1representing the generalized coordinate error vector, q, of the mechanical armrobAnd q isd,robThe actual generalized coordinate vector and the expected generalized coordinate vector of the mechanical arm, and the values of p and q are respectively 9, 5 and Λ in the embodimentrob=diag[λ1,rob,λ2,rob]Is a diagonal array with adjustable parameters; lambda [ alpha ]1,rob,λ2,robThe values in this embodiment are all 150; diag [ lambda ]1,rob,λ2,rob]Is represented by the element λ1,rob,λ2,robForming a diagonal matrix;
3) introducing an adaptive update rate in n dimensionsAs shown in formulas (3.11) - (3.15):
equation (3.11) consists of two parts:the function is increased gradually and bounded along with time and is used for solving the problems of torque saturation and flutter in the starting stage of the mechanical arm;is used to adaptively estimate the total mechanical arm uncertaintytotal,rob,iThe upper bound value of (d);
wherein,for adaptive estimation, the present embodiment takes the values of(wherein i is 1 or 2;are respectively asAndinitial value of (d); d1,rob,i∈R,d2,rob,i∈R,d3,rob,i∈ R is the setting value of the ith degree of freedom direction, and the value of this embodiment is d1,rob,i=5d2,rob,i=0.1d3,rob,i1 (wherein i takes values of 1 and 2); arctan (×) is an arctangent function; c. Crob,iThe value of the present embodiment is c, which is the setting value of the ith degree of freedom directionrob,i(ii) 5 (where i is 1 and 2); pi is the circumference ratio; t is a time variable;calculating a matrix; i; (*)-1The inverse of the matrix (#) (e.g. in formula:representation matrix M0,robThe inverse of (d);the derivative of (e.g. in formula:to representDerivative of (d);
4) calculating the value of the main power/moment required for driving the robot:
according to the actual generalized coordinate vector q of the mechanical armrobAnd actual generalized velocity vector of the mechanical armOn-line computation of the calculable term M in the dynamics of the robot arm0,rob,C0,rob,G0,robA value of (d); then, calculating the value of the main power/moment required by the driving mechanical arm according to the robot dynamic equation, the constraint, the sliding mode surface and the self-adaptive update rate considering the uncertainty in the steps 1) to 3) as shown in the formula (3.16):
in the formula, τantsmc,rob∈R2×1Calculating the main power/moment of the mechanical arm;expected generalized acceleration vector for a robotic arm ξrob∈ R is set value, and takes smaller positive number, the value of this embodiment is ξrob=0.001;(*)TIs a transpose of the matrix/vector (#);
the mechanical arm is driven by main power/moment (3.16) to enable the generalized coordinate vector q of the mechanical armrobAchieving convergence to steady state or tracking command signal q within a finite timed,rob。
The effect analysis achieved by the method of the embodiment is as follows:
to illustrate the superiority of the proposed method of the present invention, three control schemes were designed and the results are shown in FIGS. 3-7: traditional finite time convergence sliding mode control method 'C1' (b)1,c1,rob,b2,c1,rob,b3,c1,robChoose small value); traditional finite time convergence sliding mode control method 'C2' (b)1,c2,rob,b2,c2,rob,b3,c2,robChoosing a big value); the adaptive finite time convergence sliding mode control method 'C3' provided by the invention is the method of the embodiment. Wherein, the traditional finite time convergence sliding mode control methods 'C1' and 'C2' are as the formulas (3.17) and (3.18) (refer to Liu jin jade, sliding mode variable structure control MATLAB simulation, Qing Hua university Press, 386-
(3.17)
(3.18)
Wherein,
in the formula, b1,c1,rob∈R,b2,c1,rob∈R,b3,c1,rob∈ R are design parameters (b) of a traditional finite time convergence sliding mode control method' C11,c1,rob,b2,c1,rob,b3,c1,robFor positive numbers, the value is taken into account of the total uncertainty of the systemtotal,rob,iAnd take a smaller value, thisExample value b1,c1,rob=1,b2,c1,rob=8,b3,c1,rob=3);c1,robAccording to actual q of the armrobAndcalculated according to the formula (3.19); b1,c2,rob∈R,b2,c2,rob∈R,b3,c2,rob∈ R are design parameters (b) of a traditional finite time convergence sliding mode control method' C21,c2,rob,b2,c2,rob,b3,c2,robFor positive numbers, the value is taken into account of the total uncertainty of the systemtotal,rob,iAnd takes a larger value, the value of this embodiment is b1,c2,rob=10,b2,c2,rob=80,b3,c2,rob=30);c2,robAccording to actual q of the armrobAndcalculated according to the formula (3.20);
in addition, in order to compare the merits of the three control methods fairly, the other setting parameters of the two methods are the same as those of the present embodiment
In the formula, τmaxRepresenting the maximum moment of the robot arm.
Under the action of three control methods, the mechanical arm can realize tracking within a limited time (2 s). However, since the upper uncertainty bound is set to be less than the actual system uncertainty, the tracking characteristic of the control method "C1" is not guaranteed to be between 3s and 6 s. The tracking effect of the control methods "C2" and "C3" is very satisfactory by purely considering the tracking characteristic. In addition, from the viewpoint of the smooth characteristics of torque/force, the output of the control method "C3" is the smoothest, the output of the control method "C1" is smoother, and the output of the control method "C2" has serious oscillation and saturation problems.
In summary, compared with the conventional finite time convergence (Terminal) sliding mode control methods ("C1" and "C2"), the adaptive finite time convergence (Terminal) sliding mode control method ("C3") for the robot, which is provided by the invention, can ensure high tracking accuracy and smoothness of force/torque output at the same time, and has very obvious advantages. The following is further explained in conjunction with the figures:
in fig. 3, "C1 _ eq1 (" solid line "indicates)" is a tracking error curve of the joint 1 under the action of a conventional finite time convergence sliding mode control method "C1"; "C2 _ eq1 (" dashed line "indicates)" is a tracking error curve of the joint 1 under the action of a traditional finite time convergence sliding mode control method "C2"; "C3 _ eq1 (" point "indicates)" is a tracking error curve of the joint 1 under the action of the adaptive finite time convergence sliding mode control method "C3" proposed by the present invention.
Remarking: under the action of three control methods, the mechanical arm can realize tracking within a limited time (2 s). However, since the upper uncertainty bound is set to be less than the actual system uncertainty, the tracking characteristic of the control method "C1" is not guaranteed to be between 3s and 6 s. The tracking effect of the control methods "C2" and "C3" is very satisfactory by purely considering the tracking characteristic.
In fig. 4, "C1 _ eq2 (" solid line "indicates)" is a tracking error curve of the joint 2 under the action of a conventional finite time convergence sliding mode control method "C1"; "C2 _ eq2 (" dashed line "indicates)" is a tracking error curve of the joint 2 under the action of a traditional finite time convergence sliding mode control method "C2"; "C3 _ eq2 (" points "indicate)" is a tracking error curve of the joint 2 under the action of the adaptive finite time convergence sliding mode control method "C3" proposed by the present invention.
Remarking: under the action of three control methods, the mechanical arm can realize tracking within a limited time (2 s). However, since the upper uncertainty bound is set to be less than the actual system uncertainty, the tracking characteristic of the control method "C1" is not guaranteed to be between 3s and 6 s. The tracking effect of the control methods "C2" and "C3" is very satisfactory by purely considering the tracking characteristic.
In fig. 5, "C1 _ u1 (" solid line "indicates)" is the control input of the conventional finite time convergence sliding mode control method "C1" at the joint 1; "C1 _ u2 (" dashed line "indicates)" is the control input at joint 2 of the conventional finite time convergence sliding mode control method "C1".
Remarking: the output of the control method "C1" is smoother from a moment/force smoothing characteristic.
In fig. 6, "C2 _ u1 (" solid line "indicates)" is a control input of the conventional finite time convergence sliding mode control method "C2" at the joint 1; "C2 _ u2 (" dashed line "indicates)" is the control input at joint 2 of the conventional finite time convergence sliding mode control method "C2".
Remarking: from the moment/force smoothing characteristics, the output of control method "C2" suffers from severe shock and saturation problems.
In fig. 7, "C3 _ u1 (" solid line "indicates)" is the control input of the adaptive finite time convergence sliding mode control method "C3" proposed by the present invention at the joint 1; "C3 _ u2 (" dashed line "indicates)" is the control input of the adaptive finite time convergence sliding mode control method "C3" proposed by the present invention at the joint 2.
Remarking: the output of control method "C3" is the smoothest from the viewpoint of the smooth characteristics of torque/force.