Modelling and Control of a Spherical Inverted ... - SAGE Journals

International Journal of Advanced Robotic Systems

ARTICLE

Modelling and Control of a Spherical Inverted Pendulum on a Five-Bar Mechanism Regular Paper

Israel Soto1* and Ricardo Campa2 1 Institute of Engineering and Technology, Universidad Autónoma de Ciudad Juárez, Ciudad Juárez, Chihuahua, Mexico 2 Division of Graduate Studies and Research, Instituto Tecnológico de la Laguna, Torreón, Coahuila, Mexico *Corresponding author(s) E-mail: [email protected] Received 11 June 2014; Accepted 28 November 2014 DOI: 10.5772/60027 Licensee InTech. This is an open access article distributed under the terms of the Creative Commons Attribution License (http:// creativecommons.org/licenses/by/3.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

Abstract This paper describes the kinematics and dynamics model‐ ling of a mechanical system consisting of a spherical inverted pendulum whose base is mounted on a parallel planar mechanism, better known as a five-bar mechanism. The whole mechanism has four degrees of freedom, but it has only two actuators and so it is an under-actuated system. The nonlinear dynamics model of the complete system is first obtained using a non-minimal set of gener‐ alized coordinates, and then a reduced equivalent model is computed. To validate this approach, the reduced model is linearized and simulations are carried out, showing the stabilization of the system with a simple LQR controller. Experimental results on an academic prototype are also presented.

decades [1]. There exist a wide variety of inverted-pendu‐ lum-type systems, such as the pendubot, the acrobot, the pendulum on a cart, the inertial wheel pendulum and the Furuta pendulum (see, e.g., [2 - 5]). All these systems are under-actuated [6], that is, they are systems with fewer actuators than degrees of freedom (dof). The degree of under-actuation is indeed the difference between the number of dof and the number of actuators. All the examples of inverted pendulums mentioned above have a degree of under-actuation of one. This paper deals with the modelling and control of a spherical inverted pendulum (SIP), which is another member of the family of inverted pendulum systems with degree of under-actuation of two.

1. Introduction

The system consists of a rigid rod coupled in its base to an under-actuated universal joint in such a way that the extreme of the rod moves over a spherical surface with its centre at the base of the rod (see Figure 1). As such, through the motion of the base of the pendulum on the horizontal plane it is possible to balance the extreme of the rod in its upright position.

The problem of balancing an inverted pendulum has attracted the attention of control researchers in recent

The SIP has been studied by numerous researchers (see [7 - 11]). The interest in studying the SIP is due to its mathe‐

Keywords modelling, inverted pendulum, parallel mech‐ anism, under-actuated systems

Int J Adv Robot Syst, 2015, 12:0 | doi: 10.5772/60027

1

among all the elements of the system. For open-chain mechanisms (OCMs), this is a direct task and there are several methods to do it, producing a set of ordinary differential equations (ODEs). On the other hand, for a closed-chain mechanism (CCM) the existence of loops originates simultaneous algebraic constraint equations which cannot be solved explicitly. Therefore, the equations of motion for a CCM are a set of differential-algebraic equations (DAEs) [16]. We should mention that in our laboratory there exists a prototype of a SIP+FBM with some parts of the commercial system [15]. The motor drives that we use to control the system allow us to work in current (torque) mode [17]. The purpose of this paper is threefold. First, we recall the theory concerning the modelling of mechanisms with constraints, and specifically the projection methods for simplifying the resulting equations. Secondly, we apply such methods for obtaining the kinematics and dynamics model of the SIP+FBM system. At the end, we show the validity of such a model by implementing an LQR control‐ ler in both simulations and experiments in our prototype. Figure 1. Spherical inverted-pendulum on a five-bar mechanism

matical model, which is considered as a simplified model of a rocket-propelled body or a building, such that it can be used for the control of the position of a rocket, for the control of the oscillations of buildings, or simply for the study of new control techniques. According to [12], there exists a classification of SIP-type systems, depending on the mechanism used to stabilize the SIP, which can be:

2. Modelling of constrained mechanical systems It is worth mentioning that most of the theory presented in this section is not novel, and can be found, for example, in [18] and [16].

1.

An XY-table [7].

2.

An omnidirectional vehicle [13].

2.1 Kinematics model

3.

A robotic arm [8, 14].

Consider a CCM with m joints but with only n (n < m) dof; then, there should exist p = m − n holonomic constraints which allow the reduction of the order of the system from m to n ; in others words, if we choose n independent joint variables there must exist p dependent joint variables.

In this document, it is considered that the motion of the base of the SIP is controlled by means of a five-bar mechanism (FBM), which is a well-known closed-chain mechanism. Such a system has become common, due to the existence of a commercial prototype for academic and research purpos‐ es (see [15]). The complexity of the mathematical model and control of this system (SIP+FBM) is due in great measure to the kinematic constraints imposed by the closed-loop of the FBM. To the best of the authors' knowledge, the only work dealing with the modelling and control of a similar SIP +FBM mechanism is [12]. In that work, the authors consider the system as a linear parameter variable (LPV) system with structured additive perturbation. This paper uses a different approach to obtain the complete non-reduced dynamics model of the same system. The equations that govern the motion of any mechanical system must take into account the kinematic constraints 2

The remainder of this paper is organized as follows. Section 2 is devoted to the modelling of constrained systems in general. The kinematics and dynamics models of the system under study are obtained in Section 3. The valida‐ tion of the models via simulations/experiments with an LQR controller is shown in Section 4, while final conclu‐ sions are given in Section 5.


Let ρ = q T β T T ∈ ℝm be the vector of generalized joint variables (q ∈ ℝn be the vector of independent variables and β ∈ ℝ p be the vector of dependent, or constrained, varia‐ bles). The holonomic constraints can then be grouped in a vector of the form

g ( r ) = 0 Î ¡p so that, as a result, it should be possible to express β as a function of q, , i.e.,

b = b (q).

The direct kinematics problem consists in finding expres‐ sions for the pose (i.e., position and orientation) variables of an element of interest of the CCM, described by the vector x in terms of the actuated joint variables; this can be expressed as

(

)

x = f q, b ( q ) . In addition, by taking the time derivative of the previous equation we get the so-called differential kinematics equation x& = J ( r ) q& where J (ρ ) =

(1)

∂ f (q, β (q )) . ∂q

On the other hand, the inverse kinematics problem consists in establishing the value of the actuated joint coordinates that correspond to a given pose of the element of interest, and can be expressed as q = f -1 ( x ) . The inverse kinematics problem can be solved in general form by geometric or analytic approaches, such as those mentioned in [19].

System (2) has a differential index μ if μ is the minimal number of analytical differentiations h ( u , u& , t ) = 0 ,

The most general form of a differential-algebraic system of

n equations is that of an implicit differential equation [21]

é h1 ( u , u& , t ) ù ê ú ê h2 ( u , u& , t ) ú n & h ( u, u,t ) = ê ú = 0Î¡ M ê ú ê hn ( u , u& , t ) ú ë û

̣

du

d m h ( u , u& , t ) dt m

=0

(3)

According to [16], if the differential index is 1 or 2, it is usually possible to differentiate the constraint equation with respect to time once (index 1) or twice (index 2) to produce an equivalent ODE (a process sometimes called index reduction) and to achieve satisfactory results using standard numerical methods for ODEs. If the differential index is greater than 2, standard numerical methods frequently fail to converge, and sometimes converge to incorrect solutions. These DAEs resulting from the dynamical analysis of CCMs have a differential index of 3. Several methods have been proposed in the literature in order to transform them into a set of ODEs with fewer generalized coordinates so that they can be solved analytically or numerically. The most important of such methods are [20]: 1.

Direct elimination, where the surplus variables are eliminated directly, using the algebraic constraints given by the kinematics, to explicitly reduce index-3 DAEs to ODEs in a minimal set of generalized coordi‐ nates (see [22]).

2.

Explicit Lagrange-multiplier computation, using the unknown accelerations computed from the index-1 DAEs formed by appending the acceleration con‐ straints (computed by differentiating the original constraint) to the system equations (see [23] and [24]).

3.

Projection of the dynamics onto the tangent space of the constraint manifold, where the constraint-reaction dynamics equations are taken into the orthogonal and tangent subspaces of the vector space of the system generalized velocities. A family of choices exist for the projection, as surveyed by [25] and [26].

The dynamics model of a mechanism of n dof, with m generalized variables (m > n ), and subject to p = m − n holonomic constraints, can be represented by means of the following index-3 DAEs T

(2)

where u ∈ ℝn is the vector of generalized coordinates, t is the time variable, and u = dt .

dt

= 0 ,K

such that the system of equations (3) allows us to extract, by algebraic̣ manipulations, an explicit ordinary differen‐ tial system u = φ (u ) (which is called the 'underlying' ODE)

2.2 Dynamics model As mentioned in [20], the dynamics model of CCMs using either the Newton-Euler or the Euler-Lagrange methodol‐ ogy is traditionally obtained by cutting the closed loops to obtain systems with open chains and/or tree-type struc‐ tures, so that it is possible to write a set of ODEs for such chains in their corresponding generalized coordinates. The solution to these equations requires satisfying additional algebraic equations guaranteeing the closure of the cutopen loops. To this end, some Lagrange multipliers (which physically represent the forces due to the constraints) are employed. The resulting formulation, often referred to as a descriptor form, yields a system of DAEs which, in general, cannot be solved employing conventional ODE techniques.

dh ( u , u& , t )

æ ¶g ( r ) ö d ì ¶L( r , r& ) ü ¶L( r , r& ) =tr + ç í ý÷ l dt î ¶r& þ ¶r è ¶r ø

(4)

g ( r ) = 0,

(5)

̣

where the Lagrangian L(ρ,ρ ) can be defined as Israel Soto and Ricardo Campa: Modelling and Control of a Spherical Inverted Pendulum on a Five-Bar Mechanism

3

L ( r , r& ) = K ( r , r& ) - U ( r )

(6)

̣

being K(ρ,ρ ) for the kinetic energy and U(ρ ) for the potential energy of the mechanism; τρ ∈ ℝm is the vector of applied joint forces, and λ ∈ ℝ p establishes the vector of Lagrange multipliers which ensure the fulfilment of the constraints in (5). Equation (4) can be rewritten as M ( r ) r&& + C ( r , r& ) r& + g ( r ) = t r + D

T

(r )l ̣

Moreover, as explained in [27], the following properties are satisfied for M (q ) and g (q ) 1 T r& M ( r ) r& 2

g (r ) =

¶U ( r ) ¶r

Using such a matrix R(ρ), it is possible to reduce the system (7) to a new index-1 DAE system && + C ( r , r& ) q& + g ( r ) = t , M (r )q

g (r ) = 0,

(7)

where M (ρ ) ∈ ℝm×m is the inertia matrix, C (ρ,ρ ) ∈ ℝm×m is the matrix of terms generated by centrifugal and Coriolis forces, g (ρ ) ∈ ℝm represents the vector of gravitational forces, and D (ρ ) = ∂ γ (ρ ) / ∂ ρ ∈ ℝ p×m is called the constraint Jacobian matrix.

K ( r , r& ) =

variables q (which can be extracted from ρ through a function α , such that q = α(ρ)), and the constraints γ(ρ). Appendix A describes a procedure to obtain R(ρ) starting from these relations.

q = a (r )

(13)

where ¯ (ρ ) = R (ρ )T M (ρ )R (ρ ) M ̣ ̣ ̣ ̣ ¯ (ρ,ρ ) = R (ρ )T C (ρ,ρ )R (ρ ) + R (ρ )T M (ρ ) R (ρ,ρ ) C g¯ (ρ ) = R (ρ )T g (ρ )

τ¯ = R (ρ )T τρ

(8)

and notice thaṭ the states which define the dynamics of the system are q , q and ρ , which can be obtained by integrating the corresponding state equations.

(9)

Witḥ the matrix M (ρ), it is possible to compute the matrix C(ρ,ρ ) by means of the Christoffel symbols cijk (ρ) defined as cijk ( r ) =

1 é ¶M kj ( r ) ¶M ki ( r ) ¶Mij ( r ) ù + ê ú, 2 êë ¶ri ¶rj ¶rk úû

(10)

where M ij (ρ ) denotes the ij th element of the inertia matrix ̣ ̣ M (ρ ). Indeed, the kj th element Ckj (ρ,ρ ) of the matrix C (ρ,ρ )

is given by

C kj ( r , r& ) = éëc1 jk ( r ) c2 jk ( r ) L cnjk ( r ) ùû r&

(11)

The projection of dynamics (7) onto the tangent space of the constraint manifold (5) consists on finding a matrix R (ρ ) ∈ ℝm×n whose column space belongs to the null space ( ) ( ) of ̣ D(ρ), i.e., D ρ R ρ = 0. All feasible dependent velocities ρ of a constrained body belong to the space spanned by the columns of R (ρ ), which is parameterized by a vector of ̣ independent velocities q ∈ ℝn , obtaining the expression for the feasible dependent velocities

r& = R ( r ) q& .

(12)

The matrix R(ρ) for a particular system depends on the selection of the generalized coordinates ρ , the independent 4


Figure 2. Schematic diagram of the SIP+FBM

3. Modelling of the mechanism under study In this section, the kinematics and dynamics models of a SIP+FBM system, as the one shown in Figure 2, are ob‐ tained. This figure also shows the frames attached to each extreme of the FBM (Σ0 and Σ0'), as well as those of the base

(Σb) and the extreme (Σp ) of the pendulum. In general, Σi is considered to be formed by the axes xi ,yi , zi .

Figure 3. Geometric relations of the FBM

that rotates with respect to the axis yb and ϕ is the angle that

where the notations S( ⋅ ) and C( ⋅ ) are used for sin( ⋅ ) y cos( ⋅ ), respectively. Using a software tool such as MAT‐ LAB, it is possible to find the following two solutions for xb

rotates with respect to the axis xb.

and yb

The angles q1 and q2 of the active joints of the FBM determine

xb =

The angles θ and ϕ indicate the orientation of the extreme of the pendulum with respect to the frame Σb ; θ is the angle

its configuration. The angles β1 and β2 of the passive joints

depend upon q1 and q2. For the purposes of analysis, in this paper the vector of independent joint variables is defined as q = éëq1 f q

q2 ùû

T

(14)

and the vector of pose variables of the pendulum as x = éë xb

T

The problem of direct kinematics is reduced to find the coordinates of the intersection points of a circle of radius L with C1 and C2 as centres, as shown in Figure 3(a), the equations of which are

) + (y 2

b

- LS ( q1 )

( x - ( 2 L + LC ( q ))) + ( y 2

b

2

±

−

C (q1 − q2) + 2C (q1) − 2C (q2) − 1 C (q1 − q2) + 2C (q1) − 2C (q2) − 3

L S q + S (q2) 2 ( 1) L C (q2) − C (q1) + 2 2

−

C (q1 − q2) + 2C (q1) − 2C (q2) − 1 . C (q1 − q2) + 2C (q1) − 2C (q2) − 3

æ y - LS ( q1 ) ö b1 ( q ) = tan -1 ç b ÷ ç x - LC ( q ) ÷ 1 ø è b æ ö yb - LS ( q2 ) ÷ b 2 ( q ) = tan -1 ç ç xb - ( 2 L + LC ( q2 ) ) ÷ è ø

3.1.1 Direct kinematics model

- LC ( q1 )

yb =

L S q −S q 2 ( 1) ( 2)

(15)

yb f q ùû .

3.1 Kinematics model

b

±

Furthermore, from Figure 3 (b) it is possible to obtain the following relations

The lengths of the four mobile links are considered equal to L , and the length of the pendulum is L p .

(x

L 2 + C (q1) + C (q2) 2

b

)

2

= L2 ,

- LS ( q2 )

)

2

= L2 ,

3.1.2 Inverse kinematics model From Figure 3 (a), the following expressions can be defined xb = LC ( q1 ) + LC ( q1 + b1 )

(16)

yb = LS ( q1 ) + LS ( q1 + b1 )

(17)

xb = 2 L + LC ( q2 ) + LC ( q2 + b 2 )

(18)

Israel Soto and Ricardo Campa: Modelling and Control of a Spherical Inverted Pendulum on a Five-Bar Mechanism

5

yb = LS ( q2 ) + LS ( q2 + b 2 )

x& bC ( qi + b i ) + y& bS ( qi + b i ) = LS ( b i ) q& i

(19)

and on the other hand, from Figure 3 (b) it is observed that xb = rC (a ) and yb = rS (a )

(20)

where

(25)

and by taking i = 1,2 in (25), we find equation that ̣ the matrix ̣ relates the operational velocities x b and y b with the actuated ̣

̣

joint velocities q 1 and q 2

é C ( q1 + b1 ) S ( q1 + b1 ) ù é x& b ù éS ( b1 ) 0 ù é q&1 ù ê úê ú = Lê ú ê ú. & S ( b 2 ) úû ëq& 2 û êëC ( q2 + b 2 ) S ( q2 + b 2 ) úû ë yb û êë 0

æy ö r = xb2 + yb2 and a = tan -1 çç b ÷÷ è xb ø

(21)

2q1 + b1 = 2a

(22)

Now, from the differential kinematics equation (1), consid‐ ering (14) and (15), we can show that

and also

As such, making some algebraic operations with equations (16), (17) and (20)-(22), it yields æ x2 + y 2 æy ö b q1 = tan -1 çç b ÷÷ m cos -1 ç b ç 2 x L è bø è æ x2 + y 2 b b1 = ±2cos ç b ç 2L è -1

ö ÷ ÷ ø

æ ç b 2 = m2cos -1 ç ç è

( (

2L

(

)

)

2

which leads to é b&1 ù ê & ú = Jb q, b ( q ) ëb2 û

(

ö +y ÷ ÷ ÷ ø 2 b

é q&1 ù

) êq& ú

(26)

ë 2û

where −

ö + yb2 ÷ ÷ ÷ ø

J β (q,β (q )) =

S(q1 − q2 − β2) −1 S(q1 − q2 + β1 − β2) S(β1) S (q1 − q2 + β1 − β2)

− −

S (β2) S(q1 − q2 + β1 − β2)

S(q1 − q2 + β1) −1 S(q1 − q2 + β1 − β2)

.

3.1.3 Differential kinematics model

Equation (26) establishes the relation between the actuated and passive joint velocities of the FBM.

Taking the time derivative of (16)-(19) for i = 1,2, we obtain

3.2 Dynamics modelling

(

)

(23)

(

)

(24)

x& b = - LS ( qi ) q& i - LS ( qi + b i ) q& i + b&i y& b = LC ( qi ) q& i + LC ( qi + b i ) q& i + b&i

Following the approach proposed in [18], let us consider that the system in Figure 2 is cut out at point C, producing two separated OCMs: one with four dof formed by the left leg of the FBM and the SIP, and one with two dof formed by the right leg. Now, it is possible to use as a vector of generalized variables ρ = q1 β1 ϕ θ q2 β2

As such, multiplying the equations (23) and (24) by

cos(qi + βi ) and sin(qi + βi ), respectively, and adding the

results, we further obtain 6

(

C ( q1 ) q&1 + C ( q1 + b1 ) q&1 + b&1 = C ( q2 ) q& 2 + C ( q2 + b 2 ) q& 2 + b&2 .

2L

2

) )

-S ( q1 ) q&1 - S ( q1 + b1 ) q&1 + b&1 = -S ( q2 ) q& 2 - S ( q2 + b 2 ) q& 2 + b&2

ö ÷. ÷ ø

( xb - 2 L )

( xb - 2 L )

-S ( q1 + b1 ) S ( b 2 ) ù ú S ( q2 + b 2 - q1 - b1 ) ú ú C ( q1 + b1 ) S ( b 2 ) ú . 0 0 S ( q2 + b 2 - q1 - b1 ) ú ú ú 1 0 0 ú 0 1 0 û 0 0

Moreover, from (23) and (24), we get

Following a similar procedure, but starting from equations (18) and (19), we get æ æ yb ö -1 ç q2 = tan -1 çç ÷÷ m cos ç è xb - 2 L ø ç è

é S ( q2 + b 2 ) S ( b 1 ) ê ê S ( q2 + b 2 - q1 - b1 ) ê -C ( q2 + b 2 ) S ( b1 ) J(r ) = Lê êS(q + b - q - b ) 2 2 1 1 ê ê 0 ê 0 ë


T

∈ ℝ6.

In the following, we show how to obtain each of the elements of equation (7) and then how to get the reduced system (13).

The total kinetic energy of the system can be divided in: 0 4

K ( r , r& ) = åKi ( r , r& ) + Kp ( r , r& ) i =1

(27)

éC ( q1 ) + C ( q1 + b1 ) ù ê ú pb = L ê S ( q1 ) + S ( q1 + b1 ) ú , ê ú 0 ë û

where Ki is the kinetic energy associated with the link i of the FBM and K p is the kinetic energy associated with the pendulum; Ki and K p can be obtained by [27] Ki =

1 1 m p& T p& + w T I w 2 i i i 2 i i i

(28)

Kp =

1 1 m p& T p& + w T I w 2 p p p 2 p p p

(29)

̣

b

0 b

éC ( q1 + b1 ) -S ( q1 + b1 ) 0 ù ê ú R = ê S ( q1 + b1 ) C ( q1 + b1 ) 0 ú ê ú 0 0 1û ë

As the FBM is located in the XY plane, the angular velocities for each link can be defined as

where p i and ωi represent the linear and angular velocities of the centre of mass of the link i , respectively, mi and I i are the mass anḍ the inertia moment of the link i , respectively, and where p p and ω p represent the linear and angular velocities of the centre of mass of the pendulum, respective‐ ly (considering all the vectors with respect to frame Σ0). The position vectors of the centres of mass of each link for the FBM are

é S (q ) ù ê ú p p = lcp ê -C (q ) S (f ) ú ê ú ë C (q ) C (f ) û

w1 = éë0 0 q&1 ùû

T

T w 2 = éë0 0 q&1 + b&1 ùû

T T w 4 = ëé0 0 q& 2 ùû w 3 = éë0 0 q& 2 + b&2 ùû

while the angular velocity of the pendulum with respect to frame Σ0 is given by [28]

élc 1C ( q1 ) ù élc 4C ( q2 ) ù ê ú ê ú p1 = ê lc 1S ( q1 ) ú , p4 = ê lc 4S ( q2 ) ú ê ú ê ú ë 0 û ë 0 û

é r&1 ù 1 ê ú é ù w p = ëS ( r1 ) S ( r2 ) S ( r3 ) û ê r&2 ú 2 ê r&3 ú ë û

(30)

where S( ⋅ ) is a skew-symmetric matrix such that, for a

é LC ( q1 ) + lc 1C ( q1 + b1 ) ù ê ú p2 = ê LS ( q1 ) + lc 1S ( q1 + b1 ) ú , ê ú 0 ë û

vector v = v1 v2 v3 T , is defined as é 0 ê S ( v ) = ê v3 ê - v2 ë

é 2 L + LC ( q2 ) + lc 3C ( q2 + b 2 ) ù ê ú p3 = ê LS ( q2 ) + lc 3S ( q2 + b 2 ) ú , ê ú 0 ë û

- v3 0 v1

v2 ù ú - v1 ú 0 úû

and the vectors r1,r2 and r3 are the columns of the rotation

matrix from frame Σp with respect to frame Σ0, the elements

where lci is the distance from a joint to the centre of mass of

link i , as shown in Figure 2. The position vector of the centre of mass of the pendulum is described by the equation

of which are

r11 = C (q1 + β1)C (θ ) − S (q1 + β1)S (ϕ )S (θ ) r12 = − S (q1 + β1)C (ϕ ) r13 = C (q1 + β1)S (θ ) + S (q1 + β1)S (ϕ )C (θ )

p p = 0 pb + b0 Rb p p

r21 = S (q1 + β1)C (θ ) + C (q1 + β1)S (ϕ )S (θ )

where 0 pb is the position vector of the base of the SIP with

respect to frame Σ0, b p p is the position vector of the centre

of mass of the pendulum with respect to Σb, and b0R is the

rotation matrix of Σb with respect to Σ0, which are repre‐

sented as

r22 = C (q1 + β1)C (ϕ ) r23 = S (q1 + β1)S (θ ) − C (q1 + β1)S (ϕ )C (θ ) r31 = − C (ϕ )S (θ ) r32 = S (ϕ )


7

r33 = C (ϕ )C (θ ).

m22 = I 2 + I px + L 2mp + lc22m2 + lcp2 mp

− (I px − I pz + lcp2 mp )C (ϕ )2C (θ )2 + 2L lcp mp S (θ )

Therefore, using (30), we get éC ( q1 + b1 )f& - S ( q1 + b1 ) C (f )q& ù ê ú w p = êS ( q1 + b1 )f& + C ( q1 + b1 ) C (f )q& ú . ê ú q&1 + b&1 + S (f )q& ë û Moreover, in equation (28) we require the mass of each link of the FBM as well as the matrix of inertia moments with respect to a coordinate frame that passes through the centre of mass. Again, taking into consideration that the FBM moves in the horizontal plane, only the inertia moments along the z0 axis are of interest for the links, namely I 1,I 2,I 3 and I 4. For the SIP, the matrix of the inertia moments is

m23 = − C (ϕ )C (θ ) S(θ)( I px − I pz + lcp2 mp ) + L lcp mp m24 = S (ϕ ) I px + lcp2 mp + L lcp mp S (θ ) m33 = I pz + ( I px − I pz + lcp2 mp )C (θ )2 m44 = I pz + lcp2 mp m55 = m4lc42 + m3 L

2

+ lc32 + 2L lc3C (β2) + I 4 + I 3

m56 = m3 lc32 + L lc3C (β2) + I 3 m66 = m3lc32 + I 3

Using expressions (10) and (11), we can compute

given by

0

é I px ê Ip = R ê 0 ê êë 0

0ù ú 0 ú 0b RT ú I pz úû

0

0 b

I px 0

It is possible to use software for symbolic computing to obtain the matrices of the dynamics model (7). In order to ̣ obtain the matrix M (ρ ), the total kinetic energy K(ρ,ρ ) is calculated and (8) is used, yielding é m11 ê ê m12 êm M ( r ) = ê 13 ê m14 ê 0 ê êë 0

m12 m22 m23 m24 0

m13 m23 m33 m43 0

m14 m24 m34 m44 0

0

0

0

0 0 0 0 m55 m55

0 0 0 0

ù ú ú ú ú ú m56 ú ú m66 úû

where m11 = I 1 + I 2 + I px + L 2m2 + 2L 2mp + lc12m1 + lc22m2 + lcp2 mp − I px C (ϕ )2C (θ )2 + I pz C (ϕ )2C (θ )2 + 2L 2mp c (β1) − lcp2 C (ϕ )2C (θ )2 + 2L lc2m2C (β1) + 4L lcp mp C (β1 / 2)2S (θ ) + 2L lcp mp S (β1)C (θ )S (ϕ ) m12 = I 2 + I px + L 2mp + lc22m2 + lcp2 mp + L 2mp C (β1) + I pz C (ϕ )2C (θ )2 − I px C (ϕ )2C (θ )2 − lcp2 C (ϕ )2C (θ )2

+ L lc2m2C (β1) + 2L lcp mp S (θ ) + L lcp mp C (β1)S (θ )

+ L lcp mp S (β1)C (θ )S (ϕ )

m13 = − C (ϕ )C (θ ) L lcp mp 1 + C (β1) − C (ϕ )C (θ )(I px − I pz +

lcp2 mp

)S (θ )

m14 = lcp2 mp S (ϕ ) + 2L lcp mp S (ϕ )S (θ )C (β1 / 2)2 + I px S (ϕ ) + L lcp mp S (β1)C (θ )

8

é c11 c12 ê êc21 c22 êc c C ( r , r& ) = ê 31 32 êc41 c42 ê0 0 ê 0 êë 0


c13 c23 c33 c43 0

c14 c24 c34 c44 0

0

0

0 0 0 0 c55 c65

ù ú ú ú ú ú c56 ú ú c66 úû

where each element of the matrix is given by

̣ ̣ ̣ c11 = a1β 1 + a2ϕ + a3θ ̣ ̣ ̣ ̣ c12 = a1(q 1 + β 1) + a2ϕ + a3θ ̣ ̣ ̣ ̣ c13 = a2(q 1 + β 1) + a4ϕ + a5θ ̣ ̣ ̣ ̣ c14 = a3(q 1 + β 1) + a5ϕ + a6θ ̣ ̣ ̣ c21 = − a1q 1 + a7ϕ + a8θ ̣ ̣ c22 = a7ϕ + a8θ ̣ ̣ ̣ ̣ c23 = a7(q 1 + β 1) + a9ϕ + a10θ ̣ ̣ ̣ ̣ c24 = a8(q 1 + β 1) + a10ϕ + a11θ ̣ ̣ ̣ c31 = − a2q 1 − a7β 1 + a12θ ̣ ̣ ̣ c32 = − a7(q 1 + β 1) + a12θ ̣ c33 = a13θ ̣ ̣ ̣ c34 = a12(q 1 + β 1) + a13ϕ = − c43 ̣ ̣ ̣ c41 = − a3q 1 − a8β 1 − a12ϕ ̣ ̣ ̣ c42 = − a8(q 1 + β 1) − a12ϕ

c44 = 0

̣ c55 = − m3 L lc3S (β2)β 2 ̣ ̣ c56 = − m3 L lc3S (β2)(q 2 + β 2) ̣ c65 = m3 L lc3S (β2)q 2

c66 = 0

0 0 0 0

and the relation q = α (ρ ) is simply defined by

with a1 = L lcp mp C (β1)C (θ )S (ϕ )

é1 ê 0 q=ê ê0 ê ëê0

− LS (β1)(lc2m2 + L mp + lcp mp S (θ ))

a2 = C (ϕ )C (θ ) L lcp mp S (β1)

+ C (ϕ )C (θ )(I px − I pz + lcp2 mp )C (θ )S (ϕ )

a3 = − L lcp mp S (β1)S (θ )S (ϕ ) + C (θ ) L lcp mp (1 + C (β1)) + C (θ )(I px − I pz + lcp2 mp )C (ϕ )2S (θ )

Thus, matrix R (ρ ), which satisfies (12), is defined as

a4 = C (θ )S (ϕ ) L lcp mp (1 + C (β1))

é 1 ê ê J b11 ê 0 R( r ) = ê ê 0 ê ê 0 êJ ë b21

+ C (θ )S (ϕ )(I px − I pz + lcp2 mp )S (θ )

a5 =

1 C (ϕ ) I px + lcp2 mp − (I px − I pz + lcp2 mp )C (2θ ) 2 + 2C (ϕ ) L lcp mp C (β1 / 2)2S (θ )

a6 = L lcp mp 2C (β1 / 2)2C (θ )S (ϕ ) − S (β1)S (θ ) a7 = (I px − I pz + lcp2 mp )C (ϕ )S (ϕ )C (θ )2 a8 = C (θ ) L lcp mp + ( I px − I pz + lcp2 mp )C (ϕ )2S (θ )

1 C (ϕ ) I px + lcp2 mp − (I px − I pz + lcp2 mp )C (2θ ) 2 + C (ϕ ) L lcp mp C (β1 / 2)2S (θ )

a11 = L lcp mp C (θ )S (ϕ ) a12 = −

a13 = − ( I px − I pz +

J β11 = −

S (q1 − q2 − β2) −1 S (q1 − q2 + β1 − β2)

J β12 = −

S (β2) S (q1 − q2 + β1 − β2)

J β21 =

1 C (ϕ ) I px + lcp2 mp + (I px − I pz + lcp2 mp )C (2θ ) 2 lcp2 mp

0 0 0 0

0 ù ú Jb ú 12 ú 1 0 0 ú 0 1 0 ú ú 0 0 1 ú 0 0 Jb ú 22 û

where

a9 = C (θ )S (ϕ ) L lcp mp + (I px − I pz + lcp2 mp )S (θ ) a10 =

0 0 0 0 0ù ú 0 1 0 0 0ú r 0 0 1 0 0ú ú 0 0 0 1 0 ûú

S (β1) S (q1 − q2 + β1 − β2)

J β22 = −

)C (θ )S (θ )

Finally, for the vector of gravitational forces, the total potential energy (which only depends of the potential energy of the SIP) is obtained using

S (q1 − q2 + β1) −1 S (q1 − q2 + β1 − β2)

The matrices of the reduced model of the system (7) are defined as é mr 11 ê m M ( r ) = ê r 12 ê mr 13 ê ëê mr 14

U ( r ) = - mp g o T p

where go is a vector that indicates the gravitational accel‐

mr 12 mr 22 mr 23 mr 24

mr 13 mr 23 mr 33 mr 34

mr 14 ù ú mr 24 ú mr 34 ú ú mr 44 ûú

cr 13 cr 23 cr 33 cr 43

cr 14 ù ú cr 24 ú cr 34 ú ú cr 44 ûú

eration with respect to Σ0. Then, by applying (9) we get g(r ) =

¶U ( r ) ¶r

= mglcp

(

¶ C (q ) C (f )

é cr 11 cr 12 ê c c C ( r , r& ) = ê r 21 r 22 êcr 31 cr 32 ê ëêcr 41 cr 42

)

¶r

hence where the components of each one are given by g ( r ) = - mglcp éë0 0 S (f ) C (q ) C (f ) S (q ) 0 0 ùû . T

mr 11 = m66 J β221 + m22 J β211 + 2m12 J β11 + m11 mr 12 = m13 + J β11m23

The constraints of the system are given by

mr 13 = m14 + J β11m24

é L éC ( q1 ) + C ( q1 + b1 ) - 2 - C ( q2 ) - C ( q2 + b 2 ) ù ù ûú g (r ) = ê ë ê L éS ( q1 ) + S ( q1 + b1 ) - S ( q2 ) - S ( q2 + b 2 ) ù ú ë û û ë

mr 14 = J β21m56 + J β12(m12 + J β11m22) + J β21 J β22m66 mr 22 = m33


9

Figure 4. Schematic diagram of the SIP+FBM system

mr 23 = m34 mr 24 = J β12m23 mr 33 = m44 mr 34 = J β12m24 mr 44 = m66 J β222 + m22 J β212 + 2m56 J β11 + m55

̣ cr 11 = c66 J β221 + J β21m66 J β21 + c11 + c21 J β11 ̣ + J β11(c12 + c22 J β11) + J β11(m12 + J β11m22)

̣ cr 44 = c22 J β212 + J β12m22 J β12 + c55 + c65 J β22 ̣ + J β22(c56 + c66 J β22) + J β22 p (m56 + J β22m66)

and the vector of gravitational forces is defined as g ( r ) = - mp glcp éë0 0 S(f )C(q ) C(f )S(q ) 0 0 ùû

T

4. Model validation via real-time control

cr 12 = c13 + c23 J β11

4.1 Description of the prototype

cr 13 = c14 + c24 J β11

The prototype in our laboratory was integrated from different commercial components. Instead of acquiring the whole Quanser system in [15], we only purchased the FBM and the SIP as individual parts. An aluminium base for the mechanism was made, and two motors with encoders (model 2224U006 from Faulhaber) were coupled to it as the actuators for the FBM. Some servo amplifiers (model BE15A8 from Advanced Motion Control (AMC)) were employed to operate the motors in current (torque) mode. A power module (model PS2X3W24, also from AMC) was used to supply the required power.

̣ cr 14 = J β12(c12 + c22 J β11) + J β12(m12 + J β11m22) ̣ + c65 J β21 + c66 J β21 J β22 + J β21 J β22m66 ̣ cr 21 = c31 + J β11m23 + c32 J β11

cr 22 = c33 cr 23 = c34 ̣ cr 24 = J β12m23 + c32 J β12 ̣ cr 31 = c41 + J β11m24 + c42 J β11 cr 32 = c43 cr 33 = c44 ̣ cr 34 = J β12m24 + c42 J β12

̣ cr 41 = J β21(c56 + c66 J β22) + J β21(m56 + J β22m66) ̣ + c21 J β12 + c22 J β11 J β12 + J β12 J β11m22 cr 42 = c23 J β12 10

cr 43 = c24 J β12


The dynamical parameters for the FBM and the SIP were estimated from the CAD model of these elements using SolidWorks and considering the actual masses of each of their component parts. To control the mechanical system, we employ MATLAB/ Simulink and a Sensoray 626 data acquisition board as an interface for sending the control signals to the motor drives and receiving the encoder measurements. Figure 4 shows a schematic diagram of the whole system and Figure 5 shows a photograph.

A=

¶f ( z ,t

)

¶z

z = z ,t =t e e

B=

é0 0 ê ê0 0 ê0 0 ê ê0 0 =ê 0 0 ê ê0 a62 ê ê0 0 êë0 a82

¶f ( z ,t

)

¶t

Figure 5. Photograph of the experimental prototype

4.2 Controller design Most of the references that treat with spherical pendulums (e.g., [8, 14, 7]), and including the manufacturer of the commercial version of this mechanism [15], assume that the angles ϕ and θ of the pendulum are almost zero. Therefore, the system can be decoupled in two systems, and a linear‐ ized dynamics model is obtained for each system in order to apply a linear controller. We now present the linear model of the complete system (13) in order to apply an LQR controller.

a53 0 a73 0

é0 0ù ê ú 0ú ê0 ê0 0ú ê ú 0 0ú ê =ê b 0ú ê 51 ú ê 0 b62 ú ê ú ê b71 0 ú êë 0 b82 úû

a62 = glcp mp h 1 / h 3 a82 = gL lcp2 mp2 / h 3 a53 = − gL lcp2 mp2 / h 4 a73 = glcp mp h 2 / h 4 b51 = (mp lcp2 + I px ) / h 4 b71 = − L lcp mp / h 4

̣ Considering z = q q

b62 = L lcp mp / h 3

as the vector of the state variables, it is possible to represent the system (13) as the nonlinear state system

0 0

0 1 0 0 0ù ú 0 0 1 0 0ú 0 0 0 1 0ú ú 0 0 0 0 1ú , 0 0 0 0 0ú ú 0 0 0 0 0ú ú 0 0 0 0 0ú 0 0 0 0 0 úû

with

4.2.1 Model linearization T

z = z ,t =t e e

0 0

b82 = (mp lcp2 + I px ) / h 3 h 1 = I 2 + I 4 + I pz + L 2m3 + L 2mp + lc22m2 + lc42m4

é ù q& d éq ù ú -1 ê ú = f ( z ,t ) = ê dt ëq& û êë M ( r ) éët - C ( r , r& ) q& - g ( r ) ùû úû

(31)

h 2 = I 1 + I 3 + L 2m2 + L 2mp + lc12m1 + lc32m3 h 3 = I 2I px + I 4 I px + I px I pz + I px L 2m3 + I px L 2mp + I 2lcp2 mp + I px lc22m2 + I 4lcp2 mp + I px lc42m4

where the kinematic relations in Section 3.1 can be used to ̣ ̣ compute ρ and ρ in terms of q and q . Considering ze = 0 0 0

π 2

0 0 0 0

T

as the equilibri‐

um point, and as the input torques in the equilibrium point must be zero (i.e., τ¯ e = 0 0 T ), it is possible to obtain a

linearized system of the form

+ I pz lcp2 mp + L 2lcp2 m3mp + lc22lcp2 m2mp + lc42lcp2 m4mp h 4 = I 1I px + I 3 I px + I px L 2m2 + I px L 2mp + I 1lcp2 mp + I px lc12m1 + I 3lcp2 mp + I px lc32m3 + L 2lcp2 m2mp + lc12lcp2 m1mp + lc32lcp2 m3mp

4.2.2 LQR controller z& = Az + Bt where the constant matrices A and B are defined as

(32)

It is well–known in literature that system (32) can be stabilized using a linear state–feedback control law given by


11

t = -Kz.

(33)

And if K is chosen such that

0

L

Total length of

0.127

m

0.047

m

0.069

m

0.062

m

0.045

m

T

length to the com

length to the com

lc2

of link 2 length to the com

lc3

of link 3

using some positive definite matrices of proper dimensions Q and P , then we ensure that the cost function T

Units

of link 1

AT P + PA - PBG -1BT P + Q = 0,

( z Qz + t

Value

lc1

where P is found by solving the algebraic Riccati equation

¥

Description

FBM links

K = G -1BT P

J=ò

Key

)

(34)

Gt dt

is minimized. Such is known as an LQR controller. 4.3 Evaluation

length to the com

lc4

of link 4 m1

mass of link 1

0.126

kg

m2

mass of link 2

0.08535

kg

m3

mass of link 3

0.063

kg

m4

mass of link 4

0.121

kg

I1

inertia moment of

0.0017

kgm2

0.00001

kgm2

0.00001

kgm2

0.0014

kgm2

0.155

m

0.125

kg

0.0017

kgm2

0.000003

kgm2

link 1

In this section, we show the evaluation of the SIP+FBM system in a closed loop with an LQR controller. Both simulations and experiments are tested.

inertia moment of

I2

link 2 inertia moment of

I3

link 3

4.3.1 Simulations In order to apply the LQR controller to the system (13), we consider for the parameters of the system the values that are shown in Table 1. These parameters were estimated using conventional identification procedures for our academic prototype. Considering that the matrices Q and G of (34) are defined by é7 ê ê0 ê0 ê ê0 Q=ê 0 ê ê0 ê ê0 êë 0

0 0 0

0

0

0

0

ù ú 6 0 0 0 0 0 0 ú 0 6 0 0 0 0 0 ú ú 0 0 7 0 0 0 0 ú 0 0 0 0.0001 0 0 0 ú ú 0 0 0 0 0.0001 0 0 ú ú 0 0 0 0 0 0.0001 0 ú 0 0 0 0 0 0 0.0001úû

12

k13 0

length to the

lcp

pendulum com mass of the

mp

pendulum inertia moment of

I px

pendulum inertia moment of

I py

pendulum Table 1. Parameters of the SIP+FBM mechanism

with

gains

k17 = − 0.6912, k28 = − 0.2618.

k11 = − 0.6236, k22 = 4.7337,

k13 = − 4.9548,

k24 = − 0.6236,

k15 = − 0.2669,

k26 = 0.6591,

Thḛ position errors of the variables are given ̰ ̰ independent ̰ by q 1 = q1d − q1,ϕ = ϕd − ϕ , θ = θd − θ,q 2 = q2d − q2, where q1d ,ϕd ,θd , equilibrium point ze . The

initial

conditions

were

considered

as

5π 180

the K matrix is given by 0 k22

link 4

and q2d are the desired positions which correspond to the

é18 0 ù G=ê ú ë 0 18 û

ék K = ê 11 ë0

inertia moment of

I4

0 k24

k15 0

0 k26

k17 0

0ù ú k28 û


T − 5π π 0 0 0 0 . Figure 6 shows the z0 = 0 180 ̰ 2 ̰ ̰ ̰ time evolution of q 1 and q 2, while the graphics for ϕ and θ

are shown in Figure 7. From figures 6 and 7, we can observe that the errors converge to zero, i.e., that the states converge to the equilibrium point.

Figure 8. Position error of q1 and q2 (experiment)

Figure 6. Position error of q1 and q2 (simulation)

Figure 9. Position error of ϕ and θ (experiment)

Figure 7. Position error of ϕ and θ (simulation)

4.3.2 Experiments We also carried out some experiments in our prototype. We considered using the same control gains as in the simula‐ tion. A sampling period of 1ms was employed. Initial conditions for the experiments were assumed to be as close as possible to the ̰ equilibrium point. Figure 6 shows thḛ ̰ ̰ time evolution of q 1 and q 2, while the graphics for ϕ and θ

are shown in Figure 9. Some disturbances were applied by knocking the pendulum between seconds 6 and 10; the LQR controller also shows good performance in this situation. Figures 10 and 11 show the applied torques to the actuated joints for each actuated joint. We can observe that the controller keeps the errors near to zero, i.e., that the states converge to the equilibrium point.

Figure 10. Applied torque for the joint of q1


13

Figure 11. Applied torque for the joint of q2

5. Conclusions This document presented a way of obtaining the kinematics and dynamics model of a system consisting of an inverted pendulum mounted on a FBM. The dynamics model is obtained in terms of a non-minimal set of generalized coordinates and then it is reduced using an approach proposed in the literature. In order to validate this resulting model, we carried out some simulations and experiments on a didactic prototype we have in our laboratory. The resulting model is novel, however, since it uses no simpli‐ fications as in [12]. 6. Acknowledgements This work was partially supported by PROMEP, DGEST and CONACYT (grant 60230). The authors would also like to thank Jaqueline Bernal for her support in acquiring a better understanding of DAEs. 7. References [1] Aracil J, Gordillo F (2004). The inverted pendulum: A benchmark in nonlinear control. In: IFAC World Congress; August 2004; Seville, Spain. [2] Spong M, Block J (1995). The pendubot: A mecha‐ tronic system for control research and education. In: IEEE Conference on Decision and Control; Decem‐ ber 1995; New Orleans, LA, USA. [3] Spong MW (1995). The Swing up Control Problem for the Acrobot. IEEE Control Systems Magazine; 15: 49-55. [4] Spong MW, Corke P, Lozano R (1999). Nonlinear Control of the Inertia Wheel Pendulum. Automati‐ ca; 37: 1845-1851. 14


[5] Astrom KJ, Furuta K (2000). Swinging up a Pendu‐ lum by Energy Control. IEEE Control Systems Magazine; 36: 287-295. [6] Fantoni I, Lozano R (2002). Non-linear Control for Underactuated Mechanical Systems. SpringerVerlag. [7] Yang R, Kuen Y, Li Z (2000). Stabilization of a 2-dof spherical pendulum on XY table. In: IEEE Confer‐ ence on Control Applications; September 2000; Anchorage, AK, USA. [8] Albouy X, Praly L (2000). On the use of dynamic invariants and forwarding for swinging up a spherical inverted pendulum. In: IEEE Conference on Decision and Control; December 2000; Sydney, Australia. [9] Bloch A, Leonard N, Marsden J (2000). Controlled Lagrangians and the Stabilization of Mechanical Systems I: The First Matching Theorem. IEEE Transactions on Automatic Control; 45: 2253-2269. [10] Liu G, Nesic D, Mareels I (2005). Modelling and stabilization of a spherical inverted pendulum. In: IFAC World Congress; July 2005; Prague, Czech Republic. [11] Aguilar-Ibanez D, Gutierrez O, Sossa-Azuela H (2006). Lyapunov approach for the stabilization of the inverted spherical pendulum. In: IEEE Confer‐ ence on Decision and Control; December 2006; San Diego, CA, USA. [12] Ishii C, Nishitani Y, Hashimoto H (2009). Modelling and Robust Stabilization of a Closed-link 2-dof Inverted Pendulum with Gain Scheduled Control. International Journal of Modelling, Identification and Control; 6: 320-332. [13] Viet TD, Doan PT, Giang H, Kim HK, Kim SB (2012). Control of a 2-dof Omnidirectional Mobile Inverted Pendulum. Journal of Mechanical Science and Technology; 26: 2921-2928. [14] Chung C, Lee J, Lee B (2000). Balancing of an inverted pendulum with a redundant direct-drive robot. In: IEEE International Conference on Robot‐ ics and Automation; June 2000; San Francisco, CA, USA. [15] Quanser. 2 DOF Inverted Pendulum/Gantry [Internet]. Available from: http://www.quanser. com/Products/2DOF_pendulum. Accessed January 2015. [16] Dabney JB, Ghorbel FH (2002). Modeling closed kinematic chains via singular perturbations. In: IEEE American Control Conference; May 2002; Anchorage, AK, USA. [17] Alvarado O (2010). Construction, Modelling and Control of a Spherical Inverted Pendulum Mecha‐ nism (in Spanish) [Master's thesis]. Torreón, Mexico: Instituto Tecnológico de la Laguna. [18] Ghorbel FH, Chételat O, Gunawardana R, Long‐ champ R (2000). Modeling and Set Point Control of

[19] [20]

[21] [22]

[23]

Closed-chain Mechanisms: Theory and Experi‐ ment. IEEE Transactions on Control Systems Technology; 8: 801-815. Merlet JP (2006). Parallel Robots. Springer-Verlag. Khan W, Krovi V, Saha S, Angeles J (2005). Recur‐ sive Kinematics and Inverse Dynamics for a Planar 3R Parallel Manipulator. Journal of Dynamic Systems, Measurement, and Control; 127: 529-536. Hairer E, Wanner G (1996). Solving Ordinary Differential Equations II. Springer-Verlag. Kecskeméthy A, Krupp T, Hiller M (1997). Symbolic Processing of Multi-loop Mechanism Dynamics using Closed form Kinematics Solutions. Multi‐ body Systems Dynamics; 1(1): 23-45. Ascher U, Petzold LR (1998). Computer Methods for Ordinary Differential Equations and Differential-

Algebraic Equations. Society for Industrial and Applied Mathematics (SIAM). [24] Murray R, Li Z, Sastry S (1994). A Mathematical Introduction to Robotic Manipulation. CRC Press. [25] García de Jalon J, Bayo E (1994). Kinematic and Dynamic Simulation of Multibody Systems: The Real-Time Challenge. Springer-Verlag. [26] Shabana AA (2001). Computational Dynamics. Wiley. [27] Sciavicco L, Siciliano B. (2000). Modelling and Control of Robot Manipulators. Springer-Verlag. [28] Campa R, de la Torre H (2009). Pose control of robot manipulators using different orientation represen‐ tations: A comparative review. In: IEEE American Control Conference; June 2009; St. Louis, MO, USA.


15

Appendix

Given (35), it can be proved that

Computation of matrix R (ρ )

¶g ( r )

Consider a closed-chain mechanism which is described by means of the vector ρ ∈ ℝm of generalized coordinates, from which we can extract the vector q ∈ ℝn of independent coordinates using the function

¶r ¶a ( r ) ¶r

q = a (r )

R ( r ) = 0 p´ n

R ( r ) = I n´ n

Now, it is worth noticing that we can write

and let

g (q) = 0

é 0 p´ n ù Y ( r ) R( r ) = ê ú êë I nń úû

be the vector of holonomic constraints of the system. The matrix R (ρ ) ∈ ℝm×n , which allows the transformation of the system dynamics from an index-3 DAE to an index-1 DAE, as described in Section 2, can be computed using the following expression T

æ ¶a ( r ) ö R ( r ) = éë I m´ m - G ùû ç ÷ S è ¶r ø

(35)

where é ¶g ( r ) ù ê ú ¶r ú Y(r ) = ê Î ¡ m´ m ê ¶a ( r ) ú ê ú êë ¶r úû and as Ψ(ρ ) is full-rank, then it is possible to also write

where -1

T æ æ ¶a r ö ( ) ÷ é I - G ù æç ¶a ( r ) ö÷ ö÷ Î ¡nń S = çç ë m´ m û ç ¶r ÷ ÷ çç çè ¶r ÷ø è ø ÷ø è

-1

T T ¶g ( r ) æ ¶g ( r ) ¶g ( r ) ö ¶g ( r ) ç ÷ G= Î ¡ m´ m ç ¶r ÷ ¶r ¶r ¶r è ø

16


-1 é 0 p´ n ù R( r ) = Y ( r ) ê ú êë I nń úû

which agrees with the definition of R (ρ ) found in [18].