Nonlinear Analysis and Control of a Reaction Wheel-based 3D Inverted Pendulum

Transcription

1 Nonlinear Analysis and Control of a Reaction Weel-based 3D Inverted Pendulum Micael Muelebac, Gajamoan Moanaraja, and Raffaello D Andrea Abstract Tis paper presents te nonlinear analysis and control design of te Cubli, a reaction weel-based 3D inverted pendulum. Using te concept of generalized momenta, te key properties of a reaction weel-based 3D inverted pendulum are compared to te properties of a D case in order to come up wit a relatively simple and intuitive nonlinear controller. Finally, te proposed controller is implemented on te Cubli, and te experimental results are presented. I. INTRODUCTION Tis paper presents te nonlinear analysis and control of te Cubli sown in Figure. Te Cubli is a cube of 5 mm side lengt wit tree reaction weels mounted ortogonally to eac oter. Compared to oter 3D inverted pendulum test beds [] and [], te Cubli as two unique features. One is its relatively small footprint (ence te name Cubli, wic is derived from te Swiss German diminutive for cube ). Te oter feature is its ability to jump up from a resting position witout any external support, by suddenly braking its reaction weels rotating at ig angular velocities. Wile te concept of jumping up is covered in [3], and a linear controller is presented in [4], tis paper focuses on te analysis of te nonlinear model and nonlinear control strategies. In contrary to te works presented in [5] and [6], tis paper approaces te controller design from a purely mecanical point of view. Fundamental mecanical properties of te Cubli are exploited to come up wit simple and intuitive derivations wit pysical insigts ( [5]: D, reaction weel, feedback linearization; [6]: 3D, proof mass, linear controller, local stability). Te resulting smoot, asymptotically stable control law provides a relatively simple tuning strategy based on four intuitive parameters, tus making te control law suitable for practical implementations. Te remainder of tis paper is structured as follows: A brief nonlinear analysis and control of te reaction weelbased D inverted pendulum is first presented in Section II. Next, te nonlinear system dynamics is derived from first principles in Section III, wic is followed by a detailed analysis and control design in Sections IV and V, respectively. Finally, experimental results are presented in Section VI and conclusions are made in Section VII. II. ANALYSIS AND CONTROL OF A D INVERTED PENDULUM Tis section presents te analysis and nonlinear control strategy for a D reaction weel-based inverted pendulum. Te autors are wit te Institute for Dynamic Systems and Control, Swiss Federal Institute of Tecnology Züric, Switzerland. Te contact autor is M. Gajamoan, gajan@etz.c. Fig.. Cubli balancing on a corner. In te current version, te Cubli (controller) must be initialized wile olding te Cubli near te equilibrium position. Due to te similarity of some key properties in bot te D and 3D cases, tis section sets te stage for te analysis of te 3D inverted pendulum in te later sections. A. Modelling ϕ Fig.. A scematic diagram of a reaction weel based D inverted pendulum. Tis can be realized by putting te Cubli on one of its edges. Let ϕ and ψ describe te positions of te D inverted pendulum as sown in Figure. Next, let Θ w denote te reaction weel s moment of inertia, Θ denote te system s total moment of inertia around te pivot point in te body ψ

