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

Abstract

This paper describes the kinematics and dynamics modelling 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 generalized 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.

1. Introduction

The problem of balancing an inverted pendulum has attracted the attention of control researchers in recent decades [1]. There exist a wide variety of inverted-pendulum-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.

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.

Figure 1.

Spherical inverted-pendulum on a five-bar mechanism

The SIP has been studied by numerous researchers (see [7 – 11]). The interest in studying the SIP is due to its mathematical 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:

An XY-table[7].

An omnidirectional vehicle [13].

A robotic arm [8,14].

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 purposes (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 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). Qn 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 controller in both simulations and experiments in our prototype.

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 validation of the models via simulations/experiments with an LQR controller is shown in Section 4, while final conclusions are given in Section 5.

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

2.1 Kinematics model

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.

Let $\rho ={\left[\begin{array}{cc}{q}^T& {\beta }^T\end{array}\right]}^T\in {ℝ}^m$ be the vector of generalized joint variables (q ∊ ℝⁿ be the vector of independent variables and β ∊ ℝ^p be the vector of dependent, or constrained, variables). The holonomic constraints can then be grouped in a vector of the form

$$\gamma \left(\rho \right)=0\in {ℝ}^p$$

so that, as a result, it should be possible to express β as a function of q, i.e.,

$$\beta =\beta \left(q\right).$$

The direct kinematics problem consists in finding expressions 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\left(q,\beta \left(q\right)\right).$$

In addition, by taking the time derivative of the previous equation we get the so-called differential kinematics equation

$$\dot{x}=J\left(\rho \right)\dot{q}$$

(1)

where $J\left(\rho \right)=\frac{\partial f\left(q,\beta \left(q\right)\right)}{\partial 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}\left(x\right).$$

The inverse kinematics problem can be solved in general form by geometric or analytic approaches, such as those mentioned in [19].

2.2 Dynamics model

As mentioned in [20], the dynamics model of CCMs using either the Newton-Euler or the Euler-Lagrange methodology is traditionally obtained by cutting the closed loops to obtain systems with open chains and/or tree-type structures, so that it is possible to write a set of ODE s for such chains in their corresponding generalized coordinates. The solution to these equations requires satisfying additional algebraic equations guaranteeing the closure of the cut-open 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.

The most general form of a differential-algebraic system of n equations is that of an implicit differential equation [21]

$$h\left(u,\dot{u},t\right)=\left[\begin{array}{c}{h}_1\left(u,\dot{u},t\right)\\ h_2\left(u,\dot{u},t\right)\\ ⋮\\ h_n\left(u,\dot{u},t\right)\end{array}\right]=0\in {ℝ}^n$$

(2)

where u ∊ ℝⁿ is the vector of generalized coordinates, t is the time variable, and $\dot{u}=\frac{du}{dt}$ .

System (2) has a differential index μ if μ is the minimal number of analytical differentiations

$$h\left(u,\dot{u},t\right)=0,\frac{dh\left(u,\dot{u},t\right)}{dt}=0,\dots \frac{d^{\mu }h\left(u,\dot{u},t\right)}{d{t}^{\mu }}=0$$

(3)

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

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]:

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 coordinates (see [22]).

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

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 sub spaces 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

$$\frac{d}{dt}\left\{\frac{\partial ℒ(\rho ,\dot{\rho })}{\partial \dot{\rho }}\right\}-\frac{\partial ℒ(\rho ,\dot{\rho })}{\partial \rho }={\tau }_{\rho }+{\left(\frac{\partial \gamma (\rho )}{\partial \rho }\right)}^T\text{}\lambda$$

(4)

$$\gamma \left(\rho \right)=0,$$

(5)

where the Lagrangian $ℒ\left(\rho ,\dot{\rho }\right)$ can be defined as

$$ℒ\left(\rho ,\dot{\rho }\right)=\mathcal{K}\left(\rho ,\dot{\rho }\right)-\mathcal{U}\left(\rho \right)$$

(6)

being $\mathcal{K}\left(\rho ,\dot{\rho }\right)$ 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\left(\rho \right)\ddot{\rho }+C\left(\rho ,\dot{\rho }\right)\dot{\rho }+g\left(\rho \right)={\tau }_{\rho }+D^T\left(\rho \right)\lambda$$

(7)

where M(ρ)∊ℝ^m×m is the inertia matrix, $C\left(\rho ,\dot{\rho }\right)\in {ℝ}^{m\times m}$ is the matrix of terms generated by centrifugal and Coriolis forces, g(ρ)∊ℝ^m represents the vector of gravitational forces, and $D\left(\rho \right)=\partial \gamma \left(\rho \right)/\partial \rho \in {ℝ}^{p\times m}$ is called the constraint Jacobian matrix.

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

$$\mathcal{K}\left(\rho ,\dot{\rho }\right)=\frac{1}{2}{\dot{\rho }}^{T}M\left(\rho \right)\dot{\rho }$$

(8)

$$g\left(\rho \right)=\frac{\partial \mathcal{U}\left(\rho \right)}{\partial \rho }$$

(9)

With the matrix M(ρ), it is possible to compute the matrix $C(\rho ,\dot{\rho })$ by means of the Christoffel symbols c_ijk(ρ) defined as

$$c_{ijk}\left(\rho \right)=\frac{1}{2}\left[\frac{\partial M_{kj}\left(\rho \right)}{\partial {\rho }_i}+\frac{\partial M_{ki}\left(\rho \right)}{\partial {\rho }_j}-\frac{\partial M_{ij}\left(\rho \right)}{\partial {\rho }_k}\right],$$

(10)

where M_ij(ρ) denotes the ij th element of the inertia matrix M(ρ). Indeed, the kj th element $C_{kj}\left(\rho ,\dot{\rho }\right)$ of the matrix $C\left(\rho ,\dot{\rho }\right)$ is given by

$$C_{kj}\left(\rho ,\dot{\rho }\right)=\left[\begin{array}{cccc}{c}_{1jk}\left(\rho \right)& c_{2jk}\left(\rho \right)& \cdots & c_{njk}\left(\rho \right)\end{array}\right]\dot{\rho }$$

(11)

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

$$\dot{\rho }=R\left(\rho \right)\dot{q}.$$

(12)

The matrix R(ρ) for a particular system depends on the selection of the generalized coordinates ρ, the independent variables ρ (which can be extracted from ρ through a function α such that ρ = α(ρ)), and the constraints γ(ρ). Appendix A describes a procedure to obtain R(ρ) starting from these relations.

Using such a matrix R(ρ), it is possible to reduce the system (7) to a new index-1 DAE system

