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 decades [1]. There exist a wide variety of inverted-pendu‐

lum-type systems, such as the pendubot, the acrobot, the
This paper describes the kinematics and dynamics model‐
pendulum on a cart, the inertial wheel pendulum and the
ling of a mechanical system consisting of a spherical
Furuta pendulum (see, e.g., [2 - 5]). All these systems are
inverted pendulum whose base is mounted on a parallel
under-actuated [6], that is, they are systems with fewer
planar mechanism, better known as a five-bar mechanism.
actuators than degrees of freedom (dof). The degree of
The whole mechanism has four degrees of freedom, but it
under-actuation is indeed the difference between the
has only two actuators and so it is an under-actuated
number of dof and the number of actuators. All the
system. The nonlinear dynamics model of the complete
examples of inverted pendulums mentioned above have a
system is first obtained using a non-minimal set of gener‐
degree of under-actuation of one.
alized coordinates, and then a reduced equivalent model is
computed. To validate this approach, the reduced model is This paper deals with the modelling and control of a
linearized and simulations are carried out, showing the spherical inverted pendulum (SIP), which is another
stabilization of the system with a simple LQR controller. member of the family of inverted pendulum systems with
Experimental results on an academic prototype are also degree of under-actuation of two.
presented.
The system consists of a rigid rod coupled in its base to an
Keywords modelling, inverted pendulum, parallel mech‐ under-actuated universal joint in such a way that the
anism, under-actuated systems 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
1. Introduction upright position.

The problem of balancing an inverted pendulum has The SIP has been studied by numerous researchers (see [7
attracted the attention of control researchers in recent - 11]). The interest in studying the SIP is due to its mathe‐

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.

The remainder of this paper is organized as follows. Section

Figure 1. Spherical inverted-pendulum on a five-bar mechanism 2 is devoted to the modelling of constrained systems in
general. The kinematics and dynamics models of the
matical model, which is considered as a simplified model system under study are obtained in Section 3. The valida‐
of a rocket-propelled body or a building, such that it can be tion of the models via simulations/experiments with an
used for the control of the position of a rocket, for the LQR controller is shown in Section 4, while final conclu‐
sions are given in Section 5.
control of the oscillations of buildings, or simply for the
study of new control techniques.
2. Modelling of constrained mechanical systems
According to [12], there exists a classification of SIP-type
systems, depending on the mechanism used to stabilize the It is worth mentioning that most of the theory presented in
SIP, which can be: 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;
In this document, it is considered that the motion of the base then, there should exist p = m − n holonomic constraints
of the SIP is controlled by means of a five-bar mechanism which allow the reduction of the order of the system from
(FBM), which is a well-known closed-chain mechanism. m to n ; in others words, if we choose n independent joint
Such a system has become common, due to the existence of variables there must exist p dependent joint variables.
a commercial prototype for academic and research purpos‐
es (see [15]). The complexity of the mathematical model and Let ρ = q T β T T ∈ ℝm be the vector of generalized joint
control of this system (SIP+FBM) is due in great measure to variables (q ∈ ℝn be the vector of independent variables and
the kinematic constraints imposed by the closed-loop of the β ∈ ℝ p be the vector of dependent, or constrained, varia‐
FBM. bles). The holonomic constraints can then be grouped in a
vector of the form
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 g ( r ) = 0 Î ¡p
the system as a linear parameter variable (LPV) system with
structured additive perturbation. This paper uses a
so that, as a result, it should be possible to express β as a
different approach to obtain the complete non-reduced
function of q, , i.e.,
dynamics model of the same system.

The equations that govern the motion of any mechanical

b = b (q).
system must take into account the kinematic constraints

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

The direct kinematics problem consists in finding expres‐ System (2) has a differential index μ if μ is the minimal
sions for the pose (i.e., position and orientation) variables number of analytical differentiations
of an element of interest of the CCM, described by the
vector x in terms of the actuated joint variables; this can be
dh ( u , u& , t ) d m h ( u , u& , t )
expressed as h ( u , u& , t ) = 0 , = 0 ,K =0 (3)
dt dt m

