INSTITUTO POLITÉCNICO NACIONAL

(1)

INSTITUTO POLIT´ ECNICO NACIONAL

CENTRO DE INVESTIGACI ´ ON Y DESARROLLO DE TECNOLOG´ IA DIGITAL

CONTROL DE VEH´ICULOS AUT ´ONOMOS EN TAREAS COORDINADAS

TESIS

QUE PARA OBTENER EL GRADO DE:

MAESTR´IA EN CIENCIAS EN SISTEMAS DIGITALES

PRESENTA:

ING. DANIEL URESTI MORALES BAJO LA DIRECCI ´ON DE:

DR. EDUARDO JAVIER MORENO VALENZUELA DR. RICARDO RAM ´ON P´EREZ ALCOCER

NOVIEMBRE 2019 TIJUANA, B.C., M´EXICO.

(2)

(3)

(4)

(5)

Dedicatoria

A mis padres, Sotero Uresti Tagar y Marina Morales Vera, a mi esposa Nora Liliana Vazquez Talamantes, a toda mi familia que tanto amo.

(6)

Agradecimientos

A Dios por el diario aliento de vida.

Agradezco a mi familia por la educaci´on y el apoyo durante toda mi vida, que ha dado como resultado lo que realmente soy..

También agradezco al Dr. Ricardo Ramón Pérez Alcocer por dirigir este trabajo, por compartir su conocimiento, su dedicación, empeño y sobre todo paciencia.

As´ı mismo le agradezco al Dr. Eduardo Javier Moreno Valenzuela por asesorar este trabajo, anexarme a su equipo y proporcionar consejos.

Por sus comentarios, su disposición y tiempo brindados a la atención en este trabajo, le agradezco a los miembros del comité tutorial, el Dr. V´ıctor Hugo D´ıaz Ram´ırez, el Dr. Rigoberto Juaréz Salazar y el Dr. Kostantin Starkov.

Un profundo agradecimiento a mis Ex-compa˜neros, en especial al M. en C. Ale- jandro Vergudo por siempre ayudarme y darme ´animos cuando todo parec´ıa gris.

Al equipo de trabajo con el colaboré, el M. en C. Luis Gonzalo Montoya Villegas, el M. en C. Iván Alonso López Sánchez, M. en C. Jorge Alberto Montoya Chairez y al Ing. Luis Javier Quijada Rocha por brindarme su apoyo an´ımico, experiencia, y conocimiento. Al M. en C. Octavio Augusto Garc´ıa Alarcón por sus consejos y apoyo en todos los sentidos.

A mis compa˜neros, el Ing. Antonio Ortega, Ing. Israel Dominguez y Ing. Ricardo por siempre creer en mi y apoyarme moralmente para seguir adelante.

Al CONACyT y al IPN, por el apoyo econ´omico y la infraestructura brindados para la realizaci´on de este trabajo.

(7)

Control de veh´ıculos aut´ onomos en tareas coordinadas

Resumen

En esta tesis se aborda el problema de consenso de un sistema multi-agente hete- rogéneo. En particular, se analizan distintas técnicas de control aplicadas a veh´ıculos terrestres con dos ruedas y quadrotores. Los controladores aplicados al veh´ıculo terrestre y uno implementado en el quadrotor fueron tomados de la literatura. Adicio- nalmente se propone un nuevo control no lineal con compensación del modelo para el quadrotor el cual es validado tanto teórica como experimentalmente. Por otra parte, se detalla una estrategia de control de consenso que garantiza el cumplimiento del mismo siempre que los controladores aplicados a cada agente del sistema aseguren estabilidad asintótica en la tarea de regulación. De manera experimental se analiza y compara el desempeño del esquema de consenso usando controladores óptimos lineales y algoritmos de control no lineales para cada uno de los veh´ıculos del sistema de prueba. De esta comparación, los resultados muestran un mejor desempeño por parte del sistema multi-agente usando los controladores no lineales.

(8)

Control of autonomous vehicles in coor- dinated tasks

Abstrac

The problem of consensus of a heterogeneous multi-agent system is addressed in this thesis. In particular, different control techniques applied to ground two wheels vehicles and quadrotors are analyzed. The controllers applied to the ground vehicle and one implemented in the quadrotor were taken from the literature. Additionally, for the quadrotor, a non-linear control with dynamic model compensation is proposed.

This scheme is validated both theoretically and experimentally. On the other hand, a consensus control strategy is discussed. This methodology guarantees the consensus task when the controllers applied to each agent of the system ensure asymptotic stability for the regulation task. Experimentally, the performance of the consensus scheme is analyzed and compared using optimal linear controllers and non-linear control algorithms for each of the vehicles in the test system. From this comparison, the results show a better performance by the multi-agent system using the non-linear controllers.

(9)

´ Indice general

1. Introducci´on 1

1.1. Antecedentes . . . 2

1.2. Objetivos . . . 4

1.2.1. Objetivo general . . . 4

1.2.2. Objetivos espec´ıficos . . . 4

1.3. Motivaci´on . . . 4

1.4. Planteamiento del problema . . . 5

1.5. Metodolog´ıa . . . 5

1.6. Estructura de la tesis . . . 5

2. Veh´ıculo terrestre de dos ruedas 7 2.1. Modelo cinem´atico . . . 7

2.2. Controladores para el veh´ıculo terrestre de dos ruedas . . . 8

2.2.1. Control rotar y avanzar basado en LQR . . . 8

2.2.2. Control de Dixon, Dawson, Zergeroglu y Behal . . . 11

2.3. Resultados de control para regulaci´on . . . 13

2.3.1. Resultados de simulaci´on num´erica . . . 13

2.3.2. Resultados experimentales . . . 14

3. Quadrotor 19 3.1. Modelo din´amico . . . 19

3.2. Control LQR . . . 21

3.3. Control no lineal con compensaci´on del modelo . . . 24

3.4. Resultados de control para regulaci´on . . . 31

3.4.1. Resultados de simulaci´on num´erica . . . 31

3.4.2. Resultados experimentales . . . 34

4. Consenso del sistema multi-agente heterog´eneo 45 4.1. Teor´ıa de grafos . . . 45 4.2. Estrategia de consenso para un sistemas multi-agentes heterog´eneos . 47

(10)

4.3. Resultados del consenso de un sistema multi-agente heterogéneo . . . 50 4.3.1. Resultados de simulación numérica . . . 50 4.3.2. Resultados experimentales . . . 51

5. Conclusiones 59

5.1. Conclusi´on . . . 59 5.2. Trabajo Futuro . . . 60

(11)

´ Indice de figuras

2.1. Vista superior del veh´ıculo terrestre de dos ruedas en la que se incluye los marcos de referencia inercial y del cuerpo y las variables que definen su pose. . . 8 2.2. Resultados de simulación: Evolución temporal de la posición x(t)

y y(t) y el error de posición ˜x(t) y ˜y(t) empleando los controladores CVTR1 (l´ınea roja) y CVTR2 (l´ınea azul) en la tarea de regulación. . 14 2.3. Resultados de simulación: Evolución temporal de la orientación

θ(t) y error de orientación ˜θ(t) empleando los controladores CVTR1 (l´ınea roja) y CVTR2 (l´ınea azul) en la tarea de regulación. . . 15 2.4. Resultados de simulación: Velocidad lineal V (t) y angular W (t)