$$\begin{array}{l}\overline{M}\left(\rho \right)\ddot{q}+\overline{C}\left(\rho ,\dot{\rho }\right)\dot{q}+\overline{g}\left(\rho \right)=\overline{\tau },\\ \gamma \left(\rho \right)=0,\\ q=\alpha \left(\rho \right)\end{array}$$

(13)

where $\overline{M}\left(\rho \right)=R{\left(\rho \right)}^{T}M\left(\rho \right)R\left(\rho \right)$ $\overline{C}\left(\rho ,\dot{\rho }\right)=R{\left(\rho \right)}^{T}C\left(\rho ,\dot{\rho }\right)R\left(\rho \right)+R{\left(\rho \right)}^{T}M\left(\rho \right)\dot{R}\left(\rho ,\dot{\rho }\right)$

$\overline{g}\left(\rho \right)=R{\left(\rho \right)}^{T}g\left(\rho \right)$

$\overline{\tau }=R{\left(\rho \right)}^T{\tau }_{\rho }$ and notice that the states which define the dynamics of the system are $q$ , $\dot{q}$ and ρ, which can be obtained by integrating the corresponding state equations.

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 obtained. This figure also shows the frames attached to each extreme of the FBM (Σ₀ and Σ⁰), 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 x_i,y_i, z_i.

Figure 2.

Schematic diagram of the SIP+FBM

The angles θ and φ indicate the orientation of the extreme of the pendulum with respect to the frame Σ_b;θ is the angle that rotates with respect to the axis y_b and φ is the angle that rotates with respect to the axis x_b.

The angles q₁ and q₂ of the active joints of the FBM determine its configuration. The angles β₁ and β₂ of the passive joints depend upon q₁ and q₂. For the purposes of analysis, in this paper the vector of independent joint variables is defined as

$$q={\left[\begin{array}{cccc}{q}_1& \varphi & \theta & q_2\end{array}\right]}^T$$

(14)

and the vector of pose variables of the pendulum as

$$x={\left[\begin{array}{cccc}{x}_b& y_b& \varphi & \theta \end{array}\right]}^T.$$

(15)

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

3.1 Kinematics model

3.1.1 Direct kinematics model

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

$$\begin{array}{l}{\left(x_b-LC\left(q_1\right)\right)}^2+{\left(y_b-LS\left(q_1\right)\right)}^2=L^2,\hfill \\ \hfill \\ {\left(x_b-\left(2L+LC\left(q_2\right)\right)\right)}^2+{\left(y_b-LS\left(q_2\right)\right)}^2=L^2,\hfill \end{array}$$

where the notations S(·) and C(·) are used for sin(·) y cos(·), respectively. Using a software tool such as MATLAB, it is possible to find the following two solutions for x_b and y_b

Figure 3.

Geometric relations of the FBM

$$\begin{array}{ll}\begin{array}{l}{x}_b=\frac{L}{2}\left[2+C\left(q_1\right)+C\left(q_2\right)\right]\\ \pm \frac{L}{2}\left[S\left(q_1\right)-S\left(q_2\right)\right]\sqrt{-\frac{C\left(q_1-q_2\right)+2C\left(q_1\right)-2C\left(q_2\right)-1}{C\left(q_1-q_2\right)+2C\left(q_1\right)-2C\left(q_2\right)-3}}\end{array}& \begin{array}{l}{y}_b=\frac{L}{2}\left[S\left(q_1\right)+S\left(q_2\right)\right]\\ \pm \frac{L}{2}\left[C\left(q_2\right)-C\left(q_1\right)+2\right]\sqrt{-\frac{C\left(q_1-q_2\right)+2C\left(q_1\right)-2C\left(q_2\right)-1}{C\left(q_1-q_2\right)+2C\left(q_1\right)-2C\left(q_2\right)-3}}.\end{array}\end{array}$$

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

$$\begin{array}{l}{\beta }_1\left(q\right)={{\displaystyle \tan }}^{-1}\left(\frac{y_b-LS\left(q_1\right)}{x_b-LC\left(q_1\right)}\right)\hfill \\ {\beta }_2\left(q\right)={{\displaystyle \tan }}^{-1}\left(\frac{y_b-LS\left(q_2\right)}{x_b-\left(2L+LC\left(q_2\right)\right)}\right)\hfill \end{array}$$

3.1.2 Inverse kinematics model

From Figure 3 (a), the following expressions can be defined

$$x_b=LC\left(q_1\right)+LC\left(q_1+{\beta }_1\right)$$

(16)

$$y_b=LS\left(q_1\right)+LS\left(q_1+{\beta }_1\right)$$

(17)

$$x_b=2L+LC\left(q_2\right)+LC\left(q_2+{\beta }_2\right)$$

(18)

$$y_b=LS\left(q_2\right)+LS\left(q_2+{\beta }_2\right)$$

(19)

and on the other hand, from Figure 3 (b) it is observed that

$$x_b=rC\left(\alpha \right)\mathrm{and}{y}_b=rS\left(\alpha \right)$$

(20)

where

$$r=\sqrt{x_b^2+y_b^2}\mathrm{and}\alpha ={\tan }^{-1}\left(\frac{y_b}{x_b}\right)$$

(21)

and also

$$2q_1+{\beta }_1=2\alpha$$

(22)

As such, making some algebraic operations with equations (16), (17) and (20)–(22), it yields

$$\begin{array}{l}{q}_1={{\displaystyle \tan }}^{-1}\left(\frac{y_b}{x_b}\right)\mp {{\displaystyle \cos }}^{-1}\left(\frac{\sqrt{x_b^2+y_b^2}}{2L}\right)\hfill \\ {\beta }_1=\pm 2{{\displaystyle \cos }}^{-1}\left(\frac{\sqrt{x_b^2+y_b^2}}{2L}\right).\hfill \end{array}$$

Following a similar procedure, but starting from equations (18) and (19), we get

$$\begin{array}{l}{q}_2={{\displaystyle \tan }}^{-1}\left(\frac{y_b}{x_b-2L}\right)\mp {{\displaystyle \cos }}^{-1}\left(\frac{\sqrt{{\left(x_b-2L\right)}^2+y_b^2}}{2L}\right)\hfill \\ {\beta }_2=\mp 2{{\displaystyle \cos }}^{-1}\left(\frac{\sqrt{{\left(x_b-2L\right)}^2+y_b^2}}{2L}\right)\hfill \end{array}$$

3.1.3 Differential kinematics model

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

$${\dot{x}}_b=-LS\left(q_i\right){\dot{q}}_i-LS\left(q_i+{\beta }_i\right)\left({\dot{q}}_i+{\dot{\beta }}_i\right)$$

(23)

