Robust Position/Force Control of Constrained Flexible Joint Robots with Constraint Uncertainties

A novel robust control method for simultaneous position/force control of constrained flexible joint robots is proposed. The facts that the uncertainties make the usual control task unsolvable and that the equations of the controlled system are differential-algebraic make the problem dealt with considerably demanding. In order to overcome the unsolvability problem due to the constraint uncertainties the position control task is redefined in a practical way such that only a suitable subgroup of the link positions are driven to their desired trajectories. To determine the elements of the subgroup a simple algorithm of practical relevance is proposed. Under certain smoothness conditions to the contact surfaces, it is demonstrated that the position control problem can dynamically be isolated from the force control. Thus, it becomes possible to handle the position and force control tasks separately. The most significant advantage of the separation of the position and force control tasks is that it makes possible to adapt the position control methods known from free robots. Each joint is used in either position control or force control. The proposed position controller has a cascaded structure: First, trajectories for joint positions that drive the link positions to their desired values are calculated. Then, the joint torques that drive the joint positions to their calculated values are determined. A further significant benefit of the separation of the position and force control tasks arises in the force control such that the transformed equations are linear and any linear robust control approach can be used for the force control. The whole controller requires the measurement of the link and joint positions, the link and joint velocities and the contact forces, and allows modeling uncertainties in the equations of both the robot dynamics and the contact surfaces. The proposed control method is also confirmed by simulations.