(
x = f q, b ( q ) . ) such that the system of equations (3) allows us to extract,
by algebraic̣ manipulations, an explicit ordinary differen‐
In addition, by taking the time derivative of the previous tial system u = φ (u ) (which is called the 'underlying' ODE)
equation we get the so-called differential kinematics According to [16], if the differential index is 1 or 2, it is
equation usually possible to differentiate the constraint equation
with respect to time once (index 1) or twice (index 2) to
x& = J ( r ) q& (1) produce an equivalent ODE (a process sometimes called
index reduction) and to achieve satisfactory results using
∂ f (q, β (q )) standard numerical methods for ODEs. If the differential
where J (ρ ) = ∂q
. index is greater than 2, standard numerical methods
frequently fail to converge, and sometimes converge to
On the other hand, the inverse kinematics problem consists
incorrect solutions.
in establishing the value of the actuated joint coordinates
that correspond to a given pose of the element of interest, These DAEs resulting from the dynamical analysis of
and can be expressed as 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
q = f -1 ( x ) .
that they can be solved analytically or numerically.
The most important of such methods are [20]:
The inverse kinematics problem can be solved in general
form by geometric or analytic approaches, such as those 1. Direct elimination, where the surplus variables are
mentioned in [19]. eliminated directly, using the algebraic constraints
given by the kinematics, to explicitly reduce index-3
2.2 Dynamics model DAEs to ODEs in a minimal set of generalized coordi‐
nates (see [22]).
As mentioned in [20], the dynamics model of CCMs using
2. Explicit Lagrange-multiplier computation, using the
either the Newton-Euler or the Euler-Lagrange methodol‐
unknown accelerations computed from the index-1
ogy is traditionally obtained by cutting the closed loops to
DAEs formed by appending the acceleration con‐
obtain systems with open chains and/or tree-type struc‐
straints (computed by differentiating the original
tures, so that it is possible to write a set of ODEs for such
constraint) to the system equations (see [23] and [24]).
chains in their corresponding generalized coordinates. The
solution to these equations requires satisfying additional 3. Projection of the dynamics onto the tangent space of the
algebraic equations guaranteeing the closure of the cut- constraint manifold, where the constraint-reaction
open loops. To this end, some Lagrange multipliers (which dynamics equations are taken into the orthogonal and
physically represent the forces due to the constraints) are tangent subspaces of the vector space of the system
employed. The resulting formulation, often referred to as a generalized velocities. A family of choices exist for the
descriptor form, yields a system of DAEs which, in general, projection, as surveyed by [25] and [26].
cannot be solved employing conventional ODE techniques. The dynamics model of a mechanism of n dof, with m
The most general form of a differential-algebraic system of generalized variables (m > n ), and subject to p = m − n
n equations is that of an implicit differential equation [21] holonomic constraints, can be represented by means of the
following index-3 DAEs
é h1 ( u , u& , t ) ù
ê ú
ê h2 ( u , u& , t ) ú
T
d ì ¶L( r , r& ) ü ¶L( r , r& ) æ ¶g ( r ) ö
h ( u, u,t ) = ê
& ú = 0Ρ
n
(2) í ý- =tr + ç ÷ l (4)
M dt î ¶r& þ ¶r è ¶r ø
ê ú
ê hn ( u , u& , t ) ú
ë û
g ( r ) = 0, (5)
where u ∈ ℝn is the vector of generalized coordinates, t is
̣ du ̣
the time variable, and u = dt . where the Lagrangian L(ρ,ρ ) can be defined as

Israel Soto and Ricardo Campa: 3

Modelling and Control of a Spherical Inverted Pendulum on a Five-Bar Mechanism
 L ( r , r& ) = K ( r , r& ) - U ( r ) variables q (which can be extracted from ρ through a
(6)
function α , such that q = α(ρ)), and the constraints γ(ρ).
̣ Appendix A describes a procedure to obtain R(ρ) starting
being K(ρ,ρ ) for the kinetic energy and U(ρ ) for the potential from these relations.
energy of the mechanism; τρ ∈ ℝm is the vector of applied
Using such a matrix R(ρ), it is possible to reduce the system
joint forces, and λ ∈ ℝ p establishes the vector of Lagrange (7) to a new index-1 DAE system
multipliers which ensure the fulfilment of the constraints
in (5). Equation (4) can be rewritten as
M (r )q
&& + C ( r , r& ) q& + g ( r ) = t ,

M ( r ) r&& + C ( r , r& ) r& + g ( r ) = t r + D T

(r )l (7) g (r ) = 0, (13)
q = a (r )
̣
where M (ρ ) ∈ ℝm×m is the inertia matrix, C (ρ,ρ ) ∈ ℝm×m is the
matrix of terms generated by centrifugal and Coriolis where
forces, g (ρ ) ∈ ℝm represents the vector of gravitational
M̄ (ρ ) = R (ρ )T M (ρ )R (ρ )
forces, and D (ρ ) = ∂ γ (ρ ) / ∂ ρ ∈ ℝ p×m is called the constraint
̣ ̣ ̣ ̣
Jacobian matrix. C̄ (ρ,ρ ) = R (ρ )T C (ρ,ρ )R (ρ ) + R (ρ )T M (ρ ) R (ρ,ρ )
Moreover, as explained in [27], the following properties are
ḡ (ρ ) = R (ρ )T g (ρ )
satisfied for M (q ) and g (q )
τ̄ = R (ρ )T τρ
1 T
K ( r , r& ) = r& M ( r ) r& (8) and notice thaṭ the states which define the dynamics of the
2
system are q , q and ρ , which can be obtained by integrating
the corresponding state equations.
¶U ( r )
g (r ) = (9)
¶r

Witḥ the matrix M (ρ), it is possible to compute the matrix

