Controllo adattativo e algoritmi di stima per UAV

Relatore:

Ingegnere Daniele Carnevale

Correlatore:

Corrado Possieri

Candidato:

Emanuele Gentili

Università degli studi di Roma Tor Vergata

Indice

• Introduzione: cos’è uno UAV

• Modellazione: 6DOF

• Controllo adattativo: Immersion & Invariance

• Linearizzazione del modello e Osservatore dello stato

• Controllore statico in retroazione dallo stato

Cos’è uno UAV

Si distinguono per

• Dimensione (da nano, a micro, a qualche metro)

• Propulsione (motore a reazione, a elica...)

• Pilotaggio (da remoto o automatico)

• Equipaggiamento (scopi civili o militari)

Unmanned Aerial Vehicle

Aircraft Demon (BAE System)

Caratteristiche:

• Simmetria: X-Z

• Massa: 44.3 Kg

• Thrust (Max): 180 N

• Flaps: ±0.313rad(Max)

• Decollo orizzontale

Modello 6DOF

Modello dinamico dell'UAV utilizzato per il design del sistema di controllo.

Variabili:

• φ Angoli di Eulero

• ω Velocità angolari

• s Posizione

• ν Velocità lineari

• M, F Momenti e Forze esterne

• T Thrust (spinta del motore)

Modello 6DOF

Modello 6DOF (Six-degrees-of-fredom)

Cinematica:

𝜑 = 𝑅1(𝜑)𝜔 (1)

𝑠 = 𝑅2 𝜑 ν′ (2)

Dinamica:

I 𝜔 = S1(ω)Iω + M (3)

𝑚 ν = 𝑆1 ω 𝑚ν +𝑚𝑔ε Φ, θ + 𝐹 + 𝑇 (4)

Forze e Momenti Aerodinamici

• 𝐹 = 𝑆𝑄cos α 0 −sin α0 1 0

sin α 0 cos α

−𝐶𝐷𝐶𝑌−𝐶𝐿

• M = SQ𝑏𝐶𝑙 𝑐𝐶𝑚𝑏𝐶𝑛

S superficie alare dell’aircraft

Q pressione dinamica dell’aria

α angolo di attacco

C* coefficienti aerodinamici (da stimare)

Coefficienti aerodinamici

I Coefficienti aerodinamici dipendono da:

• Inclinazione dei flaps (δa, δe, δr)

(ailerons, elevators, rudder)

• Angolo di Attacco α

• Angolo di Sideslip β (tra piano X-Z e ν)

• Velocità angolari ω = [p, q, r]’

Il problema del controllo

Possiamo controllare l’Aircraft nella sua interezza tramite quattro ingressi:

• Flaps

• Throttle (Manetta)

Si vuole implementare un controllo di Trim (M = 0) e di traiettoria.

Che approccio seguire?

• Immersion & Invariance

• Linearizzazione con legge di cotrollo in retroazione dallo stato.

Immersion & Invariance

E’ un controllo adattativo basato su Varietà Invarianti.

• Si applica a sistemi non lineari parametrizzati linearmente

• Assicura la convergenza a zero dell’errore di stima dei parametri

• Garantisce l’inseguimento asintotico della traiettoria desiderata

Il termine ‘adattativo’ indica che il controllo ricava ed utilizza una stima real-time dei parametri non noti a priori.

Immersion & Invariance – Teoria (1)

𝑦 = ℎ 𝑦, 𝑥, 𝑢

𝑥 = 𝑓 𝑦, 𝑥, 𝑢 + Φ 𝑦, 𝑥 θ ξ = 𝑤

Si definisca la varietà(manifold) 𝑺

𝑺 = x, y, ξ ξ + β 𝑥, 𝑦 − θ = 0}

e l’errore di stima z𝑧 = ξ + β 𝑥, 𝑦 − θ

Allora la legge dinamica 𝑤 che rende la varietà invariante

𝑤 = −𝜕β

𝜕𝑦ℎ 𝑦, 𝑥, 𝑢 −

𝜕β

𝜕𝑥𝑓 𝑥, 𝑦, 𝑢 + Φ 𝑦, 𝑥 (ξ+β(y,x))

Sistema parametrizzato linearmente esteso.χ = (y, x) è lo stato del sistema,θ rappresenta i parametri da stimare, con

Φ(y,x)θ = φ1 𝑦, 𝑥1

⊤ ⋯ 0⋮ ⋱ ⋮0 ⋯ φ𝑛 𝑦, 𝑥𝑛

⊤

θ1⋮θ𝑛