Introduction
In diverse robotic applications such as cleaning, assembling, writing, welding or surgery, end-effectors of robot manipulators have contact with their environments. Simultaneous control of the position of the end-effector and the force between the end-effector and the environment is usually mandatory to perform the task. Applicable position/force control algorithms are strongly needed and will further expand the application areas of robots. It can be estimated that robotic tasks will include more contacts with environment in more sophisticated future applications such as service robotics. Cumhur Baspinar cumhur.baspinar@hs-rm.de 1 Hochschule RheinMain, Postfach 3251, 65022, Wiesbaden, Germany If the contact surface can be modeled as a mass-springdamper system, the force control task can be transformed into a position control task, since in this case the contact forces are roughly proportional to the deformations of the contact surfaces and can be expressed as a function of the end-effector kinematic variables. In the literature, studies in this context are called impedance control [15][16][17][18][19][20][21]. Impedance control has been extensively studied since the 1980s [14].
Another widespread approach used for simultaneous position/force control is hybrid control, in which the position and force control tasks are separated from each other in the task space [11][12][13][22][23][24][25][26][27]. Hybrid control has been controversially and intensively discussed in the literature, since, on the one hand, it provides an attractive procedure for separation of position and force control tasks, on the other hand, it can cause unstable control loops. The criticism of hybrid control is focused on its kinematic nonconsistency [23]. The most challenging system model for control purposes emerges if the contact surfaces are rigid and the contact forces are expressed by Lagrange multipliers, since the resulting system model in this case has a differentialalgebraic form. Robots modeled by differential-algebraic equations are called constrained robots [28].
Although the issue of position/force control of rigid joint robots is well understood [1,[9][10][11][12], position/force control methods for flexible joint robots are rarely encountered in the literature. A straightforward application of the control methods from rigid joint robots to flexible joint case does not lead to a satisfactory performance. The significance of joint flexibility for robot control has widely been discussed [2][3][4][5][6]. The fact that the joint flexibility creates a natural compliance between the end-effector and the contact surfaces motivates the use of flexible joint robot manipulators for simultaneous position/force control.
In [7,8,16,18] and [23], a few rarely encountered works on position/force control of flexible joint robots are given.
The work [7] addresses adaptive position/force control of uncertain constrained flexible joint robots. The controller is based on singular perturbation approach and achieves the position tracking and the boundedness of the force errors. Unfortunately, the method cannot handle robots with strong joint elasticities and requires the measurement of the joint accelerations.
The position/force control law in [8] is based on solving the singular acceleration-level inverse dynamic equations. The controller does not require the measurement of the contact forces. However, unmeasured contact forces can reduce the performance of the force control in the case of modeling uncertainties.
In [16], a passivity based force and impedance controller for rigid and flexible joint robots is proposed, where the concept of energy tanks is applied such that the force tracking controller, the impedance controller and the motor dynamics together form a passive system. The approach is also capable to stabilize unwanted contact losses. It does not deal with the robustness issues which arise due to modeling uncertainties. The work suffers under the assumption that the time derivatives of the contact forces are obtainable.
In [18], a passivity based impedance controller for flexible joint robots is proposed, where the controller possesses a cascaded structure with an inner torque feedback loop and an outer impedance control loop. The controller contains a gravity compensation term and a PD control term and does not treat the modeling uncertainties.
The work [23] is based on the concept of singular perturbation theory, which works with the restrictive assumption that the dynamics of the controlled system can be divided into a fast and a slow subsystem. The controller for the fast subsystem contains an additional robustness term compensating the effects of the parameter mismatching. The unknown system parameters are estimated in real-time.
The present work proposes a novel robust control method for position/force control of constrained flexible joint robots. Its position control part has a cascaded structure: First, trajectories for joint positions that drive the link positions to their desired values are calculated. Then, the joint torques that drive the joint positions to their calculated values are determined. The phenomenon that the controlled system with respect to the force is linear, leads to the conclusion that any linear robust control approach can be used for the force control. The force control law in the simulations of Section 5 consists of a linear high gain PID term. The contributions of the work are given as follows: -To the best of the author's knowledge, this is the first work that allows modeling uncertainties in the contact surfaces. Dealing with the uncertainties in the contact surface is quite challenging, since the constraint uncertainties can make the position control task unfeasible [1]. For this purpose, the position control task is redefined in a pragmatic way in Section 2. -The present work allows not only parameter inaccuracies in the system model but also unmodelled dynamics, which is of considerable practical relevance. -The proposed control law does not require the measurement of the joint and link accelerations and the time-derivatives of the contact forces. It requires only the measurement of the link and joint angles, the link and joint velocities and the contact forces. -It is demonstrated that under certain smoothness conditions to the contact surfaces the position control can dynamically be isolated from the force control. This result is significant since it makes possible to handle the position and force control tasks separately and to adapt the position control methods known from free robots. Each joint is used in either position control or force control. A simple algorithm is developed to determine whether a joint can be used in position control or force control. Note that by contrast with the present work, the separation of the position and force control tasks in classical hybrid control is realized kinematically.
The present work is organized as follows: In Section 2, the dynamics of constrained flexible joint robots and the robust position/force control problem are introduced. In Section 3, the dynamic equations of the controlled system is first transformed into a new form that enables to treat the position control independently from the force control. Then, a robust position control law is developed. In Section 4, the linear force control law is derived. The success of the force control depends on the success of the position control, although the position control is independent of the force control. In Section 5, the proposed control method is also confirmed by simulations.

Problem Statement
Constrained flexible joint robots of the dynamics [3] are considered, where the vectors q ∈ R n , θ ∈ R n and τ ∈ R n denote the link angles, the motor angles and the motor torques at the joints, respectively. The symmetric and positive definite matrix M(q) ∈ R n×n denotes the mass matrix of the rigid links. The vector h(q,q) ∈ R n consists of the centrifugal, Coriolis and gravitational torques. The diagonal and positive definite matrices K ∈ R n×n , T ∈ R n×n and F ∈ R n×n denote the stiffness, inertia and viscous friction matrices of the joints, respectively. Equation 3 describes the contact surfaces of the end-effector in the link coordinates q, where φ(q) ∈ R m is a vector function with independent components except at some isolated values of q. Finally, the matrix J (q) ∈ R n×m and the vector λ ∈ R m denote the transpose of the Jacobian matrix of the constraints J (q) = (∂φ(q)/∂q) T and the Lagrange multipliers, respectively. The Lagrange multipliers λ can be interpreted as the constraint forces, since they are determined by the constraint forces and vice versa. It is assumed that the number of the constraints m is less than the number of the joints n.
The system model matrices and vectors are denoted byM(q),ĥ(q,q),K,T ,F ,φ(q) andĴ (q). They may differ from the relating system matrices and vectors M(q), h(q,q), K, T , F , φ(q) and J (q), which are supposed to be unknown, and therefore, not allowed to be used in the control law. The differences between the system and model components may arise due to both parameter mismatching and unmodeled (or neglected) parts of the system dynamics.
In the following, the arguments of the vector and matrix functions will often be neglected for the sake of brevity if they are obvious from the context. Assumption 1 is required by the proposed control law and satisfied by sufficiently smooth contact surfaces. Assumption 2 is required to separate the position control problem from the force control and is only slightly restrictive since the constraint surfaces are supposed to be independent of each other.
Let the subset α be α = {1, . . . , n} − σ . In Section 3, αrows of the system dynamics Eqs. 1 and 2 will be used for the position control, and in Section 4, σ -rows for the force control.
The control problem is given as follows: Specify the system inputs τ such that the system and controller variables remain bounded in time and q α and λ converge to their desired trajectories q d α (t) and λ d (t), respectively. Note that the remaining link coordinates q σ , which are not controlled, are not independent variables. For a given trajectory of q α , the trajectory of q σ is determined by the contact surfaces (3) and the initial conditions of (1) and (2) [1].

Position control
In the following, the α-rows of the system Eqs. 1 and 2 are first transformed into a structure which is more convenient for design of position controllers, then the robust position control law is given. The α-rows of Eqs. 1 and 2 are For separating the position control from the force control, the vectors λ andq σ will be eliminated from Eq. 4. The solution of the σ -rows of Eqs. 1 and 2 for λ is Substituting Eqs. 8 into 4 yields The vectorq σ depending on the vectors q,q andq α is obtained by differentiating the constraints Eq. 3 according to time twice [1]: The solution of Eq. 10 forq σ is Substituting (11) into (9) converts the α-rows of Eq. 1 into where D(q) and g(q,q, θ σ ) are defined by Equations 5 and 12 remind the dynamics of flexible joint robots without constraints, for which many control methods have been proposed so far [2]. In the following, the robust position control method for free flexible joint robots in [2] is adapted to the constrained case. LetD(q) andĝ(q,q, θ σ ) be the models of D(q) and g(q,q, θ σ ), respectively. Assumption 3 rank D(q) = rankD(q) = n − m along the trajectories.
Assumption 3 is fundamental since Eq. 12 may have infinitely large solutions for the components ofq α if Assumption 3 does not hold at some time instants. On the other hand, Assumption 3 is only slightly restrictive, since the matrices D(q) andD(q) can be influenced by the reasonable placement of the robot in relation to the contact surfaces.
The error variables regarding the position control are defined by where θ d α and β depend on t, q,q, θ andθ and denote the desired values of θ α andθ α , respectively. θ d α is given by where K 1 and K 2 are (n−m)×(n−m) diagonal and positive definite controller matrices, ε 1 is a positive real number, B 1 is a 2(n − m) × (n − m) matrix given by 1 is an upper bound function for the Euclidean norm of the unknown n − m dimensional vector 1 and P 1 is the 2(n − m) × 2(n − m) symmetric and positive definite solution of the following Lyapunov equation for a given 2(n − m) × 2(n − m) dimensional symmetric and positive definite matrix Q 1 The vector β is defined by The position control law is given by where K 4 is a (n − m) × (n − m) diagonal and positive definite matrix, ε 3 is a positive real number and¯ 3 is an upper bound function for the Euclidean norm of the unknown n − m dimensional vector 3 To demonstrate the efficiency of the position control law Eq. 24, the error dynamics regarding r 1 , e 3 and e 4 is required. Substituting Eqs. 16, 18 and the time derivative of Eqs. 14 into 12 yields The equalitẏ The last part of the error dynamics follows from the time derivative of Eq. 17 and Eqs. 5, 24 and 25 Equations 30, 33 and 36 constitute the error dynamics for the position control.
The time derivative of Eq. 37 iṡ Substituting Eqs. 21 and 30 into Eq. 38 yieldṡ Inequality Eq. 41 follows from the fact Substituting Eqs. 33 and 36 into Eq. 41 yieldṡ The Lyapunov function V decreases as long as the error variables satisfy the inequality Let S 1 denote the smallest level surface of the Lyapunov function V 1 including the ellipsoid The solutions of the error dynamics enter S 1 in a finite time and then remain in it.
The block diagram in Fig. 1 illustrates the structure of the position controller.

Force Control
Multiplying Eq. 6 by K σ σ converts Eq. 6 into Equations 7 and 44 (the σ -rows of the system dynamics) are used for the force control. For this purpose, a desired value for θ σ is first determined such that λ converges to λ d if θ σ converges to its desired value θ d σ . Then, the control inputs τ σ are given such that θ σ converges to θ d σ . Let e 5 and e 6 denote the force error and the position error regarding the σ components of the motor angles respectively. The desired value of θ σ is given by where K 5 an m × m dimensional diagonal and positive definite matrix. Substituting Eqs. 47 and 46 into Eq. 44 yields The solution of Eq. 48 for e 5 is (49) It is obvious from Eq. 49 that the absolute values of the components of e 5 can be made arbitrarily small at any time instant by means of the diagonal elements of K 5 if the components of e 6 remain bounded in time. It means that the success of the force control depends on the boundedness of e 6 . Thus, the force control task is reduced to a position control problem at which the position error e 6 has to be kept bounded. A linear controller keeping e 6 bounded can easily be determined by means of linear control theory since the controlled system (7) with the input τ σ and the output θ σ is linear. In this section, a linear controller for the linear system (7) is not given explicitly, since design of linear controllers is a well-known topic and does not contribute to the present work. In the following section, a simple PID controller is successfully used for this purpose in the simulations.
The block diagram in Fig. 2 illustrates the structure of the whole closed loop control system.

Simulations
The proposed robust control method has been simulated on a two link flexible joint robot manipulator (Fig. 3), whose equations of motion are where m 11 = c 1 + 2c 0 cos(q 2 ) Fig. 3 Two link constrained flexible joint robot with revolute joints whose physical system parameters are the masses of the links m 1 and m 2 , the inertia moments of the links i 1 and i 2 , the lengths of the links l 1 and l 2 , the distances of the centers of mass to the joints l c 1 and l c 2 , the distance of the contact surface from the base of the robot a, the stiffness constants of the flexible joints k 1 and k 2 , the inertia moments of the joints t 1 and t 2 and friction constants of the joints f 1 and f 2 k 1 , k 2 , t 1 , t 2 , f 1 and f 2 are the diagonal components of the matrices K, T and F respectively, l 1 and l 2 are the lengths of the first and second links, a is the distance of the contact surface from the base of the robot and the coefficients from c 0 to c 4 depend on the physical system parameters m 1 , m 2 , l 1 , l c 1 , l c 2 , g, i 1 and i 2 (see Fig. 3) as follows and c 5 and c 6 are the coefficients of the viscous friction at the links.
It is assumed that the model parametersm 1 ,m 2 ,î 1 ,î 2 , l 1 ,l 2 ,l c 1 ,l c 2 ,ĝ,k 1 ,k 2 ,t 1 ,t 2 ,f 1 ,f 2 andâ differ from the system parameters nearly ±10%. The first link has been used for the position control and the second link for the force control. Note that at this example, the first joint can be used just as well for the force control and the second joint for the position control. When choosing the joints it is only decisive that the resulting matrices J σ ,Ĵ σ (Assumption 2) D andD (Assumption 3) are not singular.
The desired trajectory of the position q 1 is sinusoidal and the desired trajectory of the contact force λ is piece-wise linear (Fig. 4). The force control has been performed by a simple PID controller with a realizable differentiator part. The parameters of the force controller have been selected such that the settling time of 2 sec is reached.
In Fig. 4, the time flows of q 1 , q d 1 , q 2 , θ 1 , θ d 1 , θ 2 , θ 2 , θ d 2 , λ, λ d , τ 1 and τ 2 for the control parameters K 1 = 8, K 2 = 16, K 3 = 10 and K 4 = 10, are seen. The chattering in the time flows of τ 2 and Q d 2 is on the one hand due to the numerical errors and on the other hand to the strong elasticity (k 1 = 10 and k 2 = 10) of the joints. Although the parameters of the linear controller have been optimized using linear control theory, the remaining parameters of the controller have been determined by trial and error. High controller parameters generally provide short settling times.
Simulation of differential-algebraic equations is in general much more complicated than simulation of ordinary differential equations. Fortunately, the differential-algebraic equations of constrained flexible joint robots (1)-(3) can be reduced to the ordinary differential Eqs. 2, 11 and 12. This property allows us to use the ordinary differential Eqs. 2, 11 and 12 for simulation purposes instead of (1)- (3). The fact that the distance of the wall from the robot base a remains constant during the simulation of the example system Eqs. 50-54 confirms the validity of the simulation method used at this work (Fig. 4). It should also be noticed that during simulations of the reduced Eqs. 2, 11 and 12, the initial values of q 1 , q 2 ,q 1 andq 2 cannot be chosen Fig. 4 Time trajectories of q 1 , q d 1 , q 2 , θ 1 , θ d 1 , θ 2 , θ d 2 , λ, λ d , τ 1 , τ 2 and the calculated distance of the wall from the robot base a during the simulation, respectively arbitrarily. They must satisfy the constraint Eq. 3 and its time derivative J Tq = 0.

Conclusions
A robust control method for the simultaneous position/force control of constrained flexible joint robots has been proposed. For this purpose, the system model has been first converted into a new form that allows to separate the position and force control tasks from each other and to group the joints: The joints in the set α are used at the position control and the joints in the set σ at the force control. The success of the force control is dependent on the success of the position control. Nevertheless, the success of the position control is independent on the success of the force control.
The applicability of the controller in practice seems to depend on overcoming the difficulty that the proposed control law requires upper bound functions for the modeling uncertainties. The necessity of such upper bound functions greatly increases the modeling effort.
Funding Information Open Access funding provided by Projekt DEAL.
Open Access This article is licensed under a Creative Commons Attribution 4.0 International License, which permits use, sharing, adaptation, distribution and reproduction in any medium or format, as long as you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons licence, and indicate if changes were made. The images or other third party material in this article are included in the article's Creative Commons licence, unless indicated otherwise in a credit line to the material. If material is not included in the article's Creative Commons licence and your intended use is not permitted by statutory regulation or exceeds the permitted use, you will need to obtain permission directly from the copyright holder. To view a copy of this licence, visit http:// creativecommonshorg/licenses/by/4.0/.