C(ρ,ρ ) by means of the Christoffel symbols cijk (ρ) defined as

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

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 Figure 2. Schematic diagram of the SIP+FBM
columns of R (ρ ), which is parameterized by a vector of
̣
independent velocities q ∈ ℝn , obtaining the expression for 3. Modelling of the mechanism under study
the feasible dependent velocities
In this section, the kinematics and dynamics models of a
SIP+FBM system, as the one shown in Figure 2, are ob‐
r& = R ( r ) q& . (12)
tained. This figure also shows the frames attached to each
extreme of the FBM (Σ0 and Σ0'), as well as those of the base
The matrix R(ρ) for a particular system depends on the (Σb) and the extreme (Σp ) of the pendulum. In general, Σi is
selection of the generalized coordinates ρ , the independent considered to be formed by the axes xi ,yi , zi .

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

Figure 3. Geometric relations of the FBM

The angles θ and ϕ indicate the orientation of the extreme where the notations S( ⋅ ) and C( ⋅ ) are used for sin( ⋅ ) y
of the pendulum with respect to the frame Σb ; θ is the angle cos( ⋅ ), respectively. Using a software tool such as MAT‐
that rotates with respect to the axis yb and ϕ is the angle that 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 L

xb = 2 + C (q1) + C (q2)
2
its configuration. The angles β1 and β2 of the passive joints
depend upon q1 and q2. For the purposes of analysis, in this L C (q1 − q2) + 2C (q1) − 2C (q2) − 1
2 ( 1) ( 2)
± S q −S q −
C (q1 − q2) + 2C (q1) − 2C (q2) − 3
paper the vector of independent joint variables is defined
as L
2 ( 1)
yb = S q + S (q2)
T
q = éëq1 f q q2 ùû (14) L C (q1 − q2) + 2C (q1) − 2C (q2) − 1
± C (q2) − C (q1) + 2 − .
2 C (q1 − q2) + 2C (q1) − 2C (q2) − 3
and the vector of pose variables of the pendulum as
Furthermore, from Figure 3 (b) it is possible to obtain the
following relations
T
x = éë xb yb f q ùû . (15)
æ y - LS ( q1 ) ö
b1 ( q ) = tan -1 ç b ÷
The lengths of the four mobile links are considered equal ç x - LC ( q ) ÷
è b 1 ø
to L , and the length of the pendulum is L p .

3.1 Kinematics model æ yb - LS ( q2 ) ö

b 2 ( q ) = tan -1 ç ÷
ç xb - ( 2 L + LC ( q2 ) ) ÷
è ø
3.1.1 Direct kinematics model

The problem of direct kinematics is reduced to find the 3.1.2 Inverse kinematics model
coordinates of the intersection points of a circle of radius L
with C1 and C2 as centres, as shown in Figure 3(a), the From Figure 3 (a), the following expressions can be defined
equations of which are
xb = LC ( q1 ) + LC ( q1 + b1 ) (16)

(x - LC ( q1 ) ) + (y )
2
- LS ( q1 )
2
= L2 ,
yb = LS ( q1 ) + LS ( q1 + b1 )
b b
(17)

( x - ( 2 L + LC ( q ))) + ( y )
2
- LS ( q2 )
2
b 2 b
= L2 , xb = 2 L + LC ( q2 ) + LC ( q2 + b 2 ) (18)

Israel Soto and Ricardo Campa: 5

Modelling and Control of a Spherical Inverted Pendulum on a Five-Bar Mechanism
 yb = LS ( q2 ) + LS ( q2 + b 2 ) (19) x& bC ( qi + b i ) + y& bS ( qi + b i ) = LS ( b i ) q& i (25)

and on the other hand, from Figure 3 (b) it is observed that and by taking i = 1,2 in (25), we find
̣ the matrix
̣ equation that
relates the operational velocities x b and y b with the actuated
̣ ̣
xb = rC (a ) and yb = rS (a ) (20) joint velocities q 1 and q 2

where é C ( q1 + b1 ) S ( q1 + b1 ) ù é x& b ù éS ( b1 ) 0 ù é q&1 ù

ê úê ú = Lê ú ê ú.
êëC ( q2 + b 2 ) S ( q2 + b 2 ) úû ë yb û
& êë 0 S ( b 2 ) úû ëq& 2 û
æy ö
r = xb2 + yb2 and a = tan -1 çç b ÷÷ (21)
è xb ø Now, from the differential kinematics equation (1), consid‐
ering (14) and (15), we can show that
and also
é S ( q2 + b 2 ) S ( b 1 ) -S ( q1 + b1 ) S ( b 2 ) ù
2q1 + b1 = 2a ê 0 0 ú
ê S ( q2 + b 2 - q1 - b1 ) S ( q2 + b 2 - q1 - b1 ) ú
(22)
ê ú
-C ( q2 + b 2 ) S ( b1 ) C ( q1 + b1 ) S ( b 2 ) ú
J(r ) = Lê 0 0 .
As such, making some algebraic operations with equations êS(q + b - q - b ) S ( q2 + b 2 - q1 - b1 ) ú
(16), (17) and (20)-(22), it yields ê 2 2 1 1
ú
ê 0 1 0 0 ú
ê ú
ë 0 0 1 0 û
æy ö æ x2 + y 2 ö
q1 = tan -1 çç b ÷÷ m cos -1 ç b b ÷
x
è bø ç 2 L ÷
è ø Moreover, from (23) and (24), we get

æ x2 + y 2 ö ( ) (
-S ( q1 ) q&1 - S ( q1 + b1 ) q&1 + b&1 = -S ( q2 ) q& 2 - S ( q2 + b 2 ) q& 2 + b&2 )
b1 = ±2cos ç b ÷.
( ) (
C ( q1 ) q&1 + C ( q1 + b1 ) q&1 + b&1 = C ( q2 ) q& 2 + C ( q2 + b 2 ) q& 2 + b&2 . )
-1 b
ç 2L ÷
è ø

which leads to
Following a similar procedure, but starting from equations
(18) and (19), we get
é b&1 ù é q&1 ù
(
ê & ú = Jb q, b ( q ) ) êq& ú (26)
æ ö ëb2 û ë 2û
( xb - 2 L )
2 2
æ yb ö -1 ç
+y ÷
b
q2 = tan -1 çç ÷÷ m cos ç ÷
è xb - 2 L ø ç 2L ÷ where
è ø
S(q1 − q2 − β2) S (β2)
− −1 −
S(q1 − q2 + β1 − β2) S(q1 − q2 + β1 − β2)
æ ö
( xb - 2 L )
2
+ yb2 ÷ J β (q,β (q )) = .
ç S(β1) S(q1 − q2 + β1)
b 2 = m2cos -1 ç ÷ − −1
ç 2L ÷ S (q1 − q2 + β1 − β2) S(q1 − q2 + β1 − β2)
è ø
Equation (26) establishes the relation between the actuated
3.1.3 Differential kinematics model and passive joint velocities of the FBM.

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

Following the approach proposed in [18], let us consider

(
x& b = - LS ( qi ) q& i - LS ( qi + b i ) q& i + b&i ) (23) 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
(
y& b = LC ( qi ) q& i + LC ( qi + b i ) q& i + b&i ) (24) by the right leg. Now, it is possible to use as a vector of
T
generalized variables ρ = q1 β1 ϕ θ q2 β2 ∈ ℝ6.
As such, multiplying the equations (23) and (24) by In the following, we show how to obtain each of the
cos(qi + βi ) and sin(qi + βi ), respectively, and adding the elements of equation (7) and then how to get the reduced
results, we further obtain system (13).

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

The total kinetic energy of the system can be divided in: éC ( q1 ) + C ( q1 + b1 ) ù
ê ú
0
pb = L ê S ( q1 ) + S ( q1 + b1 ) ú ,
4
ê ú
K ( r , r& ) = åKi ( r , r& ) + Kp ( r , r& ) (27) ë 0 û
i =1

where Ki is the kinetic energy associated with the link i of é S (q ) ù

ê ú
the FBM and K p is the kinetic energy associated with the
b
p p = lcp ê -C (q ) S (f ) ú
ê ú
pendulum; Ki and K p can be obtained by [27] ë C (q ) C (f ) û

Ki =
1 1
m p& T p& + w T I w (28) éC ( q1 + b1 ) -S ( q1 + b1 ) 0 ù
2 i i i 2 i i i ê ú
0
b
R = ê S ( q1 + b1 ) C ( q1 + b1 ) 0 ú
ê ú
ë 0 0 1û
1 1
Kp = m p& T p& + w T I w (29)
2 p p p 2 p p p 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 w1 = éë0 0 q&1 ùû
T
w 2 = éë0 0 q&1 + b&1 ùû
T

the mass anḍ 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‐ w 4 = ëé0 0 q& 2 ùû w 3 = éë0 0 q& 2 + b&2 ùû
T T

ly (considering all the vectors with respect to frame Σ0).

The position vectors of the centres of mass of each link for while the angular velocity of the pendulum with respect to
the FBM are frame Σ0 is given by [28]

élc 1C ( q1 ) ù élc 4C ( q2 ) ù é r&1 ù

ê ú ê ú 1 ê ú
p1 = ê lc 1S ( q1 ) ú , p4 = ê lc 4S ( q2 ) ú w p = ëS ( r1 ) S ( r2 ) S ( r3 ) û ê r&2 ú
é ù (30)
ê ú ê ú 2
ê r&3 ú
ë 0 û ë 0 û ë û

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

é LC ( q1 ) + lc 1C ( q1 + b1 ) ù
ê ú vector v = v1 v2 v3 T , is defined as
p2 = ê LS ( q1 ) + lc 1S ( q1 + b1 ) ú ,
ê ú
ë 0 û é 0 - v3 v2 ù
ê ú
S ( v ) = ê v3 0 - v1 ú
é 2 L + LC ( q2 ) + lc 3C ( q2 + b 2 ) ù ê - v2 v1 0 úû
ë
ê ú
p3 = ê LS ( q2 ) + lc 3S ( q2 + b 2 ) ú ,
ê ú
ë 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
of which are
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 r11 = C (q1 + β1)C (θ ) − S (q1 + β1)S (ϕ )S (θ )
of mass of the pendulum is described by the equation
r12 = − S (q1 + β1)C (ϕ )

p p = 0 pb + b0 Rb p p r13 = C (q1 + β1)S (θ ) + S (q1 + β1)S (ϕ )C (θ )

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

where 0 pb is the position vector of the base of the SIP with r22 = C (q1 + β1)C (ϕ )
respect to frame Σ0, b p p is the position vector of the centre
r23 = S (q1 + β1)S (θ ) − C (q1 + β1)S (ϕ )C (θ )
of mass of the pendulum with respect to Σb, and b0R is the
rotation matrix of Σb with respect to Σ0, which are repre‐ r31 = − C (ϕ )S (θ )
sented as
r32 = S (ϕ )

Israel Soto and Ricardo Campa: 7

Modelling and Control of a Spherical Inverted Pendulum on a Five-Bar Mechanism
 r33 = C (ϕ )C (θ ). m22 = I 2 + I px + L 2mp + lc22m2 + lcp2 mp

Therefore, using (30), we get − (I px − I pz + lcp2 mp )C (ϕ )2C (θ )2 + 2L lcp mp S (θ )

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

éC ( q1 + b1 )f& - S ( q1 + b1 ) C (f )q& ù
ê ú m24 = S (ϕ ) I px + lcp2 mp + L lcp mp S (θ )
w p = êS ( q1 + b1 )f& + C ( q1 + b1 ) C (f )q& ú .
ê ú
ë q&1 + b&1 + S (f )q& û m33 = I pz + ( I px − I pz + lcp2 mp )C (θ )2

m44 = I pz + lcp2 mp
Moreover, in equation (28) we require the mass of each link
of the FBM as well as the matrix of inertia moments with m55 = m4lc42 + m3 L 2
+ lc32 + 2L lc3C (β2) + I 4 + I 3
respect to a coordinate frame that passes through the centre
m56 = m3 lc32 + L lc3C (β2) + I 3
of mass. Again, taking into consideration that the FBM
moves in the horizontal plane, only the inertia moments m66 = m3lc32 + I 3
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 Using expressions (10) and (11), we can compute
given by
é c11 c12 c13 c14 0 0 ù
ê ú
é I px 0 0ù êc21 c22 c23 c24 0 0 ú
ê ú êc c c33 c34 0 0 ú
C ( r , r& ) = ê 31 32
0 0
Ip = R ê 0
b
I px 0 ú 0b RT ú
ê ú êc41 c42 c43 c44 0 0 ú
êë 0 0 I pz úû ê0 0 0 0 c55 c56 ú
ê ú
êë 0 0 0 0 c65 c66 úû
It is possible to use software for symbolic computing to
obtain the matrices of the dynamics model (7). In order to
̣ where each element of the matrix is given by
obtain the matrix M (ρ ), the total kinetic energy K(ρ,ρ ) is ̣ ̣ ̣
calculated and (8) is used, yielding c11 = a1β 1 + a2ϕ + a3θ
̣ ̣ ̣ ̣
c12 = a1(q 1 + β 1) + a2ϕ + a3θ
é m11 m12 m13 m14 0 0 ù ̣ ̣ ̣ ̣
ê ú c13 = a2(q 1 + β 1) + a4ϕ + a5θ
ê m12 m22 m23 m24 0 0 ú ̣ ̣ ̣ ̣
êm m23 m33 m34 0 0 ú c14 = a3(q 1 + β 1) + a5ϕ + a6θ
M ( r ) = ê 13 ú
ê m14 m24 m43 m44 0 0 ú ̣ ̣ ̣
ê 0 c21 = − a1q 1 + a7ϕ + a8θ
0 0 0 m55 m56 ú
ê ú ̣ ̣
êë 0 0 0 0 m55 m66 úû c22 = a7ϕ + a8θ
̣ ̣ ̣ ̣
c23 = a7(q 1 + β 1) + a9ϕ + a10θ
where ̣ ̣ ̣ ̣
c24 = a8(q 1 + β 1) + a10ϕ + a11θ
m11 = I 1 + I 2 + I px + L 2m2 + 2L 2mp + lc12m1 + lc22m2 ̣ ̣ ̣
c31 = − a2q 1 − a7β 1 + a12θ
+ lcp2 mp − I px C (ϕ )2C (θ )2 + I pz C (ϕ )2C (θ )2 ̣ ̣ ̣
c32 = − a7(q 1 + β 1) + a12θ
+ 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 (ϕ ) c33 = a13θ
̣ ̣ ̣
c34 = a12(q 1 + β 1) + a13ϕ = − c43
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 c41 = − a3q 1 − a8β 1 − a12ϕ
̣ ̣ ̣
+ L lc2m2C (β1) + 2L lcp mp S (θ ) + L lcp mp C (β1)S (θ ) c42 = − a8(q 1 + β 1) − a12ϕ
+ L lcp mp S (β1)C (θ )S (ϕ )
c44 = 0
m13 = − C (ϕ )C (θ ) L lcp mp 1 + C (β1) ̣
c55 = − m3 L lc3S (β2)β 2
− C (ϕ )C (θ )(I px − I pz + lcp2 mp )S (θ ) ̣ ̣
c56 = − m3 L lc3S (β2)(q 2 + β 2)
m14 = lcp2 mp S (ϕ ) + 2L lcp mp S (ϕ )S (θ )C (β1 / 2)2 ̣
c65 = m3 L lc3S (β2)q 2
+ I px S (ϕ ) + L lcp mp S (β1)C (θ )
c66 = 0

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

with and the relation q = α (ρ ) is simply defined by
a1 = L lcp mp C (β1)C (θ )S (ϕ )
− LS (β1)(lc2m2 + L mp + lcp mp S (θ )) é1 0 0 0 0 0ù
ê ú
0 0 1 0 0 0ú
a2 = C (ϕ )C (θ ) L lcp mp S (β1) q=ê r
ê0 0 0 1 0 0ú
+ C (ϕ )C (θ )(I px − I pz + lcp2 mp )C (θ )S (ϕ ) ê ú
ëê0 0 0 0 1 0 ûú
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 0 0 0 ù
+ C (θ )S (ϕ )(I px − I pz + lcp2 mp )S (θ ) ê ú
ê J b11 0 0 Jb ú
12
1 ê ú
a5 = C (ϕ ) I px + lcp2 mp − (I px − I pz + lcp2 mp )C (2θ ) 0 1 0 0 ú
2 R( r ) = ê
ê 0 0 1 0 ú
+ 2C (ϕ ) L lcp mp C (β1 / 2)2S (θ ) ê ú
ê 0 0 0 1 ú
a6 = L lcp mp 2C (β1 / 2)2C (θ )S (ϕ ) − S (β1)S (θ ) êJ 0 0 Jb ú
ë b21 22 û
a7 = (I px − I pz + lcp2 mp )C (ϕ )S (ϕ )C (θ )2

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

a9 = C (θ )S (ϕ ) L lcp mp + (I px − I pz + lcp2 mp )S (θ ) S (q1 − q2 − β2)

J β11 = − −1
S (q1 − q2 + β1 − β2)
1
a10 = C (ϕ ) I px + lcp2 mp − (I px − I pz + lcp2 mp )C (2θ ) S (β2)
2
J β12 = −
+ C (ϕ ) L lcp mp C (β1 / 2)2S (θ ) S (q1 − q2 + β1 − β2)

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

J β21 =
S (q1 − q2 + β1 − β2)
1
a12 = − C (ϕ ) I px + lcp2 mp + (I px − I pz + lcp2 mp )C (2θ )
2 S (q1 − q2 + β1)
J β22 = − −1
a13 = − ( I px − I pz + lcp2 mp )C (θ )S (θ ) S (q1 − q2 + β1 − β2)

Finally, for the vector of gravitational forces, the total The matrices of the reduced model of the system (7) are
potential energy (which only depends of the potential defined as
energy of the SIP) is obtained using
é mr 11 mr 12 mr 13 mr 14 ù
ê ú
U ( r ) = - mp g o
T
m mr 22 mr 23 mr 24 ú
p
M ( r ) = ê r 12
ê mr 13 mr 23 mr 33 mr 34 ú
ê ú
where go is a vector that indicates the gravitational accel‐ ëê mr 14 mr 24 mr 34 mr 44 ûú

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

é cr 11 cr 12 cr 13 cr 14 ù
ê ú
g(r ) =
¶U ( r )
= mglcp
(
¶ C (q ) C (f ) ) c c
C ( r , r& ) = ê r 21 r 22
cr 23 cr 24 ú
êcr 31 cr 32 cr 33 cr 34 ú
¶r ¶r ê ú
ëêcr 41 cr 42 cr 43 cr 44 ûú

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 ) ù ù mr 14 = J β21m56 + J β12(m12 + J β11m22) + J β21 J β22m66

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