obtenidas con los controladores CVTR1 (l´ınea roja) y CVTR2 (l´ınea azul) durante la tarea de regulaci´on. . . 15 2.5. Configuraci´on del sistema experimental para el control del veh´ıculo

terrestre de dos ruedas. . . 16 2.6. Resultados de experimento en tiempo real: Evoluci´on temporal

de la posición x(t) y y(t) y el error de posición ˜x(t) y ˜y(t) empleando los controladores CVTR1 (l´ınea roja) y CVTR2 (l´ınea azul) en la tarea de regulación. . . 17 2.7. Resultados de experimento en tiempo real: Evolución temporal

de la orientación θ(t) y error de orientación ˜θ(t) empleando los controladores CVTR1 (l´ınea roja) y CVTR2 (l´ınea azul) en la tarea de regulación. . . 18 2.8. Resultados de experimento en tiempo real: Velocidad lineal

V (t) y angular W (t) obtenidas con los controladores CVTR1 (l´ınea roja) y CVTR2 (l´ınea azul) durante la tarea de regulaci´on. . . 18 3.1. Representaci´on del quadrotor incluyendo el marco de referencia iner-

cial I y el marco de referencia del cuerpo del veh´ıculo B. . . 20 3.2. Diagrama de bloques de la implementaci´on del controlador No lineal

con compensaci´on del modelo. . . 22

(12)

3.3. Diagrama de bloques de la implementación del controlador no lineal con compensación del modelo. . . 25 3.4. Resultados de simulación: Ruta descrita por el quadrotor en el

espacio tridimensional en la tarea de regulación aplicando los controladores CQ1 (l´ınea roja) y CQ2 (l´ınea azul). . . 33 3.5. Resultados de simulación: Evolución temporal de la posición p(t)

y el ángulo de guiñada ψ(t) empleando los controladores CQ1 (l´ınea roja) y CQ2 (l´ınea azul) en la tarea de regulación. . . 34 3.6. Resultados de simulación: Gráficas de error de posición ˜p y orien-

taci´on ˜η obtenidos con el controlador CQ1 (l´ınea roja) y controlador CQ2 (l´ınea azul). . . 35 3.7. Resultados de simulaci´on: Empuje total y momentos obtenidos

con los controladores durante la tarea de regulaci´on. . . 36 3.8. Configuraci´on del sistema experimental para el control del veh´ıculo

a´ereo. . . 37 3.9. Resultados de experimento en tiempo real: Ruta descrita por el

quadrotor en el espacio tridimensional durante la tarea de regulaci´on aplicando el control CQ1 (l´ınea roja) y el control CQ2 (l´ınea azul). . 38 3.10. Resultados de experimento en tiempo real: Evoluci´on temporal

de la posición p(t) y el ángulo de guiñada ψ(t) empleando los controladores CQ1 (l´ınea roja) y CQ2 (l´ınea azul) durante la tarea de regulación. . . 38 3.11. Resultados de experimento en tiempo real: Gráficas de error

de posici´on ˜p(t) y orientaci´on ˜η(t) obtenidos con el controlador CQ1 (l´ınea roja) y controlador CQ2 (l´ınea azul). . . 39 3.12. Resultados de experimento en tiempo real: Empuje total fpz(t)

y momentos τ (t) obtenidos con los controladores CQ1 (l´ınea roja) y CQ2 (l´ınea azul) en la tarea de regulaci´on. . . 40 3.13. Resultados de experimento en tiempo real: Ruta descrita por

el quadrotor Qball 2 en el espacio tridimensional durante la tarea de seguimiento de trayectorias usando el controlador CQ2. . . 41 3.14. Resultados de experimento en tiempo real: Evoluci´on temporal

de las señales de posición p(t) y orientación η(t) del quadrotor durante la tarea de seguimiento de trayectorias empleando el controlador CQ2. 42 3.15. Resultados de experimento en tiempo real: Gráficas de error

de posición p(t) y orientación η(t) obtenidos con el controlador CQ2 durante la tarea de seguimiento de trayectorias. . . 43 3.16. Resultados de experimento en tiempo real: Evolución temporal

de las se˜nales del empuje total f_pz(t) y momentos τ (t) calculados en el experimento de seguimiento de trayectorias con el controlador CQ2. 44

(13)

4.1. Representaci´on de un grafo. . . 46 4.2. Grafo de interacci´on del sistema multi-agente constituido por un veh´ıcu-

lo terrestre y un quadrotor. . . 50 4.3. Resultados de simulaci´on: Rutas descritas por el sistema multi-

agente en el plano xy y en el espacio tridimensional durante la tarea de consenso aplicando CLQR y CCNL. . . 52 4.4. Resultados de simulación: Evolución temporal de la posición x(t)

y y(t) del sistema multi-agente aplicando CLQR y CCNL durante la tarea de consenso. . . 53 4.5. Resultados de simulaci´on: Error de consenso de posici´on del siste-

ma multi-agente en los ejes x y y, obtenidos aplicando CLQR y CCNL en la tarea de consenso. . . 53 4.6. Resultados de simulaci´on: Velocidad lineal y velocidad angular

obtenidos aplicando CLQR y CCNL en la tarea de consenso. . . 54 4.7. Resultados de simulaci´on: Empuje total y momentos de fuerza

obtenidos aplicando CLQR y CCNL en la tarea de consenso . . . 54 4.8. Configuraci´on experimental del sistema multi-agente heterog´eneo. . . 55 4.9. Resultados de experimento en tiempo real: Rutas descritas por

el sistema multiagente en el plano xy y en el espacio tridimensional durante la tarea de consenso aplicando CLQR y CCNL. . . 56 4.10. Real-time experiment results: Evoluci´on temporal de la posici´on

x(t) and y(t) del sistema multi-agente aplicando CLQR y CCNL. . . 57 4.11. Resultados de experimento en tiempo real: Error de consenso

del sistema multi-agente en los ejes x y y. . . 57 4.12. Resultados de experimento en tiempo real: Velocidad lineal y

velocidad angular obtenidos con CLQR y CCNL en la tarea de consenso. 58 4.13. Resultados de experimento en tiempo real: Empuje total y mo-

mentos de fuerza obtenidos con CLQR y CCLN en la tarea de consenso. 58

(14)

(15)

´ Indice de tablas

2.1. Valores RMS de los errores de postura del Qbot en el experimento de regulaci´on para la comparaci´on de los controladores CVTR1 y CVTR2. Estos valores fueron calculados para 20 [s] ≤ t ≤ 30 [s]. . . . 18 3.1. Valores RMS de los errores de postura del Qball en el experimento de

regulación para la comparación de CQ1 y CQ2, para 15 ≤ t ≤ 50. . . 37 4.1. Valor promedio de las señales del error de consenso de posición en el

intervalo de tiempo 25 [s] ≤ t ≤ 45 [s]. . . 55

(16)

Cap´ıtulo 1 Introducci´ on

El uso de los veh´ıculos no tripulados, sean terrestres, aéreos o acuáticos, ha cre- cido considerablemente en diversos ámbitos como lo son la exploración, vigilancia, trasporte, entretenimiento, por mencionar algunos ejemplos. Por tal motivo, es de in- terés lograr que estos sistemas sean lo más autónomos posible; esto implica el diseño y validación de distintos algoritmos que permitan la reducción de la intervención de un piloto y también garanticen el cumplimiento de las tareas asignadas al sistema.

