A deterministic interpretation of the Kalman filter

Commenti

Transcript

A deterministic interpretation of the Kalman filter
Laboratorio di Navigazione
Laurea Specialistica in Ingegneria Informatica,
Ingegneria per l’Ambiente e il Territorio
Politecnico di Milano – Campus Como
NAVIGAZIONE
INERZIALE
1
Sistemi di riferimento
Un sistema di riferimento è un insieme di regole e misure che ci permettono di rispondere ai quesiti:
“dove si trova un punto?”, “quando è avvenuto un evento?”.
Esigenze storiche e strumentali, nonché differenti applicazioni, hanno portato alla definizione di
differenti sistemi di riferimento per geo-referenziare punti e per tempo-referenziare eventi.
Dato un sistema di riferimento si può definire un sistema di coordinate che fa corrispondere
biunivocamente punti dello spazio con una terna (x1, x2, x3) di numeri reali (per esempio coordinate
cartesiane).
Un sistema inerziale è un sistema di riferimento in cui ogni punto materiale, non soggetto a forze
esterne, resta in quiete o si muove di moto rettilineo uniforme, come postulato dalla I legge della
dinamica.
Le stelle, prese come punti materiali date le loro dimensioni rispetto alle grandi distanze relative,
possono essere considerate punti isolati (trascurabile interazione gravitazionale) e possono essere
utilizzate per definire un sistema di riferimento per cui la I legge della dinamica è soddisfatta su
scale temporali dell’ordine di qualche anno. Per fenomeni spaziali inferiori a 1000 km e temporali
inferiori al giorno, un sistema eliocentrico può essere considerato realisticamente inerziale. Tuttavia
qualunque sistema di coordinate associato a questo sistema di riferimento sarebbe scomodo per
descrivere la navigazione di un veicolo sulla (o intorno) alla terra. Un sistema traslato nel centro
della terra è sicuramente più comodo, ma non può più essere considerato inerziale per via del moto
di rivoluzione della terra intorno al sole (che implica un’accelerazione del sistema terra).
Tuttavia, per un punto prossimo alla superficie della terra la sua dinamica risente della presenza del
sole solo per la diversa attrazione che il sole esercita sul baricentro terrestre rispetto al punto
considerato. Tale differenza, detta marea, è piccola e può essere trascurata o modellizzata nel caso
di lunghi periodi di osservazione. Idem per l’effetto della luna.
Un sistema pseudo-inerziale è un sistema di riferimento con origine nel baricentro terrestre e con
assi paralleli ad un sistema inerziale (definiti tramite osservazioni astronomiche). In tale sistema
vale la legge di Newton con la convenzione che l’accelerazione di gravità includa anche i termini
mareali dovuti all’attrazione luni-solare.
Sistema pseudo-inerziale (i):
- origine nel baricentro terrestre;
- asse x3: asse istantaneo di rotazione;
- asse x1: all’incrocio tra l’eclittica e il piano equatoriale;
- asse x2: per completare terna destrorsa.
Eclittica: percorso apparente del sole visto dalla Terra
2
Un sistema pseudo-inerziale può risultare comodo per descrivere le orbite dei satelliti intorno alla
terra, ma non è conveniente per descrivere il moto di un veicolo sulla terra, che compie un moto di
rotazione attorno al proprio asse. A tal fine risulta utile definire un sistema solidale con la terra,
anche detto earth-fixed
Sistema earth-fixed (e):
- origine nel baricentro terrestre;
- asse x3: diretto verso un polo nord convenzionale;
- asse x1: all’incrocio tra il piano meridiano di Greenwich e il piano equatoriale terrestre;
- asse x2: per completare terna destrorsa.
x3
x2
x1
Sistema earth-fixed
Si noti che l’asse di rotazione terrestre varia nel tempo e quindi in generale l’asse x3 del sistema
inerziale non coincide con quello del sistema earth-fixed. Inoltre la velocità angolare della terra
subisce piccolissime variazioni e la Terra stessa subisce deformazioni di tipo geologico e legate alle
forze mareali. Tuttavia per gli scopi della navigazione, a meno di correzioni calcolabili, il corpo
della Terra può essere considerato rigido e in moto di rotazione uniforme attorno ad un asse fisso,
con periodo di 24 ore. Tutte le correzioni necessarie sono fornite dal IERS (International Earth
Rotation and Reference Systems Service). Alcuni sistemi earth-fixed sono il WGS84 e ITRF 2000.
All’interno del sistema earth-fixed possono essere usate le tradizionali coordinate cartesiane o le
coordinate geodetiche più comode per il posizionamento dei punti sulla superficie, ovvero (ϕ, λ, h)
cioè latitudine, longitudine e altezza ellisoidica rispetto ad un ellissoide geocentrico. Possiamo
allora definire un ulteriore sistema, adatto a descrivere il moto di un veicolo sulla superficie; questo
sistema è chiamato sistema di riferimento navigazionale o di livello-locale ed è definito come
segue.
Sistema navigazionale (n) o di livello-locale:
- origine in un generico punto P (non necessariamente sull’ellissoide);
- asse x1: diretto in direzione est;
- asse x2: diretto in direzione nord;
- asse x3: diretto come la normale rispetto all’ellissoide di riferimento, verso l’alto.
3
x3
x2
x1
Sistema navigazionale
Mentre il sistema navigazionale è adatto a descrivere il moto del veicolo, gli strumenti sono
tipicamente vincolati al veicolo stesso e richiedono l’introduzione di un ulteriore sistema:
Sistema veicolo o body (b):
- origine in un generico punto P;
- asse x1: diretto nella direzione di marcia;
- asse x3: perpendicolare al piano del veicolo (verso l’alto);
- asse x2: per completare terna destrorsa.
Il sistema (n) e il sistema (b) condividono l’origine del punto P, ma in genere il loro orientamento è
diverso. Si definisce imbardata (yaw) la rotazione rispetto all’asse x3, beccheggio (pitch) la
rotazione rispetto all’asse x2 e rollio (roll) la rotazione rispetto all’asse x1.
Si noti che vi sono alcuni strumenti che attraverso giunti cardanici e servo-motori si mantengono
costantemente orientati secondo il sistema (n). Più comuni (e meno costosi) sono invece i sistemi
strapdown, che seguono l’orientamento del veicolo.
x3
yaw
roll
x1
x2
pitch
Sistema veicolo e rotazioni ad esso associate
4
Strumenti inerziali
Un sistema inerziale di navigazione (INS) è un insieme formato da una piattaforma su cui sono
rigidamente montati di norma 6 sensori, tre che rilevano le accelerazioni impresse alla piattaforma
stessa (accelerometri) e tre che misurano la velocità angolare rispetto ad un sistema inerziale
(giroscopi). E’ parte dello strumento anche l’hardware che raccoglie i dati e il software che li
processa in tempo reale.
I singoli componenti di un INS sono detti IMU (Inertial Measurement Unit), termine che viene
anche utilizzato come sinonimo di INS.
Il sistema di riferimento proprio dell’INS, considerato come corpo rigido, è materializzato da una
terna di assi ortogonali che coincidono con gli assi di misura degli accelerometri. I giroscopi a loro
volta sono montati in modo che i loro assi di misura siano paralleli agli assi degli accelerometri.
Vi sono due possibili configurazioni di INS. In un caso, quello più costoso dal punto di vista
economico, la piattaforma è montata su sospensioni cardaniche che ne vincolano i movimenti e che
attraverso servo-motori mantengono il sistema di riferimento dell’INS orientato come il sistema di
livello locale (n) (o almeno lo mantengono parallelo al piano orizzontale). Altri strumenti, detti di
tipo strapdown, mantengono la piattaforma rigidamente vincolata al mezzo mobile, così che il
sistema di riferimento interno all’INS coincida con il sistema body (b).
Scheme of inertial navigation system
Accelerometri
Il principio base degli accelerometri è di misurare la forza agente su una massa di prova (proof
mass). Si distinguono principalmente due tipi di sensori, di tipo open-loop o closed-loop. Nel primo
caso gli accelerometri misurano direttamente lo spostamento della massa di prova dovuto alle forze
esterne agenti sul sensore. I sensori closed-loop invece mantengono la massa di prova in uno stato
di equilibrio generando una forza opposta alla forza applicata. Questo è ottenuto attraverso un
sistema di controllo retroazionato (feedback system) di tipo elettrico o magnetico. Il principale
vantaggio dell’architettura closed-loop è la miglior linearità della misura rispetto alle forze
applicate.
Nella sua configurazione più elementare un accelerometro è costituito da uno chassis che vincola la
massa di prova a muoversi solo lungo un asse determinato e da una molla che lega la massa di
prova allo chassis stesso. La dinamica di un tale sistema è descritta dalle seguente equazione
differenziale lineare:
m&l& + k v &l − k e l = F
dove m è la massa di prova, l è l’allungamento della molla, kv la costante di viscosità, ke la costante
elastica della molla, F la forza applicata al sensore. La soluzione omogenea (cioè quella in cui non
5
viene applicata alcuna forza) è un oscillatore armonico, smorzato nel caso di attrito. Con
F=costante, dopo che le oscillazioni iniziali si sono esaurite, vale la legge
− kel = F
ovvero misurando l’allungamento della molla e conoscendo la costante elastica è possibile misurare
la forza applicata; poiché dalla legge di Newton si ha
F = ma
conoscendo la massa di prova, si può ricavare l’accelerazione a lungo l’asse della molla.
F
m
l
Open-loop accelerometer principle
Un sistema di tipo closed-loop più vicino alla realtà è l’accelerometro a pendolo, in cui il moto della
massa di prova m è vincolato ad essere oscillante attorno ad un perno P, con un unico grado di libertà.
Se la massa di prova m subisce una certa accelerazione, il pendolo lasciato libero comincerebbe ad
oscillare, ma la variazione di posizione è percepita da un sensore ottico S, che attraverso un circuito
di reazione comanda una bobina che esercita una forza F su un magnete A in grado di riportare la
massa di prova m in quiete (vedi figura). La forza indotta dalla bobina è facilmente calcolabile
(legata alla corrente I che circola nella bobina) ed è proporzionale alla forza che si vuole misurare.
A
m
feedback
system
Close-loop accelerometer principle
6
Accelerometri ad uso scientifico con accuratezze estremamente più elevate sono installati per
esempio a bordo di satelliti geodetici per la misura del campo gravitazionale. Si tratta tipicamente di
accelerometri elettrostatici, in cui la massa di prova (di platino) è posta all’interno di un dielettrico
circondato da elettrodi. Ogni movimento della massa di prova produce una variazione di capacità
misurata agli estremi degli elettrodi. Questa differenza viene rilevata, amplificata e poi utilizzata in
un sistema retroazionato per riportare la massa di prova nella sua posizione iniziale. La tensione
applicata agli elettrodi rappresenta l’output dell’accelerometro (vedi figura).
Accelerometro elettrostatico per missioni geodetiche da satellite
L’errore di un accelerometro può essere descritto come segue:
δa = b + λa + cT (T − T0 ) + ν
dove b è un bias, λ un fattore di scala, cT una costante termica dipendente dalla temperatura T e v il
noise di misura. Valori tipici per uno strumento commerciale possono essere
b = 25 mGal
λ = 5 10 −5
cT = 0.5 mGal / °C
σ v = 40 mGal / Hz (ovvero circa 0.6 Gal per osservazione, con una frequenza di campionamento
dell’ordine di 200 Hz)
Giroscopi
Il giroscopio è uno strumento in grado di misurare la velocità angolare della rotazione impressa al
sensore rispetto ad un sistema di riferimento inerziale.
Storicamente i giroscopi erano strumenti di tipo meccanico, costituiti essenzialmente da un corpo
rigido con simmetria circolare (rotore) in grado di ruotare intorno al suo asse principale (asse di
spin). Tuttavia questi sensori meccanici sono stati utilizzati negli anni sempre meno, a vantaggio dei
sensori ottici, che presentano precisioni elevate, grande semplicità di funzionamento e costi
relativamente bassi in quanto non necessitano di parti meccaniche di alta precisione. Inoltre offrono
la possibilità di essere miniaturizzati. Gli strumenti ottici funzionano esclusivamente in modo
strapdown, a differenza di quelli meccanici che sono adatti anche a piattaforme stabilizzate.
7
Tra i giroscopi ottici è degno di nota il giroscopio a fibra ottica (FOG, Fiber Optic Gyroscope) che
funziona sulla base di un effetto relativistico noto, sin dal 1913, come effetto Sagnac dal nome del
suo scopritore.
Si consideri una spira circolare di fibra ottica in rotazione uniforme nel suo piano, con velocità
angolare ω rispetto ad un sistema inerziale; lungo la fibra, nella stessa direzione della rotazione, si
propaga un segnale luminoso che viene emesso da un sorgente E al tempo t0 e raccolto dallo stesso
sensore E al tempo t0+∆t. Quindi un osservatore solidale con la fibra vede la luce compiere
esattamente un giro ϑ’ = 2π (l’apice indica una grandezza vista dall’osservatore solidale con la
fibra), mentre un osservatore inerziale giudica che il fronte luminoso abbia girato per un angolo
ϑ = 2π+∆ϑ (vedi figura).
Effetto Sagnac
Tenendo conto che un osservatore inerziale vede muoversi il raggio luminoso a velocità c,
indipendentemente dalla velocità della sorgente, la differenza angolare risulta
∆ϑ =
ωR
c
⋅ 2π
dove R è il raggio della spira e ωR è la velocità della sorgente. Tale angolo corrisponde ad un
percorso ottico di
∆L = R ∆ϑ =
ωR 2
c
⋅ 2π =
2ωA
c
dove A è l’area abbracciata dalla spira. Se ora, a partire dalla sorgente E al tempo iniziale t0
emettiamo anche un raggio luminoso simmetrico che percorra la spira in senso inverso, è facile
capire che un osservatore inerziale vede il raggio tornare alla sorgente dopo un angolo di ϑ =2π-∆ϑ.
Se inoltre supponiamo che i due raggi di luce abbiano la stessa frequenza e siano emessi in fase,
ricombinati alla ricezione e facendoli interferire tra loro è possibile eseguire la misura dello
sfasamento, cioè
∆ϕ =
∆L+ − ∆L− 4ωA
=
λ
λc
dove ∆L+ e ∆L- sono i due cammini ottici nei due sensi opposti. La misura dello sfasamento
permette di ricavare una misura di ω, ovvero una misura della velocità angolare. Poiché 4A/λc
costituisce la sensibilità dello strumento, allo scopo di migliorarne il rendimento, la fibra ottica
viene avvolta in una bobina che contiene N spire, fino ad una lunghezza di oltre 1 km.
8
Analogamente ad un accelerometro, l’errore di un giroscopio può essere descritto come segue:
δω = b + λω + cT (T − T0 ) + ν
dove b è un bias, λ un fattore di scala, cT una costante termica dipendente dalla temperatura T e v il
noise di misura. Valori tipici per uno strumento commerciale possono essere
b = 10 −3 ° / ora
λ = 2 10 −6
cT = 5 10 −5 ° / ora °C
σ v = 6 10 −7 rad / s
Hz
(ovvero circa 10-5 rad/s per osservazione, con una frequenza di
campionamento dell’ordine di 200 Hz)
Rotazioni e velocità angolari
Richiamiamo brevemente i concetti della rotazione di un sistema di riferimento. Dette (x1,x2) le
coordinate cartesiane di un vettore nel sistema di riferimento A, (x1’,x2’) le coordinate dello stesso
vettore in un sistema B ruotato di un angolo α rispetto ad A, vale la seguente relazione
 x1 ' = + x1 cos α + x 2 sin α

 x 2 ' = − x1 sin α + x 2 cos α
