Sistema de prototipado para multirotores
Abstract
En este proyecto se tratará de realizar un autopiloto para la placa Raspberry Pi +Navio2 el cual se implementará en la Raspberry Pi y en MATLAB® + SIMULINK® . El autopiloto se dividirá en dos partes claramente diferenciadas, por un lado el software encargado de leer los sensores y enviar los datos hacia MATLAB® implementado en la Raspberry Pi , y por otro lado el mezclador y estimador implementado en MATLAB® .
Full text
Proyecto Fin de Carrera Ingeniería de Telecomunicación Formato de Publicación de la Escuela Técnica Superior de Ingeniería Autor: F. Javier Payán Somet Tutor: Juan José Murillo Fuentes Dep. Teoría de la Señal y Comunicaciones Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2013 c r g v robotics vision control Proyecto Fin de Grado Grado en Ingeniería Electrónica, Robótica y Mecatrónica Sistema de Prototipado para Multirotores Autor: Antonio González Rot Tutor: José Ángel Acosta Rodríguez Dep. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2018
Proyecto Fin de Grado Grado en Ingeniería Electrónica, Robótica y Mecatrónica Sistema de Prototipado para Multirotores Autor: Antonio González Rot Tutor: José Ángel Acosta Rodríguez Profesor Titular Dep. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2018
Proyecto Fin de Grado: Sistema de Prototipado para Multirotores Autor: Antonio González Rot Tutor: José Ángel Acosta Rodríguez El tribunal nombrado para juzgar el trabajo arriba indicado, compuesto por los siguientes profesores: Presidente: Vocal/es: Secretario: acuerdan otorgarle la calificación de: El Secretario del Tribunal Fecha:
Agradecimientos Am i familia por todo su apoyo tanto moral como económico sin el cual no estaría donde estoy ahora. A todos los profesores y maestros que he tenido durante mis años de educación en especial a todos aquellos que me inculcaron su amor por la ciencia, así como a quienes me enseñaron que la respuesta habitual no es siempre la correcta. A mis amigos por aguantar mis tonterías. Antonio González Rot Sevilla, 2018 I
Resumen En este proyecto se tratará de realizar un autopiloto para la placa Raspberry Pi +Navio2 el cual se implementará en la Raspberry Pi y en MATLAB®+ SIMULINK®. El autopiloto se dividirá en dos partes claramente diferenciadas, por un lado el software encargado de leer los sensores y enviar los datos hacia MATLAB ® implementado en la Raspberry Pi , y por otro lado el mezclador y estimador implementado en MATLAB®. III
XÍndice 4.1.2 Configuración bloques byte pack/unpack 30 4.1.3 Esquema completo 31 4.2 Envío de datos 31 4.3 Análisis e interpretación de los datos recibidos 32 4.3.1 Todos los procesos con 20 de prioridad 32 4.3.2 Todos los procesos con 20 de prioridad y uno con 15 33 4.3.3 Todos los procesos con 20 de prioridad y uno con 0 34 4.3.4 Todos los procesos con 0 de prioridad 35 4.4 Esquema completo de SIMULINK®implementado 36 5 Estimador y motor mixer 39 5.1 Filtro de Kalman para fusión de los datos procedentes de la IMU 39 5.1.1 Ecuaciones de Kalman generales 39 5.1.2 Forma de las matrices utilizadas para fusionar las dos IMUs 40 Resultados obtenidos con la fusión de las IMUs 40 Ajuste de la matriz Q para mejorar el filtrado de la señal 42 5.2 Filtro de Kalman para estimación de actitud 44 5.2.1 Estimación de la actitud de forma directa 44 Estimación de Roll yPitch 44 Estimación del Yaw 45 Cálculo en MATLAB®45 Filtro implementado para fusión y estimación directa de la actitud 45 Esquema de funcionamiento de la función implementada 47 Resultados estimación directa 47 5.2.2 Estimación de la actitud basada en el artículo 48 Ecuaciones propuestas en el artículo 48 Ecuaciones propias implementadas 48 Esquema de funcionamiento de la función implementada 49 Resultados obtenidos con la estimación del artículo 49 5.2.3 Estimación de la actitud basada en el libro 50 Ecuaciones propuestas en el libro 50 Ecuaciones finales implementadas para la fusión y estimación de la actitud 51 Esquema de funcionamiento de la función implementada 52 Resultados obtenidos con la estimación del libro 53 5.2.4 Comparación de las tres estimaciones 53 5.3 Filtro de Kalman para estimación de posición 55 5.3.1 Método de posicionamiento utilizado 55 5.3.2 EKF implementado 57 5.3.3 Resultados obtenidos 60 5.4 Subsistema EKF de SIMULINK®62 5.5 Motor mixer 62 5.5.1 Pseudoinversa 62 5.5.2 Optimización de mínimos cuadrados 64 5.6 Subsistema motor mixer de SIMULINK®66 6 Conclusiones y trabajo futuro 69 6.1 Comparación Autopiloto propio VS PX4 69 6.1.1 Comparación en actitud 69 6.2 Conclusiones 70 6.3 Trabajo futuro 71 6.3.1 Terminar filtro de posición 71 6.3.2 Implementación de controlador 71 6.3.3 Implementación del mezclador avanzado 71 6.3.4 Mejor ajuste del estimador en el sistema real 71
Índice XI 6.3.5 Mejoras en la seguridad del software 71 Índice de Figuras 73 Índice de Tablas 75 Índice de Códigos 77 Bibliografía 79 Índice alfabético 81 Glosario 81
Notación gdl Grados De Libertad IMU Inertial Measurement Unit. Unidad de Medida Inercial RC Radio Control dps Degrees Per Second. Grados Por Segundo. ◦/s SoC Sistem on a Chip (Todos los componentes en el mismo chip, CPU, Ram, GPU, etc) GPS Global Position System. Sistema Global de Posicionamiento UAV Unmanned Aerial Vehicle. Vehículo Aéreo No Tripulado EKF Extended Kalman Filter. Filtro de Kalman Extendido VICON Sistema de captura de movimiento Lidar Sensor de distancia láser por tiempo de vuelo NED North, East, Down. Norte, Este, Abajo XIII
Introducción Dime y lo olvido, enséñame y lo recuerdo, involúcrame y lo aprendo B. Franklin Durante los últimos años se ha producido un gran repunte en la investigación y desarrollo de todo tipo de vehículos aéreos no tripulados y junto a ellos distintos software centrados no solo en la lectura de los sensores de abordo sino en el control de los mismos. El principal inconveniente de este tipo de software es que por lo general son especialmente complicados y demasiado genéricos por lo que si se intentan implementar algo en ellos fuera de lo que ya existe, esta tarea se vuelve realmente complicada lo que alarga el tiempo de desarrollo de forma más que considerable. Algunos ejemplos de estos software son: •PX4 [1] que tiene 515674 líneas de código repartidas en sus 2270 ficheros. •Ardupilot [2] que tiene 901307 líneas de código repartidas en sus 2880 ficheros. A pesar de estos inconvenientes la principal ventaja de estos software es que funcionan muy bien con una gran variedad de hardware por ejemplo PX4 puede funcionar con los autopilotos ”Navio[ 3 ]”, ”Pixhawk[ 4 ]” o ”Snapdragon Flight[5]” entre muchos otros [6]. Otra de las ventajas es que puede ser utilizado para distintas configuraciones de vuelo, por ejemplo, Arducopter da soporte para multirotores, helicópteros, aviones de ala fija, entre muchas otras, así como para vehículos tanto terrestres como acuáticos [7]. Objetivos del proyecto Este proyecto se basará en el desarrollo de un software capaz leer los sensores que incorpora el autopiloto Navio2 y enviarlos a MATLAB ® donde se implementarán los estimadores de posición y actitud así como los filtros correspondientes para su correcto funcionamiento junto con un pequeño mezclador para el control de bajo nivel de los motores. Además de esto se desarrollará un programa encargado de lanzar varias copias de este software y monitorizar que todas ellas tengan un funcionamiento correcto, si esto no ocurriera se relanza una nueva copia y se repite el ciclo. De esta forma conseguimos incluir redundancia proporcionando mayor seguridad ante fallos mediante la paralelización de procesos. Motivación del proyecto La principal motivación que ha propiciado este proyecto es que hasta ahora se estaba utilizando un Firmware basado en PX4 [ 1 ] con algunas modificaciones para adaptarlas a nuestras necesidades. Sin embargo cada vez que se pretendía añadir algún modulo nuevo o modificar alguno ya existente teníamos que enfrentarnos a varios días, incluso semanas, de laboriosa integración con el objetivo de hacerlo funcionar de la forma deseada. 1
2Capítulo 1. Introducción Con esta propuesta se ha conseguido pasar de 2270 ficheros de PX4 a 63 de los cuales solo habría que modificar 4 para incluir un nuevo sensor o una nueva característica además de añadir, si existiese, la librería del mismo. De esta forma se podría llegar a reducir de forma más que significativa el desarrollo. A esto además habría que añadir el modelo en SIMULINK®y las dos funciones que integra.
Materiales y Configuración necesaria El que quiere algo conseguirá un medio, el que no una excusa Stephen Dolley En este capítulo se explicará los distintos materiales utilizados para el desarrollo del proyecto así como toda la configuración previa necesaria para su correcto funcionamiento. En primer lugar se pondrá de manifiesto los materiales utilizados junto con el software mínimo necesario para conseguir el objetivo marcado en este proyecto. Navio2 Como placa de sensores se ha utilizado el autopiloto Navio2 [ 8 ], el cual se puede ver en Figura 2.1 y cuyas características se detallan desde la Tabla 2.1 a la Tabla 2.4. Figura 2.1 Placa Navio2. 3
4Capítulo 2. Materiales y Configuración necesaria Tabla 2.1 Especificación mecánica Navio2. Características mecánicas Tamaño 55 x 66 mm Peso 23 g Temperatura operación -40 ... +85 ◦C Tabla 2.2 Especificaciones eléctricas Navio2. Características eléctricas Voltaje alimentación 4.75-5.25V Fuente de alimentación USB, Módulo de potencia, Servo rail Consumo de corriente <150mA Tabla 2.3 Puertos de la placa Navio2. Puertos Módulo de potencia UART I2C ADC 12 salidas PWM para servo Bus comunicación RC Tabla 2.4 Sensores de la placa Navio2. Sensores IMU MPU9250 de 9 gdl IMU LSM9DS1 de 9 gdl Barómetro MS5611 GPS U-blox con GLONASS Co-Procesador Cortex-M3 RC I/O LED RGB Principales características de los sensores IMU MPU9250 IMU de 9 gdl que nos proporciona aceleración lineal, velocidad angular y medida del campo magnético en tres ejes, esta última con una precisión bastante baja. Además es capaz de medir la temperatura, para realizar las correcciones pertinentes. Tabla 2.5 Especificaciones IMU MPU9250. Sensor[ 9 ] Rango Gyro Ruido Gyro Rate Rango Accel Rango Magnet. Salida Digital Alimentación Lógica Alimentación Operativa Unidades: (◦/s)dps/√Hz (g)µT(V) (V±5%) ±250 ±500 ±1000 ±2000 0.01 ±2±4 ±8±16 ±4800 I2C or SPI 1.7V to VDD or VDD 2.4V to 3.6V IMU LSM9DS1 De nuevo una IMU de 9 gdl con que nos proporciona aceleración, velocidad angular y esta vez si, una buena medida del campo magnético. Al igual que el sensor anterior también proporciona una medida de la temperatura ambiente. Tabla 2.6 Especificaciones IMU LSM9DS1. Sensor[ 10 ] Rango Gyro Rango Accel Rango Magnet. Salida Digital Alimentación Lógica Alimentación Operativa Unidades: (◦/s) (g)gauss (V) (V±5%) ±245 ±500 ±2000 ±2±4 ±8±4±8 ±12 ±16 I2C or SPI 1.7V to VDD or VDD 2.4V to 3.6V Barómetro MS5611 Sensor que nos proporciona una medida bastante precisa de presión y temperatura ambiente. Con la que posteriormente se puede hacer un cálculo de la altura.
2.2 Raspberry Pi 3 Model B 5 Tabla 2.7 Especificaciones Barómetro MS5611. Sensor[ 11 ] Rango Resolución Precisión Presión (mbar)10...1200 0.065 / 0.042 / 0.027 / 0.018 / 0.012 (1) ±1.5(2) Temperatura (◦C)-40...+85 <0.01 ±0.8 (1) ratio de sobremuestreo: 256 / 512 / 1024 / 2048 / 4096 (2) a 25◦Cy 750 mbar GPS Ublox-M8N Sensor que nos proporciona las coordenadas de longitud y latitud terrestres junto a la altura sobre el nivel del mar y el elipsoide. También recibe del satélite la unidad de tiempo del reloj atómico y un valor de precisión de la posición horizontal y vertical. Tabla 2.8 Especificaciones GPS Ublox-M8N. Sensor[ 12 ] Tipos Tasa de actualización Precisión de la velocidad Precisión en el cabeceo Precisión posición Unidades Hz m/s◦m GPS / GLONASS / GALILEO / BeiDou 5 0.05 0.3 2.5 Raspberry Pi 3 Model B La placa Navio2 de forma individual sería inútil ya que está pensada para ser utilizada con el microcomputador Raspberry Pi 3 model B, Figura 2.2. En ella será necesario instalar una versión especial de ”Raspbian”, un sistema operativo en base Linux, proporcionado directamente por Emlid, los desarrolladores de Navio [ 13 ], así como las librerías para el control de los sensores también proporcionadas por Emlid [14]. 2x USB 2.0 Micro USB Power in CPU/GPU Broadcom BCM2837 1024MB SDRAM Ethernet controller LAN9514 4x USB + 2x USB 2.0 Ethernet RJ45 Ethernet 40pins: 28x GPIO, I2C, SPI, UART 1 H D M I HDMI out 4 poles jack Regulator Raspberry Pi 3 Model B V1.2 (c) Rasberry Pi 2015 Wifi / BT Figura 2.2 Microcomputador Raspberry Pi 3 Model B.
12 Capítulo 3. Drivers para Navio2 Código 3.4 Comprobación drivers Monitor. [...] for (i=0;i<N;i++) // Se recorren el número de procesos que se desea arrancar { if ( flag [i]==1) // Si el proceso no está iniciado , se inicia { if (( drivers [i] = fork () ) < 0) // Si falla el fork se para todo { perror ("fork failure "); exit (1) ; } } if ( drivers [i] == 0 && flag[i]==1 && i==0) //Si es el primer proceso el que // no está inicado else if ( drivers [i] == 0 && flag[i]==1 && i>0) //Si el proceso no es el primero else // Si es el proceso padre se indica que el hijo ya ha arrancado y se // continua a la siguiente iteraci ón { flag [i]=0; continue ; } [...] En el siguiente fragmento se muestra como es el proceso de arranque de cada driver, en el cual se obtiene el PID del mismo, se le asigna la máxima prioridad y se inicia con el identificador del mismo y los sensores que deseamos arrancar. Código 3.5 Creación drivers Monitor. [...] sprintf (id ,"%d",i); // Se le asigna el identificador drivers [ i] = getpid () ; // Se guarda el PID con el que arranca printf (" driver with id=%s and pid=%d started\n",id , drivers [ i ]) ; setpriority (PRIO_PROCESS, drivers[i], −20);//Se le asigna prioridad máxima /∗Se inicia con los sensores que pueden funcionar en paralelo ∗(Todos menos el barómetro y el GPS)∗/ execl (" ../ drivers / drivers "," drivers ", id ,"mpu","lsm","rc","pwm","ahrs", NULL); perror ("execl () failure !\n\n"); printf ("This print is after execl () and should not have been executed if execl were successful ! \n\n"); _exit (1) ; [...] Por último se muestra como se realiza la espera del fallo de alguno de los drivers y la identificación de la misma una vez que esta ocurre. Código 3.6 Espera drivers Monitor. died_driver =wait(&status ); printf ("\n Monitor: Driver with PID= %d died\n\n", died_driver ); for (i=0;i<N;i++)//comprobacion del driver que ha fallado { if ( drivers [ i]==died_driver ) flag [i]=1; } Drivers Este software recibe como argumento de linea de comandos un identificador propio, en función del cual asigna un puerto distinto para el envío y recepción de los sockets UDP con los que se comunica con SIMULINK ® . Por otro lado también recibe los sensores que se quieren utilizar, de forma que solo se arrancan los sensores que realmente vamos a utilizar, para ello se inicia un hilo por sensor el cual una vez inicializado se queda en
3.3 Drivers 13 un bucle leyendo dicho sensor. Este bucle se mantendrá activo mientras no se supere un cierto tiempo entre cada iteración, si esto llega a ocurrir el hilo sale de la espera, termina el bucle y salimos del hilo. Por otro lado el hilo principal entra en un bucle infinito el cual en cada iteración comprueba si todos los hilos de los sensores siguen activos para en el caso de que no sea así arrancarlos de nuevo. Hecho esto, envía los datos leídos por los sensores cuyos hilos no han sufrido ningún problema mediante un socket UDP hacia MATLAB ® junto a una bandera que indica si alguno de estos ha sufrido una parada para poder obviar sus datos de así necesitarlo. Al igual que el resto de hilos tiene un temporizador que en el caso de ser superado se termina el proceso. Los sockets que utiliza el hilo main son los siguientes: •Puerto 8081+idproceso ∗3 : Datos de ambas IMUs, estimación de la actitud usando las funciones de Navio2, la diferencia de tiempo entre medidas y banderas que indica la caída de algunos de los hilos que controlan las IMUs. •Puerto 8082+idproceso ∗3 : Datos recibidos de la emisora RC junto a dos banderas que indican si ha habido algún error en los hilos que controlan los datos de la emisora y los motores. •Puerto 8083+idproceso ∗3 : Datos utilizados para la estimación de la posición, GPS, Barómetro, TotalStation, Lidar así como sus respectivas banderas para indicar la caída de algunos de los hilos. Por otro lado, cabe destacar que todos los temporizadores encargados de que ningún hilo tarde más tiempo de lo debido en ejecutarse están realizados con señales ”posix” de tiempo real, las cuales llaman a un manejador (Código 3.7) el cual en función de la señal recibida, la cual es distinta para cada hilo, desactiva una bandera que para la ejecución del hilo correspondiente. Además se activa la bandera ”someone_died” que indica que se ha producido algún problema para que el hilo principal realice la comprobación del hilo caído para relanzarlo. Si el hilo que tiene el problema es el hilo principal se reinicia el proceso completo. En el siguiente código se muestra un fragmento del manejador de la interrupción de sobretiempo de cualquiera de los hilos de lectura de los sensores, en concreto el hilo principal y el de lectura de la emisora. El resto es exactamente igual y solo difiere en la señal que se recibe. Código 3.7 Manejador de temporizador de sobretiempo. void timer_handler ( int sig , siginfo_t ∗si , void ∗uc) { someone_died=1; //Indicamos que algún hilo ha fallado /∗Detectamos que hilo falla en función de la señal de fallo ∗/ if (sig==SIGRTMIN) { main_exit=1; exit (0) ; } else if (sig==SIGRTMIN+1) { RC_exit=1; } [...] Por ultimo cabe destacar que para la temporalización de la ejecución cíclica de cada uno de los hilos se ha probado con dos tipos de esperas: •”usleep” que realiza una espera durante los µs especificados independientemente del tiempo de ejecución del propio hilo. •”clock_nanosleep” que realiza una espera absoluta pudiendo sincronizar de esta forma los procesos de forma mucho más exacta. Por ejemplo si la ejecución del hilo varía entre 1 y 2 ms y nosotros queremos una ejecución fija cada 4ms. Con clock_nanosleep podemos leer el tiempo absoluto de ejecución al principio del hilo y sumarle el tiempo que queremos que espere, de forma que realicemos la espera hasta ese tiempo absoluto independientemente del tiempo de ejecución. Además si la ejecución del hilo supera el tiempo de ejecución cíclico esperado directamente no se realiza ninguna espera. Mientras que usleep siempre realizaría una espera de 4ms al finalizar la ejecución del proceso por los que los tiempos sería prácticamente impredecibles.
14 Capítulo 3. Drivers para Navio2 En la Figura 3.4 se puede observar una comparativa de los tiempos de ejecución entre ambas opciones. Se observa que la ejecución con ”clock_nanosleep” es mucho más exacta y estable que la opción con usleep. Por otro lado en el Código 3.8 se ve la forma en que se realiza esta espera. Figura 3.4 clock_nanosleep vs usleep con tiempo objetivo 4ms. A continuación, se muestra el mecanismo de espera implementado para la repetición cíclica de los hilos de lectura de los sensores. Código 3.8 Espera entre ejecuciones. /∗Cálculo del tiempo de espera∗/ clock_gettime (CLOCK_REALTIME,&slp); if (( slp . tv_nsec+RC_rate∗1000)<(1000000000)) slp .tv_nsec+=RC_rate∗1000; else { slp . tv_sec+=1; slp .tv_nsec=slp.tv_nsec+RC_rate∗1000−1000000000; } [...] /∗Espera para la repetici ón∗/ clock_nanosleep(CLOCK_REALTIME,TIMER_ABSTIME,&slp,NULL);
3.3 Drivers 15 Esquema de funcionamiento hilo principal Comprobación fallo en hilos Hay fallo Relanzar hilo No hay fallo Activar Temporizador Enviar último dato válido Esperar ''main_rate''µs Creación Temporizador y Sockets Inicialización Lectura argumetos de entrada Creación hilos de los sensores Figura 3.5 Esquema de funcionamiento hilo principal. Aquí se puede observar la incialización de las variables necesarias para el correcto funcionamiento del hilo principal. Código 3.9 Inicialización hilo Principal Drivers. int main(int argc, char ∗argv []) { /∗Declaración e inicializaci ón de variables ∗/ signal (SIGINT,quit_handler); // Asignación de manejador de interrupcion fin programa /∗tabla que indica el sensor que se va a iniciar ∗/ bool sensor[10]={ false , false , false , false , false , false , false , false , false , false }; /∗Variables temporizadores∗/ timer_t timerid ; // Identificador struct sigevent sev; // Señal que indica el evento struct itimerspec its ; // Valor del temporizador struct itimerspec its_cero ; // Temporizador con valor 0 sigset_t mask; // Máscara de la señal struct sigaction sa; // Accion de la señal /∗Banderas que indican el fallo de un sensor∗/ bool mpu_fail, lsm_fail , baro_fail ,RC_fail,PWM_fail, lidar_fail , gps_fail , total_fail ; struct timespec slp={0,0}; // Variable para almacenar el timepo de espera /∗Diferencial de tiempo entre envío de señales∗/ float dt; struct timeval tv ; static unsigned long previoustime=0, currenttime =0; [...] En el siguiente código se muestra la forma en la que se detecta que sensores se deben arrancar a partir de la lectura de los parámetro de entrada al iniciar el proceso driver. Código 3.10 Lectura argumentos de entrada. [...] /∗Lectura de los sensores que se van a iniciar ∗/ if (argc > 2){// Si se ha recibido como parámetro algo aparte del identificador /∗ ∗sensor[0]=>mpu ∗sensor[1]=>lsm ∗sensor[2]=>baro
16 Capítulo 3. Drivers para Navio2 ∗sensor[3]=>rc ∗sensor[4]=>PWM ∗sensor[5]=>adc ∗sensor[6]=>ahrs // only if mpu is used ∗sensor[7]=>sf11c ∗sensor[8]=> totalstation ∗sensor[9]=>gps ∗ ∗/ for (int i=2;i<argc; i++){ // Se comprueba cada parámetro para ver si coincide // con el sensor if (strcmp(argv[i ], "mpu")==0)sensor[0]=true; if (strcmp(argv[i ], "lsm")==0)sensor[1]=true ; if (strcmp(argv[i ], "baro")==0)sensor[2]=true ; if (strcmp(argv[i ], "rc")==0)sensor[3]=true ; if (strcmp(argv[i ], "pwm")==0)sensor[4]=true; if (strcmp(argv[i ], "adc")==0)sensor[5]=true ; if (strcmp(argv[i ], "ahrs")==0)sensor[6]=true ; if (strcmp(argv[i ], "sf11c")==0)sensor[7]=true ; if (strcmp(argv[i ], " totalstation ")==0)sensor[8]=true ; if (strcmp(argv[i ], "gps")==0)sensor[9]=true ; } }else if (argc==2){ // Si solo se ha recibido el identificador de proceso // se hace un inicio por defecto sensor[0]= true ; sensor[1]= true ; sensor[2]= true ; sensor[3]= true ; sensor[4]= true ; sensor[5]= false ; sensor[6]= true ; sensor[7]= false ; sensor[8]= false ; sensor[9]= true ; } else{// Si no se recibe el identificador no se sigue ejecutando el proceso printf (" falta identificador del proceso\n") ; exit (0) ; } // Asignación del identificador de proceso id=atoi (argv [1]) ; printf (" identicador de proceso=%d\n\n",id); if (check_apm()) { return 1; }[...] En este fragmento se muestra la creación de los drivers, en concreto se muestran las dos IMUs, así como la declaración de las estructuras necesarias las cuales son todas similares a lo mostrado en el primer comentario. El resto de sensores tienen un procedimiento de arranque similar a los ya mostrados. Código 3.11 Creación hilos. [...] /∗∗∗ estructura sensores∗∗∗/ /∗typedef struct ∗{ ∗IdentificadorSensor _sensor; ∗shm_TipoSensor _shmmsg; //definido en Navio_Types.h ∗}lsm9ds1_imu_str;∗/ /∗Declaración de estructuras necesarias para almacenar los datos de cada sensor∗/ mpu9250_imu_str mpu9250_imu; lsm9ds1_imu_str lsm9ds1_imu; baro_str barometer; rcinput_str rcin ; rcoutput_str rcout ; adc_str adc_signals ; sf11c_str sf11c_signals ; totalStation_str totalstation_signals ; gps_str gps_signals ;
3.3 Drivers 17 /∗Se arranca mpu∗/ if (sensor [0]) { printf ("mpu launched\n"); mpu9250_imu._mpu9250_imu.initialize(); pthread_t mpu9250_imu_thread; if ( pthread_create (&mpu9250_imu_thread, NULL, acquireMPU9250Data, (void ∗)&mpu9250_imu)) { printf ("Error : Failed to create mpu9250_imu thread\n"); return 0; } } /∗Se arranca lsm∗/ if (sensor [1]) { printf ("lsm launched\n"); lsm9ds1_imu._lsm9ds1_imu. initialize () ; pthread_t lsm9ds1_imu_thread; if ( pthread_create (&lsm9ds1_imu_thread, NULL, acquireLSM9DS1Data, (void ∗)&lsm9ds1_imu)) { printf ("Error : Failed to create barometer thread \n"); return 0; } } [...] // identico para el resto de sensores [...] A continuación se puede observar como se crea el temporizador de sobretiempo y espera utilizado por el hilo principal. Código 3.12 Creación Timer. [...] /∗∗∗∗∗∗∗∗∗∗∗∗∗ Se crea el ’’watchdog’’ del hilo principal ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/ /∗Se establece el manejador para la señal del temporizador∗/ sa. sa_flags = SA_SIGINFO; sa. sa_sigaction = timer_handler ; sigemptyset(&sa.sa_mask); sigaction (SIGRTMIN, &sa, NULL); /∗Se bloquea la señal del timer temporalmente ∗/ sigemptyset(&mask); sigaddset (&mask, SIGRTMIN); sigprocmask(SIG_SETMASK, &mask, NULL); /∗Creación del temporizador ∗/ sev. sigev_notify = SIGEV_SIGNAL; sev. sigev_signo = SIGRTMIN; sev. sigev_value . sival_ptr = &timerid; timer_create (CLOCK_REALTIME, &sev, &timerid); /∗Establecimiento de los valores del temporizador∗/ its . it_value . tv_sec = 0; its . it_value .tv_nsec = main_rate∗comp_factor∗1000; its . it_interval . tv_sec = 0; its . it_interval .tv_nsec = 0; its_cero . it_value . tv_sec = 0; its_cero . it_value .tv_nsec = 0; its_cero . it_interval .tv_sec = 0; its_cero . it_interval .tv_nsec = 0; [...] El siguiente fragmento corresponde a la creación de los sockets necesarios para el envío de los datos vía UDP hacia SIMULINK ® , en concreto se muestra la creación del socket encargado de enviar los datos de actitud, ya que el resto será idéntico a excepción del puerto utilizado.
18 Capítulo 3. Drivers para Navio2 Código 3.13 Creación sockets de envío. [...] /∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ Declaración del socket ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/ /∗Variables necesarias para el socket∗/ struct sockaddr_in si_other_att ,si_other_RC, si_other_pos ; int s_att ,s_RC,s_pos, slen=sizeof ( si_other_att ); /∗Estructuras a enviar ∗/ attitude_msg att_msg; // IMUs, AHRS, dt, bandera de fallo position_msg pos_msg; // Total , GPS, baro, lidar , bandera de fallo RCIn_msg RC_msg; // Señal de radio recibida /∗Creación del socket UDP∗/ if (( s_att =socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == −1) { udp_die(" socket_att "); } /∗Reserva de memoria de la estructura a enviar ∗/ memset((char ∗) & si_other_att , 0, sizeof ( si_other_att )); /∗asignaci ón de puertos a utilizar ∗/ si_other_att . sin_family = AF_INET; si_other_att . sin_port = htons(PORT_Att+(id∗3)); if ( inet_aton (SERVER , &si_other_att.sin_addr) == 0) { fprintf ( stderr , " inet_aton () failed ( att )\n"); exit (1) ; } [...] // idéntico para el resto de sockets [...] Además dentro del bucle de ejecución se comprobará que ningún hilo asociado a la lectura de los sensores haya sido abortado por sobre tiempo, en caso de que esto ocurriera se volverá a arrancar. Código 3.14 Comprobación de fallo y re-lanzamiento de los sensores. /∗Por defecto todos los sensores siguen funcionando∗/ mpu_fail=false ; lsm_fail =false ; baro_fail =false ; RC_fail=false ; PWM_fail=false; lidar_fail =false ; gps_fail =false ; total_fail =false ; /∗si algun sensor ha fallado se comprueban todos para volver a arrancarlo ∗/ if (someone_died){ someone_died=0; timer_settime ( timerid , 0, &its_cero , NULL); /∗Se arranca ahrs∗/ if (mpu_exit){ mpu_fail=true; // ahrs_str get_ahrs ; printf ("ahrs restarted \n") ; mpu9250_imu._ahrs_required = true ; } /∗Se arranca mpu∗/ if (mpu_exit){ printf ("mpu restarted \n"); mpu9250_imu._mpu9250_imu.initialize(); pthread_t mpu9250_imu_thread; if ( pthread_create (&mpu9250_imu_thread, NULL, acquireMPU9250Data, (void ∗)&mpu9250_imu)){ printf ("Error : Failed to create mpu9250_imu thread\n"); return 0; } } [...] // identico para el resto de sensores
3.3 Drivers 19 Por último se muestra la recogida de datos y su actualización, siempre y cuando el hilo encargado siga funcionando, de los datos leídos de los sensores, así como el envío de los mismos utilizando los sockets creados anteriormente. Código 3.15 Actualización y envío del último valor válido. [...] // inicio de temporizador de timeout timer_settime ( timerid , 0, &its, NULL); // Calculo del tiempo para la ejecucion ciclica clock_gettime (CLOCK_REALTIME,&slp); if (( slp . tv_nsec+main_rate∗1000)<(1000000000)) slp .tv_nsec+=main_rate∗1000; else { slp . tv_sec+=1; slp .tv_nsec=slp.tv_nsec+main_rate∗1000−1000000000; } // calculo del incremento de tiempo entre cada envío gettimeofday(&tv,NULL); previoustime = currenttime ; currenttime = 1000000 ∗tv . tv_sec + tv . tv_usec; if ( previoustime!=0){ dt = ( currenttime −previoustime) / 1000000.0; if (dt < 1/1300.0) usleep((1/1300.0−dt)∗1000000); gettimeofday(&tv,NULL); currenttime = 1000000 ∗tv . tv_sec + tv . tv_usec; dt = ( currenttime −previoustime) / 1000000.0; }else{ dt=0; } // en caso de que no haya fallado el hilo se actualiza la estructura a enviar /∗Restablecimiento del temporizador de espera∗/ timer_settime ( timerid , 0, &its, NULL); /∗Tiempo a esperar , el actual más frecuencia del hilo ∗/ clock_gettime (CLOCK_REALTIME,&slp); if (( slp . tv_nsec+main_rate∗1000)<(1000000000)) slp .tv_nsec+=main_rate∗1000; else { slp . tv_sec+=1; slp .tv_nsec=slp.tv_nsec+main_rate∗1000−1000000000; } gettimeofday(&tv,NULL); previoustime = currenttime ; currenttime = 1000000 ∗tv . tv_sec + tv . tv_usec; /∗Cálculo del diferencial de tiempo entre iteraciones ∗/ if ( previoustime!=0){ dt = ( currenttime −previoustime) / 1000000.0; if (dt < 1/1300.0) usleep((1/1300.0−dt)∗1000000); gettimeofday(&tv,NULL); currenttime = 1000000 ∗tv . tv_sec + tv . tv_usec; dt = ( currenttime −previoustime) / 1000000.0; }else{ dt=0; } /∗Reasignación de la lectura de los sensores al mensaje a enviar , si no ha ∗fallado el hilo del sensor correspondiente ∗/ /∗Mensaje de actitud ∗/ if (!mpu_fail){ att_msg.imu_mpu=mpu9250_imu._shmmsg; att_msg.quat_mpu=mpu9250_imu._quaternion; } if (! lsm_fail )
20 Capítulo 3. Drivers para Navio2 att_msg.imu_lsm=lsm9ds1_imu._shmmsg; att_msg. fail_flags [0]=mpu_fail; att_msg. fail_flags [1]= lsm_fail ; att_msg.dt=dt; [...] // identico para todas las estructuras a enviar /∗Envío de los datos por UDP∗/ if (sendto( s_att , &att_msg, sizeof ( attitude_msg ) , 0 , ( struct sockaddr ∗) & si_other_att , slen )==−1) { udp_die("sendto_Att ()"); } [...] // identico para el resto de sockets /∗Espera para repetir ∗/ clock_nanosleep(CLOCK_REALTIME,TIMER_ABSTIME,&slp,NULL); }// Fin while(1) Esquema de funcionamiento hilo de sensor El funcionamiento de cada uno de los hilos que se encargan de leer todos los sensores siguen el esquema mostrado en la Figura 3.6. Básicamente en cada iteración actualiza el valor de la variable recibida como parámetro, la cual es accesible desde el hilo principal, en un periodo máximo establecido por ”comp_factor”, que marca el máximo número de iteraciones que se pueden perder, y ”sensor_rate”, que marca periodo estándar de cada hilo. Inicialización Creación Temporizador Reseteo Temporizador Actualización Medida Espera sensor_rate µs Figura 3.6 Esquema de funcionamiento hilo dedicado a leer los sensores. Aquí se puede observar la creación de los temporizadores de sobretiempo y espera de cada uno de los hilos de los sensores. Como se puede observar esta es bastante similar a la del hilo principal. Código 3.16 Creación temporizador de los sensores. [...] /∗∗∗∗∗∗∗∗∗∗∗∗∗ Se crea el ’’watchdog’’ del hilo de la Emisora ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/ /∗Establecimiento del manejador del temporizador∗/ sa. sa_flags = SA_SIGINFO; sa. sa_sigaction = timer_handler ; sigemptyset(&sa.sa_mask); sigaction (SIGRTMIN+N, &sa, NULL); /∗Block timer signal temporarily ∗/ sigemptyset(&mask); sigaddset (&mask, SIGRTMIN+N); sigprocmask(SIG_SETMASK, &mask, NULL); /∗Creación del temporizador∗/ sev. sigev_notify = SIGEV_SIGNAL; sev. sigev_signo = SIGRTMIN+N; sev. sigev_value . sival_ptr = &timerid;
3.3 Drivers 21 timer_create (CLOCK_REALTIME, &sev, &timerid); /∗Establecimiento de los valores del temporizador∗/ its . it_value . tv_sec = 0; its . it_value .tv_nsec = sensor_rate ∗comp_factor∗1000; its . it_interval . tv_sec = 0; its . it_interval .tv_nsec = 0; [...] La lectura de cada sensor es diferente en el sentido de que cada uno utiliza funciones y librerías propias, todas ellas proporcionadas por el fabricante de la Placa Navio2, Emlid. Aunque dado que la mayoría de los sensores utilizan el protocolo I2C todas las funciones se dedican básicamente a escribir y leer diferentes registros. A continuación se muestra un ejemplo de como se leen los datos de la IMU LSM9DS1. Código 3.17 Bucle hilo sensor(IMU LSM9DS1). [...] while (! lsm_exit) { timer_settime ( timerid , 0, &its, NULL); /∗Cálculo del tiempo de espera∗/ clock_gettime (CLOCK_REALTIME,&slp); if (( slp . tv_nsec+lsm_rate∗1000)<(1000000000)) slp .tv_nsec+=lsm_rate∗1000; else { slp . tv_sec+=1; slp .tv_nsec=slp.tv_nsec+lsm_rate∗1000−1000000000; } /∗Lectura de la medida∗/ lsm9ds1_imu−>_lsm9ds1_imu.update(); lsm9ds1_imu−>_lsm9ds1_imu.read_accelerometer(&lsm9ds1_imu−>_shmmsg._ax, &lsm9ds1_imu−>_shmmsg._ay, & lsm9ds1_imu−>_shmmsg._az); lsm9ds1_imu−>_lsm9ds1_imu.read_gyroscope(&lsm9ds1_imu−>_shmmsg._gx, &lsm9ds1_imu−>_shmmsg._gy, & lsm9ds1_imu−>_shmmsg._gz); lsm9ds1_imu−>_lsm9ds1_imu.read_magnetometer(&lsm9ds1_imu−>_shmmsg._mx, &lsm9ds1_imu−>_shmmsg._my, & lsm9ds1_imu−>_shmmsg._mz); lsm9ds1_imu−>_shmmsg._temperature = lsm9ds1_imu−>_lsm9ds1_imu.read_temperature(); /∗Espera para la repetici ón∗/ clock_nanosleep(CLOCK_REALTIME,TIMER_ABSTIME,&slp,NULL); }// Fin while(1) [...] Esquema de funcionamiento hilo de escritura en los motores Este hilo se dedica exclusivamente en recibir la señal de control enviada por MATLAB ® modificar la velocidad de los motores en función del valor recibido. Para esta comunicación se utiliza un socket UDP para cada proceso, dado que esto puede llegar a ocasionar que un mismo motor reciba distinta señal de PWM se ha incluido en este hilo un volcado a fichero de los valores que se escriben en los motores para poder realizar un análisis a posteriori en MATLAB®. Por estos motivos su esquema de funcionamiento difiere un poco de la de los demás hilos dedicados a controlar el resto de sensores.
Recepción y envio de datos en MATLAB® Si quieres ir rápido ve solo. Si quieres llegar lejos ve acompañado Proverbio Africano En este capitulo se expondrán los esquemas utilizados en SIMULINK ® junto a las funciones implementadas para la correcta recepción y análisis de datos recibidos desde la Raspberry Pi . Así como el esquema de envío desde SIMULINK®. Recepción de datos Para la recepción de los datos enviados desde la Raspberry Pi pi con la lectura de los sensores de Navio se ha utilizado el bloque de recepción UDP de SIMULINK ® el cual se ha combinado con otros dos bloque en serie, byte pack y byte unpack que se encargan de crear un único paquete de todos los datos recibidos por el bloque de recepción en un único paquete que después será separado en cada uno de los tipos de datos que se esperan recibir. Este esquema es necesario ya que mediante el bloque de recepción solo se puede recibir un tipo de dato en concreto por cada puerto y dado que en cada envío desde la Raspberry Pi hay como mínimo 2 tipos de datos distintos los datos distintos de tipo seleccionado no se podrán leer de forma correcta. Por ejemplo si enviamos un entero y un booleano y seleccionamos en matlab que vamos a recibir un entero el booleano se interpretará como un entero por lo que de los 4 bits solo el primero tendrá información útil el resto será irrelevante pero hará que el dato sea un numero del orden de ±212 . Si por otro lado especificamos que esperamos recibir un booleanos el entero se dividirá en 4 booleanos sin ningún tipo de coherencia. Configuración bloque UDP receive La forma más fácil de configurar este bloque, conociendo el total de bytes que se esperan recibir, es indicando que esperas recibir tantos datos tipo bool como bytes. Además es necesario indicar el puerto y crear un buffer de un paquete ya que si no el bloque esperá recibir N paquetes antes de expulsar uno, originando un severo retraso. Esta es la configuración que tenemos en la Figura 4.1. 29
30 Capítulo 4. Recepción y envio de datos en MATLAB® Figura 4.1 Configuración bloque UDP receive. Otra forma de configurar el bloque es eligiendo cualquier otro tipo de dato que se espera recibir teniendo en cuenta que se elija un numero que sea el primer múltiplo del número de bytes del tipo de dato elegido mayor que el número total de bytes enviados. Configuración bloques byte pack/unpack La configuración del bloque de byte pack es sencilla simplemente debemos seleccionar el mismo tipo de dato que elegimos en el bloque UDP receive comentado anteriormente así como el tipo de alineamiento de byte que tenemos en los datos enviados. En la Figura 4.2 se observa el bloque. Figura 4.2 Configuración bloque byte pack. Por otro lado en el bloque byte unpack donde realmente separamos los datos que hemos enviado desde la Raspberry Pi , como se ve en la Figura 4.3 en este bloque se debe seleccionar el número de datos de cada tipo que esperamos recibir así como el tipo de alineamiento de byte que tenemos.
4.2 Envío de datos 31 Figura 4.3 Configuración bloque byte unpack. Esquema completo En la Figura 4.4 se puede observar el esquema completo para la recepción de los datos desde la Raspberry Pi . Cabe destacar que se ha utilizado un zero-order hold para mantener el valor de los datos recibidos, además se ha realizado un enmascarado de este esquema para poder seleccionar el puerto a utilizar de forma que se pueda seleccionar desde fuera el proceso a leer de forma mucho mas rápida y sencilla. En la Figura 4.5 se puede apreciar la configuración del bloque de recepción de posición en este caso solo se pueden seleccionar los puertos desde 8083 hasta 8101 de 3 en 3, que es lo que correspondería a 7 procesos cada 3 tres puertos. UDP Receive Message Length Position Receive Terminator1 Byte Unpack Byte Unpack Byte Pack Byte Pack 1 total 2 GPS 3 baro 4 lidar 5 fail_flag Saturation Figura 4.4 Modelo de SIMULINK®completo. Figura 4.5 Configuración mascara de recepción, posición. Envío de datos Para el envío de datos la cosa se simplifica ya que en esta ocasión solamente necesitamos enviar un tipo de dato, single . Para ello utilizamos el bloque de SIMULINK ®UDP send que recibe la dirección IP a la que
32 Capítulo 4. Recepción y envio de datos en MATLAB® queremos enviar los datos y el puerto que se desea utilizar, Figura 4.4. Figura 4.6 Configuración bloque UDP send. Sin embargo, si se deseara enviar datos de tipos diferentes sería necesario realizar un byte pack para cada tipo y multiplexarlos entes del envío en el orden correcto en el que esperamos recibirlos en la Raspberry Pi . Análisis e interpretación de los datos recibidos Una vez recibidos los datos desde la Raspberry Pi en MATLAB ® ya pueden ser analizados de forma sencilla. En primer lugar vamos a comparar los datos recibidos desde los diferentes procesos corriendo en paralelo en la Raspberry Pi . Para ello vamos a realizar pruebas con distintos niveles de prioridad para cada proceso. Las distintas configuraciones de prioridad con las que se ha probado son las siguientes, siendo 20 la prioridad más baja y 0 la prioridad máxima posible, al nivel de los procesos del Sistema Operativo: Tabla 4.1 Diferentes prioridades utilizadas para los procesos de medida. Prioridad 20 Prioridad 15 Prioridad 0 Configuración 1 Todos - - Configuración 2 Todos menos id=0 id=0 - Configuración 3 Todos menos id=0 - id=0 Configuración 4 - - Todos Todos los procesos con 20 de prioridad En esta primera configuración se tratan a todos los procesos con un nivel de prioridad estándar, es decir, por debajo del resto de procesos propios del sistema operativo y al nivel de los procesos del ususario. A continuación se va a analizar la medida de aceleración realizada por ambas IMUs. 0 5 10 15 10 4 -20 0 20 aceleración en z proceso 0 0 5 10 15 10 4 -20 0 20 aceleración en z proceso 1 0 5 10 15 10 4 -20 0 20 aceleración en z proceso 2 Figura 4.7 Comparación de las medidas de las aceleraciones en y de 3 procesos distintos con prioridad 20 (imu LSM5611). 1.1 1.11 1.12 1.13 1.14 1.15 1.16 10 5 -20 -15 -10 -5 0 5 detalle superpuesto proceso0 proceso1 proceso2 Figura 4.8 Detalle de las medidas de las aceleraciones en y sobre el cuadrado (imu LSM5611).
4.3 Análisis e interpretación de los datos recibidos 33 0 5 10 15 10 4 -20 0 20 aceleración en z proceso 0 0 5 10 15 10 4 -20 0 20 aceleración en z proceso 1 0 5 10 15 10 4 -20 0 20 aceleración en z proceso 2 Figura 4.9 Comparación de las medidas de las aceleraciones en y de 3 procesos distintos con prioridad 20 (imu MPU9250). 1.1 1.11 1.12 1.13 1.14 1.15 1.16 10 5 -20 -15 -10 -5 0 5 detalle superpuesto proceso0 proceso1 proceso2 Figura 4.10 Detalle de las medidas de las aceleraciones en y sobre el cuadrado (imu MPU9250). La primera observación a destacar es que en todos los procesos no se da la misma medida de forma exacta, es decir al no estar sincronizados no dan una medida en el mismo tiempo aunque si que está dentro unos margenes bastante buenos para poder realizar algún tipo de filtrado o fusión. Por otro lado se ve como en algunas ocasiones los procesos sufren algún tipo de problema lo que provoca cierto retraso en la llegada de una nueva medida. Para tratar de solucionar estos problemas se proponen distintas configuraciones de prioridades de los procesos que se mostrarán a continuación a fin de buscar la mejor configuración de prioridades posibles. Todos los procesos con 20 de prioridad y uno con 15 Debido a los problemas descubiertos en la configuración de prioridades por defecto y que han sido comentados en el apartado anterior, se decidió probar a bajar la prioridad de uno de los procesos, en concreto al proceso 0, para darle preferencia sobre el resto aunque dejándolo por debajo de los procesos propios del sistema operativo. A continuación se muestran algunas gráficas en las que se pude comparar el comportamiento de los datos enviados por cada proceso: 0 2 4 6 8 10 12 14 16 18 10 4 -20 0 20 aceleración en z proceso 0 0 2 4 6 8 10 12 14 16 18 10 4 -20 0 20 aceleración en z proceso 1 0 2 4 6 8 10 12 14 16 18 10 4 -20 0 20 aceleración en z proceso 2 Figura 4.11 Comparación de las medidas de las aceleraciones en y de 3 procesos distintos 1 con prioridad 15 (imu LSM5611). 0.97 0.98 0.99 1 1.01 1.02 1.03 10 5 -10 -5 0 5 10 15 20 detalle superpuesto proceso0 proceso1 proceso2 Figura 4.12 Detalle de las medidas de las aceleraciones en y sobre el cuadrado (imu LSM5611).
34 Capítulo 4. Recepción y envio de datos en MATLAB® 0 2 4 6 8 10 12 14 16 18 10 4 -20 0 20 aceleración en z proceso 0 0 2 4 6 8 10 12 14 16 18 10 4 -20 0 20 aceleración en z proceso 1 0 2 4 6 8 10 12 14 16 18 10 4 -20 0 20 aceleración en z proceso 2 Figura 4.13 Comparación de las medidas de las aceleraciones en y de 3 procesos distintos 1 con prioridad 15 (imu MPU9250). 0.97 0.98 0.99 1 1.01 1.02 1.03 10 5 -10 -5 0 5 10 15 20 detalle superpuesto proceso0 proceso1 proceso2 Figura 4.14 Detalle de las medidas de las aceleraciones en y sobre el cuadrado (imu MPU9250). En estas figuras se aprecia un comportamiento algo mejor que en el caso anterior sin embargo el proceso con mayor prioridad (proceso 0) no tiene una mejoría apreciable frente al resto, de hecho es incluso peor. Por este motivo se a obviado el estudio de subir la prioridad de todos los procesos a este nivel. Todos los procesos con 20 de prioridad y uno con 0 Como siguiente prueba se propone subirle la prioridad al proceso 0 al nivel del resto de procesos del sistema operativo, es decir, asignarle prioridad 0, esta es además la máxima prioridad que se le puede asignar a un proceso del usuario. Con esta configuración los datos recibidos han sido los siguientes: 0 2 4 6 8 10 12 14 16 10 4 -20 0 20 aceleración en z proceso 0 0 2 4 6 8 10 12 14 16 10 4 -20 0 20 aceleración en z proceso 1 0 2 4 6 8 10 12 14 16 10 4 -20 0 20 aceleración en z proceso 2 Figura 4.15 Comparación de las medidas de las aceleraciones en y de 3 procesos distintos 1 con prioridad 0 (imu LSM5611). 4.7 4.8 4.9 5 5.1 5.2 5.3 10 4 -15 -10 -5 0 5 10 15 detalle superpuesto proceso0 proceso1 proceso2 Figura 4.16 Detalle de las medidas de las aceleraciones en y sobre el cuadrado (imu LSM5611).
4.3 Análisis e interpretación de los datos recibidos 35 0 2 4 6 8 10 12 14 16 10 4 -20 0 20 aceleración en z proceso 0 0 2 4 6 8 10 12 14 16 10 4 -20 0 20 aceleración en z proceso 1 0 2 4 6 8 10 12 14 16 10 4 -20 0 20 aceleración en z proceso 2 Figura 4.17 Comparación de las medidas de las aceleraciones en y de 3 procesos distintos 1 con prioridad 0 (imu MPU9250). 4.7 4.8 4.9 5 5.1 5.2 5.3 10 4 -15 -10 -5 0 5 10 15 detalle superpuesto proceso0 proceso1 proceso2 Figura 4.18 Detalle de las medidas de las aceleraciones en y sobre el cuadrado (imu MPU9250). En este caso observamos una clara mejoría en el proceso 0, vemos como no se produce ninguna perdida del mismo mientras que los otros dos si que hay algunas pérdidas esporádicas. Todos los procesos con 0 de prioridad Dados los resultados obtenidos en la configuración anterior se propone aumentar la prioridad de todos los procesos a fin de comprobar si de esta forma todos los procesos tienen el mismo comportamiento de forma de forma que se consiga un comportamiento mucho más estable en el mayor número de procesos posible. A continuación se muestran los datos recibidos con todos los procesos a máxima prioridad: 0 2 4 6 8 10 12 14 16 18 10 4 -20 0 20 aceleración en z proceso 0 0 2 4 6 8 10 12 14 16 18 10 4 -20 0 20 aceleración en z proceso 1 0 2 4 6 8 10 12 14 16 18 10 4 -20 0 20 aceleración en z proceso 2 Figura 4.19 Comparación de las medidas de las aceleraciones en y de 3 procesos distintos con prioridad 0 (imu LSM5611). 8 8.1 8.2 8.3 8.4 8.5 8.6 10 4 -15 -10 -5 0 5 10 15 20 detalle superpuesto proceso0 proceso1 proceso2 Figura 4.20 Detalle de las medidas de las aceleraciones en y sobre el cuadrado (imu LSM5611).
36 Capítulo 4. Recepción y envio de datos en MATLAB® 0 2 4 6 8 10 12 14 16 18 10 4 -20 0 20 aceleración en z proceso 0 0 2 4 6 8 10 12 14 16 18 10 4 -20 0 20 aceleración en z proceso 1 0 2 4 6 8 10 12 14 16 18 10 4 -20 0 20 aceleración en z proceso 2 Figura 4.21 Comparación de las medidas de las aceleraciones en y de 3 procesos distintos con prioridad 0(imu MPU9250). 8 8.1 8.2 8.3 8.4 8.5 8.6 10 4 -15 -10 -5 0 5 10 15 20 detalle superpuesto proceso0 proceso1 proceso2 Figura 4.22 Detalle de las medidas de las aceleraciones en y sobre el cuadrado (imu MPU9250). En base a los resultados mostrados se ha determinado que esta es la configuración ideal para implementar ya que aunque se producen alguna que otra pérdida de más respecto a solo tener un único proceso a máxima prioridad en este caso la mayoría de procesos mantienen su comportamiento lo que permitiría descartar la medida del proceso que falla mediante criterio de votación o mediante fusión sensorial. Esquema completo de SIMULINK®implementado A continuación, en la Figura 4.23 se muestra el esquema completo implementado en SIMULINK ® donde se puede observar los 3 subsistemas encargados de la recepción de los datos en bruto desde la Raspberry Pi , así como los correspondientes al EKF y motor mixer que se comentarán en el próximo capítulo. Todos los subsistemas encargados de la recepción, son similares al ya mostrado anteriormente en este capítulo y utilizan la misma metodología al igual que el bloque de envío que se encuentra a continuación del motor mixer. Por otro lado tanto el motor mixer como el EKF contienen una función de MATLAB ® con una pequeña adaptación de las entradas en el caso del EKF ya que estas funciones necesitan entrada de tipo doble por lo que es necesario la conversión de los datos recibidos. Además se obvia los datos de temperatura de las IMUs. Aún así estos subsistemas se mostrarán más adelante en este proyecto.(Sección 5.4, Sección 5.6)
4.4 Esquema completo de SIMULINK®implementado 37 Filtro Att receive RC Receive Motor mixer Pos receive imu_mpu mpu quat quat imu_lsm lsm dt att_fail_flag imu_mpu quat imu_lsm dt fail_flag1 att_receive [MPU] [LSM] MPU LSM dt GPS baro Fusion attitude Posicion general EKF imu_fusionada fusion [MPU] [LSM] att_fail_flag att_fail_flag1 [dt] [dt] attitude fusion1 total GPS baro lidar fail_flag position_receive total total gps gps baro baro lidar lidar pos_fail_flag pos_fail_flag [GPS] [baro] [GPS] [baro] Posicion fusion2 In1 UDP send2 T Mx My Mz PWM Mixer 5 T 0 Mx 0 My 0 Mz RC_channels fail_flag RC_channels_receive RC RC RC_fail_flag RC_fail_flag MPU LSM GPS baro Figura 4.23 Esquema completo implementado en SIMULINK®.
44 Capítulo 5. Estimador y motor mixer Filtro de Kalman para estimación de actitud Para la estimación de la orientación del UAV se han considerado diferentes opciones, en primer lugar se ha propuesto realizar una medida directa de la posición en roll y pitch directamente a partir de las aceleraciones y de yaw a partir del magnetómetro y fusionarlo con la integración de la medida del giróscopo. La siguiente opción que se consideró fue realizar una integración de la medida dada por el giróscopo similar a lo realizado en [ 20 ] donde a partir de la velocidad angular se calcula la matriz de rotación y posteriormente la actitud del UAV. Por último se ha probado con el filtro de Kalman extendido propuesto en el capítulo 8 del libro[ 21 ], sin ninguna simplificación junto con la medida de actitud a partir de la IMU propuesta en la primera solución. Estimación de la actitud de forma directa Para esta primera aproximación de la orientación del multirotor se ha utilizado lo mismo que propone PX4 en su ”EKF_replay” el cual replica su estimador en matlab [ 22 ]. Este cálculo consiste en utilizar las aceleraciones y a partir de la medida de la gravedad deducir el angulo de inclinación. Dado que el cálculo se realiza en quaterniones posteriormente se convierte a ángulos de euler para su mejor comprensión. Esto nos proporcionaría solo la medida de roll y pitch para el cálculo del yaw este método no es válido para ello se utiliza el magnetómetro con el objetivo de calcular el ángulo en yaw respecto al norte magnético. Eje z Yaw Eje x Pitch Eje y Eje y Pitch Eje z Roll Eje x Figura 5.11 Dibujo indicativo de los ejes de Roll,Pitch yYaw. Estimación de Roll yPitch El cálculo de los ángulos de roll y pitch se basa en el cálculo del módulo de la aceleración en los ejes cuerpo ”x” e ”y” respecto del eje cuerpo ”z” con esto obtenemos la magnitud del giro total, primer valor de los quaterniones. A continuación si no estamos en un valor que pueda llegar a generar una indeterminación se calcula la proyección de las aceleraciones en ejes cuerpo sobre el eje ”z” inercial y la normalizamos. Con estos datos ya se puede calcula directamente el valor de la actitud en quaterniones por lo que lo siguiente será convertirlo a ángulos de euler. g a z a y a X norm( , ) ay a X tiltMagnitude Figura 5.12 Dibujo explicativo del cálculo de roll ypitch.
5.2 Filtro de Kalman para estimación de actitud 45 Estimación del Yaw Para el cálculo del ángulo de yaw lo primero que se realiza es una rotación respecto a los ángulos calculados anteriormente de roll y pitch para transformar las medidas del magnetómetro en ejes cuerpo a ejes inerciales para obtener una estimación de yaw real mediante la arcotangente entre el valor sobre el eje ”y” y el eje ”x”. My Mx Mz norte yaw Figura 5.13 Dibujo indicativo del cálculo del yaw. Cálculo en MATLAB® En el siguiente fragmento de código se muestran el método que utiliza PX4 para la réplica de su filtro en matlab, la cual ha sido modificada para obtener además el ángulo de yaw . En ella se consigue calcular el roll y el pitch a partir de las aceleraciones y el yaw a partir del campo magnético. Código 5.1 Cálculo de la actitud basado en PX4. if( (xhat1(1)~=0 || xhat1(2)~=0 || xhat1(3)~=0) &&... (xhat1(7)~=0 && xhat1(8)~=0 && xhat1(9)~=0)) tiltMagnitude = atan2(norm([xhat1(1);xhat1(2)]),xhat1(3)); if (tiltMagnitude > 1e-3) tiltUnitVec = cross([xhat1(1);xhat1(2);xhat1(3)],[0;0;1]); tiltUnitVec = tiltUnitVec/norm([tiltUnitVec,tiltUnitVec]); tiltVec = tiltMagnitude*tiltUnitVec; quat = [cos(0.5*tiltMagnitude); tiltVec/tiltMagnitude*sin(0.5* tiltMagnitude)]; quat=quat/norm(quat); [~, roll, pitch]=quat2angle(quat’,’ZYX’); end mag=[xhat1(7) xhat1(8) xhat1(9)]*(eul2rotm([0 pitch roll])); yaw=atan2(mag(2),mag(1)); end Filtro implementado para fusión y estimación directa de la actitud A continuación se detalla el filtro utilizado para la fusión de las IMUs, idéntico al anterior, y la estimación de la actitud a partir de lo comentado previamente. Para ello simplemente se han ampliado las matrices correspondientes para los tres nuevos estados incluidos. Etapa de predicción: ˆx− k=Aˆxk−1+Buk−1(5.6) P− k=APk−1AT+Q(5.7) Etapa de actualización K=P− kCT[CP− kCT+R]−1(5.8)
46 Capítulo 5. Estimador y motor mixer ˆxk=ˆx− k+K(z−Cˆx− k)(5.9) Pk= (I−KC)P− k(5.10) ˆx− k= ˆax ˆay ˆaz ˆ Gx ˆ Gy ˆ Gz ˆ Mx ˆ My ˆ Mz ˆ φ ˆ θ ˆ ψ :=Estimaciones de acelerómetro, gyróscopo, magnetómetro y actitud. A:=[I9x9] [/09x3] [/03x3] [dt ·I3x3] [/03x3] [I3x3]12x12 := Asignamos de forma directa la estimación anterior a la actual y en el caso de los ángulo además integramos la estimación de los giróscopos. B=0Dado que no tenemos ninguna entrada al sistema. Q=1·10−3·diag(2.87,3.75,4.60,0.003,0.001,0.001,57.1,44.3,37,14.5,21.7,37) R=diag(R1···R21) IMU MPU9250 R1=2.26 ·10−3 R2=1.99 ·10−3 R3=5.20 ·10−3 R4=7.06 ·10−6 R5=5.09 ·10−6 R6=7.11 ·10−6 R7=1.35 ·103 R8=1.13 ·103 R9=9.52 ·103 IMU LSM9DS1 R10 =7.62 ·10−3 R11 =1.94 ·10−3 R12 =2.02 ·10−3 R13 =8.90 ·10−6 R14 =1.15 ·10−4 R15 =7.68 ·10−6 R16 =0.18 R17 =0.25 R18 =0.37 Actitud directa R19 =7.62 ·10−3 R20 =1.94 ·10−3 R21 =2.02 ·10−3 El cálculo de la matriz R y Q se ha realizado de forma similar a el filtro anterior en el que solo se fusionaba aunque se ha añadido los datos de la medida directa de actitud. z= Accmpu3x1 Gyrmpu3x1 Magmpu3x1 [Acclsm]3x1 [Gyrlsm]3x1 [Maglsm]3x1 Roll Pitch Yaw 21x1 := Almacenamos las medidas en primer lugar las medidas recibidas de la IMU MPU9250 después las de la IMU LSM9DS1, por ultimo la medida directa de la orientación según el Código 5.1 C= [I9x9] [/09x3] [I9x9] [/09x3] [/03x9] [I3x3] 12x12 := Con el objetivo de comparar la estimación con la medida de cada una de las IMUs y el cálculo de la orientación comentada anteriormente.
5.2 Filtro de Kalman para estimación de actitud 47 Esquema de funcionamiento de la función implementada Inicialización Normalizaión Magnetómetros Asignación entradas Estimación de la actitud Actualización vector medidas Asignacion de matrices del filtro Etapa de predicción Etapa de Actualización Figura 5.14 Esquema de funcionamiento de la función de estimación directa de la orientación. Resultados estimación directa Con el esquema de funcionamiento anterior y las matrices comentadas previamente se han obtenido los siguientes resultados. 0 2 4 6 8 10 tiempo simulacion (ms) 10 4 -2.5 -2 -1.5 -1 -0.5 0 0.5 1 roll (rad) Accel. Directa Roll Pitch Yaw Figura 5.15 Resultados obtenidos de la estimación directa de la actitud. En la Figura 5.15 se observa como la estimación no es del todo mala a excepción de que tiene bastante ruido a pesar de estar filtrada al limite de evitar grandes retrasos por lo que se debe buscar otra formar de
48 Capítulo 5. Estimador y motor mixer realizar la estimación. Además este método es bastante sensible a las aceleraciones lineales bruscas lo que en avión no es una gran problema pero que en un multirotor puede ocasionar grandes errores. Estimación de la actitud basada en el artículo Esta nueva estimación se basa en la propuesta realizada en el artículo [ 20 ] el cual propone una solución iterativa para el cálculo la orientación basado en sensores con diferentes frecuencias de medida, en su caso proponen como sensores una IMU y un VICON, el cual por si solo ya da una medida prácticamente perfecta. Las ecuaciones que ellos proponen, las cuales están en tiempo continuo son las siguientes: Ecuaciones propuestas en el artículo Predictor: ˙ ∆(t) = ∆(t)Ω(t)x∆(0) = ∆0(5.11) yp i=∆(t)T∆(t0 ki−τi)zi(t)t∈[t0 ki,t0 ki+1)(5.12) Observador: ˙ ˆ R(t) = ˆ R(t) Ω(t)−P(t) n ∑ i=1Liˆyi(t)−yP i(t)xˆyi(t)!x (5.13) Donde: ∆∈SO(3):=Estado interno del predictor. ∆0∈SO(3):=Valor arbitrario como condición inicial. Ω:=Velocidad angular del UAV. yP i:=Salida del predictor. zi:=Salidas anteriores del predictor. τi:=Retraso entre medidas. ˙ ˆ R:=Salida del observador, matriz de rotación correspondiente en el instante t. ˆ R(0)∈SO3 :=valor inicial de la observación. P,Li:=matrices de ganancias definidas positivas. ˆyi:=Estimación de la iteración anterior. El subíndice ”i” indica el sensor que proporciona las medidas y el operador (•)x es la matriz anti-simetrica de un vector R3\(a)xb=a×bo lo que es lo mismo: ax= a1 a2 a3 x = 0−a3a2 a30−a1 −a2a10 Ecuaciones propias implementadas Dado que nos encontramos en tiempo discreto y realmente no tenemos sensores con diferente frecuencia las ecuaciones anteriores deben ser modificadas ligeramente para garantizar convergencia. De este modo las ecuaciones realmente implementadas en MATLAB®son las siguientes: La Ecuación 5.11 pasa a ser: ∆k=∆k−1+∆k−1Ωxdt (5.14) La Ecuación 5.12 se convierte en: yP=∆T k−1∆k−1att (5.15) Por último la Ecuación 5.13 queda de la siguiente forma: ˆ Rk=Rk−1+Rk−1Ω−P1L(yP k−1−yP kxyP k−1x(5.16) Además ahora se actualiza la estimación de la actitud a partir de la matriz R, obteniendo así los valores de los ángulos de Euler (Roll,Pitch yYaw), Donde: ∆0=R0= cos(yaw)−sin(yaw)0 sin(yaw)cos(yaw)0 0 0 0 Ω:= Estado correspondiente a la velocidad angular estimada previamente por la fusión de las dos IMUs con el filtro de Kalman de la Sección 5.1.
5.2 Filtro de Kalman para estimación de actitud 49 att :=Medida directa de la orientación del UAV con el mismo método del apartado anterior. yP k−1:=Estimación actualizada con la obtención de los ángulos de Euler a partir de la matriz R. P:=Ruido del proceso asociado de la fusión de las IMUs. (Q(4:6,4:6)) L:=Matriz identidad de 3x3. Esquema de funcionamiento de la función implementada Inicialización Normalizaión Magnetómetros Asignación entradas Estimación de la actitud Fusión de las IMU's Predictor Actualización actitud Observador Figura 5.16 Esquema de funcionamiento de la función de estimación de la orientación basada en el artículo. Resultados obtenidos con la estimación del artículo Con las ecuaciones implementadas, Ecuación 5.14 a Ecuación 5.16 ya comentadas y utilizando los valores anteriores se han obtenido los siguientes resultados:: 0 2 4 6 8 10 tiempo simulacion (ms) 104 -1.5 -1 -0.5 0 0.5 1 roll (rad) Artículo Roll Pitch Yaw Figura 5.17 Resultados obtenidos de la estimación de la actitud basada en el artículo.
50 Capítulo 5. Estimador y motor mixer En la Figura 5.17 vemos como con este método los resultados no tienen prácticamente ruido alguno sin embargo dado que está ideado para tener como sensor un VICON, el cual, da una medida muchísimo más exacta y filtrada que los sensores de los que disponemos, este método de estimación introduce una deriva demasiado acentuada para que sea viable su uso. Estimación de la actitud basada en el libro Como último método para la estimación de la actitud se propone un filtro similar al que se propone en el capítulo 8 del libro ”Small Unmanned Aircraft”[ 21 ] en el que se propone un Filtro de Kalman Extendido basado en el modelo de propagación de un UAV junto a un algoritmo para ejecutarlo a una frecuencia mayor que la frecuencia de llegada de nuevos datos de los sensores. Ecuaciones propuestas en el libro Filtro de Kalman Extendido Mientras no llegan medidas nuevas (predictor): ˙ ˆx=f(ˆx,u)(5.17) A=∂f ∂x(ˆx,u)(5.18) ˙ P=AP +PAT+Q(5.19) Cuando llegan medidas nuevas (corrector): C=∂h ∂x−(ˆx,u)(5.20) L=P−CTR+CP−CT−1(5.21) ˆx+=Ly(tn)−h(ˆx−)(5.22) P+= (I−LC)P−(5.23) Modelo de propagación no lineal ˆ φ=p+qsin(φ)tan(θ)+ rcos(φ)tan(θ)(5.24) ˆ θ=qcos(φ)−rsin(φ)(5.25) Salida de los acelerómetros yaccel = ˙u+qw −rv +gsin(θ) ˙v+ru −pw −gcos(θ)sin(φ) ˙w+pv +qu −gcos(θ)cos(φ) (5.26) Donde: ˆ φ:=Estimación del ángulo de roll. ˆ θ:=Estimación del ángulo de pitch. p,q,r:=Velocidades angulares respecto de los ejes x,y,z respectivamente. u,v,w:=Velocidades lineales respecto de los ejes x,y,z respectivamente. g:=Gravedad. A continuación en el libro se desarrollan estas expresiones para eliminar las aceleraciones y velocidades ya que en el caso que ellos utilizan dan por hecho que las aceleraciones no se conocen sin embargo en nuestro caso las aceleraciones son conocidas y proporcionadas por ambas IMUs mientras que las velocidades pueden ser obtenidas integrando las mismas. Por lo tanto las ecuaciones que se implementaran en nuestro caso particular serán estas directamente junto con la fusión de las IMUs.
5.2 Filtro de Kalman para estimación de actitud 51 Sin embargo lo que si se implementará será el algoritmo que se propone el cual solo corrige la estimación cuando llega una medida nueva de alguno de los sensores y mientras esto ocurre actualiza la estimación anterior sumandola al EKF expuesto anteriormente multiplicado por la siguiente ganancia: Donde Tsensor es el periodo entre medidas del sensor y N es el numero de iteraciones entre dos muestras distintas consecutivas. Cuando llega una nueva medida directamente ejecutamos el corrector del EKF comentado anteriormente. Es importante mencionar que dado que para el cálculo de yaccel se precisa de la velocidad esta será calculada a partir de los acelerómetros e incluida como estado en el EKF. Ecuaciones finales implementadas para la fusión y estimación de la actitud De esta forma el filtro encargado de realizar la fusión y la estimación de actitud basado en este método será el siguiente: Filtro de Kalman: Cuando no hay nuevas medidas(Estimador): xk=xk−1+dtimu Nf(xk−1,u)(5.27) Pk=Pk−1+dtIMU NAPk−1+Pk−1AT+Q(5.28) Donde: x= ˆ Acc3x1 ˆ Gyr3x1 ˆ Mag3x1 ˆ Vel3x1 ˆ φ ˆ θ ˆ ψ 15x1 : Con ˆ Acc , ˆ Gyr y ˆ Mag fusión de las IMUs y ˆ Vel fusión de las integracion de las aceleraciones de cada IMU. ˆ φ,ˆ θ,ˆ ψestimación de la actitud del UAV. f(x,u) = /012x1 p+qsin(ˆ φk−1)tan(ˆ θk−1)+rcos(ˆ φk−1)tan(ˆ θk−1) qcos(ˆ φk−1)−rsin(ˆ φk−1) 0 15x1 : La fusión la mantenemos igual que la original y corregimos principalmente la actitud mediante lo propuesto en el libro. A= [I12x12] [/012x3] [/01x12]qcos(ˆ φk−1)tan(ˆ θk−1)−rsin(ˆ φk−1)tan(ˆ θk−1)qsin(ˆ φk−1)−rcos(ˆ φk−1) cos2(ˆ θk−1)0 [/01x12]−qsin(ˆ φk−1)−rcos(ˆ φk−1)0 0 [/01x12]0 0 1 15x15 Q=1·10−4·diag(1.29,3.75,4.60,0.003,0.001,0.001,571,4439,5461,22.58,19.85,52.02,74.73,56.94,469) Cuando llega una nueva medida (Corrector) L=P− kCT(R+CP− kCT)−1(5.29) xk=x− k+Lz−h(xk−1,u)(5.30) Pk= (I−LC)P− k(5.31) Donde: C= [I12x12] [/012x3] [I12x12] [/012x3] [/01x12]0gcos(ˆ θk−1)0 [/01x12]−gcos(ˆ φk−1)cos(ˆ θk−1)gsin(ˆ φk−1)sin(ˆ θk−1)0 [/01x12]gcos(ˆ θk−1)sin(ˆ φk−1)gcos(ˆ φk−1)sin(ˆ θk−1)0 [/03x12] [I3x3] 30x15 R=diag(R1···R30) : si la norma de la ultima estimación de la fusión de los acelerómetros está dentro de un cierto umbral, en este caso (5,14);
52 Capítulo 5. Estimador y motor mixer R=diag(R1···R24,1·104·(R25 ···R30)) : si la aceleración es muy alta descartamos le damos menos credibilidad a la medida. IMU MPU9250 R1=2.26 ·10−3 R2=1.99 ·10−3 R3=5.20 ·10−3 R4=7.06 ·10−6 R5=5.09 ·10−6 R6=7.11 ·10−6 R7=1.35 ·103 R8=1.13 ·103 R9=9.52 ·103 R10 =2.26 ·10−3 R11 =1.99 ·10−3 R12 =5.20 ·10−3 IMU LSM9DS1 R13 =7.62 ·10−3 R14 =1.94 ·10−3 R15 =2.02 ·10−3 R16 =8.90 ·10−6 R17 =1.15 ·10−4 R18 =7.68 ·10−6 R19 =0.18 R20 =0.25 R21 =0.37 R22 =7.62 ·10−3 R23 =1.94 ·10−3 R24 =2.02 ·10−3 yaccel R25 =2.19 ·10−3 R26 =1.27 ·10−3 R27 =5.13 ·10−3 Actitud directa R28 =1.06 ·10−4 R29 =1.69 ·10−4 R30 =6.75 ·10−4 z= Accmpu3x1 Gyrmpu3x1 Magmpu3x1 Velmpu3x1 [Acclsm]3x1 [Gyrlsm]3x1 [Maglsm]3x1 Velmpu3x1 yaccel(x− k)3x1 ˆ φ− ˆ θ− ˆ ψ− :ˆ φ−,ˆ θ−yˆ ψ−calculado de forma directa como se comento previamente h(xk−1,u) = ˆ Acck−13x1 ˆ Gyrk−13x1 ˆ Magk−13x1 ˆ Velk−13x1 yaccel(xk−1)3x1 ˆ φk−1 ˆ θk−1 ˆ ψk−1 Esquema de funcionamiento de la función implementada Inicialización Espera a datos válidos{1} Eliminación Offsets IMU{2} Etapa de predicción. Incremento Nº iter. Si el dato es nuevo{3} Si el dato no es nuevo{3} Cálculo de actitud e y_accel Etapa de Corrección Actualizacion N_ant Figura 5.18 Esquema de funcionamiento de la función de estimación de la orientación basada en el libro.
5.2 Filtro de Kalman para estimación de actitud 53 Algunos comentarios generales sobre la realización de la funcion comentada en el esquema anterior: {1} La espera de resultados validos se realiza comprobando que los valores de las aceleraciones son todos distintos de 0 que es el valor por defecto cuando no se reciben datos de la Raspberry Pi ya que el UAV siempre va a estar sometido a una aceleración en alguno de los ejes. Por otro lado también se considerará como dato invalido cuando alguno de los valores se mantenga constante durante más de 100 iteraciones consecutivas sin recibir datos diferentes, lo se puede considerar como que se ha perdido la conexión y el bloque UDP receive mantiene el ultimo valor recibido. {2} Ambas IMUs presentan un cierto offset en los sensores de aceleración lineal y velocidad angular los cual puede inducir errores al integrar dichas mediciones, por ello se ha optado por su eliminación. Para estimar el valor de la corrección se ha dejado la Raspberry Pi con la placa Navio2 estática en varias posiciones para comprobar que el offset es similar en todas ellas y se ha realizado la media de todas las medidas recibidas. De esta forma en cada iteración se elimina dicha media. {3} Por último la comprobación de si el dato utilizado para la nueva iteración es nuevo, se comprueba que sea diferente que el utilizado en la iteración anterior de esta forma se corrige solo cuando se recibe un dato nuevo. Resultados obtenidos con la estimación del libro Con estas matrices y la Ecuación 5.27 a la Ecuación 5.27 se han conseguido los siguientes resultados en la estimación de actitud. 0246810 tiempo simulacion (ms) 10 4 -2 -1.5 -1 -0.5 0 0.5 1 1.5 2 2.5 3 roll (rad) Libro Roll Pitch Yaw Figura 5.19 Resultados obtenidos de la estimación de la actitud basada en el libro. En esta ocasión vemos como nos hemos desecho completamente de la deriva del caso anterior, además se observa como la estimación está suficientemente filtrada como para poder ser utilizada. Por otro lado cabe destacar que este método es considerablemente menos sensible a las aceleraciones lineales. Comparación de las tres estimaciones
60 Capítulo 5. Estimador y motor mixer Si por el contrario no llega ninguna nueva medida de GPS las matrices tendrán la siguiente forma. C= [I12x12] [/012x3] [/012x3] [/012x3] [I12x12] [/012x3] [/012x3] [/012x3] [/01x12]0gcos(ˆ θk−1)0[/01x3] [/01x3] [/01x12]−gcos(ˆ φk−1)cos(ˆ θk−1)gsin(ˆ φk−1)sin(ˆ θk−1)0[/01x3] [/01x3] [/01x12]gcos(ˆ θk−1)sin(ˆ φk−1)gcos(ˆ φk−1)sin(ˆ θk−1)0[/01x3] [/01x3] [/03x12] [I3x3] [/03x3] [/03x3] [/01x12] [/01x3]1 0 0 [/01x3] [/01x12] [/01x3]0 1 0 [/01x3] [/01x12] [/01x3]0 0 1 [/01x3] [/01x12] [/01x3]0 0 1 [/01x3] [/01x12] [/01x3] [/01x3]1 0 0 [/01x12] [/01x3] [/01x3]0 1 0 [/01x12] [/01x3] [/01x3]0 0 1 37x21 R=diag(R1···R39) : si la norma de la ultima estimación de la fusión de los acelerómetros está dentro de un cierto umbral, en este caso (5,14); R=diag(R1···R24,1·104·(R25 ···R30),R31 ···R37) : si la aceleración es muy alta descartamos le damos menos credibilidad a la medida. IMU MPU9250 R1=2.26 ·10−3 R2=1.99 ·10−3 R3=5.20 ·10−3 R4=7.06 ·10−6 R5=5.09 ·10−6 R6=7.11 ·10−6 R7=1.35 ·103 R8=1.13 ·103 R9=9.52 ·103 R10 =2.26 ·10−3 R11 =1.99 ·10−3 R12 =5.20 ·10−3 IMU LSM9DS1 R13 =7.62 ·10−3 R14 =1.94 ·10−3 R15 =2.02 ·10−3 R16 =8.90 ·10−6 R17 =1.15 ·10−4 R18 =7.68 ·10−6 R19 =0.18 R20 =0.25 R21 =0.37 R22 =7.62 ·10−3 R23 =1.94 ·10−3 R24 =2.02 ·10−3 yaccel R25 =2.19 ·10−3 R26 =1.27 ·10−3 R27 =5.13 ·10−3 Actitud directa R28 =1.06 ·10−4 R29 =1.69 ·10−4 R30 =6.75 ·10−4 Posición NED R31 =0.1 R32 =0.1 R33 =0.1 R34 =1 Velocidad NED R35 =0.1 R36 =0.1 R37 =0.1 z= Accmpu3x1 Gyrmpu3x1 Magmpu3x1 Velmpu3x1 [Acclsm]3x1 [Gyrlsm]3x1 [Maglsm]3x1 Velmpu3x1 yaccel(x− k)3x1 ˆ φ− ˆ θ− ˆ ψ− Px Py Pz Dbaro Vx Vy Vz h(xk−1,u) = ˆ Acck−13x1 ˆ Gyrk−13x1 ˆ Magk−13x1 ˆ Velk−13x1 yaccel(xk−1)3x1 ˆ φk−1 ˆ θk−1 ˆ ψk−1 ˆ Nk−1 ˆ Ek−1 ˆ Dk−1 ˆ Dk−1 ˆ VN k−1 ˆ VE k−1 ˆ VD k−1 Resultados obtenidos A continuación se muestran los resultados obtenidos con el filtro de posición, en concreto se muestran dos experimentos, 1 el que se ha dejado el UAV completamente estático, Figura 5.27, y otro en el que se ha
5.3 Filtro de Kalman para estimación de posición 61 intentado realizar una trayectoria en L para ver su respuesta, Figura 5.28 y Figura 5.29. -0.5 0 0.5 1 1.5 2 2.5 Norte(m) -0.1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 Este(m) Deriva del GPS en una posición estática (~10s) Figura 5.27 UAV estático durante 10s. En base a este experimento se puede observar como existe una gran deriva en el filtro la cual sería interesante tratar de eliminar con algo más de tiempo en un futuro. -8 -7 -6 -5 -4 -3 -2 -1 0 1 Norte(m) -2 0 2 4 6 8 10 12 14 16 18 Este(m) Trayectoria en L, NE(~60s) Figura 5.28 Trayectoria 2D en L. -1 -0.8 -0.6 20 -0.4 -0.2 Altura(m) 0 0.2 0.4 Trayectoria en L, NED 2 0 Este(m) 10 -2 Norte(m) -4 -6 0-8 Figura 5.29 Trayectoria 3D en L.
62 Capítulo 5. Estimador y motor mixer Por otro lado vemos como a pesar de la deriva el filtro es capaz de reconocer la trayectoria seguida de forma aproximada. También se observa un ruido bastante alto en altitud provocado principalmente por el barómetro. En un futuro sería altamente prioritario arreglar el comportamiento de este filtro. Posiblemente el error se encuentre en las correcciones realizadas por la integración de las IMUs las cuales parecen no llevarse a cabo debidamente, por el pequeño diferencia de tiempo entre medidas. Subsistema EKF de SIMULINK® A continuación se muestra el interior del subsistema correspondiente al filtro EKF de simulink. 1 Fusion 1 MPU 2 LSM 3 dt Interpreted MATLAB Fcn IMU1(- temp) Interpreted MATLAB Fcn IMU2(- temp) MPU LSM dt_imu GPS baro t_sim fusion_imu att Pos 2 attitude double double double 4 GPS 5 baro Clock double double double 3 Posicion Fusion_IMU att Pos Figura 5.30 Interior del subsistema contenedor del EKF. Motor mixer Pseudoinversa Como primera solución a un posible mezclador que a partir de las señales que entregue el controlador en empuje y torque sobre los tres ejes, se propone la pseudoinversa de la matriz que indica la contribución de cada motor a la señal de control correspondiente. La matriz con las señales de control es la siguiente: F= T τx τy τz (5.48)
5.5 Motor mixer 63 y x z 1 2 35 4 6 Figura 5.31 Configuración montada en el hexarotor. En base a la Figura 5.31 se puede concluir que: T= 6 ∑ 1 Ti(5.49) τx=−LT1+LT2+Lcos60T3−Lcos60T4−Lcos60T5+Lcos60T6(5.50) τy=−0T1+0T2+Lsin 60T3−Lsin60T4+Lsin60T5−Lsin60T6(5.51) τz=−bT1+bT2−bT3+bT4+bT5−bT6(5.52) En forma matricial: F=D·x(5.53) T τx τy τz = 1 1 1 1 1 1 −L L 0.5L−0.5L−0.5L0.5L 0 0 √3/2L−√3/2L√3/2L−√3/2L −b b −b b b −b · T1 T2 T3 T4 T5 T6 (5.54) Donde: T:=Empuje total requerido al UAV τx:=Torque requerido respecto al eje x τy:=Torque requerido respecto al eje y τz:=Torque requerido respecto al eje z Ti:=Empuje individual de cada uno de los rotores L:=Longitud de cada brazo del UAV, en nuestro caso 0.55m. b:=Fricción de cada pala con el aire que genera un momento contrario al sentido de giro. Una vez planteado el problema queda claro que la solución al mismo no es única, ya que tenemos 2 grados de libertad, por ello como primera solución se propone la pseudoinversa que la solución que minimiza los empujes de cada uno. El principal problema que nos encontramos al utilizar esta solución es que la
64 Capítulo 5. Estimador y motor mixer solución obtenida no está acotado pudiendo pedirse empujes negativos o superiores a lo que cada motor puede proporcionar. De esta forma la solución al problema queda: x=D†F(5.55) T1 T2 T3 T4 T5 T6 =DT∗(D∗DT)−1; T τx τy τz (5.56) Esta solución es realmente fácil de implementar y apenas consume tiempo de computo lo que la hace ideal para ejecutarla en SIMULINK ® , sin embargo dado que no se encuentra acotada es inviable implementarla en un sistema real ya que no podemos asegurar que de una solución válida. Optimización de mínimos cuadrados Otra posible implementación del mezclador puede ser mediante ”programación cuadrática” con el objetivo de optimizar la solución de mínimos cuadrados pero manteniendo una saturación. La función a optimizar sería: J=1 2xTHx +fTx(5.57) Con, x= T1 T2 T3 T4 T5 T6 H=I6x6 f=/06x1 Y con las siguientes restricciones: Aeq ·x=beq (5.58) lb ≤x≤ub (5.59) Donde, Aeq = 1 1 1 1 1 1 −L L 0.5L−0.5L−0.5L0.5L 0 0 √3/2L−√3/2L√3/2L−√3/2L −b b −b b b −b beq = T τx τy τz lb =Tmin :=Empuje mínimo de cada motor. ub =Tmax :=Empuje máximo de cada motor. En esta ocasión nos encontramos con que somos capaces de saturar el empuje que puede dar cada motor aunque no siempre se encuentra una solución válida ya que se fuerza a que se cumpla la igualdad y al saturar puede no llegar a cumplirse. Además no es viable su implementación en SIMULINK ® ya que utiliza la función ”quadprog” que utiliza métodos numéricos para resolver la optimización y que no puede ser implementada en una ”matlab function” de SIMULINK ® por lo que si se quiere ejecutar se debe incluir en una función interpretada lo que ralentiza en exceso la ejecución de la simulación.
5.5 Motor mixer 65 Una posible mejora para conseguir siempre una solución válida es realizar la pseudoinversa, que se trata del caso óptimo cuando no tengamos saturación de ninguno de los motores y tratar de minimizar el error entre la salida deseada y posible teniendo en cuenta las saturaciones. Para ello se propone la siguiente optimización: J=1 2(AeqT−F)TH(AeqT−F) + f0(Aeq −F)(5.60) Donde al desarrollar la expresión se llega al siguiente resultado: J=1 2(TT(AT eqHAeq)T)−FTHAeqT=1 2TTˆ HT +f0T(5.61) Con la siguiente restricción: lb ≤x≤ub (5.62) Como se puede ver ahora no forzamos la igualdad ya que si no se consigue con la pseudoinversa no se puede conseguir cumpliendo las restricciones de empuje máximo y mínimo. Por otro lado mediante la matriz H se le da mucho más peso a minimizar el error en el empuje total que en los momentos sobre x,y,z. Por este motivo la matriz H tiene la siguiente forma: H= 100 1 1 1 De esta forma nos aseguramos que siempre tenemos una solución válida que minimiza el error entre lo que pide en controlador y lo que somos capaces de conseguir con los motores que tenemos. Sin embargo esto no soluciona el tiempo de ejecución ya que la iteración en la que la pseudoinversa no tenga solución válida tendremos que ejecutar este algoritmo lo que ralentizará en exceso el tiempo de ejecución. Por lo que una posible solución podría ser tabular los resultados de el máximo número de iteraciones para evitar tener que ejecutar el algoritmo, o incluso inferir una función que aproxime los resultados del mismo. Además de esto para intentar suavizar la transición entre la solución de la pseudoinversa y la optimización de mínimos cuadrados se ha modificado la Ecuación 5.61 añadiendole a la matriz ˆ H un termino constante de 5e-3. De esta forma se ha conseguido una transición mucho más suave. Por lo que la matriz implementada finalmente queda de la siguiente forma: ˆ H=AT eq ·diag(100,1,1,1,)·Aeq +5·10−3·I6x6 A continuación se muestra el empuje del motor 1 cuando se le pide al UAV un empuje total entre 5 y 55 N junto a un torque en ”x” entre -20 y 20 N/m. Donde se debe tener en cuenta que el empuje máximo y mínimo que puede proporcionar cada motor es de 8.333N y 1N respectivamente.
66 Capítulo 5. Estimador y motor mixer Figura 5.32 Empuje Motor 1. En base a la figura anterior se puede llegar a concluir que es viable la determinación de una función con la que expresar el valor de empuje de cada motor en función de las entradas ya que todos los motores son similares. En definitiva implementar algo similar a lo desarrollado en [25],[26]. Subsistema motor mixer de SIMULINK® A continuación se muestra el interior del subsistema correspondiente al motor mixer así como la máscara aplicada, en la que se definen los parámetros propios del UAV, como la longitud de los brazos o el empuje máximo y mínimo que deseamos, así como los valores propios de los motores utilizados, duty cycle máximo y mínimo.
5.6 Subsistema motor mixer de SIMULINK®67 EntradasdelControlador ParametrosMotores ParametrosUAV 1 PWM 1 T 2 Mx 3 My 4 Mz PWM_min PWMmin PWM_max PWMmax L_brazo L B B T_max Tmax T_min Tmin Interpreted MATLABFcn QP_hex Figura 5.33 Interior del subsistema contenedor del motor mixer. Figura 5.34 Máscara aplicada al mezclador.
Conclusiones y trabajo futuro Si no sabes hacia que puerto zarpa tu barco, ningún viento te será favorable Séneca En este capitulo se mostrará una comparativa entre el autopiloto desarrollado durante este trabajo frente al que se ha estado utilizando hasta ahora, PX4. Con fin de ver la viabilidad de su sustitución. Ademas se desarrollarán las conclusiones y posibles mejoras y ampliaciones a lo ya realizado. Comparación Autopiloto propio VS PX4 A continuación se mostrara una pequeña comparativa entre la salida del filtro de PX4, un más que conocido autopiloto y la salida del filtro realizado en este proyecto. Comparación en actitud En primer lugar se muestra la comparativa en actitud: 0 0.5 1 1.5 2 2.5 3 3.5 4 10 4 -0.5 0 0.5 Roll PX4 0123456 10 5 -0.5 0 0.5 Roll propio Figura 6.1 Comparación en Actitud de PX4 y el filtro propio, roll. 69
Índice de Códigos 3.1 Estructura para almacenar los datos de la IMU 10 3.2 Estructura para almacenar los datos del GPS 10 3.3 Inicialización Monitor 11 3.4 Comprobación drivers Monitor 11 3.5 Creación drivers Monitor 12 3.6 Espera drivers Monitor 12 3.7 Manejador de temporizador de sobretiempo 13 3.8 Espera entre ejecuciones 14 3.9 Inicialización hilo Principal Drivers 15 3.10 Lectura argumentos de entrada 15 3.11 Creación hilos 16 3.12 Creación Timer 17 3.13 Creación sockets de envío 17 3.14 Comprobación de fallo y re-lanzamiento de los sensores 18 3.15 Actualización y envío del último valor válido 19 3.16 Creación temporizador de los sensores 20 3.17 Bucle hilo sensor(IMU LSM9DS1) 21 3.18 Inicialización hilo PWM 22 3.19 Creación socket 22 3.20 Habilitación PWM y apertura del fichero 23 3.21 Bucle hilo de escritura PWM 23 3.22 Estructura ejemplo Byte alignment 26 5.1 Cálculo de la actitud basado en PX4 45 77
Bibliografía [1] PX4. (2018, Marzo) Px4 pro autopilot software. [Online]. Available: https://github.com/PX4/Firmware [2] Ardupilot. (2018, Marzo) Arduplane, arducopter, ardurover source. [Online]. Available: https: //github.com/ArduPilot/ardupilot [3] Emlid. (2018, Abril) Emlid. [Online]. Available: https://emlid.com/ [4] . PX4 Autopilot. (2018, Abril) Pixhaxk. [Online]. Available: https://pixhawk.org/ [5] . Qualcomm Technologies Inc. (2018, Abril) Snapdragon flight. [Online]. Available: https: //developer.qualcomm.com/hardware/qualcomm-flight [6] PX4. (2018, Marzo) Flight controller (autopilot) hardware. [Online]. Available: https://docs.px4.io/en/ flight_controller/ [7] Ardupilot. (2018, Marzo) Ardupilot. [Online]. Available: https://en.wikipedia.org/wiki/ArduPilot [8] Emlid. (2018, Marzo) Specifications section. [Online]. Available: https://emlid.com/navio/ [9] MPU-9250 Product Specification , InvenSense Inc., 6 2016, rev. 1.1. [Online]. Available: https://www.invensense.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf [10] LSM9DS1 , life.augmented, 12 2013, rev. 1. [Online]. Available: https://www.nordevx.com/datasheets/ LSM9DS1-datasheet.pdf [11] MS5611-01BA03 Barometric Pressure Sensor , Measurement Specialties ™ , 10 2012. [Online]. Available: https://infusionsystems.com/support/MS5611-01BA03.pdf [12] NEO-M8 , Ublox, 10 2015, rev. 10. [Online]. Available: https://www.u-blox.com/sites/default/files/ NEO-M8_DataSheet_(UBX-13003366).pdf [13] Emlid. (2018, Abril) Raspberry pi configuration. [Online]. Available: https://docs.emlid.com/navio2/ common/ardupilot/configuring-raspberry-pi/ [14] ——. (2018, Abril) C++ and python sensor examples for developers. [Online]. Available: https://github.com/emlid/Navio2 [15] Wikipedia. (2018, Abril) Raspberry pi. [Online]. Available: https://en.wikipedia.org/wiki/Raspberry_Pi [16] Emlid. (2018, Abril) Raspberry pi real-time kernel. [Online]. Available: https://emlid.com/raspberrypi-real-time-kernel/ [17] Intel. (2018, Abril) Raspberry pi real-time kernel. [Online]. Available: https://www.intel.es/content/ www/es/es/products/boards-kits/nuc.html [18] S.-H. Haase. (2014, January) Alignment in c. [Online]. Available: https://wr.informatik.uni-hamburg. de/_media/teaching/wintersemester_2013_2014/epc-14-haase-svenhendrik-alignmentinc-paper.pdf 79
80 Bibliografía [19] R. E. Kalman, “A new approach to linear filtering and prediction problems,” Transactions of the ASME–Journal of Basic Engineering , vol. 82, no. Series D, pp. 35–45, 1960. [Online]. Available: https://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf [20] A. Khosravian, J. Trumpf, R. Mahony, and T. Hamel, “Recursive attitude estimation in the presence of multi-rate and multi-delay vector measurements,” pp. 3199–3205, July 2015. [21] R. W. Beard and T. W. McLain, Small Unmanned Aircraft. Hardcover, 2012. [22] PX4. (2018, Junio) Aligntilt.m. [Online]. Available: https://github.com/PX4/ecl/blob/master/EKF/ matlab/EKF_replay/Common/AlignTilt.m [23] U. G. P. Office, “U.s. standard atmosphere,” p. 241, 1976. [Online]. Available: https://ntrs.nasa.gov/ archive/nasa/casi.ntrs.nasa.gov/19770009539.pdf [24] PX4. (2018, Julio) Llh2ned. [Online]. Available: https://github.com/PX4/ecl/blob/ 372f9f430bb0edb901b3265ecd0cddf8ee4d1ffb/EKF/matlab/EKF_replay/Common/LLH2NED.m [25] F. L. João C. Monteiro and L. Hsu, “Optimal control allocation of quadrotor uavs subject to actuator constraints,” p. 500, 2016. [26] G. J. J. Ducard and M.-D. Hua, “Discussion and practical aspects on control allocation for a multi-rotor helicopter,” p. 95, 2011.
Índice alfabético 81