2 fixed coordinate frame, and m tot and l represent te total mass and distance between te pivot point to te center of gravity of te wole system. Te Lagrangian [7] of te system is given by: L = ˆΘ ϕ + Θ w( ϕ + ψ) mg cos ϕ, () were ˆΘ = Θ Θ w >, m = m tot l and g is te constant gravitational acceleration. Te generalized momenta are defined by p ϕ := L ϕ = Θ ϕ + Θ w ψ, () p ψ := L ψ = Θ w( ϕ + ψ). (3) Let T denote te torque applied to te reaction weel by te motor. Now, te equations of motion can be derived using te Euler-Lagrange equations wit te torque T as a nonpotential force. Tis yields ṗ ϕ = L = mg sin ϕ, ϕ (4) ṗ ψ = L + T = T. ψ (5) Note tat te introduction of te generalized momenta in () and (3) leads to a simplified representation of te system, were (4) resembles an inverted pendulum augmented by an integrator in (5). Since te actual position of te reaction weel is not of interest, we introduce x := (ϕ, p ϕ, p ψ ) to represent te reduced set of states and describe te dynamics of te mecanical system as follows: ẋ = B. Analysis ϕ ṗ ϕ ṗ ψ = f(x, T ) = ˆΘ (p ϕ p ψ ) mg sin ϕ T. (6) ) State Space: For te subsequent analysis te state space is defined as X = {x ϕ ( π, π], p ϕ R, p ψ R}. ) Equilibria: Te equilibria of te system are given by E = {(x, T ) X R f(x, T ) = }. Evaluating ṗ ϕ = ṗ ψ = ϕ = gives te following equilibria: E = {(x, T ) X R ϕ =, p ϕ = p ψ, T = }, (7) E = {(x, T ) X R ϕ = π, p ϕ = p ψ, T = }. (8) Eac equilibrium is a closed invariant set wit a constant p ψ wic may be non-zero. Mecanically, tis means tat te pendulum may be at rest in an uprigt position wile its reaction weel rotates at a constant angular velocity. Furter linear analysis reveals tat te uprigt equilibria E is unstable, wile te anging equilibria E is stable in te sense of Lyapunov. C. Control Design Te goal of te controller is to drive te inverted pendulum towards te uprigt position ϕ = and to bring te angular velocity of te reaction weel ψ to zero, specifically lim x =. (9) t Now, consider te following feedback control law were u(ϕ, p ϕ, p ψ ) = k mg sin ϕ + k p ϕ k 3 p ψ, () k = + α ˆΘ + βγ mg + δ, k = βˆθ cos ϕ + γ( + α ˆΘ ), k 3 = βˆθ cos ϕ + γ α, β, γ, δ >. and Teorem.: Te control law T = u(ϕ, p ϕ, p ψ ) given in () makes te equilibrium point x = stable and asymptotically stable on te domain X = X \{ϕ = π, p ϕ =, p ψ = }. Proof: Consider te following Lyapunov candidate function V : x X R, V (x) = αp ϕ + mg( cos ϕ) + δ ˆΘ z, () were z = z(x) = p ϕ ( + α ˆΘ ) + β sin ϕ p ψ. Tere exists te class K function a(x) := ɛx wit ɛ < min{ mg π, α, δ ˆΘ } suc tat V (x) a( (p ϕ, ϕ, z) ) x X and V () =. Hence, V (x) is positive definite and a valid Lyapunov candidate. Te time derivative of te above Lyapunov candidate function along te closed loop trajectory is given by V (x) = mg (z β sin ϕ) sin ϕ + ˆΘ δ ˆΘ zż = mg ˆΘ β sin ϕ γ δ ˆΘ z, x X. Since V (x) x X, we conclude from Lyapunov s stability teorem tat te point x = is stable. Now, to prove asymptotic stability of x = in X, let us define te set R = {x X V (x) = }. Consider trajecories x(t) inside an invariant set of R. Clearly, tey must fullfill ϕ =, z = and te system dynamics (6). Tis implies x(t) and terefore te largest invariant set in R is x =. Now, by Krasovskii LaSalle principle [9] (Teorem 4.4) it follows tat, for any trajectory x(t), lim x(t) =, x() X. t For a similar control design and stability proof of te D reaction weel-based inverted pendulum, see []. A function a : R R + is of class K, if it is strictly increasing and a(r = ) =, lim r a(r), see [8].