$${\dot{y}}_b=LC\left(q_i\right){\dot{q}}_i+LC\left(q_i+{\beta }_i\right)\left({\dot{q}}_i+{\dot{\beta }}_i\right)$$

(24)

As such, multiplying the equations (23) and (24) by cos(q_i + β_i) and sin(q_i + β_i), respectively, and adding the results, we further obtain

$${\dot{x}}_{b}C\left(q_i+{\beta }_i\right)+{\dot{y}}_{b}S\left(q_i+{\beta }_i\right)=LS\left({\beta }_i\right){\dot{q}}_i$$

(25)

and by taking i = 1,2 in (25), we find the matrix equation that relates the operational velocities ${\dot{x}}_b$ and ${\dot{y}}_b$ with the actuated joint velocities ${\dot{q}}_1$ and ${\dot{q}}_2$

$$\left[\begin{array}{cc}C\left(q_1+{\beta }_1\right)& S\left(q_1+{\beta }_1\right)\\ C\left(q_2+{\beta }_2\right)& S\left(q_2+{\beta }_2\right)\end{array}\right]\left[\begin{array}{c}{\dot{x}}_b\\ {\dot{y}}_b\end{array}\right]=L\left[\begin{array}{cc}S\left({\beta }_1\right)& 0\\ 0& S\left({\beta }_2\right)\end{array}\right]\left[\begin{array}{c}{\dot{q}}_1\\ {\dot{q}}_2\end{array}\right].$$

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

$$J\left(\rho \right)=L\left[\begin{array}{cccc}\frac{S\left(q_2+{\beta }_2\right)S\left({\beta }_1\right)}{S\left(q_2+{\beta }_2-q_1-{\beta }_1\right)}& 0& 0& \frac{-S\left(q_1+{\beta }_1\right)S\left({\beta }_2\right)}{S\left(q_2+{\beta }_2-q_1-{\beta }_1\right)}\\ \frac{-C\left(q_2+{\beta }_2\right)S\left({\beta }_1\right)}{S\left(q_2+{\beta }_2-q_1-{\beta }_1\right)}& 0& 0& \frac{C\left(q_1+{\beta }_1\right)S\left({\beta }_2\right)}{S\left(q_2+{\beta }_2-q_1-{\beta }_1\right)}\\ 0& 1& 0& 0\\ 0& 0& 1& 0\end{array}\right].$$

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

$$\begin{array}{l}-S\left(q_1\right){\dot{q}}_1-S\left(q_1+{\beta }_1\right)\left({\dot{q}}_1+{\dot{\beta }}_1\right)=-S\left(q_2\right){\dot{q}}_2-S\left(q_2+{\beta }_2\right)\left({\dot{q}}_2+{\dot{\beta }}_2\right)\\ C\left(q_1\right){\dot{q}}_1+C\left(q_1+{\beta }_1\right)\left({\dot{q}}_1+{\dot{\beta }}_1\right)=C\left(q_2\right){\dot{q}}_2+C\left(q_2+{\beta }_2\right)\left({\dot{q}}_2+{\dot{\beta }}_2\right).\end{array}$$

which leads to

$$\left[\begin{array}{c}{\dot{\beta }}_1\\ {\dot{\beta }}_2\end{array}\right]=J_{\beta }\left(q,\beta \left(q\right)\right)\left[\begin{array}{c}{\dot{q}}_1\\ {\dot{q}}_2\end{array}\right]$$

(26)

where

$$J_{\beta }\left(q,\beta \left(q\right)\right)=\left[\begin{array}{cc}-\frac{S(q_1-q_2-{\beta }_2)}{S(q_1-q_2+{\beta }_1-{\beta }_2)}-1& -\frac{S({\beta }_2)}{S(q_1-q_2+{\beta }_1-{\beta }_2)}\\ \frac{S({\beta }_1)}{S(q_1-q_2+{\beta }_1-{\beta }_2)}& -\frac{S(q_1-q_2+{\beta }_1)}{S(q_1-q_2+{\beta }_1-{\beta }_2)}-1\end{array}\right].$$

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

3.2 Dynamics modelling

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 $\rho ={\left[\begin{array}{cccccc}{q}_1& {\beta }_1& \varphi & \theta & q_2& {\beta }_2\end{array}\right]}^T\in {ℝ}^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:

$$\mathcal{K}\left(\rho ,\dot{\rho }\right)={\displaystyle \sum _{i=1}^4}{\mathcal{K}}_i\left(\rho ,\dot{\rho }\right)+{\mathcal{K}}_p\left(\rho ,\dot{\rho }\right)$$

(27)

where K_i is the kinetic energy associated with the link i of the FBM and K_p is the kinetic energy associated with the pendulum; K_i and K_p can be obtained by [27]

$${\mathcal{K}}_i=\frac{1}{2}{m}_i{\dot{p}}_i^T{\dot{p}}_i+\frac{1}{2}{\omega }_i^T{I}_i{\omega }_i$$

(28)

$${\mathcal{K}}_p=\frac{1}{2}{m}_p{\dot{p}}_p^T{\dot{p}}_p+\frac{1}{2}{\omega }_p^T{I}_p{\omega }_p$$

(29)

where ${\dot{p}}_i$ and ω_i represent the linear and angular velocities of the centre of mass of the link i, respectively, m_i and I_i are the mass and the inertia moment of the link i, respectively, and where ${\dot{p}}_p$ and ω_p represent the linear and angular velocities of the centre of mass of the pendulum, respectively (considering all the vectors with respect to frame Σ₀).

The position vectors of the centres of mass of each link for the FBM are

$$\begin{array}{l}{p}_1=\left[\begin{array}{c}{l}_{c1}C\left(q_1\right)\\ l_{c1}S\left(q_1\right)\\ 0\end{array}\right],p_4=\left[\begin{array}{c}{l}_{c4}C\left(q_2\right)\\ l_{c4}S\left(q_2\right)\\ 0\end{array}\right]\hfill \end{array}\begin{array}{l}{p}_2=\left[\begin{array}{c}LC\left(q_1\right)+l_{c1}C\left(q_1+{\beta }_1\right)\\ LS\left(q_1\right)+l_{c1}S\left(q_1+{\beta }_1\right)\\ 0\end{array}\right],\hfill \\ p_3=\left[\begin{array}{c}2L+LC\left(q_2\right)+l_{c3}C\left(q_2+{\beta }_2\right)\\ LS\left(q_2\right)+l_{c3}S\left(q_2+{\beta }_2\right)\\ 0\end{array}\right],\hfill \end{array}$$

where l_ci is the distance from a joint to the centre of mass of link i_i as shown in Figure 2. The position vector of the centre of mass of the pendulum is described by the equation

