Estimación de la orientación de un cuadricóptero por medio de la implementación de un EKF

Transcripción

1 La Mecatrónica en Méico, Mao 27, Vol. 6, No. 2, páginas Disponie en línea en SSN: , Asociación Meicana e Mecatrónica A.. Estimación e la orientación e un cuaricóptero por meio e la implementación e un EKF Meina Gutiérrez, Sergio. Muñoz ésar, Juan José. Hernánez Gómez, Luis Héctor nstituto Politécnico Nacional, Escuela Superior e ngeniería Mecánica Eléctrica, Sección e Estuios e Posgrao e nvestigación, Unia Zacatenco Resumen En este trabajo se presentan los resultaos obtenios al implementar un filtro e Kalman etenio para la estimación e los estaos e orientación e un cuaricóptero. Se supone que estos estaos son meios a través e un acelerómetro un giroscopio, ambas meiciones presentano ruio e canal. Primero se muestra el moelo utilizao para la simulación e vuelo, mismo que es no lineal por lo que para utilizar el algoritmo e Filtro e Kalman es necesario linealizarlo iscretizarlo. Posteriormente se implementa el filtro finalmente se mie el esempeño el filtro por meio e la integral el error cuarático, en el cual se toma como error e la iferencia e los estaos reales menos los estaos estimaos por el filtro. Palabras clave: Filtro e Kalman Etenio (EKF), cuaricóptero, UAV, orientación.. ntroucción Los cuaricópteros son robots volaores con hélices ubicaas alreeor e un cuerpo principal que presentan propieaes inámicas relativamente sencillas []. Este tipo e ispositivos caen entro e la categoría e micro aeronaves no tripulaas (UAVs por sus siglas en ingles) son una e las mejores opciones en términos e maniobrabilia, aemás e poer operar en ambientes triimensionales. La aaptabilia e estas aeronaves se ve comprometia ebio a la gran complejia en el moelao e la fricción el aire, la relación entre los motores hélices, así como la no linealia e los sensores [2]. Este tipo e aeronaves son elegies para tareas como búsquea rescate, inspección remota aplicaciones militares, evitano poner en riesgo las vias e los pilotos. uano las áreas son e ifícil acceso o las coniciones e vuelo no son apropiaas las UAV proveen ventajas tales como el espegue aterrizaje en espacios pequeños la capacia e volar por encima e objetivos tanto estáticos como móviles [3]. El control e cuaricópteros ha sio estuiao en múltiples ocasiones por iversos grupos obtenieno resultaos eitosos [3]-[6] en los que se implementan técnicas como PD, PD o LQR. Mientas que la aquisición e atos se realiza por meio e filtros Kalman. Los sensores comúnmente utilizaos en el esarrollo e estas aeronaves presentan ruio ebio a las vibraciones proucias por los motores e la misma provocano que el control sea ineficiente, por lo que un filtro o estimaor es requerio urante el vuelo en tiempo real para tener meiciones certeras [7]. El filtro e Kalman es una e las herramientas más utilizaas para la estimación e estaos e forma estocástica, éste realiza una preicción a través e las meiciones aquirias en tiempo real para obtener una meición más precisa [8] eliminar el ruio e canal. 2. Moelo e la orientación el cuaricóptero 46

2 La Mecatrónica en Méico, Mao 27, Vol. 6, No. 2, páginas Disponie en línea en SSN: , Asociación Meicana e Mecatrónica A.. La inámica el cuaricóptero se puee epresar por meio e os sistemas e ecuaciones iferenciales, el primero corresponiente a la orientación el seguno a la traslación epeniente el primero, en este ocumento sólo se tratará el primero. Para moelar la orientación e la aeronave se comienza por encontrar la matriz e orientación con respecto a un marco e referencia en tierra E, posteriormente se utilizan las ecuaciones e movimiento e Lagrange para obtener su comportamiento finalmente se agregan los momentos no conservativos proucios por los motores, éstos son las acciones e control el sistema. Figura. Marcos e referencia fuerzas presentes en el cuaricóptero 2. Matriz e rotación ecuaciones e Lagrange Por meio e las rotaciones e Euler se encuentra el vector e posición el cuaricóptero, r R R R cc sc s sc css cc sss cs ss csc sc ssc cc z () Al erivar la ecuación con respecto al tiempo calcular las energías cinéticas potenciales poemos obtener las ecuaciones e movimiento e Lagrange, al resolverlas obtenemos las ecuaciones que escriben las aceleraciones angulares e la aeronave, agregano los momentos no conservativos, los cuales son eterminaos por las acciones e control, obtenemos las ecuaciones 2 a 4. w w w ( ) (2) z w w w ( ) (3) z w z w w ( ) (4) z 47

