Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots

Curso 2009 - 2010 PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 1 A todos los que desearon ver este proyecto finalizado. Gracias. PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 2 Índice 1Introducción ______________________________________________________ 3 1.1Descripción de los objetivos del proyecto _________________________________ 3 1.2Herramientas utilizadas _______________________________________________ 4 1.2.1Webots _________________________________________________________________ 4 1.2.2Otras ___________________________________________________________________ 4 1.3Contenido de la memoria ______________________________________________ 6 2Webots, herramienta de diseño y simulación ____________________________ 7 2.1VRML ______________________________________________________________ 8 2.2Scene Tree __________________________________________________________ 9 2.3Controladores ______________________________________________________ 10 2.4La ventana Log _____________________________________________________ 12 3Entorno de simulación _____________________________________________ 13 3.1Descripción del entorno ______________________________________________ 13 3.2Diseño de los diferentes elementos 3D __________________________________ 14 3.2.1Diseño del mundo _______________________________________________________ 14 3.2.2Diseño del robot volador __________________________________________________ 15 3.3Diseño del controlador _______________________________________________ 18 3.3.1Entrada de datos ________________________________________________________ 18 3.3.2El movimiento del robot __________________________________________________ 19 3.3.3Detección de colisiones ___________________________________________________ 24 3.3.4Evitación de obstáculos ___________________________________________________ 28 4Manual de usuario ________________________________________________ 31 4.1Requisitos _________________________________________________________ 31 4.2Modificaciones del entorno ___________________________________________ 32 4.2.1Añadir un elemento ______________________________________________________ 32 4.2.1Modificar y eliminar un elemento ___________________________________________ 33 4.3Modificar el controlador ______________________________________________ 35 4.3.1Cambiar de controlador ___________________________________________________ 35 4.3.2Añadir un elemento ______________________________________________________ 35 4.3.3Modificar y eliminar un elemento ___________________________________________ 36 4.4Manejo manual del robot _____________________________________________ 37 5Conclusiones _____________________________________________________ 38 5.1Posibles ampliaciones futuras _________________________________________ 39 Tabla de ilustraciones ________________________________________________ 40 Bibliografía ________________________________________________________ 41 Apéndice A: Contenido del CD _________________________________________ 42 Apéndice B: Función de detección de colisiones ___________________________ 43 Apéndice C: Código de evitación de colisiones _____________________________ 50 PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 3 1Introducción 1.1Descripción de los objetivos del proyecto La finalidad de este proyecto final de carrera es conseguir simular el comportamiento de un robot volador utilizando el software comercial de Cyberbotics, Webots. Concretamente su versión 5.1.0. Como robot volador se escogió una especie de globo dirigible, salvo que no lleva timón, de tamaño reducido. Como veremos más adelante, todos los movimientos del robot se controlan mediante hélices. Este zepelín es un robot construído en la École Polytechnique Fédérale de Lausanne (EPFL). Existe un modelo virtual bajo licencia GPL2 o superior para Webots que incluye componentes físicas y que es el punto de partida de este proyecto. Teniendo como base dicho modelo virtual, que tiene en cuenta factores necesarios como el rozamiento o la inercia, vamos a construir un entorno por el que el globo dirigible deberá volar de forma autónoma. El vuelo se realizará siguiendo un circuito fijado de antemano, y terminará en un espacio destinado para el reposo del robot. Conseguir que el robot se desplace por los puntos deseados permitirá un mayor control y por consiguiente, las aplicaciones futuras serán muy interesantes (vigilancia forestal, fotografía aérea, transporte aéreo automático, etc.). A lo largo de todo el recorrido que el robot debe realizar en el escenario creado para la simulación, también se añadirán obstáculos. Una parte muy importante de este proyecto va a ser que el globo deberá evitar la colisión con cualquier tipo de obstáculo fijo. Para así conseguir realmente un robot volador autónomo, que no precise de ningún tipo de interacción externa para realizar su cometido. La posición de los obstáculos será conocida en todo momento por el robot y éste deberá ser capaz de evitar todos aquellos que se encuentren en su trayectoria. Por lo tanto necesitamos crear un mundo virtual en Webots que contenga obstáculos. Además se introducirá el modelo del globo dirigible en este mundo. Y con todo esto se trabajará en el controlador del robot que actuará sobre los motores para manejarlo de forma automática. PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 4 1.2Herramientas utilizadas Como ya se ha comentado, el programa principal con el que se va a trabajar es la versión 5.1.0 de Webots. Pero a parte de este software, también se ha precisado el uso de diferentes programas. La mayoría de ellos habituales en el ámbito científico (Matlab, Mathematica, Autodesk Map 5…) y otros más utilizados en cualquier otro ámbito (Bloc de notas, Excel, etc.). El equipo utilizado siempre ha sido un AMD Athlon 64 Processor 3400+ a 2,4GHz y con 512 MB de memoria RAM. Y el sistema operativo sobre el que hemos ejecutado la principal herramienta de trabajo, Webots, ha sido Microsoft Windows XP Profesional SP3. 1.2.1Webots Webots es un entorno de desarrollo usado para modelar, programar y simular robots móviles. Con Webots el usuario puede diseñar complejas configuraciones de robots, con uno o más robots, iguales o diferentes, en un entorno compartido. Las propiedades de cada objeto, como forma, color, textura, masa, fricción, etc., las elije el usuario. Para equipar cada robot, se dispone de un amplio abanico de sensores y actuadores simulados. Además, los controladores de los robots pueden ser programados con el propio entorno de desarrollo integrado (IDE) o con entornos de desarrollo de terceros. El comportamiento del robot puede ser testeado en mundos con física realística. Y los programas de los controladores se pueden transferir a robots reales existentes en el mercado. Webots es usado en cerca de 700 universidades y centros de investigación alrededor de todo el mundo. Ha sido desarrollado por el Swiss Federal Institute of Technology en Lausana. Testeado, documentado y mantenido durante más de 10 años. El tiempo de desarrollo que logra ahorrar puede ser muy elevado. Este software tiene versiones para Mac OS X, Linux y Windows. Y como ya hemos comentado para la realización de este PFC hemos utilizado la versión 5.1.0 de Webots que corre bajo el sistema operativo Windows XP. En esta versión la interfaz gráfica se divide en 4 pantallas que podemos modificar, ver u ocultar a nuestro gusto. Pero estos aspectos los trataremos más adelante. 1.2.2Otras Aunque la parte más importante del desarrollo de este proyecto se ha realizado con Webots, como en cualquier otra empresa científica ha sido necesaria la utilización de otras herramientas de trabajo menos especializadas pero no por ello prescindibles. Quizá las más útiles hayan sido tanto Matlab como Mathematica, ambas herramientas han permitido realizar de forma manual los cálculos complejos que lleva a cabo el programa de forma automática. Así pues, cuando ha sido necesario corroborar los resultados obtenidos por el programa, se han utilizado estas herramientas para comprobar que los resultados eran correctos. PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 5 Como en muchas ocasiones los resultados obtenidos eran farragosos y difíciles de visualizar a simple vista, ha sido realmente útil en este aspecto disponer de una herramienta de dibujo en 3D. Autodesk Map 5 ha sido el programa instalado en el ordenador que nos ha permitido realizar este tipo de dibujos de forma sencilla. De esta manera, datos complejos como posiciones de esferas en el espacio tridimensional, trayectorias, etc., resultaban mucho más sencillos de tratar cuando los mostrábamos dibujados con esta herramienta. A parte de estos programas, también se han utilizado otros mucho más extendidos en su uso como pueden ser Microsoft Office Excel o el Bloc de notas del propio sistema operativo. El primero de ellos ha sido especialmente útil para dibujar gráficas con los datos obtenidos del programa y así poder ajustar parámetros de una forma rápida y eficaz. El bloc de notas ha facilitado la programación con su función de reemplazar texto. PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 6 1.3Contenido de la memoria En esta memoria se explica paso a paso el desarrollo del proyecto final de carrera. Y para ello se ha dividido el contenido en 5 apartados principales, intentando así que la información quede lo más ordenada posible y sea fácil de seguir. El primer apartado es una introducción que habla del objetivo del proyecto y las herramientas que han sido necesarias para realizarlo. El punto actual se incluye dentro de dicho apartado dedicado a la introducción. En un segundo apartado hablaremos de la principal herramienta utilizada, el software de Cyberbotics Webots 5.1.0. Hablaremos de las características de esta versión, sobre todo de cómo se divide su interfaz gráfica y lo más importante, cómo la hemos organizado en nuestro proyecto. El punto más importante será el que contempla la tercera parte de esta memoria. Vamos a describir el entorno de simulación. Qué es lo que necesitamos, cómo es el mundo que hemos creado para nuestras simulaciones, cómo es el robot que vamos a programar y cómo se ha diseñado todo esto. Además de la parte visual, hablaremos del controlador que maneja a nuestro robot volador. Describiremos cómo ha de ser la entrada de datos, cómo se ha logrado el movimiento del robot, la detección de obstáculos y la forma en que se evitan los mismos. Después de haber explicado todo el trabajo realizado, el siguiente apartado está enfocado al uso posterior de este proyecto. Pues va a intentar dar toda la información posible para que un futuro usuario o programador pueda aprender a utilizar este proyecto. Se expondrán los requisitos para utilizar el proyecto, la forma en que se pueden modificar aspectos del entorno como la entrada de datos, y la forma de modificar el controlador del globo dirigible. Además, una breve explicación de cómo manejar el zepelín de forma manual. Por último, se indicarán una serie de conclusiones acompañadas de posibles ámbitos de aplicación de este proyecto y también posibles mejoras o ampliaciones que se podrían implementar en un futuro. PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 7 2Webots, herramienta de diseño y simulación Este proyecto utiliza Webots (http://www.cyberbotics.com), un software comercial de simulación de robots móviles desarrollado por Cyberbotics Ltd. Aunque a fecha de hoy la versión 6.0 de Webots ha cambiado su interfaz gráfica, la licencia que se tenía para trabajar con Webots era de su versión 5.1.0. Por lo tanto todo lo relacionado con Webots a partir de este punto será haciendo referencia a esta versión. Cuando ejecutamos el programa por primera vez, debemos establecer cuál queremos que sea el directorio de trabajo. Debemos elegir una carpeta donde tengamos privilegios y él nos creará una serie de carpetas que contienen archivos necesarios para el funcionamiento de nuestro proyecto. Estas carpetas son las siguientes: controllers, data, objects, plugins, worlds, transfer, resources, lib, includes y doc. De todas ellas, vamos a prestar especial atención a controllers y worlds. Por la sencilla razón de que cuando creemos un mundo nuevo, se guardará en la carpeta worlds. Y cuando creemos un controlador nuevo para nuestro robot, se guardará en la carpeta controllers. Figura 1: Directorio de trabajo Webots 5.1.0 Como ya hemos comentado antes en esta memoria, la interfaz de Webots se divide en 4 ventanas que podemos disponer donde nos plazca, además de modificar su tamaño de forma independiente. A continuación vamos a tratar de explicar la función de cada una de ellas y cómo la hemos utilizado para este proyecto en concreto. PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 8 2.1VRML Virtual Reality Modeling Language (VRML) es el lenguaje de programación que se utiliza en Webots para modelar el mundo virtual donde se simulará el comportamiento de los robots. Aunque cabe decir que no todos los nodos y características de este lenguaje están disponibles. Este lenguaje posibilita la descripción de objetos 3D a partir de prototipos basados en formas geométricas básicas o estructuras especificadas a partir de vértices y aristas. Además se pueden añadir características a los objetos como color o materiales. Como he comentado, Webots no incluye todos los nodos de VRML97, pero sí que añade algunos muy importantes para trabajar con simulación de robots. Por ejemplo tenemos el nodo CustomRobot, que es el que hemos utilizado nosotros para crear nuestro robot volador. Este nodo nos va a permitir modelar un robot a base de utilizar otros nodos y además añadirle aspectos especiales. Por ejemplo, al tratarse de un robot, vamos a poder asociarle un controlador. Pero este tema lo veremos más adelante en un apartado específico. Nodos que aporta Webots y que podemos utilizar son el nodo DifferentialWheels para crear robots terrestres con ruedas. O el nodo Servo, que nos va a permitir modelar robots con servos y simular el comportamiento de los mismos. Con el nodo Camera podemos añadir una cámara virtual y modificar sus características de modo que obtendríamos una imagen virtual de lo que sería la imagen real si montáramos una cámara en nuestro robot. Podemos añadir sensores de distancia con el nodo DistanceSensor, e incluso un receptor GPS añadiendo el nodo GPS al robot. Si quisiéramos establecer algún tipo de comunicación, podemos incluir en la simulación los nodos Emitter y Receiver. Además de otros nodos como pueden ser Charger, Gripper, Joint, LED, Light Sensor, Physics o TouchSensor, tenemos el nodo Supervisor. Este nodo se puede utilizar para controlar el mundo y todos los robots que éste contiene. Nuestro mundo sólo va a contener un robot del tipo CustomRobot. Y aunque podríamos añadir las físicas que proporciona Webots, las vamos a programar en el controlador del propio robot para poderlas personalizar un poco más. No obstante sí que vamos a utilizar una característica de los nodos Solid muy interesante que es el atributo boundingObject. Se trata de envolver a los objetos sólidos que forman el mundo con una forma básica que nos va a permitir calcular colisiones entre objetos. De esta manera, añadiremos un nivel más de realismo a nuestra simulación. PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 15 En las orillas del río, es decir, en la tierra encontramos una serie de palmeras de diferentes tamaños. Están pensadas como objeto decorativo y no tienen incorporadas colisiones, pues se trata simplemente de mejorar el aspecto del mundo. Estas palmeras han sido importadas a partir de un modelo VRML97 ya existente. Están agrupadas y se les han aplicado una serie de transformaciones para conseguir esa disposición en concreto. Con el nodo Background lo que hacemos es definir el color del cielo, que para nuestro caso será azul. Y con 2 PointLight añadimos la luz necesaria para iluminar todos los objetos. La intensidad de la luz será de 2 unidades para la primera y 4 para la segunda. El color de la luz será blanca para ambas (1,1,1), así como el radio que será de 1000 unidades y sin atenuación. El nodo Viewpoint viene por defecto y básicamente lo único que hace es marcar cuál es el punto de vista inicial. Nosotros lo hemos cambiado a uno más acorde con el mundo. Como decíamos, la idea inicial era crear una especie de circuito, y para ello hemos creado una serie de puertas por las que el robot debe pasar. En concreto son un total de 6 puertas. No todas son iguales, pero son muy similares. Las número 1, 2, 3 y 6 están compuestas por un grupo de dos cilindros azules. La separación entre ambos cilindros es bastante ajustada al tamaño del globo dirigible. El tamaño de cada cilindro es de 0.05 metros de radio y 0,4 m de altura. La puerta 4 es un grupo de 4 cilindros dispuestos formando un cuadrado, mientras que la puerta 5 consta de 3 cilindros en zigzag. Todos los cilindros que forman las puertas son sólidos y también se les aplican colisiones. Aunque para el proceso de desarrollo éste ha sido desactivado. Los destinos que se le marcan al zepelín han sido modelados en el mundo como esferas verdes de 0,1 metros de radio con 20 subdivisiones. Por otro lado, los obstáculos marcados para evitar se muestran en el mundo como biesferas semitransparentes de color rojo. Cada obstáculo está formado por dos esferas de 0,2 metros de radio y 20 subdivisiones. Por lo tanto, el centro de la esfera superior queda desplazado (con respecto al centro del obstáculo) 0,2 metros positivos en el eje Y, mientras que la esfera inferior lo hará en el sentido negativo del eje Y. Sólo queda describir el dock, pues el diseño del robot lo dejaremos para el apartado siguiente. El dock es una estructura de color rosa donde el robot irá a descansar una vez haya finalizado su recorrido. Consta de 4 piezas agrupadas para ser reutilizadas como boundingObject. En concreto son 2 paredes de 0.1x0.5x1.0 metros separadas 1 metro de distancia en el eje X. Un techo de 1.1x0.1x1.0 metros colocado sobre las paredes anteriores. Y un suelo con forma de cilindro de 0,4 metros de radio y 0,05 metros de altura dispuesto en el centro de la parte inferior de la estructura. 3.2.2Diseño del robot volador Vamos a ver una imagen del robot real y otra del modelo del globo dirigible utilizado en Webots señalando las diferentes partes de las que se compone. De esta forma podremos entender de forma visual cómo es el robot y cómo se ha modelado. Cabe decir que este modelo es una modificación del robot blimp_asl2 que viene como ejemplo en Webots. PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 16 Figura 10: Robot volador real construido en la EPFL Figura 11: Modelo con las partes del robot volador A continuación se muestra la lista de elementos que podemos ver en la imagen: 1.- Motor de YAW + hélice 2.- Estructura del globo dirigible 3.- Baterías + sensor de distancia 4.- Motor de ascenso/descenso + hélice + GPS 5.- Procesador + sensor de distancia 6.- Cámara 800x600 color 7.- Motor de avance/retroceso + hélice 8.- Globo de helio La estructura principal (2) sobre la que se acopla el resto de componentes del robot está modelada a base de cilindros de color grisáceo para conseguir el efecto metálico. Sobre ésta, se sujeta el globo de helio (8) modelado como una esfera blanca. La forma se consigue gracias a un escalado algo mayor al doble en el eje X que en los ejes Y y Z. 1 2 4 3 8 7 6 5 PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 17 Los 3 motores de los que consta el robot para realizar sus movimientos se han montado sobre la estructura y tienen forma de cilindro para simular las hélices. El primero de ellos (1) permite la rotación del globo sobre su eje vertical, es decir, con este motor se consigue lo que llamamos yaw. Se ha montado en la parte trasera. En la parte central encontramos el segundo motor (4), que nos permitirá ascender y descender, y el receptor GPS. Por último y en la parte delantera está el tercer motor (7). Es el encargado de hacer avanzar y retroceder al robot. A cada uno de los lados del segundo motor encontramos una caja también de color metálico. La que está en la parte trasera corresponde a las baterías del robot (3). En esa posición se ha añadido un sensor de distancia para controlar la altitud del globo. Por otro lado, la caja de la parte delantera simula el microprocesador (5) y otro sensor de distancia con el mismo propósito que el anterior. Únicamente falta hablar de la cámara (6) que se sitúa en la parte frontal del robot. Para modelarla se ha utilizado otra caja (esta vez algo mayor) de un material similar al resto. PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 18 3.3Diseño del controlador Aunque en Webots podemos crear un mundo con varios robots y cada uno de ellos puede tener asociado un controlador, en nuestro proyecto únicamente existe un robot y bastará con implementar un controlador para él. Vamos a dividir el diseño del controlador en 4 partes bien diferenciadas. Por una parte hablaremos de cómo se ha diseñado la entrada de datos. Es decir, dónde y cómo se ubican esos datos que el usuario debe introducir para utilizar este proyecto. También tenemos la parte de controlar el movimiento del robot, ya que el mismo puede trabajar de forma automática o manual. El robot deberá detectar los obstáculos que le harán colisionar de entre todos los obstáculos que existan en el mundo. Y por último deberá ser capaz de evitar estos obstáculos de tal forma que logre llegar a los destinos marcados sin colisionar en ningún momento. A parte de todo lo mencionado, como ya hablamos anteriormente, los cálculos físicos del mundo se harán también en el controlador del robot. De esta forma, podemos controlar mejor todos los aspectos relacionados con el rozamiento, la inercia, etc. que para un robot aéreo cobran una mayor importancia que si trabajáramos con robots terrestres. 3.3.1Entrada de datos Los datos que el usuario debe introducir para que el controlador pueda trabajar se dividen en dos partes. Los correspondientes al circuito que se desea que realice el globo dirigible y los que pertenecen a los obstáculos que el globo debe esquivar. Los primeros son posiciones tridimensionales del mundo que deben estar en zonas accesibles por el robot. Por ejemplo, un destino no debe estar nunca dentro del espacio que corresponde al agua. El robot evitaría sumergirse gracias al tratamiento que se le ha dado a los sensores de distancia que controlan la altitud, pero es un caso que no debería darse. Con estas posiciones colocadas en forma de pila vamos a marcar el circuito que queremos que recorra el robot. O lo que es lo mismo, el robot irá de destino a destino siguiendo el orden en que los coloquemos. Debemos tener en cuenta que las 3 últimas posiciones corresponden a la maniobra de docking y por lo tanto no deberemos modificarlas a no ser que sea necesario. Docking Destino 0 Predocking 2 Destino 1 Predocking Destino 2 Destino N … … Destino N Destino 2 Predocking Destino 1 Predocking 2 Destino 0 Docking Figura 12: Orden de introducción en la pila (izquierda). Orden de extracción de la pila (derecha) La introducción de estos datos que marcan los destinos del robot debe hacerse directamente sobre el código del controlador. Una posible extensión de este proyecto primero último primero último PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 19 sería adaptar el código para que la pila donde se guardan los destinos fuera rellenada con la lectura de un fichero de entrada que los contuviera. Pero estos temas los trataremos más adelante hacia el final de la memoria. También debemos introducir los datos de los obstáculos. Estos son biesferas, y por tanto el dato que introduciremos para cada biesfera será el punto de unión de ambas esferas, lo que podríamos llamar centro del obstáculo. En esta ocasión la forma de introducirlo también será directamente en el código del controlador y será en forma de vector, pues el orden no importa a la hora de almacenar los obstáculos. Como antes, se trata de una posición 3D. Pero otro dato importante que debemos introducir sobre los obstáculos es el radio de sus esferas. En un principio el código está preparado para obstáculos de mismo radio, aunque no sería complicado adaptarlo para que los obstáculos pudieran tener radios de diferente longitud. 3.3.2El movimiento del robot Nuestro robot consta de 3 motores como ya hemos visto en la parte del diseño del robot. Cada uno de ellos va a permitir al robot realizar diferentes movimientos. Para poder referirnos más claramente a cada uno de ellos y evitar confusiones los numeraremos a partir de ahora de la siguiente manera. Cuando hagamos referencia al motor 1, estaremos hablando del motor ubicado en la parte delantera del robot (7 en la Figura 11: Modelo con las partes del robot volador). El motor 2 será el de la zona baja del robot (4 en la Figura 11). Mientras que hablaremos del motor 3 cuando nos refiramos al de la parte trasera (1 en la Figura 11). Llegados a este punto debemos hacer una distinción entre el control del robot de forma manual y el control de forma automática. Ya que el controlador del robot está preparado para poder cambiar de un modo al otro pulsando la tecla “P”. En los dos casos vamos a terminar trabajando en términos de la aceleración que le aplicamos a los motores, pero en el caso manual, ésta la controlará el usuario a su gusto con la ayuda del teclado y en el caso automático será el propio controlador el que se encargue de calcularla en todo momento. Para conocer en profundidad el funcionamiento de la física existente en el controlador y causante del movimiento del robot, se puede consultar el documento original de Jean-Christophe Zufferey et al. “Flying over the Reality Gap: From Simulated to Real Indoor Airships”, 2006 Bibliografía. Pero de momento bastará con tener constancia de que al tratarse de un robot aéreo no vamos a trabajar con velocidad como haríamos en un robot terrestre sino que nuestra herramienta para realizar los movimientos será la aceleración de los motores. Así pues, lo que haremos será aplicar un multiplicador de la aceleración a cada motor y de esta forma incrementar o disminuir el valor final de aceleración que se le aplica al motor en cuestión. Debemos saber que el sistema de coordenadas del globo es diferente al del mundo virtual de Webots. Aunque esto se solventa en la programación y a efectos prácticos el usuario no lo nota en ningún momento. Ni siquiera cuando el control del globo dirigible lo realiza él. Como la manera de utilizar el robot de forma manual se explicará en el apartado 4.4, vamos a explicar a continuación el funcionamiento automático del globo. PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 20 Subir/Bajar Al tratarse de un robot volador lo primero que vamos a tener que controlar es la altitud a la que se encuentra el robot. Podemos conocerla gracias a la posición que nos proporciona el nodo GPS. Y así, sabiendo la altitud a la que se encuentra el globo, podemos actuar sobre el motor 2 para llevarlo a otra diferente, con lo que conseguiríamos un movimiento de subir o bajar el globo en el espacio. Aunque siempre debemos tener en cuenta que si dejamos de actuar sobre el motor, el globo dirigible caerá por la acción de la aceleración gravitatoria que se ha añadido a las físicas del mundo. Por tanto, en todo momento (aunque lo que deseemos sea dejar el robot inmóvil en un punto) el motor 2 deberá ser controlado y estar actuando. Para que el globo dirigible no se descontrole en ningún momento, cuando los sensores de distancia ubicados en la parte inferior del globo detectan que éste se encuentra demasiado cerca de la superficie, el motor 2 se pone en funcionamiento en una maniobra de emergencia que le proporciona más altitud. De la misma manera, pero utilizando los datos del GPS en lugar de los de los sensores de distancia, cuando el globo se eleva fuera del rango deseado también se inicia una maniobra automática de emergencia que lo estabiliza a una altitud correcta por debajo de ese umbral. Sin embargo mientras el globo se encuentra en la zona de maniobras deseada, la altitud es controlada por el motor 2 en función de la altitud actual y la altitud del próximo destino. Pues los destinos son posiciones 3D y nos interesa alcanzarlos con la mayor exactitud posible, incluida la altitud. Figura 13: Movimiento Subir/Bajar del globo dirigible Por lo tanto, dentro del controlador que se encarga de supervisar todo lo relacionado con el robot, necesitaremos programar un “controlador” que actúe sobre el motor 2. Pero debemos distinguir entre el controlador que le decimos a Webots que asigne a nuestro robot y una parte de código de este controlador que se encargará únicamente de actuar sobre el motor 2. A continuación explicaremos cómo, después de estudiar el caso, el robot, la física asociada, etc. hemos escogido el tipo de controlador y las características que lo hacían útil para nuestro proyecto. La tarea de este controlador es saber en cada momento cuánto se debe actuar sobre el motor 2 para que la nueva altitud del globo se alcance de forma correcta en el período de tiempo correcto. Eje Y PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 21 De entre los tipos de controlador que podríamos haber escogido, después de varias pruebas hemos optado por un controlador proporcional-derivativo. La parte integral de un PID no nos aportaba nada útil y por este motivo no la hemos incorporado. Los valores que se aplican a cada una de las ganancias, varían según el estado en el que se encuentra el robot. Esto es así debido a que no se puede aplicar la misma acción si por ejemplo lo único que queremos es mantenernos a una altitud fija, que si además debemos estar en continuo movimiento. Estos valores forman parte de la programación del controlador y se pueden ver en el código del robot. Además de la parte de programación correspondiente a controlar la potencia, hace falta cambiar el sentido de ésta ya que el motor 2 está ubicado en el globo de forma que valores positivos hacen bajar al robot. Pero esto es una simple cuestión de programación relacionada con el diseño que está comentada en el código y no implica nada más que un cambio de signo a la variable que controla la potencia suministrada. Avanzar/Retroceder Partimos de la base de que el robot se encuentra en una posición actual (origen) y debe llegar a una posición final (destino). En cada iteración el origen habrá cambiado si ha habido movimiento, pero el destino permanecerá sin cambios hasta que se alcance. Para comenzar, pondremos como condición que ambas posiciones se encuentren en el mismo eje (paralelo al eje Z del mundo), pues nuestro mundo tiene profundidad en ese eje y es hacia donde nos interesa que se realice el movimiento. Y además, el robot estará orientado hacia el destino. De esta forma, poner en funcionamiento el motor 1 con un multiplicador positivo significará que el robot avanza hacia adelante a la vez que se va acercando al destino (16). Si utilizáramos un multiplicador negativo, el robot se movería hacia atrás, alejándose del destino. Ya que esto implica que el motor gire en sentido contrario y con este la hélice que aporta el movimiento. Figura 14: Movimiento Avanzar/Retroceder del robot Ya sabemos cómo vamos a comenzar a realizar el movimiento más sencillo, pero tenemos que tener en cuenta que no nos vale únicamente con llegar al destino. Si estuviéramos trabajando con un vehículo terrestre el rozamiento actuaría en nuestro favor y al dejar de suministrar potencia a los motores, el vehículo prácticamente pararía en el sitio correcto. Pero como siempre, al tratarse de un vehículo aéreo el rozamiento actuará en menor medida y la inercia hará que el robot se desplace mucho más allá del destino si lo único que hacemos es dejar de suministrar potencia a los motores al llegar al destino. Esto no es viable para nuestro proyecto y debemos controlar esta situación. Para ello lo que haremos será incorporar un controlador proporcional-derivativo al código de manera que actúe sobre la variable del multiplicador de potencia. Así lo que estaremos haciendo es controlar la potencia que se le suministra al motor 1 teniendo en Eje Z PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 22 cuenta la distancia a la que se encuentra el robot del destino. Cuando el robot se encuentre relativamente lejos, la potencia será mayor y el robot se moverá más rápidamente. Sin embargo, conforme el robot se vaya acercando al destino, la potencia con la que se actuará sobre el motor 1 irá disminuyéndose hasta el punto de utilizar incluso un multiplicador negativo para conseguir que la velocidad del globo baje hasta valores cercanos a cero en el punto del destino. De esta forma, nos estamos adelantando a lo que sucedería y conseguimos actuar antes de que el globo sobrepase el destino parándolo en el lugar correcto. Todo este funcionamiento lo realiza de nuevo un controlador, pero esta vez aplicado a la variable encargada de ajustar la potencia del motor 1. Dicho controlador es del tipo proporcional-derivativo, pero el ajuste de las variables implicadas en el controlador es diferente al del motor 2. Además, igual que en el caso anterior, según el estado en que se encuentre el robot y en el que tenga que actuar este controlador, los valores de las variables cambiarán. Hay que darse cuenta de que así como cuando trabajábamos con el motor 2 era necesario que estuviera actuando de forma continua, en este caso no va a ser necesario puesto que no hay ninguna fuerza gravitatoria ni de ningún otro tipo que esté actuando continuamente sobre el globo. Todas las fuerzas que se ejercen en esta dirección provienen del propio movimiento del robot como por ejemplo la inercia. Lo que se trata de explicar es que habrá que controlar fuerzas como la inercia con este motor, pero no fuerzas continuas como la gravedad. Izquierda/Derecha Nuestro globo consta de un tercer motor (motor 3) que le permite realizar giros en el espacio. Pero estos giros tienen una característica muy importante y que ha llevado más tiempo controlar que los demás movimientos. Y es que estamos hablando de un motor que está situado en la cola del globo dirigible a modo de timón. Esto provoca que las fuerzas ejercidas por la hélice de este motor hagan girar la parte trasera del globo. Y con ello se consigue un giro característico muy diferente al que podemos obtener con un robot terrestre que conste de ruedas diferenciales por ejemplo. Esto es debido a que el eje de rotación deja de estar en el centro o en uno de los laterales y pasa a estar en la parte delantera. Figura 15: Movimiento de rotación Izquierda/Derecha del robot Realizar este tipo de giros no es trivial y de nuevo es necesario aplicar un controlador sobre el multiplicador de la potencia del motor 3 para lograr que los giros sean correctos y a la vez precisos. PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 23 Lo primero que se intentó para solventar esta dificultad fue en primer lugar buscar la orientación correcta hacia el destino con la ayuda de unos parámetros concretos de controlador PD y una vez orientado el globo dejar de actuar sobre el motor 3 para comenzar un movimiento rectilíneo. El problema es que lo que se conseguía era un movimiento muy artificial, en el sentido de que los movimientos se dividían en fases y además la precisión era inaceptable debido sobre todo a la inercia que seguía actuando sobre el robot. Así que se abordó el problema de otra manera. Lo que se ha hecho ha sido aprovechar el autómata de estados creado para el robot. En el estado de “BUSCA_ANGULO” se utiliza un controlador PD para orientar el robot hacia su destino al igual que comentábamos anteriormente. Y una vez se consigue tener orientado el globo hacia el destino con un error lo suficientemente pequeño, se pasa al estado “AVANZA”. En el estado “AVANZA”, además de seguir controlando los dos movimientos vistos anteriormente, se empieza a controlar el motor 3 de una forma especial. Se hace con un controlador PID, similar al del estado “BUSCA_ANGULO” sólo que esta vez añadimos la parte integral del controlador para obtener el mínimo error posible en el transitorio. Pero con esto únicamente no era suficiente y la opción por la que se optó fue aplicar otro controlador PID pero esta vez sobre el error cometido en el eje X. Es decir, con un primer controlador actuamos para corregir el ángulo del robot, y con el segundo obligamos a corregirlo más o menos dependiendo de lo lejos que se encuentre el globo dirigible del eje paralelo al eje Z que debe alcanzar el globo para llegar al destino. De forma gráfica sería de la siguiente manera: Figura 16: Trayectoria del robot Eje Z Error en X Eje X PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 24 El último aspecto a tener en cuenta en este caso es que como estamos trabajando con ángulos de Euler deberemos tener especial atención en tratar los casos de ángulos positivos y negativos de la manera adecuada. De ahí que el código esté duplicado en ese aspecto, para tener en cuenta en qué caso de los dos se encuentra y cómo actuar. Movimiento conjunto Ya hemos explicado uno a uno los tipos de movimiento que el robot puede realizar y la forma de controlarlos por separado. Pero esto sólo fue el principio, pues hasta que no se tuvo controlada la altitud, no se comenzó a estudiar el caso de mover el robot en línea recta. Y una vez encontrada la configuración adecuada para llegar a un punto situado en línea recta a una altura diferente, se incorporó el controlador del giro. Primero simplemente realizar un giro buscando la orientación correcta hacia el destino, permaneciendo el globo estático. Más tarde añadiendo el movimiento y el controlador explicado más arriba para alcanzar la posición de destino siguiendo su eje paralelo al eje Z. Llegado a ese punto funcionaba, pero no era posible controlar la inercia como era necesario y el robot llegaba al destino con demasiado error en el eje X. La solución vino al tener en cuenta, en el controlador del motor 1, la línea recta que une la posición actual con el destino en lugar del error en el eje Z. Lo que podríamos llamar la hipotenusa del triángulo recto formado por el origen y el destino. Teniendo esto último en cuenta es como se han obtenido los resultados correctos y de esta forma el robot realiza los movimientos de un destino a otro con un error mínimo. Figura 17: Movimiento conjunto y errores en los ejes 3.3.3Detección de colisiones Por el momento tenemos controlado todo lo relacionado con el movimiento del robot. Ya hemos visto cómo, y somos capaces de mandar al robot desde un origen a cualquier destino. El globo dirigible llegará a ese punto cometiendo un error mínimo dentro de los márgenes aceptables. Pero vamos a introducir un elemento más al proyecto que son los obstáculos. Eje Z Error en X Eje X Error en Z Error en H PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 31 4Manual de usuario 4.1Requisitos Para la realización de este proyecto se ha utilizado un ordenador de sobremesa AMD Athlon 64 processor 3400+ a 2,4 GHz y con 512 MB de memoria RAM. EL sistema operativo ha sido Microsoft Windows XP Profesional v. 2002 con el ServicePack 3. Para ejecutar la simulación es indispensable el software de Cyberbotics Webots 5.1.0. En su defecto, será válida una versión compatible. PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 32 4.2Modificaciones del entorno Todas las modificaciones que tengan que ver con elementos del entorno, van a ser realizadas sobre el apartado visual de Webots. Sin embargo, aquellas que afecten a la tarea del robot se harán sobre el código del controlador. Esto quiere decir que si por ejemplo queremos añadir un elemento decorativo no tendremos que tocar para nada el código del controlador. Sin embargo, si el elemento que queremos añadir es un nuevo obstáculo, deberemos añadirlo tanto al entorno visual como al código del controlador. En este apartado se explicará el caso de modificar el entorno visual sin afectar al código del controlador. Las ventanas de Webots que vamos a necesitar son la ventana principal del programa donde veremos el resultado y la ventana que contiene el Scene Tree donde cambiaremos las características del mundo VRML. 4.2.1Añadir un elemento Ya que el mundo virtual está creado utilizando VRML, podríamos editar el código en cualquier editor de texto siempre y cuando respetáramos la estructura de este lenguaje. Pero como en realidad es una adaptación de este lenguaje lo recomendable es utilizar el editor de Webots. Este editor nos dará las opciones aceptadas en cada caso y lo único que deberemos hacer es ir completando los campos. En la ventana tenemos un botón que nos permite crear nodos nuevos eligiéndolos de una lista de nodos permitidos. Figura 20: Botón Insert after Con este botón vamos a crear los nodos principales y cada uno que creemos nos aparecerá en el Scene Tree en el lugar que nosotros hayamos elegido. Si pinchamos sobre el nuevo nodo, podremos darle un nombre a ese mismo nodo, lo que en VRML es un DEF. A la izquierda del nodo tenemos el símbolo + o – que nos permitirá expandir o contraer los campos. Si expandimos el nodo, podremos pinchar sobre cada uno de los campos que lo componen y en la parte derecha de la ventana completar los valores de los que deseemos. Acompañados de un círculo rojo en lugar de azul veremos que van todos aquellos nodos susceptibles de contener otros nodos (a excepción del atributo CHILDREN, que aparece en azul pero puede contener otros nodos). En cambio en azul sólo aparecerán los atributos de los nodos que contienen valores. Pongámonos en el caso de que queramos colocar un nuevo elemento decorativo en el mundo virtual. Por ejemplo una esfera que represente el Sol. El nodo que vamos a necesitar va a ser el nodo SOLID, así podremos aplicarle propiedades como la física. Entonces haremos clic sobre el botón “Insert after” y elegiremos el nodo SOLID. PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 33 Aparecerá debajo del nodo que tuviéramos seleccionado en ese momento. Y aparecerá comprimido, así que haremos clic sobre el + para expandir sus atributos. Lo primero que haremos será definir el nodo con el nombre SOL y para eso lo escribiremos en la parte derecha de la ventana cuando lo tengamos seleccionado. Ahora le daremos una forma, así que iremos al atributo CHILDREN y haremos clic nuevamente en el botón “Insert after”. Elegiremos el nodo SHAPE y aparecerá dentro de CHILDREN. El programa tiene un error de refresco y deberemos seleccionar un nodo diferente para ver los cambios. El nodo SHAPE tiene dos campos: APPEARENCE y GEOMETRY. El primero lo rellenaremos con un nodo APPEARENCE. El segundo con uno SPHERE. Este último acepta dos parámetros; el radio y el número de divisiones. Los rellenaremos con 1 y 20. Por el otro lado de los parámetros del nodo APPEARENCE sólo rellenaremos el MATERIAL con un nodo MATERIAL. Los atributos diffuseColor, emissiveColor y specularColor serán 1 1 0 para de esta forma tener una esfera amarilla. Todos los elementos se crean por defecto en la posición (0, 0, 0). Por eso vamos a trasladar el objeto entero a un lugar mejor. Pincharemos el atributo TRANSLATION del sólido que estamos creando y pondremos sus valores a -2, 6, -8. 4.2.2Modificar y eliminar un elemento Como hemos visto los nodos contienen atributos. Y al igual que cuando creamos un nodo nuevo debemos ir completando esos atributos con valores, cuando los nodos ya están creados la manera de proceder es la misma. Cualquier valor que queramos modificar, lo haremos desplegando los atributos del nodo y pinchando sobre el atributo a cambiar. En la parte derecha de la ventana podremos poner el nuevo valor e inmediatamente aparecerá en la ventana principal el cambio realizado. Webots permite transformar unos nodos en otros con el botón TRANSFORM, que está situado a la izquierda del botón “Insert after”. Este botón lo que nos va a permitir es aprovechar los atributos en común entre el nodo existente y el nodo en que queremos que éste se transforme. Nos ahorrará tiempo en caso de que necesitemos realizar esta operación. Figura 21: BotónTransform También podemos realizar las típicas operaciones de Copiar, Cortar y Pegar. Pero lo interesante es que se nos permite hacerlo sobre los nodos enteros, incluidos los atributos. Figura 22: Botones de cortar, copiar y pegar PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 34 Por último, si lo que queremos es eliminar un nodo completo sólo deberemos seleccionarlo y apretar el botón que contiene un aspa negra. Automáticamente desaparecerá del árbol y de la escena. Figura 23: Botón eliminar nodo PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 35 4.3Modificar el controlador Necesitaremos tener en el directorio de trabajo las carpetas world y controllers. Dentro de la primera deberemos colocar el archivo “mundo_XVI.wbt”. En la segunda, será necesario crear una carpeta con el nombre “c_mundo_XVI” que contenga los ficheros “Makefile”, “pila.cpp”, “punto.cpp” y “c_mundo_XVI.cpp”. Aunque este paso es recomendable realizarlo desde el propio Webots con la opción de Wizard > New robot controller, ya que de esta forma es más sencillo y evita problemas de mala estructuración. Con esta estructura de archivos estaremos en condiciones de ejecutar Webots y compilar el código desde el editor de código. Una vez compilado nos aparecerán en esta última carpeta los archivos “c_mundo_XVI.o” y “c_mundo_XVI.exe”. Ahora ya podremos lanzar la ejecución de la simulación desde la ventana principal de Webots. Obviamente, cada vez que se realice un cambio en el código fuente del controlador, habrá que volver a compilar el código. Lo podemos hacer como se acaba de explicar desde la ventana del editor de código. Una vez compilado el código con los cambios, podremos lanzar a ejecución el nuevo programa tal y como se ha comentado. 4.3.1Cambiar de controlador En un principio para utilizar este proyecto no es necesaria esta operación, pues el mundo ya viene con el controlador necesario asociado al robot. Pero en cualquier caso, se nos puede presentar la necesidad de cambiar el controlador del robot. Los pasos a seguir son los siguientes. En primer lugar es necesario crear el controlador, como ya se ha explicado anteriormente en esta memoria. Una vez el controlador ya existe, sólo hay que expandir los atributos del robot en el Scene Tree y buscar “controller”. En la parte derecha de la ventana tenemos un botón con tres puntos suspensivos que nos muestra una ventana donde elegir uno de los controladores permitidos por Webots. Si todo es correcto aparecerá el que buscamos y sólo con seleccionarlo quedará asociado al robot. 4.3.2Añadir un elemento En ocasiones no sólo vamos a modificar aspectos visuales del entorno, si no que nos va a interesar que el elemento que introduzcamos sea un elemento con el que el robot interactúe. Vamos a describir la forma de añadir los objetos más importantes. Uno de ellos son los destinos del robot. Para que el robot realice los cálculos necesarios para avanzar de un destino a otro, éstos deben ser almacenados. Para poder gestionarlos más fácilmente, los destinos que debe seguir el zepelín se almacenan en una pila. Para añadir un nuevo destino tenemos que añadir un nuevo elemento a la pila en el lugar que deseemos con la posición del destino a seguir por el robot: stack.push(Punto(pos_X, pos_Y, pos_Z)); Es muy importante dejar los 3 últimos puntos de la pila sin modificar, pues son los establecidos para la maniobra de docking. Otro tipo de objeto que nos puede interesar añadir son los obstáculos. Los obstáculos también quedan almacenados en memoria y son almacenados en forma de vector, pues no importa el orden de almacenamiento. Si queremos añadir uno podemos añadir la PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 36 posición de su centro al vector de obstáculos teniendo en cuenta que deberemos incrementar la constante “NUM_OBS” en una unidad. #define NUM_OBS 2 float Obstaculos[NUM_OBS][3]= { 0.95, 2.0, 2.0, -0.6, 2.2, 0.0 }; 4.3.3Modificar y eliminar un elemento Al igual que podemos querer añadir elementos que no sólo afectan al aspecto visual, si no que también afectan a la parte del controlador, también nos puede interesar por cualquier motivo el modificar los elementos ya existentes o incluso eliminarlos por completo. La forma de proceder es muy similar a la del apartado anterior. Se trata de localizar en el código el objeto a modificar, y cambiar directamente sobre el código los valores que se desee. Por ejemplo para cambiar de posición el primer destino del robot, lo que haríamos sería ir a la pila donde están todos almacenados y cambiar los valores de pos_X, pos_Y y pos_Z al último elemento añadido a la pila. Así la próxima vez que compilemos el código, los nuevos valores ya serán los que siga el zepelín. En caso de querer modificar las posiciones de los obstáculos no debemos cambiar el valor de la constante NUM_OBS como hacíamos antes. Únicamente cambiaremos los valores del obstáculo que queremos mover. Para cambiar el radio de las esferas que envuelven a los objetos del cálculo de colisiones lo haremos directamente sobre el campo de la estructura de la esfera. Este es uno de los motivos por los que se planteará más adelante una propuesta de ampliación de este proyecto. Visto cómo modificar los elementos, falta saber la manera de eliminarlos. Gracias al método escogido para almacenar dichos elementos su eliminación va a ser tan sencilla como suprimir del código las posiciones del elemento. Es decir, en el caso de querer eliminar un destino, lo localizaremos en la pila y suprimiremos la línea de código que lo inserta en la pila. De esta forma cuando se vuelva a compilar el código, ese destino no existirá para el robot. En el caso de los obstáculos es muy similar, pues están almacenados en un vector y si suprimimos la línea de código que lo almacena sucederá como en el caso anterior. PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 37 4.4Manejo manual del robot Como ya se ha comentado en alguna ocasión a lo largo de esta memoria, al robot volador se le han implementado 2 modos de vuelo. El primero y más importante es el modo automático. Este modo como ya se ha visto es capaz de hacer que el globo dirigible siga un circuito marcado por diferentes destino y además evitando los posibles obstáculos que pueda haber en su trayectoria sin perder esta misma. Sin embargo el modo manual consiste en la posibilidad de desactivar este “piloto automático” y manejar con el teclado todos los motores del globo. La forma de activar y desactivar el modo manual es pulsando la tecla “P”. En cualquier momento de la ejecución de la simulación se puede pulsar esta tecla y cambiará de modo. Por defecto comienza el modo automático, por lo tanto pulsar la tecla “P” significa cambiar al modo manual y volver a pulsarla activaría de nuevo el modo automático. Para manejar los 3 motores del robot se utilizará el teclado como ya se ha dicho. Concretamente activaremos el motor 1 con las teclas “↑ / ↓”. La primera hará que el globo avance y la segunda lo hará retroceder. El motor 2 lo controlamos con las flechas “A / Z”, que harán que el robot suba y baje respectivamente. Finalmente controlamos el giro del robot con el motor 3 con la ayuda de las teclas “← / →”, que giran la cola del robot hacia la derecha y hacia la izquierda. Se ha tenido en cuenta la visión del usuario y pulsar la tecla “←” hace que el robot se mueva hacia la izquierda, lo que significa que la cola del globo se mueve hacia la derecha. Figura 24: Resumen de los movimientos que realiza cada tecla A Z Z X Y X Z X PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 38 5Conclusiones El objetivo principal del proyecto era conseguir crear la simulación de un robot volador autónomo. Como el programa a utilizar era Webots y el proyectando no tenía experiencia con ese software, lo primero fue familiarizarse con él. Como apoyo se tienen 2 documentos: la guía de referencia y el manual de usuario. El programa también tiene varios ejemplos que se fueron consultando y realizando progresivamente añadiendo características a los robots creados. Finalmente cuando ya se tenía cierta soltura con el programa se comenzó a crear el mundo virtual. En este caso sí que tenía experiencia con VRML y no supuso una gran dificultad modelar los objetos como era necesario. Por lo tanto era cuestión de aprender el funcionamiento de los nuevos nodos que incorpora Webots para los robots. Con el mundo creado se añadió el modelo de ejemplo “blimp_asl2”, un globo dirigible de la EPFL. Y se creó un nuevo controlador incluyendo partes útiles del controlador original. En este nuevo controlador es donde se comenzó a trabajar con la parte de programación de este proyecto final de carrera. Lo que se pretendía era en una primera parte controlar el robot de forma manual con el teclado. Para ello se estudió el tipo de controlador más adecuado a cada una de las circunstancias y motores del robot. Más adelante cuando el robot era manejable se incorporó el seguimiento de destinos prefijados. Es decir, el robot viajaba de forma correcta y automáticamente por un circuito marcado. Finalmente se añadió una parte muy importante de código encargada de detectar y evitar obstáculos mientras se seguía el circuito marcado. Para la realización de esta parte ha sido esencial el estudio de la tesis doctoral de Enrique Bernabeu Soler, director de este proyecto. PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 39 5.1Posibles ampliaciones futuras Como en todo proyecto existen una serie de cuestiones dignas de ser estudiadas para una posible ampliación. A continuación se hablará de las que quizá deberían considerarse prioritarias, ya sea por la mejora que suponen al proyecto o bien por su utilidad posterior. Existen datos necesarios para la ejecución de la simulación que quedan almacenados en memoria para ser utilizados por los cálculos internos. Estos datos como pueden ser los destinos que debe seguir el robot o los obstáculos que aparecen en el entorno se almacenan como parte del código. Esto hace que para realizar pequeños cambios en las posiciones de estos elementos sea necesario volver a compilar el controlador. Debido a las limitaciones de Webots y del proyecto en sí, no parece viable crear una interfaz gráfica que permita al usuario la introducción de estos datos, pero sí que sería interesante modificar el código fuente para realizar la lectura de estos datos por fichero. Otro aspecto que sería interesante modificar es el tamaño de las esferas envolventes para la detección y evitación de los obstáculos. Este radio ha sido el mismo durante todo el proyecto y por eso no se ha tenido en cuenta el poder modificarlo independientemente según el objeto al que pertenezca. Una posible e interesante ampliación sería añadir al vector de obstáculos además de la posición, el radio. Y más tarde en el algoritmo que hace uso de él, utilizar este nuevo valor almacenado en lugar del fijo que se utiliza actualmente. También hay un aspecto menos importante que es el del paso de control automático a manual y viceversa. En estos momento lo que se hace es controlar el teclado y ver si la tecla “P” es pulsada. En este caso se cambia de modo con un flag de control. El inconveniente es que debido al tiempo de muestreo que es de 32 ms, puede suceder que en una pulsación pase un número par de iteraciones. En ese caso no se cambiaría de modo por la forma en que se ha programado el cambio. Por último también se podría estudiar el caso de obstáculos en puntos críticos de la trayectoria del robot. Estos pueden ser por ejemplo obstáculos muy cercanos al destino o incluso muy cercanos entre sí. Habría que estudiar en qué medida afectaría a la trayectoria del robot y cómo se podría solucionar. PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 40 Tabla de ilustraciones Figura 1: Directorio de trabajo Webots 5.1.0 ............................................................................................... 7 Figura 2: Ventana Scene Tree de Webots 5.1.0 ............................................................................................ 9 Figura 3: Creación de un controlador ........................................................................................................ 10 Figura 4: Asociación de un controlador a un CustomRobot ...................................................................... 10 Figura 5: Ventana Text Editor de Webots 5.1.0 ........................................................................................ 11 Figura 6: Ventana Log de Webots 5.1.0 ..................................................................................................... 12 Figura 7: Vista inicial del entorno de simulación ...................................................................................... 13 Figura 8: Sistema de coordenadas .............................................................................................................. 14 Figura 9: CrossSection de la tierra ............................................................................................................ 14 Figura 10: Robot volador real construido en la EPFL ............................................................................... 16 Figura 11: Modelo con las partes del robot volador ................................................................................... 16 Figura 12: Orden de introducción en la pila (izquierda). Orden de extracción de la pila (derecha) .......... 18 Figura 13: Movimiento Subir/Bajar del globo dirigible ............................................................................. 20 Figura 14: Movimiento Avanzar/Retroceder del robot .............................................................................. 21 Figura 15: Movimiento de rotación Izquierda/Derecha del robot .............................................................. 22 Figura 16: Trayectoria del robot ................................................................................................................ 23 Figura 17: Movimiento conjunto y errores en los ejes ............................................................................... 24 Figura 18: Cálculo del punto intermedio ................................................................................................... 29 Figura 19: Screenshot de la ejecución del robot evitando un obstáculo ..................................................... 29 Figura 20: Botón Insert after ..................................................................................................................... 32 Figura 21: BotónTransform ....................................................................................................................... 33 Figura 22: Botones de cortar, copiar y pegar ............................................................................................. 33 Figura 23: Botón eliminar nodo ................................................................................................................. 34 Figura 24: Resumen de los movimientos que realiza cada tecla ................................................................. 37 PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 47 obs->radio=bi.r1; }else{ mtd2=modulo_vector(cero,proyeccion2) - (bi.r0 + ((bi.r1-bi.r0)*lambda2)); obs->radio=(bi.r0 + ((bi.r1-bi.r0)*lambda2)); } //S32 --> S23 bi.r0=m2.r; bi.r1=m3.r; bi.c0=m2.c; bi.c1=m3.c; lambda1=-( (bi.c0 * (bi.c1-bi.c0)) / (modulo_sin_raiz( bi.c0,bi.c1 )) ); proyeccion1= (bi.c0 + ((bi.c1-bi.c0)^lambda1)); if(lambda<0.0){ tipo=1; mtd1=modulo_vector(cero,bi.c0)-bi.r0; obs->radio=bi.r0; }else if(lambda1>1.0){ tipo=1; mtd1=modulo_vector(cero,bi.c1)-bi.r1; obs->radio=bi.r1; }else{ mtd1=modulo_vector(cero,proyeccion1) - (bi.r0 + ((bi.r1-bi.r0)*lambda1)); obs->radio=(bi.r0 + ((bi.r1-bi.r0)*lambda1)); } if(mtd1<=mtd2){ mtdcalculado=mtd1; obs->lambda=lambda1; obs->mtd=mtdcalculado; obs->proy=proyeccion1; obs->radio=radio1; }else{ mtdcalculado=mtd2; obs->lambda=lambda2; obs->mtd=mtdcalculado; obs->proy=proyeccion2; obs->radio=radio2; } break; case 7: break; case 8: tipo=3; //S013 tri.r0=m0.r; tri.r1=m1.r; tri.r2=m3.r; tri.c0=m0.c; tri.c1=m1.c; tri.c2=m3.c; proyeccion= (tri.c0 + (((tri.c1-tri.c0)^l1bi) + ((tri.c2-tri.c0)^l2bi))); d0=modulo_vector(cero,proyeccion); mtdcalculado=d0-(tri.r0+((tri.r1-tri.r0)*l1bi)+((tri.r2-tri.r0)*l2bi)); obs->radio=(tri.r0+((tri.r1-tri.r0)*l1bi)+((tri.r2-tri.r0)*l2bi)); obs->lambda=lbi; obs->mtd=mtdcalculado; obs->proy=proyeccion; break; case 9: tipo=2; //S01 --> S10 bi.r0=m1.r; bi.r1=m0.r; bi.c0=m1.c; bi.c1=m0.c; lambda=-( (bi.c0 * (bi.c1-bi.c0)) / (modulo_sin_raiz( bi.c0,bi.c1 )) ); proyeccion= (bi.c0 + ((bi.c1-bi.c0)^lambda)); if(lambda<0.0){ tipo=1; mtdcalculado=modulo_vector(cero,bi.c0)-bi.r0; obs->radio=bi.r0; }else if(lambda>1.0){ tipo=1; mtdcalculado=modulo_vector(cero,bi.c1)-bi.r1; obs->radio=bi.r1; }else{ PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 48 mtdcalculado=(modulo_vector(cero,proyeccion)) - (bi.r0 + ((bi.r1-bi.r0)*lambda)); obs->radio=(bi.r0 + ((bi.r1-bi.r0)*lambda)); } obs->lambda=lambda; obs->mtd=mtdcalculado; obs->proy=proyeccion; break; case 10: tipo=2; //S13 bi.r0=m1.r; bi.r1=m3.r; bi.c0=m1.c; bi.c1=m3.c; lambda=-( (bi.c0 * (bi.c1-bi.c0)) / (modulo_sin_raiz( bi.c0,bi.c1 )) ); proyeccion= (bi.c0 + ((bi.c1-bi.c0)^lambda)); if(lambda<0.0){ tipo=1; mtdcalculado=modulo_vector(cero,bi.c0)-bi.r0; obs->radio=bi.r0; }else if(lambda>1.0){ tipo=1; mtdcalculado=modulo_vector(cero,bi.c1)-bi.r1; obs->radio=bi.r1; }else{ mtdcalculado=modulo_vector(cero,proyeccion) - (bi.r0 + ((bi.r1-bi.r0)*lambda)); obs->radio=(bi.r0 + ((bi.r1-bi.r0)*lambda)); } obs->lambda=lambda; obs->mtd=mtdcalculado; obs->proy=proyeccion; break; case 11: tipo=2; //S01 --> S10 bi.r0=m1.r; bi.r1=m0.r; bi.c0=m1.c; bi.c1=m0.c; lambda2=-( (bi.c0 * (bi.c1-bi.c0)) / (modulo_sin_raiz( bi.c0,bi.c1 )) ); proyeccion2= (bi.c0 + ((bi.c1-bi.c0)^lambda2)); if(lambda2<0.0){ tipo=1; mtd2=modulo_vector(cero,bi.c0)-bi.r0; radio2=bi.r0; }else if(lambda2>1.0){ tipo=1; mtd2=modulo_vector(cero,bi.c1)-bi.r1; radio2=bi.r1; }else{ mtd2=modulo_vector(cero,proyeccion2) - (bi.r0 + ((bi.r1-bi.r0)*lambda2)); radio2=(bi.r0 + ((bi.r1-bi.r0)*lambda2)); } //S13 bi.r0=m1.r; bi.r1=m3.r; bi.c0=m1.c; bi.c1=m3.c; lambda1=-( (bi.c0 * (bi.c1-bi.c0)) / (modulo_sin_raiz( bi.c0,bi.c1 )) ); proyeccion1= (bi.c0 + ((bi.c1-bi.c0)^lambda1)); if(lambda1<0.0){ tipo=1; mtd1=modulo_vector(cero,bi.c0)-bi.r0; radio1=bi.r0; }else if(lambda1>1.0){ tipo=1; mtd1=modulo_vector(cero,bi.c1)-bi.r1; radio1=bi.r1; }else{ mtd1=modulo_vector(cero,proyeccion1) - (bi.r0 + ((bi.r1-bi.r0)*lambda1)); radio1=(bi.r0 + ((bi.r1-bi.r0)*lambda1)); } if(mtd1<=mtd2){ mtdcalculado=mtd1; obs->lambda=lambda1; PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 49 obs->mtd=mtdcalculado; obs->proy=proyeccion1; obs->radio=radio1; }else{ mtdcalculado=mtd2; obs->lambda=lambda2; obs->mtd=mtdcalculado; obs->proy=proyeccion2; obs->radio=radio2; } break; default: break; } } PFC - Simulación de un robot volador autónomo mediante la utilización de la herramienta Webots 50 Apéndice C: Código de evitación de colisiones // buscamos el mas cercano que colisionara (lambda mas pequeña con mtd negativo) minlambda=10000; for(int k=0; k<NUM_OBS; k++){ if( obs[k].mtd < 0.0 // habra colision && obs[k].lambda < minlambda // es la mas cercana && obs[k].lambda>0.0 // esta por delante del robot && obs[k].lambda<1.0 // esta antes del destino (podemos dejar margen) ){ minlambda=obs[k].lambda; ilambda=k; } } // SI DISTANCIA <= 0 HAY COLISION if(obs[ilambda].mtd < 0.0){ // determinar posicion de maxima penetracion Punto maxpenetracion; maxpenetracion= origen.c + ((destino.c - origen.c)^obs[ilambda].lambda); p_intermedio = maxpenetracion - ((normalizar(cero,obs[ilambda].proy))^((obs[ilambda].mtd)*1.05)); // para evitar un globo submarino if(p_intermedio.y < 1.8){ robot_console_printf("Evitando inmersion!!"); float mtd_nueva = 0.0; mtd_nueva = obs[ilambda].mtd + (obs[ilambda].radio*2.0); if(mtd_nueva > 0) mtd_nueva = -mtd_nueva; p_intermedio = maxpenetracion - ((normalizar(obs[ilambda].proy,cero))^((mtd_nueva)*1.05)); } // control de altitud if(p_intermedio.y < 1.6){ p_intermedio.y = 1.6; } if(p_intermedio.y > 4.0){ p_intermedio.y = 4.0; } // gestionar que no haya demasiados puntos intermedios if(stack.elementos()>2 && fabs(stack.top().z-p_intermedio.z)<1.5){ stack.pop(); } stack.push(p_intermedio); robot_console_printf("cuidadiTOOOOOOORRRR!!\n"); } else{ //robot_console_printf("\n"); }