3 D. Remarks ) Interpretation of te Lyapunov function (): Te Lyapunov function () can be found by a standard backstepping approac [9], []. If te reaction weel dynamics are neglected and p ψ is assumed to be te control input, te function Ṽ (ϕ, p ϕ) = αp ϕ + mg( cos ϕ) is a valid Lyapunov function candidate. Requiring Ṽ along trajectories would imply p ψ = p ϕ ( + α ˆΘ ) + β sin ϕ, i.e. z =. Tis leads to te interpretation tat z penalizes te deviation from te control law tat would be applied to te system wen neglecting weel dynamics. Hence, decreasing te tuning parameter δ leads to a more aggressive controller tat prioritizes te tilt over te weel velocity. ) Extension of te controller (): Te Lyapunov function () can be augmented by an additional integrator state, ˆV (x) = V (x) + ν δ ˆΘ zint, were z int (t) = z + t z(τ)dτ. As a consequence, te controller () must be extended wit z int, û = u + νz int in order to provide stability of te uprigt equilibrium. Tis may also be important for practical implementation since it accounts for modelling errors. 3) Interpretation of te control law (): Te control law given in () can be rewritten as were u = β mg p ϕ + k ṗ ϕ + γ( + α ˆΘ )p ϕ γp ψ, p ψ = u + t u(τ)dτ. Tis is noting but a linear controller composed of a proportional, (double) differential, and integral parts. Furtermore γ can be interpreted as te integrator weigt. III. SYSTEM DYNAMICS OF THE REACTION WHEEL-BASED 3D INVERTED PENDULUM Let Θ denote te total moment of inertia of te full Cubli around te pivot point O (see Figure 3), Θ wi, i =,, 3 denote te moment of inertia of eac reaction weel (in te direction of te corresponding rotation axis) and define Θ w := diag(θ w, Θ w, Θ w3 ), ˆΘ := Θ Θ w. Next, let m denote te position vector from te pivot point to te center of gravity multiplied by te total mass and g denote te gravity vector. Te projection of a tensor onto a particular coordinate frame is denoted by a preceding superscript, i.e. B Θ R 3 3, B ( m) = B m R 3. Te arrow notation is used to empasize tat a vector (and tensor) sould be a priori tougt of as a linear object in a normed vector space detaced from its coordinate representation wit respect to a particular coordinate frame. Since te body fixed coordinate frame {B} is te most commonly projected coordinate frame, we usually remove its preceding superscript for te sake of simplicity. Note furter tat B ˆΘ = ˆΘ R 3 3 is positive definite. Be 3 {B} Ie 3 O Be {I} Ie Ie Be Fig. 3. Cubli balancing on te corner. B e and I e denote te principle axis of te body fixed frame {B} and inertial frame {I}. Te pivot point O is te common origin of coordinate frames {I} and {B}. Te Lagrangian of te system is given by L(ω, g, ω w, φ) = ω T ˆΘ ω + (ω + ω w ) T Θ w (ω + ω w ) + m T g, () were B ( ω ) = ω R 3 denotes te body angular velocity and B ( ω w ) = ω w R 3 denotes te reaction weel angular velocity. Te components of T R 3 contain te torques as applied to te reaction weels. In te body fixed coordinate frame, te vector m is constant wereas te time derivative of g is given by B ( g) = = ġ +ω g = ġ + ω g. Te tilde operator applied to te vector v R 3, i.e ṽ denotes te skew symmetric matrix for wic ṽa = v a olds a R 3. Introducing te generalized momenta T L p ω =: = Θ ω + Θ w ω w, (3) ω T L p ωw =: = Θ w (ω + ω w ) (4) ω w results in te equations of motion given by ġ = ω g, (5) ṗ ω = ω p ω + mg, (6) ṗ ωw = T. (7) Note tat tere are several ways of deriving te equations of motion; for example in [4] te concept of virtual power as been used. A derivation using te Lagrangian formalism is sown in te appendix and igligts te similarities ttp:// cublicdc3-appendix.pdf

