Constraint-Following Servo Control for the Trajectory Tracking of Manipulator with Flexible Joints and Mismatched Uncertainty

: Trajectory tracking is a common application method for manipulators. However, the tracking performance is hard to improve if the manipulators contain ﬂexible joints and mismatched uncertainty, especially when the trajectory is nonholonomic. On the basis of the Udwadia–Kalaba Fundamental Equation (UKFE), the prescribed position or velocity trajectories are creatively transformed into second-order standard differential form. The constraint force generated by the trajectories is obtained in closed form with the help of UKFE. Then, a high-order fractional type robust control with an embedded ﬁctitious signal is proposed to achieve practical stability of the system, even if the mismatched uncertainty exists. Only the bound of uncertainty is indispensable, rather than the exact information. A leakage type of adaptive law is proposed to estimate such bound. By introducing a dead-zone, the control will be simpliﬁed when the speciﬁc parameter enters a certain area. Validity of the proposed controller is veriﬁed by numerical simulation with two-link ﬂexible joint manipulator.


Introduction
Conventional industrial robot manipulators generally use harmonic gears for excellent performances. Meanwhile, the limited torsional stiffness caused by deformable parts also introduces flexibility into the system. The flexibility of manipulator is sometimes used to offset small errors or limit impact forces in case of potential collisions. However, it is unwanted in most situations because of the ensuing vibrations and machining error [1,2]. As manipulators are widely used in aerospace, medical, machining, and other fields, the flexibility is a non-negligible problem while optimizing the control performance [3][4][5].
In practical applications, it is often necessary for flexible joint manipulator (FJM) to follow a prescribed trajectory. To obtain accurate motion, researchers have investigated various control methods for the FJM, e.g., adaptive control [6,7], sliding mode control [8,9], neural network control [10,11], boundary control [12], fuzzy logic control [13,14], and so on. However, the design of the controllers mentioned above are all model-based, and controllers achieve the expected tracking performance only if the model and the parameters are completely available. Model-based control methods often fail when unpredictable parameters change or external disturbances occur. Therefore, Ulrich et al. proposed a composite control methodology for space manipulators affected by parametric uncertainties and joint elastic vibrations. The control is aiming to cope with the adaptive trajectory tracking issue [15]. The methodology could be decomposed into a decentralized adaptive term for system stabilizing and a linear correction term for vibration dampening. He et al. proposed a neural network controller for FJM based on state feedback [10]. The tracking performance of FJM, the bound of system state parameters, as well as the security of the robot were all improved. In [9], Zaare et al. proposed a voltage-based adaptive sliding mode control for FJM to deal with uncertainties and external disturbances in the tracking process. Global asymptotic stability is achieved by dividing the FJM into n subsystems. Combining interval triangular membership functions with Karnik-Mendel-type reduction algorithm, Kelekci et al. presented an interval type-2 fuzzy logic controller for FJM to yield improved performance of real-time trajectory and vibration control [13]. From the literatures survey, it can be seen that most of the past researches regarded the given trajectories as constraints directly. The designed controllers compare feedback positions with prescribed trajectories to obtain the following error. Then, various methods mentioned above compel the following error to zero (or a small neighborhood near zero), so that the asymptotical stability of FJM system is guaranteed. This work provides a quite special perspective: the given trajectories are regarded as a type of virtual force constraints and the corresponding constraint-following control method is applied.
In the past decades, the theory of constraint-following control in the research of trajectory tracking control has been of interest to many scholars. Compared with the aforementioned researches [6][7][8][9][10][11][13][14][15][16], there is a significant difference: the concept of such researches is to make the tracking error approaches to zero so that the motion of FJM approaches to the prescribed trajectory. However, the theory of constraint-following control regards the trajectories as virtual force constraints. The controller is designed to add calculated equivalent forces which forced the system to trace given constraints [17]. How to obtain the constraint thereby becomes a critical issue. Udwadia and Kalaba provided an effective way to solve this problem [18,19]. The Udwadia-Kalaba Fundamental Equation (UKFE) could be applied to dynamic systems containing whether holonomic or nonholonomic constraints, to obtain the constraint force in closed form. The UKFE method greatly promotes the development of control for constrained mechanical systems. Later, Chen illustrated the conception of constraint-following control systematically (also called constraint servo control) [20][21][22]. On this basis, Xu et al. designed the trajectory tracking control law for uncertain systems by combining with fuzzy control [23]. Zhao et al. solved the trajectory tracking control problem of an artificial swarm system consisting of multiple agents [24,25]. Zhao et al. realized the trajectory tracking control of uncertain systems with given forces and constraint forces [26]. The existing applications have proved the validity of UKFE. It would be an efficient method for the control of trajectory tracking for FJM, as such field is not sufficiently investigated.
The use of manipulator is mainly focused on point-to-point motion and trajectorytracking applications. For trajectory-tracking, it is often used for milling, grinding, polishing, and so on. As the manipulator has redundant degrees of freedom while machining, the trajectory is usually programmed and optimized offline in advance under comprehensive consideration of error, efficiency, and the stiffness of different poses. The control proposed in this work could be adopted in the predesigned trajectory and the calculation is simplified. It is of great practical significance.
In this paper, the trajectory tracking control problem of FJM is explored. The main contributions of this paper are fivefold. First, being different from previous studies on this topic, the given trajectories are regarded as virtual force constraints. The required constraint forces are obtained by using UKFE, whether the constraints are holonomic or nonholonomic. Second, no extra state variable, Lagrange multiplier or quasi-coordinate is required for the control design. Furthermore, there is no need for any force feedback but position and velocity information. Third, the system may contain mismatched uncertainty [27]. A fictitious signal is used to divide the original system into cascaded subsystems so that, the cascaded controls could be designed accordingly. Fourth, the given controller contains high-order fractional polynomial terms. Engineers could enhance flexibility of the controller by adjusting its order. Fifth, what indispensable is the existence of the bound of uncertainty, rather than exact uncertainty information, so that the controller has a wider range of applications. In the meantime, a leakage type adaptive law is designed for the estimation of such unknown bound information.