3 La Mecatrónica en Méico, Mao 27, Vol. 6, No. 2, páginas Disponie en línea en SSN: , Asociación Meicana e Mecatrónica A Momentos no conservativos Las velociaes angulares e los motores son implementaas como acciones e control para generar los momentos no conservativos el sistema, e acuero con la configuración mostraa en la figura éstos son el resultao e una iferencial e fuerzas están aos por las ecuaciones 5 a 7. (5) ( ) 2 2 ( ) (6) ( 3 (7) En one b son constantes epenientes e la geometría e la hélices las coniciones ambientales, l es la istancia el centro e los motores al centro e masa el cuaricóptero las velociaes angulares e los motores. Se consiera que el tiempo e respuesta el motor utilizao es maor que el el cuaricóptero, e esta manera se puee aproimar las velociaes e los motores a las obtenias en estao estae. m ) ea (8) 3. mplementación el filtro e Kalman Etenio El filtro e Kalman es un estimaor estocástico que permite la observación e estaos reucción el ruio e canal, este algoritmo necesita el moelo iscreto el sistema epresao en espacio e estaos, así como la varianza e las meiciones para realizar sus estimaciones. El moelo el cuaricóptero mostrao en las ecuaciones 5 a 7 es no lineal por lo que es necesario realizar una linealización el mismo posteriormente realizar la iscretización, aemás al ser éste un sistema que opera en tiempo real es necesario realizar una nueva linealización en caa muestreo. Figura. Diagrama a oques el simulaor e vuelo 48

4 La Mecatrónica en Méico, Mao 27, Vol. 6, No. 2, páginas Disponie en línea en SSN: , Asociación Meicana e Mecatrónica A.. 3. Linealización el moelo e orientación La linealización el sistema se realiza por meio el cálculo e los jacobianos corresponientes a caa estao. Se consiera que las acciones e control son las velociaes cuaraas e los motores para obtención e la matriz e control B. A (9) B () 3.2 Discretización el moelo e orientación Las ecuaciones en 9 muestran la forma e las matrices A B el sistema linealizao el cuaricóptero, este por meio el métoo e Euler puee ser iscretizao para poer ser implementao en el filtro e Kalman. A B u () Las matrices A B son las matrices iscretas con tiempo e muestreo e las matrices A B. A (2) 49