$$p_p={}^0{p}_b+{}_b^0{R}^b{p}_p$$

where ${}^0{p}_b$ is the position vector of the base of the SIP with respect to frame ${\Sigma }_0$ , ${}^b{p}_p$ is the position vector of the centre of mass of the pendulum with respect to Σ_b, and ${}_b^{0}R$ is the rotation matrix of Σ_b with respect to Σ₀, which are represented as

$$\begin{array}{l}{}^0{p}_b=L\left[\begin{array}{c}C\left(q_1\right)+C\left(q_1+{\beta }_1\right)\\ S\left(q_1\right)+S\left(q_1+{\beta }_1\right)\\ 0\end{array}\right],\hfill \end{array}\begin{array}{l}{}^b{p}_p=l_{cp}\left[\begin{array}{c}S\left(\theta \right)\\ -C\left(\theta \right)S\left(\varphi \right)\\ C\left(\theta \right)C\left(\varphi \right)\end{array}\right]\hfill \\ {}_b^{0}R=\left[\begin{array}{ccc}C\left(q_1+{\beta }_1\right)& -S\left(q_1+{\beta }_1\right)& 0\\ S\left(q_1+{\beta }_1\right)& C\left(q_1+{\beta }_1\right)& 0\\ 0& 0& 1\end{array}\right]\hfill \end{array}$$

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

$$\begin{array}{l}{\omega }_1={\left[\begin{array}{ccc}0& 0& {\dot{q}}_1\end{array}\right]}^T{\omega }_2={\left[\begin{array}{ccc}0& 0& {\dot{q}}_1+{\dot{\beta }}_1\end{array}\right]}^T\hfill \\ {\omega }_4={\left[\begin{array}{ccc}0& 0& {\dot{q}}_2\end{array}\right]}^T{\omega }_3={\left[\begin{array}{ccc}0& 0& {\dot{q}}_2+{\dot{\beta }}_2\end{array}\right]}^T\hfill \end{array}$$

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

$${\omega }_p=\frac{1}{2}\left[\begin{array}{ccc}S\left(r_1\right)& S\left(r_2\right)& S\left(r_3\right)\end{array}\right]\left[\begin{array}{c}{\dot{r}}_1\\ {\dot{r}}_2\\ {\dot{r}}_3\end{array}\right]$$

(30)

where S (·) is a skew-symmetric matrix such that, for a vector $v={\left[\begin{array}{ccc}{v}_1& v_2& v_3\end{array}\right]}^T$ , is defined as

$$S\left(v\right)=\left[\begin{array}{ccc}0& -v_3& v_2\\ v_3& 0& -v_1\\ -v_2& v_1& 0\end{array}\right]$$

and the vectors r₁,r₂ and r₃ are the columns of the rotation matrix from frame Σ_p with respect to frame Σ₀, the elements of which are

$r_{11}=C\left(q_1+{\beta }_1\right)C\left(\theta \right)-S\left(q_1+{\beta }_1\right)S\left(\varphi \right)S\left(\theta \right)$

$r_{12}=-S\left(q_1+{\beta }_1\right)C\left(\varphi \right)$

$r_{13}=C\left(q_1+{\beta }_1\right)S\left(\theta \right)+S\left(q_1+{\beta }_1\right)S\left(\varphi \right)C\left(\theta \right)$

$r_{21}=S\left(q_1+{\beta }_1\right)C\left(\theta \right)+C\left(q_1+{\beta }_1\right)S\left(\varphi \right)S\left(\theta \right)$

$r_{22}=C\left(q_1+{\beta }_1\right)C\left(\varphi \right)$

$r_{23}=S\left(q_1+{\beta }_1\right)S\left(\theta \right)-C\left(q_1+{\beta }_1\right)S\left(\varphi \right)C\left(\theta \right)$

$r_{31}=-C\left(\varphi \right)S\left(\theta \right)$

$r_{32}=S\left(\varphi \right)$

$r_{33}=C\left(\varphi \right)C\left(\theta \right).$

Therefore, using (30), we get

$${\omega }_p=\left[\begin{array}{c}C\left(q_1+{\beta }_1\right)\dot{\varphi }-S\left(q_1+{\beta }_1\right)C\left(\varphi \right)\dot{\theta }\\ S\left(q_1+{\beta }_1\right)\dot{\varphi }+C\left(q_1+{\beta }_1\right)C\left(\varphi \right)\dot{\theta }\\ {\dot{q}}_1+{\dot{\beta }}_1+S\left(\varphi \right)\dot{\theta }\end{array}\right].$$

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 z₀ axis are of interest for the links, namely I₁I₂,I₃ and I₄. For the SIP, the matrix of the inertia moments is given by

$${}^0{I}_p={}_b^{0}R\left[\begin{array}{ccc}{I}_{px}& 0& 0\\ 0& I_{px}& 0\\ 0& 0& I_{pz}\end{array}\right]{}_b^0{R}^T$$

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 $\mathcal{K}\left(\rho ,\dot{\rho }\right)$ is calculated and (8) is used, yielding

$$M\left(\rho \right)=\left[\begin{array}{cccccc}{m}_{11}& m_{12}& m_{13}& m_{14}& 0& 0\\ m_{12}& m_{22}& m_{23}& m_{24}& 0& 0\\ m_{13}& m_{23}& m_{33}& m_{34}& 0& 0\\ m_{14}& m_{24}& m_{43}& m_{44}& 0& 0\\ 0& 0& 0& 0& m_{55}& m_{56}\\ 0& 0& 0& 0& m_{55}& m_{66}\end{array}\right]$$

where

$\begin{array}{l}{m}_{11}=I_1+I_2+I_{px}+L^2{m}_2+2L^2{m}_p+l_{c1}^2{m}_1+l_{c2}^2{m}_2\\ +l_{cp}^2{m}_p-I_{px}C{\left(\varphi \right)}^{2}C{\left(\theta \right)}^2+I_{pz}C{\left(\varphi \right)}^{2}C{\left(\theta \right)}^2\\ +2L^2{m}_{p}c\left({\beta }_1\right)-l_{cp}^{2}C{\left(\varphi \right)}^{2}C{\left(\theta \right)}^2+2L{l}_{c2}{m}_{2}C\left({\beta }_1\right)\\ +4L{l}_{cp}{m}_{p}C{\left({\beta }_1/2\right)}^{2}S\left(\theta \right)+2L{l}_{cp}{m}_{p}S\left({\beta }_1\right)C\left(\theta \right)S\left(\varphi \right)\end{array}$

