An homotopy method for exact tracking of nonlinear nonminimum phase systems: the example of the spherical inverted pendulum

9 American Control Conference Hyatt Regency Riverfront, St. Louis, MO, USA June -, 9 FrA.5 An homotopy method for exact tracking of nonlinear nonminimum phase systems: the example of the spherical inverted pendulum Luca Consolini a, Mario Tosques b a Dipartimento di Ingegneria dell Informazione, b Dipartimento di Ingegneria Civile Parco Area delle Scienze 8/a, 43 Parma, Italy Email: luca.consolini@polirone.mn.it, mario.tosques@unipr.it Abstract This paper considers an homotopy method for solving the exact tracking problem for nonlinear affine nonminimum phase systems. The method is presented in a general setting and is applied to the special case of the spherical pendulum. This approach allows finding sufficient conditions for exact tracking for T -periodic curves and bounds on the internal dynamics. INTRODUCTION It is well known that the exact dynamic inversion problem is particularly challenging for nonlinear, nonminimum phase systems. In this case the internal dynamics are unstable and grow unbounded for generic initial conditions. This problem has been considered extensively in literature in the last few years, leading to different approaches. For instance, it can be solved through the a stable inversion based feedforward approach ([]), which is based on a Picard iteration of a suitable nonlinear operator. This method has led also to a preview-based approach ([]), that requires only a finite preview time of the output trajectory. A different perspective for stable inversion is presented in [3], based on a stable/unstable decomposition and the sequential integration of the stable and unstable subsystems in forward and backward time. Another possible approach consists in considering a path-following setting ([4]) which allows an extra degree of freedom for controlling internal dynamics. In this paper we present another approach to stable inversion. Differently from the methods above, it is not based on Picard iterations but on homotopy. Essentially, a bounded solution for the internal dynamics equation associated to a generic reference T -periodic trajectory is obtained through continuous deformation of a known bounded solution associated to a particularly simple trajectory. We have already used this method to face the exact tracking problem for some well-known nonminimum phase systems with two dimensional internal dynamics such as the VTOL (see [5]), the planar inverted pendulum (see [6]), the motorcycle and the CTOL aircraft (see[7]). This approach has allowed us finding a precise characterization of the class of trajectories for which the exact tracking problem has a solution and precise bounds on the internal dynamics norm. This paper extends this method to nonminimum phase systems with general n-dimensional internal dynamics, finding results analogous to the -dimensional case. In this paper the method has been developed for systems with T -periodic internal dynamics. The main result (Theorem ) has in common with Theorem 3 of [3] the idea of decomposing system dynamics in stable and unstable components. The main difference is that we do not need a global small gain hypothesis on the product of the stable and unstable subsystems gains. Instead, we require a similar hypothesis only in a bounded subset that grows with respect to the parameter s. This subset, for s =, represents a region that contains the trajectories of the internal dynamics. As motivating example we consider the exact tracking problem for the spherical inverted pendulum. Remark that its internal dynamics do not satisfy the hypotheses of Theorem 3 of [3]. This same problem has been considered in detail in [8], where we have proposed a method based on an analytical condition that is related to the solution of a differential equation associated to a given reference trajectory (see equation (5) of Theorem of [8]). In this paper, through the use of Theorem, we complete that analysis, obtaining sufficient conditions for exact tracking and finding bounds on the norm of the internal dynamics. More precisely, we show that it is possible to determine a constant k (that depends on the pendulum length) such that if γ k then it is possible to find initial conditions on the internal dynamics such that the pendulum follows exactly the assigned curve without overturning, finding a precise bound on pendulum maximum oscillations. The following notations will be used: R + = {x x }; a, b R, a b = min{a, b}, a b = max{a, b} and [a, b] = {x R a x b}, ]a, b[= {x R a < x < b}; θ [, π[, τ(θ) = (cos θ, sinθ) T ; x R, argx = θ, where θ [, π[ is such that x = x τ(θ); x, y R 3, x y denotes the vector cross product; x = (x,...,x n ) T, y = (y,..., y n ) T R n, x, y = n i= x iy i, x = x, x if I is a real interval, f : I R n, f = sup x I { f(x) }; for any matrix A = (a ij ) i=,...,n, j=,...,m, A = { n m } i= j= a ij is the Frobenius norm, and, if n = m, A S = /(A + A T ) denotes the symmetric part of A, while λ(a S ) and λ(a S ) denote the associated maximum and minimum eigenvalues. 978--444-454-/9/$5. 9 AACC 4

I. THE HOMOTOPY METHOD FOR THE FEEDFORWARD EXACT TRACKING PROBLEM Consider a nonlinear affine system of form ẋ = F(x) + G(x)u(t) y(t) = H(x), where x(t) R n, u(t) R m, y(t) R p and F, G, H are vector function of appropriate dimensions and F() =. Given a sufficiently regular curve γ : [, T] R n, the feedforward exact tracking problem consists in finding an initial state x() and a control function u(t) such that y(t) = γ(t), t [, T]. If the system has a well-defined relative degree and functions F(x) and G(x) are sufficiently regular, then, after a change of coordinates, it is possible to rewrite () in the following normal form (for the derivation and the details the reader may refer to Isidori s book ([9]), ξ,i = ξ,i. ξ ri,i = α i (ξ, η) + β i (ξ, η)u(t) for i =,...,m, where ξ = (ξ j,i ) = y (i) j, j =,...,m, i =,...,r j and () () η = γ(η, ξ) + δ(η, ξ)u(t), (3) when exact tracking is desired we set y(t) = γ(t), which implies ξ j,i = γ (i) j, j =,...,m, i =,...,r j. (4) Because of the hypothesis of well defined relative degree, the control u(t) can be expressed as a function of γ(t) and its derivatives, therefore (3) takes the form: η = f(η, Γ(t)), t [, T], (5) where Γ(t) = (γ (i) j (t)), j =,...,m, i =,...,r j, this last equation is called the internal dynamics equation. As usual, the problem to be faced is to find an initial condition η() such that the solution of system (5) is sufficiently small. This is particularly challenging when the origin is an unstable equilibrium, especially of hyperbolic type, as in the case of the inverted spherical pendulum considered in section V). First of all, it is not restrictive to suppose that γ is T -periodic. The homotopy approach consists in introducing the following family of differential systems η = f(η, sγ(t)), (6) depending on the parameter s [, δ[, (δ > ) and in regarding (5) as the form that family (6) assumes for s =. Applied in this context, Theorem provides sufficient conditions on f that guarantee the existence of δ > and a curve φ (defined on [, δ[) of initial data of T -periodic solutions for family (6). It provides also, by means of (3), an L norm estimate of the solution. Therefore, if δ >, the desired T -periodic solution of (5) is obtained taking the solution of (6) for s =, in correspondence to the initial data η() = φ(). The solution of (5) is obtained trough a continuous deformation until s = of a known periodic solution for s =. In the case of affine non linear systems, since f(, ) =, this solution is the constant null solution. In fact, when s =, function sγ(t) collapses to the origin, which is an equilibrium point. II. MAIN THEOREM Definition : Let Ω be an open subset of R n, δ > and F : R [, δ[ Ω R n (t, s, x) F(t, s, x), be a C map. For every (τ, s, y) R [, δ[ Ω, let x(t, τ, s, y) be the solution defined on its maximal interval of existence of system { ẋ = F(t, s, x) (7) x(τ) = y. Definition : If x : [, T] R n is a map and ρ, set x([, T]) ρ = {y R n t [, T] : y x(t) < ρ}. Theorem (Main theorem): Let Ω be an open subset of R n and F : R [, δ[ Ω R n (t, s, x) F(t, s, x), be a C map such that the following hypotheses are verified: a) (s, x) [, δ[ Ω the map t F(t, s, x) is T - periodic. b) there exists a T -periodic map x C (R, Ω), such that: x(t) = F(t,, x(t)), t R, (8) c) Set A(t, s, x) = x F(t, s, x), B(t, s, x) = s F(t, s, x) and suppose that there exists k : k < n such that, taking into account the following block decomposition of A(t, s, x) ( ) A (t, s, x) A A(t, s, x) = (t, s, x), A (t, s, x) A (t, s, x) where A (t, s, x) R k k, A (t, s, x) R k (n k), A (t, s, x) R (n k) k, A (t, s, x) R (n k) (n k), there exist smooth functions λ (s, ρ), λ (s, ρ), a (s, ρ), a (s, ρ), b(s, ρ) defined on R + R +, non decreasing in the ρ variable, such that λ(a S (t, s, x)) λ (s, ρ), λ(a S (t, s, x)) λ (s, ρ), A (t, s, x) a (s, ρ), A (t, s, x) a (s, ρ), B(t, s, x) b(s, ρ), s, ρ : x([, T]) ρ Ω, t R, x R n : x x(t) ρ, (9) where the bar denotes the closure of the set. d) Set D = { (s, ρ) R + R + λ (s, ρ) < λ (s, ρ), a (s, ρ)a (s, ρ) < (λ (s, ρ) λ (s, ρ)), } σ(s, ρ) <, α (s, ρ) < < α (s, ρ) 4

and let ψ : D R + be the function defined by ψ(s, ρ) = + σ(s, ρ) σ(s, ρ) b(s, ρ) α (s, ρ) ( α (s, ρ)), () where σ(s, ρ) = (a (s, ρ) a (s, ρ)) { (λ (s, ρ) λ (s, ρ))+ (λ (s, ρ) λ (s, ρ)) 4a (s, ρ)a (s, ρ) }, α (s, ρ) = λ (s, ρ) σ(s, ρ)a (s, ρ), α (s, ρ) = λ (s, ρ) σ(s, ρ)a (s, ρ). Suppose that (, ) D and let [, δ[ be the right-maximal interval of existence such that { ρ(s) = ψ(s, ρ(s)), s [, δ[ () ρ() = and and x([, T]) ρ(s) Ω, s [, δ[. () Then there exists a unique φ C ([, δ[, R n ) such that φ() = x() x(t,, s, φ(s)) = φ(s), s [, δ[, x(t,, s, φ(s)) x(t) ρ(s), s [, δ[, (3) in other words φ is the curve of initial values of T -periodic solutions of the family of systems {ẋ = F(t, s, x)} s [,δ[ such that φ() = x(). Proof: We want to apply Theorem. It remains only to show that its hypothesis c) is satisfied. By hypothesis d), let ρ(s) be the solution of system () defined on its rightmaximal interval of existence [, δ[ such that () holds. Let ρ = sup ρ [,δ[ {ρ(s)}. Then x([, T]) ρ Ω and (7) holds if we take as ψ in c) the function given by (). We want to show that (8) holds too. Set s [, δ[, since {(s, ρ(s)) s [, s]} is a compact subset of D, we can find an ǫ > such that (s, ρ(s) + ǫ) D, s [, s] which implies that (s, ρ) D, s [, s], ρ : ρ ρ(s) + ǫ, (4) since D has the property that if (s, ρ) D, then (s, ρ) D, ρ s, being λ (s, ρ), λ (s, ρ), a (s, ρ), a (s, ρ), b(s, ρ) non decreasing functions of ρ. Let τ [, T], s [, s], y R n be such that (s, y) Ω, t x(t, τ, s, y) is T -periodic and x(t, τ, s, y) x(t) ρ(s) + ǫ, t [, T]. Let us call, t [, T] A(t) = x F(t + τ, s, x(t + τ, τ, s, y)), B(t) = s F(t + τ, s, x(t + τ, τ, s, y)). Since (s, max t T x(t, τ, s, y) x ) D by (4), by hypotheses (9), we deduce immediately that hypotheses (), () of Theorem 3 are verified for matrix A(t). Then det(i Φ y s(t + τ, τ)) and () implies that (I Φ y s (T + τ, τ)) T+τ ψ(s, max t T τ Φ y s (T + τ, p)b(p)dp x(t, τ, s, y) x(t) ). Therefore (8) holds and Theorem can be applied. III. AN HOMOTOPY THEOREM Definition 3 (Variation equations): Set Φ y s (t, τ) the solution of the homogeneous linear system { Φ = x F(t, s, x(t, τ, s, y))φ (5) Φ(τ) = I, where I is the n-dimensional identity matrix. In the previous notation, the following theorem is a result of existence of periodic solutions for the family of systems (7) depending on parameter s. The following Theorem holds. Theorem : Let Ω be an open subset of R n and F : R [, δ[ Ω (t, s, x) F(t, s, x), be a C map such that the following hypotheses are verified: a) (s, x) [, δ[ Ω the map t F(t, s, x) is T - periodic, b) there exists a T -periodic map x C (R, Ω), such that: R n x(t) = F(t,, x(t)), t R, (6) c) there exist δ, ρ > such that x([, T]) ρ Ω and a locally lipschitz function ψ : [, δ[ [, ρ [ R +, non decreasing as function of ρ, such that the following system can be solved on [, δ[ { ρ(s) = ψ(s, ρ(s)), s [, δ[ (7) ρ() =, and the following property holds: s [, δ[, ǫ > with the property that if τ [, T], s [, s], y R n are such that (s, y) Ω, t x(t + τ, τ, s, y) is T -periodic and x(t, τ, s, y) x(t) ρ(s) + ǫ, t [, T] then det(i Φ y s (T + τ, τ)) and (I Φ y s (T + τ, τ)) T+τ Φ y τ s (T + τ, p) s F(p, s, (p, τ, s, y))dp ψ(s, max t T x(t, τ, s, y) x(t) ). (8) Then there exists and is unique the map φ C ([, δ[, R n ) such that φ() = x() (which implies that x(t,,, φ()) = x(t)), x(t,, s, φ(s)) = φ(s), (9) x(t,, s, φ(s)) x(t) ρ(s), (t, s) [, T] [, δ[, in particular x(t,, s, φ(s)), s < δ is the only T -periodic solution of system (7) contained in x([, T]) ρ such that φ() = x(). 43

m l ζ M x e 3 Fig.. Spherical pendulum constrained to follow a given periodic γ in the space. IV. SOME PROPERTIES OF HYPERBOLIC LINEAR SYSTEMS The following theorem holds, it gives a characterization for solutions of linear hyperbolic systems with T -periodicity conditions. Theorem 3: Let k be an integer: k < n and A C([, T], R n n ), A C([, T], R k k ), A C([, T], R k (n k) ), A C([, T], R (n k) k ), A C([, T], R (n k) (n k) ) be such that and set A(t) = ( A (t) A (t) A (t) A (t) f γ ), t [, T], a = sup t T { A (t) }, a = sup t T { A (t) } λ = inf t T {λ(a S (t)}, λ = sup t T {λ(a S (t)}. Suppose that where σ = λ < λ, a a < (λ λ ) () σ <, α < < α () (a a ) (λ λ )+ (λ λ ) 4a a, and α = λ σa, α = λ + σa. Then (I Φ(T, )) is invertible and if B C([, T], R n ), the solution x of the following boundary problem: { ẋ(t) = A(t)x(t) + B(t), t [, T] x() = x(t), is the unique solution of the following initial value problem { ẋ(t) = A(t)x(t) + B(t), t [, T] and x() = (I Φ(T, )) T x() + σ σ Φ(T, τ)b(τ)dτ, B α α. () V. AN APPLICATION: EXACT TRACKING PROBLEM FOR THE SPHERICAL PENDULUM Consider a spherical inverted pendulum of mass m linked to a moving base of mass M through a massless rod of length l, in Figure the pendulum is represented as the smaller sphere and the base as the bigger one. It is supposed that during the motion the force f R 3 is applied on the center of mass x of M. The problem we want to solve is the following one: given an arbitrary (not necessarily plane) T -periodic curve γ C 3 (R, R 3 ), we want to find a control force f C(R, R 3 ), applied to the point x, such that if x() = γ(), then x(t) = γ(t), t and ζ e 3 is sufficiently small, where e 3 = (,, ) T. In other words, if at the initial time x() = γ(), then x follows all the curve γ and the rod remains close to the vertical without overturning. Moreover we want to find bounds on ζ e 3 that reduce with γ maximum acceleration. As shown in Section 3 of [8], through the homotopy approach, this problem can be restated in the following form. Problem : Find initial conditions z, w, ż, ẇ such that the following family of differential systems has a T -periodic solution for s = ( ) ( ) ( ) z z g z = ẅ w l w [(ż + ẇ + (żz+ẇw) z w ) + l g( z w + ] ( ) +sl (z γ + w γ + γ 3 (z + w )) sl γ γ z() = z, w() = w ż() = ż, ẇ() = ẇ. (3) Moreover, find a non decreasing function k (with k() = ) such that (z, w) k( γ ). (4) Equation (3) can be written in the form: { ẏ = F(t, s, y) (5) y() = y, where y = (z, w, ż, ẇ), y = (z, w, ż, ẇ ) and F : R R Ω R 4, (t, s, y) F(t, s, y), is given by y 3 y F(t, s, y) = 4 d y y h(t,s, y) s d g γ d y y h(t,s, y) s d g γ moreover h(t, s, y) = y3 + y4 + (y y 3 + y y 4 ) (y + y ) + d ( (y + y )) + + s ( y γ + y γ + γ 3 (y g + y )), where d = gl, Ω = (B R ), with B = {(z, w) R (z, w) < }. Remark that for any (t, s, y) R R Ω s F(t, s, y) = y d g ( y γ + y γ + γ 3 (y + y )) d g γ y d g ( y γ + y γ + γ 3 (y + y )) d g γ y F(t, s, y) = d + h + y y h y y h y y 3 h y y 4 h y y h d + h + y y h y y 3 h y y 4 h, 44

therefore y F(t,,, ) = d d, which has the following eigenvalues (d, d, d, d) and eigenvectors v = d, v = d, v 3 = set V = (v, v, v 3, v 4 ), then y F(t, s, y) = d d + e e e 3 e 4 f d + f f 3 f 4, v 4 = d, = ( y F) + ( y F) where e i, f i are defined consequently. Setting ξ = (y, y ) and η = (y 3, y 4 ), the following bounds hold h η + ξ η ξ + d ( ξ ) + d g s γ = η ξ + ξ d + ξ + d g s γ. y h = y3( (y + y))(y y 3 + y y 4) + y (y y 3 + y y 4) ( (y + + y )) d y (y + y ) + sd g, γ y (y +y ) η ( ξ )( ξ η ) + ξ ξ η ( ξ ) + d ξ ξ +s d g γ ξ, and the same bound holds for y h. Moreover it is y3 h = (y 3 + y (y y 3 + y y 4 ) (y + y ) η ( + ξ ) ξ, and the same bound holds for y4 h. Summarizing the previous computations it follows that t R, s R, ξ = (y, y ), η = (y 3, y 4 ) : (ξ, η) Ω that e, f h + ( y y h y y h ) dφ ( ξ, η, s, γ, d), e, f y y h y y h dφ ( ξ, η, γ, d), e 3, e 4, f 3, f 4 φ 3 ( ξ, η ), where φ, φ, φ 3 are strictly increasing functions in their arguments, consequently defined. Now if we express the matrix x F with respect to the basis {v, v, v 3, v 4 }, then A(t, s, x) = V x FV = A (t, s, x) + A (t, s, x), (6) where A = d d d d, A = d e + e 3 d e + e 4 d e e 3 d e e 4 d f + f 3 d f + f 4 d f f 3 d f f 4 (d e + e 3 ) (d e + e 4 ) (d e e 3 ) (d e e 4 ) (d f + f 3 ) (d f + f 4 ) (d f f 3 ) (d f f 4 ) Set φ 4 (s, ξ, η, γ, d) = (φ + φ ) + (φ + φ 3 ),. then by the previous computations, the following bounds hold λ(a ) d φ 4, λ(a ) d + φ 4, A, A φ 4. (7) If we make the change of coordinates y = V x then system (5) becomes ẋ = V F(t, s, V x) = F(t, s, x). We want to show that this system verifies the hypotheses of Theorem. Clearly a) and b) of Theorem are satisfied since γ is T -periodic and it is sufficient to take x(t) =, t R, since F(t,, ) =. Moreover remark that x F = A(t, s, x) given by (6) and (9) is verified by (7) if we set λ (s, ρ) = d χ(s, ρ, γ ), λ (s, ρ) = d + χ(s, ρ, γ ), a (s, ρ) = a (s, ρ) = χ(s, ρ, γ ), b(s, ρ) = g γ, where χ(s, ρ, γ ) = φ 4 (s, ρ, dρ, γ ). Remark that (, ) D an let [, δ( γ )[ be such that system () is satisfied and () holds. It is possible to see that there exists k > such that δ( γ ), γ : γ k. Then by Theorem there exists a unique φ C ([, δ[, R n ) such that the solution x(t,, s, φ(s)) of system { ẋ = F(t, s, x) are T -periodic and x() = φ(s), x(t,, s, φ(s)) ρ(s, γ ). It is possible to see that there exists k, such that k > : δ( γ ), γ with γ k. Then problem ) for the inverted pendulum is solvable γ : γ k and t R (z, w) (ż, ẇ), d x(t,,, φ()) ρ(, γ ), therefore function k in (4) is given by ρ(, γ ). Figure shows the value of the bound on internal dynamics x as functions of γ for different values of d = g l, with g = 9.8. Each line corresponds to a different value of d, which varies from to. Each line ends at a value of γ which represents the maximum curve acceleration for which Theorem guarantees the existence of a T -periodic solution for s = for the pendulum internal dynamics (z, w). For example, if d =, then the method proposed here guarantees the tracking of all curves γ with γ 5.6. Moreover for all curves with γ 5.6, the method guarantees that (z, w) ρ(, 5.6).77 and (ż, ẇ) d ρ(, 5.6).77. VI. CONCLUSIONS In this paper we have presented sufficient conditions for the application of the homotopy method to the exact tracking problem for nonminimum phase nonlinear systems. It extends to nonminimum-phase systems with n-dimensional internal dynamics the results already presented in [5], [6] and [7] for the two dimensional case. This method allows 45

ρ(, γ ).5. d =.5 d =..5 d = 5 d = 3 4 5 6 7 8 9 γ Fig.. Bounds on the spherical pendulum internal dynamics with respect to γ and d. finding precise bounds on internal dynamics that depends on the curve γ that has to be exactly tracked. We have applied these result to the exact tracking problem for the inverted spherical pendulum, showing that it is possible to determine a constant k (that depends on the pendulum length) such that if γ k then it is possible to determine initial conditions on the internal dynamics such that the pendulum follows exactly the assigned curve without overturning, finding a precise bound on pendulum maximum oscillations. REFERENCES [] S. Devasia, D. Chen, and B. Paden, Nonlinear inversion-based output tracking, IEEE Transactions on Automatic Control, vol. AC-4, no. 7, pp. 93 94, July 996. [] Q. Zou, Optimal preview-based stable-inversion for output tracking of nonminimum-phase linear systems, Automatica, vol. 45, no., pp. 3 37, 9. [3] A. Pavlov and K. Pettersen, Stable inversion of non-minimum phase nonlinear systems: A convergent systems approach, Decision and Control, 7 46th IEEE Conference on, pp. 3995 4, Dec. 7. [4] C. Nielsen, L. Consolini, M. Maggiore, and M. Tosques, Path following for the pvtol: A set stabilization approach, Dec. 8, pp. 584 589. [5] L. Consolini and M. Tosques, On the vtol aircraft exact tracking with bounded internal dynamics via a poincarè map approach, IEEE Trans. On Automatic Control, vol. 5, no. 9, pp. 757 76, 7. [6], On the existence of small periodic solutions for the - dimensional inverted pendulum on a cart, SIAM Journal on Applied Mathematics, vol. 68, no., pp. 486 5, 7. [7], A homotopy method for exact output tracking of some nonminimum phase nonlinear control systems, Int. J. Robust Nonlinear Control, In press, published online. [8], On the exact tracking of the spherical inverted pendulum via an homotopy method, Systems & Control Letters, vol. 58, no., pp. 6, 9. [9] A. Isidori, Nonlinear Control Systems. Springer-Verlag New York, Inc., 995. 46