5 La Mecatrónica en Méico, Mao 27, Vol. 6, No. 2, páginas Disponie en línea en SSN: , Asociación Meicana e Mecatrónica A.. 5 B (3) 3.3 Estimación e estaos por filtro e Kalman El filtro e Kalman consta e 3 etapas principales: preicción, cálculo e la ganancia e Kalman corrección. Este proceimiento se puee epresar por meio e las ecuaciones 4 a 2. ( ), Var P (4) Q A P A P,, (5),, ) ( R P P G (6),, ) ( P G P (7) ) ( ˆ, E (8),, ˆ ˆ u B A (9) ) ˆ ( ˆ ˆ,,, D u G (2) En one ˆ representa el estao estimao por meio el filtro e Kalman es la matriz e sensores el sistema aa por la ecuación 4 correspone a las meiciones el cabeceo, razón e cabeceo, giro, razón e giro razón e guiñaa. Las matrices Q R, eterminan los ruios inámico e canal, respectivamente. (4) Este filtro se implementó en Matlab Simulin como una función entro e un simulaor e vuelo para estimar los estaos el sistema en presencia e ruio, el cual es agregao por meio e un generaor e señales aleatorias.

6 La Mecatrónica en Méico, Mao 27, Vol. 6, No. 2, páginas Disponie en línea en SSN: , Asociación Meicana e Mecatrónica A.. Figura 3. Diagrama a oques el filtro e Kalman en Matlab Simulin 4. Resultaos Para probar la eficiencia el filtro e Kalman se miieron los estaos reales estimaos el sistema urante una simulación e vuelo, la cual consta e espegue movimiento e traslación circular con cambio en la guiñaa. Durante la simulación se graficaron los errores integrales cuaráticos el cabeceo (SE), el giro la guiñaa, ésta última observaa por el filtro a que no es meia para la estimación e estaos. Figura 4. SE s obtenios en la prueba En la figura 4 se muestran los SE obtenios urante la prueba, se observa que estos son el oren e -5, inicano esto un buen esempeño, así mimo se observan picos principalmente en los puntos en los que la guiñaa cambia e cuarante en los que el cabeceo o el giro presentan un cambio súbito. 5

7 La Mecatrónica en Méico, Mao 27, Vol. 6, No. 2, páginas Disponie en línea en SSN: , Asociación Meicana e Mecatrónica A.. Figura 5. Gráfica el cabeceo vs meición vs estimación En la figura 5 se muestra en color magenta el estao real el cabeceo urante la simulación e vuelo, en color beige la señal meia en azul la estimaa por el filtro e Kalman. Se observa al igual que en la figura 4 que eiste un error maor cuano se presenta una variación en el cabeceo a pesar e esta perturbación el filtro es capaz e realizar una correcta estimación e éste. Finalmente, la figura 6 muestra una comparativa e las SE e los estaos reales contra los estaos estimaos con ruio el cabeceo el giro, se observa que la línea elgaa en color azul presenta un incremente sustancial e esta integral con los errores calculaos con la señal ruiosa, mientras que la SE e la señal estimaa, representaa por la línea gruesa en color magenta, se mantiene con un valor cercano a cero ambos casos. Figura 6. Gráficas e las SE e los estaos con ruio contra los estimaos por el EKF onclusiones El filtro e Kalman Etenio muestra una gran capacia para estimar los estaos el moelo no lineal el cuaricóptero a pesar e trabajar con un sistema linealizao igitalizao, lo cual implica péria e información. Es importante recalcar que el tiempo e muestreo seleccionao para éste 52

8 La Mecatrónica en Méico, Mao 27, Vol. 6, No. 2, páginas Disponie en línea en SSN: , Asociación Meicana e Mecatrónica A.. ebe ser lo suficientemente corto como para ser capaz e realizar estimaciones correctas, sin embargo también se ebe tomar en cuenta que éste no sea tan breve como para utilizar toos los recursos el procesaor en el que se implemente, por lo que es crucial mantener un equilibrio entre ambas consieraciones. Algunos sensores aemás e incluir ruio en las meiciones también presentan eslizamientos, tal es el caso e los giroscopios, en estos casos es conveniente agregar una moificación al filtro e Kalman en la cual se estimen parámetros que compensen esta esviación. Referencias [] Moghavvemi, M., Salih, A., Mohame, H., an Gaei, K. Moelling an PD ontroller Design for a Quarotor Unmanne Air Vehicle. nternational onference on Automation Qualit an esting Robotics (AQR), EEE, 2. [2] Kumar, V. an Michael, N. Opportunities an hallenges with Autonomous Micro Aerial Vehicles. he nternational Journal of Robotics Research. SAGE, 3(), , 22. [3] Erginer, B. an Altug, E. Moelling an PD ontrol of a Quarotor VOL Vehicle, EEE ntelligent Vehicles Smposium, EEE, , 27. [4] Bouaballah, S., Noth, A., an Siegwart, R. Pi vs lq control techniques applie to an inoor micro quarotor. nternational onference on ntelligent Robots an Sstems, EEE, volume 5, , 24. [5] Bouaballah, S., Murrieri, P., an Siegwart, R. (24a). Design an control of an inoor micro quarotor. nternational onference on Robotics an Automation (RA), EEE, volume 5, pages , 24. [6] novan, R., Ataa, A., nuna, H., Aburrahman, M. Q., ahai, A., an Yamamoto, Y. (24). A cascae controller for linearize quarotor moel. nternational onference on Avance Robotics an ntelligent Sstems (ARS), EEE, pages [7] Gooarzi, F., Lee,. Etene Kalman filter on SE (3) for geometric control of a quarotor UAV, nternational onference on Unmanne Aircraft Sstems (UAS), EEE, 26. [8] Wang, Y., Puig, V. Zonotopic etene Kalman filter an fault etection of iscrete-time nonlinear sstems applie to a quarotor helicopter, 3r onference on ontrol an Fault-olerant Sstems (Ssol), EEE,