Autonomous Helicopter Landing A Nonlinear Output Regulation Perspective

Autonomous Helicopter Landing A Nonlinear Output Regulation Perspective Andrea Serrani Department of Electrical and Computer Engineering Collaborative Center for Control Sciences The Ohio State University Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.1/36

Introduction Landing an autonomous helicopter on a vessel undergoing an unknown vertical motion The vessel oscillates vertically, but the parameter of its motion (frequencies, amplitudes and phases) are unknown Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.2/36

Introduction The problem: tracking of unknown trajectories and/or rejection of unknown disturbances The technical approach: a feedback control that incorporates an internal model" of the exogenous inputs Main features: Continuous-time adaptation of the internal model Stabilization based on combined low-amplitude / high-gain feedback Guaranteed convergence and robustness Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.3/36

Helicopter model - rigid body M p i = Rf b Ṙ = RS(ω b ) J ω b = S(ω b )Jω b + τ b q 0 = 1 2 qt ω b q = 1 2 [q 0I + S(q)]ω b p i R ω b f b τ b position of the center of mass (inertial frame) rotation matrix, (q 0,q) unit quaternions angular velocity (body frame) external force (body frame) external torque (body frame) Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.4/36

Helicopter model - control inputs a T M b T M T T T M T T a b g g Main thrust Tail thrust Longitudinal deflection of the rotor plane Lateral deflection of the rotor plane Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.5/36

Full model f b = X M Y M + Y T Z M + R T 0 0 Mg Main / tail rotors gravity τ b = τ f1 τ f2 τ f3 + R M M M + M T N M Aerodynamic forces Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.6/36

Full model X M = T M sina R M = c M b b Q M sin a Z M = T M cosacos b M M = c M a a + Q M sinb Y M = T M sinb N M = Q M cosacos b Y T = T T M T = c Q T TT 1.5 D Q T Q M = c Q M TM 1.5 + D Q M τ f1 τ f2 τ f3 = Y M h M + Z M y M + Y T h T = X M h M + Z M l M = Y M l M Y T l T Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.7/36

Full model: structure M p i = Rf b R f b g Force and torque generation process T M T T a b Ṙ = RS(ω b ) J ω b = S(ω b )Jω b + τ b τ b Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.8/36

Simplified model We let sin(a) = a, sin(b) = b, cos(a) = cos(b) = 1 neglect the contribution of T M and T T along x b, y b f b = approximate τ b with 0 0 T M + R T 0 0 Mg τ b (v) = A(T M )v + B(T M ), v := col(a,b,t T ) Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.9/36

Simplified model: structure The simplified model neglects the weak couplings in the force/moment generation mechanism M p i = Rf b f b g Force generation T M R Ṙ = RS(ω b ) J ω b = S(ω b )Jω b + τ b τ b T M Torque generation T T a b Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.10/36

Simplified model Since inertial and aerodynamic parameters are uncertain, M = M 0 + M, J = J 0 + J A(T M ) = A 0 (T M ) + A (T M ) B(T M ) = B 0 (T M ) + B (T M ) All uncertain parameters are collected into a vector where P is a compact set. µ = µ 0 + µ, µ P IR p Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.11/36

Problem statement Control objective: Synchronization of z(t) with the vertical motion z (t) of the ship, at a given safety distance H. Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.12/36

Problem statement Control objective: Synchronization of z(t) with the vertical motion z (t) of the ship, at a given safety distance H. Smooth landing, H 0, while keeping the center of mass on target and the orientation constant. Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.12/36

Problem statement Control objective: Synchronization of z(t) with the vertical motion z (t) of the ship, at a given safety distance H. Smooth landing, H 0, while keeping the center of mass on target and the orientation constant. Reference trajectory: (x ref (t),y ref (t),z ref (t)) = (0, 0,H + z (t)), R ref (t) = I Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.12/36

Problem Statement The motion z (t) is modeled as the sum of a fixed number of sinusoidal signals z (t) = N i=1 A i cos(ω i t + ϕ i ) of unknown amplitude, phase and frequency (A i,ϕ i, Ω i ), i = 1,...,N Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.13/36