Israel Soto and Ricardo Campa: 9

Modelling and Control of a Spherical Inverted Pendulum on a Five-Bar Mechanism
 Figure 4. Schematic diagram of the SIP+FBM system

mr 23 = m34 cr 43 = c24 J β12

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

mr 34 = J β12m24 and the vector of gravitational forces is defined as

mr 44 = m66 J β222 + m22 J β212 + 2m56 J β11 + m55

g ( r ) = - mp glcp éë0 0 S(f )C(q ) C(f )S(q ) 0 0 ùû
T

̣
cr 11 = c66 J β221 + J β21m66 J β21 + c11 + c21 J β11
̣
+ J β11(c12 + c22 J β11) + J β11(m12 + J β11m22) 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
cr 14 = J β12(c12 + c22 J β11) + J β12(m12 + J β11m22)
̣ whole Quanser system in [15], we only purchased the FBM
+ c65 J β21 + c66 J β21 J β22 + J β21 J β22m66 and the SIP as individual parts. An aluminium base for the
̣ mechanism was made, and two motors with encoders
cr 21 = c31 + J β11m23 + c32 J β11
(model 2224U006 from Faulhaber) were coupled to it as the
cr 22 = c33 actuators for the FBM. Some servo amplifiers (model
BE15A8 from Advanced Motion Control (AMC)) were
cr 23 = c34 employed to operate the motors in current (torque) mode.
̣ A power module (model PS2X3W24, also from AMC) was
cr 24 = J β12m23 + c32 J β12
used to supply the required power.
̣
cr 31 = c41 + J β11m24 + c42 J β11 The dynamical parameters for the FBM and the SIP were
estimated from the CAD model of these elements using
cr 32 = c43 SolidWorks and considering the actual masses of each of
cr 33 = c44 their component parts.
̣ To control the mechanical system, we employ MATLAB/
cr 34 = J β12m24 + c42 J β12
Simulink and a Sensoray 626 data acquisition board as an
̣
cr 41 = J β21(c56 + c66 J β22) + J β21(m56 + J β22m66) interface for sending the control signals to the motor drives
̣ and receiving the encoder measurements.
+ c21 J β12 + c22 J β11 J β12 + J β12 J β11m22
Figure 4 shows a schematic diagram of the whole system
cr 42 = c23 J β12 and Figure 5 shows a photograph.

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

 é0 0 0 0 1 0 0 0ù