Immersion & Invariance – Teoria (2)

Derivando l’errore rispetto al tempo (𝑧 = ξ + β 𝑥, 𝑦 − θ)

𝑧 = −𝜕β

𝜕𝑥Φ 𝑦, 𝑥 z (5)

rimane da scegliere β(y,x) in modo tale che 𝑧 converga a zero.

La scelta più ovvia è la seguente

βi(y,x) = γ𝑖 0𝑥𝑖φ𝑖 𝑦, σ 𝑑σ con γi>0 , i = 1,...,n

Ovvero risulta

𝑧 = −ГΦ 𝑦, 𝑥 Φ 𝑦, 𝑥 z con Г = diag(γ1,...,γn)

forma definita negativa, quindi z=0 è un punto di equilibrio globalmente uniformemente asintoticamente stabile del sistema.

Immersion & Invariance – Teoria (3)

Supponiamo che esista una legge 𝑢 = υ(𝑦, 𝑥, θ+z)

tale che, per ogni traiettoria a ciclo chiuso, valga

a) Φ(y(t),x(t))z(t) ∈ L2 ⇒ χ(t) ∈ L∞b) lim

𝑡→∞Φ(y(t),x(t))z(t) = 0 ⇒ lim

𝑡→∞χ(t) = χ*

Allora la seguente legge di controllo adattativa in retroazione dallo stato 𝑢 = υ(𝑦, 𝑥, ξ+β(y,x))

è tale per cui tutti i segnali a ciclo chiuso sono limitati e lo stato χconverge allo stato desiderato χ*.

Immersion & Invariance - Implementazione

Si compone di tre blocchi principali

• Attitude Controller

• Airspeed Controller (qui solo accennato)

• Trajectory Tracker

Il Trajectory Tracker produce

• φd attitude desiderata

Attitude e Airspeed Controllers calcolano

• ingressi [δ, T]’ del sistema.

Immersion & Invariance – Attitude Control

Si basa sull’annullamento della funzione energia associata a φ e ω.

Si definiscono l’errore di posa e di velocità angolare

φ = φ − φ𝑑 ω = ω −ω𝑑

e la funzione energia

𝐻 φ, ω =1

2 φ⊤𝐾1 φ +

1

2 ω⊤𝐼2 ω 𝑐𝑜𝑛 𝐾1 > 0

e si assume che il vettore dei momenti esterni M possa essere scritto come

𝑀 =

θ1⊤ρ1(𝐼1ω, 𝑑)

θ2⊤ρ2(𝐼2ω, 𝑑)

θ3⊤ρ3(𝐼3ω, 𝑑)

+ 𝐵δ

Immersion & Invariance – Attitude Control (2)

Allora si può riscrivere il sistema(eq 1-3) come un sistema hamiltoniano perturbato

φ

𝐼 ω=

0 𝑅1 φ 𝐼−1

−𝐼−1𝑅1 φ ⊤ 𝑆1(ω)

𝜕𝐻⊤

𝜕 φ

𝜕𝐻⊤

𝜕 ω

+0

𝑀 + κ(φ,ω, 𝑡)

a questo punto tramite l’ingresso δ

δ = −B−1(ξ1+𝑓1(𝐼1ω, 𝑑))⊤ρ1(𝐼1ω, 𝑑)(ξ2+𝑓2(𝐼2ω, 𝑑))⊤ρ2(𝐼2ω, 𝑑)(ξ3+𝑓3(𝐼3ω, 𝑑))⊤ρ3(𝐼3ω, 𝑑)

−B−1 (κ(φ,ω, 𝑡) +K2𝐼ω)

Immersion & Invariance – Attitude Control (3)

...il sistema diventa

φ

𝐼 ω=

0 𝑅1 φ 𝐼−1

−𝐼−1𝑅1 φ ⊤ 𝑆1 ω − 𝐾2

𝜕𝐻⊤

𝜕 φ

𝜕𝐻⊤

𝜕 ω

−0

Δ(𝑒, ω, 𝑑)

dove Δ 𝑒,ω, 𝑑 = 𝑒𝑖⊤𝜌𝑖(𝐼𝑖𝜔, 𝑑), con 𝑒𝑖 = ξ𝑖 + 𝑓𝑖 𝐼𝑖ω, 𝑑 − θ𝑖

Scegliendo come funzione 𝜌𝑖 𝐼𝑖𝜔, 𝑑 = 1, 𝐼𝑖𝜔′, e

𝑓𝑖 𝐼𝑖ω, 𝑑 = γ𝑖 0