4 between te D and 3D case. Te Lagrangian formalism also motivates te introduction of te generalized momenta. Now, using v = v + ω v, te time derivative of a vector in a rotating coordinate frame, (5)-(7) can be furter simplified to g =, (8) p ω = m g, (9) ṗ ωw = T. () Tis igligts, in particular, te similarity between te D and 3D inverted pendula. Since te norm of a vector is independent of its representation in a coordinate frame, te -norm of te impulse cange is given by p ω = m g sin φ. In tis case, φ denotes te angle between te vectors m and g. Additionally, as in te D case, p ωw is te integral of te applied torque T. IV. ANALYSIS A. Conservation of Angular Momentum From (9) it follows tat te rate of cange of p ω lies always ortogonal to m and g. Since g is constant, p ω will never cange its component in direction g during te wole trajectory. Expressed in Cubli s body fixed coordinate frame, it can be written as d dt (pt ω g) and tis is noting but te conservation of angular momentum around te axis g. Tis as a very important consequence for te control design: Independent of te control input applied, te momentum around g is conserved and, depending on te initial condition, it may be impossible to bring te system to rest. B. State Space Since te subsequent analysis and control will be carried out in te fixed body coordinate frame, te state space is defined by te set X = {x = (g, p ω, p ωw ) R 9 g = 9.8}. Note tat ω = ˆΘ (p ω p ωw ). C. Equilibria Te procedure is identical to te one presented in subsection II-B.. As can be seen from (9), te condition p ω = is fullfilled only if m g, i.e., g = ± m m g. From () it follows tat p ωw = const, T =. Using (5) and (6) leads to ω = ˆΘ (p ω p ωw ) g and p ω g, wic corresponds exactly to te conserved part of p ω. Combining everyting togeter results in te following equilibria E = {(x, T ) X R 3 g T m = g m, p ω m, ˆΘ (p ω p ωw ) m, T = }, E = {(x, T ) X R 3 g T m = g m, p ω m, ˆΘ (p ω p ωw ) m, T = }. Linearization reveals tat te uprigt equilibria E is unstable, wile te anging equilibria E is stable in te sense of Lyapunov. V. NONLINEAR CONTROL OF THE REACTION WHEEL-BASED 3D INVERTED PENDULUM Let us first define te control objective. Since te angular momentum p ω is conserved in te direction of g, te controller may only bring te component of p ω tat is ortogonal to g to zero. Hence it is convenient to split te vector p ω into two parts: one in te direction of g, and one ortogonal to it, i.e. p ω = p ω + p ω g and p ω g = ( p ω T g g) g. From (9) and te conservation of angular momentum, it follows directly tat B ( p ω ) = ṗ ω + ω p ω = mg. () Anoter reasonable addition to te control objective is te asymptotic convergence of te angular velocity of te Cubli, ω, to zero. Consequently, te control objective can be formulated as driving te system to te closed invariant set T = {x X g T m = g m, ω = ˆΘ (p ω p ωw ) =, p ω = }. In order to prove asymptotic stability, te anging equilibrium must be excluded (as will become clear later). Tis can be done by introducing te set X = X \ x, were x denotes te anging equilibrium wit ω = : x = {x X g = g m m, p ω =, p ω = p ωw }. Next, consider te controller were u = K mg + K ω + K 3 p ω K 4 p ωw, () K =( + βγ + δ)i + α ˆΘ, K =α ˆΘ p ω + β m g + p ω, K 3 =γ(i + α ˆΘ (I ggt g )), K 4 =γi, α, β, γ, δ >, and I R 3 3 is te identity matrix. Teorem 5.: Te controller given in () makes te closed invariant set T of te system defined by (8)-() stable and asymptotically stable on x X. Proof: Consider te following Lyapunov candidate function V : X R, V (x) = αp ω T p ω + m T g + m g + zt ˆΘ δ z, (3) wit z = z(x) = α ˆΘ pω + p ω + β mg p ωw. Ten te K function a(x) := ɛx, wit ɛ < min{ π m g, δ ˆΘ, α } is suc tat V (x) a( (pω, p ωw, z) ), x X. Furtermore V (x = x ) = implies x = x, were x T. Terefore V is a positive definite function and a valid Lyapunov candidate.

