POLITECNICO DI TORINO

Dipartimento di Ingegneria dell’Ambiente, del Territorio e

delle Infrastrutture

Principi di posizionamento inerziale e navigazione

Rif. «dal GPS al GNSS – cap. 19»

Alberto CINA

Corso di Geomatica
 La navigazione

Seguire e rilevare una traiettoria: alcuni metodi:

• Seguire le indicazioni (gira a destra, a sinistra …)

➔ osservazione ambiente circostante e riconoscimento di
particolari da usare come riferimento

• Cartografia: determiniamo la posizione con l’osservazione e il

riconoscimento di particolari cartografati

• Osservazione e misure verso particolari naturali noti (stelle)

• GNSS … se abbiamo la visibilità sui satelliti, altrimenti…

• Posizionamento inerziale, integrato a GNSS

 Posizionamento inerziale

Le misure inerziali sono soggette alle leggi della meccanica

classica Newtoniana:
1. Un corpo permane nel suo stato di quiete sino a quando
non è disturbato da forze esterne
2. La forza è proporzionale alla variazione di velocità nel
tempo e quindi alla variazione di posizione
➔ la capacità di misurare la forza permette di conoscere
l’accelerazione e quindi la variazione di posizione
(usiamodegli accelerometri)

➔ è necessario conoscere la direzione della forza e

dunque degli accelerometri durante il moto del corpo
(usiamo dei giroscopi)
Accelerometri e giroscopi sono strumenti che fanno parte di un
«SISTEMA INERZIALE»
 Richiami su principi fisici: QUANTITÀ DI MOTO
È una grandezza vettoriale che misura la capacità:
di un corpo di
interagire dinamicamente con altri corpi. Un punto di massa m
con velocità v ha una quantità di moto Q pari a:
   n 

Q = mv e con n punti: Q =  Qi
i =1
Nella meccanica classica la quantità di moto è correlata al 2°
principio della dinamica. La forza è proporzionale alla rapidità con
cui varia la quantità di moto. Nell’ipotesi di massa costante:
  
dQ d (mv ) dv  
= =m = ma = F
dt dt dt
 
Nella meccanica relativistica,
posto c la velocità della luce,
Q = mv
1
Con: = Fattore di LORENTZ
2
1− v
c2
 Richiami su principi fisici: MOMENTO ANGOLARE
È legato alle rotazioni nello spazio. Costituisce l’equivalente per le
rotazioni della quantità di moto per le traslazioni.

Il momento angolare L di un punto di massa m

rispetto a un polo Ω, è il prodotto vettoriale:
    
L = r xQ = r xmv  n 
e con n punti: L =  Li
i =1
La sua variazione nel tempo   
è il momento torcente  = r xF
   
  dQ          
dL d 
=
dt dt
( )
r xQ =
dr
dt
xQ + r x
dt
= (v − v0 )xQ + r xF = v xQ + v0 xQ + 

 dL
Ricordando che v e Q sono paralleli, con v0=0:  =
dt
 Richiami : CONSERVAZIONE DEL MOMENTO ANGOLARE
L si conserva (L=cost.) se il momento delle forze esterne ( ) è nullo:

  dL   
Se  =0 allora  = = r xF = 0 per cui L = costante
dt
Ciò può avvenire per diversi motivi:
• il sistema è isolato (F=0)
• la forza F è applicata in un punto dell’asse di rotazione (r=0)
• il prodotto vettoriale si annulla in quanto F parallelo a r

Il momento angolare L può essere espresso come il prodotto del

momento d’inerzia I per la velocità angolare :
   2  
L = r xmv = r m = I
La variazione di L nel tempo è il prodotto del momento d’inerzia per
l’accelerazione angolare α :
  
dL d (I ) d 
= =I = I
dt dt dt
 Richiami : FORZA DI CORIOLIS

È una forza apparente Fc a cui è soggetto un corpo che si muove

con velocità v, osservata da un sistema di riferimento in moto
circolare con velocità angolare  rispetto a un sistema inerziale
(SI). Essa è data da:    
FC = maC = 2mxv
Esempio: disco rotante sul quale si muove un corpo C a velocità v
e con osservatore posto in O:

O

SI C
v

Osservatore inerziale: C si Osservatore rotante: C si

muove con traiettoria rettilinea muove con traiettoria curvilinea

L’accelerazione di Coriolis è utile per esprimere le forze in

diversi sistemi di riferimento
 Il giroscopio meccanico
È uno strumento in grado di misurare velocità angolari.
L • Rotore con massa m e g elevata (10000-
 g
20000 giri/min) sospeso con giunti cardanici
Fr
(GIMBAL) ad un involucro
• Momento  ➔ momento angolare L lungo
r l’asse di spin 
dL 
• In assenza di Forze perturbatrici: = = 0
dt
➔ l’asse spin non varia l’orientamento
Asse di spin X
qualunque posizione assume l’involucro

Asse di spin

Cosa succede se applichiamo una coppia eterna?

 La PRECESSIONE del giroscopio
Applichiamo una coppia perturbatrice
Y CZ con forze F parallele a Y ➔ si forma
F
un momento angolare LZ
Z
Z ➔ L’asse X ruota intorno a Y per
F CZ disporsi ⊥ al piano della coppia CZ
Y (movimento di PRECESSIONE con
X
velocità angolare )
Raggiunta tale posizione i momenti angolari si sommano (L=L+LZ).
Raggiunta tale posizione LZ va ad aumentare la velocità di rotazione
del disco (poco influente in quanto già molto elevata)
Posizione d’equilibrio: quando l’asse di spin è ⊥ al piano della coppia.
La precessione cessa perché F non ha più componenti devianti

N:B.: Coppia CZ ➔ giroscopio precede con rotazione intorno a Y

Coppia CY ➔ giroscopio precede con rotazione intorno a Z
C LEGGE DEL
La precessione è una «difesa» del
=
giroscopio da forze perturbatrici I g GIROSCOPIO
 La GIROBUSSOLA