$\begin{array}{l}{m}_{12}=I_2+I_{px}+L^2{m}_p+l_{c2}^2{m}_2+l_{cp}^2{m}_p+L^2{m}_{p}C\left({\beta }_1\right)\\ +I_{pz}C{\left(\varphi \right)}^{2}C{\left(\theta \right)}^2-I_{px}C{\left(\varphi \right)}^{2}C{\left(\theta \right)}^2-l_{cp}^{2}C{\left(\varphi \right)}^{2}C{\left(\theta \right)}^2\\ +L{l}_{c2}{m}_{2}C\left({\beta }_1\right)+2L{l}_{cp}{m}_{p}S\left(\theta \right)+L{l}_{cp}{m}_{p}C\left({\beta }_1\right)S\left(\theta \right)\\ +L{l}_{cp}{m}_{p}S\left({\beta }_1\right)C\left(\theta \right)S\left(\varphi \right)\end{array}$

$\begin{array}{l}{m}_{13}=-C\left(\varphi \right)C\left(\theta \right)L{l}_{cp}{m}_p\left[1+C\left({\beta }_1\right)\right]\\ -C\left(\varphi \right)C\left(\theta \right)\left(I_{px}-I_{pz}+l_{cp}^2{m}_p\right)S\left(\theta \right)\end{array}$

$\begin{array}{l}{m}_{14}=l_{cp}^2{m}_{p}S\left(\varphi \right)+2L{l}_{cp}{m}_{p}S\left(\varphi \right)S\left(\theta \right)C{\left({\beta }_1/2\right)}^2\\ +I_{px}S\left(\varphi \right)+L{l}_{cp}{m}_{p}S\left({\beta }_1\right)C\left(\theta \right)\end{array}$

$\begin{array}{l}{m}_{22}=I_2+I_{px}+L^2{m}_p+l_{c2}^2{m}_2+l_{cp}^2{m}_p\\ -\left(I_{px}-I_{pz}+l_{cp}^2{m}_p\right)C{\left(\varphi \right)}^{2}C{\left(\theta \right)}^2+2L{l}_{cp}{m}_{p}S\left(\theta \right)\end{array}$

$m_{23}=-C\left(\varphi \right)C\left(\theta \right)\left[S(\theta )\left(I_{px}-I_{pz}+l_{cp}^2{m}_p\right)+L{l}_{cp}{m}_p\right]$

$m_{24}=S\left(\varphi \right)\left[I_{px}+l_{cp}^2{m}_p+L{l}_{cp}{m}_{p}S\left(\theta \right)\right]$

$m_{33}=I_{pz}+\left(I_{px}-I_{pz}+l_{cp}^2{m}_p\right)C{\left(\theta \right)}^2$

$m_{44}=I_{pz}+l_{cp}^2{m}_p$

$m_{55}=m_4{l}_{c4}^2+m_3\left[L^2+l_{c3}^2+2L{l}_{c3}C\left({\beta }_2\right)\right]+I_4+I_3$

$m_{56}=m_3\left[l_{c3}^2+L{l}_{c3}C\left({\beta }_2\right)\right]+I_3$

$m_{66}=m_3{l}_{c3}^2+I_3$

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

$$C\left(\rho ,\dot{\rho }\right)=\left[\begin{array}{cccccc}{c}_{11}& c_{12}& c_{13}& c_{14}& 0& 0\\ c_{21}& c_{22}& c_{23}& c_{24}& 0& 0\\ c_{31}& c_{32}& c_{33}& c_{34}& 0& 0\\ c_{41}& c_{42}& c_{43}& c_{44}& 0& 0\\ 0& 0& 0& 0& c_{55}& c_{56}\\ 0& 0& 0& 0& c_{65}& c_{66}\end{array}\right]$$

where each element of the matrix is given by

$c_{11}=a_1{\dot{\beta }}_1+a_2\dot{\varphi }+a_3\dot{\theta }$

$c_{12}=a_1\left({\dot{q}}_1+{\dot{\beta }}_1\right)+a_2\dot{\varphi }+a_3\dot{\theta }$

$c_{13}=a_2\left({\dot{q}}_1+{\dot{\beta }}_1\right)+a_4\dot{\varphi }+a_5\dot{\theta }$

$c_{14}=a_3\left({\dot{q}}_1+{\dot{\beta }}_1\right)+a_5\dot{\varphi }+a_6\dot{\theta }$

$c_{21}=-a_1{\dot{q}}_1+a_7\dot{\varphi }+a_8\dot{\theta }$

$c_{22}=a_7\dot{\varphi }+a_8\dot{\theta }$

$c_{23}=a_7\left({\dot{q}}_1+{\dot{\beta }}_1\right)+a_9\dot{\varphi }+a_{10}\dot{\theta }$

$c_{24}=a_8\left({\dot{q}}_1+{\dot{\beta }}_1\right)+a_{10}\dot{\varphi }+a_{11}\dot{\theta }$

$c_{31}=-a_2{\dot{q}}_1-a_7{\dot{\beta }}_1+a_{12}\dot{\theta }$

$c_{32}=-a_7\left({\dot{q}}_1+{\dot{\beta }}_1\right)+a_{12}\dot{\theta }$

$c_{33}=a_{13}\dot{\theta }$

$c_{34}=a_{12}\left({\dot{q}}_1+{\dot{\beta }}_1\right)+a_{13}\dot{\varphi }=-c_{43}$

$c_{41}=-a_3{\dot{q}}_1-a_8{\dot{\beta }}_1-a_{12}\dot{\varphi }$

$c_{42}=-a_8\left({\dot{q}}_1+{\dot{\beta }}_1\right)-a_{12}\dot{\varphi }$

$c_{44}=0$

$c_{55}=-m_{3}L{l}_{c3}S\left({\beta }_2\right){\dot{\beta }}_2$

$c_{56}=-m_{3}L{l}_{c3}S\left({\beta }_2\right)\left({\dot{q}}_2+{\dot{\beta }}_2\right)$

$c_{65}=m_{3}L{l}_{c3}S\left({\beta }_2\right){\dot{q}}_2$

$c_{66}=0$

with $\begin{array}{l}{a}_1=L{l}_{cp}{m}_{p}C\left({\beta }_1\right)C\left(\theta \right)S\left(\varphi \right)\\ -LS\left({\beta }_1\right)\left(l_{c2}{m}_2+L{m}_p+l_{cp}{m}_{p}S\left(\theta \right)\right)\end{array}$

$\begin{array}{l}{a}_2=C\left(\varphi \right)C\left(\theta \right)L{l}_{cp}{m}_{p}S\left({\beta }_1\right)\\ +C\left(\varphi \right)C\left(\theta \right)\left(I_{px}-I_{pz}+l_{cp}^2{m}_p\right)C\left(\theta \right)S\left(\varphi \right)\end{array}$