Dynamic System under Constraints
Consider the dynamic system in [17]: where q ∈ R n is the vector of coordinates, t ∈ R is the time, τ ∈ R n denotes the torque from motor, and σ ∈ Σ ⊂ R p is the p-dimensional uncertain parameter with compact and bounded set Σ which represents the possible bounding of σ. M(q, σ, t), R(q,q, σ, t)q, and D(q(t)), σ(t), t) denote the inertial term, the centrifugal and Coriolis terms, and the gravitation term, respectively, with appropriate dimensions.

Remark 1.
Based on the work in [21], most existing control problems, such as trajectory following control, optimal control, and stabilization, can be transformed into constraint form (8).

Remark 2.
In the past, the positive definiteness of the inertia matrix was widely perceived as a fact, however, it has been proved as an assumption rather than a fact in [28] with some special cases in which the inertia matrix is not positive definite.

Assumption 2.
For any T and β, there exists at least one solutionq of the constraint Equation (8). In other words, constraint Equation (8) is consistent.
In (3), the constraints are arbitrary, sometimes causing the constraint Equation (8) to be unrealistic, however, Assumption 2 avoids this problem. In other words, the given constraints should be reasonable. Theorem 1. Ref. [29] A dynamic system described by (1) and required to obey constraints (8). Subject to Assumptions 1 and 2, the force which compels the system to follow the given constraints is calculated by where the superscript "+" denotes the Moore-Penrose generalized inverse and Q c obeys the Lagrange's form of d'Alembert's principle.

Remark 3.
According to Theorem 1, we can get constraint force meeting the constraints (8), but this is model-based, which means we need to obtain an accurate model. On this basis, the control input applied to force the system with known uncertainty to meet constraints (8) can be acquired. In more realistic situations, an adaptive control, which is designed for the situation of lacking of uncertainty information, will be shown later.