x2
x1’
x2’
o in forma vettoriale
x1
x' = U x
B
A
con
 cos α
U AB = 
− sin α
sin α 
cos α 
Notiamo che se lo stesso risultato può essere ottenuto mantenendo fisso il sistema di riferimento e
( )
+
ruotando il vettore di un angolo -α, ovvero R AB = U AB .
Nel caso tri-dimensionale una qualunque rotazione può essere ottenuta combinando tre singole
rotazioni rispetto agli assi cartesiani, ovvero
0
1

U 1 = 0 cos α 1
0 − sin α 1
0 
sin α 1  ,
cos α 1 
cos α 2
U 2 =  0
 sin α 2
0 − sin α 2 
1
0  ,
0 cos α 2 
 cos α 3
U 3 = − sin α 3
 0
sin α 3
cos α 3
0
0
0
1
U AB = U 3 (α 1 ) U 2 (α 2 ) U 1 (α 3 ) =
 cos α 3 cos α 2
= − sin α 3 cos α 2

sin α 2
sin α 3 cos α 1 + cos α 3 sin α 2 sin α 1
cos α 3 cos α 1 − sin α 3 sin α 2 sin α 1
− cos α 2 sin α 1
sin α 3 sin α 1 − cos α 3 sin α 2 cos α 1 
cos α 3 sin α 1 + sin α 3 sin α 2 cos α 1 