En la actualidad se ha alcanzado grandes avances en cuestión de los sistemas no tripulados, en el área de diseño, tecnolog´ıa de adquisición de datos, hasta control que es donde se aloja este trabajo.

Este trabajo se enfoca en torno al control de dos de estos tipos de veh´ıculos no tripulados, el veh´ıculo terrestre de dos ruedas y el veh´ıculo a´ereo llamado quadrotor.

El veh´ıculo terrestre de dos ruedas posee como principal caracter´ıstica que su desplazamiento en el plano horizontal se lleva a cabo por medio de la variación de las velocidades de sus dos ruedas, el diseño de esquemas de control para esta plataforma presenta dificultades debido a las restricciones no holónomicas propias de su modelo cinemático [1]-[4].

Por otra parte, los quadrotores son veh´ıculos aéreos con una estructura en forma de x cual cuentan con cuatro motores alimentados por energ´ıa eléctrica a los cuales se les acoplan hélices que generan las fuerzas y momentos necesarios para que realicen la tarea de vuelo asignada. Con estos actuadores el veh´ıculo puede desplazarse, mantenerse en el aire y cambiar su orientación. El control de estos dispositivos resulta complejo por ser un sistema sub-actuado, debido a que posee seis grados libertad y sólo cuatro actuadores [5]-[8].

Si bien estos veh´ıculos brindan utilidad en sus tareas asignadas, el uso de un grupo de estos trabajando en conjunto incrementa sus capacidades y su funcionalidad,

(17)

pudiendo realizar tareas complejas más allá de la capacidad de un veh´ıculo individual. Este enfoque de un grupo de veh´ıculos realizando tareas en conjunto requiere de estrategias de control cooperativo que garanticen el cumplimiento de la misma [11]-[20]. Russell y Norving en [9] definen a un agente como una entidad autónoma capaz de percibir mediante sensores e interactuar por medio de actuadores con el ambiente. Además, si un grupo de veh´ıculos autónomos posee una red de trabajo donde tienen la capacidad de trasmitir información entre si, puede identificarse como un sistema multi-agente.

El problema de control cooperativo de veh´ıculos no tripulados que aborda este trabajo es el consenso en sistemas multi-agente. El principal objetivo del consenso es que los agentes concuerden en un valor com´un como posici´on, velocidad o postura por medio de interacciones entre si. Por lo tanto es necesario implementar esquemas de control a cada uno de los agentes, de tal modo que puedan realizar la tarea individual asignada y alcanzar el objetivo conjunto. alcanzar dicho objetivo en conjunto.

Este problema de consenso, es fundamental para la coordinaci´on distribuida en los sistemas multi-agente [10].

1.1. Antecedentes

Los sistemas no tripulados han ocupado un sitio importante en el ámbito civil y militar, y cada d´ıa surgen más aplicaciones para estos dispositivos. Estos sistemas poseen ventajas en su diseño que permite la realización de tareas complejas sin necesidad de poner en peligro a un piloto. Hoy en d´ıa se busca que estos sistemas realicen tareas cada vez más complejas sin la necesidad de un operador y all´ı radica el interés de diseñar controladores eficientes. El diseño de controladores para que estos sistemas realicen su tarea a pesar de distintas condiciones ha impulsado a la comunidad cient´ıfica a realizar contribuciones en este campo de estudio.

Esquemas de control no lineal para seguimiento de trayectorias para el veh´ıculo terrestre de dos ruedas se presentaron en [1]-[2]. De igual manera en [3] se introdujo un controlador para seguimiento de trayectorias con entradas saturadas. En [4], los autores presentaron un controlador dise˜nado con base la teor´ıa de Lyapunov para estabilizaci´on y seguimiento de trayectorias.

Controladores LQR y PID lineal para un quadrotor, el cual realiza la tarea de regulación, son presentados y validados experimentalmente en [6]. Un control backs- tepping fue mostrado en [5] para seguimiento de trayectorias evaluado en simulación numérica su desempeño. Los autores en [7] desarrollaron un control adaptable pro- bado con resultados en simulaciones numéricas y en experimentos. El controlador tipo PID no lineal para seguimiento de trayectorias fue presentado en [8] obteniendo

(18)

mejores resultados en la comparaci´on experimental con un controlador por modos deslizantes.

Por otro lado, el trabajo presentado por Dong et. al. [11]-[12] se propusieron condiciones necesarias y suficientes para que los sistemas multi-agente con topolog´ıas de interacción cambiantes alcancen formaciones variantes en el tiempo. Esta metodolog´ıa se basa en la solución de la ecuación algebraica de Riccati para determinar las matrices de ganancias del control de formación y en la teor´ıa de estabilidad de Lyapunov para validar teóricamente dicha ley de control.

En [13] presentaron los resultados experimentales del seguimiento de una trayectoria circular de un quadrotor alrededor de un veh´ıculo terrestre el cual sigue una trayectoria lineal. Para ello se propuso un controlador por modos deslizantes para mitigar las perturbaciones ambientales en el quadrotor. En [14], los autores desarrollaron un algoritmo para un sistema constituido por un equipo de robots con ruedas que permite la cobertura de un ´area predefinida. En [15] se introdujo las extensiones de un algoritmo de consenso para sistemas de segundo orden.

La implementación del control de 2 manipuladores aéreos cooperativos, para llevar un objeto desconocido en conjunto (transportación cooperativa aérea segura), cada uno con un brazo robótico de 2 grados de libertad se presentó en [16]. El algoritmo de estimación paramétrica en l´ınea fue diseñado para estimar con ayuda de sensores multi-eje fuerza-(momento), los parámetros f´ısicos desconocidos de la carga en común, tales como masa y momento de inercia. Basándose en un control por modos deslizantes adaptativo, es generada la trayectoria deseada de cada manipulador aéreo para seguimiento de las referencias del correspondiente efector final.

El consenso robusto de múltiples quadrotores para formación de vuelo como una solución para el problema de sistemas multi-agentes se presentó en [17]. Se propuso un algoritmo de control super twisting para las dinámicas traslacional y rotacional de cada agente tal que este algoritmo de control lo conduzca a la superficie deslizante para navegación de cada agente.

En [18] se presentó un novedoso control por modos deslizantes integral para seguimiento de trayectoria de un quadrotor aplicado para resolver el problema de consenso de un sistema multi-agente heterogéneo el cual está constituido por el quadrotor y tres veh´ıculos terrestres de dos ruedas. Los autores en [19] muestran un algoritmo de control para veh´ıculos terrestres de dos ruedas denominado rotar y avanzar basado en el control óptimo LQR, como solución del problema de consenso para un sistema multi-agente constituido por cinco de estos veh´ıculos, sincronizándose no uniforme- mente en el tiempo. En [20] fue investigado el problema de consenso para sistemas multi-agente heterogéneo con topolog´ıas de interacción cambiantes, implementando un control óptimo LQR para regulación de un quadrotor y un algoritmo de control

(19)

para rotar y avanzar basado en LQR para tres veh´ıculos terrestres de dos ruedas.

1.2. Objetivos

1.2.1. Objetivo general

Estudiar y validar te´orica y experimentalmente esquemas de control que garanticen estabilidad a un conjunto de veh´ıculos aut´onomos al realizar tareas coordinadas.

1.2.2. Objetivos espec´ıficos

Revisar el estado del arte en control coordinado de veh´ıculos aut´onomos.

Estudiar el modelo cinem´atico del robot m´ovil con dos ruedas.

