1. Navegacinintegradayaplicacionesa (mltiples)vehculosareosnotripulados JuanGiribet GrupodeProcesamientodeSeales,IdentificacinyControl FacultaddeIngenieraUniversidaddeBuenosAires e InstitutoArgentinodeMatemtica-CONICET Consejo Nacional de Investigaciones Cientficas y TcnicasGPSIC- Grupo de Procesamiento de Seales, Identificacin y Control

2. Esquema 3. Sobre el GPSIC Grupo de Procesamiento de Seales, Identificacin y Control VANT multi-rotores: 4. Desarrollar un sistema de navegacin que fusione informacin de diversos instrumentos (sensores inerciales, GPS y cmara) para obtener la posicin, velocidad y orientacin de cada uno de losVANT que conforman una flotilla. UMI 3 acelermetros: 3 girscopos: ( )b f t ( )b ib t pi(t) (i) (b) 0 ( ) 0 0 S 5. 1 ( )t 2 ( )t GPS Posicin: pi(t) pi(t) (i) (b) 6. Cmara Posicin: pi(t) pi(t) p1 i(t) p2 i(t) (i) (b) 7. Navegacin inercial 8. UMI COMPUTADORA f i b (t) wi b (t) i-simo vehculo Sistema de navegacin inercial ( ) ( ) ( ) ( ) ( ) ( ) b b b i i i b b b i ii t t t f t f t f t 9. 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 rotacin (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 otacin. El "ngulo vectorial" ( 0), es el vector invariante para la rotacin, 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 10. ngulos de Euler Toda transformacin de una terna de referencia a otra puede realizarse haciendo tres rotaciones sucesivas: 1. Rotacin de un ngulo alrededor del eje z original. 2. Rotacin de un ngulo alrededor del nuevo eje y. 3. Rotacin de un ngulo alrededor del nuevo eje x. Generalmente, se asocia los ejes x, y, z con ejes del vehculo, por lo que los ngulos de Euler describen la orientacin del mismo. Por esta razn se los denomina a los ngulos: Yaw, Pitch y Roll. 11. ( ) 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 navegacin inercial Ecuaciones de la cinemtica: ( )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 12. Dada la sucesin de tiempos {tk}, es posible obtener un modelo discreto del error del INS Para el diseo del sistema de navegacin integrado se har la suposicin que los ruidos de la UMI son procesos estocsticos (de media nula y gaussianos), Dinmica 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 13. Suponiendo una flotilla de n vehculos, se puede definir el error de la flotilla como el proceso estocstico, 14. Cmara como sensores de posicin 15. Posicionamiento con Cmara p1 z b y b x b p0 p2 z i y i xi q2 q1 16. Es posible determinar con la cmara la posicin del i-simo vehculo pi a partir de su orientacin (es decir, Ci) y la observacin de al menos dos vehculos pj, pk distinguibles con la cmara. 17. Modelo del error de la Cmara El modelo del error de la cmara se obtiene perturbando la ecuacin: 18. Navegacin integrada 19. FiltroFiltro Navegacin integrada INS Comparador INS con sensor externo Medicin externa Compensacin del error 20. Filtro de fusin 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 afn en la peturbacin: ( ) ( ( ), ( ), ( )) ( ( )) ( ) 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 informacin del sistema: ( ) (z( )) ( ) k k k k t y t h t t 21. 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 22. Sea pi GPS y vi GPS la posicin y velocidad dada por el receptor del i-simo vehculo en los instantes de tiempo {tk}k. Estas mediciones estn afectadas por los ruidos (de media cero, blancos y gaussianos) pi, vi. Integracin INS/GPS 23. Integracin INS/Cmara El esquema propuesto para integrar el INS con la cmara es similar al de la integracin INS/GPS. Bajo ciertas condiciones, con la cmara es posible calcular la posicin del i-simo vehculo pi c. 24. Simulaciones 25. INS Ruido de los girscopos: 1.5*10-3 rad/seg Ruido de los acelermetros: 5*10-3 m/s2 Errores en condiciones iniciales: Posicin: 10m Orientacin: 10 GPS Ruido en posicin: 15m Ruido en velocidad: 0.2m/s Cmara Ruido de medicin: 0.5m (equiv.) Mscara: 70 26. Sistema INS/GPS Error en posicin 27. Sistema INS/GPS Error en orientacin 28. Sistema INS/GPS/Cmara Error en posicin 29. Sistema INS/GPS/Cmara Error en orientacin 30. Sistema INS/GPS/Cmara vehculos visibles 31. Conclusiones /Trabajos futuros HARDWARE (Parrot ARDrone) El vehculo utilizado no permite instalar sensores adicionales (girscopos, acelermetros, GPS y una cmara). (Hexarotor) Mejorar el procesamiento de la cmara (actualmente < 10Hz). (Hexarotor) Desarrollar la interfaz con ROS. ALGORITMO El sistema de navegacin INS/GPS de cada uno de los vehculos es independiente del resto. Al integrar la cmara los errores del sistema de navegacin de un vehculo inciden en los restantes (incremento en el costo computacional y adems la necesidad de una comunicacin entre vehculos). la fusin ptima del INS, el GPS y la cmara debe ser encarada con un esquema centralizado. Soluciones sub-ptimas pueden resolverse mediante un esquema descentralizado. La posicin relativa de los vehculos inciden en el desempeo del sistema de navegacin. 32. MUCHAS GRACIAS GPSIC - Grupo de Procesamiento de Seales, Identificacin y Control http://psic.fi.uba.ar