cos α 2 cos α 1
9
Per derivare il legame tra matrice di rotazione e velocità angolari (da utilizzare poi per le misure dei
giroscopi) consideriamo degli angoli infinitesimi dα1 , dα2 , dα3 , cosicché cos dα ≈ 1 ,
sin dα ≈ dα . Le tre matrici di rotazione diventano
0
1

1
dU 1 = 0
0 − dα 1
 1
dU 2 =  0
dα 2
0 
dα 1  ,
1 
0 − dα 2 
1
0  ,
0
1 
 1
dU 3 = − dα 3
 0
dα 3
1
0
0
0
1
e la rotazione congiunta, trascurando i termini di secondo ordine, si può scrivere come
dU AB = dU 3 (dα 1 ) dU 2 (dα 2 ) dU 1 (dα 3 ) = I − dA
 0
dA =  dα 3
− dα 2
− dα 3
0
dα 1
dα 2 
− dα 1 
0 
Sfruttando questa matrice, la rotazione compiuta in un tempo t0 viene definita come
U AB (t 0 ) = U AB
mentre la rotazione in un tempo t0+dt, con dt infinitesimo, può essere vista come la combinazione
di due rotazioni successive, ovvero
U AB (t 0 + dt ) = U AB (dt ) U AB (t 0 ) = ( I − dA) U AB
Calcolando ora la derivata nel tempo, si ottiene
U AB (t 0 + dt ) − U AB (t 0 )
( I − dA) U AB − U AB
dA B
B
&
U A = lim
= lim
= − lim
U A = −Ω ABU AB
dt →0
dt
dt
→
0
→
0
dt
dt
dt
dove la matrice delle velocità angolari Ω è definita come

 0
 dα