ê ú
ê0 0 0 0 0 1 0 0ú
ê0 0 0 0 0 0 1 0ú
ê ú
¶f ( z ,t ) ê0 0 0 0 0 0 0 1ú
A= =ê ,
¶z 0 0 a53 0 0 0 0 0ú
z = z ,t =t
e e
ê ú
ê0 a62 0 0 0 0 0 0ú
ê ú
ê0 0 a73 0 0 0 0 0ú
êë0 a82 0 0 0 0 0 0 úû

é0 0ù
ê ú
ê0 0ú
ê0 0ú
ê ú
¶f ( z ,t ) ê 0 0ú
B= =ê
¶t b 0ú
z = z ,t =t
e e
ê 51 ú
ê 0 b62 ú
Figure 5. Photograph of the experimental prototype ê ú
ê b71 0 ú
êë 0 b82 úû
4.2 Controller design

Most of the references that treat with spherical pendulums with

(e.g., [8, 14, 7]), and including the manufacturer of the
a62 = glcp mp h 1 / h 3
commercial version of this mechanism [15], assume that the
angles ϕ and θ of the pendulum are almost zero. Therefore,
a82 = gL lcp2 mp2 / h 3
the system can be decoupled in two systems, and a linear‐
ized dynamics model is obtained for each system in order a53 = − gL lcp2 mp2 / h 4
to apply a linear controller.
a73 = glcp mp h 2 / h 4
We now present the linear model of the complete system
(13) in order to apply an LQR controller. b51 = (mp lcp2 + I px ) / h 4

