arXiv:1705.05727v1 [cs.RO] 13 May 2017 A General Scheme Implicit Force Control for a Flexible-Link Manipulator C. Murrugarra a,c,∗, O. De Castrob,c, J.C. Griecoc, G. Fernandezc aEl Bosque University, Electronic Enginering Program, Bogot´a D.C., Colombia. bSan Buenaventura University, Systems Engineering Program, Bogot´a D.C., Colombia. cSimon Bolivar University, Departament of Electronics and Circuits, Caracas, Venezuela Abstract In this paper we propose an implicit force control scheme for a one-link flexible manipulator that interact with a compliant environment. The controller was based in the mathematical model of the manipulator, considering the dynamics of the beam flexible and the gravitational force. With this method, the controller parameters are obtained from the structural parameters of the beam (link) of the manipulator. This controller ensure the stability based in the Lyapunov Theory. The controller proposed has two closed loops: the inner loop is a tracking control with gravitational force and vibration frequencies compensation and the outer loop is a implicit force control. To evaluate the performance of the controller, we have considered to three different manipulators (the length, the diameter were modified) and three environments with compliance modified. The results obtained from simulations verify the asymptotic tracking and regulated in position and force respectively and the vibrations suppression of the beam in a finite time. Keywords: Manipulator Flexible, Force Control, Modelling, Tracking Control, Vibrations, Flexible Structures ∗Corresponding author Email address: cmurrugarra@unbosque.edu.co (C. Murrugarra ) Preprint submitted to Journal of Nonlinear Analysis: Hybrid Systems September 20, 2018 1. Introduction In the feedback control theory two types of controllers can be identified: un- constrained and constrained controllers. The unconstrained controller is used when the end-effector is not in contact with the environment, for example in robotics: feedback control for regulated and tracking control for position and ve- 5 locity the end-effector respectively. The constrained controller is used when the end-effector is in contact with the environment, the force controller is classified inside constrained controller for robotics. The applications in control of force from manipulators have a combination of the two types of controllers, because is necessary first to localize the end-effector of the manipulator in the workspace 10 in a point desired and then regulate to the force desired. The following a review of the state of the art in force control. The hybrid con- troller proposed by Raibert & Craig in [1] and [2] is based on the workspace orthogonal decomposition in two subspaces: position control and force control. In [3] the system dynamics was included into the position-force controller. The 15 impedance control by Hogan [4] combines both, position and force signals used in the complete manipulator-environment interaction. Such controllers can be used when the manipulator is in contact with the environment and also when it’s not in contact with the environment. The explicit force control [5] uses a force- error to regulated the closed loop. The implicit force control uses a tracking 20 controller in stationary state to regulate the force applied to the environment. In this paper we propose a scheme of Implicit Force Control for a one-link flexi- ble manipulator, where the end-effector interacts with a compliant environment in the x −y plane or vertical plane. The control scheme has two closed loop controllers. The inner loop is a tracking controller with gravity and vibration 25 frequencies compensation. The outer loop is a implicit force controller. The scheme of force control this based on a dimensional finite mathematical model of the manipulator [6], [7]. This paper describes: 1) The mathematical model of the manipulator 2) The control scheme proposed 3) The stability analysis, 4) The results and analysis obtained and 5) Conclusions. 30 2 2. Mathematical model The dynamics has been modelled in the joint space, where the system is the one-link flexible manipulator with rigid rotational joint. The gravitational force and the constrained environment are considered in this model. 2.1. Assumptions 35 The development mathematical model was based in the following assump- tions: 1. The dynamics of the system has been obtained from the motion equation of Euler-Lagrange[8]. 2. The links were modelled as a beam Euler-Bernoulli (EB)[9]. 40 3. The transversal deformation was calculated in any point of the beam using the modes-assumed method. 4. The clamped-free beam as conditions of boundary of the beam. M(℘) ¨℘+ C(℘, ˙℘) ˙℘+ g(℘) + η(℘) = τ −τe (1) The equation of motion of the system when the manipulator is in contact with the environment is define by equation (1), where M(℘) the Inertia Matrix, 45 C(℘, ˙℘) the Coriolis Matrix, g(℘) the gravitational force vector, η(℘) the vibra- tions frequency of the beam vector, τ torque vector and τe ∈Rnx1 the torque vector, caused by the environment as a reaction force when the end-effector apply a force on environment. The Fig. 1 show it the manipulator in contact with environment. 50 2.2. Equation We show the mathematical expressions of the dynamic nonlinear model in spaces states of the manipulator. The equation (2), represents the evolution of the system in the time, from L = Pn i Ki−Pn i Vi, where Ki is the kinetic energy for each link, Li is the potential energy for each link, and ℘is the generalized 55 coordinates of system corresponding to modes of vibrations of the beam. In 3 y0 F w(x,t) θ(t) Fd Fdx Fdy y1 Entorno x1 x0 Ke Figure 1: Flexible manipulator and compliant environment. this case ℘= [θi qij], where θi is the rotation angle in the articulation and qij is the generalized coordinate associated to temporal of modes or vibration frequencies, i is the number associated of the link, j is the number of modes of vibration flexible link, and Qi is the generalized force for each d.o.f of the 60 system. d dt  ∂L d ˙℘i  −∂L ∂℘i = Qi (2) We have considered the planar position of the manipulator, the equation (3) define the position end-effector of the manipulator, considering the deformation of the beam in the end-effector. The kinetic energy are represent by equations (4) and (5), where: A, ρ, Ib and l, are cross-sectional area, uniform mass density, 65 inertia and length of link respectively of the beam (link). P  ˆx y   T =  ˆxcos(θi(t)) −wi(ˆx, t)sin(θi(t)) ˆxsin(θi(t)) + wi(ˆx, t)cos(θi(t))   T (3) Ki = 1 2 Z l1 0 ˙P(ˆx)T ˙P(ˆx)dm (4) Ki = 1 2 ˙θ2 i Ibi + 1 2ρiAi ˙θ2 i Z li 0 w2 i (ˆx, t)dˆx + 1 2ρiAi Z li 0 ˙w2 i (ˆx, t)dˆx + ρiAi ˙θi Z li 0 ˆx ˙wi(ˆx, t)dˆx (5) 4 The potential energy (V) has two components, the component associated to the gravitational force (Vg) and the component associated to the beam deformation (Ve). The equations (6)-(9) represent the potencial energy of the system. 70 V = Vg + Ve (6) Vg = − Z li 0 gT Pdm (7) Ve = 1 2EI Z li 0 [w” i (ˆx, t)]2dˆx (8) V = ρiAig l2 1 2 sin(θi) + ρiAigcos(θi) Z li 0 w1(ˆx, t)dˆx +1 2EI Z li 0 [w” i (ˆx, t)]2dˆx (9) From (5) and (9) and replace in (2) for one link i.e. i = 1, and using the separa- bility principle [10], for wi(ˆx, t), the equation of motion might be obtained, fur- thermore expanded wi(ˆx, t) = Pν j=1 φij(ˆx)qij(t) for 2 modes (ν = 2), and rep- resenting the equation of the system by state variables (10), where x is the state 75 vector and expanding for describe variables x = [θ1 ˙θ1 q11 ˙q11 q12 ˙q12]T , and the control vector u = τ −τe. ˙x = f(x) + ̺(x)u y = h(x) (10) The equation (11), representing the mathematical model in state variables, where the constants was defined by: b0 = a0ρ1A1, b1 = ρ1A1 a2 1 a0 , b2 = ρ1A1a1, b3 = a1 a0 b4 = l2 1 2 ρ1A1, b5 = a2ρ1A1 b6 = EIa3, b7 = a2 a0 b8 = Ib1 and a0 = 80 R l1 0 φ11(ˆx)2dˆx, a1 = R l1 0 φ11(ˆx)ˆxdˆx, a2 = R l1 0 φ11(ˆx)dˆx, a3 = R l1 0 h dφ2 11ˆx dˆx2 i2 dˆx, is important remarking that the equation (11) can be expanded for n modes of vibrations, for details see [6]. ˙x1 = x2 ˙x2 =  τ −x3[2b01x2x4 + b21(x2 2 + b71gc1) −b31b61 + b4gc1 + b51gs1] −x5[x2(2b02x6 + b22x2) + 5 b62b32 + b52gs1] + b22b72gc1  b01x2 3 + b11 + b02x2 5 + b12 + b8 −1 ˙x3 = x4 ˙x4 =  x2 2x3 −b31 ˙x2 −b71gc1 −b61 b01 x3  ˙x5 = x6 ˙x6 =  x2 2x5 −b32 ˙x2 −b72gc1 −b62 b02 x5  (11) 3. Control Scheme 85 We propose a control scheme with two closed-loop. The Fig. 2 show this scheme. The inner loop is a tracking control with gravity and vibration frequen- cies compensation. The outer loop is a force controller. The control scheme force transforms the force error into a position difference in the components planar x and y. This difference is added to the reference of the tracking controller in- 90 ner loop. The environment has been modelled as a deformable environment or compliant surface. When the manipulator makes contact with the environment a reaction force is generated Fc = (Fcx, Fcy) and this components are feedback to the force controller. Force Control Inverse Kinematics PD Control Flexible Manipulator Environment JT g(q) h(q) S S S S Fd Fc P = ( X ,Y ) d d d Figure 2: Implicit Force Controller for the flexible-link manipulator. 6 4. Tracking Control Loop 95 We propose a theorem that ensures the global asymptotic stability of the PD (Proportional-Derivative) tracking controller, with both gravity and vibra- tion frequencies compensation on manipulator. The theorem (1), calculate the parameters of tuning of the tracking controller based on the dynamics of the manipulator. This parameters only depend of the structure of the beam and 100 the spacial configuration of the manipulator. Theorem 1. Consider the nonlinear open-loop system ˙x = f(x)+̺(x)u, repre- senting the rotation joint of the flexible-link manipulator, with x −y workspace, under gravity influence. In order to define a closed-loop tracking controller using the control law: 105 u = Kp ˜℘+ Kv ˙˜℘+ M(℘)[ ¨℘d + ∆˙˜℘] + C(℘, ˙℘)[ ˙℘d + ∆˜℘] + g(℘) + η(℘) (12) assuming that ℘d and ˙℘d as a set of vector functions, ¨℘d is a constant and the closed-loop equation system-controller is non autonomous in the space then it can be assured that existence an unique equilibrium point, located at the origin and with global asymptotic stability for Kp and Kv > 0, where ˜℘and ˙˜℘are position velocity error vectors. The Kp and Kv, are the proportional and derivative 110 matrices and must be symmetric and positive, furthermore ∆= K−1 v Kp must be a nonsingular matrix. Proof. 1 Let the desired reference position ℘d for the controller be a feasible trajectory, defined in the manipulator workspace and the feedback control law given by the expression (13): 115 u = Kp ˜℘+ Kv ˙˜℘+ M(℘)[ ¨℘d + ∆˙˜℘] + C(℘, ˙℘)[ ˙℘d + ∆˜℘] + g(℘) + η(℘) (13) where Kp and Kv ∈Rnxn are symmetric and positive definite matrices and ∆= K−1 v Kp is a nonsingular matrix. Rewriting the control law (13) in functions of the [ ˜℘T ˙˜℘], obtained the equation controller (14) d dt  ˜℘ ˙˜℘  =   ˙˜℘ h −Kp ˜℘−Kv ˙˜℘−C(℘, ˙℘)[ ˙˜℘+∆˜℘] i M(℘)   (14) 7 where (14) is a non autonomous differential equation, with an equilibrium point in the origin [ ˜℘T ˙˜℘] = 0 ε R2n. 120 The equation (15) will be used as the Lyapunov candidate function, this function is defined from dynamics of the manipulator. _ (t, ˜℘, ˙˜℘) = [ ˙˜℘+ ∆˜℘]T M(℘)[ ˙˜℘+ ∆˜℘] + ˜℘T Kp ˜℘ (15) According to statement of Robotics Theory, the inertia matrix M(℘) is sym- metric and positive definite, and by definition Kp is also symmetric and positive definite [11], it can be assured that W is also globally positive definite. Replacing 125 Kp = Kv∆in [ ˙˜℘+ ∆˜℘]T M(℘) [12], we obtain: ˙_ (t, ˜℘, ˙˜℘) = −˙˜℘ T Kv ˙˜℘−˜℘T ∆T Kv∆˜℘ (16) Given that by definition Kv is symmetric and positive definite and ∆is a nonsin- gular matrix, their product is positive definite, proving that ˙W < 0 is a globally negative definite matrix. We can conclude that the system has global asymp- totic stability in the theorem (1), for any symmetric positive definite matrix Kp 130 and Kv. 5. Force Controller 5.1. Force-Torque In order to write (1), we can suppose that the manipulator end-effector is in contact with environment. In other hand, applying the virtual work princi- 135 ple [13], we can consider that the forces vector applied by the manipulator on the environment can be associated with the Jacobian (17), obtaining a finite dimensional model when the manipulator is it contact with the environment. τe = J(℘)T fc (17) where the J(℘), is the Jacobian Matrix, that associates the velocity vector in the joint ˙℘with the velocity in the end-effector. In other words, a transformation 140 from angular space to cartesian space. The Jacobian used in our expressions 8 have been calculated directly of the end-effector position in cartesian space P(x, y), considering the transversal deformations of the beam and fc, as the contact force. J(℘) = ∂P(x, y) ∂℘ (18) Since P(x, y) is written in terms of (θi, qij), the kinematics velocity equation for 145 the end-effector will be: ˙P(x, y) = Jθ1(q) ˙θ1 + Jq11(q) ˙q11 + . . . + Jq1ν(q) ˙q1ν (19) The mathematical model for the flexible manipulator has been developed for two vibration modes (ν = 2), with the resulting Jacobian Matrix is: ˙P(x, y) = Jθ1(q) ˙θ1 + Jq11(q) ˙q11 + Jq1ν(q) ˙q12 (20) J(℘) =   −l1s1 −w1c1 −φ11(l1)s1 −φ12(l1)s1 −l1c1 −w1s1 φ11(l1)c1 φ12(l1)c1 0 0 0 0 0 0 0 0 0 1 1 1   (21) 5.2. Environment Model 150 The end-effector/contact-surface interaction is very difficult to model. In this case the environment has been modelled as a compliant environment without friction. fc = Ke h P(x, y) −P0(x, y) i (22) The contact force fc have been representing as a position difference between end-effector P(x, y) and contact point P0(x, y) more a Ke, that represent the 155 environment stiffness coefficient. Since we consider a compliant environment, can be represent as a constant symmetric positive definite matrix. 9 5.3. Control Law The implicit force control scheme was constructed as the outer loop that asso- ciates the contact force (22) with a position-velocity vector so that the difference 160 between the desired force (fd) and the contact force (fc) can be translated to a position and velocity difference ∆P(x, y) and ∆˙P(x, y) respectively, adding the last one to the reference signals of the tracking controller inner loop. In [14] an implicit force regulation scheme is proposed, with an outer force control PI, where the control law is given by (25), being the controller proportional 165 contribution. ˙Pd(x, y) = −K−1 e kf Z t 0 f(τ)dτ (23) ˙Pd(x, y) = −k∆f(t) = k[fc(t) −fd] (24) Where k = K−1 e kf, and integrating in time the position reference for the force control law: Pd(x, y) = −K−1 e kf Z t 0 ∆f(τ)dτ (25) Integrator Fd Fc K − q + −K q − Figure 3: Force/position-velocity relationship scheme control 5.4. Stability 170 In order to ensure the controller stability, we have defined our force controller based on (23) and (24), defining the velocity and position of the outer loop . Considering the desired force fd as a constant, the controller (23) ensure an 10 asymptotically exact regulation, while the inner loop provides an asymptotically exact tracking. The inner velocity loops with bounded errors can be seen as: 175 lim t→∞sup∥∆f(t)∥≤1 kf S (26) If limt→∞sup∥∆˙x(t)∥≤S and S ǫ Rn, then 0 ≤S ≤∞and kf > 0 lim t→∞sup∥∆˙x(t)∥≤S (27) 6. Results and analysis The proposed control scheme was tested using MatLab and Simulink. The simulations have been constructed so the manipulators end-effector was located in P(x, y) = (0, 0)m as the initial position. The environment was located in 180 Pe(x, y) = (0.7071, 0.7071)m, therefore the manipulator will be, at first, under the tracking controller action, until the end-effector makes contact with the en- vironment. Once the contact is made, the force controller is activated. The simulations results are show it in the Fig. 4 and Fig. 5 for the one-link flexible manipulator. The parameters physical of the beam were: 1m. long aluminum 185 beam with cross section diameter 10−3m, inertia Im = 1.3254−06Kgm2, elastic- ity coefficient EI = 34.3612Nm2, modes-shapes ν = 2. The parameters of the tracking controller are Kp = diag[160 100 100], Kv = diag[30 1 0.5]. For the model of the environment Ke = 86.9N/m as stiffness coefficient of the envi- ronment. The force desired is fd = 5N. These results show that the end-effector 190 reaches the final position desired applying to the environment the desired force. We also can observe that transversal deformations in stationary state converge to error = 0. Furthermore, in the Fig. 6, we are presenting as additional results the rela- tion position-force, with three environments (Ke1 = 20N/m, Ke2 = 86.4N/m, 195 Ke3 = 200N/m), this results proof as the environment is deform it, when the manipulator apply a force (verify the environment mathematical model), visu- alizing this deformation and parallel at this the convergence the both control loops (errorposition and errorforce →0, in a finite time). 11 (a) (b) 0.65 0.7 0.75 0.8 0.85 0.9 0.95 1 1.05 0 1 2 3 4 5 6 7 8 9 10 m. t x_exp vs t xf_ref vs t 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0 1 2 3 4 5 6 7 8 9 10 m. t yend_exp Vs t yf_ref vs t (c) (d) 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 0 1 2 3 4 5 6 7 8 9 10 rad. t theta_exp vs t thetap_ref vs t -0.02 -0.015 -0.01 -0.005 0 0.005 0.01 0 1 2 3 4 5 6 7 8 9 10 m. t w(x,t) vs t (e) (f) -0.025 -0.02 -0.015 -0.01 -0.005 0 0.005 0.01 0 1 2 3 4 5 6 7 8 9 10 m. t q11 vs t -0.0002 -0.00015 -0.0001 -5e-005 0 5e-005 0 1 2 3 4 5 6 7 8 9 10 m. t q12 vs t Figure 4: Results of the Implicit Force Controller: Environment = 86.9N/m, fd = 5N.(a) Cartesian position end-effector in the axis x. (b) Cartesian position end-effector in the axis y (c) Joint Position θ(t). (d) Transversal deformation w(ˆx, t). (e) First frequency of deformation E-B beam. (f) Second frequency of deformation E-B beam. 12 0 1 2 3 4 5 6 0 1 2 3 4 5 6 7 8 9 10 N. t fd vs t fc vs t (a) -4 -3.5 -3 -2.5 -2 -1.5 -1 -0.5 0 0.5 0 1 2 3 4 5 6 7 8 9 10 N. t fdx vs t fcx vs t (b) -0.5 0 0.5 1 1.5 2 2.5 3 3.5 4 0 1 2 3 4 5 6 7 8 9 10 N. t fdy vs t fcy vs t (c) Figure 5: Results of the Implicit Force Controller: Environment = 86.9N/m, fd = 5N. (a) Total Force applied to environment. (b) Force component Fx applied to the environment. (c) Force component Fy applied to the environment. 13 -5 -4 -3 -2 -1 0 0.45 0.5 0.55 0.6 0.65 0.7 fx (N) x (m) Ke=20N/m Ke=86.9N/m Ke=200N/m 0 1 2 3 4 5 0.7 0.75 0.8 0.85 0.9 fy (N) y (m) Ke=20N/m Ke=86.9N/m Ke=200N/m Figure 6: Relation Force-Position for enviroments with compliance variable. 7. Conclusion 200 This paper proposes a general method for implement a Implicit Force Con- troller based on dynamics of the manipulator. With the control scheme pro- posed is possible that the manipulator realize two works, considering indirectly the effect of the impact. The stability analysis was based on Lyapunov The- ory, ensure the global asymptotic stability of the control scheme by obtaining 205 a unique equilibrium point for controller constants Kp and Kv, considering the compensation of the gravitational force and vibrational frequencies of the beam. Furthermore this method, can be used to prove of the resistance of materials, because is possible know the limits of the system (beam) associated with the vibrations amplitudes, before the deform completely when have been applied 210 a reference force hight or when the environment is less compliant. The results were satisfactory and validate the proposed controller. Acknowledgments The authors thanks for the financial support provided by the El Bosque Uni- versity, Electronic Enginering Program, San Buenaventura University, Systems 215 Engineering Program and University Simon Bol´ıvar, Electronics and Circuits Department. 14 References [1] M. Raibert, J. Craig, Hybrid position-force control of manipulators., trans- actions of ASME, Journal of Dynamic Systems, Measurement and Con- 220 trol (102) (1981) 126–133. [2] T. Yoshikawa, Dynamics hybrid position/force control of robot manipulators-description of hand constraints and calculation of joints driv- ing forces, In proceedings IEEE, Conference Robotics Automation (1986) 1393–1398. 225 [3] T. Yoshikawa, Dynamics hybrid position/force control of manipulators de- scription of hand constraints and calculation of joint driving force, IEEE Journal of Robotics and Automation Ra-3 (5) (1987) 386–392. [4] N. Hogan, Impedance control: an approach to manipulation: Theory, im- plementation, application, Journal of dynamic Systems, Measurement and 230 Control,ASME 107. [5] D. Whitney, Historical perspective ans state of the art in robot force control, The Internartional Journal of Robotics Research 6 (1) (1987) 3–14. [6] C. Murrugarra, J. Grieco, G. Fernandez, M. Armada, Modelling of a 3d flexible manipulator: A generalized equation for n vibration frequencies, 235 Measurement and Control in Robotics 1 (1) (2003) 199–204. [7] C. Murrugarra, J. Grieco, G. Fernandez, O. De Castro, A generalized math- ematical model for flexible link manipulators with n vibration frequencies and friction in the joint, ISRA 2006, International Symposium on Robotics and Automation 1 (1) (2006) 23–28. 240 [8] M. Spong, S. Hutchinson, M. Vidyasagar, Robot Modeling and Control, Wiley, 2005. URL https://books.google.com.co/books?id=wGapQAAACAAJ 15 [9] W. Thomson, Theory of Vibration with Applications, 5th Edition, Prentice Hall, USA, 1993. 245 [10] S. Moorehead, Position and force control of flexible manipulators, Master’s thesis, University of Waterloo, Waterloo,Ontario, Canada (1996). [11] C. Murrugarra, J. Grieco, G. Fernandez, O. De Castro, Design of a pd position control based on the lyapunov theory for a robot manipulator flexible-link, Proceedings IEEE International Conference on Robotics and 250 Biomimetics ROBIO 2006. 1 (1). [12] R. Kelly, V. Santib´a˜nez, Control de Movimiento de Robots Manipuladores., Pearson Prentice Hall, Spain, 2003. [13] L. Meirovitch, Dynamics and Control of Structures, Wiley Interscience Publication, USA, 1990. 255 [14] J. De-Shutter, H. Van-Brussel, Compliant robot motion ii. a control ap- proach based on external control loops, The Internartional Journal of Robotics Research 7 (4) (1988) 18–33. 16 arXiv:1705.05727v1 [cs.RO] 13 May 2017 A General Scheme Implicit Force Control for a Flexible-Link Manipulator C. Murrugarra a,c,∗, O. De Castrob,c, J.C. Griecoc, G. Fernandezc aEl Bosque University, Electronic Enginering Program, Bogot´a D.C., Colombia. bSan Buenaventura University, Systems Engineering Program, Bogot´a D.C., Colombia. cSimon Bolivar University, Departament of Electronics and Circuits, Caracas, Venezuela Abstract In this paper we propose an implicit force control scheme for a one-link flexible manipulator that interact with a compliant environment. The controller was based in the mathematical model of the manipulator, considering the dynamics of the beam flexible and the gravitational force. With this method, the controller parameters are obtained from the structural parameters of the beam (link) of the manipulator. This controller ensure the stability based in the Lyapunov Theory. The controller proposed has two closed loops: the inner loop is a tracking control with gravitational force and vibration frequencies compensation and the outer loop is a implicit force control. To evaluate the performance of the controller, we have considered to three different manipulators (the length, the diameter were modified) and three environments with compliance modified. The results obtained from simulations verify the asymptotic tracking and regulated in position and force respectively and the vibrations suppression of the beam in a finite time. Keywords: Manipulator Flexible, Force Control, Modelling, Tracking Control, Vibrations, Flexible Structures ∗Corresponding author Email address: cmurrugarra@unbosque.edu.co (C. Murrugarra ) Preprint submitted to Journal of Nonlinear Analysis: Hybrid Systems September 20, 2018 1. Introduction In the feedback control theory two types of controllers can be identified: un- constrained and constrained controllers. The unconstrained controller is used when the end-effector is not in contact with the environment, for example in robotics: feedback control for regulated and tracking control for position and ve- 5 locity the end-effector respectively. The constrained controller is used when the end-effector is in contact with the environment, the force controller is classified inside constrained controller for robotics. The applications in control of force from manipulators have a combination of the two types of controllers, because is necessary first to localize the end-effector of the manipulator in the workspace 10 in a point desired and then regulate to the force desired. The following a review of the state of the art in force control. The hybrid con- troller proposed by Raibert & Craig in [1] and [2] is based on the workspace orthogonal decomposition in two subspaces: position control and force control. In [3] the system dynamics was included into the position-force controller. The 15 impedance control by Hogan [4] combines both, position and force signals used in the complete manipulator-environment interaction. Such controllers can be used when the manipulator is in contact with the environment and also when it’s not in contact with the environment. The explicit force control [5] uses a force- error to regulated the closed loop. The implicit force control uses a tracking 20 controller in stationary state to regulate the force applied to the environment. In this paper we propose a scheme of Implicit Force Control for a one-link flexi- ble manipulator, where the end-effector interacts with a compliant environment in the x −y plane or vertical plane. The control scheme has two closed loop controllers. The inner loop is a tracking controller with gravity and vibration 25 frequencies compensation. The outer loop is a implicit force controller. The scheme of force control this based on a dimensional finite mathematical model of the manipulator [6], [7]. This paper describes: 1) The mathematical model of the manipulator 2) The control scheme proposed 3) The stability analysis, 4) The results and analysis obtained and 5) Conclusions. 30 2 2. Mathematical model The dynamics has been modelled in the joint space, where the system is the one-link flexible manipulator with rigid rotational joint. The gravitational force and the constrained environment are considered in this model. 2.1. Assumptions 35 The development mathematical model was based in the following assump- tions: 1. The dynamics of the system has been obtained from the motion equation of Euler-Lagrange[8]. 2. The links were modelled as a beam Euler-Bernoulli (EB)[9]. 40 3. The transversal deformation was calculated in any point of the beam using the modes-assumed method. 4. The clamped-free beam as conditions of boundary of the beam. M(℘) ¨℘+ C(℘, ˙℘) ˙℘+ g(℘) + η(℘) = τ −τe (1) The equation of motion of the system when the manipulator is in contact with the environment is define by equation (1), where M(℘) the Inertia Matrix, 45 C(℘, ˙℘) the Coriolis Matrix, g(℘) the gravitational force vector, η(℘) the vibra- tions frequency of the beam vector, τ torque vector and τe ∈Rnx1 the torque vector, caused by the environment as a reaction force when the end-effector apply a force on environment. The Fig. 1 show it the manipulator in contact with environment. 50 2.2. Equation We show the mathematical expressions of the dynamic nonlinear model in spaces states of the manipulator. The equation (2), represents the evolution of the system in the time, from L = Pn i Ki−Pn i Vi, where Ki is the kinetic energy for each link, Li is the potential energy for each link, and ℘is the generalized 55 coordinates of system corresponding to modes of vibrations of the beam. In 3 y0 F w(x,t) θ(t) Fd Fdx Fdy y1 Entorno x1 x0 Ke Figure 1: Flexible manipulator and compliant environment. this case ℘= [θi qij], where θi is the rotation angle in the articulation and qij is the generalized coordinate associated to temporal of modes or vibration frequencies, i is the number associated of the link, j is the number of modes of vibration flexible link, and Qi is the generalized force for each d.o.f of the 60 system. d dt  ∂L d ˙℘i  −∂L ∂℘i = Qi (2) We have considered the planar position of the manipulator, the equation (3) define the position end-effector of the manipulator, considering the deformation of the beam in the end-effector. The kinetic energy are represent by equations (4) and (5), where: A, ρ, Ib and l, are cross-sectional area, uniform mass density, 65 inertia and length of link respectively of the beam (link). P  ˆx y   T =  ˆxcos(θi(t)) −wi(ˆx, t)sin(θi(t)) ˆxsin(θi(t)) + wi(ˆx, t)cos(θi(t))   T (3) Ki = 1 2 Z l1 0 ˙P(ˆx)T ˙P(ˆx)dm (4) Ki = 1 2 ˙θ2 i Ibi + 1 2ρiAi ˙θ2 i Z li 0 w2 i (ˆx, t)dˆx + 1 2ρiAi Z li 0 ˙w2 i (ˆx, t)dˆx + ρiAi ˙θi Z li 0 ˆx ˙wi(ˆx, t)dˆx (5) 4 The potential energy (V) has two components, the component associated to the gravitational force (Vg) and the component associated to the beam deformation (Ve). The equations (6)-(9) represent the potencial energy of the system. 70 V = Vg + Ve (6) Vg = − Z li 0 gT Pdm (7) Ve = 1 2EI Z li 0 [w” i (ˆx, t)]2dˆx (8) V = ρiAig l2 1 2 sin(θi) + ρiAigcos(θi) Z li 0 w1(ˆx, t)dˆx +1 2EI Z li 0 [w” i (ˆx, t)]2dˆx (9) From (5) and (9) and replace in (2) for one link i.e. i = 1, and using the separa- bility principle [10], for wi(ˆx, t), the equation of motion might be obtained, fur- thermore expanded wi(ˆx, t) = Pν j=1 φij(ˆx)qij(t) for 2 modes (ν = 2), and rep- resenting the equation of the system by state variables (10), where x is the state 75 vector and expanding for describe variables x = [θ1 ˙θ1 q11 ˙q11 q12 ˙q12]T , and the control vector u = τ −τe. ˙x = f(x) + ̺(x)u y = h(x) (10) The equation (11), representing the mathematical model in state variables, where the constants was defined by: b0 = a0ρ1A1, b1 = ρ1A1 a2 1 a0 , b2 = ρ1A1a1, b3 = a1 a0 b4 = l2 1 2 ρ1A1, b5 = a2ρ1A1 b6 = EIa3, b7 = a2 a0 b8 = Ib1 and a0 = 80 R l1 0 φ11(ˆx)2dˆx, a1 = R l1 0 φ11(ˆx)ˆxdˆx, a2 = R l1 0 φ11(ˆx)dˆx, a3 = R l1 0 h dφ2 11ˆx dˆx2 i2 dˆx, is important remarking that the equation (11) can be expanded for n modes of vibrations, for details see [6]. ˙x1 = x2 ˙x2 =  τ −x3[2b01x2x4 + b21(x2 2 + b71gc1) −b31b61 + b4gc1 + b51gs1] −x5[x2(2b02x6 + b22x2) + 5 b62b32 + b52gs1] + b22b72gc1  b01x2 3 + b11 + b02x2 5 + b12 + b8 −1 ˙x3 = x4 ˙x4 =  x2 2x3 −b31 ˙x2 −b71gc1 −b61 b01 x3  ˙x5 = x6 ˙x6 =  x2 2x5 −b32 ˙x2 −b72gc1 −b62 b02 x5  (11) 3. Control Scheme 85 We propose a control scheme with two closed-loop. The Fig. 2 show this scheme. The inner loop is a tracking control with gravity and vibration frequen- cies compensation. The outer loop is a force controller. The control scheme force transforms the force error into a position difference in the components planar x and y. This difference is added to the reference of the tracking controller in- 90 ner loop. The environment has been modelled as a deformable environment or compliant surface. When the manipulator makes contact with the environment a reaction force is generated Fc = (Fcx, Fcy) and this components are feedback to the force controller. Force Control Inverse Kinematics PD Control Flexible Manipulator Environment JT g(q) h(q) S S S S Fd Fc P = ( X ,Y ) d d d Figure 2: Implicit Force Controller for the flexible-link manipulator. 6 4. Tracking Control Loop 95 We propose a theorem that ensures the global asymptotic stability of the PD (Proportional-Derivative) tracking controller, with both gravity and vibra- tion frequencies compensation on manipulator. The theorem (1), calculate the parameters of tuning of the tracking controller based on the dynamics of the manipulator. This parameters only depend of the structure of the beam and 100 the spacial configuration of the manipulator. Theorem 1. Consider the nonlinear open-loop system ˙x = f(x)+̺(x)u, repre- senting the rotation joint of the flexible-link manipulator, with x −y workspace, under gravity influence. In order to define a closed-loop tracking controller using the control law: 105 u = Kp ˜℘+ Kv ˙˜℘+ M(℘)[ ¨℘d + ∆˙˜℘] + C(℘, ˙℘)[ ˙℘d + ∆˜℘] + g(℘) + η(℘) (12) assuming that ℘d and ˙℘d as a set of vector functions, ¨℘d is a constant and the closed-loop equation system-controller is non autonomous in the space then it can be assured that existence an unique equilibrium point, located at the origin and with global asymptotic stability for Kp and Kv > 0, where ˜℘and ˙˜℘are position velocity error vectors. The Kp and Kv, are the proportional and derivative 110 matrices and must be symmetric and positive, furthermore ∆= K−1 v Kp must be a nonsingular matrix. Proof. 1 Let the desired reference position ℘d for the controller be a feasible trajectory, defined in the manipulator workspace and the feedback control law given by the expression (13): 115 u = Kp ˜℘+ Kv ˙˜℘+ M(℘)[ ¨℘d + ∆˙˜℘] + C(℘, ˙℘)[ ˙℘d + ∆˜℘] + g(℘) + η(℘) (13) where Kp and Kv ∈Rnxn are symmetric and positive definite matrices and ∆= K−1 v Kp is a nonsingular matrix. Rewriting the control law (13) in functions of the [ ˜℘T ˙˜℘], obtained the equation controller (14) d dt  ˜℘ ˙˜℘  =   ˙˜℘ h −Kp ˜℘−Kv ˙˜℘−C(℘, ˙℘)[ ˙˜℘+∆˜℘] i M(℘)   (14) 7 where (14) is a non autonomous differential equation, with an equilibrium point in the origin [ ˜℘T ˙˜℘] = 0 ε R2n. 120 The equation (15) will be used as the Lyapunov candidate function, this function is defined from dynamics of the manipulator. _ (t, ˜℘, ˙˜℘) = [ ˙˜℘+ ∆˜℘]T M(℘)[ ˙˜℘+ ∆˜℘] + ˜℘T Kp ˜℘ (15) According to statement of Robotics Theory, the inertia matrix M(℘) is sym- metric and positive definite, and by definition Kp is also symmetric and positive definite [11], it can be assured that W is also globally positive definite. Replacing 125 Kp = Kv∆in [ ˙˜℘+ ∆˜℘]T M(℘) [12], we obtain: ˙_ (t, ˜℘, ˙˜℘) = −˙˜℘ T Kv ˙˜℘−˜℘T ∆T Kv∆˜℘ (16) Given that by definition Kv is symmetric and positive definite and ∆is a nonsin- gular matrix, their product is positive definite, proving that ˙W < 0 is a globally negative definite matrix. We can conclude that the system has global asymp- totic stability in the theorem (1), for any symmetric positive definite matrix Kp 130 and Kv. 5. Force Controller 5.1. Force-Torque In order to write (1), we can suppose that the manipulator end-effector is in contact with environment. In other hand, applying the virtual work princi- 135 ple [13], we can consider that the forces vector applied by the manipulator on the environment can be associated with the Jacobian (17), obtaining a finite dimensional model when the manipulator is it contact with the environment. τe = J(℘)T fc (17) where the J(℘), is the Jacobian Matrix, that associates the velocity vector in the joint ˙℘with the velocity in the end-effector. In other words, a transformation 140 from angular space to cartesian space. The Jacobian used in our expressions 8 have been calculated directly of the end-effector position in cartesian space P(x, y), considering the transversal deformations of the beam and fc, as the contact force. J(℘) = ∂P(x, y) ∂℘ (18) Since P(x, y) is written in terms of (θi, qij), the kinematics velocity equation for 145 the end-effector will be: ˙P(x, y) = Jθ1(q) ˙θ1 + Jq11(q) ˙q11 + . . . + Jq1ν(q) ˙q1ν (19) The mathematical model for the flexible manipulator has been developed for two vibration modes (ν = 2), with the resulting Jacobian Matrix is: ˙P(x, y) = Jθ1(q) ˙θ1 + Jq11(q) ˙q11 + Jq1ν(q) ˙q12 (20) J(℘) =   −l1s1 −w1c1 −φ11(l1)s1 −φ12(l1)s1 −l1c1 −w1s1 φ11(l1)c1 φ12(l1)c1 0 0 0 0 0 0 0 0 0 1 1 1   (21) 5.2. Environment Model 150 The end-effector/contact-surface interaction is very difficult to model. In this case the environment has been modelled as a compliant environment without friction. fc = Ke h P(x, y) −P0(x, y) i (22) The contact force fc have been representing as a position difference between end-effector P(x, y) and contact point P0(x, y) more a Ke, that represent the 155 environment stiffness coefficient. Since we consider a compliant environment, can be represent as a constant symmetric positive definite matrix. 9 5.3. Control Law The implicit force control scheme was constructed as the outer loop that asso- ciates the contact force (22) with a position-velocity vector so that the difference 160 between the desired force (fd) and the contact force (fc) can be translated to a position and velocity difference ∆P(x, y) and ∆˙P(x, y) respectively, adding the last one to the reference signals of the tracking controller inner loop. In [14] an implicit force regulation scheme is proposed, with an outer force control PI, where the control law is given by (25), being the controller proportional 165 contribution. ˙Pd(x, y) = −K−1 e kf Z t 0 f(τ)dτ (23) ˙Pd(x, y) = −k∆f(t) = k[fc(t) −fd] (24) Where k = K−1 e kf, and integrating in time the position reference for the force control law: Pd(x, y) = −K−1 e kf Z t 0 ∆f(τ)dτ (25) Integrator Fd Fc K − q + −K q − Figure 3: Force/position-velocity relationship scheme control 5.4. Stability 170 In order to ensure the controller stability, we have defined our force controller based on (23) and (24), defining the velocity and position of the outer loop . Considering the desired force fd as a constant, the controller (23) ensure an 10 asymptotically exact regulation, while the inner loop provides an asymptotically exact tracking. The inner velocity loops with bounded errors can be seen as: 175 lim t→∞sup∥∆f(t)∥≤1 kf S (26) If limt→∞sup∥∆˙x(t)∥≤S and S ǫ Rn, then 0 ≤S ≤∞and kf > 0 lim t→∞sup∥∆˙x(t)∥≤S (27) 6. Results and analysis The proposed control scheme was tested using MatLab and Simulink. The simulations have been constructed so the manipulators end-effector was located in P(x, y) = (0, 0)m as the initial position. The environment was located in 180 Pe(x, y) = (0.7071, 0.7071)m, therefore the manipulator will be, at first, under the tracking controller action, until the end-effector makes contact with the en- vironment. Once the contact is made, the force controller is activated. The simulations results are show it in the Fig. 4 and Fig. 5 for the one-link flexible manipulator. The parameters physical of the beam were: 1m. long aluminum 185 beam with cross section diameter 10−3m, inertia Im = 1.3254−06Kgm2, elastic- ity coefficient EI = 34.3612Nm2, modes-shapes ν = 2. The parameters of the tracking controller are Kp = diag[160 100 100], Kv = diag[30 1 0.5]. For the model of the environment Ke = 86.9N/m as stiffness coefficient of the envi- ronment. The force desired is fd = 5N. These results show that the end-effector 190 reaches the final position desired applying to the environment the desired force. We also can observe that transversal deformations in stationary state converge to error = 0. Furthermore, in the Fig. 6, we are presenting as additional results the rela- tion position-force, with three environments (Ke1 = 20N/m, Ke2 = 86.4N/m, 195 Ke3 = 200N/m), this results proof as the environment is deform it, when the manipulator apply a force (verify the environment mathematical model), visu- alizing this deformation and parallel at this the convergence the both control loops (errorposition and errorforce →0, in a finite time). 11 (a) (b) 0.65 0.7 0.75 0.8 0.85 0.9 0.95 1 1.05 0 1 2 3 4 5 6 7 8 9 10 m. t x_exp vs t xf_ref vs t 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0 1 2 3 4 5 6 7 8 9 10 m. t yend_exp Vs t yf_ref vs t (c) (d) 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 0 1 2 3 4 5 6 7 8 9 10 rad. t theta_exp vs t thetap_ref vs t -0.02 -0.015 -0.01 -0.005 0 0.005 0.01 0 1 2 3 4 5 6 7 8 9 10 m. t w(x,t) vs t (e) (f) -0.025 -0.02 -0.015 -0.01 -0.005 0 0.005 0.01 0 1 2 3 4 5 6 7 8 9 10 m. t q11 vs t -0.0002 -0.00015 -0.0001 -5e-005 0 5e-005 0 1 2 3 4 5 6 7 8 9 10 m. t q12 vs t Figure 4: Results of the Implicit Force Controller: Environment = 86.9N/m, fd = 5N.(a) Cartesian position end-effector in the axis x. (b) Cartesian position end-effector in the axis y (c) Joint Position θ(t). (d) Transversal deformation w(ˆx, t). (e) First frequency of deformation E-B beam. (f) Second frequency of deformation E-B beam. 12 0 1 2 3 4 5 6 0 1 2 3 4 5 6 7 8 9 10 N. t fd vs t fc vs t (a) -4 -3.5 -3 -2.5 -2 -1.5 -1 -0.5 0 0.5 0 1 2 3 4 5 6 7 8 9 10 N. t fdx vs t fcx vs t (b) -0.5 0 0.5 1 1.5 2 2.5 3 3.5 4 0 1 2 3 4 5 6 7 8 9 10 N. t fdy vs t fcy vs t (c) Figure 5: Results of the Implicit Force Controller: Environment = 86.9N/m, fd = 5N. (a) Total Force applied to environment. (b) Force component Fx applied to the environment. (c) Force component Fy applied to the environment. 13 -5 -4 -3 -2 -1 0 0.45 0.5 0.55 0.6 0.65 0.7 fx (N) x (m) Ke=20N/m Ke=86.9N/m Ke=200N/m 0 1 2 3 4 5 0.7 0.75 0.8 0.85 0.9 fy (N) y (m) Ke=20N/m Ke=86.9N/m Ke=200N/m Figure 6: Relation Force-Position for enviroments with compliance variable. 7. Conclusion 200 This paper proposes a general method for implement a Implicit Force Con- troller based on dynamics of the manipulator. With the control scheme pro- posed is possible that the manipulator realize two works, considering indirectly the effect of the impact. The stability analysis was based on Lyapunov The- ory, ensure the global asymptotic stability of the control scheme by obtaining 205 a unique equilibrium point for controller constants Kp and Kv, considering the compensation of the gravitational force and vibrational frequencies of the beam. Furthermore this method, can be used to prove of the resistance of materials, because is possible know the limits of the system (beam) associated with the vibrations amplitudes, before the deform completely when have been applied 210 a reference force hight or when the environment is less compliant. The results were satisfactory and validate the proposed controller. Acknowledgments The authors thanks for the financial support provided by the El Bosque Uni- versity, Electronic Enginering Program, San Buenaventura University, Systems 215 Engineering Program and University Simon Bol´ıvar, Electronics and Circuits Department. 14 References [1] M. Raibert, J. Craig, Hybrid position-force control of manipulators., trans- actions of ASME, Journal of Dynamic Systems, Measurement and Con- 220 trol (102) (1981) 126–133. [2] T. Yoshikawa, Dynamics hybrid position/force control of robot manipulators-description of hand constraints and calculation of joints driv- ing forces, In proceedings IEEE, Conference Robotics Automation (1986) 1393–1398. 225 [3] T. Yoshikawa, Dynamics hybrid position/force control of manipulators de- scription of hand constraints and calculation of joint driving force, IEEE Journal of Robotics and Automation Ra-3 (5) (1987) 386–392. [4] N. Hogan, Impedance control: an approach to manipulation: Theory, im- plementation, application, Journal of dynamic Systems, Measurement and 230 Control,ASME 107. [5] D. Whitney, Historical perspective ans state of the art in robot force control, The Internartional Journal of Robotics Research 6 (1) (1987) 3–14. [6] C. Murrugarra, J. Grieco, G. Fernandez, M. Armada, Modelling of a 3d flexible manipulator: A generalized equation for n vibration frequencies, 235 Measurement and Control in Robotics 1 (1) (2003) 199–204. [7] C. Murrugarra, J. Grieco, G. Fernandez, O. De Castro, A generalized math- ematical model for flexible link manipulators with n vibration frequencies and friction in the joint, ISRA 2006, International Symposium on Robotics and Automation 1 (1) (2006) 23–28. 240 [8] M. Spong, S. Hutchinson, M. Vidyasagar, Robot Modeling and Control, Wiley, 2005. URL https://books.google.com.co/books?id=wGapQAAACAAJ 15 [9] W. Thomson, Theory of Vibration with Applications, 5th Edition, Prentice Hall, USA, 1993. 245 [10] S. Moorehead, Position and force control of flexible manipulators, Master’s thesis, University of Waterloo, Waterloo,Ontario, Canada (1996). [11] C. Murrugarra, J. Grieco, G. Fernandez, O. De Castro, Design of a pd position control based on the lyapunov theory for a robot manipulator flexible-link, Proceedings IEEE International Conference on Robotics and 250 Biomimetics ROBIO 2006. 1 (1). [12] R. Kelly, V. Santib´a˜nez, Control de Movimiento de Robots Manipuladores., Pearson Prentice Hall, Spain, 2003. [13] L. Meirovitch, Dynamics and Control of Structures, Wiley Interscience Publication, USA, 1990. 255 [14] J. De-Shutter, H. Van-Brussel, Compliant robot motion ii. a control ap- proach based on external control loops, The Internartional Journal of Robotics Research 7 (4) (1988) 18–33. 16