Problem Statement The problem fits naturally in the framework of nonlinear output regulation theory, as z ref (t) is generated by Ḣ = 0 exosystem ẇ = S(ϱ)w z ref (t) = H + r(w) where ϱ = col(ω 1,...,Ω N ), r(w) = Qw ( 0 Ωi S(ϱ) = diag(s 1,...,S N ), S i = Ω i 0 ) Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.14/36

Problem Statement Given any (arbitrarily small) δ > 0, Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.15/36

Problem Statement Given any (arbitrarily small) δ > 0, design a controller η = ϕ(η,e,ė,q,ω b ) T M = ψ TM (η,e,ė,q,ω b ) v = ψ v (η,e,ė,q,ω b ) where e := (x, y, z z ref ), Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.15/36

Problem Statement Given any (arbitrarily small) δ > 0, design a controller η = ϕ(η,e,ė,q,ω b ) T M = ψ TM (η,e,ė,q,ω b ) v = ψ v (η,e,ė,q,ω b ) where e := (x, y, z z ref ), such that lim z(t) t zref (t) = 0, e(t) δ, q(t) δ, t T Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.15/36

Problem Statement Given any (arbitrarily small) δ > 0, design a controller η = ϕ(η,e,ė,q,ω b ) T M = ψ TM (η,e,ė,q,ω b ) v = ψ v (η,e,ė,q,ω b ) where e := (x, y, z z ref ), such that lim z(t) t zref (t) = 0, e(t) δ, q(t) δ, t T with a semi-global domain of attraction, for all µ P. Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.15/36

Controller structure The vertical error dynamics is stabilized wrapping a nonlinear regulator around T M and e z (t), ė z (t) Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.16/36

Controller structure The vertical error dynamics is stabilized wrapping a nonlinear regulator around T M and e z (t), ė z (t) The design of the regulator is non-standard, since the natural frequencies of the exosystem are unknown The lateral, longitudinal, and attitude dynamics are stabilized using v as input. Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.16/36

Controller structure The vertical error dynamics is stabilized wrapping a nonlinear regulator around T M and e z (t), ė z (t) The design of the regulator is non-standard, since the natural frequencies of the exosystem are unknown The lateral, longitudinal, and attitude dynamics are stabilized using v as input. Note that we need to stabilize a 10-dimensional system using a 3-dimensional input. Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.16/36

Controller structure The vertical error dynamics is stabilized wrapping a nonlinear regulator around T M and e z (t), ė z (t) The design of the regulator is non-standard, since the natural frequencies of the exosystem are unknown The lateral, longitudinal, and attitude dynamics are stabilized using v as input. Note that we need to stabilize a 10-dimensional system using a 3-dimensional input. The two subsystems are not decoupled! Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.16/36

Controller structure A T M Adaptive regulator Vertical dynamics e z (T M,e z ) q Robust stabilizer v (e x, e y, q) Lateral, longitudinal, attitude dynamics B Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.17/36