$\begin{array}{l}{a}_3=-L{l}_{cp}{m}_{p}S\left({\beta }_1\right)S\left(\theta \right)S\left(\varphi \right)+C\left(\theta \right)L{l}_{cp}{m}_p\left(1+C\left({\beta }_1\right)\right)\\ +C\left(\theta \right)\left(I_{px}-I_{pz}+l_{cp}^2{m}_p\right)C{\left(\varphi \right)}^{2}S\left(\theta \right)\end{array}$

$\begin{array}{l}{a}_4=C\left(\theta \right)S\left(\varphi \right)L{l}_{cp}{m}_p\left(1+C\left({\beta }_1\right)\right)\\ +C\left(\theta \right)S\left(\varphi \right)\left(I_{px}-I_{pz}+l_{cp}^2{m}_p\right)S\left(\theta \right)\end{array}$

$\begin{array}{l}{a}_5=\frac{1}{2}C\left(\varphi \right)\left[I_{px}+l_{cp}^2{m}_p-\left(I_{px}-I_{pz}+l_{cp}^2{m}_p\right)C\left(2\theta \right)\right]\\ +2C\left(\varphi \right)L{l}_{cp}{m}_{p}C{\left({\beta }_1/2\right)}^{2}S\left(\theta \right)\end{array}$

$a_6=L{l}_{cp}{m}_p\left[2C{\left({\beta }_1/2\right)}^{2}C\left(\theta \right)S\left(\varphi \right)-S\left({\beta }_1\right)S\left(\theta \right)\right]$

$a_7=\left(I_{px}-I_{pz}+l_{cp}^2{m}_p\right)C\left(\varphi \right)S\left(\varphi \right)C{\left(\theta \right)}^2$

$a_8=C\left(\theta \right)\left[L{l}_{cp}{m}_p+\left(I_{px}-I_{pz}+l_{cp}^2{m}_p\right)C{\left(\varphi \right)}^{2}S\left(\theta \right)\right]$

$a_9=C\left(\theta \right)S\left(\varphi \right)\left[L{l}_{cp}{m}_p+\left(I_{px}-I_{pz}+l_{cp}^2{m}_p\right)S\left(\theta \right)\right]$

$\begin{array}{l}{a}_{10}=\frac{1}{2}C\left(\varphi \right)\left[I_{px}+l_{cp}^2{m}_p-\left(I_{px}-I_{pz}+l_{cp}^2{m}_p\right)C\left(2\theta \right)\right]\\ +C\left(\varphi \right)L{l}_{cp}{m}_{p}C{\left({\beta }_1/2\right)}^{2}S\left(\theta \right)\end{array}$

$a_{11}=L{l}_{cp}{m}_{p}C\left(\theta \right)S\left(\varphi \right)$

$a_{12}=-\frac{1}{2}C\left(\varphi \right)\left[I_{px}+l_{cp}^2{m}_p+\left(I_{px}-I_{pz}+l_{cp}^2{m}_p\right)C\left(2\theta \right)\right]$

$a_{13}=-\left(I_{px}-I_{pz}+l_{cp}^2{m}_p\right)C\left(\theta \right)S\left(\theta \right)$

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

$$\mathcal{U}\left(\rho \right)=-m{p}_p^T{g}_o$$

where g₀ is a vector that indicates the gravitational acceleration with respect to Σ₀. Then, by applying (9) we get

$$g\left(\rho \right)=\frac{\partial U\left(\rho \right)}{\partial \rho }=mg{l}_{cp}\frac{\partial \left(C\left(\theta \right)C\left(\varphi \right)\right)}{\partial \rho }$$

hence

$$g\left(\rho \right)=-mg{l}_{cp}{\left[\begin{array}{cccccc}0& 0& S\left(\varphi \right)C\left(\theta \right)& C\left(\varphi \right)S\left(\theta \right)& 0& 0\end{array}\right]}^T.$$

The constraints of the system are given by

$$\gamma \left(\rho \right)=\left[\begin{array}{c}L\left[C\left(q_1\right)+C\left(q_1+{\beta }_1\right)-2-C\left(q_2\right)-C\left(q_2+{\beta }_2\right)\right]\\ L\left[S\left(q_1\right)+S\left(q_1+{\beta }_1\right)-S\left(q_2\right)-S\left(q_2+{\beta }_2\right)\right]\end{array}\right]$$

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

$$q=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\end{array}\right]\rho$$

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

$$R\left(\rho \right)=\left[\begin{array}{cccc}1& 0& 0& 0\\ J_{{\beta }_{11}}& 0& 0& J_{{\beta }_{12}}\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\\ J_{{\beta }_{21}}& 0& 0& J_{{\beta }_{22}}\end{array}\right]$$

where $J_{{\beta }_{11}}=-\frac{S\left(q_1-q_2-{\beta }_2\right)}{S\left(q_1-q_2+{\beta }_1-{\beta }_2\right)}-1$

$J_{{\beta }_{12}}=-\frac{S\left({\beta }_2\right)}{S\left(q_1-q_2+{\beta }_1-{\beta }_2\right)}$

$J_{{\beta }_{21}}=\frac{S\left({\beta }_1\right)}{S\left(q_1-q_2+{\beta }_1-{\beta }_2\right)}$

$J_{{\beta }_{22}}=-\frac{S\left(q_1-q_2+{\beta }_1\right)}{S\left(q_1-q_2+{\beta }_1-{\beta }_2\right)}-1$

The matrices of the reduced model of the system (7) are defined as

$$\begin{array}{l}\overline{M}\left(\rho \right)=\left[\begin{array}{cccc}{m}_{r11}& m_{r12}& m_{r13}& m_{r14}\\ m_{r12}& m_{r22}& m_{r23}& m_{r24}\\ m_{r13}& m_{r23}& m_{r33}& m_{r34}\\ m_{r14}& m_{r24}& m_{r34}& m_{r44}\end{array}\right]\hfill \\ \overline{C}\left(\rho ,\dot{\rho }\right)=\left[\begin{array}{cccc}{c}_{r11}& c_{r12}& c_{r13}& c_{r14}\\ c_{r21}& c_{r22}& c_{r23}& c_{r24}\\ c_{r31}& c_{r32}& c_{r33}& c_{r34}\\ c_{r41}& c_{r42}& c_{r43}& c_{r44}\end{array}\right]\hfill \end{array}$$

where the components of each one are given by

$m_{r11}=m_{66}{J}_{{\beta }_{21}}^2+m_{22}{J}_{{\beta }_{11}}^2+2m_{12}{J}_{{\beta }_{11}}+m_{11}$