4.2.1 Model linearization b71 = − L lcp mp / h 4

̣
T
Considering z = q q as the vector of the state variables, b62 = L lcp mp / h 3
it is possible to represent the system (13) as the nonlinear
state system b82 = (mp lcp2 + I px ) / h 3

h 1 = I 2 + I 4 + I pz + L 2m3 + L 2mp + lc22m2 + lc42m4

d éq ù é q& ù h 2 = I 1 + I 3 + L 2m2 + L 2mp + lc12m1 + lc32m3

ê ú = f ( z ,t ) = ê ú (31)
êë M ( r ) éët - C ( r , r& ) q& - g ( r ) ùû úû
-1
dt ëq& û
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 + I pz lcp2 mp + L 2lcp2 m3mp + lc22lcp2 m2mp
compute ρ and ρ in terms of q and q .
+ lc42lcp2 m4mp
π T
Considering ze = 0 0 0 2
0 0 0 0 as the equilibri‐ h 4 = I 1I px + I 3 I px + I px L 2m2 + I px L 2mp + I 1lcp2 mp
um point, and as the input torques in the equilibrium point
+ I px lc12m1 + I 3lcp2 mp + I px lc32m3 + L 2lcp2 m2mp
must be zero (i.e., τ̄ e = 0 0 T ), it is possible to obtain a
+ lc12lcp2 m1mp + lc32lcp2 m3mp
linearized system of the form

