Pendubot trajectory planning and control using virtual holonomic constraint approach

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.


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.
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 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.
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 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
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], ) ( ) , ( ) , ( q P q q K q q L     (1) With the definition above, the equations of motion for a controlled mechanical system with several degrees of freedom can be written as: (2) In which qi is the vector of generalized coordinates and i q  is vector of generalized velocities and u is vector of independent control inputs and (B(q)u)i denote generalized forces.
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 ) , ( q q C  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 Using this model, and after identifying the parameters of the model, the motion planning and control design of the Pendubot will be concerned.

System Identification
The parameters p1 to p5 that were used in previous section are defined as: (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 of the physical parameters of the Pendubot setup were known.These values are shown in Table 1.
Table 1: Known Parameters of the investigated Pendubot [6] 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) 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: 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 = 6 is used and the link is tracking a reference signal that is shown in 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 2.  Validating data that was captured from the real system showed the precision of the estimated parameters.

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 θ: 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: and its first and second derivatives as: 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): . So the derivation of ( 17) is 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 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.
Last step in motion planning via virtual holonomic constraint, is computing the integral of reduced dynamics (17) which is always integrable, provided is not zero.
Theorem1: Suppose that the function of ( 17) with initial conditions exists and is continuously differentiable, then along this solution the function: preserves its zero value" [7].
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.

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.

Transverse Linearization
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 (2-28), in which asymptotic stability will ensure the exponential orbital stability of limit cycles of nonlinear system.
First we change the coordinates of the system to obtain a new set of coordinates which can be written as: 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: Now using this new set of coordinates, we can derive the dynamics of the system in terms of and u, so we can rewrite the dynamics as: 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: Substituting ( 34) in ( 33) and then ( 33) into (32), we will find: The coefficients of the equation ( 28) will be defined as: 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).

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: 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: 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 3 by 3 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:

SIMULATION AND EXPERIMENTAL RESULTS
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: In equation ( 45

CONCLUSION
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.

Fig. 1 .
Fig.1.The picture of the Pendubot [5], first link is actuated and second link is passive

Figure 3
Figure3shows the mapped friction for the actuated link.
) [8]  and  must be derived from the equation of reduced dynamics (17).


denotes i th eigenvalue of the transition matrix.
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.