Untitled SCIENCE & TECHNOLOGY DEVELOPMENT, Vol 18, No K6 2015 Trang 76 Pendubot trajectory planning and control using virtual holonomic constraint approach Cao Van Kien Ho Pham Huy Anh Ho Chi Minh[.]
SCIENCE & TECHNOLOGY DEVELOPMENT, Vol.18, No.K6 - 2015 Pendubot trajectory planning and control using virtual holonomic constraint approach Cao Van Kien Ho Pham Huy Anh Ho Chi Minh city University of Technology, VNU-HCM, Vietnam (Manuscript Received on July 15, 2015, Manuscript Revised August 30, 2015) ABSTRACT In this paper, the virtual holonomic constraint approach is initiatively applied for the trajectory planning and control design of a typical double link underactuated mechanical system, called the Pendubot The goal is to create synchronous oscillations of both links After modeling the system using Euler-Lagrangian equations of motion, the parameters of the model are identified with optimization techniques Using this model, the trajectory planning is done via Virtual Holonomic Constraint approach on the basis of re-parameterization of the motion according to geometrical relations among the generalized coordinates of the system Keywords: pendubot, trajectory planning and control, virtual holonomic constraint approach, 2-DOF underactuated system INTRODUCTION The problem of trajectory planning and control of underactuated mechanical systems have attracted vast interest during last decades [1] This underactuation can increase the performance of these systems in terms of dexterity and energy efficiency and also lowers the weight of the system as well as manufacturing costs There are many instances of applications of underactuated mechanical systems in real life Underwater vehicles, water machines, helicopters, mobile robots and underactuated robot arms are some examples of engineering applications of underactuated robotics in both fully actuated and underactuated manipulators However, in case of fully actuated manipulators, with considering the dynamical constraints regarding velocity and acceleration, any timing along the defined path can be achieved But in case of robotic manipulators with passive degrees of freedom, due to existence of underactuated and unstable internal dynamics, which are characterized by unbounded solutions of the dynamical equations, the problem of trajectory planning and control design, are more complex and need fundamental nonlinear approaches to be solved Defining a required motion, planning a proper trajectory to perform the required motion and designing a control system which performs the motion are three steps of problem formulation In this paper, the virtual holonomic constraint approach is used to solve the problem of trajectory planning and control design of a two link underactuated robot, namely the Pendubot The Trang 76 TẠP CHÍ PHÁT TRIỂN KH&CN, TẬP 18, SỐ K6- 2015 idea of virtual holonomic constraints which has its roots in analytical mechanics, is to reparameterizing the motions according to geometrical relations among the generalized coordinates [2], and then imposing those constraints with feedback control Having the knowledge about the constraints, it is possible to analytically find a linear approximation of the nonlinear system, in which asymptotic stability implies exponential orbital stability of periodic motions The approach is completely analytical and can be generalizable to systems with arbitrary degree of underactuation [3] The rest of this paper is organized as follows The second section is dedicated to explanation about modeling and identification of the Pendubot system, and it continues by solving the problem of trajectory planning and control design for the Pendubot In the next section the results of implementing virtual holonomic constraint approach on a Pendubot are presented Finally, in section 4, a conclusion for the whole work is given MODELLING PENDUBOT We can also describe the dynamics of the controlled system in terms of inertia matrix denoted by M(q) and the matrix of Coriolis and centrifugal forces denoted by C(q, q) and the vector of gravitational forces G(q), using a second order differential equation: (3) On the basis of equations of motion for a dynamical system, we can present a mathematical definition for fully-actuated and underactuated mechanical systems which says: Assuming that the matrix B(q) has full rank, If the dimension of the vector of independent control inputs, u, is smaller than dimension of vector of generalized coordinates, the system is underactuated and if they have the same dimension, the system is fully actuated Pendubot is a planar two link robot, in which first link is actuated with a DC motor that is equipped with a Harmonic drive, and the second link is passive So in this robot we have the simplest case of underactuation which is of degree one A picture of the Pendubot is depicted in Figure 1: The dynamics of the Pendubot are described using Euler-Lagrange equations Aiming this, Lagrangian is defined as the difference of kinetic energy and potential energy of the system [4], L(q,q) K(q,q) P(q) (1) With the definition above, the equations of motion for a controlled mechanical system with several degrees of freedom can be written as: (2) q1 q2 In which qi is the vector of generalized coordinates and q i is vector of generalized velocities and u is vector of independent control inputs and (B(q)u)i denote generalized forces Fig.1 The picture of the Pendubot [5], first link is actuated and second link is passive Trang 77 SCIENCE & TECHNOLOGY DEVELOPMENT, Vol.18, No.K6 - 2015 Considering q1 and q2 and following the equation (3), the dynamics of the Pendubot can be modelled as: of the physical parameters of the Pendubot setup were known These values are shown in Table Table 1: Known Parameters of the investigated Pendubot [6] (4) with (5-7) Using this model, and after identifying the parameters of the model, the motion planning and control design of the Pendubot will be concerned PROPOSED VIRTUAL HOLONOMIC CONSTRAINTS METHOD FOR PENDUBOT Besides the known physical parameters of the setup which were given in Table 1, it was also required to identify inertia of the first link JC1, where a Harmonic drive is attached to the DC motor, and this Harmonic drive produces considerable friction which should be modelled, identified, and compensated with the controller Here we consider the Coulomb friction and viscous friction present in the actuated link that can be expressed by the following equation: (13) 3.1 System Identification The parameters p1 to p5 that were used in previous section are defined as: For identification of the friction, the second link was disconnected from the setup, and the remaining one link Pendulum was modelled with the following equation: (14) (8 – 12) in which m1 and m2 denote the mass of first and second link, r1 and r2 represent the distance to the center of mass for the first and second link respectively, l1 and l2 denote the length of first link and second link, JC1 and JC2 denote the inertia o the first link and second link and g denotes gravitational constant On the basis of the physical measurements over the system, some of the values Trang 78 In equation (14), JC1 denotes the inertia of the link, b is the coefficient of viscous friction, cn and cp are the coefficients of Coulomb friction, KDC is the torque constant of the DC motor (which is equipped with a Harmonic drive), q is the angular position of the link and u is the input signal The system is identified in closed-loop scheme where a proportional gain controller with the gain Kp = is used and the link is tracking a reference signal that is shown in Figure The signal u is defined as: TẠP CHÍ PHÁT TRIỂN KH&CN, TẬP 18, SỐ K6- 2015 Table 2: Identified values of the model (15) parameters Figure shows the mapped friction for the actuated link Validating data that was captured from the real system showed the precision of the estimated parameters Fig.2: Reference signal used for identification in closed-loop 3.2 Pendubot Motion Planning via Virtual Holonomic Constraint For planning the desired motion for the system, virtual holonomic constraint approach is applied The idea is to define some geometrical relations among the generalized coordinates of the system, and imposing those relations with feedback control The term virtual is derived of the fact that these constraints are not physically present in the system and they are reproduced by means of feedback action Defining constraint function ( ) , we can express generalized coordinates of the system as functions of θ: Fig.3: A map of the viscous and Coulomb friction for the actuated link After capturing data from the system, the nonlinear least squares method is applied for identifying the parameters JC1, b, cn , cp and KDC that are shown in Table (16) On the basis of analytical mechanics, we can reduce the number of differential equations of Euler-Lagrange system (2) by substituting (16) in underactuated equation of motion (4) to obtain reduced-order dynamics of the system (2) in the form of the following second order differential equation: (17) For deriving ( * ) , (* ) , (* ) , one can define ( ) and its first and second derivatives as: Trang 79 SCIENCE & TECHNOLOGY DEVELOPMENT, Vol.18, No.K6 - 2015 qi i ( ) , (18) q i ' i ( ) , (19) q i ' ' i ( ) ' i ( ) , (20) By substituting (18) to (20) into controlled Lagrangian system (21): (21) Assuming that the control law makes (18) invariant and the initial conditions are consistent with (18) and (19), the dynamics in the reduced form can be rewritten as (22): Last step in motion planning via virtual holonomic constraint, is computing the integral of reduced dynamics (17) which is always integrable, provided i ( * ) is not zero Theorem1: Suppose that the function ( ) has only isolated zeros If the solution [ ( t ), ( t )] of (17) with initial conditions ( ) , ( ) exists and is continuously differentiable, then along this solution the function: (22) Then ( ) , ( ) , ( ) now can be written (27) as, preserves its zero value” [7] (23-25) B is a function with B ( q ) B ( q ) u So the derivation of (17) is In which finished [7] For checking the existence of periodic solutions for the equation of reduced dynamics (17), there is a sufficient condition To check this condition, one needs to compute the equilibrium points of (17), which are given by solutions of (e)0, and the following number: (26) If is positive then the equilibrium of (17) is a center and if is negative, then the equilibrium is a saddle So if is greater than zero, then there are periodic solutions for the equation of reduced dynamics Trang 80 Later we will use integral (27) as a part of transverse linearized system in which deriving this state together with the other two to zero will provide exponential orbital stability for the limit cycles 3.3 Control design Designing the controller for underactuated mechanical systems is a challenging control problem, which needs fundamental nonlinear approaches For the case of periodic motions, the problem consists on designing feedback control that ensures orbital stability [7] In this paper, a virtual holonomic constraints approach is applied for control of oscillations of the Pendubot In the next section it is shown that how we use a novel analytical approach, called transverse linearization, for reducing the challenging problem which we mentioned above, to the simpler problem of designing the controller for asymptotically stabilizing a linear time variant system, that makes the nonlinear system exponentially orbital stable 3.3.1 Transverse Linearization TẠP CHÍ PHÁT TRIỂN KH&CN, TẬP 18, SOÁ K6- 2015 In this section, the aim is to find a linear approximation of non-linear dynamics which is called transverse linearization The main idea is to construct the dynamics transverse to the orbit by an appropriate change of coordinate system [3] Then we can linearize these transverse dynamics in a vicinity of the trajectory The importance of this method is that we can analytically derive the coefficients of the linear time-periodic system (228), in which asymptotic stability will ensure the exponential orbital stability of limit cycles of nonlinear system A(t)B(t) with I , y , y are functions and u is the signal that is used for feedback transformation, and a proper choice of this signal will lead us to the target that was inputoutput linearization of non-linear dynamics: u y 2 2 y (33) (34) Substituting (34) in (33) and then (33) into (32), we will find: (35) T (28) new First we change the coordinates of the system to obtain a new set of coordinates which can be written as: yi qi i () (36) (29) where i = 1, 2, dim(q)-1 The aim of control design is to exponentially drive these new coordinates together with the integral defined above, to zero so that feedback control action will enforce the defined constraint to remain invariant After differentiating these new coordinates, we will find: y i q i we can rewrite the equation of From this new reduced order dynamics as: Now considering the linearized dynamics for the scalar I: I g I I g y y g y y g with: ' i ( ) (38) (30) y i q i [ ' ' i ( ) ' i ( ) ] (37) (31) Now using this new set of coordinates, we can derive the dynamics of the system in terms of y i , y i , y i , , , and u, so we can rewrite the In (38) [8] dynamics as: 1 1u y u ' in which 1 must be derived from the equation of reduced dynamics (17) (32) y , y i , , , y , y i , , , y , y i , , , y , y i , , and The coefficients of the equation (28) will be defined as: g I A(t ) gy 0 g y (39) Trang 81 SCIENCE & TECHNOLOGY DEVELOPMENT, Vol.18, No.K6 - 2015 g B (t ) (40) Now the challenging problem of obtaining exponential orbital stability for the nonlinear dynamics (17) has reduced to simpler problem of asymptotically stabilizing the linear system of transverse dynamics (28) 3.3.2 Designing the Controller For aiming the asymptotic stability of the transverse dynamics, the gain variant controller [K1 K2 K3] was used, in which the gains were defined as: (41) The formula for the control law is defined as: u control I K1 K K * y y (42) In equation (34), I, y and y denote the transverse coordinates of the system which we showed how to compute them in the previous section The goal of the feedback control is to drive the transverse coordinates I, y and y of the linear system asymptotically to zero, and this will ensure the exponential orbital stability for the nonlinear system Aiming this, the gains of the controller are obtained with an optimization process in which the cost function (43) is defined as: I C y y Trang 82 2 2 t2 (43) In equation (43), I, y and y are the solution of differential equation (28) in an arbitrary range of time which is denoted by t (which is chosen as 10 seconds in simulations) Another alternative for control design was to use the transition matrix of this periodic motion The transition matrix can be obtained by solving the differential equation of transverse linearized system with the by identity matrix as the initial condition in exactly one time period of the desired periodic motion, so the matrix which contains the last points of the solution is called fundamental matrix If the eigenvalues of this matrix are inside the unit circle, it implies that the controller is stabilizing with any initial condition On this basis, the second norm of the vector of eigenvalues of the fundamental matrix was used as the cost function for the optimization process to find the gains of the controller Equation (44) gives the mathematical expression for this alternative cost function: 1 C 2 3 (44) In this equation i denotes ith eigenvalue of the transition matrix SIMULATION EXPERIMENTAL RESULTS AND In this section, some of the results for three types of typical motions of a Pendubot were proposed These motions are sorted as downwarddownward, downward-upward and upwardupward motions for the first and second arms respectively On the basis of explanations presented in previous section, first the constraint function was chosen, which represents the geometrical relation among the generalized coordinates of the pendubot These functions can be chosen analytically in most of the cases Here a linear constraint function is applied in the form of: TAÏP CHÍ PHÁT TRIỂN KH&CN, TẬP 18, SỐ K6- 2015 k 0 0 (45) In equation (45), 0 and 0 are equilibriums for the first and second link respectively, and and denotes the angel of the first and the second link Considering the constraint function (45), we can plan different trajectories for the Pendubot by choosing different equilibriums and different values for parameter k, which should be selected by considering the sufficient condition for existence of periodic solutions for the equation of reduced dynamics The figures below show the results of simulations for three types of planned motions Fig.5 Results of closed-loop simulations for downward-upward motions with 0 0, and k = -1,7: (a) phase plot of the motion of under-actuated link; (b) how the angle of first link changes during a 10 second period of time; (c) how the angle of second link changes during a 10 second period of time; (d), (e), (f) the states of transverse linearized system are deriving to zero to guarantee the orbital stability of limit cycles Fig.4 Results of closed-loop simulations for downward-downward motions with , and k = -2: (a) phase plot of the motion of under-actuated link; (b) how the angle of first link changes during a 10 second period of time; (c) how the angle of second link changes during a 10 second period of time; (d), (e), (f) the states of transverse linearized system are deriving to zero to guarantee the orbital stability of limit cycles Trang 83 SCIENCE & TECHNOLOGY DEVELOPMENT, Vol.18, No.K6 - 2015 CONCLUSION Fig.6 Results of closed-loop simulations for upwardupward motions with 0 , and 2 k = -0,5: (a) phase plot of the motion of under-actuated link; (b) how the angle of first link changes during a 10 second period of time; (c) how the angle of second link changes during a 10 second period of time; (d), (e), (f) the states of transverse linearized system are deriving to zero to guarantee the orbital stability of limit cycles Trang 84 This paper introduced a novel virtual holonomic constraint approach initially applied for trajectory planning and control design for a Pendubot First the system was modeled using Euler-Lagrange equations of motion and unknown parameters of the model were identified by a nonlinear least square method, using the real data which were captured from the system For trajectory planning, a virtual geometrical relation among the generalized coordinates of the first and second link was defined and then the equation of reduced dynamics was derived Then the sufficient condition for the existence of periodic solutions for this equation was analyzed In the last step of trajectory planning part, the integral of the motion was computed For the control design, a linear approximation of nonlinear dynamics was computed via transverse linearization, and using different methods of optimization, we found the controllers which made the transverse linearized system asymptotically stable, and this guaranteed the exponential orbital stability of limit cycles Results were presented, and approved the precision of the performance of Pendubot motions with this proposed method ACKNOWLEDGEMENT This research is funded by the Ho Chi Minh city University of Technology, VNU-HCM (under Project TSĐH-2015-ĐĐT-04) and the DCSELAB, VNU-HCM, Vietnam TẠP CHÍ PHÁT TRIỂN KH&CN, TẬP 18, SỐ K6- 2015 Hoạch định quỹ đạo điều khiển hệ lắc ngược Pendubot ứng dụng hướng tiếp cận ràng buộc Holonomic Ảo Hồ Phạm Huy Ánh Cao Văn Kiên Trường Đại học Bách Khoa, ĐHQG-HCM, Việt Nam TĨM TẮT mơ hình nhận dạng đầy đủ, toán Bài báo khảo sát hướng tiếp cận ràng hoạch định quỹ đạo điều khiển quăng hệ buộc Holonomic Ảo dùng để hoạch định quỹ lắc ngược kép hồn tất thơng đạo điều khiển hệ lắc ngược kép qua hướng tiếp cận ràng buộc Holonomic Ảo Pendubot Mục tiêu nhằm tạo dao Cốt lõi nằm ưu khả tái thông động đồng hai khớp hệ số hóa quy luật chuyển động hệ Pendubot Sau mơ hình hệ lắc ngược Pendubot thơng qua tương quan tọa độ hình phương trình chuyển động Eulerhọc mà hướng tiếp cận Holonomic Ảo có Lagrange, ta dùng kỹ thuật tối ưu để nhận dạng thơng số mơ hình Dựa Từ khóa: hệ lắc ngược kép Pendubot, hoạch định quỹ đạo điều khiển hệ Pendubot, hướng tiếp cận Ràng buộc Holonomic Ảo, hệ truyền động underactuated bậc tự REFERENCES [1] Nnaedozie Pauling Ikegwuonu Anek “Control of Underactuated MechanicalSystems”, Ph.D dissertation, Technische Universiteit Eindhoven, 2003 [2] Pedro X La Hera, Leonid B Freidovich, Anton S Shiriaev, Uwe Mettin, “New approach for swinging up the Furuta pendulum: Theory and experiments”, Journal of Mechateronics, 2009 Elsevier Ltd [3] Pedro X La Hera, ”Underactuated Mechanical Systems: Contributions to trajectory planning, analysis, and control”, Ph.D dissertation, Umea University, 2010 [4] Mark W Spong, Seth Hutchinson, M Vidyasagar, “Robot Modeling and Control”, first edition, JOHN WILEY & SONS, INC [5] L Freidovich, A Robertsson, A Shiriaev, R Johansson, “Periodic motions of the Pendubot via virtual holonomic constraints:Theory and experiments”, Automatica 44 (2008) 785 – 791, 2007 [6] U Mettin, “Principles for planning and analyzing motion of underactuated mechanical systems and redundant manipulators”, Ph.D dissertation, Umea University, 2010 [7] Anton Shiriaev, John W Perram, and Carlos Canudas-de-Wit, “Constructive Tool for Orbital Stabilization of Underactuated Nonlinear Systems: Virtual Constraints Approach”, IEEE Transaction on Automatic Control, VOL 50, NO 8, AUGUST 2005 [8] Anton S Shiriaev, Member, IEEE, Leonid B Freidovich, Member, IEEE, and Sergei V Gusev, Senior Member, IEEE, “Transverse Linearization for Controlled Mechanical Systems With Several Passive Degrees of Freedom”, IEEE Transaction on Automatic Control, VOL 55, NO 4, April 2010 Trang 85 ... hướng tiếp cận ràng hoạch định quỹ đạo điều khiển quăng hệ buộc Holonomic Ảo dùng để hoạch định quỹ lắc ngược kép hồn tất thơng đạo điều khiển hệ lắc ngược kép qua hướng tiếp cận ràng buộc Holonomic... hướng tiếp cận Holonomic Ảo có Lagrange, ta dùng kỹ thuật tối ưu để nhận dạng thơng số mơ hình Dựa Từ khóa: hệ lắc ngược kép Pendubot, hoạch định quỹ đạo điều khiển hệ Pendubot, hướng tiếp cận Ràng. .. Vietnam TẠP CHÍ PHÁT TRIỂN KH&CN, TẬP 18, SỐ K6- 2015 Hoạch định quỹ đạo điều khiển hệ lắc ngược Pendubot ứng dụng hướng tiếp cận ràng buộc Holonomic Ảo Hồ Phạm Huy Ánh Cao Văn Kiên Trường