Diapositivas de la presentación del PhD Juan Giribet en el Wescis 2014 - Tucuman, Argentina

1. Navegaciónintegradayaplicacionesa
(múltiples)vehículosaéreosnotripulados
JuanGiribet
GrupodeProcesamientodeSeñales,IdentificaciónyControl
FacultaddeIngeniería–UniversidaddeBuenosAires
e
InstitutoArgentinodeMatemática-CONICET
Consejo Nacional de
Investigaciones Científicas y
TécnicasGPSIC- Grupo de
Procesamiento de Señales,
Identificación y Control

2. Esquema

3. Sobre el GPSIC
Grupo de Procesamiento de Señales, Identificación y Control
• VANT multi-rotores:

6. Desarrollar un sistema de navegación que fusione
información de diversos instrumentos (sensores inerciales, GPS
y cámara) para obtener la posición, velocidad y orientación de
cada uno de losVANT que conforman una flotilla.
UMI
3 acelerómetros:
3 giróscopos:
( )b
f t
 ( )b
ib t
pi(t)
(i)
(b)
0
( ) 0
0
S
  
  
  
   
       
      

7. 1
( )t
2
( )t
GPS
Posición: pi(t)
pi(t)
(i)
(b)

8. Cámara
Posición: pi(t)
pi(t)
p1
i(t)
p2
i(t)
(i)
(b)

9. Navegación inercial

10. UMI COMPUTADORA
f i
b
(t)
wi
b
(t)
i-ésimo vehículo
Sistema de navegación inercial
( ) ( ) ( )
( ) ( ) ( )
b b b
i i i
b b b
i ii
t t t
f t f t f t
  

 
 

11. 2
2
0
1 cos( )
( ) exp(S( )) ( ) ( )
!
Si 1
( ) ( )
k
k
senS
M I S S
k
M I S
 
   
 

 



    
 

 ( ) ( )
( )
i i i i i
i i
C M C I S C C C
C S C
  
 
    

Toda matriz de rotación (3) (salvo la identidad) tiene un único autovalor
real ( ) cuyo autovector asociado define el eje alrededor del cual se rota,
es decir el único vector invariante para la r
M SO



3
otación.
El "ángulo vectorial" ( 0), es el vector invariante para la rotación, cuya norma
define el ángulo rotado alrededor de (en sentido positivo).
  


 
  3
, , , (ángulos de Euler)
T
    
: matriz calculada por el algorimto inercial
: matriz de cambio de base real.
i
i
C
C
3 3
( ) ( ), , 1S v S v v v    

12. Ángulos de Euler
Toda transformación de una terna de referencia a otra puede
realizarse haciendo tres rotaciones sucesivas:
1. Rotación de un ángulo alrededor del eje z original.
2. Rotación de un ángulo alrededor del nuevo eje y.
3. Rotación de un ángulo alrededor del nuevo eje x.
Generalmente, se asocia los ejes x, y, z con ejes del vehículo, por lo que los
ángulos de Euler describen la orientación del mismo. Por esta razón se los
denomina a los ángulos: Yaw, Pitch y Roll.

13. ( )
i i
i i b i b
b b
i i b i b
b b ib b ib
p v
v C f C f
C C S C
 
  
   

 
 
 
,
( ) ( )
i i
i i i i
i
i i i
b b b b
p p p v v v
C I S C C S C
 
 
   
   
Errores del sistema de navegación inercial
Ecuaciones de la cinemática:
( )i i
b bC S C 
( )
i i
i i b i b
b b
i b
b ib
p v
v S C f C f
C
 
  
 

 
 0 0 0 0
0 0 ( ) 0
0 0 0 0
i i
b
i i b i i ib
b b b
i
b
p I p
v S C f v C
f
C
 

 

 
      
                 
             

14. Dada la sucesión de tiempos {tk}, es posible obtener un modelo
discreto del error del INS
Para el diseño del sistema de navegación integrado se hará
la suposición que los ruidos de la UMI son procesos
estocásticos (de media nula y gaussianos),
Dinámica del Error del INS
( , , )T T T T
i i i iX p v  
1( ) ( ) ( ) ( ) ( )i k i k i k i k i kX t A t X t B t t  
( ) ( ) ( )
( ) ( ) ( )
b b b
i i i
b b b
i ii
t t t
f t f t f t
  

 
 
 ( ) ( ), ( )b b
i i it t f t  

15. Suponiendo una flotilla de n vehículos, se puede definir el error de
la flotilla como el proceso estocástico,

16. Cámara como sensores
de posición

17. Posicionamiento con Cámara
p1
z
b
y
b
x
b
p0
p2
z
i
y
i
xi
q2
q1

18. Es posible determinar con la cámara la posición del i-ésimo
vehículo pi a partir de su orientación (es decir, Ci) y la observación
de al menos dos vehículos pj, pk distinguibles con la cámara.