Vertical error dynamics M z = (1 2q 2 1 2q 2 2) }{{} this may vanish T M + Mg Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.18/36

Vertical error dynamics choosing T M as M z = (1 2q 2 1 2q 2 2) }{{} this may vanish T M + Mg T M = gm 0 u 1 sat c (2q 2 1 + 2q 2 2), 0 < c < 1 external control Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.18/36

Vertical error dynamics choosing T M as M z = (1 2q 2 1 2q 2 2) }{{} this may vanish T M + Mg T M = gm 0 u 1 sat c (2q 2 1 + 2q 2 2), 0 < c < 1 external control we get M z = φ z c (q)u + g[m M 0φ z c (q)] = u + gm, if q is small! Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.18/36

Vertical error dynamics If q(t) is kept small so that φ z c(q(t)) 1, the input u needed to keep z(t) z ref (t) is u ss (w,µ) = M r(w) gm = MQS 2 (ϱ)w gm Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.19/36

Vertical error dynamics If q(t) is kept small so that φ z c(q(t)) 1, the input u needed to keep z(t) z ref (t) is u ss (w,µ) = M r(w) gm = MQS 2 (ϱ)w gm The control u ss (w,µ) is generated by the internal model where τ S(ϱ)w = Φ(ϱ)τ(w,µ) w u ss = Γ(ϱ)τ(w,µ) τ(w,µ) = ( gm Mw ) Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.19/36

Design of the internal model The internal model (Φ(ϱ), Γ(ϱ)) can not be used as such, as ϱ is unknown: we need a realization that is suitable for adaptation. Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.20/36

Design of the internal model The internal model (Φ(ϱ), Γ(ϱ)) can not be used as such, as ϱ is unknown: we need a realization that is suitable for adaptation. Let F 2 IR 2N 2N be Hurwitz, and G 2 IR 2N 1 be such that (F 2,G 2 ) is controllable. Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.20/36

Design of the internal model The internal model (Φ(ϱ), Γ(ϱ)) can not be used as such, as ϱ is unknown: we need a realization that is suitable for adaptation. Let F 2 IR 2N 2N be Hurwitz, and G 2 IR 2N 1 be such that (F 2,G 2 ) is controllable. Then, there exists H 2 IR 1 2N such that the pair F = 0 H 2 G 2 F 2, G = 0 G 2 Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.20/36

Design of the internal model The internal model (Φ(ϱ), Γ(ϱ)) can not be used as such, as ϱ is unknown: we need a realization that is suitable for adaptation. Let F 2 IR 2N 2N be Hurwitz, and G 2 IR 2N 1 be such that (F 2,G 2 ) is controllable. Then, there exists H 2 IR 1 2N such that the pair F = 0 H 2 G 2 F 2 is controllable, and F is Hurwitz., G = 0 G 2 Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.20/36

Design of the internal model Then, for any ϱ IR N, there exists Ψ 2,ϱ IR 1 2N such that Ψ ϱ = (1 Ψ 2,ϱ ) assigns the eigenvalues of Φ(ϱ) to F + GΨ ϱ, Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.21/36

Design of the internal model Then, for any ϱ IR N, there exists Ψ 2,ϱ IR 1 2N such that Ψ ϱ = (1 Ψ 2,ϱ ) assigns the eigenvalues of Φ(ϱ) to F + GΨ ϱ, hence (Φ(ϱ), Γ(ϱ)) is similar to (F + GΨ ϱ, Ψ ϱ ) Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.21/36

Design of the internal model Then, for any ϱ IR N, there exists Ψ 2,ϱ IR 1 2N such that Ψ ϱ = (1 Ψ 2,ϱ ) assigns the eigenvalues of Φ(ϱ) to F + GΨ ϱ, hence (Φ(ϱ), Γ(ϱ)) is similar to (F + GΨ ϱ, Ψ ϱ ) The advantage is that the uncertainties are now lumped in Ψ ϱ. Φ(ϱ) Γ(ϱ) G F Ψ ϱ Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.21/36

Design of the regulator We replace Ψ ϱ by an estimate ˆΨ = (1 ˆΨ 2 ), and implement the adaptive internal model-based regulator ξ = (F + GˆΨ)ξ + Gu st d dt ˆΨ 2 = γ 1 ξ T 2 u st, γ > 0 u = ˆΨξ + u st with ξ = col(ξ 1, ξ 2 ) IR IR 2N, where the stabilizing control u st is selected as the high-gain feedback u st = k 2 (ė z + k 1 e z ), k 1,k 2 > 0. Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.22/36

Regulator structure adaptive internal model + u saturated control T M vertical error dynamics e z u st high-gain stabilizer The dynamic regulator yields boundedness of all internal variables. It steers asymptotically the vertical error to zero, only if the attitude error is kept sufficiently small. Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.23/36

Regulator structure adaptive internal model + u saturated control T M vertical error dynamics e z u st high-gain stabilizer The dynamic regulator yields boundedness of all internal variables. It steers asymptotically the vertical error to zero, only if the attitude error is kept sufficiently small. We need finite-time convergence of q(t) to a small ball Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.23/36

Lateral/ longitudinal dynamics The choice of T M to regulate the vertical error dynamics affects the lateral and longitudinal dynamics as well. ẋ = x 2 Mẋ 2 = d(t)q 0 q 2 + m(q,t)q 1 q 3 + n x (q)y z (e z,w) ẏ = y 2 Mẏ 2 = d(t)q 0 q 1 + m(q,t)q 2 q 3 + n y (q)y z (e z,w) The dynamics are time-varying due to the exogenous system, and perturbed by e z (t) and w(t). Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.24/36

Lateral/ longitudinal dynamics vanishes asymptotically if q is small closed-loop vertical error dynamics y z Mẏ 2 = d(t)q 0 (t)q 1 + m(q, t)q 2 q 3 + +n y (q)y z Mẋ 2 = d(t)q 0 (t)q 2 + m(q, t)q 1 q 3 + +n x (q)y z (q,q 0 ) q T M q = 1 2 [q 0I + S(q)]ω b J ω b = S(ω b )Jω b + +A(T M )v + B(T M ) v Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.25/36

Multi-objective control The only DOF left is the choice of v, which must accomplish the following tasks: Robustly stabilize the attitude dynamics, sending q(t) in a neighborhood of the origin in finite time Render the lateral/longitudinal dynamics Input-to-State stable with respect to the disturbance induced by the vertical dynamics Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.26/36

Multi-objective control The only DOF left is the choice of v, which must accomplish the following tasks: Robustly stabilize the attitude dynamics, sending q(t) in a neighborhood of the origin in finite time Render the lateral/longitudinal dynamics Input-to-State stable with respect to the disturbance induced by the vertical dynamics Stabilize the interconnected subsystem (lat./long./attitude) Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.26/36

Multi-objective control T M y z (e z,w) v q = 1 2 [q 0I + S(q)]ω b J ω b = S(ω b )Jω b + +A(T M )v + B(T M ) q Mẏ 2 = d(t)q 0 (t)q 1 + Mẋ 2 = d(t)q 0 (t)q 2 + In principle, we could use q as a virtual control for the lateral/long. dynamics, but the system is not in feedback form. However, the lateral/long. dynamics is in feedforward form and the attitude dynamics is in strict feedback form Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.27/36

Multi-objective control T M y z (e z,w) v q = 1 2 [q 0I + S(q)]ω b J ω b = S(ω b )Jω b + +A(T M )v + B(T M ) q Mẏ 2 = d(t)q 0 (t)q 1 + Mẋ 2 = d(t)q 0 (t)q 2 + In principle, we could use q as a virtual control for the lateral/long. dynamics, but the system is not in feedback form. However, the lateral/long. dynamics is in feedforward form and the attitude dynamics is in strict feedback form Use a combined high/gain - low/amplitude control to induce a time-scale separation between the two subsystems. Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.27/36

Next step: attitude dynamics Since τ b (v) = A(T M )v + B(T M ), Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.28/36

Next step: attitude dynamics Since τ b (v) = A(T M )v + B(T M ), choose v = A 1 0 (T M )[ṽ B 0 (T M )] Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.28/36

Next step: attitude dynamics Since τ b (v) = A(T M )v + B(T M ), choose where v = A 1 0 (T M )[ṽ B 0 (T M )] ṽ = K 4 (ω + K 3 q) }{{} + K 4K 3 u 2 high-gain feedback low-amplitude, u 2 (t) λ 2 Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.28/36

Next step: attitude dynamics Since τ b (v) = A(T M )v + B(T M ), choose where v = A 1 0 (T M )[ṽ B 0 (T M )] ṽ = K 4 (ω + K 3 q) }{{} + K 4K 3 u 2 high-gain feedback low-amplitude, u 2 (t) λ 2 The control u 2 will be designed to stabilize the lateral dynamics Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.28/36

Attitude dynamics: main result It can be shown that, for any compact set of initial conditions for (q(t),ω(t)), and for any T > 0 there exists a choice of K 3 > 0, K 4 > 0 and λ 2 > 0 such that: The trajectory (q(t),ω(t)) is bounded for all t 0, and q 0 (t) does not change sign φ z c(q(t)) = 1 for all t T. Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.29/36

Attitude dynamics: main result It can be shown that, for any compact set of initial conditions for (q(t),ω(t)), and for any T > 0 there exists a choice of K 3 > 0, K 4 > 0 and λ 2 > 0 such that: The trajectory (q(t),ω(t)) is bounded for all t 0, and q 0 (t) does not change sign φ z c(q(t)) = 1 for all t T. Hence, q(t) is brought in finite time in a neighborhood of the origin. This is already enough to conclude that lim z(t) t zref (t) = 0. Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.29/36

Putting everything together Now it s time to design the bounded control u 2 to stabilize the interconnection of the attitude and the lateral/long. dynamics. We will use q 1 and q 2 as virtual controls" for y and x respectively. To remove drifts, we augment the dynamics with the bank of integrators η x = x, η y = y, η q = q 3 and introduce smooth vector saturation functions σ(s): σ (s) := dσ(s)/ds 2 s, sσ(s) > 0 s 0, σ(0) = 0. σ(s) = sgn(s) for s 1. s < σ(s) < 1 for s < 1. Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.30/36

Putting everything together Define new state variables as ζ 0 := η y η x, ζ 1 := y x + λ 0 σ( K 0 λ 0 ζ 0 ) ζ 2 := y 2 x 2 + λ 1σ( K 1 λ 1 ζ 1 ) η q and choose the nested saturation" control u 2 = λ 2 σ( K 2 λ 2 ζ 2 ) Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.31/36

Main result It can be shown that there exists a choice of the gains K 0, K 1, K 2 and the saturation levels λ 0, λ 1, λ 2 such that: Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.32/36

Main result It can be shown that there exists a choice of the gains K 0, K 1, K 2 and the saturation levels λ 0, λ 1, λ 2 such that: The interconnected system satisfies an asymptotic I/O bound with respect to the external disturbance all trajectories are bounded. Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.32/36

Main result It can be shown that there exists a choice of the gains K 0, K 1, K 2 and the saturation levels λ 0, λ 1, λ 2 such that: The system is indeed a small-gain theorem interconnection of two (weak) ISS-systems overall system is (weakly) ISS, and the gain can be assigned through K 4. (ζ 0,ζ 1,ζ 2 ) u 2 q (q,ω) Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.32/36

A case study: a small AUV For the simulations, we use the full nonlinear model, with parameter uncertainties up to 20% of the nominal values. J x = 0.142413 J y = 0.271256 J z = 0.271492 l M = 0.015 y M = 0 h M = 0.2943 l T = 0.8715 h T = 0.1154 M = 4.9 C Q M = 0.004452 DQ M = 0.6304 cq M = 25.23 C Q T = 0.005066 DQ T = 0.008488 cq T = 25.23 Nominal parameters of the plant Vertical dynamics k 1 = 0.1 k 2 = 45 γ = 1 Lateral/longit. dynamics K 0 = 0.09 K 1 = 0.081 K 2 = 0.75 Attitude dynamics K 3 = 0.8 K 4 = 30 ε = 0.1 Saturation levels λ 0 = 2000 λ 1 = 8.1 λ 2 = 0.2952 Controller parameters Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.33/36

Simulation results - vertical error Regulation error z(t) z (t) 7 [m] 6 5 4 3 adaptation OFF adaptation ON landing 2 1 0 1 0 50 100 150 200 250 300 350 400 time [s] Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.34/36

Simulation results - attitude 1 scalar part of the quaternion q 0 (t) 0.99 0.98 0.97 0.96 0 5 10 15 20 25 30 0.2 vector part of the quaternion q(t) 0.1 0 0.1 0.2 0 5 10 15 20 25 30 35 40 s Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.35/36

Simulation results - attitude Steady-state behavior of the attitude 20 x 10 3 vector part of the quaternion q(t) 15 10 5 0 5 350 355 360 365 370 375 380 385 390 395 400 s Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.35/36

Simulation results - lateral/ long. 12 10 8 6 4 2 0 x(t) 2 0 20 40 60 80 100 120 140 160 180 200 15 y(t) 10 5 0 5 0 50 100 150 200 250 300 s Applications of Nonlinear Output Regulation. CeSOS-NTNU 2005 p.36/36