Dynamic Model of Flexible Joint Manipulator under Constraints
According to the summary of Spong [2], we consider a FJM system which contains uncertainties described by the following dynamic model: where q 1 = [q 11 q 12 · · · q 1n ] T and q 2 = [q 21 q 22 · · · q 2n ] T are the generalized coordinates of links and angles, respectively, with each component denotes the angle of corresponding part, q = [q T 1 q T 2 ] T denotes the generalized coordinates of the system. The revolute joints of the manipulator system can be modeled as linear torsional spring with stiffness K ∈ R n×n , where K is diagonal and totally positive. The matrix M(q 1 , σ 1 , t) and I(σ 2 , t) are the inertial terms of links and actuators, respectively, R(q 1 ,q 1 , σ 1 , t)q 1 is the centrifugal and Coriolis force, D(q 1 , σ 1 , t) is the mixed term of gravitation force, Coulomb/viscous friction and external disturbances, τ is an n-dimensional vector which stands for the input force from actuators. The uncertainties σ 1 and σ 2 may be fast timevarying and mismatched. Assumption 3. The inertia matrix M(q 1 , σ 1 , t) has upper and lower bounds [30]. Therefore, there exist constantsρ, ρ > 0, making the following formula true for any q 1 and σ 1 : From (10) we know that the system is under-actuated, which means the control torque from actuators cannot affect the links directly. Forces are transmitted by the elastic elements in joints. On the other hand, the uncertainty may be mismatched. It cannot be controlled directly by designing the input τ. To solve these problems, two steps are taken to design the control.
First, a fictitious control signal is designed and implanted into the system. The first part of (10), and it can be rearranged as follows: whereτ is the implanted fictitious control and the first part of (10) is "controlled" byτ. Second, multiply K −1 (σ 1 , t) on both sides of (12), let x 1 = q 2 −τ, x 2 =q 2 −τ, then we can get the system as follows: whereM(q 1 , σ 1 , t): . By taking such steps, the system (10) is divided into link position subsystem (13) and joint position subsystem (14) with respective control input, as well makes the system full-actuated. Now, we consider the uncertainty in designing the controlτ and τ. Decompose the matricesM,R,D, I and K as follows: whereM,R,D,Ī,K stand for the nominal part, while ∆M, ∆R, ∆D, ∆I, ∆K denote uncertain parts. LetĒ where E is the identity matrix, thus ∆E(q 1 , σ 1 , t) =Ē(q 1 )B(q 1 , σ 1 , t).
In industrial engineering applications, the actuator is fixed to the link in general. Thus, the link position subsystem (13) is usually driven to approximately follow the given trajectory.

Assumption 4.
For given q 1 ∈ R n , T(q 1 , t) is of full rank.

Assumption 5.
For any T(q 1 , t) subject to Assumption 4 and given L ∈ R m×m , L > 0, let for given (q 1 , t) ∈ R n × R, there exists a (possibly unknown) constant κ l > −1: where λ min (·) denotes the minimum characteristic value of matrix.
Remark 4. Assumption 4 assures the matrix T(q 1 , t)T T (q 1 , t) is invertible and the given constraints are reasonable. Generally, we can hardly get the accurate value of the constant κ l as the lack of information about the uncertainty bound. However, in the special case that there is no uncertainty, which meansM =M, B = 0, N = 0, the constant can be chosen as κ l = 0. By continuity, Assumption 5 limits the effect of uncertainty on the inertial function M with a certain unidirectional threshold (i.e., it is bounded except a certain single direction), the effect is embodied as the possible deviation of M fromM.

