Controllo adattativo e algoritmi di stima per...
Transcript of Controllo adattativo e algoritmi di stima per...
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 (Speed,Thrust,Quota)
Figura: Velocità, Thrust e Quota (riferimenti e inseguimento)
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.
Controllore statico - Risultati
Figura: innalzamento di quota(da 30 a 60m) Figura: errore di inseguimento di traiettoria
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