5 Next, V is evaluated along trajectories of te closed loop system: V (x) = αpω Tṗ ω + m T ġ + zt ˆΘ δ ż = m T g ˆΘ (β gm + z) + zt ˆΘ δ ż = β( gm) T ˆΘ ( gm) + zt ˆΘ ( mg + δ ż) = β( gm) T ˆΘ ( gm) γ δ zt ˆΘ z, x X. Since V (x) x X, we conclude from Lyapunov s stability teorem tat te point x T is stable. To prove aymptotic stability of te set T in X define te set R := {x X V (x) = }. Te condition V (x) = leads immediately to z =, m g, suc tat R can be rewritten as R = {x X m g, p ωw = α ˆΘ pω +p ω }. Now, let us consider te system dynamics of a trajectory inside an invariant set contained in R. Tis gives g m g = m g ġ = m ω g ġ = ω g (4) g m, z = ω = αp ω ω p ω (5) However, since pω g by definition, (4) and (5) imply ω = and pω =. Tis sows tat T is te largest invariant subset of R. Now, by Krasovskii LaSalle principle [9] (Teorem 4.4), it follows tat, for any trajectory x(t), A. Remarks lim x(t) = x f, x() X, x f T. t ) Interpretation of te Lyapunov function (3): Similar to te D case, te Lyapunov function (3) can be found via a two-step backstepping approac. Te reduced Lyapunov function Ṽ (x) = αp ω T p ω + m T g + m g can be used to demonstrate stability for p ωw = α ˆΘ pω + p ω + β mg, i.e. z =. Terefore z can again be interpreted as penalty term, see Section II-D. ) Extension of te controller (): As in te D case, te Lyapunov function (3) can be extended wit an additional state, ˆV (x) = V (x)+ ν δ zt int ˆΘ z int, z int (t) = z + t z(τ)dτ. Straigtforward derivation sows tat te resulting controller must be augmented by z int, i.e. û = u + νz int. Integral control is useful in practice to prevent steady state deviations caused by modelling errors. B. Interpretation of te control law () Rewriting () yields u =ṗ ω + γp ω + α ˆΘ (ṗ ω + γp ω )+ m(βġ + (δ + γβ)g) γp ωw, (6) were p ωw = u + t u(τ)dτ. Terefore te controller given in () is a linear PID controller in te variables p ωw, pω w and g. Te nonlinearity of te controller lies in te projection of p ω into pω and p g ω, and te computation of te g-vector from te quaternion based position estimate. Now, for small inclination angles of te body, te above nonlinear effects can be neglected to give a linearized controller tat as a similar performance to te proposed nonlinear controller. C. Offset-Correction Filter Altoug te Cubli is stabilized in uprigt position, te desired equilibrium (x T ) may not be reaced in practice. Tis is due to modelling errors, m of te parameter m. Denote ˆm = m + m, te wrong estimate of te parameter m. Since te Cubli cannot reac an equilibrium position oter tan m g (see Section IV-C), te modelling errors can be reduced by projecting ˆm on g. Tis adaptation can be made iteratively, but must be carried out very slowly, in order to keep te uprigt equilibrium stable. Since te controller is implemented in discrete time, tis leads to te following euristic algoritm: ˆm = ˆm, (7) ˆm k+ = µ ˆmT k g g + ( µ) ˆm k, (8) g g ˆm k+ = ˆm k+ ˆm k ˆm k+, (9) were µ (µ ) represents te time constant of te adaptation. Note tat (9) is required to ensure tat te magnitude of ˆm is not affected. VI. EXPERIMENTAL RESULTS Tis section presents disturbance rejection measurements of te proposed controller. Te controller given in () is implemented on te Cubli wit a sampling time of T s =5 ms along wit te algoritm proposed in [] for state estimation. Figures 4 and 5 sow disturbance rejection plots of te proposed controller. A disturbance of rougly. Nm was applied for 6 ms on eac of te reaction weels simultaneously. Te control input sown in Figure 6, were te controller reaces te steady state control input in less tan.3 s. Finally, te controller attained a root mean square (RMS) inclination error of less tan.5 at steady state.for small inclination angles, te linearized counterpart of te proposed controller also gave a similar performance. Note tat te experimental setup did not allow large inclinations due to te saturation of te motor torques (8 [mnm]).