Estudiar estrategias de control coordinado de veh´ıculos aut´onomos.

Familiarizarse con las plataformas experimentales.

Realizar simulaciones num´ericas de los controles estudiados.

Realizar experimentos de tiempo real y evaluar los resultados.

1.3. Motivaci´ on

Los veh´ıculos no tripulados como el quadrotor, y los m´oviles terrestres cada vez son utilizados con mayor frecuencia en tareas espec´ıficas como lo son:

Vigilancia y reconocimiento.

Rescate y protecci´on civil.

Trasporte.

Uno de los objetivos principales que se busca en estos dispositivos es que realicen cada una de sus tareas de forma autónoma, efectiva, y con la menor participación de un piloto. El uso de un grupo de estos veh´ıculos trabajando en conjunto incrementa sus capacidades y su funcionalidad, permitiendo as´ı, realizar tareas complejas más allá de la capacidad de un veh´ıculo individual. Para lograr este objetivo es necesario implementar controladores que permitan realizar las tareas asignadas tanto individual como en un grupo coordinado de veh´ıculos. Adicionalmente, la naturaleza sub-actuada y la dinámica no lineal altamente acoplada complican el diseño de di- chos controladores por lo que aún resulta un reto importante desarrollar estrategias altamente eficientes tanto para los veh´ıculos individualmente como para el conjunto.

(20)

1.4. Planteamiento del problema

Considerando un conjunto de veh´ıculos autónomos con distintas caracter´ısticas cinemáticas y dinámicas, se establece la tarea de que todos ellos alcancen una misma posición en el plano horizontal. A esta problemática se le conoce como tarea de consenso de posición de un sistema multi-agente heterogéneo. Para resolver esta asignación existen distintas técnicas que permiten diseñar esquemas de control que analizan el sistema como uno solo, o algunos otros que lo hacen por separado. En este

´

ultimo enfoque es en el que se centra este trabajo en el cual se toma como hipótesis que es posible asegurar que el consenso de posición se cumple cuando los algoritmos de control diseñados para cada uno de los agentes garantizan estabilidad asintótica al realizar la tarea de regulación de posición, y que el grafo de interacción asociado al sistema tiene un árbol de expansión dirigido para cada instante de tiempo.

1.5. Metodolog´ıa

Las actividades en el presente trabajo se distribuyeron y estructuraron de tal manera que cada una contribuyera a cumplir cada uno de los objetivos espec´ıficos, y as´ı llevar a la investigación al cumplimiento del objetivo general. Como primera acti- vidad, se estudió tanto el modelo dinámico del quadrotor, como el modelo cinemático del robot móvil con dos ruedas. Posteriormente, se utilizaron las representaciones matemáticas del robot móvil con dos ruedas y el quadrotor, para realizar simulaciones numéricas donde se implementaron controladores lineales y no lineales. Después, se estudiaron distintos tipos de controladores para realizar la tarea de consenso de un sistema multi-agente. A continuación, por medio de las plataformas, Qball 2 y Qbot se evaluaron experimentalmente los controladores estudiados. Más adelante, se realizaron simulaciones numéricas de la tarea de consenso utilizando los esquemas de control estudiados. Finalmente, de forma experimental se implementaron los controladores en la tarea de consenso del sistema multi-agente constituido por el veh´ıculo terrestre de dos ruedas y el quadrotor y se evaluaron los resultados. De esta manera se cumplió el objetivo general.

1.6. Estructura de la tesis

Este documento se encuentra estructurado como sigue: El cap´ıtulo 2 describe el modelo cinemático del veh´ıculo terrestre de dos ruedas, y dos esquemas de control tomados de la literatura que resuelven el problema de regulación de posición. El primero de ellos es conocido como control de rotar y avanzar mientras que el segundo es un controlador no lineal propuesto por Dixon, Dawson, Zergeroglu y Behal. Los

(21)

controles antes mencionados son analizados y su desempeño es comparado tanto en simulación como experimentalmente. El modelo dinámico del quadrotor, y las leyes de control óptimo LQR y control no lineal con compensación del modelo se abordan en el cap´ıtulo 3. Del mismo modo se presentan los resultados de simulación numérica e implementación experimental. El cap´ıtulo 4 presenta conceptos básicos relaciona- dos con la teor´ıa de grafos que son necesarios para abordar el problema de consenso de sistemas multi-agentes. Un protocolo de consenso es propuesto empleando controladores que garantizan estabilidad asintótica y la eficiencia del método desarrollado es validado tanto teórica como experimentalmente. Finalmente, en el cap´ıtulo 5 se exponen las conclusiones obtenidas de este trabajo de tesis.

(22)

Cap´ıtulo 2

Veh´ıculo terrestre de dos ruedas

Uno de los agentes que conforman el sistema estudiado en este trabajo de tesis es el veh´ıculo terrestre de dos ruedas. Este veh´ıculo consta de tres ruedas, dos de ellas acopladas a un actuador independiente y una tercera que se mueve libremente y sirve como soporte. El desplazamiento en el plano horizontal xy se produce haciendo girar las dos ruedas actuadas a distintas velocidades. En este cap´ıtulo se estudia el comportamiento del veh´ıculo terrestre de dos ruedas aplicando dos controladores que resuelven el problema de regulación de posición, los cuales fueron diseñados considerando el modelo cinemático de dicho veh´ıculo. Esto con el objetivo de que posteriormente los esquemas de control sean aplicados en la solución del problema de consenso.

2.1. Modelo cinem´ atico

El modelo cinemático del veh´ıculo terrestre de dos ruedas, en el cual la posición del centro de rotación del veh´ıculo y el origen del marco de referencia del cuerpo coinciden [1]-[4], está dado por

˙q = S(q)v, (2.1)

donde q(t), ˙q(t) ∈ R³ se definen como

q =x y θ^T , (2.2)

˙q = ˙x ˙y ˙θ^T , (2.3)

además, x(t) y y(t) denotan la posición del centro de masa del veh´ıculo en el marco de referencia inercial y θ(t) representa la orientación del veh´ıculo. En la figura 2.1 se muestran la vista superior del veh´ıculo terrestre junto con las variables que determi- nan su pose. Adicionalmente, ˙x(t) y ˙y(t) expresan las componentes cartesianas de la

(23)

velocidad lineal denotada por V (t) ∈ R, ˙θ(t) representa la velocidad angular tambi´en denotada por W (t). Finalmente, S(q) ∈ R^3×2 es la matriz de transformaci´on definida como

S(q) =





cos(θ) 0 sen(θ) 0

0 1



, (2.4)

y v ∈ R²es el vector de entradas constituido por la velocidad lineal y angular v = V WT

. (2.5)

Figura 2.1: Vista superior del veh´ıculo terrestre de dos ruedas en la que se incluye los marcos de referencia inercial y del cuerpo y las variables que definen su pose.

2.2. Controladores para el veh´ıculo terrestre de dos ruedas

En esta secci´on se presentan dos esquemas de control para el veh´ıculo terrestre de dos ruedas los cuales permiten alcanzar posiciones deseadas en el plano horizontal.

El primero de ellos es un controlador de rotar y avanzar basado en la teor´ıa de control óptimo LQR propuesto en [20] y el segundo es un control de regulación de posición y orientación introducido en [2].

2.2.1. Control rotar y avanzar basado en LQR

El regulador cuadrático lineal (LQR por sus siglas en inglés) es un método bien conocido que proporciona ganancias óptimas al controlador por realimentación de