Si orienta secondo il NORD «vero» (astronomico), dato dalla
posizione istantanea dell’asse di rotazione terrestre.
La rotazione terrestre fa cambiare continuamente la direzione della
verticale in un sistema inerziale. Un esempio nel piano equatoriale:

Giroscopio libero: l’asse di spin

mantiene inalterata la sua direzione
indipendentemente da quella della
verticale

Giroscopio con Z vincolato alla

verticale (pendolo): il pendolo segue
la verticale locale e da continuamente
una rotazione intorno a Y (coppia CY)
➔ precessione intorno a Z fino a
quando l’asse di spin X non è ⊥ a CY

X si pone in direzione dell’asse rotazione terrestre

 La GIROBUSSOLA
Ad una latitudine  la posizione d’equilibrio sarà sempre nord-sud.
La girobussola può apprezzare la
componente orizzontale della rotazione
terrestre : 1 =  cos 
Se il giroscopio è vincolato in altezza
con un filo (detto di «torsione») che lo
sostiene lungo la verticale, possiamo
considerare la sola componente nord-
sud.
C
Dalla legge del giroscopio:  =
I g
C = I g 1 = I g cos
C
= Apprezziamo la v. rotaz.
I g cos  terrestre  nota 
La girobussola può essere abbinata ad un
teodolite, per limitare la propagazione
dell’errore angolare in lunghe gallerie
 Il giroscopio OTTICO (FOG - Fiber Optic Gyroscope)
Il principio si basa sull’effetto relativistico di Sagnac (1913)

E(t+Δt)
Spira circolare di fibra ottica di area
R
 Segnale luminoso E(t) A, in rotazione () in un sistema
Sistema
inerziale inerziale, percorsa da un segnale
luminoso E(t0).
Il segnale è raccolto da un osservatore solidale:
• Alla fibra: dopo  ’ = 2
• Al SI: dopo  ’ = 2 + Δ a causa della rotazione (Δ =  Δt)
La differenza di percorso osservata dai due 
osservatori sarà (dimostrazione nel libro GNSS):  = 2A
c
Se percorriamo la spira con un segnale in senso 
opposto, la differenza di percorso osservata sarà:  = −2 A
c
 − (−) 4A
Lo sfasamento tra i 2 segnali è:  = =  proporzionale alla
 c
4A
v. angolare  di un fattore di «sensibilità del giroscopio»:
c
Per aumentare la sensibilità si aumenta l’area e quindi si usano bobine
 ACCELEROMETRO MECCANICO
È uno strumento in grado di misurare una «accelerazione specifica»
f, data dall’accelerazione del moto (a) e di quella di gravità (g).
  
f =a−g
m aX ➔ Separare le due componenti per
X
calcolare l’ascissa curvilinea con un
procedimento di doppia integrazione
 ➔ Conoscere un modello di gravità g
Massa «di prova» m, vincolata ad un chassis per mezzo di una
molla. Lo chassis vincola la massa a muoversi secondo X. Quando
lo strumento (o il mezzo a cui è vincolato) subisce un’accelerazione
aX, si crea una forza F=maX che comprime la molla di , proporzionale
a F tramite la costante K della molla e misurabile su di una scala:
aX aX Può essere preferibile misurare non
F = ma X = K   = =
K 0 lo spostamento  ma fornire la
m reazione opposta per riportare la
0 = K m valore caratteristico massa nella posizione iniziale,
accelerometro misurando la forza
 Le componenti dell’accelerazione
Ricordiamo che l’accelerazione specifica f è data
 dall’accelerazione
del moto (a) e da quella di gravità (g). f =a−g
Se disponiamo un accelerometro su di un piano soggetto alla
forza di gravità g, la massa è attratta verso il basso e l’output
dell’accelerometro è di segno opposto all’accelerazione locale
Per meglio interpretare questa relazione ricorriamo a 2 esempi, con
misura dell’accelerazione con piattaforma inerziale:
Sensore fermo su un Sensore in caduta libera
tavolo: nel vuoto:

a = 0  f = −g a=g  f =0
 MAGNETOMETRI
È uno strumento in grado di misurare il campo magnetico terrestre,
fornendone la direzione; alcuni anche il modulo.
La sua più semplice realizzazione è un ago
magnetizzato in sospensione che si orienta
parallelamente al Nord magnetico, ma ne
esistono anche di vari altri tipi.
Il campo magnetico terrestre (o
geomagnetico) è una caratteristica
naturale del nostro pianeta. Assimilabile ad
un dipolo con angolo 11.3° rispetto all’asse
di rotazione terrestre
L’unità di misura del campo magnetico è il TESLA (T nel S.I.) ma in
pratica si usano i sottomultipli (nT=10-9T) o il Gauss (1G=10-4T)
Il magnetometro fornisce la direzione del nord magnetico che va
corretta della declinazione magnetica per ottenere il nord
geografico. Essi possono essere presenti sui sensori inerziali
 I SENSORI INERZIALI
Costituiti in generale da una terna di accelerometri e giroscopi,
solidali ad una piattaforma, disposti secondo una terna cartesiana
Zb

gZ Chiameremo questo sistema di

riferimento strumentale, sistema
aZ gX Body o b-frame.
aX Xb
aY
Possono essere classificati a
seconda della capacità di
gY
Yb
elaborazione dello strumento
stesso in:
ISA (Inertial Sensor Assembly): fornisce i dati «grezzi» di
accelerazione e velocità angolare;
IMU (Inertial Measurement Unit): i dati grezzi sono compensati degli
errori sistematici (bias) di misura;
INS (Inertial Navigation System): forniscono la stima della posizione
e assetto mediante processamento dei dati grezzi con gli algoritmi
di navigazione.
 CONFIGURAZIONI di sensori inerziali

GIMBALED: piattaforma dei sensori cardani

