- Email: [email protected]
New approach for swinging up the Furuta pendulum: Theory and experiments
Mechatronics 19 (2009) 1240–1250
Contents lists available at ScienceDirect
Mechatronics journal homepage: www.elsevier.com/locate/mechatronics
New approach for swinging up the Furuta pendulum: Theory and experiments Pedro X. La Hera a,*, Leonid B. Freidovich a, Anton S. Shiriaev a,b, Uwe Mettin a a b
Department of Applied Physics and Electronics, UmeaUniversity, SE-901 87 Umea, Sweden Department of Engineering Cybernetics, Norwegian University of Science and Technology, NO-7491 Trondheim, Norway
a r t i c l e
i n f o
Article history: Received 1 February 2008 Accepted 21 July 2009
PACS: 45.81.+r 45.50.Dd Keywords: Furuta pendulum Virtual holonomic constraints Motion planning Orbital stabilization of periodic trajectories Implementation
a b s t r a c t The problem of swinging up inverted pendulums has often been solved by stabilizing a particular class of homoclinic structures present in the dynamics of a physical pendulum. Here, new arguments are suggested to show how other homoclinic curves can be preplanned for dynamics of the passive-link of the robot. This is done by reparameterizing the motions according to geometrical relations among the generalized coordinates, which are known as virtual holonomic constraints. After that, conditions that guarantee the existence of periodic solutions surrounding the planned homoclinic orbits are derived. The corresponding trajectories, in contrast to homoclinic curves, admit efficient design of feedback control laws ensuring exponential orbital stabilization. The method is illustrated by simulations and supported by experimental studies on the Furuta pendulum. The implementation issues are discussed in detail. Ó 2009 Elsevier Ltd. All rights reserved.
1. Introduction For decades underactuated inverted pendulums have been used as benchmark set-ups for testing nonlinear control design methods. The original problem consists on stabilizing the pendulum around the upright equilibrium. To this end, local stabilizing controllers can be designed for the linearization of the system dynamics around the unstable equilibrium. This ensures asymptotic stability of the upright equilibrium, but often with a small domain of attraction. Thus, a swinging up strategy has to be introduced to reach its vicinity from a far located initial point, typically representing the system at rest. For this purpose a nontrivial motion planning technique is required, since finding feasible motions for underactuated system that are not feedback linearizable is a challenging task. A feedback controller that makes the planed trajectory orbitally stable must be designed as well. Various arguments have been suggested, and experimentally validated, for a number of popular setups belonging to this family. As examples we have the Furuta pendulum [3,5], the Pendubot [2,18,8], the pendulum on a cart [6,11,16], the Acrobot [9], and the inertia wheel pendulum [17].
* Corresponding author. Tel.: +46 090786 97 22; fax: +46 090786 64 69. E-mail addresses: [email protected], [email protected] (P.X. La Hera), [email protected] (L.B. Freidovich), [email protected] (A.S. Shiriaev), [email protected] (U. Mettin). 0957-4158/$ - see front matter Ó 2009 Elsevier Ltd. All rights reserved. doi:10.1016/j.mechatronics.2009.07.005
For all these setups it has been suggested to explicitly use the hyperbolic structure that appears when all degrees of freedom, except one, are kept at target positions. The remaining degree of freedom, with appropriately chosen initial conditions, follows motions defined by the homoclinic curves of a physical pendulum. The integrals of the unforced system or its subsystems can form an objective function for minimization to achieve orbital stabilization [3–5]. However, it is worth to note that the exponential convergence of the objective function (Lyapunov function) does not necessarily imply exponential orbital stability of the homoclinic curve, i.e. it does not ensure that the distance to the homoclinic curve approaches zero exponentially. The target homoclinic orbit for swinging up has also some deficiencies. One of them is the presence of unbounded motions (perpetual one-directional rotations) in an arbitrary small neighborhood of the target orbit. To avoid this, we propose introducing other homoclinic structures into dynamics of the system by feedback action. This is important for the following reasons. It is necessary to restrict the travel of the revolute joints for some experimental setups. This can be done by designing homoclinic curves that are surrounded only by bounded periodic trajectories. It appears that any of these periodic trajectories, in contrast to homoclinic curves, admit efficient design of feedback control laws ensuring exponential orbital stabilization with a guaranteed rate of convergence. This is important for carrying out experimental studies.
1241
P.X. La Hera et al. / Mechatronics 19 (2009) 1240–1250
Below we suggest such an approach for solving the swing-up problem with the Furuta pendulum taken as an example. It is shown how to plan a large family of homoclinic orbits taking into account the constraints of limited travel. Furthermore, it is shown how to stabilize nearby periodic motions with a guaranteed rate of convergence. The paper is organized as follows. In Section 2 we describe the Furuta pendulum model, formulate the objectives of the study, and show some preliminary steps for solving the problem. The main theoretical contribution of this work, a new motion planning technique, is described in Section 3. In Section 4 results of computer simulations are shown, while the results of experiments together with some details of implementation are described in Section 5. Concluding remarks are given in Section 6.
Design a feedback controller to exponentially orbitally stabilize a suitable motion from the family. 2.3. Planning a motion via imposing a geometric relation between / and h Describing all the possible swing-up motions of the Furuta pendulum as a response to some smooth control torques, at first glance, seems to be an intractable task. The next arguments, however, give some insights into searching some of these motions. Suppose that the motion ½/H ðtÞ; hH ðtÞ of (1) and (2) with s/ ¼ s/H ðtÞ is a homoclinic curve of the dynamics defined for all time moments between 1, and such that the following two conditions hold I: limt!þ1 hH ðtÞ ¼ 0 and limt!1 hH ðtÞ ¼ 0 II: limt!þ1 /H ðtÞ ¼ /0H and limt!1 /H ðtÞ ¼ /0H .
2. Preliminaries 2.1. Dynamics of the Furuta pendulum The Furuta pendulum is a mechanical system with two degrees of freedom, see Fig. 1, where / is used to denote the angle of the arm’s rotation in the horizontal plane, and h the angle of the pendulum attached to the end of this arm. The behavior of the pendulum is influenced by the acceleration of the arm, which is actuated by a DC motor. The equations of motion of the Furuta pendulum are [12], 2
€ þ p cos h €h þ 2p sin h cos h h_ /_ ðp1 þ p2 sin hÞ / 3 2 2 _ p sin h h ¼ s/
ð1Þ
€ þ ðp þ p Þ €h p sin h cos h /_ 2 p sin h ¼ 0 p3 cos h / 2 5 2 4
ð2Þ
3
where s/ is the external torque that allows to control the arm rotation. The constants p1 –p5 are positive and defined by physical parameters of the setup, see (A.2) in Appendix A.
Suppose in addition that there exists a C 2 -smooth function UðÞ, such that
/H ðtÞ ¼ UH ðhH ðtÞÞ;
8 t:
ð3Þ
This geometric relation is known as a virtual holonomic constraint [14]. The first and second time derivatives of /H ðtÞ are
d d / ðtÞ ¼ U0H ðhH ðtÞÞ hH ðtÞ; dt H dt 2 2 2 d d d / ðtÞ ¼ U00H ðhH ðtÞÞ hH ðtÞ þ U0H ðhH ðtÞÞ 2 hH ðtÞ: 2 H dt dt dt Substituting these relations and (3) into Eq. (2) of the system dynamics, one obtains the second order differential equation, known as the reduced dynamics ðp2 þ p5 þ p3 cos hH U0H ðhH ÞÞ €hH |fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl} ¼aðhH Þ
2.2. Problem formulation
þ ðp3 cos hH U00H ðhH Þ p2 sin hH cos hH ½U0 ðhH Þ2 Þ h_ 2H þ p4 sinðhH Þ ¼ 0 |fflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflffl} |fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl}
Our objectives are the following: Plan a family of feasible motions of the Furuta pendulum that bring the pendulum from a vicinity of the downward equilibrium to a sufficiently small neighborhood of the upright equilibrium satisfying restrictions on admissible travel of the links.
¼bðhH Þ
ð4Þ
¼cðhH Þ
The homoclinic curve hH ðtÞ, described earlier, is a solution of this equation. Suppose that neither the function UH ðhH Þ nor the particular solution – homoclinic curve hH ðtÞ – are known. The contribution of this paper is to show that the existence of homoclinic curves for dynamics of (4) can be deduced from a few properties of
Fig. 1. The Furuta pendulum built at the Department of Applied Physics and Electronics, UmeaUniversity.
1242
P.X. La Hera et al. / Mechatronics 19 (2009) 1240–1250
Fig. 2. Two homoclinic orbits of the equilibrium at h ¼ 0 are shown on the phase portrait of (4). One of them intersects the line fh_ ¼ 0g at a1 , the other at a2 . In (b) an exemplified periodic trajectory orbiting two homoclinic curves is depicted.
the coefficients aðhH Þ; bðhH Þ, and cðhH Þ. As a result, shaping new homoclinic structures1 for planning swing-up trajectories for the Furuta pendulum becomes feasible.
n R o s p cos sU00 ðsÞp sin s cos s½U0 ðsÞ2 3 2 2 ds p2 þp5 þp3 cos sU0 ðsÞ 0 e
0
hH ð0Þ ¼ a1 ;
3.1. Planning new homoclinic curves for swinging up Motivated by the observations above, let us start the search for swing-up motions considering the relation
8t
ð5Þ
which is supposed to be satisfied along this motion, and where the scalar function UðÞ is unknown. If (5) is imposed on the dynamics, it immediately implies the next relations between angular velocities and accelerations
d d / ¼ U0 ðhÞ h; dt dt
ai
p4 sin s ds p2 þ p5 þ p3 cos s U0 ðsÞ
are equal to zeros. Furthermore, these homoclinic curves can be found as the solutions of (4) with initial conditions
3. Main results
/ðtÞ ¼ UðhðtÞÞ;
Z
2
d
dt
/ ¼ U00 ðhÞ 2
d h dt
2
2
þ U0 ðhÞ
d
dt
2
h:
By substituting these relations into (2), we obtain the second order differential equation (4). Important facts on qualitative behavior of its solutions are given in the next statements. 2
h_ H ð0Þ ¼ 0 and hH ð0Þ ¼ a2 ;
h_ H ð0Þ ¼ 0:
ð8Þ
Statement (A) is obvious. Statement (B) is proved in [15]. To validate (C) we observe that the system (4) is integrable [13] and that _ the integral function IðhðtÞ; hðtÞ; h0 ; h_ 0 Þ, defined by Z I ¼ h_ 2 ðtÞ exp 2
hðtÞ
h0
bðsÞ ds aðsÞ
Z h_ 2
hðtÞ
0
Z exp 2
h0
s h0
bðsÞ 2cðsÞ ds ds ; aðsÞ aðsÞ ð9Þ
remains zero on the solution hðtÞ of (4) initiated at ½h0 ; h_ 0 . If we assume that a homoclinic curve hH ðtÞ of the equilibrium heq ¼ 0 of (4) does exist, then: In the expression (9) we can let h0 and h_ 0 converge along the homoclinic curve (in negative time) to the equilibrium, i.e.
h0 ,0;
h_ 0 ,0
Lemma 1. Let UðhÞ be a C -smooth function defined on IU ¼ ð2p e; 2p þ eÞ with e > 0, such that the function aðhÞ ¼ p2 þ p5 þ p3 cos h U0 ðhÞ is separated from zero2 on IU . Then, the following statements hold.
There is a point along a homoclinic curve at which h_ H should change sign:
(A) The equilibria of the dynamical system (4), i.e. the solutions of the equation cðheq Þ ¼ 0, are
Therefore, evaluating (9) at these points of the homoclinic curve, from I ¼ 0 we obtain the equality
heq ¼ k p;
k 2 Z ¼ f. . . ; 2; 1; 0; 1; 2; . . .g
ð6Þ
(B) Consider the equilibrium h ¼ kp with k 2 Z of the dynamical system (4). If the constant
xk :¼
p4 cos h p2 þ p5 þ p3 cos h U0ðhÞh¼kp
9 t :
hH ðt Þ ¼ a;
h_ H ðt Þ ¼ 0
0 ¼ h_ 2H ðt Þ n R o" Z hH ðt Þ bðsÞ 2 ds a ð s Þ hH ð1Þ h_ 2H ð1Þ e
n R o # s bðsÞ 2 ds 2cðsÞ a ð s Þ hH ð1Þ e ds aðsÞ hH ð1Þ hH ðt Þ
ð7Þ
is positive, then this equilibrium is a center. If, in opposite, xk is negative, then this equilibrium is a saddle. (C) Suppose x1 > 0; x0 < 0; x1 > 0, then the dynamical system (4) has two homoclinic curves of the ‘upright’ equilibrium3 heq ¼ 0, see Fig. 2a, if and only if for some a1 ; a2 , with a1 2 ð2p; pÞ and a2 2 ðp; 2pÞ, the integrals
1 Note that the case of homoclinic curves of a physical pendulum, discussed in the introduction, fits naturally into these settings with UH ðhH Þ being equal to a constant. 2 In other words, it is either positive or negative for all h 2 IU ¼ ½2p e; 2p þ e. 3 Note that h ¼ 0 corresponds to the upright equilibrium for the Furuta pendulum.
that reduces to
0¼
Z 0
a
Z exp 2 0
s
bðsÞ 2cðsÞ ds ds: aðsÞ aðsÞ
Substituting the expressions for aðhÞ; bðhÞ; cðhÞ as in (4), we arrive to the formula in the statement. To prove, in opposite, that the ‘upright’ equilibrium of (4) has two homoclinic curves, observe that the condition x0 < 0 implies that it is hyperbolic; thus, it has one-dimensional stable and unstable manifolds. In turn, the property (C) ensures that both branches of unstable manifolds intersect the line with h_ ¼ 0 at points a1 and a2 . To see that these points indeed belong to homoclinic curves of the equilibrium heq ¼ 0, it is enough to notice that the phase portrait of the dynamical system (4) has the mirror symmetry with
1243
P.X. La Hera et al. / Mechatronics 19 (2009) 1240–1250
respect to the line fh_ ¼ 0g. Hence, the solutions initiated in (8) converge to the ‘upright’ equilibrium in positive time (along its stable manifold) as they do in negative time (along its unstable manifold). h Swinging up the Furuta pendulum based on stabilization of homoclinic curves, defined in Lemma 1, might be challenging, as motivated in the introduction. Indeed, in order to compensate for parametric uncertainties and effects of unmodelled dynamics the swing-up motion should be complemented with a feedback controller. This controller should stabilize the dynamics of the system transverse to the orbit of such a motion in the four dimensional state-space of the Furuta pendulum. Presumably such a controller, if constructed, would be non-robust and of limited use for experimental studies. The conclusion comes from observing that the linearization of the dynamics transverse to each of the homoclinic curves is a linear control system with time varying coefficients, for which designing a stabilizing controller, estimating a rate of convergence, and verifying robustness (even for a known controller) are difficult tasks. To overcome the problem, we suggest to stabilize orbitally a nearby periodic solution of (4) rather than the homoclinic orbit itself. This periodic solution should be surrounding the homoclinic curves close enough, see Fig. 2b. Exponential orbital stabilization of such a periodic solution, if it exists, can be achieved based on the recently proposed algorithms of [14]. In the next statement, further mild sufficient conditions on the constraint function UðhÞ of (5) are given to ensure the presence of a strip of periodic solutions surrounding two homoclinic curves of the ‘upright’ equilibrium on the phase portrait. Lemma 2. Suppose that the conditions of Lemma 1 are valid. If the constants xð2Þ and x2 defined by (7) are both negative, then all solutions hðtÞ of the dynamical system (4), with initial conditions at _ hð0Þ ¼ 0 and
3.2. Choosing the constraint function To plan a swing-up motion for the Furuta pendulum using the Lemmas above, we need to find a function UðhÞ of (5) satisfying several properties. To show that such functions do exist, and that the conditions of Lemmas 1 and 2 are mild and easy to meet, let us present one of them as an example; namely, let
UðhÞ ¼ K arctanðhÞ:
Here, the value of the constant K limits the interval of possible target travel for the /-variable along a swing-up motion within 1 2 K p; 12 K p . With the choice (10) the corresponding coefficients of (4) are
aðhÞ ¼ p2 þ p5 þ
Kp3 cosðhÞ ð1 þ h2 Þ
Proof. Proof is based on the fact that the phase portrait of (4) is symmetric with respect to fh_ ¼ 0g axis. Two homoclinic curves of the upright equilibrium, see Fig. 2, cannot be attractive or repellent for nearby solutions. This solutions should be bounded due to continuous dependence of solutions on initial conditions, see e.g. [7]. Hence, nearby solutions, i.e. the ones with initial conditions _ at hð0Þ ¼ 0 and
p < hð0Þ < p þ d or p d < hð0Þ < p are all periodic for small d > 0. Repeating the same arguments for nearby solutions of established periodic trajectories, we observe that they are valid everywhere between the hyperbolic equilibria at 2p. h
bðhÞ ¼
;
K cos h ð1 þ h2 Þ2
ð2p3 h þ Kp2 sin hÞ: ð11Þ
Let us rewrite all the conditions of Lemma 1 and 2 as constraints on the constant parameter K, with the parameters of the model given in (A.2): (C1) The function aðhÞ should be separated from zero on
½2p e; 2p þ e with some e > 0: The value a p2 ¼ p2 þ p5 is positive, therefore the condition holds if aðhÞ > 0 for all h 2 ½2p; 2p. The minimal value of aðÞ on this interval can be found as
n o min faðhÞg ¼ min að2pÞ; að2pÞ; aðhfig cr Þ ;
h2½2p;2p
where hfig cr are the critical values, i.e. solutions of the equation 0
0 ¼ a ðhcr Þ ¼
p < hð0Þ < 2p or 2p < hð0Þ < p are periodic.
ð10Þ
K p3 1 þ h2cr
"
# 2 hcr cosðhcr Þ ; sinðhcr Þ þ 1 þ h2cr
which can be numerically found and that are apparently independent of K. Finally, the condition of positiveness of aðhÞ within ½2p; 2p can be rewritten as the inequality
K P 38:51
p2 þ p5 ; p3
ð12Þ
where the number is related to a solution of the transcendental equation for hcr . (C2) The values of the five constants xk with k ¼ 2; 1; 0; 1; 2, defined by
p4 cosðhÞ xk ¼ K p2 þ p5 þ p3 cosðhÞ 1þh2
; h¼kp
see (7), should have the following signs
Fig. 3. (a) The phase portrait for (4) when condition (C) of Lemma 1 is satisfied. (b) The phase portrait for (4) when condition (C) of Lemma 1 is not valid.
1244
P.X. La Hera et al. / Mechatronics 19 (2009) 1240–1250
separatrix) of the hyperbolic equilibrium heq ¼ 0 are bounded (or not), see Fig. 3. A simple sufficient condition for this property can be given in terms of the values
60 40
Fða; KÞ at a ¼ 2p; p; p; 2p
20
F(π) and F(2π)
If for some K both inequalities 0
Fð2p; KÞ Fðp; KÞ < 0;
−20
Fð2p; KÞ Fðp; KÞ < 0
hold, then the condition holds as well. The values of Fðp; KÞ 3 are shown in Fig. 4. and Fð2p; KÞ as functions of K n ¼ K p2pþp 5
−40
As seen, when K n < 0:01 both functions Fðp; KÞ and Fð2p; KÞ are negative, while for most of positive values of K n on the plot they have different signs. Nevertheless, both functions are difficult to compute accurately for K n P 6:6. The behavior of Fðp; KÞ and Fð2p; KÞ is the same. To conclude, condition (C) of Lemma 1 is ensured if
−60 −80 −100
0
1
2
3 Kn
4
5
6
3 Fig. 4. The values of Fðp; KÞ and Fð2p; KÞ vs. K n ¼ K p2pþp , the upper (blue) and the 5 lower (red), respectively. As seen, they have different signs for most of positive values of K n , while around zero, K n < 0:01, they are negative. (For interpretation of the references to colour in this figure legend, the reader is referred to the web version of this article.)
xð2Þ < 0; xð1Þ > 0; x0 < 0; x1 > 0; x2 < 0: These conditions are equivalent to the following three independent inequalities for the parameter K
K > ð1 þ 4p2 Þ
p2 þ p5 p þ p5 p þ p5 ; K > 2 ; K < ð1 þ p2 Þ 2 : p3 p3 p3 ð13Þ
(C3) The value of Fðai ; KÞ given by
Z 0
ai
nR e
s 2Kðsp3 cos sþp2 K sin s cos sÞ d 0 ð1þs2 Þððp2 þp5 Þð1þs2 Þþp3 K cos sÞ
s
o
p4 ð1 þ s2 Þ sin s ds ðp2 þ p5 Þð1 þ s2 Þ þ p3 K cos s
should be zeros for some a2 2 ðp; 2pÞ and a1 2 ð2p; pÞ. The fact that this condition is valid (or not) has intuitive geometrical interpretation: the stable and unstable manifolds (the
0:01
p2 þ p5 p þ p5 < K < 6:6 2 ; p3 p3
ð14Þ
where the upper bound can be enlarged via a more accurate computation. 4. Results of computer simulations Here some results of numerical simulations are given for the model (1) and (2) with parameters defined by (A.2) in the appendix. 4.1. Choosing the target trajectory for swinging up Motion planning for swinging up is carried out with the constrained function (10), where K ¼ 1=p has been chosen. Such value of K satisfies all the conditions of Lemma 1 and 2 described by (12)–(14). The phase portrait of the corresponding dynamical system (4) is shown in Fig. 5, where as predicted the upright equilibrium heq ¼ 0 has homoclinic curves surrounded by a strip of periodic solutions. In the following simulation results, the trajectory depicted with the bold line in Fig. 5a and b is chosen as an example of a swing-up motion and is to be orbitally stabilized.
Fig. 5. (a) Phase portrait of the solutions for the reduced dynamics (4) with the virtual constraint (10). The trajectory marked with bold line in (a) and (b) represents the periodic solution chosen for swinging up. This solution has a period T 4:0454 s.
P.X. La Hera et al. / Mechatronics 19 (2009) 1240–1250
1245
_ Fig. 6. Results of numerical simulations for the swing-up stage for the closed-loop system with initial conditions defined in (15): (a) the pendulum angle hðtÞ vs. its velocity hðtÞ _ for the target orbit; (b) the pendulum angle hðtÞ vs. its velocity hðtÞ; (c) the arm angle / vs. the pendulum angle h and the target arctan-relation (10); (d) the pendulum angle h vs. time.
4.2. Simulation with a stabilizing controller for the periodic orbit
4.3. Simulation of combining a switching law and stabilizing controllers
A stabilizing state feedback controller4 for this cycle has been designed following the steps presented in [13, Appendix C, p. 1174]. The performance of the closed-loop system is illustrated in Fig. 6a–d, where we show the target orbit and the trajectory of the closed-loop system initiated at
/ ¼ 0;
/_ ¼ 0;
h ¼ p;
h_ ¼ 0;
ð15Þ
which is the downward open-loop stable equilibrium. The simulated motion of the closed-loop system is in agreement with the theoretical results, i.e. the solution converges to the target cycle with the relation between the angles (see Fig. 6) being an arctan function. 4 The controller design is not discussed here. It is beyond the topic of this article; a detailed description of its derivation can be found in [13,15,14] while a specification to the Furuta pendulum case can be found in [12].
A switching law and a local controller are needed around the upright equilibrium in order to keep the pendulum balanced. With initial conditions around the downward resting equilibrium, the upright position is approached first when h ¼ 0 and 1:34 < h_ < 0, see Fig. 7. In our case, the opposite phase is chosen. This means that _ < 0:3, h_ and h are the switching is to be performed when j/j < 0:1, j/j within the intervals (0,1.34) and (0.24,0.24), respectively. In Fig. 8 we show results of a simulation of the swing-up and the stabilizing controller with the switching proposed. As it is seen, the swing-up strategy brings the pendulum close to h ¼ 0 and then the locally stabilizing controller takes over. It is known that a successful switching depends very much on the region of attraction,5 of the local balancing controller. 5 Note that we have chosen a linear stabilizing controller only for simplicity. The region of attraction can be enlarged with other designs, see e.g. [1,10].
1246
P.X. La Hera et al. / Mechatronics 19 (2009) 1240–1250
15
loaded contains a control function, which is executed according to a sampling period.
10
5.1. Sampling Switching Region
Dtheta [rad/s]
5
So far it has been assumed that disturbances are negligible and that the input signal is realizable and the states are available at every instant of time. However, the dSPACE board used, as well as any other digital hardware, works with an internal processor that runs in discrete time. For this reason, the feedback control law must be sampled, meaning that we have to apply for t : k h < t 6 ðk þ 1Þ h the control signal to the motor
0
−5
def
ur ðtÞ ¼ u½k ¼ Sat0:5 ðuðk hÞÞ;
−10
−15 −6
−4
−2
0 theta [rad]
2
4
ð16Þ
where k is a natural number, the small constant h > 0 is the sampling time (h ¼ 0:0008 s in our case), and uðk hÞ is related to the input torque s/ (see Appendix A) from (1) at t ¼ k h, and SatM ðÞ is the standard saturation function with the cut-off level M ¼ 0:5, which is due to the limitation of the available control torque.
6
_ Fig. 7. The switching region specified as a range of h and h.
5.2. Velocity estimation 5. Sampling, real-time implementation and experimental results
"
x^_ 1 ^x_
4
5
2
0
0
−5
−2
−10
−4
0 θ
2
4
6
þ
ð17Þ
^_ h ¼ SatM2 ðv^ 2 Þ;
ð18Þ
_ h; _ uÞ ¼ 0 f0 ð/; h; /;
10
−2
v^ 1 v^ 2
where either
6
−4
¼
^ /_ ¼ SatM1 ðv^ 1 Þ;
15
−15 −6
#
2ðy1 ^x1 Þ=e1 ; 2ðy2 ^x2 Þ=e2 2 " # " # ðy1 ^x1 Þ=e21 v^_ 1 ¼ f ð^x ; ^x ; /; ^_ ^_ ; h; uÞ þ 0 1 2 ðy2 ^x2 Þ=e22 v^_ 2
θ
dθ/dt
Let us discuss issues related to the controller’s real-time implementation. The Furuta pendulum (see Fig. 1) built in our lab is equipped as follows. Rotation of the arm is controlled by a DC motor, while the pendulum moves freely in the plane, orthogonal to the arm. The DC motor is a brush-commutated Penta M1, equipped with a servo amplifier SCA-SS-70-10. The angular positions are measured with optical incremental encoders: Eltra EH53A (1024 imp/rev) for the arms angle and Wachendorff WDG 40S-2500ABN-H05-SB5-AAC (2500 imp/rev) for the pendulum’s angle. The setup is connected to a dSPACE board 1104 used to run the real-time application. The user interface consists of a PC equipped with ControlDesk (Ó dSPACE GmbH), used for on-line communication with the dSPACE board. Matlab/Simulink (Ó Mathworks) is the software used for the implementation of algorithms as well as simulations. Simulink enables the generation and deployment of production C code for use in real-time embedded systems. The compiled code is loaded into the dSPACE’s processor. The program
The angles h and / are the states measured by the encoders. The angular velocities, which are needed for the feedback control law implementation, are not measured but can be estimated by applying a high gain observer. A high gain observer (HGO) is a fast nonlinear (or linear) full order observer with high observer gains chosen via pole placement, followed by a saturation. Saturation is needed in order to protect the plant from peaking [7, Section 14.5]. We apply
−6
0
ð19Þ
5
10
15
time
Fig. 8. Results of numerical simulations for the closed-loop system with the proposed switching strategy and initial conditions defined in (15): (a) the pendulum angle hðtÞ vs. _ its velocity hðtÞ; (b) the pendulum angle hðtÞ vs. time.
P.X. La Hera et al. / Mechatronics 19 (2009) 1240–1250
1247
Fig. 9. Results of experimental validation for the swing-up motion for the closed-loop system with initial conditions defined in (15): (a) the pendulum angle hðtÞ vs. its velocity _ _ hðtÞ for the target orbit; (b) the pendulum angle hðtÞ vs. its velocity hðtÞ observed in an experiment; (c) the arm angle / vs. the pendulum angle h observed in an experiment and the target arctan-relation (10); (d) the pendulum angle hðtÞ vs. time.
_ h; _ uÞ is a nominal model defined by (1) and (2), the small or f0 ð/; h; /; parameters e1;2 > 0 are to be tuned,
5.3. Experimental results for swinging up
Experimental results with a feedback controller designed as in [12] are presented in Fig. 9a–d for the initial conditions (15). It can be seen that the proposed swing-up method works in practice. In fact, the desired cycle is approximately achieved. Irrespective of variation in initial conditions, experiments show reproducibility and stability. The mismatch between experimental and desired motions, see Fig. 9c, is thought to be produced by the following factors: the friction torque at the pendulum shaft is not compensated; discretization of the measurements does not allow us to use sufficiently fast differentiators and leads to introduction of additional delays imposed by our observers; and there are presumably input and output delays present in the hardware. However, it is experimentally verified that the steady-state in the experiment is not destroyed by those factors. This validates robustness of our design.
y1
y2
¼
/ðtÞ hðtÞ
þ wðtÞ with wðtÞ ¼
/½k /ðtÞ
h½k hðtÞ
is the measured output with the ‘measurement noise’ defined by sampling and encoders’ quantization6:
h½k ¼
2 p 2500 hðkhÞ ; 2500 2 p
/½k ¼
2p 1024 /ðkhÞ ; 1024 2p
8t : k h < t 6 ðk þ 1Þ h; and ‘‘bc” denotes the floor function. The sampling period is set to be h ¼ 0:0008 s. and the observer parameters are taken as e1 ¼ 20 h and e2 ¼ 10 h, while M 1 ¼ M2 ¼ 20 rad=s, which corresponds to a reasonable level of upper bounds on the speeds. 6
Actually, there is also some smoothing done by the Simulink interface.
1248
P.X. La Hera et al. / Mechatronics 19 (2009) 1240–1250
6
10
4
5
2 θ [rad]
dθ/dt [rad/sec]
Experimental Phase Portrait 15
0
0
−5
−2
−10
−4
−15 −6
−4
−2
0 θ [red]
2
4
θ trajectory
−6 10
6
15
20
25 time [sec]
30
35
40
_ Fig. 10. Results of numerical simulations for the closed-loop system with the proposed switching strategy: (a) the pendulum angle hðtÞ vs. its velocity hðtÞ; (b) the pendulum angle hðtÞ vs. time.
Torque vs Input Signal
−3
4
0.7
x 10
Mapped friction
3
0.6
Torque = 0.51*u+0.002
2 1
0.4 u
Torque[Nm]
0.5
0
0.3 −1 0.2
−2
0.1
0 0
−3
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
−4 −0.1 −0.08 −0.06 −0.04 −0.02
Input Signal [Volt]
0
0.02
0.04
0.06
0.08
0.1
velocity
Fig. A.1. Results of identification experiments.
5.4. Experimental results for combining switching and stabilizing controllers Let us show now the results of switching controller described earlier. As above, switching is done whenever j/j < 0:1 ½rad; _ < 0:3 ½rad=s; 0 < h_ < 1:34 ½rad=s, and jhj < 0:24 ½rad. The local j/j controller is LQR designed for the linearization of the system dynamics (1) and (2). In Fig. 10a and b we show results of combining swinging up, switching, and local stabilizing controller. In experiments7 with different initial conditions, the state h converges to the desired steady state as expected. It is worth mentioning that once the switching is done, and the pendulum is stabilized, the friction compensation is deactivated. This is done to avoid the chat-
7 Movies with recorded experiments are available at http://www.control.umu.se/ Set_Ups/Furuta_Pendulum/Furuta_Pendulum_info.html.
tering of the input signal due to numerical errors unavoidable when the velocity is close to zero.
6. Conclusions We have suggested a new systematic approach for planning trajectories suitable for swinging up the Furuta pendulum. Our arguments are based on an accurate planning of a periodic trajectory enclosing two homoclinic curves, and results from analysis of a reduced dynamical system. This reduced dynamics describes the motions of the non-actuated link. For the resulting motion, the arm and pendulum angles are regulated by some control input to satisfy a certain geometric relation. This geometric relation, known as virtual holonomic constraint, ensures certain restrictions on the target motion of the pendulum during the swinging up. This is necessary in order to deal with hardware constraints imposed
P.X. La Hera et al. / Mechatronics 19 (2009) 1240–1250
by the maximum allowable motion of the rotational link. The preplanned cycle has been robustly stabilized by the control method introduced in [13], which ensures local exponential orbital stability with a specified rate of convergence. The overall performance is illustrated by numerical simulations and experimental studies. Acknowledgement We would like to acknowledge the work of Anders Sandberg and Sven Elmå, for the design and building of the Furuta pendulum at the Department of Applied Physics and Electronics, UmeaUniversity. Our work has been partially supported by the Kempe Foundation and the Swedish Research Council under Grants 2005-4182 and 2008-5243. Appendix A. Parameters of the model Using basic principles, the parameters of the dynamic model of the Furuta pendulum (1) and (2) can be computed as follows:
1 2 2 p1 ¼ J þ ðM þ mp Þla ½kg m2 ; p2 ¼ M þ mp lp ½kg m2 ; 4 1 1 p3 ¼ M þ mp lp la ½kg m2 ; p4 ¼ M þ mp lp g ½Nm; 2 2 1 2 2 p5 ¼ mp lp ½kg m ; 12 ðA:1Þ where the values of the physical parameters for our experimental setup, see Fig. 1, are: the length of the arm la ¼ 0:15 ½m, the mass of the arm ma ¼ 0:298 ½kg, the length of the pendulum lp ¼ 0:26 ½m, the mass of the pendulum mp ¼ 3:2 102 ½kg, and the mass of the bob at the end of the pendulum M ¼ 7:5 103 ½kg. An estimate of the arm’s inertia according to CAD drawings is J a ¼ 7:682 104 ½kg m2 . For these values the coefficients p1 –p5 of (1) and (2) are
p1 ¼ 1:8777 103 ;
p2 ¼ 1:3122 103 ;
p4 ¼ 5:9301 102 ;
p5 ¼ 1:77 104
p3 ¼ 9:0675 104 ; ðA:2Þ
In order to apply the control law, described in [13,12], the values of the parameters above must be verified and the control signal should appropriately shape the torque s/ , applied to the arm. This external torque consists of various components and can be modeled as
s/ ¼ K DC u þ F /fr F /fr
with u being the control input and being the friction torque. The electromotive torque constant K DC is estimated using a dynamometer attached at the end of the arm along the horizontal plane. By applying input voltage of different levels the corresponding force can be measured, producing a linear relation (see Fig. A.1a). The torque is calculated by multiplying this force with the distance from the motor axis to the end of the arm. The gain K DC is the slope of this linear relation, which is estimated to be K DC ¼ 0:51 ½Nm=V. The friction torque F /fr can be modeled as a nonlinear static function of velocity. Its approximation can be used for partial compensation of the friction present in the arm. The compensation scheme adopted here takes into account Coulomb and viscous friction and uses estimates of the arm velocity done applying (18) and (19). In Fig. A.1b the static friction map identified for our setup using a standard procedure is shown. The compensation law is defined as
( ufr ¼
_ 0:032 0:002 /;
if _ if 0:033 0:002 /;
/_ > 0 /_ < 0
1249
The parameters p1 –p5 from (1) and (2) need to be verified via identification, because approximations given by (A.1) might not be in accordance with the real setup. It is done in two steps: identification of the arm parameters when the pendulum is detached and identification of the pendulum parameters when the arm is fixed. Due to the complex shape of the arm (see Fig. 1), the approximated value p1 related with its inertia needs to be verified. For this purpose, the pendulum was detached from the setup. With such a modification the equation of motion (1) of the arm becomes
€ ¼ K DC u þ F / : p1 / fr The value for the parameter p1 can be calculated from this equation assuming that the Coulomb part of the F /fr is perfectly compensated by some control input u ¼ uC . This assumption leads to the _ Then, the transfer € ¼ K DC ðu uC Þ K DC bh /. arm dynamics: p1 / function from the input voltage u uC to the arm velocity /_ is K DC =p1 : If we apply a step input of some magnitude sþK DC bh =p1 u uC ¼ u0 , the gain K DC =p1 can be calculated from the step response dividing the magnitude of the steady-state velocity /_ ¼ /_ 0 by the input level. The identified value is K DC ¼ 0:0034. p1 ¼ /_ =ðuu 0 CÞ Let us proceed with verification of the pendulum parameters. The experiment considers only the movement of the pendulum with the arm fixed. No input is given, meaning that / and its derivatives are zeros. Free oscillations of the pendulum are recorded. For this experiment, the model (1) and (2) is reduced to:
ðp2 þ p5 Þ €h p4 sinðhÞ ¼ sh ; where the values of p2 ; p5 ; p4 are to be identified and the torque sh mainly consists of friction. The pendulum is manually positioned at some initial condition hð0Þ where it is left free, producing oscillations around the downward position p. The torque sh can be modeled as viscous friction with a damping parameter bh , so that we obtain: h p4 sinðhÞ þ bh h_ ¼ 0. The parameters are fit to the data ðp2 þ p5 Þ € using the nonlinear least square method. This estimation results in the values p2 þ p5 ¼ 0:0014 ½kg m2 and p4 ¼ 0:0617 ½Nm, from which p2 ¼ 0:0013 ½kg m2 . These parameters are clearly close to the calculated ones in (A.2). Appendix B. Supplementary material Supplementary data associated with this article can be found, in the online version, at doi:10.1016/j.mechatronics.2009.07.005. References [1] Ohsumi A, Izumikawa T. Nonlinear control of swing-up and stabilization of an inverted pendulum. In: Proc 34th Conf Decision and Control; 1995. p. 3873–80. [2] Fantoni I, Lozano R, Spong MW. Energy based control of the Pendubot. IEEE Trans Autom Control 2000;45(4):725–9. [3] Fantoni I, Lozano R. Stabilization of the Furuta pendulum around its homoclinic orbit. Int J Control 2002;75(6):390–8. [4] Fradkov AL. Swinging control of nonlinear oscillations. Int J Control 1996;64:1189–202. [5] Gordillo F, Acosta JA, Aracil J. A new swing-up law for the Furuta pendulum. Int J Control 2003;76(8):836–44. [6] Furuta K, Yamakita M, Kobayashi S. Swing up control of inverted pendulum. Proc Ind Elect Control Instrum 1991:2193–8. [7] Khalil HK. Nonlinear systems. third ed. Prentice-Hall; 2002. [8] Kolesnichenko O, Shiriaev A. Partial stabilization of underactuated Euler– Lagrange systems via a class of feedback transformations. Syst Control Lett 2002;45(2):121–32. [9] Mahindrakar A, Banavar R. A swing-up of the Acrobot based on a simple pendulum strategy. Int J Control 2005;78(6):424–9. [10] Nair S, Leonard NE. A normal form for energy shaping: application to the Furuta pendulum. In: Proc 41st Conf Decision andControl; 2002. p. 516–21.
1250
P.X. La Hera et al. / Mechatronics 19 (2009) 1240–1250
[11] Wei Q, Dayawansa WP, Levine WS. Nonlinear controller for an inverted pendulum having restricted travel. Automatica 1995;31(6):841–50. [12] Shiriaev AS, Freidovich LB, Robertsson A, Johansson R, Sandberg A. Virtualholonomic-constraints-based design of stable oscillations of Furuta pendulum: theory and experiments. IEEE Trans Robot 2007;23(4):827–32. [13] Shiriaev AS, Perram JW, Canudas-de-Wit C. Constructive tool for orbital stabilization of underactuated nonlinear systems: virtual constraints approach. IEEE Trans Autom Control 2005;50(8):1164–76. [14] Shiriaev AS, Freidovich LB, Manchester IR. Can we make a robot ballerina perform a pirouette? Orbital stabilization of underactuated mechanical systems. Ann Rev Control 2008;32(2):5–8.
[15] Shiriaev AS, Robertsson A, Perram J, Sandberg A. Periodic motion planning for virtually constrained Euler–Lagrange systems. Syst Control Lett 2006;55(11):900–7. [16] Shiriaev AS, Pogromsky A, Ludvigsen H, Egeland O. On global properties of passivity based control of an inverted pendulum. Int J Robust Nonlinear Control 2000;10(4):283–300. [17] Spong MW, Corke P, Lozano R. Nonlinear control of the reaction wheel pendulum. Automatica 2001;37(11):1845–51. [18] Zhang M, Tarn TJ. Hybrid control of the Pendubot. IEEE/ASME Trans Mechatron 2002;7(1):79–86.
Contents lists available at ScienceDirect
Mechatronics journal homepage: www.elsevier.com/locate/mechatronics
New approach for swinging up the Furuta pendulum: Theory and experiments Pedro X. La Hera a,*, Leonid B. Freidovich a, Anton S. Shiriaev a,b, Uwe Mettin a a b
Department of Applied Physics and Electronics, UmeaUniversity, SE-901 87 Umea, Sweden Department of Engineering Cybernetics, Norwegian University of Science and Technology, NO-7491 Trondheim, Norway
a r t i c l e
i n f o
Article history: Received 1 February 2008 Accepted 21 July 2009
PACS: 45.81.+r 45.50.Dd Keywords: Furuta pendulum Virtual holonomic constraints Motion planning Orbital stabilization of periodic trajectories Implementation
a b s t r a c t The problem of swinging up inverted pendulums has often been solved by stabilizing a particular class of homoclinic structures present in the dynamics of a physical pendulum. Here, new arguments are suggested to show how other homoclinic curves can be preplanned for dynamics of the passive-link of the robot. This is done by reparameterizing the motions according to geometrical relations among the generalized coordinates, which are known as virtual holonomic constraints. After that, conditions that guarantee the existence of periodic solutions surrounding the planned homoclinic orbits are derived. The corresponding trajectories, in contrast to homoclinic curves, admit efficient design of feedback control laws ensuring exponential orbital stabilization. The method is illustrated by simulations and supported by experimental studies on the Furuta pendulum. The implementation issues are discussed in detail. Ó 2009 Elsevier Ltd. All rights reserved.
1. Introduction For decades underactuated inverted pendulums have been used as benchmark set-ups for testing nonlinear control design methods. The original problem consists on stabilizing the pendulum around the upright equilibrium. To this end, local stabilizing controllers can be designed for the linearization of the system dynamics around the unstable equilibrium. This ensures asymptotic stability of the upright equilibrium, but often with a small domain of attraction. Thus, a swinging up strategy has to be introduced to reach its vicinity from a far located initial point, typically representing the system at rest. For this purpose a nontrivial motion planning technique is required, since finding feasible motions for underactuated system that are not feedback linearizable is a challenging task. A feedback controller that makes the planed trajectory orbitally stable must be designed as well. Various arguments have been suggested, and experimentally validated, for a number of popular setups belonging to this family. As examples we have the Furuta pendulum [3,5], the Pendubot [2,18,8], the pendulum on a cart [6,11,16], the Acrobot [9], and the inertia wheel pendulum [17].
* Corresponding author. Tel.: +46 090786 97 22; fax: +46 090786 64 69. E-mail addresses: [email protected], [email protected] (P.X. La Hera), [email protected] (L.B. Freidovich), [email protected] (A.S. Shiriaev), [email protected] (U. Mettin). 0957-4158/$ - see front matter Ó 2009 Elsevier Ltd. All rights reserved. doi:10.1016/j.mechatronics.2009.07.005
For all these setups it has been suggested to explicitly use the hyperbolic structure that appears when all degrees of freedom, except one, are kept at target positions. The remaining degree of freedom, with appropriately chosen initial conditions, follows motions defined by the homoclinic curves of a physical pendulum. The integrals of the unforced system or its subsystems can form an objective function for minimization to achieve orbital stabilization [3–5]. However, it is worth to note that the exponential convergence of the objective function (Lyapunov function) does not necessarily imply exponential orbital stability of the homoclinic curve, i.e. it does not ensure that the distance to the homoclinic curve approaches zero exponentially. The target homoclinic orbit for swinging up has also some deficiencies. One of them is the presence of unbounded motions (perpetual one-directional rotations) in an arbitrary small neighborhood of the target orbit. To avoid this, we propose introducing other homoclinic structures into dynamics of the system by feedback action. This is important for the following reasons. It is necessary to restrict the travel of the revolute joints for some experimental setups. This can be done by designing homoclinic curves that are surrounded only by bounded periodic trajectories. It appears that any of these periodic trajectories, in contrast to homoclinic curves, admit efficient design of feedback control laws ensuring exponential orbital stabilization with a guaranteed rate of convergence. This is important for carrying out experimental studies.
1241
P.X. La Hera et al. / Mechatronics 19 (2009) 1240–1250
Below we suggest such an approach for solving the swing-up problem with the Furuta pendulum taken as an example. It is shown how to plan a large family of homoclinic orbits taking into account the constraints of limited travel. Furthermore, it is shown how to stabilize nearby periodic motions with a guaranteed rate of convergence. The paper is organized as follows. In Section 2 we describe the Furuta pendulum model, formulate the objectives of the study, and show some preliminary steps for solving the problem. The main theoretical contribution of this work, a new motion planning technique, is described in Section 3. In Section 4 results of computer simulations are shown, while the results of experiments together with some details of implementation are described in Section 5. Concluding remarks are given in Section 6.
Design a feedback controller to exponentially orbitally stabilize a suitable motion from the family. 2.3. Planning a motion via imposing a geometric relation between / and h Describing all the possible swing-up motions of the Furuta pendulum as a response to some smooth control torques, at first glance, seems to be an intractable task. The next arguments, however, give some insights into searching some of these motions. Suppose that the motion ½/H ðtÞ; hH ðtÞ of (1) and (2) with s/ ¼ s/H ðtÞ is a homoclinic curve of the dynamics defined for all time moments between 1, and such that the following two conditions hold I: limt!þ1 hH ðtÞ ¼ 0 and limt!1 hH ðtÞ ¼ 0 II: limt!þ1 /H ðtÞ ¼ /0H and limt!1 /H ðtÞ ¼ /0H .
2. Preliminaries 2.1. Dynamics of the Furuta pendulum The Furuta pendulum is a mechanical system with two degrees of freedom, see Fig. 1, where / is used to denote the angle of the arm’s rotation in the horizontal plane, and h the angle of the pendulum attached to the end of this arm. The behavior of the pendulum is influenced by the acceleration of the arm, which is actuated by a DC motor. The equations of motion of the Furuta pendulum are [12], 2
€ þ p cos h €h þ 2p sin h cos h h_ /_ ðp1 þ p2 sin hÞ / 3 2 2 _ p sin h h ¼ s/
ð1Þ
€ þ ðp þ p Þ €h p sin h cos h /_ 2 p sin h ¼ 0 p3 cos h / 2 5 2 4
ð2Þ
3
where s/ is the external torque that allows to control the arm rotation. The constants p1 –p5 are positive and defined by physical parameters of the setup, see (A.2) in Appendix A.
Suppose in addition that there exists a C 2 -smooth function UðÞ, such that
/H ðtÞ ¼ UH ðhH ðtÞÞ;
8 t:
ð3Þ
This geometric relation is known as a virtual holonomic constraint [14]. The first and second time derivatives of /H ðtÞ are
d d / ðtÞ ¼ U0H ðhH ðtÞÞ hH ðtÞ; dt H dt 2 2 2 d d d / ðtÞ ¼ U00H ðhH ðtÞÞ hH ðtÞ þ U0H ðhH ðtÞÞ 2 hH ðtÞ: 2 H dt dt dt Substituting these relations and (3) into Eq. (2) of the system dynamics, one obtains the second order differential equation, known as the reduced dynamics ðp2 þ p5 þ p3 cos hH U0H ðhH ÞÞ €hH |fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl} ¼aðhH Þ
2.2. Problem formulation
þ ðp3 cos hH U00H ðhH Þ p2 sin hH cos hH ½U0 ðhH Þ2 Þ h_ 2H þ p4 sinðhH Þ ¼ 0 |fflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflffl} |fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl}
Our objectives are the following: Plan a family of feasible motions of the Furuta pendulum that bring the pendulum from a vicinity of the downward equilibrium to a sufficiently small neighborhood of the upright equilibrium satisfying restrictions on admissible travel of the links.
¼bðhH Þ
ð4Þ
¼cðhH Þ
The homoclinic curve hH ðtÞ, described earlier, is a solution of this equation. Suppose that neither the function UH ðhH Þ nor the particular solution – homoclinic curve hH ðtÞ – are known. The contribution of this paper is to show that the existence of homoclinic curves for dynamics of (4) can be deduced from a few properties of
Fig. 1. The Furuta pendulum built at the Department of Applied Physics and Electronics, UmeaUniversity.
1242
P.X. La Hera et al. / Mechatronics 19 (2009) 1240–1250
Fig. 2. Two homoclinic orbits of the equilibrium at h ¼ 0 are shown on the phase portrait of (4). One of them intersects the line fh_ ¼ 0g at a1 , the other at a2 . In (b) an exemplified periodic trajectory orbiting two homoclinic curves is depicted.
the coefficients aðhH Þ; bðhH Þ, and cðhH Þ. As a result, shaping new homoclinic structures1 for planning swing-up trajectories for the Furuta pendulum becomes feasible.
n R o s p cos sU00 ðsÞp sin s cos s½U0 ðsÞ2 3 2 2 ds p2 þp5 þp3 cos sU0 ðsÞ 0 e
0
hH ð0Þ ¼ a1 ;
3.1. Planning new homoclinic curves for swinging up Motivated by the observations above, let us start the search for swing-up motions considering the relation
8t
ð5Þ
which is supposed to be satisfied along this motion, and where the scalar function UðÞ is unknown. If (5) is imposed on the dynamics, it immediately implies the next relations between angular velocities and accelerations
d d / ¼ U0 ðhÞ h; dt dt
ai
p4 sin s ds p2 þ p5 þ p3 cos s U0 ðsÞ
are equal to zeros. Furthermore, these homoclinic curves can be found as the solutions of (4) with initial conditions
3. Main results
/ðtÞ ¼ UðhðtÞÞ;
Z
2
d
dt
/ ¼ U00 ðhÞ 2
d h dt
2
2
þ U0 ðhÞ
d
dt
2
h:
By substituting these relations into (2), we obtain the second order differential equation (4). Important facts on qualitative behavior of its solutions are given in the next statements. 2
h_ H ð0Þ ¼ 0 and hH ð0Þ ¼ a2 ;
h_ H ð0Þ ¼ 0:
ð8Þ
Statement (A) is obvious. Statement (B) is proved in [15]. To validate (C) we observe that the system (4) is integrable [13] and that _ the integral function IðhðtÞ; hðtÞ; h0 ; h_ 0 Þ, defined by Z I ¼ h_ 2 ðtÞ exp 2
hðtÞ
h0
bðsÞ ds aðsÞ
Z h_ 2
hðtÞ
0
Z exp 2
h0
s h0
bðsÞ 2cðsÞ ds ds ; aðsÞ aðsÞ ð9Þ
remains zero on the solution hðtÞ of (4) initiated at ½h0 ; h_ 0 . If we assume that a homoclinic curve hH ðtÞ of the equilibrium heq ¼ 0 of (4) does exist, then: In the expression (9) we can let h0 and h_ 0 converge along the homoclinic curve (in negative time) to the equilibrium, i.e.
h0 ,0;
h_ 0 ,0
Lemma 1. Let UðhÞ be a C -smooth function defined on IU ¼ ð2p e; 2p þ eÞ with e > 0, such that the function aðhÞ ¼ p2 þ p5 þ p3 cos h U0 ðhÞ is separated from zero2 on IU . Then, the following statements hold.
There is a point along a homoclinic curve at which h_ H should change sign:
(A) The equilibria of the dynamical system (4), i.e. the solutions of the equation cðheq Þ ¼ 0, are
Therefore, evaluating (9) at these points of the homoclinic curve, from I ¼ 0 we obtain the equality
heq ¼ k p;
k 2 Z ¼ f. . . ; 2; 1; 0; 1; 2; . . .g
ð6Þ
(B) Consider the equilibrium h ¼ kp with k 2 Z of the dynamical system (4). If the constant
xk :¼
p4 cos h p2 þ p5 þ p3 cos h U0ðhÞh¼kp
9 t :
hH ðt Þ ¼ a;
h_ H ðt Þ ¼ 0
0 ¼ h_ 2H ðt Þ n R o" Z hH ðt Þ bðsÞ 2 ds a ð s Þ hH ð1Þ h_ 2H ð1Þ e
n R o # s bðsÞ 2 ds 2cðsÞ a ð s Þ hH ð1Þ e ds aðsÞ hH ð1Þ hH ðt Þ
ð7Þ
is positive, then this equilibrium is a center. If, in opposite, xk is negative, then this equilibrium is a saddle. (C) Suppose x1 > 0; x0 < 0; x1 > 0, then the dynamical system (4) has two homoclinic curves of the ‘upright’ equilibrium3 heq ¼ 0, see Fig. 2a, if and only if for some a1 ; a2 , with a1 2 ð2p; pÞ and a2 2 ðp; 2pÞ, the integrals
1 Note that the case of homoclinic curves of a physical pendulum, discussed in the introduction, fits naturally into these settings with UH ðhH Þ being equal to a constant. 2 In other words, it is either positive or negative for all h 2 IU ¼ ½2p e; 2p þ e. 3 Note that h ¼ 0 corresponds to the upright equilibrium for the Furuta pendulum.
that reduces to
0¼
Z 0
a
Z exp 2 0
s
bðsÞ 2cðsÞ ds ds: aðsÞ aðsÞ
Substituting the expressions for aðhÞ; bðhÞ; cðhÞ as in (4), we arrive to the formula in the statement. To prove, in opposite, that the ‘upright’ equilibrium of (4) has two homoclinic curves, observe that the condition x0 < 0 implies that it is hyperbolic; thus, it has one-dimensional stable and unstable manifolds. In turn, the property (C) ensures that both branches of unstable manifolds intersect the line with h_ ¼ 0 at points a1 and a2 . To see that these points indeed belong to homoclinic curves of the equilibrium heq ¼ 0, it is enough to notice that the phase portrait of the dynamical system (4) has the mirror symmetry with
1243
P.X. La Hera et al. / Mechatronics 19 (2009) 1240–1250
respect to the line fh_ ¼ 0g. Hence, the solutions initiated in (8) converge to the ‘upright’ equilibrium in positive time (along its stable manifold) as they do in negative time (along its unstable manifold). h Swinging up the Furuta pendulum based on stabilization of homoclinic curves, defined in Lemma 1, might be challenging, as motivated in the introduction. Indeed, in order to compensate for parametric uncertainties and effects of unmodelled dynamics the swing-up motion should be complemented with a feedback controller. This controller should stabilize the dynamics of the system transverse to the orbit of such a motion in the four dimensional state-space of the Furuta pendulum. Presumably such a controller, if constructed, would be non-robust and of limited use for experimental studies. The conclusion comes from observing that the linearization of the dynamics transverse to each of the homoclinic curves is a linear control system with time varying coefficients, for which designing a stabilizing controller, estimating a rate of convergence, and verifying robustness (even for a known controller) are difficult tasks. To overcome the problem, we suggest to stabilize orbitally a nearby periodic solution of (4) rather than the homoclinic orbit itself. This periodic solution should be surrounding the homoclinic curves close enough, see Fig. 2b. Exponential orbital stabilization of such a periodic solution, if it exists, can be achieved based on the recently proposed algorithms of [14]. In the next statement, further mild sufficient conditions on the constraint function UðhÞ of (5) are given to ensure the presence of a strip of periodic solutions surrounding two homoclinic curves of the ‘upright’ equilibrium on the phase portrait. Lemma 2. Suppose that the conditions of Lemma 1 are valid. If the constants xð2Þ and x2 defined by (7) are both negative, then all solutions hðtÞ of the dynamical system (4), with initial conditions at _ hð0Þ ¼ 0 and
3.2. Choosing the constraint function To plan a swing-up motion for the Furuta pendulum using the Lemmas above, we need to find a function UðhÞ of (5) satisfying several properties. To show that such functions do exist, and that the conditions of Lemmas 1 and 2 are mild and easy to meet, let us present one of them as an example; namely, let
UðhÞ ¼ K arctanðhÞ:
Here, the value of the constant K limits the interval of possible target travel for the /-variable along a swing-up motion within 1 2 K p; 12 K p . With the choice (10) the corresponding coefficients of (4) are
aðhÞ ¼ p2 þ p5 þ
Kp3 cosðhÞ ð1 þ h2 Þ
Proof. Proof is based on the fact that the phase portrait of (4) is symmetric with respect to fh_ ¼ 0g axis. Two homoclinic curves of the upright equilibrium, see Fig. 2, cannot be attractive or repellent for nearby solutions. This solutions should be bounded due to continuous dependence of solutions on initial conditions, see e.g. [7]. Hence, nearby solutions, i.e. the ones with initial conditions _ at hð0Þ ¼ 0 and
p < hð0Þ < p þ d or p d < hð0Þ < p are all periodic for small d > 0. Repeating the same arguments for nearby solutions of established periodic trajectories, we observe that they are valid everywhere between the hyperbolic equilibria at 2p. h
bðhÞ ¼
;
K cos h ð1 þ h2 Þ2
ð2p3 h þ Kp2 sin hÞ: ð11Þ
Let us rewrite all the conditions of Lemma 1 and 2 as constraints on the constant parameter K, with the parameters of the model given in (A.2): (C1) The function aðhÞ should be separated from zero on
½2p e; 2p þ e with some e > 0: The value a p2 ¼ p2 þ p5 is positive, therefore the condition holds if aðhÞ > 0 for all h 2 ½2p; 2p. The minimal value of aðÞ on this interval can be found as
n o min faðhÞg ¼ min að2pÞ; að2pÞ; aðhfig cr Þ ;
h2½2p;2p
where hfig cr are the critical values, i.e. solutions of the equation 0
0 ¼ a ðhcr Þ ¼
p < hð0Þ < 2p or 2p < hð0Þ < p are periodic.
ð10Þ
K p3 1 þ h2cr
"
# 2 hcr cosðhcr Þ ; sinðhcr Þ þ 1 þ h2cr
which can be numerically found and that are apparently independent of K. Finally, the condition of positiveness of aðhÞ within ½2p; 2p can be rewritten as the inequality
K P 38:51
p2 þ p5 ; p3
ð12Þ
where the number is related to a solution of the transcendental equation for hcr . (C2) The values of the five constants xk with k ¼ 2; 1; 0; 1; 2, defined by
p4 cosðhÞ xk ¼ K p2 þ p5 þ p3 cosðhÞ 1þh2
; h¼kp
see (7), should have the following signs
Fig. 3. (a) The phase portrait for (4) when condition (C) of Lemma 1 is satisfied. (b) The phase portrait for (4) when condition (C) of Lemma 1 is not valid.
1244
P.X. La Hera et al. / Mechatronics 19 (2009) 1240–1250
separatrix) of the hyperbolic equilibrium heq ¼ 0 are bounded (or not), see Fig. 3. A simple sufficient condition for this property can be given in terms of the values
60 40
Fða; KÞ at a ¼ 2p; p; p; 2p
20
F(π) and F(2π)
If for some K both inequalities 0
Fð2p; KÞ Fðp; KÞ < 0;
−20
Fð2p; KÞ Fðp; KÞ < 0
hold, then the condition holds as well. The values of Fðp; KÞ 3 are shown in Fig. 4. and Fð2p; KÞ as functions of K n ¼ K p2pþp 5
−40
As seen, when K n < 0:01 both functions Fðp; KÞ and Fð2p; KÞ are negative, while for most of positive values of K n on the plot they have different signs. Nevertheless, both functions are difficult to compute accurately for K n P 6:6. The behavior of Fðp; KÞ and Fð2p; KÞ is the same. To conclude, condition (C) of Lemma 1 is ensured if
−60 −80 −100
0
1
2
3 Kn
4
5
6
3 Fig. 4. The values of Fðp; KÞ and Fð2p; KÞ vs. K n ¼ K p2pþp , the upper (blue) and the 5 lower (red), respectively. As seen, they have different signs for most of positive values of K n , while around zero, K n < 0:01, they are negative. (For interpretation of the references to colour in this figure legend, the reader is referred to the web version of this article.)
xð2Þ < 0; xð1Þ > 0; x0 < 0; x1 > 0; x2 < 0: These conditions are equivalent to the following three independent inequalities for the parameter K
K > ð1 þ 4p2 Þ
p2 þ p5 p þ p5 p þ p5 ; K > 2 ; K < ð1 þ p2 Þ 2 : p3 p3 p3 ð13Þ
(C3) The value of Fðai ; KÞ given by
Z 0
ai
nR e
s 2Kðsp3 cos sþp2 K sin s cos sÞ d 0 ð1þs2 Þððp2 þp5 Þð1þs2 Þþp3 K cos sÞ
s
o
p4 ð1 þ s2 Þ sin s ds ðp2 þ p5 Þð1 þ s2 Þ þ p3 K cos s
should be zeros for some a2 2 ðp; 2pÞ and a1 2 ð2p; pÞ. The fact that this condition is valid (or not) has intuitive geometrical interpretation: the stable and unstable manifolds (the
0:01
p2 þ p5 p þ p5 < K < 6:6 2 ; p3 p3
ð14Þ
where the upper bound can be enlarged via a more accurate computation. 4. Results of computer simulations Here some results of numerical simulations are given for the model (1) and (2) with parameters defined by (A.2) in the appendix. 4.1. Choosing the target trajectory for swinging up Motion planning for swinging up is carried out with the constrained function (10), where K ¼ 1=p has been chosen. Such value of K satisfies all the conditions of Lemma 1 and 2 described by (12)–(14). The phase portrait of the corresponding dynamical system (4) is shown in Fig. 5, where as predicted the upright equilibrium heq ¼ 0 has homoclinic curves surrounded by a strip of periodic solutions. In the following simulation results, the trajectory depicted with the bold line in Fig. 5a and b is chosen as an example of a swing-up motion and is to be orbitally stabilized.
Fig. 5. (a) Phase portrait of the solutions for the reduced dynamics (4) with the virtual constraint (10). The trajectory marked with bold line in (a) and (b) represents the periodic solution chosen for swinging up. This solution has a period T 4:0454 s.
P.X. La Hera et al. / Mechatronics 19 (2009) 1240–1250
1245
_ Fig. 6. Results of numerical simulations for the swing-up stage for the closed-loop system with initial conditions defined in (15): (a) the pendulum angle hðtÞ vs. its velocity hðtÞ _ for the target orbit; (b) the pendulum angle hðtÞ vs. its velocity hðtÞ; (c) the arm angle / vs. the pendulum angle h and the target arctan-relation (10); (d) the pendulum angle h vs. time.
4.2. Simulation with a stabilizing controller for the periodic orbit
4.3. Simulation of combining a switching law and stabilizing controllers
A stabilizing state feedback controller4 for this cycle has been designed following the steps presented in [13, Appendix C, p. 1174]. The performance of the closed-loop system is illustrated in Fig. 6a–d, where we show the target orbit and the trajectory of the closed-loop system initiated at
/ ¼ 0;
/_ ¼ 0;
h ¼ p;
h_ ¼ 0;
ð15Þ
which is the downward open-loop stable equilibrium. The simulated motion of the closed-loop system is in agreement with the theoretical results, i.e. the solution converges to the target cycle with the relation between the angles (see Fig. 6) being an arctan function. 4 The controller design is not discussed here. It is beyond the topic of this article; a detailed description of its derivation can be found in [13,15,14] while a specification to the Furuta pendulum case can be found in [12].
A switching law and a local controller are needed around the upright equilibrium in order to keep the pendulum balanced. With initial conditions around the downward resting equilibrium, the upright position is approached first when h ¼ 0 and 1:34 < h_ < 0, see Fig. 7. In our case, the opposite phase is chosen. This means that _ < 0:3, h_ and h are the switching is to be performed when j/j < 0:1, j/j within the intervals (0,1.34) and (0.24,0.24), respectively. In Fig. 8 we show results of a simulation of the swing-up and the stabilizing controller with the switching proposed. As it is seen, the swing-up strategy brings the pendulum close to h ¼ 0 and then the locally stabilizing controller takes over. It is known that a successful switching depends very much on the region of attraction,5 of the local balancing controller. 5 Note that we have chosen a linear stabilizing controller only for simplicity. The region of attraction can be enlarged with other designs, see e.g. [1,10].
1246
P.X. La Hera et al. / Mechatronics 19 (2009) 1240–1250
15
loaded contains a control function, which is executed according to a sampling period.
10
5.1. Sampling Switching Region
Dtheta [rad/s]
5
So far it has been assumed that disturbances are negligible and that the input signal is realizable and the states are available at every instant of time. However, the dSPACE board used, as well as any other digital hardware, works with an internal processor that runs in discrete time. For this reason, the feedback control law must be sampled, meaning that we have to apply for t : k h < t 6 ðk þ 1Þ h the control signal to the motor
0
−5
def
ur ðtÞ ¼ u½k ¼ Sat0:5 ðuðk hÞÞ;
−10
−15 −6
−4
−2
0 theta [rad]
2
4
ð16Þ
where k is a natural number, the small constant h > 0 is the sampling time (h ¼ 0:0008 s in our case), and uðk hÞ is related to the input torque s/ (see Appendix A) from (1) at t ¼ k h, and SatM ðÞ is the standard saturation function with the cut-off level M ¼ 0:5, which is due to the limitation of the available control torque.
6
_ Fig. 7. The switching region specified as a range of h and h.
5.2. Velocity estimation 5. Sampling, real-time implementation and experimental results
"
x^_ 1 ^x_
4
5
2
0
0
−5
−2
−10
−4
0 θ
2
4
6
þ
ð17Þ
^_ h ¼ SatM2 ðv^ 2 Þ;
ð18Þ
_ h; _ uÞ ¼ 0 f0 ð/; h; /;
10
−2
v^ 1 v^ 2
where either
6
−4
¼
^ /_ ¼ SatM1 ðv^ 1 Þ;
15
−15 −6
#
2ðy1 ^x1 Þ=e1 ; 2ðy2 ^x2 Þ=e2 2 " # " # ðy1 ^x1 Þ=e21 v^_ 1 ¼ f ð^x ; ^x ; /; ^_ ^_ ; h; uÞ þ 0 1 2 ðy2 ^x2 Þ=e22 v^_ 2
θ
dθ/dt
Let us discuss issues related to the controller’s real-time implementation. The Furuta pendulum (see Fig. 1) built in our lab is equipped as follows. Rotation of the arm is controlled by a DC motor, while the pendulum moves freely in the plane, orthogonal to the arm. The DC motor is a brush-commutated Penta M1, equipped with a servo amplifier SCA-SS-70-10. The angular positions are measured with optical incremental encoders: Eltra EH53A (1024 imp/rev) for the arms angle and Wachendorff WDG 40S-2500ABN-H05-SB5-AAC (2500 imp/rev) for the pendulum’s angle. The setup is connected to a dSPACE board 1104 used to run the real-time application. The user interface consists of a PC equipped with ControlDesk (Ó dSPACE GmbH), used for on-line communication with the dSPACE board. Matlab/Simulink (Ó Mathworks) is the software used for the implementation of algorithms as well as simulations. Simulink enables the generation and deployment of production C code for use in real-time embedded systems. The compiled code is loaded into the dSPACE’s processor. The program
The angles h and / are the states measured by the encoders. The angular velocities, which are needed for the feedback control law implementation, are not measured but can be estimated by applying a high gain observer. A high gain observer (HGO) is a fast nonlinear (or linear) full order observer with high observer gains chosen via pole placement, followed by a saturation. Saturation is needed in order to protect the plant from peaking [7, Section 14.5]. We apply
−6
0
ð19Þ
5
10
15
time
Fig. 8. Results of numerical simulations for the closed-loop system with the proposed switching strategy and initial conditions defined in (15): (a) the pendulum angle hðtÞ vs. _ its velocity hðtÞ; (b) the pendulum angle hðtÞ vs. time.
P.X. La Hera et al. / Mechatronics 19 (2009) 1240–1250
1247
Fig. 9. Results of experimental validation for the swing-up motion for the closed-loop system with initial conditions defined in (15): (a) the pendulum angle hðtÞ vs. its velocity _ _ hðtÞ for the target orbit; (b) the pendulum angle hðtÞ vs. its velocity hðtÞ observed in an experiment; (c) the arm angle / vs. the pendulum angle h observed in an experiment and the target arctan-relation (10); (d) the pendulum angle hðtÞ vs. time.
_ h; _ uÞ is a nominal model defined by (1) and (2), the small or f0 ð/; h; /; parameters e1;2 > 0 are to be tuned,
5.3. Experimental results for swinging up
Experimental results with a feedback controller designed as in [12] are presented in Fig. 9a–d for the initial conditions (15). It can be seen that the proposed swing-up method works in practice. In fact, the desired cycle is approximately achieved. Irrespective of variation in initial conditions, experiments show reproducibility and stability. The mismatch between experimental and desired motions, see Fig. 9c, is thought to be produced by the following factors: the friction torque at the pendulum shaft is not compensated; discretization of the measurements does not allow us to use sufficiently fast differentiators and leads to introduction of additional delays imposed by our observers; and there are presumably input and output delays present in the hardware. However, it is experimentally verified that the steady-state in the experiment is not destroyed by those factors. This validates robustness of our design.
y1
y2
¼
/ðtÞ hðtÞ
þ wðtÞ with wðtÞ ¼
/½k /ðtÞ
h½k hðtÞ
is the measured output with the ‘measurement noise’ defined by sampling and encoders’ quantization6:
h½k ¼
2 p 2500 hðkhÞ ; 2500 2 p
/½k ¼
2p 1024 /ðkhÞ ; 1024 2p
8t : k h < t 6 ðk þ 1Þ h; and ‘‘bc” denotes the floor function. The sampling period is set to be h ¼ 0:0008 s. and the observer parameters are taken as e1 ¼ 20 h and e2 ¼ 10 h, while M 1 ¼ M2 ¼ 20 rad=s, which corresponds to a reasonable level of upper bounds on the speeds. 6
Actually, there is also some smoothing done by the Simulink interface.
1248
P.X. La Hera et al. / Mechatronics 19 (2009) 1240–1250
6
10
4
5
2 θ [rad]
dθ/dt [rad/sec]
Experimental Phase Portrait 15
0
0
−5
−2
−10
−4
−15 −6
−4
−2
0 θ [red]
2
4
θ trajectory
−6 10
6
15
20
25 time [sec]
30
35
40
_ Fig. 10. Results of numerical simulations for the closed-loop system with the proposed switching strategy: (a) the pendulum angle hðtÞ vs. its velocity hðtÞ; (b) the pendulum angle hðtÞ vs. time.
Torque vs Input Signal
−3
4
0.7
x 10
Mapped friction
3
0.6
Torque = 0.51*u+0.002
2 1
0.4 u
Torque[Nm]
0.5
0
0.3 −1 0.2
−2
0.1
0 0
−3
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
−4 −0.1 −0.08 −0.06 −0.04 −0.02
Input Signal [Volt]
0
0.02
0.04
0.06
0.08
0.1
velocity
Fig. A.1. Results of identification experiments.
5.4. Experimental results for combining switching and stabilizing controllers Let us show now the results of switching controller described earlier. As above, switching is done whenever j/j < 0:1 ½rad; _ < 0:3 ½rad=s; 0 < h_ < 1:34 ½rad=s, and jhj < 0:24 ½rad. The local j/j controller is LQR designed for the linearization of the system dynamics (1) and (2). In Fig. 10a and b we show results of combining swinging up, switching, and local stabilizing controller. In experiments7 with different initial conditions, the state h converges to the desired steady state as expected. It is worth mentioning that once the switching is done, and the pendulum is stabilized, the friction compensation is deactivated. This is done to avoid the chat-
7 Movies with recorded experiments are available at http://www.control.umu.se/ Set_Ups/Furuta_Pendulum/Furuta_Pendulum_info.html.
tering of the input signal due to numerical errors unavoidable when the velocity is close to zero.
6. Conclusions We have suggested a new systematic approach for planning trajectories suitable for swinging up the Furuta pendulum. Our arguments are based on an accurate planning of a periodic trajectory enclosing two homoclinic curves, and results from analysis of a reduced dynamical system. This reduced dynamics describes the motions of the non-actuated link. For the resulting motion, the arm and pendulum angles are regulated by some control input to satisfy a certain geometric relation. This geometric relation, known as virtual holonomic constraint, ensures certain restrictions on the target motion of the pendulum during the swinging up. This is necessary in order to deal with hardware constraints imposed
P.X. La Hera et al. / Mechatronics 19 (2009) 1240–1250
by the maximum allowable motion of the rotational link. The preplanned cycle has been robustly stabilized by the control method introduced in [13], which ensures local exponential orbital stability with a specified rate of convergence. The overall performance is illustrated by numerical simulations and experimental studies. Acknowledgement We would like to acknowledge the work of Anders Sandberg and Sven Elmå, for the design and building of the Furuta pendulum at the Department of Applied Physics and Electronics, UmeaUniversity. Our work has been partially supported by the Kempe Foundation and the Swedish Research Council under Grants 2005-4182 and 2008-5243. Appendix A. Parameters of the model Using basic principles, the parameters of the dynamic model of the Furuta pendulum (1) and (2) can be computed as follows:
1 2 2 p1 ¼ J þ ðM þ mp Þla ½kg m2 ; p2 ¼ M þ mp lp ½kg m2 ; 4 1 1 p3 ¼ M þ mp lp la ½kg m2 ; p4 ¼ M þ mp lp g ½Nm; 2 2 1 2 2 p5 ¼ mp lp ½kg m ; 12 ðA:1Þ where the values of the physical parameters for our experimental setup, see Fig. 1, are: the length of the arm la ¼ 0:15 ½m, the mass of the arm ma ¼ 0:298 ½kg, the length of the pendulum lp ¼ 0:26 ½m, the mass of the pendulum mp ¼ 3:2 102 ½kg, and the mass of the bob at the end of the pendulum M ¼ 7:5 103 ½kg. An estimate of the arm’s inertia according to CAD drawings is J a ¼ 7:682 104 ½kg m2 . For these values the coefficients p1 –p5 of (1) and (2) are
p1 ¼ 1:8777 103 ;
p2 ¼ 1:3122 103 ;
p4 ¼ 5:9301 102 ;
p5 ¼ 1:77 104
p3 ¼ 9:0675 104 ; ðA:2Þ
In order to apply the control law, described in [13,12], the values of the parameters above must be verified and the control signal should appropriately shape the torque s/ , applied to the arm. This external torque consists of various components and can be modeled as
s/ ¼ K DC u þ F /fr F /fr
with u being the control input and being the friction torque. The electromotive torque constant K DC is estimated using a dynamometer attached at the end of the arm along the horizontal plane. By applying input voltage of different levels the corresponding force can be measured, producing a linear relation (see Fig. A.1a). The torque is calculated by multiplying this force with the distance from the motor axis to the end of the arm. The gain K DC is the slope of this linear relation, which is estimated to be K DC ¼ 0:51 ½Nm=V. The friction torque F /fr can be modeled as a nonlinear static function of velocity. Its approximation can be used for partial compensation of the friction present in the arm. The compensation scheme adopted here takes into account Coulomb and viscous friction and uses estimates of the arm velocity done applying (18) and (19). In Fig. A.1b the static friction map identified for our setup using a standard procedure is shown. The compensation law is defined as
( ufr ¼
_ 0:032 0:002 /;
if _ if 0:033 0:002 /;
/_ > 0 /_ < 0
1249
The parameters p1 –p5 from (1) and (2) need to be verified via identification, because approximations given by (A.1) might not be in accordance with the real setup. It is done in two steps: identification of the arm parameters when the pendulum is detached and identification of the pendulum parameters when the arm is fixed. Due to the complex shape of the arm (see Fig. 1), the approximated value p1 related with its inertia needs to be verified. For this purpose, the pendulum was detached from the setup. With such a modification the equation of motion (1) of the arm becomes
€ ¼ K DC u þ F / : p1 / fr The value for the parameter p1 can be calculated from this equation assuming that the Coulomb part of the F /fr is perfectly compensated by some control input u ¼ uC . This assumption leads to the _ Then, the transfer € ¼ K DC ðu uC Þ K DC bh /. arm dynamics: p1 / function from the input voltage u uC to the arm velocity /_ is K DC =p1 : If we apply a step input of some magnitude sþK DC bh =p1 u uC ¼ u0 , the gain K DC =p1 can be calculated from the step response dividing the magnitude of the steady-state velocity /_ ¼ /_ 0 by the input level. The identified value is K DC ¼ 0:0034. p1 ¼ /_ =ðuu 0 CÞ Let us proceed with verification of the pendulum parameters. The experiment considers only the movement of the pendulum with the arm fixed. No input is given, meaning that / and its derivatives are zeros. Free oscillations of the pendulum are recorded. For this experiment, the model (1) and (2) is reduced to:
ðp2 þ p5 Þ €h p4 sinðhÞ ¼ sh ; where the values of p2 ; p5 ; p4 are to be identified and the torque sh mainly consists of friction. The pendulum is manually positioned at some initial condition hð0Þ where it is left free, producing oscillations around the downward position p. The torque sh can be modeled as viscous friction with a damping parameter bh , so that we obtain: h p4 sinðhÞ þ bh h_ ¼ 0. The parameters are fit to the data ðp2 þ p5 Þ € using the nonlinear least square method. This estimation results in the values p2 þ p5 ¼ 0:0014 ½kg m2 and p4 ¼ 0:0617 ½Nm, from which p2 ¼ 0:0013 ½kg m2 . These parameters are clearly close to the calculated ones in (A.2). Appendix B. Supplementary material Supplementary data associated with this article can be found, in the online version, at doi:10.1016/j.mechatronics.2009.07.005. References [1] Ohsumi A, Izumikawa T. Nonlinear control of swing-up and stabilization of an inverted pendulum. In: Proc 34th Conf Decision and Control; 1995. p. 3873–80. [2] Fantoni I, Lozano R, Spong MW. Energy based control of the Pendubot. IEEE Trans Autom Control 2000;45(4):725–9. [3] Fantoni I, Lozano R. Stabilization of the Furuta pendulum around its homoclinic orbit. Int J Control 2002;75(6):390–8. [4] Fradkov AL. Swinging control of nonlinear oscillations. Int J Control 1996;64:1189–202. [5] Gordillo F, Acosta JA, Aracil J. A new swing-up law for the Furuta pendulum. Int J Control 2003;76(8):836–44. [6] Furuta K, Yamakita M, Kobayashi S. Swing up control of inverted pendulum. Proc Ind Elect Control Instrum 1991:2193–8. [7] Khalil HK. Nonlinear systems. third ed. Prentice-Hall; 2002. [8] Kolesnichenko O, Shiriaev A. Partial stabilization of underactuated Euler– Lagrange systems via a class of feedback transformations. Syst Control Lett 2002;45(2):121–32. [9] Mahindrakar A, Banavar R. A swing-up of the Acrobot based on a simple pendulum strategy. Int J Control 2005;78(6):424–9. [10] Nair S, Leonard NE. A normal form for energy shaping: application to the Furuta pendulum. In: Proc 41st Conf Decision andControl; 2002. p. 516–21.
1250
P.X. La Hera et al. / Mechatronics 19 (2009) 1240–1250
[11] Wei Q, Dayawansa WP, Levine WS. Nonlinear controller for an inverted pendulum having restricted travel. Automatica 1995;31(6):841–50. [12] Shiriaev AS, Freidovich LB, Robertsson A, Johansson R, Sandberg A. Virtualholonomic-constraints-based design of stable oscillations of Furuta pendulum: theory and experiments. IEEE Trans Robot 2007;23(4):827–32. [13] Shiriaev AS, Perram JW, Canudas-de-Wit C. Constructive tool for orbital stabilization of underactuated nonlinear systems: virtual constraints approach. IEEE Trans Autom Control 2005;50(8):1164–76. [14] Shiriaev AS, Freidovich LB, Manchester IR. Can we make a robot ballerina perform a pirouette? Orbital stabilization of underactuated mechanical systems. Ann Rev Control 2008;32(2):5–8.
[15] Shiriaev AS, Robertsson A, Perram J, Sandberg A. Periodic motion planning for virtually constrained Euler–Lagrange systems. Syst Control Lett 2006;55(11):900–7. [16] Shiriaev AS, Pogromsky A, Ludvigsen H, Egeland O. On global properties of passivity based control of an inverted pendulum. Int J Robust Nonlinear Control 2000;10(4):283–300. [17] Spong MW, Corke P, Lozano R. Nonlinear control of the reaction wheel pendulum. Automatica 2001;37(11):1845–51. [18] Zhang M, Tarn TJ. Hybrid control of the Pendubot. IEEE/ASME Trans Mechatron 2002;7(1):79–86.