$m_{r12}=m_{13}+J_{{\beta }_{11}}{m}_{23}$

$m_{r13}=m_{14}+J_{{\beta }_{11}}{m}_{24}$

$m_{r14}=J_{{\beta }_{21}}{m}_{56}+J_{{\beta }_{12}}\left(m_{12}+J_{{\beta }_{11}}{m}_{22}\right)+J_{{\beta }_{21}}{J}_{{\beta }_{22}}{m}_{66}$

$m_{r22}=m_{33}$

$m_{r23}=m_{34}$

$m_{r24}=J_{{\beta }_{12}}{m}_{23}$

$m_{r33}=m_{44}$

$m_{r34}=J_{{\beta }_{12}}{m}_{24}$

$m_{r44}=m_{66}{J}_{{\beta }_{22}}^2+m_{22}{J}_{{\beta }_{12}}^2+2m_{56}{J}_{{\beta }_{11}}+m_{55}$

$\begin{array}{l}{c}_{r11}=c_{66}{J}_{{\beta }_{21}}^2+{\dot{J}}_{{\beta }_{21}}{m}_{66}{J}_{{\beta }_{21}}+c_{11}+c_{21}{J}_{{\beta }_{11}}\\ +J_{{\beta }_{11}}\left(c_{12}+c_{22}{J}_{{\beta }_{11}}\right)+{\dot{J}}_{{\beta }_{11}}\left(m_{12}+J_{{\beta }_{11}}{m}_{22}\right)\end{array}$

$c_{r12}=c_{13}+c_{23}{J}_{{\beta }_{11}}$

$c_{r13}=c_{14}+c_{24}{J}_{{\beta }_{11}}$

$\begin{array}{l}{c}_{r14}=J_{{\beta }_{12}}\left(c_{12}+c_{22}{J}_{{\beta }_{11}}\right)+{\dot{J}}_{{\beta }_{12}}\left(m_{12}+J_{{\beta }_{11}}{m}_{22}\right)\\ +c_{65}{J}_{{\beta }_{21}}+c_{66}{J}_{{\beta }_{21}}{J}_{{\beta }_{22}}+J_{{\beta }_{21}}{\dot{J}}_{{\beta }_{22}}{m}_{66}\end{array}$

$c_{r21}=c_{31}+{\dot{J}}_{{\beta }_{11}}{m}_{23}+c_{32}{J}_{{\beta }_{11}}$

$c_{r22}=c_{33}$

$c_{r23}=c_{34}$

$c_{r24}={\dot{J}}_{{\beta }_{12}}{m}_{23}+c_{32}{J}_{{\beta }_{12}}$

$c_{r31}=c_{41}+{\dot{J}}_{{\beta }_{11}}{m}_{24}+c_{42}{J}_{{\beta }_{11}}$

$c_{r32}=c_{43}$

$c_{r33}=c_{44}$

$c_{r34}={\dot{J}}_{{\beta }_{12}}{m}_{24}+c_{42}{J}_{{\beta }_{12}}$

$\begin{array}{l}{c}_{r41}=J_{{\beta }_{21}}\left(c_{56}+c_{66}{J}_{{\beta }_{22}}\right)+{\dot{J}}_{{\beta }_{21}}\left(m_{56}+J_{{\beta }_{22}}{m}_{66}\right)\\ +c_{21}{J}_{{\beta }_{12}}+c_{22}{J}_{{\beta }_{11}}{J}_{{\beta }_{12}}+J_{{\beta }_{12}}{\dot{J}}_{{\beta }_{11}}{m}_{22}\end{array}$

$c_{r42}=c_{23}{J}_{{\beta }_{12}}$

$c_{r43}=c_{24}{J}_{{\beta }_{12}}$

$\begin{array}{l}{c}_{r44}=c_{22}{J}_{{\beta }_{12}}^2+{\dot{J}}_{{\beta }_{12}}{m}_{22}{J}_{{\beta }_{12}}+c_{55}+c_{65}{J}_{{\beta }_{22}}\\ +J_{{\beta }_{22}}\left(c_{56}+c_{66}{J}_{{\beta }_{22}}\right)+{\dot{J}}_{{\beta }_{22}}p\left(m_{56}+J_{{\beta }_{22}}{m}_{66}\right)\end{array}$

and the vector of gravitational forces is defined as

$$\overline{g}\left(\rho \right)=-m_{p}g{l}_{cp}{\left[\begin{array}{cccccc}0& 0& S(\varphi )C(\theta )& C(\varphi )S(\theta )& 0& 0\end{array}\right]}^T$$

4. Model validation via real-time control

4.1 Description of the prototype

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.

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.

Figure 4.

Schematic diagram of the SIP+FBM system

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

4.2.1 Model linearization

Considering $z={\left[\begin{array}{cc}q& \dot{q}\end{array}\right]}^T$ as the vector of the state variables, it is possible to represent the system (13) as the nonlinear state system

$$\frac{d}{dt}\left[\begin{array}{c}q\\ \dot{q}\end{array}\right]=f\left(z,\overline{\tau }\right)=\left[\begin{array}{c}\dot{q}\\ \overline{M}{\left(\rho \right)}^{-1}\left[\overline{\tau }-\overline{C}\left(\rho ,\dot{\rho }\right)\dot{q}-\overline{g}(\rho )\right]\end{array}\right]$$

(31)

where the kinematic relations in Section 3.1 can be used to compute $\rho$ and $\dot{\rho }$ in terms of $q$ and $\dot{q}$ .

Considering $z_e={\left[\begin{array}{cccccccc}0& 0& 0& \frac{\pi }{2}& 0& 0& 0& 0\end{array}\right]}^T$ as the equilibrium point, and as the input torques in the equilibrium point must be zero (i.e., ${\overline{\tau }}_e={\left[\begin{array}{cc}0& 0\end{array}\right]}^T$ ), it is possible to obtain a linearized system of the form

$$\dot{z}=Az+B\overline{\tau }$$

(32)

where the constant matrices A and B are defined as

$$\begin{array}{l}A={\frac{\partial f\left(z,\overline{\tau }\right)}{\partial z}|}_{z=z_e,\overline{\tau }={\overline{\tau }}_e}=\left[\begin{array}{cccccccc}0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 1\\ 0& 0& a_{53}& 0& 0& 0& 0& 0\\ 0& a_{62}& 0& 0& 0& 0& 0& 0\\ 0& 0& a_{73}& 0& 0& 0& 0& 0\\ 0& a_{82}& 0& 0& 0& 0& 0& 0\end{array}\right],\hfill \\ B={\frac{\partial f\left(z,\overline{\tau }\right)}{\partial \overline{\tau }}|}_{z=z_e,\overline{\tau }={\overline{\tau }}_e}=\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ 0& 0\\ b_{51}& 0\\ 0& b_{62}\\ b_{71}& 0\\ 0& b_{82}\end{array}\right]\hfill \end{array}$$