(24)

estados. Considerando el sistema lineal invariante en el tiempo dado por la ecuaci´on

x = Ax + Bu,˙ (2.6)

donde las matrices A ∈ R^n×n y B ∈ R^n×m son constantes, el control LQR permite obtener las ganancias del control por realimentaci´on de estados denotado por

u(t) = −Kx(t), (2.7)

tal que minimizan la funci´on de costo dada por J =

Z ∞ 0

(x^TQx + u^TRu) dt, (2.8)

donde las matrices Q ∈ R^n×n y R ∈ R^m×m son no negativa y definida positiva, respectivamente. Note que el segundo t´ermino de (2.8) considera el costo de energ´ıa de las se˜nales de control.

Asumiendo que el sistema es completamente controlable, el ´ındice de desempe˜no se minimiza cuando

K = R⁻¹B^TP. (2.9)

siendo la matriz P ∈ R^n×n la soluci´on de

A^TP + AP − P BR⁻¹B^TP + Q = 0. (2.10) La ecuación (2.10) se denomina comúnmente ecuación matricial de Riccati. Basado en la teor´ıa anterior, en [20] se propuso un controlador de posición para veh´ıculos terrestres de dos ruedas bajo la suposición de que el movimiento del veh´ıculo a lo largo de los ejes x , y es independiente. De este modo, la dinámica desacoplada del veh´ıculo está dada por

˙

x₁(t) = A₁x₁(t) + B₁u_x(t), (2.11) y˙₁(t) = A1y₁(t) + B1uy(t), (2.12) donde x₁(t) = [x(t) v_x(t)]^T y y₁(t) = [y(t) v_y(t)]^T, son los estados del sistema, x(t) y y(t) representan la posici´on del veh´ıculo en el plano, v_x(t) y v_y(t) son las velocidades lineales a lo largo del eje x y y, respectivamente, ux(t) y uy(t) ∈ R, son las entradas de control y las matrices A₁ y B₁ est´an dadas por

A₁ =0 1 0 0

, B₁ =0 1

. (2.13)

(25)

Considerando este modelo dinámico es posible desarrollar un esquema de regula- ción de posición del robot móvil en el plano xy basado en técnicas de control óptimo LQR. Para ello se definen los vectores de error como

x˜1 = x1d− x1, (2.14)

y˜₁ = y_1d− y₁, (2.15)

donde x_1d = [x_d v_xd]^T y y_1d = [x_d v_yd]^T son vectores de los estados deseados. Note que como el objetivo de control es regulaci´on de posici´on, las velocidades lineales vx

y v_y son nulas. Por lo tanto, las din´amicas del error de posici´on quedan expresadas como

˙˜x₁(t) = A_e1x˜₁(t) + B_e1u_x(t), (2.16)

˙˜y₁(t) = Ae1y˜₁(t) + Be1uy(t) (2.17) donde

A_e1 =0 1 0 0

, B_e1 = 0

−1

. (2.18)

De este modo, se establece la ley de control ux(t) y uy(t) como

u_x(t) = −K_1xx˜₁(t), (2.19) u_y(t) = −K_1yy˜₁(t), (2.20) donde las ganancias de realimentaci´on K_1x, K_1y ∈ R^1×2 son matrices constantes calculadas de la siguiente manera

K_1x= K_1y= R⁻¹₁ B_e1^TP₁, (2.21) obteniendo P de la soluci´on de la ecuaci´on de Riccati dada en (2.10).

Establecidas las entradas de control para las din´amicas de posici´on es necesario definir como son empleadas en el esquema de control rotar y avanzar. De inicio hay que hacer notar que el algoritmo de control incluye un conjunto de pasos que se realizan de manera secuencial en un intervalo de tiempo [t_k, t_k+ ∆t), donde t_k≥ 0 y

∆t > 0 define la duración. Una vez establecido el valor de ∆t se procede a obtener la dirección de giro del veh´ıculo que hará que el marco de referencia del cuerpo apunte hacia la posición objetivo, posteriormente se comanda una velocidad angular W_c constante hasta que el ángulo formado entre el vector generado con la posición del robot y el punto objetivo y el eje x del marco de referencia en el cuerpo sea menor que un valor . Observe que el robot móvil de dos ruedas no posee entradas de control de fuerza o torque por lo que no es posible aplicar directamente los controles presentados

(26)

en (2.19)-(2.20), por lo tanto se integran dichas señales de entrada obteniendo v_x(t) y v_y(t) para después calcular la velocidad lineal que se aplica al robot usando la siguiente ecuación

V = q

v²_x(t) + v²_y(t). (2.22)

Algoritmo 1 Algoritmo de rotar y avanzar Entrada: x_1d(t_k), y_1d(t_k), x₁(t_k), y₁(t_k), θ(t_k).

Salida: V (t), W (t).

1: si t = t_k entonces

2: θ_T = atan2(y_1d(t_k) − y₁(t_k), x_1d(t_k) − x₁(t_k));

3: si (x_1d(t_k),y_1d(t_k)) est´a en el quadrante I, entonces

4: R_d= 1, M_d = 1, θ_d= θ_T;

5: si no, si (x_1d(t_k),y_1d(t_k)) est´a en el quadrante II, entonces

6: R_d= −1, M_d = −1, θ_d= θ_T − π;

7: si no, si (x_1d(t_k),y_1d(t_k)) est´a en el quadrante III, entonces

8: R_d= 1, M_d = −1, θ_d= θ_T;

9: si no, si (x_1d(t_k),y_1d(t_k)) est´a en el quadrante IV, entonces

10: R_d= −1, M_d = 1, θ_d= θ_T + π;

11: fin si

12: fin si

13: mientras t_k≤ t ≤ t_k+ ∆t hacer

14: si |θ_d(t_k) − θ(t)| > entonces

15: W (t) = R_dW_c;

16: si no

17: V (t) =q

v²_x(t) + v²_y(t);

18: V (t) = M_dV (t);

19: fin si

20: fin mientras

Finalmente, al alcanzar el instante de tiempo t = t_k + ∆t se asigna el tiempo actual a t_k y se repite el proceso. En el algoritmo 1 se presenta este metodolog´ıa de manera detallada. En el resto de este documento este controlador ser´a identificado como CVTR1, acr´onimo de Control del Veh´ıculo Terrestre con Ruedas 1.

2.2.2. Control de Dixon, Dawson, Zergeroglu y Behal

El segundo esquema de control para el veh´ıculo terrestre de dos ruedas que se estudi´o en este trabajo de tesis fue el propuesto por Dixon y otros en [2]. El objetivo de control consiste en obtener las entradas de velocidad lineal V (t) y angular W (t)

(27)

tales que los errores de posici´on ˜x(t), ˜y(t), y de orientaci´on ˜θ(t) definidos como

˜

x = x_d− x (2.23)

˜

y = yd− y (2.24)

θ˜ = θ_d− θ (2.25)

tiendan a cero cuando el tiempo tiende a infinito.

El dise˜no del controlador se realiza sobre el error expresado en el marco de referencia del veh´ıculo el cual se obtiene al aplicar la siguiente transformaci´on lineal



 e₁ e₂ e3



=





cos(θ) sen(θ) 0

− sen(θ) cos(θ) 0

0 0 1









˜ x

˜ y θ˜



. (2.26)