𝐼𝑖ω

𝜌𝑖 σ, 𝑑 𝑑σ

siamo certi che l’errore di stima converge a zero e assieme ad esso anche il sistema perturbato. Per cui

lim𝑡→∞

(φ 𝑡 − φ𝑑 𝑡 ) = 0

Trajectory Tracker

• Produce i riferimenti φd per l’Attitude Controller

• Implementa due tipologie di traiettoria: rette e archi di circonferenza

𝑒𝑠 = 𝑎𝑥 + 𝑏𝑦 + 𝑐 = 0𝑒𝑐 = 𝑥 − 𝑎 2 + 𝑥 − 𝑏 2 − 𝑐2 = 0

Se i due manifold sopra definiti risultano uguali a zero, allora l’Aircraft è sulla traiettoria desiderata.

Si procede sempre derivando le due equazioni rispetto al tempo e cercando una legge di controllo tale per cui 𝑒𝑠 e 𝑒𝑐 convergono a zero.

Trajectory Tracker (1)

Cinematica:

𝑥 = 𝑉2cos λ 𝑦 = 𝑉2sin λ 𝑧 = 𝑉1𝑠𝑖𝑛γ

Dinamica del manifold:

𝑒𝑠 = 𝑉2 𝑎2 + 𝑏2sin(λ + arctan(𝑎

𝑏))

𝑒𝑐 = 2𝑉2 𝑒𝑐 + 𝑐2sin(λ + arctan(𝑥 − 𝑎

𝑦 − 𝑏))

Ricavando rispetto a θ e ψ

𝑃𝑎𝑡ℎ 𝑎𝑛𝑔𝑙𝑒: γ = θ − 𝑎𝑟𝑐𝑡𝑎𝑛(𝑣𝑠𝑖𝑛Φ + 𝑤𝑐𝑜𝑠Φ

𝑢)

𝐻𝑒𝑎𝑑𝑖𝑛𝑔 𝑎𝑛𝑔𝑙𝑒: λ = ψ + arctan(𝑣𝑐𝑜𝑠Φ − 𝑤𝑠𝑖𝑛Φ

𝑉1𝑐𝑜𝑠γ)

λ𝑠 = −arctan𝑎

𝑏− arctan(𝑘𝑝𝑒𝑠)+ (π)

λ𝑐 = −arctan𝑥 − 𝑎

𝑦 − 𝑏− arctan(𝑘𝑝𝑒𝑐)+(π)

Selezioni per λ

Trajectory Tracker(2)

Allora possiamo assegnare i tre seguenti angoli di eulero

Φ = arctan(𝐺

𝑐𝑜𝑠α)

θ = arctan𝑣𝑠𝑖𝑛Φ + 𝑤𝑐𝑜𝑠Φ

𝑢− arctan(kh ℎ − ℎ𝑑 )

ψ = −arctan𝑣𝑐𝑜𝑠Φ − 𝑤𝑠𝑖𝑛Φ

𝑉1 cos 𝛾+ λ∗

E tutte le traiettorie convergono verso i due manifold

𝑒𝑠 oppure 𝑒𝑐 .

Costanti

𝐺 = 𝑉2 𝑔𝑐 (forza centripeta)

𝑉1 = 𝑢2 + 𝑣𝑠𝑖𝑛Φ + 𝑤𝑐𝑜𝑠Φ 2

𝑉2 = 𝑉𝑐𝑜𝑠𝛾

I&I – Risultati simulativi (quota)

Figura: innalzamento di quota da 30 a 60 metri

I&I – Risultati simulativi (angoli di Eulero)

Figura: Angoli di Eulero (riferimento e inseguimento)

I&I – Risultati simulativi (Speed,Thrust,Quota)

Figura: Velocità, Thrust e Quota (riferimenti e inseguimento)

I&I – Risultati simulativi (Flaps)

Figura: Inclinazione dei Flaps (δa, δe, δr)

Linearizzazione

Si approssima il sistema in un punto di lavoro (xe,ue):

• Steady–Flight Trimmato ( 𝑥 = 0 = 𝑓 𝑥𝑒 , 𝑢𝑒 )

• Utilizzo della ‘f_solve’ di Matlab per calcolo di (xe,ue)

Si sono fissate

• ω = 0;

• Φ(roll), ψ(yaw)=0;

• VΦ = 30m/s,Vθ=0

• δa, δr = 0

Si sono lasciate libere

• Vψ (V lungo z’)

• θ (pitch)

• δe (elevator)• Tx (Thrust lungo x’)

Linearizzazione

Si ottengono i valori