Robust Servo Control Design
In Section 3, the whole system is divided into two parts. According to the new structure of the system, the fictitious controlτ should be given first. The proposedτ consists of three parts: p 11 (q 1 ,q 1 , t) is the constraint control designed according to UKFE. The compatible control p 12 (q 1 ,q 1 , t) is employed to cope with the incompatibility of the initial condition. As the uncertainty is always inevitable, p 13 (q 1 ,q 1 , t) is the adaptive part designed to counteract the effects of uncertainty. Assumption 6. (i) There exists a (possibly unknown) j-dimensional constant vector ψ 1 ∈ (0, ∞) j and a known function (ii) For given (q 1 ,q 1 , t) ∈ R n × R n × R, the function Υ 1 (·) is C 1 , concave and nondecreasing with respect to each component of its argument ψ 1 , i.e., for any ψ 11 , ψ 12 ∈ (0, ∞) j : Assumption 7. (i) There exists a (possibly unknown) s-dimensional constant vector ψ 2 ∈ (0, ∞) s and a known function (ii) For each (q 1 ,q 1 , x 1 , x 2 , t) ∈ R n × R n × R n × R n × R, the function Υ 2 (·) is C 1 , concave, and non-decreasing with respect to each component of its argument ψ 2 , i.e., for any ψ 21 , ψ 22 : Remark 5. Assumptions 6 and 7 refer to properties on the possible bound of uncertainty. The constant vector ψ 1,2 is unknown because it is related to the unknown boundary set of Σ 1,2 . In the special case that Υ 1,2 is linear with respect to ψ 1,2 , they (Assumptions 6 and 7) are always reached. The mathematical expressions are as follows: Υ 2 has a similar argument. Next, it will be demonstrated by the linear Υ 1,2 we choose.
Let p 11 (q 1 ,q 1 , t) =M 1 2 (q 1 )(T(q 1 , t)M − 1 2 (q 1 )) + [β(q 1 ,q 1 , t) The purpose of proposing control p 11 is to compel the nominal system (the system without uncertainty) to meet the given constraints by exerting constraint force. Dimension of the designed control is the same as that of the input control τ. Thus, the design is feasible. The constraints we consider is ideally met T(q 1 , t)q 1 = α(q 1 , t). However, only approximate constraints satisfy such conditions in practical applications. The reason is the effect of uncertainty and the deviation between the initial condition of the system and the expected constraints.
For given positive scalars ε 1 > 0 and Γ ≥ 1, the proposed virtual control is where with The parameterψ 1 is determined by the following adaptive law:ψ where constant k 1 ∈ R + ,ψ 1 (t 0 ) ∈ (0, ∞) j , t 0 is the initial time.

Remark 6.
It can be seen that (30) is a fractional polynomial function with high-order terms. The designed parameter Γ determines the specific formula of the proposed control. Engineers could adjust the flexibility of the control with different orders. Large Γ should be used when the initial state is far away from the desired position, i.e., ρ 1 1. This is because a larger Γ would lead to a larger control input in the very beginning so that the following error would decrease faster. On the contrary, for the simplicity purpose, a small Γ is better if the initial condition is close to the position where it should be. Now, we give the control torque of the whole system τ with a constant ε 2 > 0, the proposed real control τ is with where ρ 2 = (x 2 + Px 1 )Υ 2 (ψ 2 , q 1 ,q 1 , x 1 , x 2 , t), K p ∈ R n×n and K d ∈ R n×n are diagonal positive gain matrices.
The parameterψ 2 is governed by the following adaptive law:ψ

Remark 7.
The adaptive laws (32) and (35) are used to estimate the possible boundary of the uncertainty. As they are of leakage type, the parameters (ψ 1 andψ 2 ) may decrease when the constraint following error reduces. The constant scalars k 1 and k 2 determine the rate of convergence.
In the following proof process, for convenience, the parameters of functions are retained only when they may be confusing. A Lyapunov function candidate we choose as follows: Then, we prove that this Lyapunov function is legitimate.According to (37), the lower bound is analyzed: where h 1 = min{ 1 2 λ min (L), 1 2 (1 + κ l )k −1 1 }. Similarly, the upper bound is given by where h 2 = max{ 1 2 λ max (L), 1 2 (1 + κ l )k −1 1 }, λ max (·) denotes the maximum characteristic value of matrix.
As Ω 1 is positive definite, V 2 is also positive definite. Then, we can get the upper bound of V 2 : with Therefore, according to (39)-(42), where a 1 = min{h 1 , h 3 }, a 2 = max{h 2 , h 4 }. Yet we have proved V is positive definite and decrescent. For given uncertainty parameters σ 1 ∈ Σ 1 and σ 2 ∈ Σ 2 with the corresponding δ(t) of the controlled system, the derivative of V 1 is given bẏ these terms will be analyzed separately.
Using decomposition ofM −1 ,R,D in (15) and by Equation (16), we have (9) and (26), under the nominal condition (i.e.,no uncertainty exists, According to the given p 12 in (27) and after performing some algebraic operations, we have According to the given p 13 in (29) and with ∆E =ĒB in (16), one has Based on (31), we have ρ 1 = x 3 Υ 1 (ψ 1 , q 1 ,q 1 , t), after matrix cancellation: Adopting the Rayleigh's principle and invoking (18) in Assumption 5, we can get therefore, we can show that based on Assumption 6: Based on the inequality ab ≤ 1 2 (a 2 + b 2 ), a, b ∈ R, for any ω > 0, we have where λ A ≥ LTM −1 is a constant. Thus, according to (31), for all ε 1 > 0, this lead to By Assumption 6, we have this leads to Combine the second term on the right-hand side (RHS) of (45) with the adaptive law (32), we can get According to (59) and (60), we havė Based on (15), introducing the decomposition of I and K, we havė Based on Assumption 7 and the control (33) and (34), it can be seen thaṫ If ρ 2 > ε 2 , we have Thus, for all ε 2 > 0, The third and fourth terms on the RHS of (64) can be simplified as The uniform ultimate boundedness performance is determined by The radius of uniform ultimate boundedness ball is determined by d . By (78), it is shown that d and ξ are positively correlated, which means that d approaches to 0 when ξ approaches to 0. By (74) and (78), ξ approaches to 0 while both of ε 1 and ε 2 are close to 0, and k 1 , k 2 approach to infinity. Thus, if ε 1,2 → 0, k 1,2 → ∞, then d → 0. Figure 1 shows the design process of the proposed adaptive robust controller, where τ is the final controller. The design process is shown as follows.
Step 1. Decomposing the FJM into rigid link and flexible joint, simplify the flexibility of FJM as linear torsion spring, and get the dynamic model of the FJM system.
Step 2. Implant a fictious controlτ into the rigid part and rewrite the dynamic model.
Step 3. Transform the constraint equation into second order differential form, and get the constraint control p 11 by UKFE equation.
Step 4. Get the compatible control p 12 based on the initial deviation to the desired trajectory.
Step 5. Design the adaptive law 1 to deal with uncertainty in rigid part, on this basis, get the adaptive robust control p 13 .
Step 6. Get the fictious controlτ by adding p 11 , p 12 and p 13 .
Step 7. Design the nominal control for the flexible part.
Step 8. Design the adaptive law 2 to deal with the flexible joint subsystem. On this basis, get the adaptive robust control p 2 .
Step 9. Get the final controller τ by addingτ, p 2 , and the nominal control.
The relationship between different magnitude of uncertainty and the control performance is also investigated. For instance, the range of ∆m is chosen asm = [0.1, 0.5], and the range of ∆K is set asK = [0.1, 0.5]. The result of the average following errorē 1 andē 2 with respect to different pairs of ∆m and ∆K are shown in Figures 7 and 8 and the average control costτ 1 andτ 2 with respect to different pairs of ∆m and ∆K are shown in Figures 9 and 10, respectively. Obviously, the proposed control has strong robustness since the average following error and the average control cost varies in a small range when the magnitude of uncertainty changes.

Conclusions
The constrained FJM system has strong coupling and nonlinearity. The performance would worsen when mismatched uncertainty arises. This research is on the basis of the frame built by Udwadia and Kalaba. The desired trajectory is not used to obtain the position error directly. It is treated as a constraint applied to the system and utilized to acquire the constraint force. A UKFE-based control is designed for the nominal portion of the system. No extra coordinate or Lagrange multiplier is needed other than its original ones. For the mismatched uncertainty in the system, the entire system can be divided into two cascaded subsystems by implanting a virtual control. An adaptive robust control with high-order terms is proposed to guarantee the uniform boundedness and uniform ultimate boundedness, even the information of uncertainty is poorly known. The system performance could be regulated easily by adjusting the order of the controller. Both theoretical analysis and simulation guarantees the effectiveness of the proposed control.