De este modo, la din´amica del error expresado en el marco de referencia del cuerpo se obtiene calculando la derivada de (2.26) y usando el modelo cinem´atico en (2.1).

As´ı, la din´amica del error en lazo abierto se expresa como





˙e₁

˙e₂

˙e₃



=





−V + W e₂

−W e₁

−W



. (2.27)

Para esta din´amica los autores propusieron la siguiente ley de control

V W

=

k₁e₁ k₂e₃+ e²₂sen(t)

, (2.28)

donde k₁, k₂ ∈ R son ganancias constantes positivas. Sustituyendo (2.28) en (2.27) se obtiene la din´amica del error en lazo cerrado dada por





˙e₁

˙e₂

˙e₃



=





−k₁e₁+ W e₂

−W e₁

−k₂e₃+ e²₂sen(t)



. (2.29)

La prueba de estabilidad presentada por los autores garantiza que el vector de errores expresados en el marco de referencia del veh´ıculo converge a cero de manera asint´otica globalmente, y como consecuencia tambi´en lo hacen los errores de pose

˜

x(t), ˜y(t) y ˜θ(t). En el resto de este documento este controlador ser´a denotado como CVTR2.

(28)

2.3. Resultados de control para regulaci´ on

2.3.1. Resultados de simulaci´ on num´ erica

Con el objetivo de estudiar y validar los esquemas CVTR1 y CVTR2 se realizaron simulaciones numéricas. Para tal estudio se empleó el modelo cinemático presentado en (2.1) y se programó el algoritmo de control en Matlab-Simulink.

El tiempo que determina el intervalo de actualización ∆t se definió en 5 [s], y las matrices de pesos del control CVTR1 se establecieron en Q = diag{1, 0} y R = 1, para ambas dinámicas de posición. As´ı, las ganancias obtenidas para K_1x y K_1y fueron

K1x= K1y = −1 −1.4142 . (2.30) Por otra parte, las ganancias del controlador CVTR2 se eligieron como sigue:

k₁ = 2.2, k₂ = 1. (2.31)

La orientaci´on deseada θ_d para el controlador CVTR2 es calculada mediante los pasos 1-15 del algoritmo 1.

La duración de la simulación fue de 30 segundos y las condiciones iniciales de posición y velocidad del veh´ıculo se definieron en x(0) = 0 [m], y(0) = 0 [m] y θ(0) = 0 [rad]. Finalmente, los valores deseados de posición se eligieron como

x_d(t) = 0.75 [m], y_d(t) = 0.75 [m]. (2.32) Los resultados obtenidos se muestran en las figuras 2.2-2.4. En el lado derecho de la figura 2.2 se presenta las gráficas con las trayectorias descritas por el veh´ıculo terrestre de dos ruedas en x(t) y y(t) as´ı como el valor de referencia y de lado iz- quierdo se presenta las gráficas del error de posición ˜x(t) y ˜y(t) obtenidas durante la simulación aplicando ambos controladores. Se presentan las señales de orienta- ción θ(t) y error de orientación ˜θ(t) obtenidos en la figura 2.3. Note que en ambos controladores el valor deseado de orientación se genera en tiempo de ejecución de- pendiendo de la posición inicial del veh´ıculo y la referencia deseada. Por otra parte, las señales de velocidad lineal V (t) y angular W (t) aplicadas con los controladores CVTR1 y CVTR2 se muestran en la figura 2.4. En estos resultados se puede observar como el veh´ıculo alcanza la posición deseada empleando un sólo intervalo de actualización con el control CVTR1, esto debido a que la simulación no considera los efectos dinámicos del sistema incluidos la inercia, de modo que el veh´ıculo se detiene instantáneamente al dejar de aplicar la entrada de velocidad angular. Esto permite que que el valor de velocidad angula W se pueda seleccionar tan grande como se desee mientras que el valor del parámetro se puede seleccionar tan pequeño como

(29)

se quiera. En particular para esta simulaci´on se seleccion´o W = 0.5 y = 0.005 [rad].

Por otra parte, se observa que al emplear el control CVTR2 la orientación converge al valor deseado mientras la posición se mantiene cercana al valor de referencia con errores menores a 0.1 [m] después de 25 [s] de simulación.

0 10 20 30

0 0.5 1 1.5

(a)

Tiempo [s]

x(t)[m]

0 10 20 30

−0.5 0 0.5

(b)

t [s]

˜x(t)[m]

0 10 20 30

0 0.5 1 1.5

(c)

t [s]

y(t)[m]

0 10 20 30

−0.5 0 0.5

(d)

t [s]

˜y(t)[m]

CVTR1 CVTR2

xd CVTR1

CVTR2

CVTR1 CVTR2 yd

CVTR1 CVTR2

Figura 2.2: Resultados de simulación: Evolución temporal de la posición x(t) y y(t) y el error de posición ˜x(t) y ˜y(t) empleando los controladores CVTR1 (l´ınea roja) y CVTR2 (l´ınea azul) en la tarea de regulación.

2.3.2. Resultados experimentales

Con el fin de comparar el desempeño de los dos esquemas de control estudiados se realizaron experimentos en tiempo real con la plataforma Qbot de Quanser, el cual se integró con el sistema de visión Optitrack para realizar la realimentación visual de la posición en el espacio tridimensional.

El veh´ıculo Qbot de Quanser es un sistema robótico autónomo terrestre. Este dispositivo está constituido de una plataforma Yujn Robot Kobuki, una cámara Kinect RGB de Microsoft y un sensor de profundidad. El sistema informático integrado en el veh´ıculo usa una computadora Gumstix DuoVero para ejecutar el software de control en tiempo real. La programación de las estrategias de control en esta plataforma se realiza en Matlab-Simulink, en la computadora de mando, y posteriormente con las bibliotecas QUARC de Quanser, estos modelos son compilados y descargados en la computadora embebida Gumstix del veh´ıculo. Por otra parte, el sistema de

(30)

0 10 20 30

−1

−0.5 0 0.5 1

(a)

t [s]

θ(t)[rad]

0 10 20 30

−1 0 1

(b)

t [s]

˜ θ(

t)[rad]

0 10 20 30

−2

−1 0 1 2

(c)

t [s]

θ(t)[m]

0 10 20 30

−2

−1 0 1 2

(d)

t [s]

˜ θ(t)[rad]

CVTR1

θ_d CVTR1

CVTR2

θ_d CVTR2

Figura 2.3: Resultados de simulación: Evolución temporal de la orientación θ(t) y error de orientación ˜θ(t) empleando los controladores CVTR1 (l´ınea roja) y CVTR2 (l´ınea azul) en la tarea de regulación.

0 10 20 30

−1 0 1

(a)

t[s]

V(t)[m s]

0 10 20 30

−5 0 5

(b)

t[s]

W(t)[rad s]

CVTR1

CVTR2 CVTR1

CVTR2

Figura 2.4: Resultados de simulaci´on: Velocidad lineal V (t) y angular W (t) obtenidas con los controladores CVTR1 (l´ınea roja) y CVTR2 (l´ınea azul) durante la tarea de regulaci´on.

visión Optitrack permite estimar la posición y orientación de distintos objetos en el espacio tridimensional. Este sistema consta de un arreglo de 6 cámaras modelo

“flex 3 ” conectadas a una computadora de mando, la cual procesa las imágenes en el software “Motive” y proporciona estimados de pose de los objetos de interés. Para determinar la pose de un objeto como m´ınimo deben colocarse 3 marcadores que reflejan la luz ultravioleta emitida por las cámaras. Finalmente, la interacción entre