dA
3
lim
= lim 
dt → 0 dt
dt →0 
dt
 dα 2
− dt

−
dα 3
dt
0
dα 1
dt
dα 2 
dt   0
dα 1  
 = ω3
−
dt  
 − ω 2
0 

− ω3
0
ω1
ω2 
− ω1  = Ω AB
0 
Questa espressione può essere ulteriormente elaborata. Per le proprietà delle rotazioni, si ha
U AB U BA = I
ovvero due rotazioni opposte si annullano. Derivando questa relazione si ottiene
10
U& AB U BA + U AB U& BA = 0
Ora moltiplicando entrambi i lati dell’equazione differenziale U& AB = −Ω ABU AB per la matrice U BA ,
con qualche passaggio algebrico si ottiene
U& AB U BA = −Ω AB U AB U BA
U& AB U BA = −Ω AB
− U AB U& BA = −Ω AB
− U BA U AB U& BA = −U BA Ω AB
U& BA = U BA Ω AB
Equazioni di navigazione
Le equazioni di navigazione mettono in relazione le incognite del problema (ovvero posizione,
velocità e assetto del veicolo istante per istante) con le osservazioni disponibili dagli accelerometri e
dai giroscopi (ed eventualmente dal GPS). A seconda del sistema di riferimento scelto le equazioni
possono essere più o meno complicate dal punto di vista analitico. Considereremo il caso di un
sistema inerziale (adatto a descrivere la navigazione nello spazio, per esempio le orbite di un
satellite artificiale) e di un sistema earth-fixed (più adatto a descrivere la navigazione terrestre,
richiedendo poi un ulteriore passaggio al sistema di livello locale).
Navigazione in un sistema inerziale
In un sistema inerziale la seconda legge di Newton della dinamica può essere scritta come
i
i
F tot = m &x&
dove Ftot rappresenta la forza complessiva applicata al corpo di massa m lungo i tre assi del sistema
di riferimento e &x& è l’accelerazione dovuta a tale forza. Se il corpo è soggetto ad un campo
gravitazionale, la precedente equazione può essere esplicitata come
i
i
m &x& = F + m g
i
dove g è l’accelerazione gravitazionale. Dividendo l’equazione per m, si ottiene la relazione
fondamentale
&x&i = f i + g i
dove f è la forza specifica (non gravitazionale), cioè la forza per unità di massa, applicata alla massa
m. Poiché lo stato del veicolo in movimento è solitamente descritto in termini di posizione e
velocità (ed eventualmente assetto), la precedente equazione differenziale (in tre dimensioni) è
separata in un sistema di due equazioni differenziali (sempre in tre dimensioni), ovvero
11
d xi
i
= x&

 dt

 d x& i
