T reba ll F ina l de G rau
INGENIERÍA ELECTRÓNICA INDUSTRIAL Y AUTOMÁTICA
Simulación de mecanismos de asignación de tareas para sistemas multi-robot
JOSÉ ANTONIO GÁLVEZ MELERO
Tutor
JOSÉ GUERRERO SASTRE
Escola Politècnica Superior
Universitat de les Illes Balears
Í NDICE GENERAL
Índice general i
Acrónimos iii
Resumen v
1 Introducción 1
1.1 Motivación y Contextualización. . . 1
1.2 Objetivos . . . 2
1.3 Estructura de la memoria. . . 2
2 infraestructura hardware y software 5 2.1 Hardware utilizado . . . 5
2.1.1 Robot Pioneer 3DX. . . 5
2.1.2 Ordenador portátil . . . 7
2.2 Software utilizado . . . 7
2.2.1 Características de Robotics Operating System (ROS) . . . 7
2.2.2 Stage . . . 11
2.2.3 Entorno de Stage . . . 11
2.2.4 Limitaciones encontradas durante la implementación. . . 14
3 Algoritmo de evitación de obstáculos implementado 15 3.1 Navegación . . . 15
3.2 Vector Field Histogram (VFH): Vector Field Histogram . . . 17
4 Contenidos e implementación del programa 21 4.1 Implementación del algoritmo VFH . . . 21
4.2 Dirección objetivo . . . 24
4.3 Algoritmo para la recogida de objetos . . . 26
4.4 Nodo trasmisor (Talker) . . . 27
5 Pruebas experimentales y adaptación al robot real 31 5.1 Pruebas sobre el simulador Stage . . . 31
5.2 Pruebas sobre el robot real PIONEER 3DX . . . 34
5.2.1 RosAria. . . 35
5.2.2 Adaptación al entorno real . . . 37
5.2.3 Resultado final de las pruebas sobre el robot real . . . 40
ii ÍNDICE GENERAL
6 Conclusiones y trabajos futuros 47
6.1 Conclusiones . . . 47 6.2 Trabajos futuros . . . 48
A código 49
A.1 move_robot2.cpp . . . 49 A.2 Talker.cpp . . . 63 A.3 Configuración mundo Stage . . . 66
Bibliografía 69
A CRÓNIMOS
VFH Vector Field Histogram ROS Robotics Operating System SA Sense and Act
SPA Sense, Plan and Act P,SA Plan, Sense-Act
R ESUMEN
El objetivo de este proyecto es implementar los algoritmos necesarios para que un sistema formado por múltiples robots (sistema multi-robot) pueda llevar a cabo la recogida de un conjunto de objetos distribuidos en un entorno. Para llevar a cabo este objetivo se ha implementado un mecanismo de asignación de tareas entre los robots.
Este último consiste en que cada robot se le asigna la recogida del objeto más próximo a él, y a través de un sistema central que se comunica con ellos, se tendrá información sobre los objetos recogidos. Los diferentes algoritmos se han implementado en el en- torno Robotics Operating System (ROS). Como algoritmo de evitación de obstáculos se ha utilizado Vector Field Histogram (VFH). En primer lugar, se ha implementado el sistema sobre el simulador multi-robot Stage y finalmente, sobre el robot PIONEER 3DX.
Palabras clave:ROS, evitación, recogida, objetos, coordinación.
C
APÍTULO1
I NTRODUCCIÓN
1.1 Motivación y Contextualización
En la actualidad, la robótica móvil es un foco muy importante en el ámbito de la in- vestigación ya que se encuentran tanto en la industria, como en diferentes servicios cotidianos. Estos robots pueden estar diseñados para trabajar en el espacio terrestre, marino, aéreo, etc. Normalmente, son sistemas autónomos que ayudan a las personas a trabajar en un determinado ambiente de trabajo.
Tareas que resulten difíciles de llevar a cabo para un único robot, pueden conver- tirse en más asequibles con la ayuda de otros robots. Además, estos sistemas son más robustos a los errores, y ofrecen una mayor flexibilidad. A partir de ahora, a estos siste- mas se les llamará sistemas multi-robot. Para esta asignación de tareas, se pueden usar diferentes mecanismos, en dónde se puedan asignar diferentes objetivos a distintos robots. Se deberían diferenciar entre los mecanismosswarm, conocidos también como
”enjambre", y los mecanismos deauctiono "subasta". Los mecanismos de enjambre no necesitan de coordinación explícita entre los diferentes dispositivos que forman el sistema, ya que simplemente responden a un programa muy simple implementado previamente en cada robot. Estos comportamientos se inspiran en diferentes colonias de insectos. Este comportamiento será el que se verá reflejado en este proyecto, ya que entre los robots no existe una comunicación directa. Por otro lado, en los mecanismos de subasta existe una comunicación entre los robots, ya que se necesita saber que tarea ejecutar en cada momento. Esta comunicación permite a los robots realizar tareas más complejas. Estos últimos no se han tenido en cuenta en este proyecto.
En primer lugar se ha utilizado el simulador Stage para probar los diferentes algorit- mos. Posteriormente, se ha usado el robot real PIONEER 3DX que es el que se encuentra en el laboratorio de robótica de la universidad. A diferencia del robot que se usa en el simulador, donde se ha usado un láser, el robot real tiene 16 sensores ultrasónicos.
Para ello, se deberá adaptar la lectura de los diferentes sensores para que el algoritmo
1. INTRODUCCIÓN
de evitación de obstáculos se comporte de la misma manera en ambas plataformas.
En los siguientes capítulos, se explicará la dificultad de adaptar el simulador al robot real, debido a las limitaciones que presenta Stage, y a las diferencias en la lectura de datos. Para poder conocer la posición del robot, se han usado los clásicos sistemas de odometría, muy usados en los entornos de robótica móvil.
Para interactuar tanto con el simulador, como con el propio robot, se utilizará Ro- botics Operating System (ROS). Este conjunto de librerías permitirá desarrollar este trabajo gracias a las posibilidades que ofrece a la hora escribir el código fuente.
Para poder alcanzar un objetivo sin colisionar con otros obstáculos, se utiliza el algoritmo de evitación Vector Field Histogram (VFH). Este algoritmo, con la ayuda de la información sensorial, se coordinará con un algoritmo para la recogida de los objetos, y será capaz de dirigir nuestro robot para poder evitar los diferentes obstáculos que interfieran en el camino hacia el objetivo. Además, este algoritmo se ha debido adaptar al robot real PIONEER 3DX.
1.2 Objetivos
El objetivo principal de este proyecto será la implementación de los algoritmos para la recogida de objetos sobre el simulador Stage, y a continuación, la adaptación de estos algoritmos sobre el robot real. Para ello, se han realizado las siguientes tareas:
• Implementación del algoritmoVFHpara la evitación de obstáculos.
• Implementación del algoritmo para la recogida de objetos basado enswarm.
• Implementación de un nodo central para comunicar a los robots la situación de los objetos.
• Adaptación de los algoritmos implementados sobre el simulador al robot real.
De esta manera, con la ayuda de los diferentes robots en el simulador, la tarea prin- cipal que se realizará será la coordinación de los robots para una recogida y posterior descarga de todos los objetos dispuestos en la zona de trabajo. Los robots se dirigirán a los objetos que se encuentren a menor distancia en ese instante de tiempo.
1.3 Estructura de la memoria
La memoria consta de 7 capítulos, incluido el presente capítulo de introducción:
• Capítulo 2: Se explican los medios hardware (PIONEER 3DX y ordenador portátil) y software (ROSy el simulador Stage) que se han utilizado para llevar a cabo la ejecución del proyecto.
• Capítulo 3: Comprende la explicación del algoritmo de evitación de obstáculos que se ha implementado.
1.3. Estructura de la memoria
• Capítulo 4: Se explican como se ha llevado a cabo el programa a nivel de código y estructura.
• Capítulo 5: Comprende las pruebas experimentales que se han realizado tanto en el simulador como en el robot real.
• Capítulo 6: Se explican las conclusiones del proyecto y los posibles trabajos futuros que se pueden implementar sobre él.
C
APÍTULO2
INFRAESTRUCTURA HARDWARE Y SOFTWARE
A continuación se describirán los elementos hardware (Robot PIONEER 3DX y ordena- dor portátil) y software (ROSy simulador Stage) que se han utilizado para llevar a cabo el proyecto.
2.1 Hardware utilizado
2.1.1 Robot Pioneer 3DX
Para la ejecución del proyecto, a parte de la utilización del simulador Stage, se ha utilizado el robot PIONEER 3DX. Para la comunicación entre el programa y el robot se ha usado el puerto serie.
Como características principales del PIONEER 3DX, se pueden ver las siguientes:
está compuesto por dos ruedas diferenciales de dos motores y ofrece un peso ligero para su fácil maniobrabilidad. Este robot puede incorporar nuevos elementos como cámaras, GPS, etc. Este viene con 16 unidades de ultrasonidos incorporadas en su estructura, 8 dispuestas en su parte delantera y otras 8 en su parte trasera. El rango de lectura de estos sensores va de 10cm hasta 4m de distancia y tienen una frecuencia de lectura de 25Hz. Las dimensiones del robot se pueden ver en la figura2.2.
A pesar de su sencillez, los robots PIONEER son uno de los robots móviles más utilizados para temas de investigación, debido a su versatilidad y fiabilidad y la facilidad que ofrecen para incorporar nuevos dispositivos como son cámaras, láser o GPS[1].
El robot PIONEER 3DX que se dispone en el laboratorio y que podemos ver en la figura2.1tiene un microprocesador HITACHI H8S/2357 de 18MHz, e incluyen un microprocesador con ARCOS. ARCOS se corresponde con el software que permite el control del robot y proporciona la estructura cliente/servidor. Para realizar la co- municación entre el ordenador y el robot se usarán las librerías ARIA. Estas librerías proporcionan el soporte para comunicaciones a través del puerto serie RS-232.
2. INFRAESTRUCTURA HARDWARE Y SOFTWARE
Figura 2.2: Dimensiones PIONEER 3DX
Figura 2.1: PIONER 3DX que se usará en el proyecto
Como especificaciones más importantes, se podrían mencionar las siguientes:
• Su estructura esta formada por aluminio de 16 mm recubierto con polvo.
• Los neumáticos están compuestos de caucho de espuma.
• Esta estructura constituye un peso de 9kg, cuando este está en ausencia de baterías, por lo que podemos ver la ligereza del robot mencionada anteriormente.
• Por lo que hace al movimiento diferencial, tiene un radio de giro de 26.7 cm, y puede alcanzar una velocidad en línea recta de hasta 1.2 m/s. Las ruedas pueden llegar a una velocidad de rotación máxima de 300º/s.
• Puede navegar de forma autónoma entre 8 y 10 horas con ayuda de las 3 baterías.
• La inclinación máxima en la que pueden trabajar es de un 25 %. No obstante, cuando el robot contiene baterías, alcanza un peso de 17 kg, por lo que dificulta el trabajo en inclinaciones próximas al 25 %.
2.2. Software utilizado
Figura 2.4: Ordenador utilizado 2.1.2 Ordenador portátil
Para la realización del proyecto se ha situado sobre el robot un ordenador Sony Vaio como el que se aprecia en la figura2.3con un procesador Intel(R) Pentium(R) de 1.80 GHz, utilizando un sistema operativo Ubuntu 16.04 (figura2.4).
Figura 2.3: Disposición del ordenador sobre el robot
La elección del sistema operativo viene ligada a la herramientaROS, en la cual únicamente se pueden desarrollar proyectos a través de Linux. A la hora de utilizar este sistema operativo, se optó por la partición de disco entre Linux y Windows, ya que el sistema operativo por defecto que incluía este ordenador era el sistema Windows.
2.2 Software utilizado
Robotics Operating System (ROS) es un conjunto de librerías software y herramientas que nos ayudan a construir diferentes aplicaciones para robots. Durante este capítulo, se explicarán como funcionan estas herramientas, como se crea la interacción tanto con el simulador Stage como con el robot real, y también se mencionarán las limitaciones que presentan.
2.2.1 Características deROS
Como antes se ha mencionado, se trata de un conjunto de librerías, que presentan una gran flexibilidad y permiten desarrollar diferentes aplicaciones software para robots.
Como objetivo principal,ROStiene la finalidad de simplificar la tarea de crear un comportamiento completo para un robot, a través de la amplia variedad de plataformas robóticas.
Este marco de trabajo constituye un gran proyecto que se empezó en la Univer- sidad de Stanford a mediados del 2000, que incluían el robot Stanford AI Robot y el
2. INFRAESTRUCTURA HARDWARE Y SOFTWARE
programa Personal Robots. A través de estas herramientas iniciales, se crearon dife- rentes prototipos de sistemas software destinados al uso robótico. En 2007, Willow Garage proporcionó recursos para ampliar estos conceptos y seguir desarrollando el proyecto[2]. Con el tiempo, el software se fue desarollando utilizando la licencia de código abierto, hasta convertirse en una plataforma ámpliamente utilizada en la comu- nidad de investigación robótica.
EnROSse puede trabajar en diferentes lenguajes de programación. Para este pro- grama se ha usado la version Kinetic deROSusando código en C++, pero también hubiera sido posible usar otros como por ejemplo Python o Lisp. Además, en la robótica actual es muy frecuente este marco de trabajo ya que permite la ejecución de progra- mas con un tiempo de ejecución largo, o otros que están constituidos por programas muy costosos computacionalmente.ROStambién permite la ejecución de diversos nodos a la vez, hecho que será muy importante en este proyecto.
A través de un conjunto de procesos, se simula un núcleo que es el encargado de gestionar los diferentes procesos de nuestros dispositivos. Está constituido por un conjunto de paquetes, que lo forman un número de nodos independientes controlados por el núcleo antes mencionado. Estos nodos se pueden comunicar entre ellos utili- zando la estructura Publicador/Suscriptor. Para formarse más concretamente en este ámbito, se debe visitar la página web oficial, donde se encuentran una gran cantidad de tutoriales[3].
Figura 2.5: Esquema de funcionamiento
En la figura2.5se muestran cuatro nodos activos. A continuación, se explican más concretamente los conceptos[4] que constituyen la estructura deROSy que se pueden
2.2. Software utilizado
apreciar en la figura2.5:
• Nodo: El nodo es el encargado de realizar la computación enROS. Dentro de un sistema de control podemos encontrar varios nodos, donde cada uno se encargue de realizar una tarea específica de nuestro robot. En este caso, uno de los nodos, por ejemplo, seria el nodo que nos permite enviar los puntos de los objetos a los diferentes robots.
• Tópicos: Cuando hablamos de tópicos se hace referencia a todos los canales que permiten comunicarnos y transmitir datos con el robot. Cada uno de los tópicos puede tener varios suscriptores. Esta herramienta nos proporciona información sobre algún tópico o publicar datos directamente sobre la red. Los comandos más característicos ligados a esta herramienta son los siguientes:
– rostopic list: Muestra información sobre los tópicos que se encuentran activos. Si escribimos esta instrucción durante la ejecución del programa, el resultado es el que se puede ver en la figura2.6:
Figura 2.6: rostopic list del programa
Más concretamente en la figura2.6se pueden apreciar todos los tópicos activos en el momento en que se ejecuta el programa. De esta manera, se puede obtener información de diferentes características del robot 1, por ejemplo, como la velocidad, a través del tópico/robot_1/cmd_vel, o también de la posición en la que se encuentra en ese momento, mediante el tópi- co/robot_1/odom. Para obtener esta información se utilizará el comando rostopic echo, que se explicará a continuación.
– rostopic echo /topic: Nos muestra la información de un tópico en particular.
Si realizamos esta función para el tópico del láser del programa, obtenemos la respuesta que vemos en la figura2.7:
2. INFRAESTRUCTURA HARDWARE Y SOFTWARE
Figura 2.7: rostopic echo del láser del robot
En esta figura se muestran las características y valores del sensor láser.
angle_minyangle_maxcorresponden con el ángulo máximo y mínimo de lectura del sensor. Además, el valor deangle_incrementmuestra cual es el incremento de ángulo en el cual el sensor vuelve a tomar una medida.
De esta manera, todos los valores que recoge este tópico se muestran en el arrayranges, donde estos valores varían entre 0 y 8.
– rostopic pub /topic type args: Nos permite publicar datos sobre un tópico a través de la línia de comandos y utilizando un formato específico para cada tópico en concreto.
• Servicios: EnROS, si lo que queremos es enviar directamente información de un nodo a otro, necesitamos realizar un servicio. De esta manera, se constituye la estructura cliente/servidor que nos permitirá establecer una conexión entre dos nodos en la cual los demás nodos no tendrán acceso. Los comandos más utilizados para este concepto son los siguientes:
– rosservice list: Se muestran los servicios que están activos.
– rosservice info /service: Aporta la información sobre un servicio en concreto.
En la figura2.5, el nodo A se encarga de transmitir un mismo mensaje a dos nodos diferentes, a través de dos tópicos (tópico 1 y tópico 2). Para que los nodos B y D reciban la información de este mensaje, ambos deben suscribirse a sus tópicos correspondien- tes. Además, el nodo B solicita un servicio al nodo C, el cual le da respuesta gracias al servicio que se ha creado entre ambos.
También se permite el registro de todos los mensajes que han sido publicados en todos los tópicos. Estos mensajes se pueden guardan en un archivo de tipo bag. De esta manera, al sumar todas estas características que reúneROS, se consigue que sea el entorno más adecuado para desarrollar diferentes proyectos en robótica, y en este caso, el proyecto que se describe en esta memoria.
2.2. Software utilizado
Además de los comandos que se han mencionado anteriormente, existen otras órdenes que se usan más frecuentemente y que suelen ser muy importantes a la hora de llevar a cabo la ejecución de un proyecto.
• roscd: Nos permite cambiar a un directorio de un paquete o pila.
• roscore: Probablemente la más importante, ejecuta todos los procesos necesarios para dar soporte a la ejecución completa deROS. Esta orden siempre tiene que estar activa ya que nos permitirá la comunicación entre los nodos.
• rosrun: Permite ejecutar una aplicación de un paquete sin la necesidad de cam- biar su directorio.
• catkin_make: Este comando es el encargado de la compilación de todos los paquetes. Debe ejecutarse en la carpeta donde tenemos nuestro proyecto.
2.2.2 Stage
Stage es un software libre que a través deROS, nos permite llevar a cabo la simulación de nuestro proyecto. Proporciona un mundo virtual, el cual podemos editar a nues- tro gusto, distribuyendo varios robots con sensores, objetos y otros elementos de un entorno[5], tal y como se puede ver en la figura2.8. Para ligar estos robots al código principal, se han utilizado los tópicos deROS.
Aunque se trate de un modelo simple, los creadores proporcionaron las herramien- tas necesarias para que fuera lo suficientemente realista como para permitir simular una gran variedad de proyectos de diferentes dificultades computacionales.
2.2.3 Entorno de Stage
En el siguiente apartado se describirá como se ha llevado a cabo todo el entorno de simulación para realizar el proyecto.
Como se ha mencionado anteriormente, Stage nos permite modificar diferentes entornos a nuestro gusto, para poder añadir las características necesarias para realizar el proyecto[5]. En el entorno de la figura2.8, se han añadido dos PIONEER 2DX, debido a que el uso de los PIONEER 3DX no estaban disponibles para esta versión de Stage. No obstante, ambos robots son muy parecidos, y se permitirá el uso del robot real PIONEER 3DX para las pruebas sobre el robot real. Además, se han añadido 5 objetos distribuidos en diferentes posiciones del entorno. En la figura2.8podemos ver el resultado de uno de los entornos que se ha generado para el proyecto.
2. INFRAESTRUCTURA HARDWARE Y SOFTWARE
Figura 2.8: Entorno creado en Stage para la simulación
Como se puede ver en la figura2.8, los objetos corresponden con los puntos de color verde que se ven en el mapa. Este entorno se ha implementado a través de un fichero de configuración llamadoexemple_1.world, donde se pueden ir modificando tanto las características de los robots como las de los objetos. Estos objetos están ligados a imágenes, que en Stage se llamanbitmaps, que son las que se muestran en el entorno final. Los objetos se han declarado como "planos"de planta, llamados"floorplans"y en el siguiente fragmento de código se muestra un ejemplo de como configurar un objeto para añadirle diferentes propiedades:
floorplan2 (
name "negro"
size [0.15 0.15 0.15]
pose [ -6.0 0.5 0 0 ] color "green"
bitmap "bitmaps/negro.png"
gripper_return 1 )
En este fragmento de código se puede apreciar como se ha declarado el objeto que corresponde con la posición x = -6.0 e y = 0.5. Se le han asignado las siguientes características:
• Nombre: En este caso "negro".
2.2. Software utilizado
• Tamaño del objeto: Correspondiente con el valor de size. Como se puede apreciar, se ha asignado un tamaño lo más realista posible comparado con el tamaño del Pioneer.
• Posición: En "pose"se ha asignado la posición en la que estará el objeto en el entorno.
• Color: Corresponde con el color del objeto, en este caso, se ha asignado el color verde ya que es un color llamativo y que se diferencia del gris de los obstáculos del entorno.
• Bitmap: Bitmap corresponde con la imagen que enlaza Stage con el objeto. De es- ta manera, negro.png corresponde con la imagen de un cuadrado de color negro, que se ha enlazado con este mapa para crear el objeto que se está describiendo.
• gripper return: En este fragmento de código se indica que el objeto es habilitado a la recogida por parte del robot. No obstante, esta nueva versión de Stage no ha permitido la simulación de la recogida mediante pinzas del objeto.
Los demás objetos se han declarado al igual que este, cambiando los valores de
"pose", para poder distribuirlos por diferentes lugares del mapa. A continuación se puede apreciar como se ha llevado a cabo la descripción del robot en la simulación:
pioneer2dx (
pose [ -4.0 -6.000 0 0 ] color "blue"
sicklaser (
# plug the ../examples/ctrl/lasernoise.cc module into this laser ctrl "lasernoise"
alwayson 1 # don’t wait for a subscriber )
gripper(pose [0.22 0 0 0]) gripper
(
#paddle_size[ 0.66 0.1 0.4 ]
#paddle_state["open" "down"]
autosnatch 1
# size[ 0.2 0.3 0.2 ] pose [ 0.22 0 0 0 ] )
)
Los valores que se han asignado han sido los siguientes:
• Pose: Al igual que los objetos, a los robots se le ha asignado una posición inicial.
En esta también se le ha asignado un valor de la orientación inicial, correspon- diente con el último valor del vector "pose".
2. INFRAESTRUCTURA HARDWARE Y SOFTWARE
• Sicklaser: Se ha declarado el láser. Debido a queVFHutiliza los valores del lá- ser, no se han podido utilizar los ultrasonidos en el simulador. No obstante, se ha simulado el comportamiento de estos a través del láser, para la posterior incorporación en el robot real. Este procedimiento se explicará más adelante.
• gripper: Finalmente, se han declarado las pinzas. No obstante, como se ha men- cionará, Stage no ha permitido la utilización de estas a través del simulador.
Por último, para la puesta a punto del entorno, se ha procedido con los siguientes comandos:
• En primer lugar, se selecciona el directorio donde tenemos nuestro fichero de configuración correspondiente con el código que hemos configurado del mapa:
• Finalmente, en esta carpeta, usamos el siguiente comando para compilar nuestro entorno, que en este caso tiene el nombre deexemple_1.world:
rosrun stage_ros stageros exemple_1.world
Finalmente, el programa se iniciará automáticamente en el simulador si se han asignado los tópicos de los robots con el mismo nombre que tienen en Stage.
2.2.4 Limitaciones encontradas durante la implementación
Durante la elaboración del proyecto se han encontrado ciertas limitaciones en el simu- lador. Stage únicamente nos permite la incorporación de un único sensor a nuestro robot simulado, de esta manera, no se ha podido incorporar al robot un modelo de sensores de 16 ultrasonidos. Por lo tanto, el robot en Stage cuenta con un láser para la detección de obstáculos. Durante el capítulo 4 se explicará como se han solucionado estas limitaciones. Además, no ha sido posible la incorporación de unas pinzas para llevar a cabo la simulación de recogida de objetos y posterior descarga. Para suplir este hecho, el robot se dirigirá hacia el objeto, y cuando esté en esa posición, volverá hacia la zona de descarga. Todas estas limitaciones se han encontrado a medida que se iba elaborando el proyecto.
Además, como antes se ha explicado, se ha utilizado el robot PIONEER 2DX en el simulador, aunque finalmente se ha podido adaptar el programa para usar el robot real PIONEER 3DX que se dispone en el laboratorio. Esta adaptación se explicará en el capítulo 5.
C
APÍTULO3
A LGORITMO DE EVITACIÓN DE OBSTÁCULOS IMPLEMENTADO
En este capítulo se explicará el algoritmo que se ha llevado a cabo para la evitación de obstáculos.
3.1 Navegación
En este proyecto, todos los robots deben ir recogiendo los diferentes objetos que hay en el mapa, y de esta manera, se necesita de un algoritmo para llegar a ellos sin colisionar.
Cuando hablamos de navegación, hacemos referencia al conjunto de procesos orienta- dos a encontrar un camino que una un punto de origen con un punto de destino y su correcto seguimiento sin colisiones.
Como bien se sabe, la navegación es uno de los elementos más importantes para realizar con éxito diferentes aplicaciones de sistemas de robots móviles. Todos estos robots cuentan con algún tipo de mecanismo para evitar colisiones, algoritmos que detectan un obstáculo y realizan una serie de operaciones para no colisionar con ob- jetos. Los algoritmos que se van desarrollando en los últimos años son cada vez más complejos, debido a que incluyen otros aspectos como la desviación de los obstáculos, a diferencia de otros algoritmos que solo incluyen la detección de estos [6].
Para organizar todo este tipo de procesos, se necesita de establecer un tipo de arquitectura. Una arquitectura nos permite organizar todas las diferentes funciona- lidades que puede realizar un robot: creación de un mapa, planificación y evitación de obstáculos. Según el comportamiento de estos paradigmas, podemos destacar 3 arquitecturas[6]:
• Sense, Plan and Act (SPA)
• Sense and Act (SA)
3. ALGORITMO DE EVITACIÓN DE OBSTÁCULOS IMPLEMENTADO
Figura 3.2: Etapas en la navegaciónSA
• Plan, Sense-Act (P,SA) (híbrida)
En este caso, la arquitectura implementada no es exactamente la llamada Sense and Act (SA), sinó que se trata de una arquitectura híbrida. La arquitecturaSAes la que comúnmente se llama arquitectura basada en comportamientos. Este tipo de arquitec- tura está inspirada en la fisiología animal, donde el robot responde a los obstáculos de una forma reactiva. En la figura3.2se pueden apreciar las etapas de la navegación. Este modelo tiene diferentes ventajas y desventajas respecto a los demás antes mencionado.
Como ventaja principal, podríamos destacar que su coste computacional no es tan ele- vado como en el resto, ya que no se necesita un conocimiento exhaustivo del entorno, y por lo tanto, no se necesita de un mapa del entorno.
No obstante, como desventaja principal se encuentra el hecho de la difícil sincroni- zación entre los diferentes comportamientos, además de la gran dependencia en los sensores, ya que constantemente se necesita recurrir a ellos para realizar un compor- tamiento u otro. Además, la ausencia de la planificación puede ser un problema si el tiempo de muestreo de los sensores no es suficientemente elevado como para detectar los obstáculos en todo momento. Por otro lado, en una arquitecturaSPA, se obtienen primeramente los valores de los sensores, a continuación se planifica cual es la mejor opción de navegación, y finalmente se ejecuta ese comando de actuación (figura3.1).
De esta manera, en el proyecto no se utiliza una arquitecturaSA, niSPA, sinó que es más bien una arquitectura híbrida debido a las características de implementación del proyecto.
Figura 3.1: Etapas para la navegaciónSPA
Existen muchos algoritmos de evitación de obstáculos, como por ejemplo los al- goritmos Bug o el algoritmo de evitación por campo de potencial. No obstante, para este proyecto, se ha decidido usar el algoritmo Vector Field Histogram (VFH) que se explicará a continuación.
3.2. VFH: Vector Field Histogram
3.2 VFH: Vector Field Histogram
El algoritmo Vector Field Histogram fue propuesto y desarollado por Johann Borenstein y Yoram Koren durante el año 1991. Permite la detección de obstáculos que se descono- cen, y que por lo tanto, necesitan de su conocimiento a través de los sensores que lleva a bordo el robot, y posteriormente evita las colisiones.
VFHusa un diagrama en coordenadas cartesianas en dos dimensiones, que co- múnmente se llama Histogram Grid. Este histograma se divide en diferentes celdas, y continuamente se va actualizando a través de la información que proporcionan los sensores[7].
Cabe diferenciar dos procesos para reducir la información y de esta manera poder computarla para posteriormente ajustar una velocidad determinada y una orienta- ción para poder evitar el obstáculo. En un primer proceso, se reduce el anteriormente mencionado Histogram Grid a un vector de una sola dimensión, llamado Historgrama polar, donde cada índice del vector corresponde con un sector de alrededor del robot, completando un ángulo de 360º. Dentro de cada uno de estos sectores, se encuentra un valor que corresponde con la densidad polar del obstáculo. A través de este primer proceso, se puede realizar el segundo proceso, en el cual se selecciona una dirección hacia el objetivo según las densidades polares más bajas.
Histogram Grid
Primeramente, como antes se ha mencionado, se divide la información del entorno en una primera cuadrícula llamada Histogram Grid como se puede ver en la figura 3.3. El número de celdas de la cuadrícula vendrá determinada según la resolución que se quiera obtener en el resultado del algoritmo. Cabe destacar que este número de celdas marcará las decisiones del algoritmo, además del coste computacional. Además, la cuadrícula del histograma puede ser global del mapa, o solamente del entorno del robot.
Figura 3.3: Histogram grid
3. ALGORITMO DE EVITACIÓN DE OBSTÁCULOS IMPLEMENTADO
Polar Obstacle Density
A partir de los valores que se obtienen de las celdas (Certainty values), se procede con el calculo del vector polar. Primeramente, se debe definir la ventana activa. Esta ventana es la que proporciona el robot a través de la lectura de los sensores, e indica que celdas se deben considerar para el cálculo de este vector. A cada sector se le asigna un valor k, de tal manera que si H corresponde con el vector polar obstáculo,hk representa al vector para el sector K. Para calcular estos sectores se usa la siguiente formula:
hk=X
i,j
mi,j
mi,j=(c∗i,j)2(a−bdi,j) donde:
• A y b corresponde con dos constantes, positivas y que cumplen la condición a-b(w-1)/2)=1. W corresponde con el tamaño de la ventana activa.
• Por otro lado,ci,j∗ es el Certainly value de esa celda activa (i,j).
• dijse corresponde con el tamaño de la celda activa (i,j) y el centro del robot.
• Finalmente,mi,jes la magnitud del vector polar obstáculo en la celda (i,j).
En la figura3.4se puede apreciar un ejemplo del polar obstacle density:
Figura 3.4: Polar obstacle density
Posteriormente, se establece una serie de direcciones candidatas, a través de los vectores que anteriormente se han calculado. De esta manera, se obtendrán direcciones válidas y direcciones no válidas. Estas direcciones válidas constituyen un umbral, y el conjunto de estos sectores es el llamado ’valle’ candidato. Por lo tanto, se debe de escoger el mejor ’valle’ candidato de entre todos los posibles.VFHhace una diferencia entre dos clases de valles, según el número de sectores consecutivos que lo forman.
3.2. VFH: Vector Field Histogram
Para finalizar con el algoritmo, se debe calcular la velocidad que debe llevar el robot para evitar ese obstáculo. De esta manera, se realizan los siguientes cálculos:
• H’c corresponde con el valor del vector polar obstáculo en la dirección que se ha decidido tomar.
• El cambio de velocidad vendrá calculado a través de la fórmula:
V0=V max(1−H"c/H m) donde
H"c=mi n(H0c,H m)
siendo Hm una constante que causa una suficiente reducción de la velocidad del robot para evitar la colisión.
• Finalmente, la velocidad del robot vendrá determinada por:
V=V0(1−Ω/Ωmax)+V mi n
dondeΩes la velocidad de giro que actualmente lleva el robot, yΩmax es la velocidad máxima de giro que puede alcanzar el robot.
Este algoritmo presenta muchas ventajas comparado con otros algoritmos de evi- tación de obstáculos. Se ha demostrado que absolutamente en todos los casos,VFH produce caminos óptimos a la hora de dirigirse hacia un objetivo. Además,VFHes uno de los algoritmos más usados actualmente para la evitación de obstáculos en la robótica móvil, junto con el algoritmo de ventana dinámica.
Cabe destacar que a partir de este algoritmo, se han ido realizando modificaciones para resolver algunas necesidades que presentaba el algoritmo original. De ahí surge VFH+, donde a diferencia deVFH, en este se tiene en cuenta también la cinemática del robot.
C
APÍTULO4
C ONTENIDOS E IMPLEMENTACIÓN DEL PROGRAMA
Durante este capítulo se explicarán los diferentes algoritmos que se han implementado para llevar a cabo el objetivo del proyecto. Estos se dividen en algoritmoVFH, programa para la comunicación con los robots y envío de posiciones de los objetos y el mecanismo de asignación de los objetos a recoger.
4.1 Implementación del algoritmo VFH
Para implementar el algoritmoVFH, se han llevado a cabo modificaciones sobre un algoritmo original [9]. A través de los cambios realizados, se ha conseguido adaptar el código a los objetivos del proyecto. Este algoritmo será el encargado de realizar la evitación de obstáculos. De esta manera, se debía combinar el algoritmo junto con el objetivo principal del proyecto, el cual es la coordinación entre los robots para la recogida de los diferentes objetos.
Para utilizar el algoritmoVFH, primeramente se deben inicializar los parámetros deVFH, como son la velocidad máxima o el rango máximo de lectura del láser. En esta inicialización, que se realiza en el nodo principal, además de establecer los paráme- tros, se deben asignar los tópicos de odometría, lectura del láser y el publicador de la velocidad del robot. A partir de ahora, todos los ejemplos que se explican se realizan con dos robots aunque el proyecto es extensible a un número mayor de robots. Por lo tanto, como se usan dos robots, se ha realizado el siguiente procedimiento para poder atribuir de manera correcta estos tópicos:
if(rob_id==1){
cmd_vel_topic_name +="1";
odom_topic_+="1";
scan_topic_+="1";
4. CONTENIDOS E IMPLEMENTACIÓN DEL PROGRAMA
}else{
cmd_vel_topic_name +="0";
odom_topic_+="0";
scan_topic_+="0";
}
cmd_vel_topic_name += "/cmd_vel";
odom_topic_+="/odom";
scan_topic_+="/base_scan_1";
Como se puede apreciar, primeramente se lee el valor del id del robot que se intro- duce a través de la consola. Este valor se guarda en la variablerob_idy según este valor, se construye un string o otro dependiendo del robot, que posteriormente será el tópico que leeráVFH. Además, se puede ver como también se guarda el tópico de velocidad, correspondiente al stringcmd_vel_topic_name:
scan_subscriber_ = nh_.subscribe(
scan_topic_, 1, &VFH_node::scanCallback, this);
printf("Subscriber=%s\n",scan_topic_.c_str());
odom_subscriber_ = nh_.subscribe(
odom_topic_, 1, &VFH_node::odomCallback, this);
vel_publisher_= nh_.advertise<geometry_msgs::Twist>(cmd_vel_topic_name,5);
printf("vel publisher=%s\n",cmd_vel_topic_name.c_str());
Nh_corresponde con el nombre del nodo,scan_topic_es el tópico del láser y odom_topic_el tópico de odometría. A continuación, estos parámetros son leídos por los métodos deVFH, el cual devuelve unos comandos de giro (chosen_turnrate) y velocidad (chosen_speed) que a continuación son leídos en el nodo principal:
m_vfh->Update_VFH(m_laser_ranges, (int) (m_robotVel), desiredAngle + 90.0, desiredDist, currGoalDistanceTolerance, chosen_speed,
chosen_turnrate);
geometry_msgs::Twist cmd_vel;
if(chosen_turnrate!=0){
cmd_vel.linear.x=(float)(chosen_speed)/1000.0;
cmd_vel.angular.z= DEG2RAD(chosen_turnrate);
vel_publisher_.publish(cmd_vel);
ROS_DEBUG("chosen_speed %d, chosen_turnrate %d", chosen_speed, chosen_turnrate);
salto=1;
}
4.1. Implementación del algoritmo VFH
Como se puede apreciar, estos parámetros son los que se asignan a los tópicos del robot. No obstante, este método se ha modificado de tal manera que el robot se dirija también hacia el objetivo, a la vez que evita los obstáculos. Se ha modificado el algoritmoVFHañadiendo la variable salto, la cual permite al programa realizar una lectura o no de estos parámetros. Más concretamente, cuando el valor de salto es 1, significa queVFHha ordenado el giro debido a que se ha detectado un objeto. No obstante, cuando la variable tiene el valor 0, significa que no hay ningún obstáculo en el camino y de esta manera se puede girar y avanzar según se haya calculado para poder dirigirse al objetivo. Así se atribuye una mayor prioridad al algoritmoVFH. El robot únicamente se dirigirá hacia el objetivo siempre y cuando la variablechosen_turnrate correspondiente al giro enVFHsea de valor 0.
Adaptación del láser a los ultrasonidos
El algoritmoVFHse ha realizado para ser usado en un simulador donde el robot dis- ponga de un sensor láser. No obstante, se ha debido adaptar este sensor a los 8 sensores ultrasónicos que tiene el robot real PIONEER 3DX en la parte frontal. Sobre la lectura del láser se han realizado las siguientes operaciones:
• En primer lugar, se ha dividido el array de valores de la lectura del láser en 8 porciones, que corresponde con el número de ultrasonidos de la parte frontal del robot.
• De cada una de estas partes, se ha cogido el valor de lectura de menor alcance.
• Finalmente, este valor se le ha otorgado a todos los valores del array que corres- ponden a esa porción, como se puede ver en la figura4.1.
Figura 4.1: Adaptación de láser a ultrasonidos
4. CONTENIDOS E IMPLEMENTACIÓN DEL PROGRAMA
4.2 Dirección objetivo
Uno de los puntos más importantes de este proyecto, como bien se ha explicado, es la correcta orientación hacia los objetos para su posterior recogida.
Primeramente, en el nodo principal se ha llevado a cabo la suscripción a la odome- tría del robot. Este hecho es muy importante ya que de esta manera se consigue saber en todo momento cual es la posición del robot, además de la orientación de éste en todo momento. En el métodocallBackOdomse leen las coordenadas que tiene el robot en ese momento. Los pasos para calcular la dirección que debe tomar el robot para dirigirse al objetivo son los siguientes:
• En primer lugar, se leen las coordenadasxeydel robot.Posejuntamente con positionextraen la posición estimada del robot en el marco odométrico. A través de ellas, finalmente se indica que componente se quiere obtener,xoy. Para realizar esta tarea se usa la siguiente instrucción:
Xrobot=odom.pose.pose.position.x;
Yrobot=odom.pose.pose.position.y;
Esta lectura será posteriormente utilizada para calcular la distancia que hay entre la posición actual del robot y los diferentes objetos.
• A continuación, se procede con la lectura de la orientaciónzywdel robot. Tanto zcomowrepresentan una orientación en el espacio expresada en cuaterniones.
Se realiza el mismo procedimiento, no obstante, como estas componentes per- tenecen a orientación y no posición, se sustituyepositionpororientation. Esta lectura es necesaria para el cálculo de la orientación actual del robot, que se usará posteriormente para orientar el robot hacia el objeto a recoger [10]:
current_z=odom.pose.pose.orientation.z;
current_w=odom.pose.pose.orientation.w;
angulo=(2*atan2(current_z,current_w));
• Por último, se llaman a dos métodos, por una parte,callBackAnguloque se en- carga de calcular el ángulo necesario para dirigirse hacia ese objetivo, y por otra parte,rotateRobot, que es el encargado de calcular la orientación hacia un objeto fijado. Para calcular el angulo objetivo expresado en grados se deben realizar una serie de operaciones:
– El primer paso es calcular laxeyfinales correspondientes al objeto, y tenien- do en cuenta la posición actual del robot. Para esta operación simplemente se ha realizado la diferencia entre la variablexcorrespondiente al objetivo y la variablexcorrespondiente al robot en ese instante. La misma operación se realiza para la componente y:
Xfinal=xObjetivo-Xrobot;
Yfinal=yObjetivo-Yrobot;
4.2. Dirección objetivo
– Cuando se tienen ambas coordenadas, se debe saber a que cuadrante perte- nece cada punto final. Para ello, se evalúan los signos de ambas componen- tes, y se asigna un valor a una variable, llamadacambiola cual nos indicará que factor deberemos añadir o quitar para obtener el valor que queremos.
A continuación, calculamos el ángulo que forman ambas componentes, realizando la operaciónang l e=at an2(Y f i nal,X f i nal). Por último, la variableanguloDEGobjetivoserá la que nos indique el ángulo al que debe orientarse el robot para digirse al objetivo.
Por lo tanto, en cada momento se va realizando este bucle para actualizar el angulo objetivo y de esta manera rotar siempre el robot en la dirección correcta.
Cuando se ha calculado tanto el ángulo actual como el ángulo objetivo, se procede a ejecutar el giro correspondiente para orientarlo de manera correcta hacia el objeto.
Este apartado es el que se realiza en el métodorotateRobot. En primer lugar, guar- damos en una variable el ángulo del robot en ese mismo instante. Ese ángulo lo expre- samos en grados según el criterio de la figura4.2:
Figura 4.2: Ejes de coordenadas del robot
A continuación, cuando ya sabemos el ángulo actual del robot, lo comparamos con el ángulo que debe tener para orientarse hacia el objeto. Para la comparación, se realiza una resta entre ambos, y si el valor absoluto de ésta es inferior a un umbral predeterminado, se considera que ya está bien orientado y por lo tanto simplemente se le aplica una velocidad lineal y una rotación a lo largo del eje z de valor 0.
En cambio, si esta condición no se cumple, significa que el robot tiene que reorien- tarse para ir hacia el objetivo. De esta manera, se necesita de saber en que sentido se debe rotar, para conseguir que el giro sea el mínimo. Para saber este sentido, se realiza el seno de la diferencia entre ambos ángulos. Si este resultado da mayor a 0, se rota en sentido negativo (de ángulos mayores a menores), en caso contrario, se rota en sentido
4. CONTENIDOS E IMPLEMENTACIÓN DEL PROGRAMA
positivo. Gracias a esto, siempre se rotará lo mínimo para orientarse en un ángulo concreto.
si (sin(angulo actual - angulo objetivo)>0){
rotar hacia izquierda;
}sino{
rotar hacia derecha;
}
Para la velocidad de rotación se ha realizado un controlador proporcional, de esta manera, la velocidad depende de una constante que multiplica la diferencia, en valor absoluto, que hay entre el ángulo objetivo y el ángulo actual:
vr ot aci on=K·∆Φ
Además, para hacer más fluido el movimiento, durante la rotación del robot también se le aplica una velocidad lineal. Esta tiene un valor de una décima parte de la velocidad máxima. Este valor se ha elegido a través de la realización de diferentes pruebas, donde se ha comprobado que es un buen valor para una rotación fluida.
En cuanto al algoritmo implementado para la elección del objeto, se ha decidido que cada robot se dirija al objeto que más cerca tiene en ese instante de tiempo. Esta decisión se ha tomado ya que, como se ha visto, el algoritmoVFHúnicamente evita obs- táculos, y puede que al dirigirse a un objeto, se aleje de este debido a las características del objeto a esquivar. De esta manera, si en todo momento se actualizan las distancias a los objetos, se puede cambiar la dirección y por lo tanto, minimizar el recorrido del robot durante la búsqueda y recogida. Este algoritmo es el que se ha implementado en la funciónpuntoMasCercano(), que se explica en el siguiente apartado.
4.3 Algoritmo para la recogida de objetos
La tarea de recogida de objetos se basa en que los diferentes robots deben dirigirse hacia la zona donde están dispuestos los objetos, a continuación simular su recogida, y finalmente su descarga en una zona común. Para saber a que objeto dirigirse en cada momento, los robots calculan que objeto tienen más cerca en ese momento. En la funciónpuntoMasCercano()se implementa el algoritmo de calcular el punto más próximo al robot. Este ha sido el algoritmo que se ha implementado para decidir a que objetivo se dirige un robot en un instante determinado. Se ha considerado la mejor opción ya que durante la evitación de obstáculos puede que un robot se aleje de un objetivo el cual había sido indicado inicialmente, y que por lo tanto, complique la tarea de recogida de éste. A continuación, se muestran los pasos seguidos para su implementación:
4.4. Nodo trasmisor (Talker)
• Primeramente, se consulta el valor de la variablellegada. Esta variable indica si el robot ha llegado a la zona de recogida del objeto. Esta variable se modifica en la funciónmove_forward.Este valor será muy importante ya que se encargará de que el robot, una vez recogido el objeto, se dirija hacia la zona de descarga.
• A continuación, si el valor de la variablellegadaes "false", significa que el robot no ha alcanzado el objeto y por lo tanto tiene que proceder con el cálculo del punto más cercano. No obstante, anteriormente se evalúa otra variable, llamada descarga. La función de esta variable es indicar si el objeto ha llegado a su punto de descarga o no después de recoger el objeto.
• Cuando el robot no ha alcanzado el objeto, se procede con el cálculo del punto más cercano. Para ello, se evalúan en un bucle todas las distancias entre la po- sición actual del robot, y cada uno de los puntos de los objetos, que están en el array puntosX[] (valores X de los puntos) y puntosY[] (valores Y de los puntos).
Por lo tanto, se va guardando la distancia mínima en una variable, al igual que el objeto que corresponde con esa distancia. Finalmente, el objeto que corresponde con esa distancia será objetivo del robot para ese instante de tiempo.
• Por otra parte, si el valor de la variablellegadaes true, significa que el robot debe dirigirse hacia la zona de descarga, y por lo tanto, no debe evaluar en ese momento cual es el objeto que tiene más cerca. Este será el caso cuando un robot ya haya recogido el objeto. El objetivo que tendrá en ese instante será el punto de descarga común.
Llegada al punto objetivo
Cuando el robot se ha posicionado sobre la zona donde está el objeto, se deben realizar una serie de acciones para indicar que ese objeto se considera como recogido, ya que como se ha visto en las limitaciones de Stage, no se ha podido implementar la recogida de los objetos a través de las pinzas.
Primeramente, se comprueba la distancia entre el punto actual del robot y el punto objetivo. Cuando esa distancia es inferior a un determinado umbral, en este caso 0.2, se considerará que el robot ha conseguido llegar al objetivo y por lo tanto que ha recogido el objeto. Para ello, se deben modificar los valores de las variables descarga y llegada, anteriormente explicadas, y de esta manera, que el robot se dirija a la zona de descarga y no proceda con el cálculo del objeto más cercano hasta que no haya dejado el objeto recogido.
Para indicar que un objeto se ha recogido, se modifica el conjunto de puntos que corresponden a las coordenadas de los objetos, y se introduce el valor 0 al objeto que ha sido recogido.
4.4 Nodo trasmisor (Talker)
Una de los objetivos de este proyecto es la buena coordinación entre ambos robots, de modo que cuando un objeto es recogido, ambos den la recogida de ese objeto como fi- nalizada. Para ello, se ha implementado un nodo que se encarga de recibir información de los robots, además de trasmitirle a estos información sobre los puntos de recogida
4. CONTENIDOS E IMPLEMENTACIÓN DEL PROGRAMA
de los objetos. Este nodo será llamadotalker. Las funciones principales de este nodo son las siguientes:
• Recibir los objetos que han recogido los robots.
• Enviar a todos los robots los puntos modificados según los objetos que se han recogido.
En primer lugar, debemos ligar el nodo principal con este nodo, es decir, debemos realizar la conexión cliente/servidor. Debido a que se usan dos robots, se han creado dos métodos para cada robot, uno para recibir los datos y otro para enviarlos. De esta manera, se evitan problemas de convergencia. El nodo talker publicará a través del tópicochatter_1al robot 1, y se suscribirá a él a través del tópicopoints_1. Se ha llevado a cabo de la siguiente manera:
• Primero se declaran los publicadores hacia los robots. A continuación se muestra el publicador hacia robot 1:
ros::Publisher chatter_pub1 = n.advertise<std_msgs::String>("chatter_1", 1000);
• También se debe realizar la suscripción para poder recibir los datos de los robots.
A continuación se muestra la lectura a través del robot 1:
ros::Subscriber sub1 = n.subscribe("points_1", 1000, mainCallBack1);
En los métodos de lectura, se reciben los valores y posteriormente se enviarán estos valores nuevamente a ambos robots, para que se tenga constancia en todo momento de que objetos se han recogido. El formato de los mensajes que se transmiten se explicará más adelante. En la figura4.3se muestra un esquema para comprender mejor este funcionamiento entre nodos:
Figura 4.3: Esquema de funcionamiento nodo transmisor
4.4. Nodo trasmisor (Talker)
Como vemos,este mismo procedimiento se utiliza para realizar las estas operacio- nes en el robot 0, cambiando los nombres de los suscriptores y publicadores.
Datos a comunicar entre robots
Inicialmente, ambos robots deben conocer la disposición de los objetos. Esta tarea de comunicación es la que realiza el nodo transmisor que se ha explicado anteriormente.
Los datos que se envían son la lista de puntos que debe seguir el robot para encon- trar los objetos. De esta manera, a través de los métodos del nodo talker, se envían los puntos hacia los diferentes robots.
msg.data=objectives;
loop_rate1.sleep();
chatter_pub0.publish(msg);
loop_rate1.sleep();
chatter_pub1.publish(msg);
Primeramente se ha declarado que los mensajes a enviar serán del tipo string y se asigna el string a los datos de envío. A continuación, se realiza un sleep de tal manera que permite al nodo la correcta asignación de los datos y su correcta transmisión. Fi- nalmente, se publica este mensaje a través del comando publish. El string con los datos de los puntos tiene el siguiente formato:
std::string objectives("x1;y1;x2;y2;x3;y3;x4;y4;x5;y5;xDescarga;yDescarga");
Los puntos están ordenados de tal manera que cada 2 puntos corresponden a 1 objeto en el mapa. No obstante, el último par de valores no corresponde con la posición de un objeto, sinó que se interpretará como el punto de descarga de los objetos.
En cuanto a la lectura de la información de los robots, como se ha mencionado antes, esta se realiza a través de ligar un método a cada uno de los robots usando los suscriptores. Esta trasmisión lo que produce es que se actualicen los valores de los puntos en el programa principal, y como se ha mencionado antes, ambos robots tengan constancia de los objetos recogidos.
El proceso de eliminación de un objeto de la ruta, se realiza a través de la anulación de sus coordenadas en el string. De esta manera, esas coordenadas se establecen a los valores 0, y a la hora de evaluar los puntos, el robot identifica que ese objeto ya ha sido recogido en otro momento. No obstante, este hecho no se realiza en el transmisor, sino que es el propio programa del robot que se encarga de transmitir esta información de la recogida del objeto. Debido a las condiciones del entorno, no existe el problema de encontrar un objeto en la posición 0,0 y que este se confunda. No obstante, si hubiera un objeto en esa posición, se podrían cambiar las condiciones para que fuera otro valor el que interpretase como la recogida de un objeto.
En el nodo principal (que se ejecuta en cada robot) se escribe e interpreta el string de puntos explicado anteriormente. Para ello, se realizará la conversión de este string a floats, donde cada uno de estos valores se guardarán en dos arrays diferentes, uno para los valores de las posiciones X de los objetos, y unos para los valores de las posiciones
4. CONTENIDOS E IMPLEMENTACIÓN DEL PROGRAMA
Y. Para realizar esta conversión, primeramente se debe declarar una variable del tipo string, la cual nos servirá para guardar la cadena de puntos inicial. Para realizar la conversión a floats, se utiliza la conocida funciónsscanf, que tiene este formato:
sscanf(datos,"\%lf,\%lf",&puntosX[0],&puntosY[0]);
En este ejemplo, los dos primeros valores del string se guardarían en la posición 0 del array puntosX y puntosY respectivamente. La conversión a floats se ha implementado para poder modificar los arrays de manera más simple y de esta manera tratar las posiciones de los objetos más fácilmente. Además, para realizar la eliminación del objeto que antes se ha mencionado, la modificación del punto se hace sobre el array de floats, que posteriormente se convierte a un string para enviar al nodo transmisor.
C
APÍTULO5
P RUEBAS EXPERIMENTALES Y ADAPTACIÓN AL ROBOT REAL
En el siguiente capítulo se van a mostrar las pruebas realizadas en el simulador Stage, además de los cambios que se han implementado sobre el programa para poder adap- tarlo al robot real. Se explicarán estos experimentos realizados para poder validar el correcto funcionamiento del proyecto juntamente con una secuencia de imágenes que verifican el programa, todo ello realizado tanto sobre el robot PIONEER 3DX como en el simulador Stage.
5.1 Pruebas sobre el simulador Stage
Para comprobar el correcto funcionamiento del programa se han realizado una serie de pruebas en el simulador Stage, además de las pruebas sobre el robot real. De esta manera, se dispone de dos robots, además de los diferentes objetos que corresponden con los cuadros verdes.
Primeramente, se inicializan ambos robots, a través de dos consolas diferentes, asignando el id 0 y el id 1 al ejecutable PAQUETE1 que se encuentra en la siguiente ruta:
catkin3/devel/lib/PAQUETE1. Para llevar a cabo esta acción, introducimos los siguientes comandos en consolas distintas:./PAQUETE1 1 y ./PAQUETE1 0.
A continuación, los robots se quedan a la espera de saber cuál es la disposición de los objetos en el entorno. De esta manera, se ejecutará el nodo transmisor a través de su ejecutable y con el comando./talker, también en otra consola distinta. Cuando se lleva a cabo esta instrucción, los robots reciben los puntos y el programa empieza a realizar los cálculos pertinentes para dirigirse sin esquivar obstáculos al objeto, en este caso y como se ha explicado, al que más cerca tienen en ese instante de tiempo.
De esta manera, según el entorno de la figura5.1el robot 1 (azul) se dirigirá hacia el objeto que se encuentra en la posición (-3,-4) y el robot 0 (rojo) también hacia el objeto de la posición (-3,-4). Finalmente, el robot 1 alcanza este objeto, y comunica al nodo
5. PRUEBAS EXPERIMENTALES Y ADAPTACIÓN AL ROBOT REAL
transmisor que el objetivo ha sido recogido. En la figura5.1también se pueden apreciar las dos consolas que monitorizan cada uno de los robots. La superior corresponde con el robot de color azul, donde se observa el mensaje por pantalla en el cual se comunica que ha alcanzado el objeto. Por otra parte, la consola de la parte inferior monitoriza al robot 0 de color rojo.
Figura 5.1: Imagen 1 Prueba sobre Stage
A continuación, el robot 0 leerá el mensaje, y cambiará de objetivo (Figura5.2), dirigiéndose hacia la posición (-6,0.5) la cual corresponde con el objeto que se puede recoger y que más cerca está de él en ese instante.
Figura 5.2: Imagen 2 Prueba sobre Stage
El robot 1 se dirige hacia la zona de descarga (0,-6), mientras que el robot 0 se dirige hacia el objetivo antes mencionado. A continuación, cuando el robot 0 alcance el objeto, se dirigirá hacia la zona de descarga, como se puede ver en la figura5.3.
A medida que se van acercando hacia nuevos objetivos, se van evitando los obstácu- los a través del algoritmoVFH. Como veremos, podría darse la situación que a medida que se dirige hacia un objetivo, debido a las características del entorno y el algoritmo VFH, el robot actualiza el objetivo debido a que en un instante de tiempo el objetivo que inicialmente había calculado ya no es el que más cerca tiene.
5.1. Pruebas sobre el simulador Stage
Figura 5.3: Imagen 3 Pruebas sobre Stage
A continuación, cuando los robots llegan al punto de descarga, vuelven a realizar el cálculo para asignar un objetivo a recoger, teniendo en cuenta que algunos objetos ya han sido recogidos, y que por lo tanto se deben eliminar de la lista de objetivos pendientes. Finalmente, el programa se acaba cuando todos los objetos que existen en el entorno han sido recogidos y posteriormente descargados en la zona de descarga.
Para complementar mejor estas pruebas, también se ha llevado a cabo la ejecución del proyecto sobre un entorno distinto de Stage. Para ello, se ha creado un nuevo mapa (Figura5.4), donde se han distribuido dos objetos (cuadrados verdes), y donde también se han asignado dos robots para la tarea. Al igual que en la prueba anterior, los resultados obtenidos han sido satisfactorios.
Figura 5.4: Nuevo entorno y distribución para la segunda prueba
Si observamos el comportamiento, vemos que es el mismo que anteriormente. En el instante inicial, ambos robots se dirigen hacia el objeto más próximo (Figura5.5), en este caso el que corresponde con el punto (4,-14).
El robot azul llega a a este punto, comunica que el objeto ha sido recogido, y por
5. PRUEBAS EXPERIMENTALES Y ADAPTACIÓN AL ROBOT REAL
Figura 5.5: Imagen 1 segunda prueba
Figura 5.6: Imagen 2 segunda prueba
lo tanto se dirige a la zona de descarga que en este caso se ha atribuido a la posición inicial del robot. Debido a que el robot azul ha sido el encargado de recoger el objeto, el rojo cambia el objetivo, y se dirige hacia el único objeto que queda disponible por recoger (Figura5.6). Finalmente, se lleva a cabo esta recogida y posterior descarga y finaliza el programa.
5.2 Pruebas sobre el robot real PIONEER 3DX
A continuación se explicarán los pasos para poder realizar las pruebas con el robot real.
5.2. Pruebas sobre el robot real PIONEER 3DX 5.2.1 RosAria
Para realizar la conexión entre el robot y el programa, necesitamos las librerias Ro- sAria del paqueteROS. Estas librerias nos permitirán la comunicación entre el robot PIONEER 3DX y el ordenador. Para la instalación de estas librerías, se procede de la siguiente manera:
• En primer lugar, deberemos crear nuestro directorio de trabajo, en él, además de instalar las librerías de RosAria, también estarán todos los demás nodos del programa. Para realizar esta operación se procede con los siguientes comandos en la consola:
mkdir -p ~/catkin3/src cd ~/catkin3/src
catkin_init_workspace A continuación, compilamos:
cd catkin3 catkin_make
Una vez compilado el entorno, se procede con la descarga del repositorio de RosAria:
cd catkin3/src
git clone https://github.com/amor-ros-pkg/rosaria.git
• A continuación, se configura el entorno shell para habilitar la ejecución de otros comandos específicos de ros:
source ~/catkin3/devel/setup.bash
• Finalmente, con todos los elementos configurados, se procede con la instalación de RosAria:
rosdep update
rosdep install rosaria catkin_make
Antes de explicar el siguiente proceso es necesario haber comprendido los conceptos principales deROS, ya que es necesario de entender los tópicos que nos proporciona RosAria, puesto que posteriormente los deberemos ligar a nuestro programa. Los tópicos más importantes son los siguientes:
• RosAria/pose: Este tópico es el que permite saber en todo momento la informa- ción sobre odometría de nuestro robot. Como se puede apreciar, es diferente al tópico que se usaba en el simulador (robot_1/odompara el robot 1, por ejemplo).
5. PRUEBAS EXPERIMENTALES Y ADAPTACIÓN AL ROBOT REAL
• RosAria/cmd_vel: La función de este tópico es publicar órdenes de velocidad sobre el pioneer.
• RosAria/sonar: Finalmente, este tópico recoge la información que proviene de los sensores de ultrasonido del robot. Para extraer de manera correcta esta informa- ción, primeramente se debe saber que tipo de mensaje nos proporcionará este tópico. En este caso, se trata de un mensaje de tipo PointCloud. Este mensaje lo podemos analizar mejor a través de la instrucciónrostopic echo. Como se puede apreciar en la figura5.7, para cada sensor ultrasónico, se muestran tres valores: x, y ,z.
Figura 5.7: Rostopic echo del sonar del robot
A través del puerto serie se enviará toda la información entre el robot y el programa.
Cabe mencionar que toda esta información puede ser enviada, recibida y procesada gracias aROS. Cuando se ha instalado RosAria, se puede proceder a realizar la comuni- cación para posteriormente realizar las pruebas.
• Para empezar, se deben establecer los permisos de lectura y escritura a través del puerto serie. La función que se usará en este caso seráchmod. Además, esta función también nos permitirá el envío y recibimiento entre el robot y el ordenador. Para la conexión con el PIONEER, el puerto usado es el/dev/ttyUSB0.
Por lo tanto, se procede con la instrucción:
sudo chmod 777 /dev/ttyUSB0
Una vez se ha realizado la conexión, se iniciaROS, que nos permitirá la comuni- cación entre todos los procesos del proyecto:
5.2. Pruebas sobre el robot real PIONEER 3DX
roscore
A continuación, con el programa compilado, se procede con el inicio del nodo RosAria:
rosrun rosaria RosAria _port:/dev/ttyUSB0
Cuando se ha ejecutado esta instrucción, a través derostopic_list se pueden apreciar los tópicos que antes se han mencionado y que ahora están activos, como se ve en la figura5.8.
Figura 5.8: Lista de tópicos
5.2.2 Adaptación al entorno real
Para llevar a cabo la ejecución del proyecto, se deben realizar una serie de cambios en el código para poder configurar el robot y ejecutar el programa sobre el robot real.
Ajuste de parámetros y tópicos
La ejecución del programa a través del robot real requiere una serie de cambios en el programa. De esta manera, uno de los hechos más importantes es asignar correcta- mente los diferentes tópicos del robot ya que su nombre es diferente entre el simulador Stage y los proporcionados por RosAria.
Para ello, se han substituido tanto los tópicos de velocidad, odometría y láser que estaban enlazados con elVFHpor los nuevos tópicos:
5. PRUEBAS EXPERIMENTALES Y ADAPTACIÓN AL ROBOT REAL
Tópicos
Stage PIONEER 3DX (RosAria)
robot_1/base_scan_1 /RosAria/sonar robot_1/odom /RosAria/pose robot_1/cmd_vel /RosAria/cmd_vel
Cuadro 5.1: Cuadro de tópicos
Lectura de sensores
En el apartado anterior se ha explicado como se instalaba el paquete de RosAria, y su importancia para la conexión de nuestro programa con el robot. Además, se ha mencionado que para la lectura de los ultrasonidos, se ha usado el tópicoRosAria/sonar el cual se encarga de proporcionar mensajes del tipo PointCloud.
Este mensaje devuelve un array de puntos 3d llamadopoints. De esta manera, cada punto se corresponderá a la lectura de un sensor y estará compuesto por tres componentes, que en este caso seránx, y, z, que corresponden con las coordenadas donde se encuentra un obstáculo respecto al robot, tal y como se ha podido apreciar en el ejemplo de la figura5.7del capítulo anterior:
x=0.0689999982715 y=-1.0
z=0.0
En la figura5.7se muestra el resultado al realizar la instrucciónrostopic echopara el tópicoRosAria/sonar. Al saber de esta información, se ha procedido a extraer la informa- ción de los sensores de la parte delantera del robot, ya que el láser que usa el simulador únicamente cubre esta región. Además, no solo basta el hecho de extraer la información xeydel robot. El array que se usa enVFHestá formado por valores de distancias y no de coordenadas. De esta manera, se debe pasar las coordenadas cartesianas al formato del mapaVFHque necesita el algoritmo de evitación de obstáculos. Para ello, se ha asignado al array de distancias deVFHla distancia entre el robot y el objeto:
d i st anci a= q
x2+y2+c t e
Además, la información sensorial se ha debido ordenar en el array de manera inver- sa ya que en el simulador el orden de lectura a través de los sensores es inverso. A esta conclusión se llegó a través de la ejecución de diversas pruebas, donde se pudo compro- bar que el robot no respondía bien, y en vez de esquivar el obstáculo, se orientaba hacia él. También cabe mencionar que durante las pruebas se modificaron los valores que se introducían en el array. Es decir, como se puede apreciar en la fórmula la distancia total, al final se le ha sumado un valor de una constante. Este valor es para adecuar la distancia mínima a la cual el robot debe rotar ya que de esta manera, se adecua mejor al comportamiento que tiene el simulador. No sólo se han ajustado los valores a través de esta constante, sinó que también se ha multiplicado cada coordenada por un valor de 1000, para adecuarlo a los valores que necesita el mapaVFHpara realizar la evitación.
Finalmente, cuando ya se ha asignado la distancia para ese sensor, esta se introduce en
5.2. Pruebas sobre el robot real PIONEER 3DX
Figura 5.9: Sistema de coordenadas del PIONEER 3DX
Figura 5.10: Sistema de coordendas del robot del simulador
el array. Como recordatorio, cabe decir que se introduce una misma distancia en 45 valores del array, ya que se corresponde con el valor de un sensor (360/8).
Cabe destacar que el sistema de coordenadas que usa el robot real no es el mismo que el que usa el simulador. Para el robot real, el sistema de coordenadas es el que se muestra en la figura5.9.
No obstante, para el simulador, el sistema de coordenadas que se usa corresponde con el de la figura5.10.
A efectos prácticos, la diferencia se aprecia a la hora de la posición relativa que ocupan los objetos en el entorno. Para el robot real, la posición (0,-7) no será la misma que para el simulador. No obstante, a la hora de detectar obstáculos no afecta para nada