19. Modelo del error de la Cámara
El modelo del error de la cámara se obtiene perturbando la ecuación:

20. Navegación integrada

21. FiltroFiltro
Navegación integrada
INS
Comparador
INS con sensor
externo
Medición
externa
Compensación
del error

22. Filtro de fusión de datos
( ) ( )
( ) ( ) ( ) ( ) ( ) ( ) ( )
( ) ( ) ( ( )) ( ) ( ( )) ( ) ( ( ))
i i
bi i b i i b
b b b
bi i b i i b
ibb b ib b b ib
p t v t
v t C t f t g C t f t g C t f t
C t C t S t C t S t C t S t

  

    
  
 
 
1
Dados los instantes de tiempo :
( ) ( ( ), ( ), ( )), ( ) ( ), ( )
Tenemos el modelo afín en la peturbación:
( ) ( ( ), ( ), ( )) ( ( )) ( )
k
i b b
k k k b k k ib k k
b b
ibk k k k k k
t
z t p t v t C t t t f t
z t f z t t f t g z t t
  
 
 
 
Supongamos que en tenemos información del sistema:
( ) (z( )) ( )
k
k k k
t
y t h t t 

23. Filtro de Kalman Extendido
1( ) ( ( ), ( ), ( )) ( ( )) ( )
( ) ( ( )) ( )
b b
ibk k k k k k
k k k
z t f z t t f t g z t t
y t h z t t
 


  

 
0 0
0 0
1 1 1
1 1 1 1 1 1
1 1 1 1
1 1
( ) 0( ) ( ( ))
( ) 0( ( ))
( ) ( ( ), ( ), ( ))
( ( ), ( ), ( )),
( ( ))
k k
k k
b b
ibk k k k
T T
k k k k k k k
b b
ibk k k k
k k
R Covz t E z t
Q CovP Cov z t
z t f z t t f t
P A P A B Q B
f
A z t t f t
z
B g z t





  

     
   
 
  
 
  
 

 




 
 
1
( ) ( ) ( ) ( ( ))
( )
( ( ))
k k k k k
T T
k k k k k k k
k k k k
k k
z t z t K y t h z t
K P C C P C R
P I K C P
h
C z t
z
 
 


   


 

 




( ) ( ( ), ( ), ( ))
( ) ( ) ( )
i i i
k k k b k
k k k
z t p t v t C t
X t z t z t

 
1( ) ( )
( ) ( )
k k k k k
k k k k
X t A X t B
Y t C X t


  
 

24. Sea pi
GPS y vi
GPS la posición y velocidad dada por el receptor del i-ésimo vehículo
en los instantes de tiempo {tk}k. Estas mediciones están afectadas por los ruidos
(de media cero, blancos y gaussianos) ηpi, ηvi.
Integración INS/GPS

25. Integración INS/Cámara
El esquema propuesto para integrar el INS con la cámara es similar
al de la integración INS/GPS. Bajo ciertas condiciones, con la
cámara es posible calcular la posición del i-ésimo vehículo pi
c.

26. Simulaciones

27. INS
Ruido de los giróscopos: 1.5*10-3 rad/seg
Ruido de los acelerómetros: 5*10-3 m/s2
Errores en condiciones iniciales:
Posición: 10m
Orientación: 10º
GPS
Ruido en posición: 15m
Ruido en velocidad: 0.2m/s
Cámara
Ruido de medición: 0.5m (equiv.)
Máscara: 70º

28. Sistema INS/GPS – Error en posición

29. Sistema INS/GPS – Error en orientación

30. Sistema INS/GPS/Cámara – Error en posición

31. Sistema INS/GPS/Cámara – Error en orientación

32. Sistema INS/GPS/Cámara – vehículos visibles

35. Conclusiones /Trabajos futuros
• HARDWARE
• (Parrot ARDrone) El vehículo utilizado no permite instalar sensores adicionales
(giróscopos, acelerómetros, GPS y una cámara).
• (Hexarotor) Mejorar el procesamiento de la cámara (actualmente < 10Hz).
• (Hexarotor) Desarrollar la interfaz con ROS.
• ALGORITMO
• El sistema de navegación INS/GPS de cada uno de los vehículos es independiente del
resto. Al integrar la cámara los errores del sistema de navegación de un vehículo inciden
en los restantes (incremento en el costo computacional y además la necesidad de una
comunicación entre vehículos).
• la fusión óptima del INS, el GPS y la cámara debe ser encarada con un esquema
centralizado. Soluciones sub-óptimas pueden resolverse mediante un esquema
descentralizado.
• La posición relativa de los vehículos inciden en el desempeño del sistema de navegación.

36. MUCHAS GRACIAS
GPSIC - Grupo de Procesamiento de Señales,
Identificación y Control
http://psic.fi.uba.ar