Passivity based on energy tank for cartesian impedance control of dlr space robots with floating base and elastic joints

Journal of Computer Science and Cybernetics, V.34, N.1 (2018), 49–61 DOI 10.15625/1813-9663/34/1/11027 PASSIVITY BASED ON ENERGY TANK FOR CARTESIAN IMPEDANCE CONTROL OF DLR SPACE ROBOTS WITH FLOATING BASE AND ELASTIC JOINTS LE TIEN LUC German Aerospace Center (DLR), Institute of Robotics and Mechatronics Luc.Le-Tien@dlr.de
Abstract. This paper presents a control structure for orbital servicing mission of CEASAR robotic arm developed by German Aerospace Center (DLR). In order to reduce mass the CEASAR arm is equipped with Harmonic-Drives with high ratio which unfortunately lead to high joint elasticity and high motor friction and have to be considered in controller design for successful manipulator in-orbit operations. Therefore, in this control structure, for high tracking control a cascaded position controller based on state feedback control structure with observer-based friction compensation and for safe interaction control with the environment a Cartesian impedance controller is used, which is designed based on using energy tank method to ensure passivity of the controlled system. The proposed control methods are very efficient and practicable. Furthermore, they are very robust with dynamic parameter uncertainties, coupling dynamics, and can simultaneously provide good results in term of the position accuracy and dynamic behavior. Simulation results validate practical effciency of the controllers. Keywords. Impedance control, floating base robots, space robots, flexible joint robots. 1. INTRODUCTION In recent years the use of robots in space has become more and more of interest. With increasing capability of sophisticated autonomy, the robot can be used in such applications as • Exploration of distant planets, • Orbital servicing/repair in low earth orbit or geostationary earth orbit, • De-orbiting of failed satellites, • Constructions of heavy structures (e.g. Space Station, Planetary Bases). . . In this paper the control issues of a space robotic arm for orbital servicing missions are considered. Since lightweight and a high load/weight ratio are essential for space robotics, the design of the robot can be optimized by using Harmonic-Drive gear with high gear ratio to reduce robot weight [1, 2]. But, high gear ratio causes high motor friction and high joint elasticity, which on the other hand are challenging problems for the robot control. So, for control design the robot is modeled as a redundant free-flying base robot with flexible joints. In the designed missions the space robot is expected to achieve various tasks, such as capturing a target, constructing a large structure and autonomously maintaining on-orbit systems. c© 2018 Vietnam Academy of Science & Technology 50 LE TIEN LUC Figure 1. Target scenario of the space robot In order to allow safe dynamic interaction between the robot and its working environment, a Cartesian impedance controller is needed to reach an interactive behavior with a mass- spring-damper-like disturbance response via active control. In fixed-base robotic systems, the dynamic interaction between the robot’s operational space motions and forces was addressed in the operational Space [3, 4]. The control of free- flying robots for space applications was introduced in [5, 6]. Furthermore, in order to consider uncertainties of the robot parameters or varying parameters, an adaptive control scheme was introduced in [7]. In case of the redundant robot Cartesian impedance control in task space has to take null-space effects into account [4]. The redundant degree of freedom (DOF) can be used to execute several independent tasks while following a strict hierarchy. Furthermore, in [8] a Cartesian mass matrix is used for control design instead of desired one. But the system passivity could not be ensured for time-varying control gains. In [9] a Cartesian impedance control was introduced based on the concept of energy tank [10, 11], which can be applied to reproduce time-varying stiffness and therefore ensure stable behavior. In this paper Cartesian impedance control based on energy tank for free-flying base robots with elastic joints is addressed for space applications. It should fulfil the requirements of space missions and must be robust enough for implementation. The paper is organized as follows. Section 2 introduces the dynamic robot model. In Section 3 the control goal for Cartesian impedance control is defined. Section 4 presents the control design for Cartesian impedance controller based on energy tank. The stability of the controlled system is analyzed. Finally, the obtained performance is verified by simulations reported in Section 5. PASSIVITY BASED ON ENERGY TANK FOR CARTESIAN IMPEDANCE CONTROL... 51 2. MODELING ROBOT DYNAMICS Let us consider a redundant space robot with 7 DOF (n = 7). The design with 7 joints has some advantages: • Increased working area, • Increased obstacle avoidance capabilities, • Some redundancy in case of a joint failure. For control design, the robot is modeled as a flexible joint robot with free-flying base. This robot is equipped with motor position sensors and link torque sensors, which can be used for control. The simplified dynamics of this space robot can be described by u = Bθ¨ + τ + τf , (1) [ −Fb τ ] + [ JTb JT ] Fext = Ms (xb, q) [ x¨b q¨ ] + Cs (xb, q, x˙b, q˙) [ x˙b q˙ ] , (2) τ = K (θ − q) . (3) Therein, xb ∈ R6, q ∈ Rn and θ ∈ Rn are the base, link and motor positions, respectively. u ∈ Rn, τf ∈ Rn present the motor torque and the friction torque. The transmission torque between motor and link dynamics τ ∈ Rn is modeled as a linear function of the motor and the link position with the diagonal and positive definite joint stiffness matrix K ∈ Rn×n and can be measured by strain gauge based torque sensors. Fb, Fext ∈ R6 represent the extern force torque acting on the base and the end-effector (TCP), respectively. Jb (xb, q) ∈ R6×6, J (xb, q) ∈ R6×n are the Jacobian matrices related to the base, and to the arm. Furthermore, the motor inertia matrix B ∈ Rn×n is diagonal and positive definite. Ms (xb, q) ∈ Rn×n, Cs (xb, q) ∈ Rn×n are the mass and the centrifugal/Coriolis matrix, respectively, and Ms can be rewritten as Ms = [ Mb Mc MTc M ] with  Mb ∈ R6×6 Mc ∈ R6×n M ∈ Rn×n. (4) Finally, in order to facilitate the controller design and the stability analysis, the following four assumptions are needed. P.1: The mass matrix Ms(xb, q) ∈ Rn×n is symmetric and positive definite and Ms(xb, q) = Ms (xb, q) T ≥ 0. P.2: The Cartesian mass matrix Λ (xb, q) is positive definite and symmetric Iλmin ≤ Λ ≤ Iλmax with λmin, λmax being maximal and minimal eigenvalues of Λ (xb, q) . P.3: For space robots the maximal joint velocity is limited and it yields −Iγmax ≤ Λ˙ ≤ Iγmax with γmax > 0. 52 LE TIEN LUC P.4: In the following it is assumed that total linear and angular momentum is zero H = Mbx˙b + Mcq˙, which describes the resulting disturbance motion of the base when there is joint motion q˙ in the manipulator arm, can be neglected. It is noted that this motion can be actively compensated by satellite. Figure 2. Robot control structure 3. CONTROL GOAL In the following it is assumed that the position and orientation of the manipulator’s end- effector is defined by x = f(xb, q) ∈ R6, where f(xb, q) represents the forward kinematics of the manipulator and is known. Then, let us define the Cartesian position errors as ex = x− xd. (5) The goal of the impedance Cartesian control is to achieve the dynamic behavior of the end-effector like a mass-spring-damper system in present of the external force and torque Fext Λ (xb, q) e¨x +Dc (xb, q) e˙x +Kc (xb, q) ex = Fext (6) with Λ (xb, q), Dc (xb, q), Kc (xb, q) ∈ R6×6 being the Cartesian mass matrix of the robot, the control damping matrix and the control stiffness matrix, respectively. PASSIVITY BASED ON ENERGY TANK FOR CARTESIAN IMPEDANCE CONTROL... 53 In order to achieve good dynamic behavior the control damping matrix Dc and the control stiffness matrix Kc in (6) are computed online depending on ex = Fext the Cartesian mass matrix Λ (xb, q) . So, for a given positive definite, symmetric matrix Λ (xb, q) , matrices P (xb, q) , Q (xb, q) ∈ R6×6 can be found so that Λ = PQ. By choosing matrices{ Dc (xb, q) = 2P (xb, q)DξKωQ (xb, q) Kc (xb, q) = P (xb, q)K 2 ωQ (xb, q) (7) with positive definite and diagonal constant matrices Dξ(Dξ = diag (ξi) with 0 < ξ ≤ 1) and Kω, the matrices Dc (xb, q) and Kc (xb, q) are positive definite and symmetric as well. If ξi = 1 the closed-loop system has six real poles, otherwise six complex poles. Obviously, (P.2) leads to { Dcmin ≤ Dc (xb, q) ≤ Dcmax Kcmin ≤ Kc (xb, q) ≤ Kcmax . (8) Now, the system can be decoupled by choosing a new coordinate exq = Qex. It leads to six decoupled mass-spring-damper subsystems with the desired damping and stiffness behavior e¨xq + 2DξKω e˙xp +K 2 ωexp. (9) It is noticed that in this control law the control gain Dc (xb, q) and Kc (xb, q) vary with time. Therefore, it cannot ensure passivity of the pair {e˙, Fext} with respect to the closed- loop system (6) and the storage function V = 1 2 e˙Tx (xb, q)e˙x + 1 2 eTxKc(xb, q)ex. (10) 4. PROPOSED CARTESIAN IMPEDANCE CONTROL In order to eliminate the friction effects and reduce the motor inertia, the Cartesian im- pedance control is designed by using a cascaded structure [7] consisting of a torque controller as inner control loop and a Cartesian impedance controller as outer control loop in Figure 2. In this control structure the Cartesian impedance controller computes the desired link torque for the torque controller. 4.1. Torque controller Let us define the desired link torque as τd. Then, for a given desired torque vector τd, a torque controller [10, 11] u = KT (τd − τ)−KS τ˙ + τd + τf , (11) with positive definite and diagonal control matrices KT ,KSKT ,KS can stabilize the tor- que dynamics around the equilibrium point τ = τd. The friction effects τf are preferably compensated by using observer-based friction compensation [14]. The singular perturbation theory leads to the following link dynamics, with the assump- tion of no external forces/toques on the base (Fb = 0) 54 LE TIEN LUC [ 0 τd ] + [ JTb JT ] Fext = Mgs (xb, q) [ x¨b q¨ ] + Cs (xb, q, x˙b, q˙) [ x˙b q˙ ] (12) with Mgs = [ Mb Mc MTc ( M + (I +KT ) −1B ) ] . (13) In case of the redundant manipulator, it is well known that some motions of the joints are embedded in the null space of the manipulator’s Jacobian matrix J(q), which do not affect the end-effector position and orientation. Therefore, the desired torque is proposed as τd = J TFc +Nτn (13a) with Fc ∈ Rn being the desired Cartesian impedance force. τn ∈ Rn is an arbitrary genera- lized joint torque of the manipulator, which is projected to the null space of JT through the projection matrix N (xb, q) ∈ Rn×n. In this paper we assume that the null space behavior is characterized in joint space by a desired positive definite stiffness Kn and a desired positive definite damping Dn as well as an equilibrium position qn. So, the desired nullspace torque can be computed by a joint level PD controller and chosen as τn = Kn (qn − q)−Dnq˙. (14) In the following the desired Cartesian impedance torque Fc can be computed to realize the closed-loop dynamics (6). 4.2. Cartesian impedance control design Let us define Js = [ I 0 0 J ] and Ns = [ 0 N ] . (15) Hereby, I and 0 denote the appropriate identity matrix and zero matrix. By inserting (13), (15) into (11) the robot dynamics (11) can be rewritten as JTs [ −Fb + JTb Fext Fc + Fext ] +Nsτn = Mgs (xb, q) [ x¨b q¨ ] + Cs [ x˙b q˙ ] . (16) From the definition of the generalized Jacobian Js in (15), the general velocity vector in Cartesian coordinates xs = [xbx] T can be written as[ x˙b x˙ ] = J [ x˙b q˙ ] (17) which yields the relevant mapping between general joint and general Cartesian acceleration of the complete system’s dynamics PASSIVITY BASED ON ENERGY TANK FOR CARTESIAN IMPEDANCE CONTROL... 55 [ x¨b x¨ ] = Js [ x¨b q¨ ] + J˙s [ x˙b q˙ ] . (18) By pre-multiplying (16) with (JsM −1 gs ) and using (18) one can obtain the relationship between the general Cartesian acceleration and the Cartesian commanded force Fc.[ JTb Fext Fc + Fext ] + JsM −1 gs Nsτn = Λs [ x¨b x¨ ] + Φs (19) with  Λs (xb, q) = ( JsM −1 gs J T s )−1 ≡ [ Λb Λc ΛTc Λ ] Φs (xb, q, x˙b, q˙) = ( JsM −1 gs Cs − J˙s )[ x˙b q˙ ] ≡ [ Φs1 Φs2 ] . (20) By using definitions (13) and (15) it follows JsM −1 gs Nsτn = [ −M−1b McM−1σ N JM−1σ N ] τn (21) with Mσ = ( M + (I +KT ) −1B ) −MTc M−1b Mc. (22) For the dynamic consistency of the null space, the projection matrix N should be chosen so that JM−1σ N = 0. In [4] this was proposed by N (xb, q) = ( I − JTΛσJM−1σ ) . (23) with Λσ being an equivalent Cartesian mass matrix of the manipulator and defined by Λσ (xb, q) = ( JM−1σ J T )−1 . (24) It is noticed that outside of the singular configuration of the manipulator the matrix Λσ and the respective matrix Λs are full rank and invertible. For the chosen N in (23), the general Cartesian dynamics (19) is reduced into [ −Fb + JTb Fext Fc + Fext ] − [ M−1b McM −1 σ Nτn 0 ] = [ Λb Λc ΛTc Λ ] [ x¨b x¨ ] + [ Φs1 Φs2 ] . (25) By canceling out the base acceleration x¨b in (25) one becomes the equation of robot motion in Cartesian space Fc + ΨFext = Λσx¨− ΛTc Λ−1b M−1b McM−1σ Nτn − ΛTc Λ−1b Φs1 + Φs2 (26) with { Λσ (xb, q) = Λ− ΛTc Λ−1b Λc Ψ (xb, q) = I − ΛTc Λ−1b ΛTb . 56 LE TIEN LUC Now, a Cartesian control law is proposed as Fc = Λσx¨d−ΛTc Λ−1b M−1b McM−1σ Nτn−ΛTc Λ−1b Φs1 + Φs2−Dc (xb, q) e˙x−Kc (xb, q) ex. (27) Substituting (27) into (26) yields Λσ (xb, q) e¨x +Dc (xb, q) e˙x +Kc (xb, q) ex = ΨFext. (28) The expression in (28) establishes a relationship through a generalized mechanical im- pedance between the vector of resulting forces ΨFext and the vector of displacements ex. In order to avoid the coupled motion attributed by Ψ it is necessary to measure the forces \torquesFext or to simplify the dynamic equation of the system. 4.3. Cartesian impedance controller based on energy tank From assumption P.4, the velocity x˙b in local coordinates of the base robot can be neglected and the constraint for the dynamics is given by{ xb (t) = const x˙b(t) = x¨b(t) = 0. (29) Hence, from (25) the dynamic equation of the manipulator is given by Fc + Fext = Λ (q) x¨+ ϕs2 ( xb, q, x ′′) . (30) Now, the Cartesian impedance control law can be developed by using the dynamic equa- tion (30). Because the proposed control gains Kc, Dc in Sec. 3 vary with time, a Cartesian impedance control law as [8] cannot ensure passivity of the pair {e˙x, Fext} using the storage function V = 1 2 e˙TxΛ(xb, q)e˙x + 1 2 eTxKc(xb, q)ex. (31) Therefore, a new control law is proposed based on energy tank which is used to store the energy dissipated by the controlled system. By introducing a state variable xt ∈ R (xt (t = 0) > 0 to avoid singularity) with the store function of the tank Tt (xt) = 1 2 xTt xt, (32) the closed-loop dynamics (6) is expanded and given by Λ (xb, q) e¨x +Dvar (xb, q, 0, q˙) e˙x +Kcconstex − wxt = Fextx˙t = δ xt ( e˙TxDvar(xb, q,0, q˙)e˙x )− wT e˙x (33) with Dc (xb, q) = Dvar(xb, q,0, q˙) + 1 2 Λ˙(xb, q). (34) In the following it is resumed the desired damping matrix Dc (xb, q) is chosen big enough and together with the assumption P.3 it yields Dvar(xb, q,0, q˙) > 0. PASSIVITY BASED ON ENERGY TANK FOR CARTESIAN IMPEDANCE CONTROL... 57 Furthermore, Kcconst ∈ Rn×n is the constant control stiffness and from (8) is chosen by Kcconst = Kcmin . δ (with 0 < δ ≤ 1) is a constant to scale the dissipated energy in the tank and simultaneously to ensure this being not larger than dissipated energy of the main control. Finally, w ∈ Rn presents a new control input to control the energy exchange between the main control law and the tank, and is chosen by w =  − (Kc −Kcconst) ex xt if Tt (xt) ≥  0 if Tt (xt) < . (35) For the desired dynamics (33) the control input Fc in (30) is proposed by Fc = Λ (xb, q) x¨d +Dc (xb, q) e˙x + Kcconstex − wxt + Φs2. (36) Figure 3. Step response of the controller 58 LE TIEN LUC Table 1. Torque controller parameters for the DLR space robot Joint KD KTK −1 KSK−1 1 2 3 4 5 6 7 -2.0 -4.0 -4.5 -2.0 -2.0 0.0 0.0 1.590 4.966 4.461 2.495 2.709 1.940 1.521 0.0068798 0.0182072 0.0083539 0.0016764 0.0221534 0.0060800 0.0053030 Table 2. Impedance controller parameters for the DLR space robot x ( % & ) y ( % & ) z ( % & ) Roll ( %& '() ) Pitch ( %& '() ) Yaw ( %& '() ) Desired impedance stiffness * 800 800 800 200 200 200 Desired impedance damping +, 0.7 0.7 0.7 0.7 0.7 0.7 Figure 4. Desired point-to-point trajectory If Tt (xt) ≥ ε the desired closed-loop dynamics (6) is present, otherwise a new closed-loop dynamics Λ (xb, q) e¨x +Dc (xb, q) e˙x + Kcconstex = Fext (37) PASSIVITY BASED ON ENERGY TANK FOR CARTESIAN IMPEDANCE CONTROL... 59 is created. Now, we consider the store function V1 = 1 2 e˙TxΛ(xb, q)e˙x + 1 2 eTxKcconstex + 1 2 x2t . (38) Then, taking the derivative of the function V1 and using equations (28), (33) we obtain V˙1 = e˙ T xFext − e˙Tx (1− δ)Dvare˙x ≤ e˙TxFext. (39) Obviously, the controlled system (33) ensures passivity of the pair {e˙x, Fext}. 5. SIMULATIONS The complete control structure of the robot is proposed in Figure 2 consisting of a joint torque controller, a tracking joint position controller (state feedback controller with position integrator terms) and a Cartesian impedance controller which allows the robot work in two control modes, either with high position accuracy or with safe interaction. Figure 5. Cartesian translation position errors 60 LE TIEN LUC Because of the slow system dynamics and the high required computing time, the robot dynamics and inverse kinematics as well as the control gains of the Cartesian impedance controller are computed online at 1.33 kHz sampling rate, whereas the position controller, the torque controller, the Cartesian impedance controller, as well as the friction compensation are implemented at 4 kHz sampling rate. All simulations with the DLR space robot are implemented with control parameters represented in the Tables 1, 2 for the torque and the Cartesian impedance controller, re- spectively. At first, the control performance in terms of the dynamic behavior of controller is vali- dated by using step response results. It can be seen in Figure 3 that the proposed controller can damp oscillations of the Cartesian position quite well. In the next experiment, a point to point trajectory in Figure 4 is chosen in order to show the position tracking accuracy of the robot. Figure 5 shows the reached translation position accuracy. It can be seen that the controller can achieve position errors in the order of magnitude of 1cm. 6. CONCLUSIONS This paper presented a novel method to derive a dynamic model for a free-floating robot in operational space, which is necessary for the control design. Furthermore, a Cartesian impedance control design is derived from this, based on using energy tank to ensure passivity of the controlled system. Therefore, the controlled system can achieve stable and good dynamic behavior for the interaction control between the space robot and the environment. Finally, the effectiveness of the method is shown with simulation results. REFERENCES [1] B. Scha¨fer, K. Landzettel, A. Albu-Scha¨ffer, and G. Hirzinger, “ROKVISS: Orbita l testbed for tele-presence experiments, novel robotic components and dynamics models verification ”, Proceedings of 8th ESA Workshop on Advanced Space Technologies for Robotics and Automation (ASTRA), Noordwijk, The Netherlands, November 2 - 4, 2004. [2] B. Scha¨fer, B. Rebele, and K. Landzettel, “ROKVISS - Space robotics dynamics and control per- formance experiments at the ISS” in Proceedings of ACA 2004, IFAC Symposium on Automatic Control in Aerospace, St. Petersburg, pp.333-338, June 2004. [3] N. Hogan, “Impedance control: An approach to manipulation: Parts I, II, and III-theory, im- plementation, and applications”, Journal of Dynamic Systems Measurement, and Control, vol. 107, pp. 1–24, 1985. [4] Khatib, “A unified approach for motion and force control of robot manipulators: The operational space formulation”, IEEE Journal of Robotics and Automation, vol. 3, no. 1, pp. 1115–1120, 1987. [5] Y. Umetami, and K. Yoshida, “Resolved motion rate control of space manipulators with gene- ralized Jacobian matrix”, IEEE Transactions on Robotics and Automation, vol. 5, no. 3, June 1989. [6] Y. Xu, and T. Kanade, Space Robotics: Dynamics and Control. Kluwer Academic Publishers, 1993. [7] S. Abiko, and G. Hirzinger, Adaptive control for a torque controlled free-floating space robot with kinematic and dynamic model uncertainty ”, IEEE/RSJ International Conference on Intelligent Robots and Systems, St. Louis, MO, USA, 10-15 Oct. 2009. PASSIVITY BASED ON ENERGY TANK FOR CARTESIAN IMPEDANCE CONTROL... 61 [8] C. Ott, “Cartesian Impedance Control of Redundant and Flexible-Joint Robots”. Springer-Verlag Berlin Heidelberg, 2008. [9] F. Ferraguti, C. Secchi, and C. Fantuzzi, “A tank-based approach to impedance control with variable stiffness”, IEEE International Conference on Robotics and Automation, Karlsruhe, Ger- many, 6-10 May 2013. pp. 4948–4953. [10] M. Franken, S. Stramigioli, S. Misra, C. Secchi, and A. Macchelli, “Bilateral telemanipulation with time delays: A two-layer approach combining passivity and transparency”, IEEE Tran- saction on Robotics, vol. 27, no. 4, pp. 741–756, Aug. 2011. [11] Franchi, C. Secchi, H. I. Son, H. H. Bu¨lthoff, and P. R. Giordano, “Bilateral telemanipulation of groups of mobile robots with timevarying topology”, IEEE Transaction on Robotics, vol. 28, no. 5, pp. 1019–1033, Oct. 2012. [12] Albu-Scha¨ffer, C. Ott, and G. Hirzinger, “A unified passivity- based control framework for position, torque and impedance control of flexible joint robots”, The International Journal of Robotics Research, vol. 26, no. 1, pp. 23–39, 2007. [13] L. Tian, and A.A. Goldenberg, “Robust adaptive control of flexible joint robots with joint torque feedback”, IEEE International Conference on Robotics and Automation, Nagoya, Japan, 21-27 May 1995, pp. 1229–1234. [14] Le-Tien, A. Albu-Scha¨ffer, A. De Luca, and G. Hirzinger, “Friction observer and compensation for control of robots with joint torque measurement”, IEEE/RSJ International Conference on Intelligent Robots and Systems, Nice, France, 22-26 Sept. 2008. Received on December 28, 2017 Revised on June 23, 2018