isolata meccanicamente dal moto del
veicolo con giunti cardanici (gimbal)
controllati dai giroscopi. Piattaforma
stabile
La piattaforma non subisce rotazioni
e fornisce l’output degli accelero-
metri nel sistema inerziale
Anelli
cardanici

Gruppo di 3
accelerometri e
3 giroscopi
STRAPDOWN: sensori inerziali solidali
ad una piattaforma rigida vincolata al
mezzo in movimento.
Gli accelerometri seguono il mezzo
nella sua dinamica e forniscono
l’output nel sistema body
 Esempi di sensori inerziali

IMU Strapdown

IMU Gimbaled
 Calcolo POSIZIONE con IMU – esempio monodimensionale
La posizione con IMU può essere risolta a partire da una posizione
iniziale. Supponiamo un moto monodimensionale lungo X, nota la
posizione iniziale X(t0) e misurata l’accelerazione aX.
La posizione sarà data da una doppia integrazione:

t
X (t ) = X (t0 ) +  X ( )d = X (t0 ) + X (t ) ( t − t0 ) velocità
t0
t
1 1
X (t ) = X (t 0 ) +  X ( )d =X (t 0 ) + X (t0 ) ( t − t0 ) + X (t )t02 + X (t )t 2 posizione
t0
2 2

N.B.: prima di eseguire l’integrazione è necessario separare dalla

misura dell’accelerometro l’accelerazione di gravità, per integrare
solo l’accelerazione dovuta al moto
 Calcolo POSIZIONE con IMU – esempio 2D
Per la stima della posizione le accelerazioni devono essere nello
stesso sistema di riferimento e,n del moto. Per le 2 configurazioni:

Soluzione meccanica Gimbaled: Soluzione analitica Strapdown:

grazie ai giroscopi gli accelerome- occorre conoscere gli assetti (A)
tri sono posizionati con servo mo- degli accelerometri per portarli dal
tori nel sistema inerziale sistema body a quello del moto

a X (t − t0 )
1
e = e0 +  a X dt = e0 +
2
 e   e0   cos A sin A  a X 
  =   +  
2
  
n = n0 +  aY dt = n0 + aY (t − t0 )
1 2
 n   n0  − sin A cos A aY 
2
N.B.: le velocità angolari dei giroscopi sono date in un sistema inerziale
 Misura delle inclinazioni con accelerometri
Un accelerometro triassiale misura le accelerazioni specifiche sui 3
assi: da esse sono ricavabili le inclinazioni
Tempo ax ay az
…………………………
2927;-0.972003;0.041515;0.057403;
2932;-0.972003;0.041515;0.057403;
2937;-0.972003;0.041515;0.057403;
2942;-0.972003;0.041515;0.057403;
2947;-0.972003;0.041515;0.057403;
2952;-0.972003;0.041515;0.057403;
2957;-0.972003;0.041515;0.059450;

Ax
  = arctg
Ax 2 + Ay 2 + Az 2
Ay
 = arctg
 Ax 2 + Ay 2 + Az 2
Sistema di riferimento: 
Ax 2 + Ay 2
interno, legato all’oggetto 1g  = arctg
Az
(body)
 BIAS negli IMU

Errori sistematici nelle misure degli accelerometri ba (bax, bay, baz)

e dei giroscopi bg (bgx, bgy, bgz).
Consideriamo una componente «statica» (S), fondamentalmente
uguale durante la misura e una componente «dinamica» (D)
variabile nel corso della misura. I bias totali saranno la somma delle
due componenti:
• per gli accelerometri: ba=baS+baD
• per i giroscopi: bg=bgS+bgD

I bias degli accelerometri sono generalmente espressi in Gal,

unità SI (1 Gal = 0.01 m/s2)
I bias dei giroscopi sono generalmente espressi in °/ora
Possiamo classificare gli IMU in base ai loro bias
 CLASSIFICAZIONE IMU in base ai BIAS
Bias
Categoria Accelero- Girosco- Applicazioni derive e costi indicativi:
metri [mg] pi [ °/ora]
Navali, sottomarini, veicoli spaziali.
Marine grade 0.01 0.001 Deriva: entro 1 miglio/giorno
Costo: > 1M€.
Veicoli militari, velivoli civili.
Aviation grade 0.03 - 0.1 0.01 Deriva circa 1 miglio/ora
Costo: > 100 k€
Intermediate Piccoli velivoli, elicotteri.
grade 0.1 - 1 0.1 Deriva circa 10 miglia/ora
Costo: 20 – 50 k€
Droni UAV, MMS (Mobile Mapping System)
Tactical grade 1-10 1 – 100 Soluzione di navigazione valida pochi minuti
➔ indispensabile integrazione con GNSS
Costo: 5-20 k€
Automotive Industria automobilistica (ABS).
grade >10 >100 Hanno ridottissime dimensioni.
Costo: da pochi € a poche centinaia di €
 Accelerometri e modello d’errore

Errori sistematici: gli accelerometri possono essere influenzati da:

Z
• un bias traslativo;
• un fattore di scala; X
• la non ortogonalità dei tre assi

Y
Posto l le misure reali, t le misure teoriche, e il bias traslativo e E la
matrice con i fattori di scala e non ortogonalità, possiamo esprimere
il modello come
(l ) =  E (t ) + e
 t1 
 lx   e e   t1   e  e e  
1 2 3 4 1 2 3 4
1 e1 1 1 1 e 1 e1 1
          t2 
 l y  = e e   t2  +  e  = e
1 2 3 4 1 2 3 4
e e e e 
 l  e
2 2 2 2 2 2 2 2
t 3 
 z 
1
3 e2
3 e   t3   e
3
3
4
3
 e
 
1
3 e 2
3 e3
3 e   
4
3
1
 Accelerometri e modello d’errore