(31)

la computadora y el robot se realiza a través de una comunicación inalámbrica v´ıa ethernet. En la figura 2.5 se muestra un diagrama del sistema de prueba.

Figura 2.5: Configuraci´on del sistema experimental para el control del veh´ıculo terrestre de dos ruedas.

La tarea establecida para el veh´ıculo terrestre consistió en ir de un punto inicial a una posición final para lo cual se definió el tiempo del experimento en 30 segundos.

Note que el control rotar y avanzar (CVTR1) requiere actualizar el ángulo deseado θ_den cada intervalo de tiempo definido por ∆t, por lo tanto para la implementación se uso un valor 5 segundos para este parámetro. De manera similar, el controlador de Dixon y otros (CVTR2) también se implementó actualizando el valor deseado del

´

angulo de la misma manera que en el algoritmo 1 de rotar y avanzar lo hace y con la misma frecuencia.

Las ganancias del controlador rotar y avanzar se establecieron con los valores usados en la simulaci´on mientras que en el caso del control de Dixon y otros se obtu- vieron a partir de un proceso de prueba y error dando como resultado los siguientes valores

k₁ = 4.5, k₂ = 2.5. (2.33)

Los resultados obtenidos se muestran en las gráficas de las figuras 2.6- 2.8. En la figuras 2.6 se presenta la evolución temporal de la pose del robot móvil as´ı como el valor de referencia establecido. De igual forma se presentan las señales de error

(32)

obtenidas con ambos controladores. El comportamiento de la orientación del veh´ıculo terrestre se muestra en la figura 2.7. Adicionalmente, en la figura 2.8 se proporcionan las entradas de control entregadas por los controles. Como se puede observar, el control CVTR2 proporciona un mejor desempeño en la tarea asignada, esto debido a que el control de CVTR1 emplea una estrategia precaria para regular la orientación.

0 10 20 30

0 0.5 1 1.5

(a)

Tiempo [s]

x(t)[m]

0 10 20 30

−0.5 0 0.5

(b)

t [s]

˜x(t)[m]

0 10 20 30

0 0.5 1 1.5

(c)

t [s]

y(t)[m]

0 10 20 30

−0.5 0 0.5

(d)

t [s]

˜y(t)[m]

CVTR1 CVTR2 xd

CVTR1 CVTR2

CVTR1 CVTR2 yd

CVTR1 CVTR2

Figura 2.6: Resultados de experimento en tiempo real: Evolución temporal de la posición x(t) y y(t) y el error de posición ˜x(t) y ˜y(t) empleando los controladores CVTR1 (l´ınea roja) y CVTR2 (l´ınea azul) en la tarea de regulación.

Por último, con el objetivo de obtener una métrica cuantitativa del desempeño de los controladores se calcularon valores RMS (Root Mean Square, por sus siglas en inglés) de las señales del error de posición ˜x(t) y ˜y(t) que resultaron de los experimentos en el intervalo de tiempo dado por 20 [s] ≤ t ≤ 30[s]. En la tabla 2.1 se puede observar que el controlador CVTR2 presenta los mejores resultados en cuanto a la disminución del error, ya que muestra los valores más pequeños, los cuales han sido resaltados en negritas.

(33)

0 10 20 30 0

0.5 1 1.5 2

(a)

t [s]

θ(t)[rad]

0 10 20 30

−1

−0.5 0 0.5 1

(b)

t [s]

˜ θ(t)[rad]

0 10 20 30

−2 0 2

(c)

t [s]

θ(t)[m]

0 10 20 30

−2

−1 0 1 2

(d)

t [s]

˜ θ(t)[rad]

CVTR1

θd CVTR1

CVTR2

θ_d CVTR2

Figura 2.7: Resultados de experimento en tiempo real: Evolución temporal de la orientación θ(t) y error de orientación ˜θ(t) empleando los controladores CVTR1 (l´ınea roja) y CVTR2 (l´ınea azul) en la tarea de regulación.

0 10 20 30

−0.5 0 0.5

(a)

t[s]

V(t)[m s]

0 10 20 30

−5 0 5

(b)

t[s]

W(t)[rad s]

CVTR1 CVTR2 CVTR1 CVTR2

Figura 2.8: Resultados de experimento en tiempo real: Velocidad lineal V (t) y angular W (t) obtenidas con los controladores CVTR1 (l´ınea roja) y CVTR2 (l´ınea azul) durante la tarea de regulaci´on.

CVTR1 CVTR2

˜

x 0.0118 0.0087

˜

y 0.0314 0.0138

Tabla 2.1: Valores RMS de los errores de postura del Qbot en el experimento de regulaci´on para la comparaci´on de los controladores CVTR1 y CVTR2. Estos valores fueron calculados para 20 [s] ≤ t ≤ 30 [s].

(34)

Cap´ıtulo 3 Quadrotor

El segundo tipo de veh´ıculo autónomo que se ha considerado en este trabajo para formar parte del sistema multi-agente heterogéneo es el quadrotor. Él quadrotor es un dispositivo el cual por medio cuatro rotores puede moverse en el espacio tridimensional. Este es un sistema sub-actuado, posee únicamente cuatro actuadores para controlar seis grados de libertad, lo cual agrega complejidad el diseñar leyes de control eficientes que permitan realizar tareas de regulación as´ı como de seguimiento de trayectoria. En este cap´ıtulo se estudia el desempeño de dos estrategias de control aplicadas al quadrotor, las cuales garantizan el cumplimiento de la tarea de regulación de posición. Además, uno de los esquemas es una propuesta original que garantiza seguimiento de trayectorias.

3.1. Modelo din´ amico

El modelo dinámico del quadrotor que se presenta en este trabajo se obtiene considerado la aeronave como un cuerpo r´ıgido que se mueve en un espacio tridimensional, [7]-[8], [22]-[24]. Este volumen se somete una fuerza de empuje generada por los rotores dando lugar al desplazamiento del quadrotor en el espacio, as´ı como también los momentos de fuerza que producen los movimientos de cabeceo, balanceo y guiñada.

La figura 3.1 muestra la representación de los marcos de referencia y los movimientos que puede realizar el quadrotor. Las ecuaciones de movimiento que representan la dinámica del quadrotor están dadas por

m¨p + mge_z = R(η)e_zu_T, (3.1) H(η)¨η + C(η, ˙η) ˙η = W (η)^Tτ , (3.2) donde

(35)

Figura 3.1: Representaci´on del quadrotor incluyendo el marco de referencia inercial I y el marco de referencia del cuerpo del veh´ıculo B.

p = [x y z]^T es el vector de posici´on del centro de masa del veh´ıculo respecto al marco de referencia inercial.

η = [φ θ ψ]^T es la representación de los ángulos de Euler definidos por el balanceo φ, que es el giro sobre el eje x, cabeceo θ que el giro sobre el eje y, y finalmente la guiñada ψ que es el giro realizado sobre el eje z.

H(η) = W (η)^−TIoW (η)⁻¹, es la matriz de inercia rotacional,

C(η, ˙η) = W (η)^−T[S(W )I_o−I_oW (η)⁻¹W (η)]W (η)˙ ⁻¹, es la matriz de Coriolis la cual contiene los elementos centr´ıfugos y girosc´opicos del sistema.

m es la masa del quadrotor, g es la constante gravitacional.