i
i

= f +g
 dt
Come è immediato vedere la determinazione di posizione e velocità richiede la conoscenza (o la
modellizzazione) del campo gravitazionale terrestre.
Nel caso più comune di sistemi strapdown, gli accelerometri misurano la forza specifica f b nel
sistema body. Per poter utilizzare la precedente relazione è necessario applicare la seguente
rotazione
i
f = U bi f
b
dove U bi è la matrice di rotazione dal sistema body al sistema inerziale. Questa matrice cambia con
il tempo e può essere derivata dalle misure dei giroscopi, integrando numericamente l’equazione
differenziale
U& bi = −Ω biU bi
Si noti come nel caso di una rotazione rispetto ad un singolo asse, per esempio
 cos α
U = − sin α
 0
B
A
sin α
cos α
0
0
0
1
Ω
B
A
0
= ω
 0
−ω
0
0
0
0
0
l’equazione differenziale precedente diventa
dα

 − sin α dt

dα
− cos α
dt

0


cos α
dα
dt
dα
− sin α
dt
0

0
0

0 = − ω

 0
0

dα

 − sin α dt

dα
− cos α
dt

0


dα
dt
dα
− sin α
dt
0

0
− ω sin α
 

0 = − ω cos α

0
0 

cos α
−ω
0
0
0  cos α
0 − sin α
0  0
sin α
cos α
0
0
0
1
ω cos α 0
− ω sin α 0
0
1
da cui segue
12
dα
=ω
dt
t
α (t ) = ∫ ω (τ )dτ + α 0
t0
che richiede la conoscenza dell’assetto iniziale.
Navigazione in un sistema earth-fixed
L’equazione della navigazione in un sistema earth-fixed può essere ottenuto sulla base del teorema
di Coriolis, che descrive velocità e accelerazione di un punto, visto in due sistemi di riferimento
qualsiasi in moto (rigido) uno rispetto all’altro.
Un qualunque vettore di posizione può essere visto o nel sistema inerziale o nel sistema earth fixed,
semplicemente applicando la rotazione istantanea (e l’eventuale traslazione dell’origine) tra i due
sistemi, ovvero
i
i
e
x (t ) = x 0 (t ) + U ei (t ) x (t )
Per semplificare la notazione si omette di scrivere la dipendenza dal tempo, che riguarda comunque
tutti i vettori e le matrici coinvolte nell’equazioni seguenti. Applichiamo una prima differenziazione
nel tempo, ottenendo
i
i
e
e
i
e
e
x& = x& 0 + U& ei x + U ei x& = x& 0 + U ei ( x& + Ω ie x )
e successivamente una seconda differenziazione nel tempo, cioè
&x&i = &x&i0 + U& ei x& e + U ei &x&e + U& ei Ω ie x e + U ei Ω& ie x e + U ei Ω ie x& e =
i
e
e
e
e
= &x&0 + U ei ( &x& + 2Ω ie x& + Ω ie Ω ie x + Ω& ie x )
Moltiplicando entrambi i termini dell’equazione per U ie e ricordando che
U ie U ei = I
si ottiene la dinamica del punto rispetto ad un sistema di riferimento rotante
&x&e = U ie ( &x&i − &x&i0 ) − ( 2 Ω ie x& e + Ω ie Ω ie x e + Ω& ie x e )
Analizziamo i diversi termini (con particolare attenzione alle accelerazioni apparenti sperimentate
dall’oggetto in movimento nel sistema earth-fixed)
e
- &x& =accelerazione del punto vista nel sistema earth-fixed
-
&x&i =accelerazione del punto vista nel sistema inerziale
-
&x&i0 = accelerazione dell’origine del sistema di riferimento inerziale. Poiché il sistema inerziale e
il sistema earth-fixed hanno la stessa origine (centro della Terra) questo termine risulta pari a 0
(la trasformazione tra i due sistemi di riferimento è solo una rotazione e non c’è traslazione).
U ie = rotazione istantanea dal sistema di riferimento inerziale e quello earth-fixed. La rotazione
avviene intorno all’asse x3, coincidente fra i due sistemi di riferimento.
-
13
-
Ω ie = matrice delle velocità angolari tra i due sistemi di riferimento in rotazione fra loro. Poiché
la Terra ruota attorno all’asse x3, supponendo il moto circolare uniforme, si ha ω1 = ω 2 = 0 ,
ω 3 = ω E = 2π 24ore = costante .
-
− 2Ω ie x& = −2ω ∧ x& = accelerazione di Coriolis che appare quando un oggetto è in movimento
rispetto ad un sistema di riferimento in rotazione. A causa di questa accelerazione l’oggetto in
movimento subisce una deflessione verso destra rispetto alla direzione della velocità
nell’emisfero boreale e una deflessione verso sinistra nell’emisfero australe. Questa
accelerazione è diretta perpendicolarmente al piano formato dalla velocità dell’oggetto e
dall’asse di rotazione del sistema di riferimento. Si annulla quando la velocità è diretta come
l’asse di rotazione. Svolge un ruolo fondamentale nella circolazione atmosferica.
e
e
− Ω ie Ω ie x = −ω ∧ (ω ∧ x ) = accelerazione centrifuga dovuta alla rotazione del sistema earth
fixed rispetto al sistema inerziale. E’ diretta verso l’esterno, perpendicolarmente all’asse di
rotazione della Terra.
e
− Ω& ie x = accelerazione tangenziale dovuta alla variazione della velocità angolare del sistema
earth-fixed. Poiché la velocità della Terra è assunta costante, questo termine risulta pari a 0.
e
-
-
e
Con le opportune semplificazioni, l’equazione della dinamica diventa
&x&e = U ie &x&i − 2 Ω ie x& e − Ω ie Ω ie x e
Ricordando che l’accelerazione nel sistema inerziale, per la seconda legge di Newton, è dato da:
&x&i = f i + g i
si ha
&x&e = U ie ( f i + g i ) − 2 Ω ie x& e − Ω ie Ω ie x e = f e + g e − 2 Ω ie x& e − Ω ie Ω ie x e
dove g
e
e f
e
rappresentano rispettivamente l’accelerazione gravitazionale e le forze specifiche
non gravitazionali applicate all’oggetto espresse nel sistema earth-fixed.
Tipicamente l’accelerazione gravitazionale e l’accelerazione centrifuga sono raggruppate insieme
per formare il vettore accelerazione di gravità
g = g − Ω ie Ω ie x
e
e
e
Si ottiene così l’espressione finale della dinamica in un sistema earth-fixed
&x&e = f e + g e − 2 Ω ie x& e
Tenendo conto che le forze specifiche f sono misurate nel sistema di riferimento interno del veicolo
(sistema body), si ha
e
f = U ie U bi f
b
14
dove U bi è la matrice di rotazione dal sistema body al sistema inerziale che può essere derivata dalle
misure dei giroscopi, mentre U ie è la matrice di rotazione nota dal sistema inerziale al sistema
earth-fixed, che dipende dall’istante di osservazione t, ovvero
 cos ω E t sin ω E t 0