Il modello d’errore si esprime in una matrice di calibrazione con 12
parametri dove:
• gli elementi diagonali sono i fattori di scala dei 3 assi;
• gli elementi fuori diagonale sono i fattori di non ortogonalità o
misalignment
• la 4° colonna contiene i valori dei bias traslativi sui 3 assi;

 t1  ➔ Stimabili con misure fatte

 lx   e e  
1 2 3 4
1 e1 e1 1
     t2  ruotando lo strumento su 6
 l y  = e
1 2 3 4
e e e  facce ortogonali
 l  e
2 2 2 2
t 3 
 z 
1
3 e2
3 e3
3 e   
4
3 Per le sei facce essi sono di
1 volta in volta:

g  −g  0  0  0  0 

           
 0   0   g   − g   0   0 
t1Fac. = ;t = ;t = ;t = ;t = ;t =
 0  2 Fac.  0  3 Fac.  0  4 Fac.  0  5 Fac.  g  6 Fac.  − g 
           
 
1  1  
1  1  
1  1 
 Accelerometri e modello d’errore
In questo modo si costruisce un sistema col seguente bilancio di
equazioni e incognite:
• 12 incognite relative agli elementi della matrice 3x4 di
calibrazione:
• 3 equazioni per ogni epoca di misura e per ognuna delle 6 facce.
Il sistema può essere scritto per ogni epoca di misura (es. 1°
faccia):

 lx  e1 g 0 0
1
0 e14 0 0 0 0 0 0 0
   
 ly  =  0 0 0 0 e12 g 0 0 e24 0 0 0 0
l   0 0 e31 g 0 0 e34 
 z  0 0 0 0 0 0
e11 e12 e13 e14 e12 e22 e23 e24 e31 e32 e33 e34

Da risolvere con mq
 Procedure e matrice di calibrazione

1.0238937e+00 1.1799075e-02 -4.4547501e-03 5.1224124e-02

1.7951685e-02 1.0252416e+00 1.3150092e-02 5.9625874e-02
5.6249677e-03 -2.2480179e-02 1.0248541e+00 5.8273893e-02

iniziali [g] calibrati [g] rotazioni [gon]

tempo ax ay az ax ay az Rx Ry FI
7060 0.05286 0.07283 1.09189 0.00439 -0.00324 1.00806 -0.2047 0.2774 100.3447
7065 0.05286 0.07283 1.09189 0.00439 -0.00324 1.00806 -0.2047 0.2774 100.3447
7070 0.05286 0.07283 1.09189 0.00439 -0.00324 1.00806 -0.2047 0.2774 100.3447

Errori accidentali: derive termiche non modellate, elettronica

 Effetto dei BIAS sugli ACCELEROMETRI

Bias «non compensati» deviano la soluzione di posizione e velocità.

Analizziamo un caso 1D sugli accelerometri con bias ba:
con una prima integrazione abbiamo la velocità:

v =  adt =  (X + ba )dt = X + ba t

con una seconda integrazione abbiamo la posizione:

X =  vdt =  (X + ba t )dt =  Xdt +  ba tdt =X + ba t

  1 2
2
Un bias sulle accelerazioni produce un bias sulla posizione
proporzionale a:
• al tempo t sulle velocità
• al quadrato del tempo sulle posizioni
 Effetto dei BIAS sui GIROSCOPI
L’errore δ sull’angolo di assetto è proporzionale al tempo intercorso
dalla posizione iniziale δ=bg t. Esso si ripercuote sull’accelerometro
al quale fornisce una errata direzione di proiezione.
Ciò provoca un errore X g nella misura
dell’accelerometro, legato principalmente alla
rotazione del vettore forza di gravità g.
Esso può essere scritto:
X g = g sin   g = gbg t
L’effetto sulla velocità sarà proporzionale al quadrato
del tempo:
v =  (X + X g )dt =  Xdt +  gbg tdt = X + gbg t 2
1
2
L’effetto sulla posizione sarà proporzionale al cubo del tempo:
1 1
X =  vdt =  Xdt + gbg  t 2 dt = X + gbg t 3
2 6
 Effetto dei bias su POSIZIONE e VELOCITÀ: sintesi
Bias misura Bias velocità Bias posizioni

ba v a
Bias
accelerometri f(t) f(t2)

t t t

bg v a
Bias giroscopi
f(t2) f(t3)

t t t

N.B.:questi bias possono

essere parzialmente
compensati con una calibra-
zione, ruotando lo strumento
su ognuna delle 6 facce.
 Sistema inerziale: eclittica ed equatore celeste

Eclittica: intersezione del