6 inclination angle [rad] 8 x time[s] Fig. 4. Disturbance rejection measurement. Depicted is te resulting inclination angle of te nonlinear controller. In tis case an asymetric disturbance of rougly.[nm] is applied simultaneously on two reaction weels. control input [A] time [s] Fig. 6. Disturbance rejection measurement. Depicted is te resulting control input of te nonlinear controller. Te different colors depict te different vector components of u T. In tis case an asymetric disturbance of rougly.[nm] is applied simultaneously on two reaction weels. body angular velocity [rad] time [s] Fig. 5. Disturbance rejection measurement. Depicted is te resulting body angle velocity, ω of te nonlinear controller. Te different colors depict te different vector components of ω. In tis case an asymetric disturbance of rougly.[nm] is applied simultaneously on two reaction weels. VII. CONCLUSION Tis paper represents a furter step in te development of te Cubli: a 3D inverted pendulum wit a relatively small footprint. By using te concept of generalized momenta, te dynamics of te reaction weel-based 3D inverted pendulum were put in relation to te D case. Te key properties of te pendulum system were analyzed and a controller was proposed, wic was sown to asymptotically stabilize te uprigt equilibrium. Finally, te controller was implemented on te Cubli, tuned using its intuitive parameters, and its performance was evaluated. [] S. Trimpe and R. D Andrea, Accelerometer-based tilt estimation of a rigid body wit only rotational degrees of freedom, in IEEE International Conference on Robotics and Automation (ICRA),, pp [3] M. Gajamoan, M. Merz, I. Tommen, and R. D Andrea, Te Cubli: A cube tat can jump up and balance, in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),, pp [4] M. Gajamoan, M. Muelebac, T. Widmer, and R. DAndrea, Te Cubli: A reaction weel based 3D inverted pendulum, in Proceedings of te European Control Conference, 3. [5] S. Co and N. McClamroc, Feedback control of triaxial attitude control testbed actuated by two proof mass devices, in Proceedings of te 4st IEEE Conference on Decision and Control,, pp [6] M. W. Spong, P. Corke, and R. Lozano, Brief nonlinear control of te reaction weel pendulum, Automatica, pp ,. [7] L. Cornelius, Te variational principles of mecanics, 4t ed. University of Toronto Press Toronto, 97. [8] H. Kalil, Nonlinear Systems, Section Definition 4.3. Upper Saddle River, New Jersey: Prentice Hall, 996. [9], Nonlinear Systems. Upper Saddle River, New Jersey: Prentice Hall, 996. [] R. Olfati-Saber, Nonlinear control of underactuated mecanical systems wit application to robotics and aerospace veicles, P.D. dissertation, Massacusetts Institute of Tecnology,, ttp://engineering. dartmout.edu/ olfati/papers/pdtesis.pdf visited on ACKNOWLEDGEMENTS Te autors would like to express teir gratitude towards Igor Tommen, Corzillius Marc-Andrea, Hans Ulric Honegger, Alex Braun, Tobias Widmer, Felix Berkenkamp, and Sonja Segmueller for teir significant contribution to te mecanical and electronic design of te Cubli. REFERENCES [] D. Bernstein, N. McClamroc, and A. Bloc, Development of air spindle and triaxial air bearing testbeds for spacecraft dynamics and control experiments, in American Control Conference,, pp