Giáo trình robot - Phần 4 pptx

Part III Motion Control Introduction to Part III Consider the dynamic model of a robot manipulator with n degrees of freedom, rigid links, no friction at the joints and with ideal actuators, (3.18), which we repeat here for ease of reference: M(q) ¨ q + C(q, ˙ q) ˙ q + g(q)=τ . (III.1) In terms of the state vector  q T ˙ q T  T these equations are rewritten as d dt ⎡ ⎣ q ˙ q ⎤ ⎦ = ⎡ ⎣ ˙ q M(q) −1 [τ (t) − C(q, ˙ q) ˙ q −g(q)] ⎤ ⎦ where M(q) ∈ IR n×n is the inertia matrix, C(q, ˙ q) ˙ q ∈ IR n is the vector of centrifugal and Coriolis forces, g(q) ∈ IR n is the vector of gravitational torques and τ ∈ IR n is a vector of external forces and torques applied at the joints. The vectors q, ˙ q, ¨ q ∈ IR n denote the position, velocity and joint acceleration respectively. The problem of motion control, tracking control, for robot manipulators may be formulated in the following terms. Consider the dynamic model of an n-DOF robot (III.1). Given a set of vectorial bounded functions q d , ˙ q d and ¨ q d referred to as desired joint positions, velocities and accelerations we wish to find a vectorial function τ such that the positions q, associated to the robot’s joint coordinates follow q d accurately. In more formal terms, the objective of motion control consists in finding τ such that lim t→∞ ˜ q(t)=0 where ˜ q ∈ IR n stands for the joint position errors vector or is simply called position error, and is defined by ˜ q(t):=q d (t) − q(t) . 224 Part III Considering the previous definition, the vector ˙ ˜ q(t)= ˙ q d (t) − ˙ q(t) stands for the velocity error. The control objective is achieved if the manipulator’s joint variables follow asymptotically the trajectory of the desired motion. The computation of the vector τ involves in general, a vectorial nonlinear function of q, ˙ q and ¨ q. This function is called “control law” or simply, “con- troller”. It is important to recall that robot manipulators are equipped with sensors to measure position and velocity at each joint henceforth, the vectors q and ˙ q are measurable and may be used by the controllers. In some robots, only measurement of joint position is available and joint velocities may be estimated. In general, a motion control law may be expressed as τ = τ (q, ˙ q, ¨ q, q d , ˙ q d , ¨ q d ,M(q),C(q, ˙ q), g(q)) . However, for practical purposes it is desirable that the controller does not depend on the joint acceleration ¨ q since accelerometers are usually highly sensitive to noise. Figure III.1 presents the block-diagram of a robot in closed loop with a motion controller. ROBOT CONTROLLER τ q ˙ q q d ˙ q d ¨ q d Figure III.1. Motion control: closed-loop system In this third part of the textbook we carry out the stability analysis of a group of motion controllers for robot manipulators. As for the position control problem, the methodology to analyze the stability may be summarized in the following steps. 1. Derivation of the closed-loop dynamic equation. Such an equation is ob- tained by replacing the control action control τ in the dynamic model of the manipulator. In general, the closed-loop equation is a nonautonomous nonlinear ordinary differential equation since q d = q d (t). 2. Representation of the closed-loop equation in the state-space form, d dt  q d − q ˙ q d − ˙ q  = f(q, ˙ q, q d , ˙ q d , ¨ q d ,M(q),C(q, ˙ q), g(q)) . Introduction to Part III 225 This closed-loop equation may be regarded as a dynamic system whose inputs are q d , ˙ q d and ¨ q d , and whose outputs are the state vectors ˜ q = q d −q and ˙ ˜ q = ˙ q d − ˙ q. Figure III.2 shows the corresponding block-diagram. CONTROLLER ROBOT + ¨ q d ˙ q d q d ˜ q ˙ ˜ q Figure III.2. Motion control closed-loop system in its input–output representation 3. Study of the existence and possible unicity of the equilibrium for the closed-loop equation d dt  ˜ q ˙ ˜ q  = ˜ f(t, ˜ q, ˙ ˜ q) (III.2) where ˜ f is obtained by replacing q with q d (t) − ˜ q and ˙ q with ˙ q d (t) − ˙ ˜ q. Whence the dependence of ˜ f on t. That is, the closed-loop system equation is nonautonomous. Thus, for Equation (III.2) we want to verify that the origin, [ ˜ q T , ˙ ˜ q T ] T = 0 ∈ IR 2n is an equilibrium and whether it is unique. 4. Proposal of a Lyapunov function candidate to study the stability of any equilibrium of interest for the closed-loop equation, by using the Theorems 2.2, 2.3 and 2.4. In particular, verification of the required properties, i.e. positivity and, negativity of the time derivative. Notice that in this case, we cannot use La Salle’s theorem (cf. Theorem 2.7) since the closed-loop system is described, in general, by a nonautonomous differential equation. 5. Alternatively to step 4, in the case that the proposed Lyapunov func- tion candidate appears to be inappropriate (that is, if it does not satisfy all of the required conditions) to establish the stability properties of the equilibrium under study, we may use Lemma 2.2 by proposing a positive definite function whose characteristics allow one to determine the quali- tative behavior of the solutions of the closed-loop equation. In particular, the convergence of part of the state. The rest of this third part is divided in three chapters. The controllers that we consider are, in order, • Computed torque control and computed torque+ control. • PD control with compensation and PD+ control. 226 Part III • Feedforward control and PD plus feedforward control. For references regarding the problem of motion control of robot manipu- lators see the Introduction of Part II on page 139. 10 Computed-torque Control and Computed-torque+ Control In this chapter we study the motion controllers: • Computed-torque control and • Computed-torque+ control. Computed-torque control allows one to obtain a linear closed-loop equation in terms of the state variables. This fact has no precedent in the study of the controllers studied in this text so far. On the other hand, computed-torque+ control is characterized for being a dynamic controller, that is, its complete control law includes additional state variables. Finally, it is worth anticipating that both of these controllers satisfy the motion control objective with a trivial choice of their design parameters. The contents of this chapter have been taken from the references cited at the end. The reader interested in going deeper into the material presented here is invited to consult these and the references therein. 10.1 Computed-torque Control The dynamic model (III.1) that characterizes the behavior of robot manipula- tors is in general, composed of nonlinear functions of the state variables (joint positions and velocities). This feature of the dynamic model might lead us to believe that given any controller, the differential equation that models the control system in closed loop should also be composed of nonlinear functions of the corresponding state variables. This intuition is confirmed for the case of all the control laws studied in previous chapters. Nevertheless, there exists a controller which is also nonlinear in the state variables but which leads to a closed-loop control system which is described by a linear differential equation. This controller is capable of fulfilling the motion control objective, globally 228 10 Computed-torque Control and Computed-torque+ Control and moreover with a trivial selection of its design parameters. It receives the name computed-torque control. The computed-torque control law is given by τ = M(q)  ¨ q d + K v ˙ ˜ q + K p ˜ q  + C(q, ˙ q) ˙ q + g(q) , (10.1) where K v and K p are symmetric positive definite design matrices and ˜ q = q d − q denotes as usual, the position error. Notice that the control law (10.1) contains the terms K p ˜ q + K v ˙ ˜ q which are of the PD type. However, these terms are actually premultiplied by the inertia matrix M(q d − ˜ q). Therefore this is not a linear controller as the PD, since the position and velocity gains are not constant but they depend explicitly on the position error ˜ q. This may be clearly seen when expressing the computed-torque control law given by (10.1) as τ = M(q d − ˜ q)K p ˜ q + M(q d − ˜ q)K v ˙ ˜ q + M(q) ¨ q d + C(q, ˙ q) ˙ q + g(q) . Computed-torque control was one of the first model-based motion control approaches created for manipulators, that is, in which one makes explicit use of the knowledge of the matrices M(q), C(q, ˙ q) and of the vector g(q). Furthermore, observe that the desired trajectory of motion q d (t), and its derivatives ˙ q d (t) and ¨ q d (t), as well as the position and velocity measurements q(t) and ˙ q(t), are used to compute the control action (10.1). The block-diagram that corresponds to computed-torque control of robot manipulators is presented in Figure 10.1. q ˙ q Σ Σ Σ Σ M(q) C(q, ˙ q) ROBOT g(q) τ K v K p ¨ q d ˙ q d q d Figure 10.1. Block-diagram: computed-torque control The closed-loop equation is obtained by substituting the control action τ from (10.1) in the equation of the robot model (III.1) to obtain M(q) ¨ q = M(q)  ¨ q d + K v ˙ ˜ q + K p ˜ q  . (10.2) 10.1 Computed-torque Control 229 Since M(q) is a positive definite matrix (Property 4.1) and therefore it is also invertible, Equation (10.2) reduces to ¨ ˜ q + K v ˙ ˜ q + K p ˜ q = 0 which in turn, may be expressed in terms of the state vector  ˜ q T ˙ ˜ q T  T as d dt ⎡ ⎣ ˜ q ˙ ˜ q ⎤ ⎦ = ⎡ ⎣ ˙ ˜ q −K p ˜ q −K v ˙ ˜ q ⎤ ⎦ = ⎡ ⎣ 0 I −K p −K v ⎤ ⎦ ⎡ ⎣ ˜ q ˙ ˜ q ⎤ ⎦ , (10.3) where I is the identity matrix of dimension n. It is important to remark that the closed-loop Equation (10.3) is repre- sented by a linear autonomous differential equation, whose unique equilibrium point is given by  ˜ q T ˙ ˜ q T  T = 0 ∈ IR 2n . The unicity of the equilibrium fol- lows from the fact that the matrix K p is designed to be positive definite and therefore nonsingular. Since the closed-loop Equation (10.3) is linear and autonomous, its so- lutions may be obtained in closed form and be used to conclude about the stability of the origin. Nevertheless, for pedagogical purposes we proceed to analyze the stability of the origin as an equilibrium point of the closed-loop equation. We do this using Lyapunov’s direct method. To that end, we start by introducing the constant ε satisfying λ min {K v } >ε>0 . Multiplying by x T x where x ∈ IR n is any nonzero vector, we obtain λ min {K v }x T x >εx T x. Since K v is by design, a symmetric matrix then x T K v x ≥ λ min {K v }x T x and therefore, x T [K v − εI] x > 0 ∀ x = 0 ∈ IR n . This means that the matrix K v − εI is positive definite, i.e. K v − εI > 0 . (10.4) Considering all this, the positivity of the matrix K p and that of the con- stant ε we conclude that K p + εK v − ε 2 I>0 . (10.5) 230 10 Computed-torque Control and Computed-torque+ Control Consider next the Lyapunov function candidate V ( ˜ q, ˙ ˜ q)= 1 2 ⎡ ⎣ ˜ q ˙ ˜ q ⎤ ⎦ T ⎡ ⎣ K p + εK v εI εI I ⎤ ⎦ ⎡ ⎣ ˜ q ˙ ˜ q ⎤ ⎦ = 1 2  ˙ ˜ q + ε ˜ q  T  ˙ ˜ q + ε ˜ q  + 1 2 ˜ q T  K p + εK v − ε 2 I  ˜ q (10.6) where the constant ε satisfies (10.4) and of course, also (10.5). From this, it follows that the function (10.6) is globally positive definite. This may be more clear if we rewrite the Lyapunov function candidate V ( ˜ q, ˙ ˜ q) in (10.6) as V ( ˜ q, ˙ ˜ q)= 1 2 ˙ ˜ q T ˙ ˜ q + 1 2 ˜ q T [K p + εK v ]˜q + ε ˜ q T ˙ ˜ q . Evaluating the total time derivative of V ( ˜ q, ˙ ˜ q) we get ˙ V ( ˜ q, ˙ ˜ q)= ¨ ˜ q T ˙ ˜ q + ˜ q T [K p + εK v ] ˙ ˜ q + ε ˙ ˜ q T ˙ ˜ q + ε ˜ q T ¨ ˜ q . Substituting ¨ ˜ q from the closed-loop Equation (10.3) in the previous ex- pression and making some simplifications we obtain ˙ V ( ˜ q, ˙ ˜ q)=− ˙ ˜ q T [K v − εI] ˙ ˜ q −ε ˜ q T K p ˜ q = − ⎡ ⎣ ˜ q ˙ ˜ q ⎤ ⎦ T ⎡ ⎣ εK p 0 0 K v − εI ⎤ ⎦ ⎡ ⎣ ˜ q ˙ ˜ q ⎤ ⎦ . (10.7) Now, since ε is chosen so that K v − εI > 0, and since K p is by design positive definite, the function ˙ V ( ˜ q, ˙ ˜ q) in (10.7) is globally negative definite. In view of Theorem 2.4, we conclude that the origin  ˜ q T ˙ ˜ q T  T = 0 ∈ IR 2n of the closed-loop equation is globally uniformly asymptotically stable and therefore lim t→∞ ˙ ˜ q(t)=0 lim t→∞ ˜ q(t)=0 from which it follows that the motion control objective is achieved. As a matter of fact, since Equation (10.3) is linear and autonomous this is equivalent to global exponential stability of the origin. For practical purposes, the design matrices K p and K v may be chosen diag- onal. This means that the closed-loop Equation (10.3) represents a decoupled multivariable linear system that is, the dynamic behavior of the errors of each joint position is governed by second-order linear differential equations which are independent of each other. In this scenario the selection of the matrices K p and K v may be made specifically as 10.1 Computed-torque Control 231 K p = diag  ω 2 1 , ···,ω 2 n  K v = diag {2ω 1 , ···, 2ω n } . With this choice, each joint responds as a critically damped linear system with bandwidth ω i . The bandwidth ω i defines the velocity of the joint in question and consequently, the decay exponential rate of the errors ˜ q(t) and ˙ ˜ q(t). Therefore, in view of these expressions we may not only guarantee the control objective but we may also govern the performance of the closed-loop control system. Example 10.1. Consider the equation of a pendulum of length l and mass m concentrated at its tip, subject to the action of gravity g and to which is applied a torque τ at the axis of rotation that is, ml 2 ¨q + mgl sin(q)=τ, where q is the angular position with respect to the vertical. For this example we have M(q)=ml 2 , C(q, ˙q)=0andg(q)=mgl sin(q). The computed-torque control law (10.1), is given by τ = ml 2  ¨q d + k v ˙ ˜q + k p ˜q  + mgl sin(q), with k v > 0, k p > 0. With this control strategy it is guaranteed that the motion control objective is achieved globally. ♦ Next, we present the experimental results obtained for the Pelican proto- type presented in Chapter 5 under computed-torque control. Example 10.2. Consider the Pelican prototype robot studied in Chap- ter 5, and shown in Figure 5.2. Consider the computed-torque control law (10.1) on this robot for motion control. The desired reference trajectory, q d (t), is given by Equation (5.7). The desired velocities and accelerations ˙ q d (t) and ¨ q d (t), were ana- lytically found, and they correspond to Equations (5.8) and (5.9), respectively. The symmetric positive definite matrices K p and K v are chosen as K p = diag{ω 2 1 ,ω 2 2 } = diag{1500, 14000} [1/s] K v = diag{2ω 1 , 2ω 2 } = diag{77.46, 236.64}  1/s 2  , where we used ω 1 =38.7 [rad/s] and ω 2 = 118.3 [rad/s]. [...]... −λI ξ2 ξ2 0 −λI q 2 34 10 Computed-torque Control and Computed-torque+ Control g(q) ¨ qd M (q) Σ τ Σ ROBOT q ˙ q ˙ C(q, q) 1 p+λ Σ Σ Kv Kp p p+λ ˙ qd qd Σ Σ Figure 10.3 Computed-torque+ control ⎡ ⎤ ˜ q −I ] ⎣ ⎦ − [ 0 I ] ⎣ ⎦ ˙ ˜ ξ2 q ⎡ ν = [ −I ξ1 ⎤ (10.11) where ξ 1 , ξ 2 ∈ IRn are the new state variables To derive the closed-loop equation we combine first the dynamic equation of the robot (III.1) with... control objective for robot manipulators may be achieved globally by means of computed-torque control Computed-torque control belongs to the so-called class of feedback linearizing controllers Roughly, the technique of feedback linearization in its simplest form consists in applying a control law such that the closed-loop equations are linear Historically, the motivation to develop feedback-linearization... described by linear differential equation While computed-torque control was one of the first model-based controllers for robot manipulators, and rapidly gained popularity it has the disadvantages of other feedback-linearizing controllers: first, it requires a considerable computing load since the torque has to be computed on-line so that the closed-loop system equations become linear and autonomous, and... know the desired trajectories q d (t), q d (t) and q d (t) ˙ as well as to have the measurements q(t) and q(t) Figure 11 .4 depicts the corresponding block-diagram of the PD+ control for robot manipulators g(q) ¨ qd qd Σ ˙ C(q, q) ˙ qd M (q) Kv τ ROBOT q ˙ q Kp Σ Σ Figure 11 .4 Block-diagram: PD+ control ˙ Notice that in the particular case of position control, that is, when q d = ¨ q d = 0 ∈ IRn , PD+... “Introduction to robotics: Mechanics and control”, Addison– Wesley Spong M., Vidyasagar M., 1989, Robot dynamics and control”, John Wiley and Sons Yoshikawa T., 1990, “Foundations of robotics: Analysis and control”, The MIT Press The stability analysis for the computed-torque controller as presented in Section 10.1 follows the guidelines of • Wen J T., Bayard D., 1988, “New class of control law for robotic... 1989 “Adaptive motion control design of robot manipulators: An input-output approach”, International Journal of Control, Vol 50, No 6, September, pp 2563–2581 Problems • 239 Kelly R., 1990, “Adaptive computed torque plus compensation control for robot manipulators ”, Mechanism and Machine Theory, Vol 25, No 2, pp 161–165 Problems 1 Consider the Cartesian robot 2-DOF shown in Figure 10.5 z0 q2 z0 m2... mgl sin(q) = τ − τe ¨ To control the motion of this device we use a computed-torque controller that is, ˙ ˜ ¨ ˜ τ = ml2 qd + kv q + kp q + mgl sin(q), where kp > 0 and kv > 0 Show that lim q (t) = ˜ t→∞ τe kp ml2 Hint: Obtain the closed-loop equation in terms of the state vector 240 10 Computed-torque Control and Computed-torque+ Control ⎡ ⎣ q− ˜ τe ⎤T kp ml2 ⎦ ˙ q ˜ and show that the origin is globally... computed-torque+ controller (10.8), reduces to the computed-torque controller given by (10.1) in the particular case of manipulators that do not have the centrifugal and forces ˙ matrix C(q, q) Such is the case for example, of Cartesian manipulators Next, we present the experimentation results obtained for the computedtorque+ control on the Pelican robot Example 10.3 Consider the 2-DOF prototype robot. .. q2 ˜ −0.01 −0.02 0 2 4 6 8 10 t [s] Figure 10 .4 Graph of position errors against time The symmetric positive definite matrices Kp and Kv , and the constant λ are taken as 2 2 Kp = diag{ω1 , ω2 } = diag{1500, 140 00} [1/s] 1/s2 Kv = diag{2ω1 , 2ω2 } = diag{77 .46 , 236. 64} λ = 60 The initial conditions of the controller state variables are fixed at... that the first two terms on the right-hand side of the control law (11.1) correspond to the PD control law PD control with compensation is model-based, that is, the control law explicitly uses the terms from the ˙ model of the robot (III.1), M (q), C(q, q) and g(q) Figure 11.1 presents the block-diagram that corresponds to the PD control law with compensation The closed-loop equation is obtained by substituting . 140 00} [1/s] K v = diag{2ω 1 , 2ω 2 } = diag{77 .46 , 236. 64}  1/s 2  , where we used ω 1 =38.7 [rad/s] and ω 2 = 118.3 [rad/s]. 232 10 Computed-torque Control and Computed-torque+ Control 0 246 810 −0.02 −0.01 0.00 0.01 0.02 [rad] ˜q 1 ˜q 2 t. block-diagram that corresponds to computed-torque control of robot manipulators is presented in Figure 10.1. q ˙ q Σ Σ Σ Σ M(q) C(q, ˙ q) ROBOT g(q) τ K v K p ¨ q d ˙ q d q d Figure 10.1. Block-diagram:. proto- type presented in Chapter 5 under computed-torque control. Example 10.2. Consider the Pelican prototype robot studied in Chap- ter 5, and shown in Figure 5.2. Consider the computed-torque