piano orbitale della Terra
intorno al sole, con la sfera
celeste
L’intersezione del piano
equatoriale con la sfera celeste
è detto Equatore celeste
l'asse di rotazione della Terra
non è perpendicolare al suo
piano orbitale, il piano
equatoriale non è parallelo al
piano dell'eclittica, ma forma
con esso un angolo di circa 23°
La linea d'intersezione tra i due piani
27' noto come inclinazione
definisce due punti equinoziali: quello in cui
dell'eclittica.
il Sole passa da sud a nord dell'equatore
celeste (cioè l'equinozio di primavera) viene
chiamato punto vernale.
 Navigazione inerziale: SISTEMI DI RIFERIMENTO
Inerziale (i-frame): origine centro
di massa della Terra e senza
rotazioni rispetto alle stelle fisse.
Zi asse polare, Xi intersezione tra
eclittica e piano equatoriale (V),
Yi completa la terna destrorsa.
Terrestre (e-frame) o ECEF:
origine centro di massa della Terra
e in rotazione (e) rispetto a SI.
Ze asse rotazione per un polo
medio, Xe intersezione piano
Di navigazione (n-frame) o terna equatoriale e meridiano GW, Ye
euleriana: origine nella posizione completa la terna destrorsa.
del sistema di navigazione.
Zn normale per il punto di strumentale (b-frame) o body:
navigazione, Yn tangente al Xb, Yb , Zb solidali al veicolo. Per
meridiano verso nord, Xn completa allinearlo a n-frame occorre
la terna destrorsa. conoscere assetti Ω, , K.
 TRASFORMAZIONI tra sistemi di riferimento
Possono essere risolte con rototraslazioni 3D, noti i parametri di
assetto angolare (Attitude) e le traslazioni. È importante portare il
sistema body negli altri sistemi di riferimento.
Coseni direttori: raccolti in una matrice R(3x3) le cui colonne rap-
presentano i versori nel sistema body, proiettati sull’altro sistema:
 c11 c12 c13 
Rbn = c21 c22 c23  Trasformazione di un vettore r n = Rbn r b
r da body a n-frame:
c31 c32 c33 
Angoli di Eulero: 3 successive rotazioni intorno agli assi del sistema
body per allinearlo all’altro sistema di riferimento:
• Ω (rollio - ROLL): rotazione antioraria intorno a X;
•  (beccheggio-PITCH): rotazione oraria intorno a Y;
• K (sbandamento o imbardata - YAW): rotazione antioraria intorno a Z.
Tali rotazioni sono quelle che possono essere misurate dai giroscopi
in un sistema inerziale i-frame
 Da n-frame a b-frame (angoli Eulero)

1 0 0  cos  0 − sin    cos K sin K 0

Rnb = R X RY RZ = 0 cos  sin    0 1 0  − sin K cos K 0 =
0 − sin  cos    sin  0 cos    0 0 1
 cos  cos K cos  sin K − sin  
= sin  sin  cos K − cos  sin K sin  sin  sin K − cos  cos K sin  cos  
cos  sin  cos K + sin  sin K cos  sin  sin K − sin  cos K cos  cos  

 1 K − 
Per piccole rotazioni la matrice di Rnb = − K 1  
rotazione assume la forma linearizzata:
  −  1 

Per il passaggio da b-frame a  1 −K  

n-frame ricordiamo le proprietà Rbn = Rnb = Rnb = RXT RYT RZT =  K −  
−1 T
1
di ortogonalità della matrice di −   1 
rotazione:
 Da n-frame a b-frame - velocità angolari

Possiamo propagare la matrice di rotazione nel tempo, note le

velocità angolari   K  del sistema body b-frame rispetto al
 
sistema di navigazione n-frame. La velocità di cambiamento della
matrice R sarà (omettendo la dimostrazione):


Rb = lim
n R b
n
= lim
Rb
n
(t + t ) − Rb (t )
n
= Rbn  bn
t →0 t t →0 t

Con Ω matrice «emisimmetrica» delle velocità angolari del b-frame

rispetto a n-frame:
 0 − K  
 
 bn =  K 0 − 
−  
 0 

 Da e-frame a n-frame
La trasformazione può essere ricavato dalle coordinate geografiche
(lat. , long. , alt. ellissoidica h) del punto di navigazione, a partire
dalle equazioni parametriche dell’ellissoide ad altezza ellissoidica h:

 X 0 = (N + h )cos  cos 

Y0 = ( N + h ) cos  sin 
a
con: N= 
1 − e sin 
 
2 2

 0
Z = N (1 − e 2
) + h sin 

 X n   − sin  cos  0  X e − X 0 
     
 Yn  = − sin  cos  − sin  sin  cos   Ye − Y0 
 Z   cos  cos  cos  sin  sin   Z e − Z 0 
 n 

 X e   X 0   − sin  cos 
T
Per le relazioni 0   Xn 
      
inverse, da n-frame  Ye  =  Y0  + − sin  cos  − sin  sin  cos    Yn 
a b-frame:  Z   Z   cos  cos  cos  sin  sin   Z 
 e  0   n
 Da e-frame a i-frame
Consideriamo la rotazione del piano equatoriale (l’asse Z di
rotazione terrestre si mantiene uguale nei due sistemi) del valore
et , con e velocità di rotazione terrestre:

 X i   cos et sin et 0  X e 

     
 Yi  = − sin et cos et 0 Ye 
Z   0 1 Z e 
 i  0

Analogamente per le relazioni inverse, da i-frame a e-frame:

 X e  cos et − sin et 0  X i 

     
 Ye  =  sin et cos et 0 Yi 
Z   0 1 Z i 
 e  0
 Equazioni di navigazione 2D in un sistema inerziale i-frame
Caso teorico, da cui derivare poi le equazioni 3D. Stimiamo
posizione e velocità di un:
• veicolo vincolato in un piano inerziale Xi Zi;
• sistema strapdown con 2 accelerometri e 1 giroscopio.
 è l’angolo, misurato dal
giroscopio, per allineare i
sistemi i-frame e b-frame:
 = Yb   =  Yb dt

 f Xi   cos  sin   f Xb 
e portare le accelerazioni   =    
«specifiche» misurate, nel SI:  f Zi  − sin  cos   f Zb 

Separiamo le accelerazioni v Xi = f Xi + g Xi
«vere» da quelle specifiche a = v
con un modello di gravità noto: vZi = f Zi + g Zi
 Equazioni di navigazione 2D in un sistema Inerziale i-frame

Integriamo nel tempo una Diagramma di flusso:

volta per ottenere le velocità:

v Xi =  v Xi dt = X i

vZi =  vZi dt = Z i

… e una seconda volta per

ottenere le posizioni nel SI:

X i =  v Xi dt

Z i =  vZi dt
 Equazioni di navigazione 2D in un sistema n-frame
Esprimiamo le equazioni precedenti nel sistema di navigazione, con
ipotesi di Terra sferica (normale e verticale coincidono).
• Veicolo vincolato in un piano meridiano Xi Zi e vincolato alla
verticale locale;
• Sistema strapdown con 2 accelerometri e 1 giroscopio.
vx
La velocità angolare  =
del veicolo sarà: R+h
Ora però l’angolo d’assetto  misurato
dal giroscopio, è modificato per la
rotazione del sistema locale rispetto a SI:

 vX
 = b −  = b −   =  dt
R+h
portiamo le accelerazioni «specifiche»  f Xn   cos  sin   f Xb 
  =    
misurate, nel sistema di navigazione:  f Zn  − sin  cos   f Zb 
 Equazioni di navigazione 2D in un sistema n-frame
Per le accelerazioni «vere» in n-frame, togliamo da quelle inerziali
(a) l’accelerazione di Coriolis e g nella sola componente verticale Zn:
Diagramma di flusso:
v
aC = 2xv = 2v = 2 X v
R+h
2
v
v Xn = a Xn − 2 X
R+h
v v
vZn = aZn − 2 X Z + g
R+h f Zn f Xn
z

Integriamo per la velocità:

v Xn =  v Xn dt = X n

vZn =  vZn dt = Z n
X n =  v Xn dt
e integriamo ancora per la posizione:
Z n =  vZn dt
 Cenni su navigazione 3D
Da 2D a 3D aumenta la complessità delle equazioni ma con simile
ragionamento. All’accelerazione misurata nel sistema body va
sommato g per ottenere le accelerazioni del moto. Sia r il vettore
posizione del punto nel sistema inerziale:
  
d 2r
vi = 2 = f + g
dt i
consideriamo l’accelerazione di Coriolis nel sistema di navigazione,
«rotante» rispetto a quello inerziale.
L’accelerazione è sempre misurata nel sistema body e andrà fatto
un cambio di sistema di riferimento, con una matrice di rotazione R:

a i = Rbi f b

 dr
Integriamo poi una volta per stima della velocità: vi =
dt i
E infine una seconda volta per la stima della posizione.
 Navigazione geodetica integrata: PRINCIPI DI BASE
Definiamo, con misure inerziali e GNSS, la posizione e l’assetto di
una terna cartesiana solidale al veicolo e la sua evoluzione rispetto
ad un sistema di riferimento terrestre. Due gli aspetti:
• Navigazione in tempo reale (RT): la soluzione in tempo reale (o
quasi…) consente il controllo del veicolo e di «reagire» per
portarlo su di una traiettoria voluta.
• Navigazione in post elaborazione (PP) o «navigazione
geodetica»: definiamo posizioni e assetti a posteriori, senza
voler guidare il veicolo. Serve per georeferenziare sensore in un
sistema terrestre (Camere fotogrammetriche, LIDAR …)
Bias su acccelerometri e
giroscopi fanno derivare la
soluzione nel tempo.
➔ necessità di una tecnica
indipendente (navigazione
integrata)
integrazione GNSS e IMU: posizionamento inerziale assistito
Le misure inerziali possono essere integrate con quelle da
magnetometri, odometri, barometri, fotogrammetria, cartografia
numerica... La tecnica con miglior produttività è quella GNSS.
Tipologia delle misure e modelli d’errore sono radicalmente diversi.
• Misure IMU comparate con quelle del sensore indipendente e
valutate le differenze Δ;
• Δ combinate, con la loro dinamica e peso, con filtro di Kalman;
➔ soluzione integrata e stima dei bias dei sensori
Introduzione al filtro di Kalman – minimi quadrati sequenziali
Lo scopo è aggiornare la vecchia soluzione a fronte delle nuove
misure, senza dover risolvere tutto il sistema ma aggiornando solo la
soluzione. Partiamo dai mq. Sequenziali dove la nuova stima a
partire da quella precedente e dalla nuove misure è data da:
xˆnew = xˆold + K (bnew − Anew xold )
−1
con K matrice «guadagno» K = N new T
Anew Pnew
È poi possibile stimare la matrice dei cofattori delle nuove stime, a
partire da quella delle vecchie stime e delle nuove misure con la
legge di propagazione della varianza:
QXX new = ( I − KAnew )C XX old ( I − KAnew )T + KCbb K T
Se il vettore dei parametri stimati x non cambia il suo stato, il
procedimento può essere risolto in questa maniera senza dover
risolvere l’intero sistema a seguito delle nuove misure.
Parleremo allora di minimi quadrati sequenziali.
 KF: predizione e correzione
Supponiamo invece che il nuovo vettore di stato all’epoca k sia
diverso da quello all’epoca k-1 (ricevitore GNSS cinematico, errore
nella posizione per la deriva dell’orologio o altro ancora).
Procedura in due passi:
• «predizione» che usa equazioni di stato
• «correzione» che usa equazioni di misura.
IP: cambiamento di stato come una relazione lineare con errore k .
lo stato all’epoca k potrà essere predetto a partire dal precedente:

xˆ k = Fk −1 xˆk −1 +  k  xˆ k − Fk −1 xˆk −1 =  k
F: «matrice di transizione». All’arrivo della nuova misura b k
possiamo eseguire una correzione della grandezza predetta con
errore e k :
bk = Ak xˆk + ek
 KF: scrittura del sistema completo
sistema completo con k epoche di
misura. Ad ogni epoca  A0 x0 = b0 + e0
corrisponderanno due equazioni, una  x = F x +
per la previsione e una per la  1 0 0 0
correzione a seguito della vera misura:
 A1 x1 = b1 + e1
In forma matriciale:  x = F x +
 A0 0  b0   e0 


2 1 1 1

 A2 x2 = b2 + e2
0 0 0
− F     
I 0 0 0  0   0 
 0
 0 A1 0 0 0  x0   b1   e1   x = F x +
      
 3 2 2 2
 0 − F1 I 0 0  x1   0    1 
 0 0 A2 0 0  x2  −  b2  =  e2   ................
  
 0 0 − F2 I 0  ...   0    2   Ak −1 xk −1 = bk −1 + ek −1
   
 ... ... ... ... ...  xk   ...   ...  
 xk = Fk −1 xk −1 +  k −1

 
 0 0 0 − Fk −1 I   0    k −1 
 0     
 0 0 0 Ak   b3   ek 
 KF: procedura ricorsiva
il sistema può essere risolto “in blocco” con minimi quadrati.
Tuttavia per stimare il vettore di stato ad ogni epoca (per conoscere in
RT la soluzione di posizione e velocità di navigazione) dobbiamo
usare una procedura ricorsiva che ad ogni epoca di misura aggiorni la
stima precedente, con un filtro di Kalman.

Ad ogni passo si eseguono le operazioni:

• Aggiornamento del valore precedente xˆ k | j con j < k ;
• Correzione del valore all’epoca k ➔ x̂ ;
kk
• Previsione del valore successivo.

Il filtro di Kalman avviene allora attraverso quattro fasi:

1. Predizione del nuovo vettore di stato (parametri) dalle vecchie stime
2. Predizione della matrice di cofattori
3. Calcolo della matrice guadagno o di Kalman
4. Correzione delle stime predette in base alle nuove misure
 Fasi del Filtro di Kalman
1. Predizione del nuovo vettore di stato dalle vecchie stime:

xˆk k −1 = Fk −1 xˆk −1 k −1 +  k
2. Predizione della matrice dei cofattori:
N k−1k −1 = Fk −1 N k−−11 k −1 FkT−1 + Q
3. Calcolo della matrice guadagno o di Kalman:

Kk = N −1
k k −1
T
(
A Ak N
k
−1
k k −1
A + Qee
T
k )−1

4. Correzione delle stime predette in base alle nuove misure:

Stima del nuovo vettore di stato: xˆ
kk
( )
= xˆk k −1 + K k bk − Ak xˆk k −1 + ek
stima della matrice cofattori nuovo vettore di stato:

N k−1k = Qxx k | k = (I − K k Ak )N k−1k −1 (I − K k Ak ) + K k Qee k K kT

T
 Matrice di transizione, filtering e smoothing
La matrice di transizione contiene la
dinamica del moto. Spesso, nel
posizionamento inerziale o GNSS, essa
è decisa dall’utente utilizzando scenari
di moto predefiniti all’interno del
software:
nel filtro di Kalman non si risolve tutto il sistema ma solo l’ultimo
valore, che rappresenta la migliore stima dei parametri alla data
epoca di misura. Esistono due modalità:
filtering: ad ogni epoca i parametri incogniti sono determinati
facendo uso di tutte le informazioni fino a tale epoca. Ad esempio
all’epoca k vengono usate solo le prime k misure in quanto non sono
ancora state eseguite le k+1, k+2 …, N,
smoothing: si determinano le soluzioni delle epoche precedenti in
modo che anch’esse tengano conto di tutte le misure eseguite. La
soluzione che si ottiene è quella che si avrebbe con mq “in blocco”
 Navigazione geodetica integrata GNSS e IMU

Tecnica: IMU GNSS

Affidabilità: • basso noise; • maggiore noise;
• non soggetto a jamming. • può essere soggetto a jamming.
• la precisione diminuisce col • precisione indipendente dal
Posizione: tempo; tempo;
• indipendente dalla visibilità • dipende dalla visibilità satellitare
satellitare (ottimo nei (DOP) ed è sensibile ai cycle
canyon urbani). slip.
Assetto: Rileva assetti accurati con i Non si rilevano assetti se non con
giroscopi. sistemi multi antenna.
Rate misura: Alta frequenza (100-200 Hz). Frequenza medio bassa
(1-20 Hz).
Inerziale, influenzato dalla
Sistema di gravità. Deve essere noto un Solidale alla Terra (ECEF – Earth
riferimento: modello per correggere le Centered Earth Fixed).
accelerazioni specifiche
Sistemi inerziali e GNSS sono complementari ed indipendenti.
 Accoppiamento «libero» (loosely coupled integration)
• GNSS e IMU funzionano autonomamente e forniscono
entrambi la stima di posizione e velocità
➔ le differenze entrano in un filtro di Kalman che stima la soluzione
di navigazione e un modello d’errore per le successive epoche IMU

• Accoppiamento realizzabile semplicemente

• Funziona solo con soluzione autonoma GNSS (4 satelliti)
• Richiede corretta matrice varianza covarianza delle posizioni
GNSS per una corretta pesatura delle equazioni d’osservazione
 Accoppiamento «stretto» (tightly coupled integration)
• Misure di pseudoranges e di fase GNSS compensate con
accelerazioni e velocità angolari IMU, con un filtro di Kalman,
per la stima di posizione assetti e velocità.

• Maggior affidabilità e robustezza

• Non richiede la matrice di varianza covarianza posizione GNSS
• GNSS di ausilio alla navigazione anche senza soluzione
autonoma
• Maggior complessità di calcolo e integrazione
 Applicazioni: i Mobile Mapping Systems (MMS)

• Un Sistema di Rilevamento Mobile (MMS – Mobile Mapping

System) è un veicolo che viene solitamente impiegato per
integrare le informazioni acquisite da sensori differenti relative
alle strade e a ciò che le circonda.

• I primi studi per la realizzazione di un tale veicolo sono stati

condotti in Canada negli anni Ottanta (Mobile Highway Inventory
System - MHIS).

• Ad oggi, si contano ormai più di un centinaio di sistemi MMS nel

mondo, sviluppati per le più varie applicazioni e in particolare:

• catasto stradale, GIS urbani: misura delle grandezze

geometriche della strada
• manutenzione e sicurezza stradale: stato della
superficie stradale censimento della segnaletica
orizzontale e verticale
 Sensori in un Mobile Mapping Systems
• MMS progettati per acquisire dati in continuo, ad una velocità di
circa 50-80 km/h.
• La georeferenziazione dei dati (ovvero, l’attribuzione per
ciascuna grandezza misurata, dell’esatta posizione rispetto ad
un sistema di riferimento predefinito) avviene con:

GNSS
INS
Odometri
Camere
LiDAR
ESEMPI DI MOBILE MAPPING SYSTEMS
MMS sperimentale POLITO

3 ricevitori GNSS doppia

frequenza ➔ determinare
posizione e assetti
autonomamente
precisione posizione antenne
GNSS  = ± 2 cm e una distanza
fra le antenne di 180 cm
➔ precisione di ± 15 cm a 10 m

In assenza segnale GNSS ➔

posizione e assetti determinati con
misure da accelerometri e
giroscopi IMU.
Importante la calibrazione del
sensore, tanto più esso è a basso
costo.
 MMS e sistemi di riferimento
Il sistema richiede una calibrazione iniziale per:
stima dei parametri di orientamento interno delle camere e definizione
della curva di distorsione
stima dei parametri di orientamento e di calibrazione tra il “body-frame”
ed il “navigation-frame”
(RS1): sistema di riferimento
Geodetico (ETRF2000)
(RS4): sistema di “navigazione”,
solidale al veicolo
(RS2): sistema “body” definito
dalla posizione dell’antenna di
poppa e centro IMU.

(RS3): sistema di riferimento

della camera
CALIBRAZIONE MMS

RS1RS2RS3RS4:
rototraslazioni con parametri da
determinare in una fase di calibrazione
dei sensori e delle geometrie
 Precisione e affidabilità di IMU Tactical Grade
• È stato effettuato uno studio per valutare l’affidabilità del sistema
inerziale in assenza del segnale GNSS.
• In particolare, sono state simulate in laboratorio delle perdite di segnale
GPS di durata variabile, da 10 a 80 secondi, valutando l’errore della
soluzione IMU rispetto alla soluzione GPS completa.

ERRORE
3D

400-CC

Per brevi perdite di segnale

le prestazioni dei due
sensori sono simili

700-CA
60
 Sincronizzazione tra i sensori

• L’integrazione di sensori diversi è collegata alla sincronizzazione

temporale: le misure di ciascun sensore devono poter essere associate
ad un unico riferimento temporale.
• Conviene utilizzare il tempo GPS come scala di riferimento.

GPS

PC

IMU
 Problemi di sincronizzazione tra i sensori

Nell’integrazione tra IMU e GNSS occorre considerare due diversi

sfalsamenti: uno legato alla misura degli assetti, e dovuto alla deriva dei
giroscopi, e l’altro legato allo sfalsamento tra i sistemi di riferimento
temporali dei due sensori.

Deriva
Andamento giroscopi
angolo 

Sfalsamento
temporale
 Acquisizione con MMS

D = ~5 m P
Dmax = ~20 m

• V=50 Km/h circa 14 m/s

• Frequenza telecamera 4-5 Hz
• in media 1 fotogramma ogni circa 3 m
• Considerando una massima distanza di circa 15 m (con 10 m di
sovrapposizione), ogni punto viene descritto da almeno 4 immagini
• Noti i parametri d’orientamento ➔ eq. Collinearità nelle 3 incognite
(EP, NP, QP) da risolvere minimi quadrati
Immagini rilevate da Mobile Mapping Vehicle (MMV)
particolare
 Rilievo ad alto rendimento e
restituzione fotogrammetrica
 Grandezze stradali rilevabili

“Gruppo
segnaletico
unitario
extraurbano” (Art.
128-254)

Restituzione di
cartellonistica e altro
ancora …

“restituzione” delle
geometrie stradali
Un esempio di traiettoria
con GNSS e IMU
GEOMATICA e rilievo integrato, anche a basso costo
Geomatica e rilievo integrato in casi d’emergenza
 Applicazioni: gli UAV
Gli UAV (Unmanned Aerial Vehicles), comunemente detti droni, sono
velivoli senza pilota impiegati per scopi militari fin dagli anni ’60.
I vantaggi principali di tali velivoli sono:
• Assenza di personale a bordo
• Quote di volo molto basse
• Leggerezza e portabilità

Negli ultimi anni, accanto agli scopi militari si

stanno sviluppando anche per scopi civili.
• Gli UAV possono essere controllati da
remoto, ad esempio da terra, oppure
possono seguire una rotta prestabilita
mediante un autopilota.
• Il “cuore” dell’autopilota è basato proprio
sull’integrazione tra GNSS ed IMU, per il
controllo automatico, in tempo reale, del
velivolo.
 Applicazioni: Pedestrian Navigation
Lo scopo della Navigazione Pedestre (Pedestrian Navigation) è
determinare l’istantanea velocità del pedone e ricavare la traiettoria. Si
possono inoltre determinare la postura e la mobilità della persona.
Le applicazioni in questo campo sono indirizzate soprattutto verso i
servizi civili, come ad esempio i vigili del fuoco, la protezione civile, le
guardie forestali, per il controllo in tempo reale della posizione delle
squadre di soccorso.
L’integrazione tra GNSS ed IMU in questo caso consente infatti di
determinare la posizione dell’utente anche in ambienti urbani, oppure
ricchi di vegetazione. In questo caso, un interesse particolare è rivolto
alla miniaturizzazione delle componenti dell’architettura (antenna e
ricevitore GNSS, piattaforma IMU).

Problemi legati
alla posizione
della sensoristica:
 Applicazioni legate alla Pratica Sportiva

L’impiego di strumentazioni miniaturizzate ha avuto una ripercussione

anche per una serie di applicazioni legate alla pratica sportiva, e al
monitoraggio di vetture ed atleti durante le gare e gli allenamenti. Ad
esempio:

Campionato NASCAR®
Monitoraggio delle posizioni
durante le gare
 Sci Agonistico: confronto delle
traiettorie durante le gare e gli
allenamenti

Golf: studio sull’efficienza

dinamica dei diversi colpi
 Studi medici #1
Studio e monitoraggio
della corretta postura…

Studi medici #2
…e progetto di protesi artificiali
per le gambe