U = − sin ω E t cos ω E t 0

0
0
1
e
i
assumendo che al tempo t=0 il sistema inerziale e il sistema earth-fixed siano allineati.
Poiché lo stato del veicolo in movimento è solitamente descritto in termini di posizione, velocità ed
assetto, le equazioni di navigazione del veicolo sono espresse tramite il seguente sistema di
equazioni differenziali, ovvero
d xe
e
= x&

 dt
 e
 d x& = U e U i f b + g e − 2Ω e x& e
i
b
i
 dt

 i
U& b = U bi Ω ib
dove f
b
e Ω ib sono grandezze osservate, g , U ie e Ω ie sono grandezze modellate (in funzione del
e
e
e
tempo di osservazione) ed infine x , x& e U bi sono le incognite del problema.
L’equazione differenziale che descrive la dinamica nel sistema earth-fixed può essere divisa in un
sistema di tre equazioni differenziali, una per ogni componente; infatti, ricordando che
0
Ω = ω E
 0
e
i
− ωE
0
0
0
0
0
si ha
− ω E2

Ω ie Ω ie =  0
 0

0
−ω
0
2
E
+ x&2e 


e
− 2Ω ie x& = 2ω E  − x&1e 
 0 


0

0
0
(accelerazione di Coriolis)
15
 x1e 
 
e
− Ω ie Ω ie x = ω E2  x 2e 
0
 
(accelerazione centripeta)
Inoltre, in prima approssimazione, il campo gravitazionale della Terra può essere assunto uguale al
campo prodotto da una massa puntiforme posta al centro della Terra e avente massa uguale alla
massa della Terra stessa. In questo modo, per la legge di gravitazione universale, si ha
e
g =
GM
x
e
g =−
e 2
GM
x
e 3
x
e
dove G è la costante di gravitazione universale e M la massa della Terra. Espressa per componenti
la precedente relazione diventa
e
g =−
[( x )
e 2
1
GM
+ ( x 2e ) 2 + ( x3e ) 2
]
3/ 2
 x1e 
 e
 x2 
 x3e 
 
Occorre sottolineare che questa approssimazione sferica del campo gravitazionale introduce errori
non accettabili nella navigazione, per cui si preferisce utilizzare almeno un’approssimazione che
tenga conto della forma prevalentemente ellissoidica del pianeta Terra. Esistono anche modelli
globali e locali del campo gravitazionale terrestre che permettono di ridurre ulteriormente l’errore
dovuto a questo termine, al costo di una maggior complessità computazionale.
Sostituendo i singoli termini, l’equazioni della navigazione nel sistema earth-fixed componente per
componente
dx1e
 e
dx 2
dx3e

dx&1e

 e
dx& 2

 e
dx& 3