4.2.2 LQR controller

z& = Az + Bt (32)
It is well–known in literature that system (32) can be
stabilized using a linear state–feedback control law given
by
where the constant matrices A and B are defined as

Israel Soto and Ricardo Campa: 11

Modelling and Control of a Spherical Inverted Pendulum on a Five-Bar Mechanism
 t = -Kz. (33) Key Description Value Units

L Total length of 0.127 m

And if K is chosen such that FBM links

lc1 length to the com 0.047 m

K = G -1BT P of link 1

lc2 length to the com 0.069 m

where P is found by solving the algebraic Riccati equation of link 2

lc3 length to the com 0.062 m

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

lc4 length to the com 0.045 m

using some positive definite matrices of proper dimensions of link 4
Q and P , then we ensure that the cost function
m1 mass of link 1 0.126 kg

m2 mass of link 2 0.08535 kg

( z Qz + t )
¥
J=ò T T
Gt dt (34)
0
m3 mass of link 3 0.063 kg

m4 mass of link 4 0.121 kg

is minimized. Such is known as an LQR controller.
I1 inertia moment of 0.0017 kgm2
4.3 Evaluation link 1

I2 inertia moment of 0.00001 kgm2

In this section, we show the evaluation of the SIP+FBM
link 2
system in a closed loop with an LQR controller. Both
simulations and experiments are tested. I3 inertia moment of 0.00001 kgm2
link 3
4.3.1 Simulations
I4 inertia moment of 0.0014 kgm2
link 4
In order to apply the LQR controller to the system (13), we
consider for the parameters of the system the values that lcp length to the 0.155 m
are shown in Table 1. These parameters were estimated pendulum com
using conventional identification procedures for our
mp mass of the 0.125 kg
academic prototype.
pendulum

Considering that the matrices Q and G of (34) are defined I px inertia moment of 0.0017 kgm2
by pendulum

I py inertia moment of 0.000003 kgm2

é7 0 0 0 0 0 ù 0 0 pendulum
ê ú
ê0 6 0 0 0 0 0 0 ú
Table 1. Parameters of the SIP+FBM mechanism
ê0 0 6 0 0 0 0 0 ú
ê ú
ê0 0 0 7 0 0 0 0 ú
Q=ê with gains k11 = − 0.6236, k13 = − 4.9548, k15 = − 0.2669,
0 0 0 0 0.0001 0 0 0 ú
ê ú k17 = − 0.6912, k22 = 4.7337, k24 = − 0.6236, k26 = 0.6591,
ê0 0 0 0 0 0.0001 0 0 ú
ê ú k28 = − 0.2618.
ê0 0 0 0 0 0 0.0001 0 ú
êë 0 0 0 0 0 0 0 0.0001úû Thḛ position errors of the
̰ ̰ independent
̰ variables are given
by q 1 = q1d − q1,ϕ = ϕd − ϕ , θ = θd − θ,q 2 = q2d − q2, where q1d ,ϕd ,θd ,
and q2d are the desired positions which correspond to the
é18 0 ù
G=ê ú equilibrium point ze .
ë 0 18 û
The initial conditions were considered as
5π − 5π π T
the K matrix is given by z0 = 0 0 0 0 0 . Figure 6 shows the
180 180 ̰ 2 ̰ ̰ ̰
time evolution of q 1 and q 2, while the graphics for ϕ and θ
ék 0 k13 0 k15 0 k17 0ù are shown in Figure 7. From figures 6 and 7, we can observe
K = ê 11 ú
ë0 k22 0 k24 0 k26 0 k28 û that the errors converge to zero, i.e., that the states converge
to the equilibrium point.

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

 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̰ thḛ
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
Figure 10. Applied torque for the joint of q1
converge to the equilibrium point.

Israel Soto and Ricardo Campa: 13