ez es el vector unitario que apunta en direcci´on del eje z, en el marco de referencia inercial.

u_T es el empuje total generado por los rotores.

τ = [τ_φ τ_θ τ_ψ]^T es el vector de momentos de gui˜nada, balanceo y cabeceo expresado en el marco de referencia del cuerpo.

R(η) es una matriz de rotaci´on que relaciona el marco de referencia inercial con el cuerpo y que est´a dada por

R(η) =





C_θS_ψ S_θC_ψS_φ− S_ψC_φ S_θC_ψC_φ+ S_ψS_φ C_θC_ψ S_θS_ψS_φ+ C_ψC_φ S_θS_ψC_φ− C_ψS_φ

−S_φ C_θS_φ C_θC_φ



.

(36)

I_o representa el tensor de inercia.

W (η) es una matriz de transformaci´on que relaciona la velocidad angular con la raz´on de cambio del vector η la cual se define como

W (η) =





1 Sφtθ CφTθ

0 C_φ −S_φ

0 S_φ/C_θ C_φ/C_θ



,

donde Sx, Cx y Tx representan las funciones sen(x), cos(x) y tan(x), respectivamente.

Las din´amicas del quadrotor (3.1) y (3.2) satisfacen las siguientes propiedades, Propiedad 1 La La matriz H(η) es sim´etrica y positiva definida,

H(η) = H^T(η),

λ_max{H(η)}kxk² > x^TH(η)x > λ_min{H(η)}kxk² > 0, (3.3) para todo x, η ∈ R³, x 6= 0, donde dada una matriz M ∈ R^n×n, λ_min{M } y λ_max{M } representan el m´ınimo y m´aximo eigenvalor de la matriz, respectivamente.

Propiedad 2 La matriz ¹₂H(η) − C(η, ˙˙ η) es una matriz anti-sim´etrica, es decir, x^T 1

2

H(η) − C(η, ˙˙ η)

x = 0, ∀x, η, ˙η ∈ R³. (3.4)

3.2. Control LQR

En esta sección se presenta el controlador LQR diseñado para tarea de regulación de posición de un quadrotor, el cual fue presentado en [20]. El esquema de control es representado mediante diagrama de bloques en la figura 3.2. El diseño de la ley de control se realiza sobre un versión simplificada de la dinámica del veh´ıculo presentada en (3.1)-(3.2) la cual es el resultado de considerar las siguientes suposiciones:

Los ángulos φ, θ y ψ son pequeños durante el vuelo as´ı que la orientación de los marcos de referencia inercial y del veh´ıculo coincide.

El quadrotor es sim´etrico en los tres ejes, por lo cual la matriz de inercia I_o est´a dada por I_o = diag{I_xx, I_yy, I_zz}.

Las fuerzas centr´ıfugas y centr´ıpetas son peque˜nas y por lo tanto se omiten.

El empuje de cada rotor est´a representado por la din´amica de primer orden siguiente

f_i = K_wv = K_w ω s + ωu_i,

(37)

Figura 3.2: Diagrama de bloques de la implementaci´on del controlador No lineal con compensaci´on del modelo.

donde u_i es la entrada PWM del actuador, ω denota el ancho de banda del actuador y Kw es una ganancia positiva.

Adicionalmente, para el dise˜no del controlador se definen cuatro entradas de control para el sistema denotadas por u_th, u_φ, u_θ y u_ψ que mantienen la siguiente relaci´on con las entradas del sistema

uT = 4K_w_s+ω^ω u_th, τ_φ= 2K_wl_s+ω^ω u_φ,

τ_θ = 2K_wl_s+ω^ω u_θ, τ_ψ = K_nu_ψ, (3.5) donde K_n es una ganancia positiva y l es la distancia entre el centro de masa del veh´ıculo y el rotor.

Considerando las suposiciones antes mencionadas la din´amica del quadrotor en los ejes x y y se linealiza como sigue

x˙2(t) = Ax2x2(t) + B2uθ(t), (3.6)

˙

y₂(t) = A_y2y₂(t) + B₂u_φ(t), (3.7) las matrices de estado A_x2, A_y2 ∈ R^5×5 est´an dadas por

A_x2=







0 1 0 0 0

0 0 g 0 0

0 0 0 1 0

0 0 0 0 ^2K_I^w^l

xx

0 0 0 0 −w







, A_y2=







0 1 0 0 0

0 0 −g 0 0

0 0 0 1 0

0 0 0 0 ^2K_I^w^l

xx

0 0 0 0 −w







, (3.8)

la matriz de entrada B₂ ∈ R^5×1 se define como B₂ = [0 0 0 0 w]^T, y los vectores de estado x2, y₂ ∈ R⁵ se expresan como x2(t) = [x(t) ˙x(t) θ(t) ˙θ(t) p(t)]^T y y₂(t) =

(38)

[y(t) ˙y(t) φ(t) ˙φ(t) q(t)]^T . Note que los pares (A_x2, B_x2) y (A_y2, B_y2) son ambos controlables.

Por otra parte, la din´amica de altura del quadrotor afectada por el empuje total y por la gravedad est´a dada por

m¨z =

4

X

i=1

f_icos(θ) cos(φ) − mg. (3.9)

Considerando (3.6) y (3.7) es posible desarrollar un esquema de regulación de posición del quadrotor en el plano xy basado en técnicas de control óptimo LQR.

Considerando los vectores de estado deseados x_2d = [x_d ˙x_d φ_d φ˙_d 0]^T y y_2d = [y_d y˙_d θ_d θ˙_d 0]^T donde las posiciones deseadas x_d y y_d son constantes y los ´angulos de orientaci´on φ_d y θ_d son nulos, los vectores de error de estado se definen como

˜

x_x =x_2d− x₂, (3.10)

˜

x_y =y_2d− y₂. (3.11)

Note que como el objetivo de control es regulación de posición, los estados ˙x₂ y ˙y₂ son nulos. Por lo tanto las dinámicas del error de posición quedan expresadas como

˙˜x₂(t) = Aex2x˜2(t) + Be2ux(t), (3.12)

˙˜y₂(t) = A_ey2y˜₂(t) + B_e2u_y(t) (3.13) donde

A_ex2 =







0 1 0 0 0

0 0 g 0 0

0 0 0 1 0

0 0 0 0 ^2K_I ^w^l

xx

0 0 0 0 w







, A_ey2 =







0 1 0 0 0

0 0 −g 0 0

0 0 0 1 0

0 0 0 0 ^2K_I^w^l

xx

0 0 0 0 w







. (3.14)

B_e2 = [0 0 0 0 − w]^T, y los vectores de estado ˜x₂(t) = [˜x₂(t) ˙˜x₂(t) ˜θ₂(t)θ˙˜₂(t) − p(t)]^T y ˜y₂(t) = [˜y₂(t) ˙˜y₂(t) ˜φ₂(t)φ˙˜₂(t) − q(t)]^T. As´ı las entradas de control en el plano xy para el sistema se establecen como

u_θ(t) = −K_x2x˜₂(t), (3.15)

u_φ(t) = −K_y2y˜₂(t), (3.16)

donde las ganancias de realimentaci´on K_x2, K_y2 ∈ R^1×5 son matrices constantes calculadas como

K_x2 = R⁻¹B_e2^TP_x2_˜ , (3.17) K_y2 = R⁻¹B_e2^TP_y2_˜ , (3.18) (3.19)