dt = x&1e
dt = x& 2e
dt = x& 3e
dt = f1e −
[( x )
dt = f 2e −
[( x )
dt = f 3e −
[( x )
e 2
1
e 2
1
e 2
1
GM
]
x3e
e 2 3/ 2
3
+ (x ) + (x )
e 2
2
]
x 2e + ω E2 x 2e − 2ω E x&1e
e 2 3/ 2
3
+ (x ) + (x )
GM
e 2
2
]
x1e + ω E2 x1e + 2ω E x& 2e
e 2 3/ 2
3
+ (x ) + (x )
GM
e 2
2
16
Un utile suddivisione dell’equazione differenziale U& bi = U bi Ω ib nelle diverse componenti richiede
l’introduzione della matrice di Rodriguez e della notazione dei quaternioni, argomenti che non
rientrano negli obiettivi di questo corso. Con questa ulteriore suddivisione il problema della
determinazione dello stato del veicolo può essere separato in due problemi disgiunti:
- prima si determina l’assetto del veicolo sulla base delle osservazioni dei giroscopi e
indipendentemente dalla posizione e velocità del veicolo stesso;
- poi si risolve il sistema di equazioni differenziali con posizione e velocità incognite,
supponendo noto l’assetto e ruotando le forze specifiche misurate dagli accelerometri nel
sistema earth-fixed.
Analisi dell’errore
Al fine di studiare le performance dei sistemi inerziali, consideriamo uno scenario molto
semplificato, assumendo che la Terra sia sferica e non rotante (utilizzeremo quindi un sistema di
riferimento inerziale) con un campo gravitazionale dipendente solo dalla distanza dal centro della
Terra. Supponiamo inoltre che l’asse x3 passi per il centro dell’area di interesse, cioè che il veicolo
si muova nell’intorno del polo nord (diametro massimo di qualche chilometro). Con queste ipotesi,
si può assumere in prima approssimazione che le coordinate cartesiane x1 e x2 descrivano il moto
“orizzontale”, mentre la coordinata x3 il moto verticale.
Il moto della dinamica in un sistema così approssimato possono essere descritte come
&x&i = f i + g i = f i −
GM
x
i 3
x
i
Ora le forze non gravitazionali sono osservate dagli accelerometri a bordo del veicolo con un certo
errore di misura, ovvero
f 0 = f +ν
i
i
i
ovvero possiamo scrivere
&x&i = −
GM
x
i 3
x + f 0 −ν
i
i
i
Linearizziamo l’equazione ottenuta al fine di studiare la propagazione del noise
&x&i = −
GM ~ i
i
i
i
i
x + J (x − ~
x ) + f 0 −ν
3
i
~
x
dove
17

 ∂
 ∂x i
 1
 ∂
J = GM  i
 ∂x1

 ∂
 ∂x i
 1
x1i
x
i 3
x 2i
x
i 3
x3i
x
i 3

∂ x1i 
∂x3i x i 3 

i 
∂ x2

∂x3i x i 3 

∂ x3i 
∂x3i x i 3 

∂ x1i
∂x 2i x i 3
∂ x 2i
∂x 2i x i 3
∂ x3i
∂x 2i x i 3
(matrice jacobiana)
Nel calcolo delle derivate parziali si distinguono due casi
x ki
∂ x ki
∂
=
∂x ki x i 3 ∂x ki ( x1i ) 2 + ( x 2i ) 2 + ( x3i ) 2
[
=
[( x )
i 2
1
]
]
3/ 2
=
[
3 i 2
( x1 ) + ( x 2i ) 2 + ( x3i ) 2
2
3
( x1i ) 2 + ( x 2i ) 2 + ( x3i ) 2
+ ( x 2i ) 2 + ( x3i ) 2
3/ 2
− x ki
[
]
] [
]
1/ 2
2 x ki
=
] ( )  =
] 
 (xi )2 + (xi )2 + (xi )2 − 3 xi
2
3
k
(
)
(
)
(
)
x
x
x
=
+
+
 1
i 2
i 2
i 2 3/ 2
i 2
i 2
i 2 3/ 2
 ( x1 ) + ( x 2 ) + ( x3 )
( x1 ) + ( x 2 ) + ( x3 )
2


3 x ki
1
1
=
−


3
/
2
i
2
i
2
i
2
( x1i ) 2 + ( x 2i ) 2 + ( x3i ) 2
 ( x1 ) + ( x 2 ) + ( x3 ) 