Modelling and Control of a Spherical Inverted Pendulum on a Five-Bar Mechanism
 [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. Springer-
Verlag.
[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.
Figure 11. Applied torque for the joint of q2
[10] Liu G, Nesic D, Mareels I (2005). Modelling and
stabilization of a spherical inverted pendulum. In:
5. Conclusions IFAC World Congress; July 2005; Prague, Czech
Republic.
This document presented a way of obtaining the kinematics [11] Aguilar-Ibanez D, Gutierrez O, Sossa-Azuela H
and dynamics model of a system consisting of an inverted (2006). Lyapunov approach for the stabilization of
pendulum mounted on a FBM. The dynamics model is the inverted spherical pendulum. In: IEEE Confer‐
obtained in terms of a non-minimal set of generalized ence on Decision and Control; December 2006; San
coordinates and then it is reduced using an approach Diego, CA, USA.
proposed in the literature. In order to validate this resulting [12] Ishii C, Nishitani Y, Hashimoto H (2009). Modelling
model, we carried out some simulations and experiments and Robust Stabilization of a Closed-link 2-dof
on a didactic prototype we have in our laboratory. The Inverted Pendulum with Gain Scheduled Control.
resulting model is novel, however, since it uses no simpli‐ International Journal of Modelling, Identification
fications as in [12]. and Control; 6: 320-332.
[13] Viet TD, Doan PT, Giang H, Kim HK, Kim SB (2012).
6. Acknowledgements Control of a 2-dof Omnidirectional Mobile Inverted
Pendulum. Journal of Mechanical Science and
This work was partially supported by PROMEP, DGEST
Technology; 26: 2921-2928.
and CONACYT (grant 60230). The authors would also like
[14] Chung C, Lee J, Lee B (2000). Balancing of an
to thank Jaqueline Bernal for her support in acquiring a
inverted pendulum with a redundant direct-drive
better understanding of DAEs.
robot. In: IEEE International Conference on Robot‐
ics and Automation; June 2000; San Francisco, CA,
7. References
USA.
[1] Aracil J, Gordillo F (2004). The inverted pendulum: [15] Quanser. 2 DOF Inverted Pendulum/Gantry
A benchmark in nonlinear control. In: IFAC World [Internet]. Available from: http://www.quanser.
Congress; August 2004; Seville, Spain. com/Products/2DOF_pendulum. Accessed January
2015.
[2] Spong M, Block J (1995). The pendubot: A mecha‐
[16] Dabney JB, Ghorbel FH (2002). Modeling closed
tronic system for control research and education. In:
kinematic chains via singular perturbations. In:
IEEE Conference on Decision and Control; Decem‐
IEEE American Control Conference; May 2002;
ber 1995; New Orleans, LA, USA.
Anchorage, AK, USA.
[3] Spong MW (1995). The Swing up Control Problem
[17] Alvarado O (2010). Construction, Modelling and
for the Acrobot. IEEE Control Systems Magazine;
Control of a Spherical Inverted Pendulum Mecha‐
15: 49-55.
nism (in Spanish) [Master's thesis]. Torreón,
[4] Spong MW, Corke P, Lozano R (1999). Nonlinear Mexico: Instituto Tecnológico de la Laguna.
Control of the Inertia Wheel Pendulum. Automati‐ [18] Ghorbel FH, Chételat O, Gunawardana R, Long‐
ca; 37: 1845-1851. champ R (2000). Modeling and Set Point Control of

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

 Closed-chain Mechanisms: Theory and Experi‐ Algebraic Equations. Society for Industrial and
ment. IEEE Transactions on Control Systems Applied Mathematics (SIAM).
Technology; 8: 801-815. [24] Murray R, Li Z, Sastry S (1994). A Mathematical
[19] Merlet JP (2006). Parallel Robots. Springer-Verlag. Introduction to Robotic Manipulation. CRC Press.
[20] Khan W, Krovi V, Saha S, Angeles J (2005). Recur‐ [25] García de Jalon J, Bayo E (1994). Kinematic and
sive Kinematics and Inverse Dynamics for a Planar Dynamic Simulation of Multibody Systems: The
3R Parallel Manipulator. Journal of Dynamic Real-Time Challenge. Springer-Verlag.
Systems, Measurement, and Control; 127: 529-536. [26] Shabana AA (2001). Computational Dynamics.
[21] Hairer E, Wanner G (1996). Solving Ordinary Wiley.
Differential Equations II. Springer-Verlag.
[27] Sciavicco L, Siciliano B. (2000). Modelling and
[22] Kecskeméthy A, Krupp T, Hiller M (1997). Symbolic Control of Robot Manipulators. Springer-Verlag.
Processing of Multi-loop Mechanism Dynamics
[28] Campa R, de la Torre H (2009). Pose control of robot
using Closed form Kinematics Solutions. Multi‐
manipulators using different orientation represen‐
body Systems Dynamics; 1(1): 23-45.
tations: A comparative review. In: IEEE American
[23] Ascher U, Petzold LR (1998). Computer Methods for
Control Conference; June 2009; St. Louis, MO, USA.
Ordinary Differential Equations and Differential-

Israel Soto and Ricardo Campa: 15

Modelling and Control of a Spherical Inverted Pendulum on a Five-Bar Mechanism
 Appendix Given (35), it can be proved that

Computation of matrix R (ρ )
Consider a closed-chain mechanism which is described by ¶g ( r )
R ( r ) = 0 p´ n
means of the vector ρ ∈ ℝm of generalized coordinates, from ¶r
which we can extract the vector q ∈ ℝn of independent
coordinates using the function
¶a ( r )
R ( r ) = I n´ n
¶r
q = a (r )

Now, it is worth noticing that we can write

and let

g (q) = 0 é 0 p´ n ù
Y ( r ) R( r ) = ê ú
êë I n´n úû
be the vector of holonomic constraints of the system.
The matrix R (ρ ) ∈ ℝm×n , which allows the transformation of where
the system dynamics from an index-3 DAE to an index-1
DAE, as described in Section 2, can be computed using the é ¶g ( r ) ù
following expression ê ú
¶r ú
Y(r ) = ê Î ¡ m´ m
ê ¶a ( r ) ú
T
ê ú
æ ¶a ( r ) ö êë ¶r úû
R ( r ) = éë I m´ m - G ùû ç ÷ S (35)
è ¶r ø
and as Ψ(ρ ) is full-rank, then it is possible to also write
where
-1 é 0 p´ n ù
R( r ) = Y ( r ) ê ú
-1 êë I n´n úû
æ æ ¶a r ö
( ) ÷ é I - G ù æç ¶a ( r ) ö÷ ö÷ Î ¡n´n
T

S = çç ë m´ m û ç ¶r ÷ ÷
çç çè ¶r ÷ø è ø ÷ø
è which agrees with the definition of R (ρ ) found in [18].

-1
¶g ( r ) æ ¶g ( r ) ¶g ( r ) ö ¶g ( r )
T T

G= ç ÷ Î ¡ m´ m
¶r ç ¶r ¶r ÷ ¶r
è ø

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