• Vψ = 3.6 m/s

• θ = 0.1195 rad (6.8°)

• δe = - 0.022 rad (1.45°)

• Tx = 42.5 N

Si fissa lo stato 𝑥 = Φ θ ψ ℎ 𝑝 𝑞 𝑟 𝑉𝑥′ 𝑉𝑦′𝑉𝑧′ ′

e l’ingresso 𝑢 = [δ𝑎 δ𝑒 δ𝑟 𝑇𝑥]′

Tramite l’approssimazione di Taylor (al primo ordine) si ottiene

𝑥 = 𝐴𝑥 + 𝐵𝑢𝑦 = 𝐶𝑥 + 𝐷𝑢

con A,B,C definite di lato e D = 0.

Punto di lavoro:

𝑥𝑒 = [0 0.1195 0 60 0 0 0 30 0 3.6]′𝑢𝑒 = [0 0.022 0 42.5]′Jacobiano della funzione f(x,u)

𝐴 = 𝛻𝑥𝑓 𝑥𝑒 , 𝑢𝑒B = 𝛻𝑢𝑓 𝑥𝑒 , 𝑢𝑒

𝐶 =

0 0 1 0 0 0 0 0 00 0 0 1 0 0 0 0 00 0 0 0 1 0 0 0 00 0 0 0 0 1 0 0 00 0 0 0 0 0 1 0 00 0 0 0 0 0 0 1 0

Osservatore e legge di controllo

Serve per stimare l’intero stato del sistema, partendo dalle misure dello stato 𝑦 𝑡 disponibili, dalla conoscenza delle matrici A,B,C,D del sistema e dell’ingresso 𝑢(𝑡)

ξ(𝑡) = 𝐻ξ 𝑡 + 𝑅𝑢 𝑡 + 𝑉𝑦(𝑡)

Sotto opportune condizioni, l’errore di stima 𝑒 𝑡 = 𝑥 𝑡 − ξ 𝑡

converge a zero e l’osservatore diventa

ξ 𝑡 = 𝐴ξ 𝑡 + 𝐵𝑢 𝑡 + 𝑉(𝑦 𝑡 − (𝐶ξ 𝑡 + 𝐷𝑢 𝑡 )

una copia del sistema più un ingresso che è la differenza tra le uscite del sistema vero e quello stimato, pesato tramite la matrice V.

Osservatore e legge di controllo (2)

Dopo aver ricostruito lo stato, si può effettuare una stabilizzazione in retroazione dallo stato, tramite una opportuna legge di controllo

𝑢 𝑡 = 𝐹ξ 𝑡 + 𝑣(𝑡)

È possibile esprimere il sistema complessivo 𝑥 𝑡 , 𝑒 𝑡 come segue 𝑥(𝑡) 𝑒(𝑡)

=𝐴 + 𝐵𝐹 −𝐵𝐹

0 𝐴 − 𝑉𝐶

𝑥(𝑡)𝑒(𝑡)

+𝐵0𝑣(𝑡)

Per stabilizzare il sistema nell’origine dello spazio di stato occorre che gli autovalori di (A-VC) e (A +BF) siano a parte reale negativa.

Osservatore e legge di controllo (3)

Dopo un tempo t sufficientemente grande, l’errore 𝑒 𝑡 = 0, ed

estraendo la prima delle due equazioni, si ha 𝑥 = 𝐴 + 𝐵𝐹 𝑥 𝑡 + 𝐵𝑣(𝑡)

e ricordando la definizione di punto di equilibrio (𝑓 𝑥𝑒 , 𝑢𝑒 = 0), si trova l’ingresso di equilibrio funzione dello stato desiderato 𝑥𝑒

𝑣𝑒 = −𝐵−1 𝐴 + 𝐵𝐹 𝑥𝑒

ovvero una legge di controllo statica in retroazione dallo stato.

Osservatore e legge di controllo(3)

Figura: osservatore con controllore statico

Controllore statico - Risultati

Figura: innalzamento di quota(da 30 a 60m) Figura: errore di inseguimento di traiettoria

Controllore statico - Risultati

Figura: angoli di eulero Figura: Velocità angolari

Conclusioni e sviluppi futuri

Immersion & Invariance

• Uso combinato con osservatori non lineari, con misure rumorose.

• Estensione anche per i VTOL

Controllore statico

• Pianificazione oculata delle matrici V e F

• Linearizzazione in n punti di lavoro con n controllori ad hoc

• Switch tra un linearizzato e il successivo

Fine

Grazie per l’attenzione!!! =)