with

$a_{62}=g{l}_{cp}{m}_p{h}_1/h_3$

$a_{82}=gL{l}_{cp}^2{m}_p^2/h_3$

$a_{53}=-gL{l}_{cp}^2{m}_p^2/h_4$

$a_{73}=g{l}_{cp}{m}_p{h}_2/h_4$

$b_{51}=\left(m_p{l}_{cp}^2+I_{px}\right)/h_4$

$b_{71}=-L{l}_{cp}{m}_p/h_4$

$b_{62}=L{l}_{cp}{m}_p/h_3$

$b_{82}=\left(m_p{l}_{cp}^2+I_{px}\right)/h_3$

$h_1=I_2+I_4+I_{pz}+L^2{m}_3+L^2{m}_p+l_{c2}^2{m}_2+l_{c4}^2{m}_4$

$h_2=I_1+I_3+L^2{m}_2+L^2{m}_p+l_{c1}^2{m}_1+l_{c3}^2{m}_3$

$\begin{array}{l}{h}_3=I_2{I}_{px}+I_4{I}_{px}+I_{px}{I}_{pz}+I_{px}{L}^2{m}_3+I_{px}{L}^2{m}_p\\ +I_2{l}_{cp}^2{m}_p+I_{px}{l}_{c2}^2{m}_2+I_4{l}_{cp}^2{m}_p+I_{px}{l}_{c4}^2{m}_4\\ +I_{pz}{l}_{cp}^2{m}_p+L^2{l}_{cp}^2{m}_3{m}_p+l_{c2}^2{l}_{cp}^2{m}_2{m}_p\\ +l_{c4}^2{l}_{cp}^2{m}_4{m}_p\end{array}$

$\begin{array}{l}{h}_4=I_1{I}_{px}+I_3{I}_{px}+I_{px}{L}^2{m}_2+I_{px}{L}^2{m}_p+I_1{l}_{cp}^2{m}_p\\ +I_{px}{l}_{c1}^2{m}_1+I_3{l}_{cp}^2{m}_p+I_{px}{l}_{c3}^2{m}_3+L^2{l}_{cp}^2{m}_2{m}_p\\ +l_{c1}^2{l}_{cp}^2{m}_1{m}_p+l_{c3}^2{l}_{cp}^2{m}_3{m}_p\end{array}$

4.2.2 LQR controller

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

$$\overline{\tau }=-Kz.$$

(33)

And if K is chosen such that

$$K=G^{-1}{B}^{T}P$$

where P is found by solving the algebraic Riccati equation

$$A^{T}P+PA-PB{G}^{-1}{B}^{T}P+Q=\mathrm{0,}$$

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

$$J={\displaystyle {\int }_0^{\infty }\left(z^{T}Qz+{\overline{\tau }}^{T}G\overline{\tau }\right)dt}$$

(34)

is minimized. Such is known as an LQR controller.

4.3 Evaluation

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.

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

$$\begin{array}{l}Q=\left[\begin{array}{cccccccc}7& 0& 0& 0& 0& 0& 0& 0\\ 0& 6& 0& 0& 0& 0& 0& 0\\ 0& 0& 6& 0& 0& 0& 0& 0\\ 0& 0& 0& 7& 0& 0& 0& 0\\ 0& 0& 0& 0& 0.0001& 0& 0& 0\\ 0& 0& 0& 0& 0& 0.0001& 0& 0\\ 0& 0& 0& 0& 0& 0& 0.0001& 0\\ 0& 0& 0& 0& 0& 0& 0& 0.0001\end{array}\right]\hfill \\ G=\left[\begin{array}{cc}18& 0\\ 0& 18\end{array}\right]\hfill \end{array}$$

the K matrix is given by

$$K=\left[\begin{array}{cccccccc}{k}_{11}& 0& k_{13}& 0& k_{15}& 0& k_{17}& 0\\ 0& k_{22}& 0& k_{24}& 0& k_{26}& 0& k_{28}\end{array}\right]$$

Table 1.

Parameters of the SIP+FBM mechanism

Key	Description	Value	Units
L	Total length of FBM links	0.127	m
lc1	length to the com of link 1	0.047	m
lc2	length to the com of link 2	0.069	m
lc3	length to the com of link 3	0.062	m
lc4	length to the com of link 4	0.045	m
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 link 1	0.0017	kgm2
I2	inertia moment of link 2	0.00001	kgm2
I3	inertia moment of link 3	0.00001	kgm2
I4	inertia moment of link 4	0.0014	kgm2
lcp	length to the pendulum com	0.155	m
mp	mass of the pendulum	0.125	kg
IpX	inertia moment of pendulum	0.0017	kgm2
Ipy	inertia moment of pendulum	0.000003	kgm2

with gains k₁₁ = −0.6236, k₁₃ = −4.9548, k₁₅ = −0.2669, k₁₇ = −0.6912, k₂₂ = 4.7337, k₂₄ = −0.6236, k₂₆ = 0.6591, k₂₈ = −0.2618.

The position errors of the independent variables are given by ${\tilde{q}}_1=q_{1d}-q_1,\tilde{\varphi }={\varphi }_d-\varphi$ , $\tilde{\theta }={\theta }_d-\theta ,$ ${\tilde{q}}_2=q_{2d}-q_2$ , where $q_{1d},$ ${\varphi }_d,$ ${\theta }_d,$ and q_2d are the desired positions which correspond to the equilibrium point z_e.

The initial conditions were considered as $z_0={\left[\begin{array}{cccccccc}0& \frac{5\pi }{180}& \frac{-5\pi }{180}& \frac{\pi }{2}& 0& 0& 0& 0\end{array}\right]}^T$ . Figure 6 shows the time evolution of ${\tilde{q}}_1$ and ${\tilde{q}}_2$ , while the graphics for $\tilde{\varphi }$ and $\tilde{\theta }$ 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 6.

Position error of q₁ and q₂ (simulation)

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 simulation. 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 ${\tilde{q}}_1$ and ${\tilde{q}}_2$ , while the graphics for $\tilde{\varphi }$ and $\tilde{\theta }$ 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 8.

Position error of q₁ and q₂ (experiment)

Figure 9.

Position error of φ and θ (experiment)

Figure 10.

Applied torque for the joint of q₁

Figure 11.

Applied torque for the joint of q₂

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 simplifications as in [12].