1
[
[
]
[
]
[
=
i 2
2
− x ki
[( x )
i 2 1/ 2
3
]
3/ 2
[
[
i 2
1
]
]
+ ( x 2i ) 2 + ( x3i ) 2
]
3/ 2
2
=
1/ 2
3 i 2
( x1 ) + ( x 2i ) 2 + ( x3i ) 2 2 x hi
2
=
3
( x1i ) 2 + ( x 2i ) 2 + ( x3i ) 2
1
[
( )
x ki
∂ x ki
∂
=
∂x hi x i 3 ∂x hi ( x1i ) 2 + ( x 2i ) 2 + ( x3i ) 2
=
i 2
1
con k ≠ h


3 x ki x hi
−

i 2
i 2
i 2 
 ( x1 ) + ( x 2 ) + ( x3 ) 
In altri termini la matrice jacobiana può essere scritta come
J =−
GM
(~
x ) + (~
x 2i ) 2 + ( ~
x3i ) 2
[
i 2
1
]
3/ 2
( I − 3P )
dove il proiettore radiale P è definito come
( )
2
~
x1i
 i ~i
1
P = ~i 2 ~i 2 ~i 2 ~
x1 x 2
( x1 ) + ( x 2 ) + ( x3 )  ~ i ~ i
 x1 x3
~
x1i ~
x 2i
2
~
xi
( )
2
i i
2 3
~
x ~
x
~
x1i ~
x3i  ~ i ~ i +
 x x
~
x 2i ~
x3i  =
i 2
~
i 2
~
x
x3 

( )
( )
18
L’equazione della dinamica può essere spezzata in due parti
&x& + δ &x&i ,
&x&i = ~
i
una che descrive la traiettoria di riferimento dovuta all’applicazione delle forze specifiche osservate
i
GM ~ i
i
&~
x + f0
x& = −
3
i
~
x
e un’altra che invece descrive l’effetto del noise
δ &x&i = −
GM
(~
x1i ) 2 + ( ~
x 2i ) 2 + ( ~
x3i ) 2
[
]
3/ 2
( I − 3P ) δ x − ν
i
i
dove
δ x i = x i − ~x i
Nel caso particolare dello schema considerato, ricordando il raggio medio terrestre R=6378 km,
valgono le seguenti approssimazioni
~
x1
~ 10 −3 ,
R
~
x2
~ 10 −3 ,
R
~
x3 ~ R
cosicché, a meno di errori dell’ordine di 10-3, è possibile considerare le seguenti approssimazioni
(~
x1i ) 2 + ( ~
x 2i ) 2 + ( ~
x3i ) 2 ≅ R 2
( )
2
~
x1i
1  i ~i
P ≅ 2 ~
x1 x 2
R  ~i
 x1 R
~
x1i ~
x 2i
2
~
x 2i
~
xiR
( )
2
~
x1i R  0 0 0

~
x 2i R  ≅ 0 0 0
R 2  0 0 1

ottenendo il seguente sistema di equazioni differenziali per la dinamica dovuta al noise
δ&x&1i = −ω s2δx1i − ν 1i
 i
i
i
2
δ&x&2 = −ω s δx 2 − ν 2
δ&x&i = 2ω 2δx i − ν i
3
3
s
 1
dove
ωs =
GM
≅ 1.24 10 −3 rad / s
3
R
è detta frequenza di Schuler e corrisponde ad un periodo di T=84.46 minuti.
19
Se ora supponiamo di applicare un noise di misura solo in un certo istante, possiamo studiare come
evolve la dinamica del sistema in seguito a questo ingresso impulsivo considerando il sistema di
equazioni omogeneo associato. In altri termini, se il sistema è asintoticamente stabile, l’effetto del
noise impulsivo deve esaurirsi dopo un certo transitorio. In alternativa si possono verificare
comportamenti oscillanti o divergenti.
Il sistema omogeneo associato può essere scritto come
δ&x&1i + ω s2δx1i = 0
 i
2
i
δ&x&2 + ω s δx 2 = 0
δ&x&i − 2ω 2δx i = 0
3
s
 1
Esistono ovviamente le soluzioni triviali ( x1 = 0 , x 2 = 0 , x3 = 0 ) che corrispondono a non applicare
al sistema alcun segnale impulsivo del noise e quindi a lasciare il veicolo fermo; queste soluzioni
ovviamente non sono di nostro interesse.
La prima considerazione che si può dedurre è che le componenti cartesiane hanno una dinamica
disaccoppiata; nel caso di modelli più complessi questo è verificato solo in parte, nel senso che le
due componenti “orizzontali” (x1,x2) si legano tra di loro (si pensi per esempio all’accelerazione di
Coriolis nel caso in cui si modellizzi anche la rotazione della Terra).
La seconda considerazione è che le componenti “orizzontali” (x1,x2) hanno una dinamica
completamente diversa dalla componente “verticale” (x3). La soluzione delle prime due equazioni
differenziali è infatti un oscillatore armonico con frequenza pari alla frequenza di Schuler, cioè
δx ki (t ) = δx0 sin(ω s t )
k=1,2
dove δx0 è l’ampiezza massima dell’oscillazione. Questo significa che l’effetto del noise non tende
ad esaurirsi (il sistema non è asintoticamente stabile), ma è limitato e quindi controllabile nel
tempo. Lo stesso non accade nel caso della terza equazione differenziale che descrive il moto
“verticale”, la cui soluzione è un oscillatore armonico inverso
(
δx3i (t ) = δx0 exp 2ω s t
)
dove δx0 è lo spostamento verticale iniziale a t=0 legato all’impulso iniziale. Dopo un intervallo di
tempo di dieci minuti, l’errore è già tre volte più grande dell’errore iniziale. E’ per questo motivo
che i sistemi inerziali “stand-alone” possono essere utilizzati per la navigazione “orizzontale”, ma
non per quella in “verticale”, dove richiedono il supporto di altre tecniche come per esempio il GPS.
Vedi programma MATLAB su simulazione degli errori.
Bibliografia
B. Hofmann-Wellenhof, K. Legat, M Wieser (2003). “Navigation. Principles of positioning and
guidance”. Springer-Verlag.
F. Sansò (2006). “Navigazione geodetica e rilevamento cinematico”. Polipress
L. Biagi (2003). Appunti del corso di Misure Geodetiche.
http://geomatica.como.polimi.it/corsi/misure_geodetiche/
20

Documenti analoghi