Proyecto final de carrera - Departament d'Enginyeria...

Proyecto final de carrera: Auto localización de robots móviles y modelado del entorno mediante visión

estereoscópica

Realizado por: Carlos Ezquerra Cerdán Director de proyecto: Miguel Ángel García García

Año 2006 Ingeniería informática

Unsiversidad Rovira i Virgili

1

Índice

1. Introducción ……………………………………………. 2

2. Objetivos del proyecto ……………………………………………. 4

3. Especificaciones ……………………………………………. 5

3.1 Hardware: ……………………………………………. 5

3.1.1 Robot Pioneer ……………...……………………. 5

3.1.2 Cámara Digiclops ………………………………...… 7

3.1.3 Conexión blue-tooth ……………...……………………. 8

4. Diseño general …………………………………………… 10

5. Movimiento inteligente del robot explorador …………………… 12

5.1 Introducción. …………………………………………… 12

5.2 Objetivos …………………………………………… 12

5.3 Diseño …………………………………………… 12

5.3.1 Mapa de decisiones ..….……………………………... 13

5.4 Implementación …………………………………………… 15

5.5 Entorno de trabajo …………………………………………… 16

5.6 Pruebas y resultados. …………………………………………… 16

5.7 Problemáticas detectadas y soluciones propuestas ……………. 26

5.8 Conclusiones. …………………………………………… 30

6. Auto localización del robot explorador …………………………………… 31

6.1 Introducción …………………………………………… 31

6.2 Objetivos …………………………………………… 32

6.3 Algoritmo slam3D …………………………………………… 33

6.4 Diseño …………………………………………… 37

6.5 Entorno de pruebas …………………………………………… 38

6.6 Implementación en el robot …………………………………… 38

6.7 Evaluación del algoritmo en el robot explorador ……………. 40

6.8 Conclusiones ………………………………………….... 71

7. Desarrollo del proyecto …………………………………………... 72

8. Conclusiones …………………………………………... 73

9. Recursos utilizados …………………………………………... 74

10. Manual de uso. …………………………………………... 75

2

1 Introducción

Hoy en día el uso de robots está ampliamente extendido, de manera que

encontramos desde robots artificieros hasta robots exploradores, pasando por la

infinidad de tipos de robots que podemos encontrar, tales como los brazos

articulados. En este proyecto nos centraremos en los robots exploradores.

Los robots exploradores son aquellos que disponen de algún mecanismo de visión,

ya sea por medio de cámaras o sensores, que les permiten explorar un entorno

desconocido sin colisionar con los distintos obstáculos que se pueden encontrar en

el terreno. Un ejemplo claro de robots exploradores son los robots Oportunity y

Spirit de la Mars Exploration Rover (MER) Misión. Estos robots fueron enviados

por la NASA en el año 2003 al planeta Marte para realizar un estudio de su

superficie geológica.

En nuestro caso, para implementar el proyecto, hemos elegido el robot explorador

Pioneer P2AT mostrado en la figura 1.

Fig. 1

El robot Pioneer P2AT es un robot explorador que cuenta con un anillo de 8

sensores de ultrasonidos para localizar los obstáculos, pero en nuestro caso se ha

decidido que la exploración no se hará usando los sensores de ultrasonidos, sino que

lo haremos mediante una cámara de visión estéreo. La cámara elegida para el caso

es la cámara digiclops, mostrada en la figura 2.

Fig. 2

3

La cámara dispone de 3 objetivos, por lo que podremos realizar imágenes en 3

dimensiones en las que podremos identificar la posición exacta de un punto de la

imagen respecto donde estamos nosotros.

Una vez tenemos definido que es un robot explorador y elegido el robot adecuado

para la puesta en práctica de nuestro proyecto, veremos el apartado de la auto

localización del robot.

Como ya sabemos, la auto localización de un robot es muy importante. Dado que en

entornos desconocidos puede que no dispongamos de GPS1, por ejemplo en otro

planeta que no sea la tierra, necesitaremos un sistema de auto localización del robot,

de manera que podamos saber en cada momento en la posición en la que está. De los

elementos de los que disponemos con los que podríamos hacer una auto localización

eficiente del robot tendremos los sensores de las ruedas del robot y la cámara.

Desechamos el uso de los sensores de las ruedas por el grado de error que tienen y el

que podrían tener dependiendo del terreno en el que se moviera el robot y nos

quedamos con la cámara. Para la auto localización se utilizará un algoritmo de auto

localización mediante la comparación de imágenes de una cámara de visión estéreo.

Dicho algoritmo ha sido cedido para su uso por el Dr.Miguel Ángel García García.

Por lo tanto, en este proyecto trataremos el algoritmo de movimiento del robot, el

algoritmo de auto localización y los errores y problemas con los que nos podemos

encontrar en ambos algoritmos.

1 Global Positioning System: sistema de localización por medio de más de 24 satélites que mediante ondas de radio indican al receptor su longitud, latitud y altitud en cualquier lugar de la tierra. Nótese que éste sistema sólo es útil en la tierra dada la posición de los satélites.

4

2. Objetivos del proyecto

El objetivo principal del proyecto es la programación, evaluación e integración de

los algoritmos de movimiento del robot, así como la integración y evaluación del

algoritmo de auto localización del robot.

Para conseguir estos objetivos deberemos definir una serie de objetivos a cumplir en

cada una de las partes en las que podemos dividir este proyecto. Estos objetivos que

debemos marcar son:

1. Estudio y comprensión de las librerías de movimiento del robot.

2. Programación, integración en el robot y evaluación del algoritmo implementado,

viendo las limitaciones de éste y buscando soluciones satisfactorias para ellas.

3. Integración del algoritmo de auto localización con el programa de movimientos

del robot.

4. Evaluación del algoritmo de auto localización en diferentes entornos.

Éstos son los objetivos que deberemos cumplir en éste proyecto.

5

3 Especificaciones

Los elementos utilizados en el proyecto han sido:

1. Robot Pioneer P2AT

2. Cámara Digiclops

3. Librerías Aria, Digiclops y Triclops

4. Conexión blue-tooth entre PC y Robot.

A continuación comentaremos cada uno de éstos elementos.

3.1 Hardware

3.1.1 Robot Pioneer

El robot seleccionado para el proyecto es el Pioneer P2AT. (Figuras 1 y 3).

Fig. 3

El robot Pioneer P2AT es un robot todo terreno muy versátil, con software compatible

con todos los MobileRobots2, elegido por gran parte de las empresas que requieren un

robot de alto rendimiento con un grado alto de personalización. Fácil de usar y con gran

poder es un robot excepcional para su uso en el exterior.

El Pioneer P2AT ofrece la opción de utilizar un computador incorporado en el mismo

robot, con lo que se podrá utilizar y programar con los programas y lenguajes

convencionales y ya no se deberá programar directamente sobre el microcontrolador.

Además dispone de conexión ethernet y otras funciones autónomas. En nuestro caso

además se le han añadido los conectores usb para conectar el emisor/receptor de blue-

tooth y de una tarjeta firewire para la conexión de la cámara.

2 MobileRobots es la gama de robots móviles de la empresa ActiveMedia Robotics.

6

El Pioneer P2AT dispone de ruedas gigantes, que le permiten superar gran cantidad de

obstáculos, además de motores que le permiten alcanzar una velocidad de 0.8 metros

por segundo y cargar más de 30 Kg.

El robot Pioneer P2AT combinado con la librería Aria es capaz de:

Vagar de forma aleatoria.

Ser conducido mediante un joystick o teclado.

Planear caminos.

Mostrar un mapa de su sonar.

Localizarse usando el sonar.

Comunicar la información de control y los sensores.

Simular mediante el simulador Saphira el funcionamiento del robot.

El robot Pioneer P2AT está indicado para:

Mapeado de terrenos.

Navegación.

Monitorización.

Reconocimiento.

Visión.

Manipulación.

Cooperación y otros comportamientos.

La distribución de los ejes de coordenadas del robot la podemos ver en la figura 4.

Fig. 4

Se ha podido comprobar que el robot tiene errores tanto en el desplazamiento como en

el giro, de manera que se ha medido un desplazamiento de 50 cm. dando como resultado

un desplazamiento de sólo 46.5 cm. Por lo tanto los movimientos del robot de cara a la

auto localización se usarán sólo de forma orientativa para mirar que se realiza la auto

localización de forma correcta.

7

3.1.4 Cámara Digiclops

La cámara elegida para su integración con el robot ha sido la cámara Digiclops (figura

5), fabricada por Point Grey Resesarch. La cámara Digiclops de visión estéreo es un

dispositivo sensorial que permite la captura de imágenes en 3 dimensiones en tiempo

real, para aplicaciones como medida, modelado VRML, robots móviles, etc.

Digiclops es una cámara calibrada que proporciona imágenes estéreo al software

Triclops, que será el encargado de extraer la información de dichas imágenes.

Digiclops es un revolucionario sistema de visión que combina las imágenes estéreo en

3D con la tecnología de las cámaras digitales. La cámara Digiclops realiza imágenes de

alta calidad, con excelente calibración de la cámara, transmisión de imágenes digitales y

configuración de los múltiples sensores. La combinación de sus características convierte

a la cámara Digiclops en la cámara ideal para cualquier aplicación que requiera de un

sistema de visión en 3 dimensiones.

Fig. 5

Especificaciones:

– Tres objetivos Sony ICX084.

– Sensor de imagen de alta definición con píxeles cuadrados.

– Formato VGA (640 x 480)

– Búsqueda progresiva.

– Iluminación mínima: 6 Lux

– Lentes optimizadas para mejorar la visualización.

– Alimentación: 12V 400mA proporcionada por cable firewire.

8

Para tratar las imágenes 3D y obtener la información de ellas lo hacemos como se ve

en la figura 6. La posición X, Y, Z de un punto en la escena puede ser determinada

por medio de la triangulación de los puntos en la escena.

Fig. 6

La triangulación la realizaremos entre los puntos Pl, Pr y Pt obtenidos por la cámara

de la izquierda, la de la derecha y la de arriba respectivamente bh y bv son las líneas

horizontales y verticales del desplazamiento de la cámara. F es la longitud local de

las cámaras.

3.1.3 Conexión blue-tooth

Para la conexión entre nuestro PC y el PC interno del robot se utilizará una conexión

mediante ondas de las conocidas como blue-tooth.

Para la configuración de la conexión blue-tooth en entorno de desarrollo Linux se

utilizarán los paquetes bluez-utils y bluez-pan.

Una vez instalados los paquetes se deberá configurar la conexión para que cuando se

conecte por medio del blue-tooth se configure de forma automática la conexión con la

dirección IP seleccionada, que en nuestro caso será 10.0.0.1 para el servidor y 10.0.0.2

para el cliente.

Por último deberemos activar el demonio pand seleccionando el tipo adecuado de

servicio, ya sea servidor o cliente. Para el servidor deberemos escribir:

Pand –listen –role GN

Para el cliente el comando es:

Pand –search –role PANU –service GN

Con esto, por medio de una conexión ssh, nos podremos conectar al PC interno del

robot desde nuestro PC.

9

Deberemos tener en cuenta las limitaciones de la conexión blue-tooth, de manera que si

se utilizan comandos que requieran la transferencia de gran cantidad de información o

ventanas gráficas se colgará la conexión, con lo que se requerirá el reinicio de los

equipos que estén conectados. Por lo tanto, no mostraremos en tiempo real de forma

gráfica los pasos que sigue el robot, sino que se guardará en un fichero de movimientos

que puede ser consultado en cualquier momento.

10

4.Diseño general

Para el diseño general de la aplicación crearemos diferentes módulos que serán

utilizados desde un programa principal por medio de llamadas a sus funciones. De esta

manera dividiremos los módulos en 2 tipos:

Módulos de movimiento del robot.

Módulos de auto localización del robot.

El módulo de movimiento del robot contiene todos aquellos módulos que se utilizan

para la conexión del robot, conexión de la cámara y decisiones de movimientos.

El módulo de auto localización engloba todos los módulos del algoritmo Slam3D junto

con el módulo que realiza la llamada a éste, que será el que se comunique con el

programa principal. (diagrama 7).

Fig. 7

El robot generará diferentes archivos para su consulta. Éstos se eliminarán en cada

movimiento para evitar la sobresaturación del disco y la degradación de su sistema de

ficheros. Estos archivos son:

Fichero_movimientos en él se guardarán todos los movimientos

hechos por el robot. Este archivo no se borrará y podrá ser consultado en

cualquier momento, ya que se abre, se escribe y se cierra para cada uno

de los movimientos que se realiza.

Módulo de movimientos

Módulo de auto localización

Módulo de conexión con auto localización

Programa principal

Movimiento?

11

Puntos_sensor fichero de puntos que muestra el mapa que ha creado

el robot para decidir si girar a la izquierda, a la derecha o seguir recto.

Este fichero se sobrescribirá en cada movimiento.

Out*.ppm foto que se realiza antes de cada movimiento. El archivo se

borrará después de cada movimiento y auto localización para impedir

que se sature el disco.

Out*.pts fichero de puntos extraídos de la fotografía. Igual que el

anterior se borrará después de realizar el movimiento y la auto

localización.

c0, c1, d0, d1, e0, e1 ficheros utilizados para la auto localización. En

ellos tenemos los puntos identificados en las imágenes, los

emparejamientos entre los puntos y los puntos que quedan después de la

filtración de puntos incorrectos. Estos archivos se eliminarán después de

hacer cada auto localización.

Para evitar que se borren los archivos, ya que puede ser que nos interese consultarlos, se

ha diseñado el programa de manera que tengamos 2 posibles modos de ejecución. En el

modo guiado el robot preguntará al usuario si desea realizar el siguiente movimiento

antes de borrar los ficheros, de manera que antes de realizar el movimiento el usuario

podrá recuperar toda la información. En el modo no guiado el robot explorará sin

consultar al usuario y borrará o sobrescribirá los ficheros en cada uno de los

movimientos. En nuestro caso, y dado que queremos evaluar el correcto funcionamiento

de los programas, utilizaremos el modo guiado.

El programa principal será el encargado de establecer el modo de funcionamiento del

programa, realizará las conexiones con el robot y con la cámara, hará la fotografía y

extraerá los puntos de ella por medio de llamadas a las funciones de la librería triclops.

Una vez tengamos la fotografía con sus puntos definidos llamará al módulo de

movimientos para que decida que movimiento realizar, al módulo de auto localización y

finalmente a la librería Aria para realizar el movimiento adecuado con el robot.

En cada uno de los apartados del proyecto veremos más a fondo el diseño de estos

módulos.

12

5. Movimiento inteligente del robot explorador

5.1 Introducción

Un robot explorador es aquel que se programa para la exploración de entornos

desconocidos y que cuenta con diferentes sensores o sistemas de visión que le permiten

explorar el entorno.

En los robots exploradores es básico encontrar un movimiento que, además de no

colisionar con los obstáculos, permita explorar todo el terreno en el menor tiempo

posible. Basándonos en esto trabajaremos para conseguir explorar todo el terreno, sin

que se produzcan colisiones con obstáculos.

Para la exploración del terreno se dispone de un robot Pioneer P2AT dotado de un anillo

de 8 sensores de ultrasonidos y una cámara de visión estéreo Digiclops. Combinando

ambas tecnologías conseguiremos el movimiento propuesto.

5.2 Objetivos

Los objetivos que se han marcado en este apartado del proyecto son:

Implementar un algoritmo de decisión de movimiento en el robot

explorador, mediante el sistema de visión de la cámara, con la finalidad

de que se explore de forma correcta un entorno desconocido.

Evaluar dicho algoritmo para comprobar su correcto funcionamiento

tanto en casos de uso normal como en los casos más extremos.

Buscar soluciones a los problemas que se puedan encontrar para todos

los casos y así conseguir el algoritmo correcto.

Identificar los mínimos locales y solucionarlos.

Identificar las limitaciones del algoritmo de movimiento.

5.3 Diseño

Como se ha comentado con anterioridad, en este apartado se mostrará es diseño del

módulo de movimientos del robot explorador.

El módulo de movimientos del robot explorador se compone de los siguientes módulos:

Módulo funciones_robot En él se encuentran las funciones de

inicialización y movimientos del robot.

Módulo cámara en él encontramos las funciones para inicializar y

finalizar la cámara.

13

Módulo movimientos en él encontramos las funciones para detectar si

puede haber colisión con algún obstáculo y la decisión del movimiento a

realizar.

Módulo sensores módulo para tratar los sensores de ultrasonidos del

robot.

Todos estos módulos serán llamados directamente por el programa principal, y como ya

hemos visto antes generarán una serie de ficheros para poder comprobar y consultar la

correcta realización del movimiento.

Además se ha definido la estructura punto, con la que trabajarán algunos módulos, que

contendrá toda la información relacionada a un punto de la imagen.

5.3.1 Mapa de decisiones

Para decidir el movimiento adecuado a realizar por el robot, a partir de la imagen con

sus puntos encontrados, el robot construye un mapa con los puntos encontrados a una

cierta altura. Para construir este mapa se buscará entre todos los puntos encontrados en

la imagen, si hay alguno que esté a una distancia menor de la distancia que nos vamos a

mover. En nuestro caso hemos añadido una distancia de seguridad a esa distancia.

Si encontramos algún punto a menor distancia que la determinada como distancia de

colisión, haremos un corte horizontal de los obstáculos a la altura del punto encontrado.

En el caso en el que no encontremos ningún punto susceptible de colisión se elegirá una

fila, que en nuestro caso será la que ocupa la posición 125, cercana al centro de la

imagen. Por lo tanto, se guardarán todos los puntos de la imagen que estén a la altura

deseada (en píxeles). Una vez guardados los puntos se procederá a trabajar con ellos

para encontrar la mejor opción de giro.

Dado que éste sistema sólo será correcto cuando trabajemos con obstáculos en forma de

polígonos tales como cubos, rectángulos, etc., se ha añadido otro mapa, que se creará

plasmando en el suelo todos los obstáculos, de manera que quedaría el mapa como si

viéramos los obstáculos desde una vista aérea. Por medio de los 2 mapas conseguiremos

encontrar la mejor opción de giro.

Un ejemplo de mapa, visto con el programa gnuplot, se puede ver en la figura 8.

14

Fig. 8

Éste es un ejemplo de mapa con el que nos podríamos encontrar. El eje vertical muestra

los metros a los que se encuentra el punto en cuestión y el eje horizontal el píxel de la

imagen en el que se encuentra dicho punto. Como ya hemos dicho antes la altura se fija

a un punto, que éste será el de colisión con el objeto, en el caso en el que haya posible

colisión, o el de la mitad de la imagen en el caso en el que no la haya. Los puntos que

encontramos a distancia 0 no es que el robot está tocando contra ellos, sino que son

puntos que no se han podido detectar con la cámara, por lo que los tomaremos como

espacio libre.

Se ha decidido que el robot seguirá recto si no hay peligro inminente de colisión y

queda espacio por explorar. Con que queda espacio para explorar queremos decir que no

se encuentra con que la mayoría de puntos encontrados se encuentran a una distancia

menor de 2 metros y el robot, aunque siguiera hacia delante, no podría pasar por

ninguno de los espacios libres, por lo que es inútil seguir hacia delante y ya se da la

superficie por explorada. En este caso realizaremos un movimiento aleatorio, que en

este caso hemos decidido que será girar hacia la izquierda.

Si encontramos un obstáculo con el que tenemos una posible colisión giraremos hacia el

lado en el que el robot tenga mayor espacio libre y que quepa el robot por él. En el caso

en el que no quepa por ninguno de los lados el robot girará a la izquierda por defecto

para evitar colisionar con el obstáculo.

15

Por último, para detectar y salir de un mínimo local se han creado las funciones que

cuando detectan una secuencia de giros izquierda-derecha-izquierda o derecha-

izquierda-derecha giran 50 grados en intervalos de 5 grados para no perder la auto

localización. Con esto conseguiremos salir del mínimo local de forma satisfactoria.

5.4 Implementación

Se han creado las siguientes funciones dentro de los módulos:

• Módulo de funciones_robot:

o robotInitialize inicializa la conexión con el robot y el propio robot.

o robotForward mueve el robot hacia delante tantos centímetros como

se indique por parámetro.

o robotRight gira el robot hacia la derecha tantos grados como se

indique por parámetro.

o robotLeft gira el robot hacia la izquierda tantos grados como se

indique por parámetro.

o robotClose para el robot y la conexión con su microcontrolador

• Módulo de cámara:

o cameraInitialize inicializa la cámara y su conexión.

o cameraClose cierra la conexión con la cámara.

• Módulo de movimientos:

o busca_direccion busca la dirección hacia la que debe girar el robot

para evitar los obstáculos.

o Collision devuelve si hay posible colisión del robot con algún

obstáculo.

• Módulo sensores:

o busca_colision devuelve si hay una posible colisión en el próximo

movimiento usando los sensores para comprobarlo.

o busca_mayor_distancia devuelve el número del sensor de ultrasonidos

que está más lejano a un obstáculo.

o busca_menor_distancia devuelve el número del sensor de ultrasonidos

que está más próximo a un obstáculo.

16

5.5 Entorno de trabajo

El entorno de trabajo con el que trabajará en el robot explorador es un sistema operativo

linux Red Hat 7.3, en el que programaremos los algoritmos en lenguaje C ayudándonos

de las librerías:

- Aria funciones de movimiento del robot y uso de los ultrasonidos para mirar

la distancia correcta a los obstáculos.

- Digiclops funciones para la captación de imágenes por medio de la cámara.

- Triclops funciones para la captación de imágenes 3D y de puntos en ellas

por medio de la cámara de visión estéreo.

En cuanto al entorno de pruebas en el que se encuentra el robot es el laboratorio de

robótica, en el cual encontraremos diferentes obstáculos tales como sillas, mesas y

armarios para comprobar el correcto funcionamiento del algoritmo de decisión de

movimientos.

5.6 Pruebas y resultados

Se han realizado diversas pruebas para comprobar el funcionamiento correcto del

programa que controla el movimiento del robot. Las pruebas se han realizado con la

cámara sólo y con la cámara y los ultrasonidos de forma simultánea. Los resultados que

hemos obtenido son los siguientes:

- Prueba 1

Dejamos al robot vía libre por delante, de manera que debería avanzar sin problemas al

no detectar obstáculos a menos de 2 metros.

Trabajará con la siguiente foto:

17

Esta es la foto realizada por la cámara digiclops y para decidir el siguiente movimiento

a realizar buscará si hay algún punto menor a la distancia de colisión y los que haya a

menos de 2 metros. El mapa que crea el robot a partir de la foto es:

Si nos fijamos en la escala inferior, vemos que en el mapa se distingue en el rango

[0-50] la silla más cercana por la parte de la izquierda, en el rango [50-100] se

identifican las dos sillas más lejanas. Del 100 al 250 se cogería todo como camino libre,

ya que todos los puntos encontrados están a distancia mayor de 2 metros y para acabar

en el rango [250-300] se detecta el armario.

Dado todo esto, el robot ve que delante de él no hay obstáculos y no se puede explorar

puntos a distancia mayor de 2 metros, por lo que decide seguir recto.

- Prueba2

En esta prueba se ha querido comprobar que el giro a la derecha se realiza de forma

correcta cuando encontramos un obstáculo a la izquierda, el robot puede colisionar con

él y tiene espacio hacia la derecha para seguir explorando, por lo que deberá girar hacia

este lado para seguir explorando.

La foto que trataremos es la siguiente:

18

Como podemos observar en la foto la caja que tenemos a la izquierda impediría que el

robot siguiera recto, ya que se produciría la colisión contra ella.

El mapa que genera el robot a partir de la foto es:

En el mapa podemos observar que la cara de la caja que mira hacia el robot no la puede

detectar, ya que no hay suficiente iluminación para captar los puntos. Sin embargo si

que capta los del lateral de la caja, que es el intervalo [90-100]. El robot decidirá seguir

hacia la derecha porque la zona libre hacia la izquierda es mayor, que es el intervalo

[100-245]. En el caso en el que hubiera un obstáculo en éste rango el robot podría

decidir girar hacia la izquierda lo que podría provocar la colisión contra la caja.

19

En el apartado de problemas y soluciones se ha propuesto y puesto en práctica la

solución para casos como éstos. Aunque en este caso en particular comprobamos que el

robot lo haría de forma correcta y giraría hacia la derecha.

- Prueba 3

En este caso tenemos el obstáculo en el lado derecho, por lo que queremos comprobar

que el robot gira de forma correcta hacia la izquierda al detectar el obstáculo y el

espacio libre del que se dispone hacia la izquierda.

La foto con la que tratamos es:

Como podemos observar en este caso el espacio libre queda hacia la izquierda y la

iluminación de la caja es mejor, por lo que el robot debería detectar puntos de la caja.

Esto lo podremos ver en el siguiente mapa:

20

El robot consigue detectar algunos de los puntos de la caja, aunque en el rango [200-

300] no encuentre ningún punto. Gracias a los puntos detectados el robot podrá girar de

forma correcta hacia la izquierda, aunque nos podríamos encontrar con el mismo

problema que en la prueba anterior si en la zona de la izquierda tuviéramos algún

obstáculo que sí se detectase.

- Prueba 4

En esta prueba se ha querido comprobar que funciona de forma correcta la exploración

de zonas en las que no podemos explorar más, es decir, que el robot no cabe físicamente

por ninguno de los lados, ya que en ambos hay obstáculos. Si miramos la foto a tratar

veremos este hecho:

21

En este caso la caja tapa los posibles huecos por los que podría pasar el robot. Esto se ve

en el siguiente mapa:

En el mapa podemos observar como el robot detecta las sillas en la zona izquierda y a la

derecha detecta el armario, por lo que es físicamente imposible que el robot quepa por

los espacios que quedan disponibles, de manera que el robot decidirá hacer un giro

estático (sin ir hacia delante) hacia la izquierda, con la finalidad de acabar encontrando

un espacio libre por el que seguir explorando. Éste movimiento se realizará todas las

veces que sean necesarias hasta encontrar un espacio libre por el que pasar el robot.

- Prueba 5

En esta prueba se volverá a evaluar el giro hacia la izquierda al haber peligro de colisión

si seguimos recto. La diferencia con el caso anterior en el que evaluábamos el mismo

caso es que en este caso hemos probado con una caja con diferentes colores, por lo que

se detectarán los puntos de la caja. Esta podría ser una solución a los problemas vistos

en las pruebas anteriores, aunque no sería efectiva, ya que necesitaremos obstáculos con

diferentes puntos susceptibles de ser captados por la cámara. La foto que tratáremos es:

22

Si vemos el mapa generado por el robot comprobaremos que la cantidad de puntos

captados por la cámara es sustancialmente más grande.

Como se puede observar en este caso se detectan prácticamente todos los puntos de la

caja, por lo que el robot no tiene problema alguno para decidir hacia el lado al que debe

girar y éste es hacia la izquierda.

- Prueba 6

En esta prueba se ponen los obstáculos a más de 2 metros, distancia que se ha

considerado suficiente como para afirmar que nos debemos acercar más a los obstáculos

para poder explorarlos de forma correcta.

En la foto podemos observar que el obstáculo está en una posición lo suficientemente

lejana.

23

El mapa que genera el robot a partir de la foto y sus puntos detectados es:

En el podemos observar que los puntos que se captan son lejanos o no se captan, por lo

que el robot decidirá seguir recto con la finalidad de acercarse más a los obstáculos y

hacer la exploración de forma correcta.

- Prueba 7

En este caso se ha querido ver el caso que tendríamos en el caso en el que nos

encontrásemos con un obstáculo lejano (con el que no tendremos problemas de colisión,

ya que está por encima de los 50 cm. que nos moveremos) y nos acercaremos 50 cm.,

que como ya hemos dicho con anterioridad realmente serán 46.5 cm.

La primera imagen que tenemos es:

24

Con su mapa de puntos:

Se observa que no hay problemas de colisión y no detectamos todo el espacio cubierto,

de manera que nos acercaremos más al obstáculo y seguidamente decidiremos hacia

donde debemos girar.

La imagen que se obtiene después de avanzar 46.5 cm. es:

25

Y su mapa de puntos es:

Como podemos observar en este caso el obstáculo ya se detecta en una zona de posible

colisión (50 cm. más una pequeña distancia de seguridad) por lo que el robot en este

caso decidirá girar hacia la izquierda, que es el lado por el que encontramos mayor

distancia libre.

- Prueba 8

Por último comprobaremos uno de los errores que era previsible que sucediese, que es

el caso en el que un obstáculo esté tan cerca que la cámara sea incapaz de captar sus

puntos, por lo que el robot previsiblemente decidiría seguir recto con lo que se

26

produciría la colisión segura. Éste caso nos dará paso a la sección de las soluciones

propuestas para que no ocurran este tipo de cosas.

En la imagen se puede apreciar que el obstáculo está tan cercano al robot que no se

pueden distinguir los puntos.

La zona azul es toda un obstáculo.

El mapa de puntos generado es:

Por lo que podemos ver en el mapa no se detecta ningún punto y aquellos que se

detectan lejanos (a más de 2 metros) se producen por la reflexión de la luz en la caja.

Este pequeño grupo de puntos se puede ver en la parte derecha de la imagen.

El robot decide seguir recto y se produce la colisión contra el obstáculo.

27

5.7 Problemáticas detectadas y soluciones propuestas

Como se ha visto en las pruebas de movimiento hay casos en el que la cámara no es

suficiente para detectar un obstáculo cercano.

Una posible solución ya se ha dado antes, utilizando sólo obstáculos de los cuales la

cámara sea capaz de identificar puntos, pero como ya se ha dicho, la solución no es

buena, ya que en la mayoría de los casos en los que se utilice un robot explorador no se

podrá elegir el color y la fisonomía de los obstáculos. Pongamos como ejemplo un robot

explorador en la superficie de Marte. Los obstáculos que encontrará en la superficie

marciana serán en su mayoría rocas de diferentes colores y formas y, evidentemente, no

se podrá elegir las formas ni los colores.

Con todo lo visto tenemos un problema de difícil solución, ya que sólo utilizando la

cámara será muy difícil, por no decir imposible, captar los puntos de los obstáculos muy

cercanos.

La solución que se propone y se ha puesto en práctica es utilizar la medición de

distancia por ultrasonidos aprovechando que el robot Pioneer dispone de ella.

5.7.1 Medición de distancias mediante ultrasonidos

Los ultrasonidos son una onda acústica cuya frecuencia está por encima del límite

perceptible por el oído humano (aproximadamente 20 Khz.). Muchos animales como los

delfines y los murciélagos lo utilizan de forma parecida al radar en su orientación.

Nosotros los utilizaremos de la misma forma que los animales para decidir si hay

obstáculo o no lo hay.

Basándonos en el tiempo que tarda la señal en ir desde el emisor hasta el objeto

obstáculo y volver rebotada desde éste hasta el receptor y conociendo la velocidad del

sonido podemos calcular con suficiente precisión y exactitud la distancia entre el objeto

y nuestros receptores.

Para explicarlo de forma correcta explicaremos las bases físicas que lo explican:

• Los ultrasonidos son antes que nada sonido, exactamente igual que los que

oímos normalmente, salvo que tienen una frecuencia mayor que la máxima

audible por el oído humano. Ésta comienza desde unos 16 Hz y tiene un límite

superior de aproximadamente 20 Khz., mientras que nosotros vamos a utilizar

28

sonido con una frecuencia de 40 Khz. A este tipo de sonidos es a lo que

llamamos Ultrasonidos.

• El sonido se desplaza en el aire a una velocidad de 340 m/s a una temperatura de

20º C, con una variación térmica de 0.6 m/s por cada grado centígrado. Así, por

ejemplo, a 30º C de temperatura del aire, la velocidad del sonido es de 340 m/s +

(0.6 m/s/º C * 10º C) = 346 m/s (Intervienen otros parámetros pero que vamos a

dejar de lado en este trabajo por complicarnos excesivamente sin aportarnos

mayor resolución)

• La distancia d recorrida por un móvil es directamente proporcional a la

velocidad v de éste y al tiempo t empleado en ello. Podemos escribirlo como una

fórmula en la sencilla forma:

d = v * t

dónde d es la distancia recorrida en metros, v la velocidad en metros por

segundo y t el tiempo en segundos.

Esta misma relación la podemos escribir de otra forma que nos relacione el

tiempo con las otras variables, estableciendo que el tiempo t que tarda un móvil

en recorrer una distancia d es inversamente proporcional a la velocidad v del

mismo e directamente proporcional a la distancia d a recorrer:

t = d / v

dónde t el tiempo en segundos, v la velocidad en metros por segundo y d es la

distancia en metros.

• Si deseamos medir una distancia usando los ultrasonidos tenemos que emitir un

pulso ultrasónico breve, esperar que llegue hasta el obstáculo a medir, se refleje

en éste y vuelva hasta nuestro dispositivo sensor, midiendo entonces el tiempo t

que ha tardado en hacer todo el trayecto de ida y vuelta mediante nuestro

microcontrolador PIC. Conociendo la velocidad v a que se ha desplazado nuestra

onda tal como hemos visto, podemos fácilmente calcular según lo expresado en

el punto anterior la distancia d entre el sensor y el objeto.

29

• Por ejemplo:

Un objeto situado a 15 cm. de nuestro sensor, con una temperatura del aire de

20º C, debe emitir un eco tras nuestro pulso que debe tardar en llegar t = (0.15

m / 340 m/s) * 2 = 0.000882 segundos (o escrito de otra forma 882 uS,

microsegundos)

Nota el detalle de que multiplicamos por 2 el tiempo empleado debido a que el

sonido debe ir y volver reflejado desde el objeto, por lo que recorre 2 veces la

distancia que separa al sensor del objeto.

5.7.2 Puesta en práctica de los ultrasonidos

Con lo visto en el apartado anterior podemos estar seguros de medir la distancia al

obstáculo de forma correcta, pero no queremos dejar de lado la cámara para realizar esta

función, dado que hemos comprobado en las pruebas que excepto para algunos casos

aislados la edición de las distancias por medio de la cámara funciona de forma correcta.

Por esto se ha decidido utilizar los 2 métodos de forma simultánea. Para esto le daremos

prioridad a la cámara, de manera que cuando la cámara decida que no hay obstáculo y

por lo tanto podemos seguir recto activaremos los ultrasonidos para comprobar que

realmente no hay peligro de colisión. En el caso en el que los ultrasonidos detectaran un

peligro inminente de colisión pasaría a tener el control del robot el módulo de los

ultrasonidos durante el siguiente movimiento.

El módulo de ultrasonidos controla el movimiento del robot de la forma siguiente:

El robot dispone de 8 emisores y receptores de ultrasonidos dispuestos en

la parte delantera del robot tal como muestra la figura siguiente.

30

Miraremos cual es el emisor/receptor que nos da una distancia más

pequeña al obstáculo. esto se hará de forma ponderada teniendo en

cuenta la posición de los emisores de ultrasonidos. Debemos anotar que

es diferente que el emisor 0 nos de una distancia menor a 20 cm., con lo

que no habría problema de colisión, en cambio si el emisor/receptor 3

diera una distancia menor a 20 cm. habría un riesgo de colisión elevado.

Por lo tanto, para el emisor/receptor 3 y 4 tendremos una distancia para

una posible colisión mayor que de los demás emisores/receptores (60

cm.), para 2, 1, 5, y 6 una distancia de 20 cm. y finalmente para 0 y 7 una

distancia de 11 cm., que es el grosor de las ruedas.

Una vez hemos detectado posible colisión por parte de uno o más

emisores/receptores miraremos el de menor distancia, y por lo tanto,

sabremos hacia que lado está el objeto más próximo con el que tenemos

riesgo de colisión. Si el de menor distancia es 1, 2 ó 3 giraremos hacia la

derecha. En cambio si es 4, 5 ó 6 giraremos hacia la izquierda.

Debemos anotar que el giro se realiza de forma estática, es decir, que

como hemos visto, nuestro robot tiene 3 grados de libertad en X, Z y

YAW (giro en Y), por lo tanto podremos hacer el giro sin cambiar la Z.

esto se traduce en que aunque hubiese 2 o más emisores/receptores que

detectaran colisión, cada uno que hiciese girar a un lado (uno a izquierda

y el otro a la derecha) no colisionaríamos con el obstáculo al elegir uno

de los lados, ya que no variaremos nuestra Z.

Una vez seleccionado el movimiento por parte de los ultrasonidos ya le

podremos devolver el control al módulo de control por cámara.

31

5.8 Conclusiones

Se ha realizado, por medio de un algoritmo de captación de puntos, un algoritmo para el

movimiento de un robot explorador. Sobre el podremos sacar las siguientes

conclusiones:

El algoritmo funciona de forma correcta para gran cantidad de los casos,

en el que el algoritmo elige de forma correcta si girar hacia el lado

derecho o el lado izquierdo.

El algoritmo de captación de puntos identificativos por medio de la

cámara tiene diversas limitaciones, de manera que en algunos casos

provoca que el algoritmo del movimiento del robot no funcione de la

forma adecuada.

Las limitaciones consisten en que la cámara es incapaz de captar puntos

de obstáculos muy próximos a ella así como también es incapaz de

captar puntos de superficies lisas y sin cambios de color.

Para subsanar estas limitaciones se ha recurrido al uso de los

emisores/receptores de ultrasonidos de los que dispone el robot. De esta

forma daremos prioridad a lo seleccionado por la cámara, pero en el caso

en el que la cámara diga que se puede avanzar recto recurriremos a los

ultrasonidos para comprobar que lo que decía el algoritmo de

movimiento por medio de la cámara es correcto. En el caso en el que no

sea correcto el módulo de ultrasonidos pasará a tener el control del

movimiento del robot durante ese movimiento.

Se ha tenido en cuenta la posibilidad de obstáculos móviles para evitar

que el robot en movimiento colisione contra ellos.

Se ha buscado una solución satisfactoria al problema que nos

encontramos cuando el robot se encuentra en un mínimo local, de

manera que se podrá salir del mínimo local sin que el robot pierda su

auto localización.

Se ha comprobado y demostrado el funcionamiento correcto del

algoritmo de movimiento por medio de diferentes pruebas e imágenes.

Con todo lo visto podremos afirmar que el algoritmo de movimiento del robot funciona

de forma correcta, tal como se había marcado en los objetivos, que por lo tanto se han

cumplido.

32

6. Auto localización del robot explorador

6.1 Introducción

Una vez visto el correcto funcionamiento del movimiento del robot explorador

pasamos al apartado de la auto localización del robot.

Este apartado tiene una gran importancia en el mundo de la robótica móvil, ya que

en gran cantidad de casos es vital conocer la posición real en la que se encuentra el

robot. Pongamos como ejemplo otra vez un robot explorando la superficie del

planeta Marte. En la superficie de dicho planeta a parte de encontrar rocas también

encontraremos arena, en la que el robot podría embarrancar provocando, al realizar

diferentes movimientos, que los sensores del robot informen de que el robot se ha

movido cuando realmente el robot no se ha movido y sus ruedas han derrapado

sobre la arena. Además, como ya hemos visto con anterioridad, los movimientos

realizados por el robot mediante sus sensores no son exactos (nótese que se movía

46.5 cm. cuando realmente se le había dicho que se moviera 50 cm.) por lo que esa

pequeña inexactitud que tienen multiplicado por la gran cantidad de movimientos

que realizará el robot se transformará en un desfase bastante grande.

Para evitar estas inexactitudes hemos decidido dotar al robot explorador de un

algoritmo de auto localización. Dicho algoritmo ha sido realizado íntegramente por

el Dr. Miguel Ángel García, profesor de la universidad Rovira i Virgili, y que ha

cedido el uso de dicho algoritmo para poder realizar este proyecto.

El algoritmo de auto localización utilizado es el slam3D, que utiliza una cámara con

visión estero, como la que se dispone en el robot, para realizar su auto localización

por medio de la comparación de las fotografías hechas por la cámara.

Vista la importancia de la auto localización del robot, integraremos el algoritmo de

auto localización en el robot con la finalidad de evaluar si es viable utilizar dicho

algoritmo en robots exploradores así como comprobar el funcionamiento de dicho

algoritmo en diferentes ambientes y en las circunstancias más extremas.

33

6.2 Objetivos

El objetivo principal de éste apartado del proyecto es integrar el algoritmo de auto

localización dado en el robot explorador, así como la evaluación de dicho algoritmo

en cuanto a su correcto funcionamiento. Por lo tanto, los objetivos marcados para

este apartado son:

Integración correcta del algoritmo de auto localización con el algoritmo

de movimiento del robot realizado en el apartado anterior.

Evaluación de dicho algoritmo para comprobar el correcto

funcionamiento de éste con un robot explorador.

Comprobar e identificar las limitaciones del algoritmo, así como

encontrar posibles soluciones a dichas limitaciones.

34

6.3 Algoritmo slam3D

El algoritmo de auto localización utilizado en el proyecto es el algoritmo slam3D.

Éste algoritmo se basa en la comparación de diversas fotografías conseguidas mediante

una cámara de visión estéreo para saber cual es el movimiento que se ha realizado. El

funcionamiento del algoritmo es el siguiente:

Se realizan 2 fotografías consecutivas habiendo un movimiento entre

ellas 2. En el caso en el que no hubiese ningún movimiento entre ellas el

algoritmo debería devolver un desplazamiento igual a 0.

Se identifican puntos representativos en dichas fotografías.

Se identifican los puntos que son iguales pero que tienen un

desplazamiento entre ellos.

Se filtran los emparejamientos de puntos producidos por el ruido.

Se calcula la distancia entre los puntos cogiendo todos los puntos y

haciendo una ponderación.

Veamos como funciona el algoritmo por medio de un ejemplo:

Realizamos las fotografías e identificamos los puntos representativos en ellas:

Primera imagen:

Segunda imagen:

35

Como podemos ver en las imágenes ya se ha hecho el emparejamiento de puntos.

Nótese en la parte superior de la imagen los puntos mal emparejados producidos por

el ruido.

A continuación filtraremos los emparejamientos incorrectos, de manera que las

imágenes que tendremos serán:

Primera imagen:

Segunda imagen:

36

En estas imágenes se puede identificar como se han eliminado de forma correcta los

emparejamientos producidos por el ruido.

Por último haremos otra filtración y calcularemos la distancia que se ha movido el

robot haciendo la ponderación de los enlaces, con lo que las imágenes que nos

quedarán serán:

Primera imagen:

Segunda imagen:

Con lo que ya podremos calcular la distancia. En este caso nos habíamos movido 50

centímetros en dirección hacia delante (eje Z positivo del robot). Debemos tener en

cuenta que nos hemos movido 50 cm. según los sensores del robot, que como ya se

ha dicho antes, tienen un error bastante grande.

El resultado obtenido por medio del algoritmo de auto localización es: Translation -0.006932 -0.005879 0.402671 Pitch(X) 0.312178 Yaw(Y) -0.172794 Roll(Z) -0.040578 Error 59.177221

37

Viendo los campos del resultado podemos ver que tenemos ciertos errores de

cálculo, pero que los errores son bastante pequeños.

En los campos Translation tenemos:

-0.006932 desplazamiento en el eje X del robot. Éste desplazamiento es de 6

milímetros, por lo que lo podremos considerar despreciable.

-0.005879 desplazamiento en el eje Y del robot. Igual que en el caso anterior se

puede considerar despreciable, ya que es de sólo 5 milímetros.

0.402671 desplazamiento en el eje Z del robot. En él se puede apreciar el

movimiento realizado, que es de 40 cm. hacia delante. Mediante una cinta métrica se

ha medido el desplazamiento real del robot, que ha sido de 43 cm., por lo que el

algoritmo da un valor bastante aproximado.

Pasamos a ver los giros que ha habido en los ejes:

Pitch 0.312178 giro que ha habido en el eje X. El valor de 0.3 grados que nos

devuelve el algoritmo es un pequeño error, aunque parte de éste está producido por

la imprecisión del robot, que al producirse un movimiento en uno de los ejes se

puede producir pequeños movimientos en los otros.

Yaw -0.172794 giro que ha habido en el eje Y. Tenemos un caso similar al caso

anterior, en el que se produce un pequeño error por imprecisiones en el robot.

Roll -0.040578 giro que ha habido en el eje Z. En éste caso el error es muy

pequeño, tan sólo de 0.04 grados, por lo que se considerará despreciable.

Con esto hemos visto el funcionamiento del algoritmo a evaluar.

38

6.4 Diseño

El diseño de este apartado del proyecto se divide en 2 partes diferenciadas:

- Módulo de auto localización, que permite la llamada del programa principal al

programa slam3D.

- Programa slam3D.

Dado que en este proyecto no se ha diseñado ni implementado el programa slam3D no

hablaremos del diseño de éste módulo.

Sí que hablaremos sin embargo del módulo que nos permite hacer las llamadas al

programa slam3D.

El programa slam3D tiene un fichero de configuración en el que le tenemos que pasar

las 2 imágenes que se tratarán para emparejar los puntos. Dicho fichero, llamado

sequence, deberá ser modificado en cada uno de los movimientos que realiza el robot.

Estas modificaciones se harán desde el módulo autolocation de nuestra implementación.

Una vez realizados los cambios necesarios, deberemos llamar al programa slam3D, que

imprimirá en el fichero de movimientos el movimiento que se ha realizado.

Como una decisión de diseño también se ha tenido que tomar que si un fichero de

puntos contiene 0 puntos, es decir, que el robot no ha sido capaz de identificar ningún

punto de la imagen, ese fichero no se deberá pasar al programa de auto localización, por

lo que realizaremos el siguiente movimiento y calcularemos la auto localización de 2

movimientos, siendo conscientes de que podemos encontrar un error algo más grande

que el que encontraríamos si hiciésemos un solo movimiento. Esto se debe a que no

tiene sentido pasarle al algoritmo de auto localización un fichero vacío, ya que no

conseguirá emparejar ningún punto.

39

6.5 Entorno de pruebas

Las pruebas se han querido realizar en 2 tipos de entornos. El primero elegido es el

laboratorio de robótica, en el que encontramos diferentes tipos de obstáculos, de manera

que se puede comprobar el funcionamiento correcto para cuando previsiblemente

encontraremos gran cantidad de puntos, debido a la gran cantidad de obstáculos

diferentes con los que nos encontraremos. El problema con el que nos podremos

encontrar aquí es que el robot no distingue puntos de las patas de las sillas, ya que éstas

son cromadas y reflejan la luz. Otro problema con el que nos podemos encontrar es que

el robot se acerque tanto a una pared o un armario que no pueda distinguir ningún

punto, ya que la superficie es homogénea. En este caso el problema de la colisión está

solucionado dado que se utilizarán los ultrasonidos, pero se puede perder la auto

localización correcta si en pocos movimientos no encontramos una fotografía en la que

podamos distinguir y emparejar puntos.

El segundo entorno de pruebas elegido ha sido el pasillo exterior del laboratorio. En este

entorno se captan mejor los puntos gracias a la luz. En él no tenemos más obstáculos

que la barandilla y las paredes, por lo que a priori debería funcionar el algoritmo de

forma correcta.

Se ha comprobado que el algoritmo funciona de forma correcta en ambos escenarios, tal

como mostraremos en el apartado de pruebas.

6.6 Implementación en el robot

Dado que el algoritmo slam3D estaba implementado en C para Linux ya no será

necesario implementarlo. Lo que si será necesario será integrarlo en el robot junto al

programa que controla los movimientos.

Dado que para realizar la auto localización necesitaremos 2 imágenes se deberá realizar

2 imágenes antes de realizar la auto localización. Esto puede provocar algunos

problemas, ya que deberemos hacer la fotografía, acto seguido haremos la auto

localización y después se le preguntará al usuario si quiere realizar el movimiento, pero

el movimiento se realizará sobre la fotografía que se ha tomado antes de la auto

localización y dado que el proceso de auto localización es lento (en torno a 30

segundos) el entorno podría haber cambiado si disponemos de diversos obstáculos

móviles. Dado que en los entornos que se desea explorar con el robot no disponemos de

gran cantidad de obstáculos móviles obviaremos este posible problema. Además, como

se ha comentado en el apartado anterior, se ha dotado al robot de un sistema de

40

detección de colisión por medio de ultrasonidos mientras está en movimiento, lo que

garantiza que no colisionará con los obstáculos.

Para integrar el módulo de auto localización con el de movimientos utilizaremos un

nuevo módulo que modificará el fichero de configuración del programa de auto

localización y llamará a éste. Se ha modificado el programa slam3D para que escriba el

resultado directamente en el fichero de movimientos y no lo haga por pantalla, como lo

hacía por defecto.

41

7.6 Evaluación del algoritmo en el robot explorador

Para la correcta evaluación del algoritmo hemos realizado diferentes pruebas

comprobando sus resultados, tanto en giros como en movimientos hacia delante. En

cada una de las pruebas realizadas hemos analizado las fotografías tomadas por las

cámaras así como los puntos elegidos y sus emparejamientos.

Prueba 1

En esta prueba se quiere comprobar que el algoritmo funciona de forma correcta cuando

no ha habido movimiento del robot o éste se ha intentado mover pero sus ruedas están

atrapadas, de manera que ,aunque el robot devuelva que se ha movido 50 cm. hacia

delante, no se ha podido mover.

Las fotografías tomadas con el emparejamiento de puntos han sido:

Nótese que hay ciertos puntos emparejados que devuelven un pequeño desplazamiento.

Éstos puntos están marcados con un círculo rojo en la segunda fotografía.

Los puntos incorrectos se deberán eliminar en el proceso de filtrado.

Después del primer filtrado de las imágenes obtenemos:

42

Como se puede observar los puntos marcados antes con el círculo rojo han

desaparecido, ya que se han desechado por ser incorrectos.

Después de la segunda filtración obtenemos:

43

Donde ya podemos apreciar los puntos elegidos para calcular la distancia recorrida por

el robot.

El resultado que devuelve el algoritmo es: Translation 0.000824 -0.000633 -0.000244 Pitch(X) 0.012788 Yaw(Y) 0.014146 Roll(Z) -0.002612 Error 8.067394

Con lo que podemos apreciar que los errores cometidos por el robot en este caso son

prácticamente inapreciables, menores a 1 milímetro en desplazamientos y menor a 0.1

grados en giros.

Prueba 2

En esta prueba se ha querido comprobar que se realiza el cálculo de los desplazamientos

sobre el eje Z (hacia delante) de forma correcta. Para esto hemos colocado el robot en

una posición sin obstáculos delante, por lo que el robot decidirá seguir recto 50 cm.

Las fotos obtenidas con sus puntos identificados son:

44

En estas fotos apreciamos bastantes emparejamientos incorrectos (marcados con un

circulo rojo) que deberá filtrar el algoritmo.

Después del filtrado obtenemos:

Y haciendo el segundo filtrado se obtiene:

Por lo que el cálculo de la distancia nos devuelve: Translation -0.001138 0.005834 0.464195 Pitch(X) 0.243812 Yaw(Y) -0.053211 Roll(Z) -0.565056 Error 61.749334

45

Con la cinta métrica se ha medido la distancia real que se ha movido el robot en el eje Z,

que ha sido de 46.5 cm.

Viendo los resultados obtenido por el algoritmo de auto localización vemos que el dato

retornado en cuanto al movimiento en el eje Z es correcto. El error es despreciable. En

cambio tenemos algunos errores en los cálculos de los giros. Parte de éstos errores se

producen por el algoritmo de auto localización, pero es la parte más pequeña, ya que la

mayor parte del error se produce por los errores en el movimiento del robot. De todas

formas el error máximo está en torno al medio grado, que es un valor prácticamente

despreciable.

Prueba 3

En esta prueba se ha querido comprobar que se realizan de forma correcta los cálculos

de los grados en el caso de los giros hacia la izquierda. Para realizar esta prueba se ha

puesto un obstáculo delante del robot dejándole espacio para explorar por el lado

izquierdo, por lo que el robot decidirá girar hacia la izquierda. El robot debería girar 5

grados hacia la izquierda pero, como se ha podido comprobar, realmente gira en torno a

2 grados, por lo que el algoritmo de auto localización nos debería devolver en torno a

los 2 grados para ser correcto.

La foto que trataremos es:

Filtramos los puntos que no son correctos y nos quedará:

46

Y haciendo el segundo filtrado nos queda:

Dando como resultado el movimiento: Nuevo Movimiento Translation -0.000181 0.008490 0.017059 Pitch(X) -0.075986 Yaw(Y) 1.686430 Roll(Z) -0.349223 Error 45.381230

Como podemos ver, si dejamos de lado los pequeños errores, el movimiento se produce

de 1.6 grados en el eje Y, por lo tanto podremos decir que la auto localización

funcionaría de forma correcta en este caso pero con un pequeño error, que en parte será

debido al giro del robot y otro pequeño error del algoritmo de auto localización.

Prueba 4

De la misma forma que antes se ha querido comprobar los giros hacia la derecha. Para

esto se pone un obstáculo en el lado izquierdo del robot, por lo que el robot decidirá

girar hacia la derecha. Con ello, las imágenes que obtendremos son:

47

Y haciendo la filtración obtendremos:

Con lo que los resultados devueltos por el programa de auto localización son:

Nuevo Movimiento

Translation 0.006127 0.001168 -0.000707

Pitch(X) -0.033169 Yaw(Y) -1.923543 Roll(Z) 0.248585

Error 33.351548

Donde vemos que el movimiento realizado ha sido de 2 grados hacia la derecha, además

de los pequeños errores que están en torno a centésimas de milímetro en los

desplazamientos en los ejes y en menos de 0.3 grados en los giros sobre los ejes.

Una vez se ha visto el correcto funcionamiento del algoritmo de auto localización del

robot se probarán diversos movimientos libres, tanto por el laboratorio como por un

entorno exterior. Para estos casos ya no pondremos las imágenes con los puntos y los

filtrados dada la cantidad de pruebas que tenemos que hacer para verificar el

funcionamiento correcto y la extensión de éstas.

48

Prueba 5

Se ha querido realizar diversas pruebas en otras zonas del laboratorio de robótica y ésta

es una de ellas. En ella tenemos una secuencia de movimientos del robot en las que se

mueve de forma libre por el laboratorio, como podemos ver se realiza de forma correcta

tanto el movimiento como la auto localización

La secuencia de imágenes que obtenemos es:

Fig. 1 Fig.2

El movimiento realizado entre las figuras 1 y 2 es, supuestamente, de 50 cm. Hacia

delante. El resultado que nos devuelve la auto localización es:

Translation 0.027025 -0.016987 0.436839

Pitch(X) 0.428932 Yaw(Y) -0.176388 Roll(Z) 0.653457

Error 47.604457

En el encontramos pequeños errores de los giros en los ejes, pero el resultado más

importante es el de 43.6 cm. Hacia delante, que es lo que realmente se ha movido el

robot.

Fig. 3 Fig.4

49

Igual que antes el movimiento entre las figuras 2 y 3 es, otra vez supuestamente ya que

nos movemos siguiendo los sensores del robot, de 50 cm. Hacia delante, es decir, en el

eje Z del robot.

Los resultados son:

Translation 0.029868 -0.009686 0.523805


Error 59.713067

Vemos pequeños errores en los giros en torno al medio grado, y que el robot ha

avanzado una distancia realmente de 0.52 cm.

Otra vez tenemos un movimiento entre las figuras 3 y 4 hacia delante de 50 cm. Los

resultados obtenidos son:

Translation 0.056848 -0.011693 0.510023

Pitch(X) 0.583067 Yaw(Y) -0.064909 Roll(Z) -0.175913

Error 44.840349

Igual que en los movimientos anteriores tenemos algún pequeño error prácticamente

despreciable en los giros sobre los ejes. El movimiento real del robot ha sido de 0.51

cm. en su eje Z.

Fig. 5 Fig. 6

Entre las figuras 4 y 5 tenemos un avance de 50 cm.

Los resultados obtenidos son:

Translation 0.070247 -0.014282 0.519166

Pitch(X) -0.317659 Yaw(Y) 0.247615 Roll(Z) 0.129556

Error 55.394193

50

Como en todos los demás movimientos tenemos pequeños errores prácticamente

despreciables. El movimiento real ha sido de 51.9 cm.

Entre las figuras 5 y 6 volvemos a tener un avance de 50 cm.


Translation 0.061890 -0.016380 0.512949

Pitch(X) 0.630717 Yaw(Y) 0.063784 Roll(Z) 0.060798

Error 85.124385



Fig. 7 Fig. 8

Avance de 50 cm. entre las imágenes 6 y 7.


Translation 0.050451 -0.007752 0.515882


Error 59.558509



Avance de 50 cm. entre las figuras 7 y 8.

Resultados obtenidos:

Translation 0.048011 -0.005463 0.497446

Pitch(X) -0.221154 Yaw(Y) -0.129963 Roll(Z) -0.582447

Error 46.326806

Los errores vuelven a ser despreciables y el movimiento es correcto.

51

Fig. 9 Fig. 10

Entre las figuras 8 y 9 volvemos a tener un avance de 50 cm. Los resultados obtenidos

son:

Translation 0.037613 0.021406 0.396241


Error 98.816665

En este caso vemos que los errores son más grandes. Estos errores se producen por la

dificultad de emparejar los puntos de ambas imágenes.

Entre las imágenes 9 y 10 se ha realizado un giro, supuestamente de 5 grados hacia la

izquierda, dado que el robot da todo el terreno por explorado al físicamente no caber por

los espacios libres que quedan. Los resultados retornados por el algoritmo de auto

localización son:

Translation -0.005865 -0.000867 -0.001479

Pitch(X) 0.029426 Yaw(Y) 2.473567 Roll(Z) -0.070617

Error 37.417012

Con lo que vemos que el giro real captado por el algoritmo de auto localización es de

2.47 grados.

Fig. 11 Fig. 12

Entre las imágenes 10 y 11 volvemos a tener un giro de 5 grados al ser imposible seguir

recto y encontrar todo el espacio ocupado. Los resultados devueltos son:

52

Translation 0.019949 0.002149 -0.003365


Error 57.270717

Por lo que el giro captado es de 3.6 grados.

Viendo los resultados en esta prueba podemos afirmar que el programa de auto

localización funciona de forma correcta en la zona del laboratorio explorada.

Prueba 6

En esta prueba se ha querido comprobar el correcto funcionamiento del programa de

auto localización en el exterior. Los resultados obtenidos han sido los siguientes:

Fig. 1 Fig. 2

Entre las imágenes 1 y 2 de este apartado tenemos un giro hacia la derecha, por lo tanto

deberíamos tener un giro negativo sobre el eje Y. Los resultados que obtenemos con la

auto localización son:

Translation 0.017254 -0.002822 -0.000592


Error 27.874151

Como podemos observar, en este caso se realiza de forma correcta la auto localización,

ya que como se ha dicho con anterioridad los grados que gira el robot no son

exactamente los que le pasamos como parámetro a su función de movimiento.

53

Fig. 3 Fig. 4

Entre las imágenes 2 y 3 se realiza otro giro a la derecha, que comprobando el fichero

de movimientos vemos que ha sido de 2.56 grados. Los resultados son:

Translation 0.012204 -0.003518 -0.017699


Error 34.030696

Entre las imágenes 3 y 4 se debería haber hecho un giro hacia la izquierda de 5 grados,

pero dado que el robot se había quedado prácticamente sin batería, apenas hizo el giro

de unas décimas de grado. Esto ha sido captado por la cámara y, por medio del

algoritmo de auto localización, ha quedado reflejado en el fichero de movimientos. El

resultado obtenido es:

Translation -0.008246 0.005014 0.011453


Error 30.488299

Como se puede ver el giro tan sólo ha sido de 0.3 grados, por lo que en este caso

también ha funcionado de forma correcta el algoritmo de auto localización.

Prueba 7

Para finalizar haremos una prueba en el exterior. Debemos tener en cuenta cuando el

robot está funcionando en el exterior que la cámara tan sólo guarda los puntos

encontrados a una cierta distancia máxima, ya que los encontrados a una distancia

mayor se considerará que tienen un nivel de ruido demasiado elevado para tenerlos en

cuenta. Por lo tanto, para que funcione de forma correcta la auto localización, debería

haber puntos captados por debajo de esa distancia.

En esta prueba podemos comprobar lo que ocurre cuando los puntos que se pueden

captar son muy lejanos, con lo que los errores que encontraremos serán muy grandes.

54

Fig. 1 Fig. 2

Entre las imágenes 1 y 2 tenemos un avance de 50 cm. En cambio los resultados que

obtenemos del fichero de movimientos son:

Translation -0.415303 -0.009779 -0.003948


Error 51.638584

Como podemos ver, los datos son completamente erróneos, ya que el algoritmo de auto

localización detecta un giro de 6.8 grados hacia la derecha. Esto se debe a que

prácticamente no tenemos puntos, y los pocos que tenemos están a una distancia que

linda con la distancia máxima seleccionada para considerar que un punto es erróneo.

Fig. 3 Fig. 4

A medida que nos vamos acercando más hacia la barandilla los errores cometidos son

menores. Pongamos el ejemplo entre las imágenes 2 y 3, cuyos resultados son:

Translation 0.039373 -0.015876 0.160861


Error 61.111252

Como vemos en este caso los errores en los grados son más pequeños y ya se acerca

más al movimiento correcto, ya que se detecta que se ha movido el robot 16 cm. hacia

delante.

55

Entre las figuras 3 y 4 ya podemos encontrar un número de puntos mayor, por lo que es

de esperar que los resultados sean mejores que los anteriores. Los resultados obtenidos

son:

Translation -0.154947 -0.023010 0.208938


Error 55.529031

En este caso ya se detecta el avance de 20 cm.

Para solucionar estos casos se debería aumentar la distancia máxima a la que

consideramos un punto erróneo, ya que será mejor tener puntos con alguna desviación

que trabajar con los puntos con los que se ha trabajado en la última prueba. Además de

aumentar la distancia se debería adecuar el algoritmo de autolocalización para

situaciones donde tenemos varios puntos similares con un entorno también similar.

Una vez hemos validado el algoritmo de autolocalización, encontrando sus errores,

procedemos a hacer las pruebas pertinentes por todo el laboratorio y algunas más por el

exterior.

Prueba 8

El lugar desde donde empieza el robot es el siguiente:

Fig. 1

Por el pasillo el robot pasa sobrándole aproximadamente 20 cm. Por cada uno de sus

lados. Los movimientos que realiza el robot y los valores retornados por la auto

localización son:

56

Movimiento Translación slam3D Rotación slam3D

Hacia delante

X: -0.025038

Y: -0.041356

Z: 0.447597

Pitch(X): 1.530729

Yaw(Y): -0.917444

Roll(Z): -0.251264

Hacia delante

X:-0.000261

Y:0.003035

Z:0.495578

Pitch(X): 0.151632

Yaw(Y): -0.655127

Roll(Z): -0.056774

Hacia delante

X:0.152546

Y:-0.357287

Z:0.548065

Pitch(X):15.025910

Yaw(Y): 4.586093

Roll(Z): 0.216975

Giro hacia la derecha

X:0.016702

Y:-0.003961

Z: 0.036942

Pitch(X): 0.322939

Yaw(Y): -2.953108

Roll(Z): -0.031830


X: 0.009525

Y: -0.000326

Z: -0.004046

Pitch(X):-0.128891

Yaw(Y): -2.979805

Roll(Z): 0.236883


X: 0.023951

Y: -0.000304

Z: -0.002946

Pitch(X):0.028099

Yaw(Y): -2.787259

Roll(Z): -0.043730


X:0.008387

Y:0.002596

Z:-0.001597

Pitch(X):-0.115697

Yaw(Y): -2.738749

Roll(Z): -0.034546


X:0.015092

Y:-0.001518

Z:-0.003204

Pitch(X):0.082448

Yaw(Y):-2.872979

Roll(Z): 0.086270


X:0.014409

Y:0.001566

Z:-0.003205

Pitch(X):-0.076194

Yaw(Y):-3.066295

Roll(Z): 0.145288


X:0.014150

Y:-0.007633

Z:-0.004601

Pitch(X):0.303518

Yaw(Y):-2.782358

Roll(Z) 0.108322

Hacia delante

X:0.031233

Y:-0.032387

Pitch(X):1.697234

Yaw(Y):-2.701975

57

Z:0.435373 Roll(Z): 0.443707

Giro hacia la izquierda

X:-0.011302

Y:0.004318

Z:0.046500

Pitch(X):0.047806

Yaw(Y):1.596241

Roll(Z):-0.021628


X:-0.012500

Y:-0.007526

Z:-0.001652

Pitch(X): 0.337493

Yaw(Y): 2.082374

Roll(Z): 0.190753


X:0.002375

Y:0.003868

Z:-0.003146

Pitch(X):-0.107273

Yaw(Y): -2.566985

Roll(Z): -0.050412


X:-0.006064

Y:-0.004750

Z:0.003089

Pitch(X):0.242432

Yaw(Y): 2.087975

Roll(Z): -0.096117


X:-0.007886

Y:0.008515

Z:0.002685

Pitch(X):-0.310060

Yaw(Y): 2.502797

Roll(Z): -0.273324


X:-0.030975

Y:-0.001595

Z:0.003786

Pitch(X):0.173393

Yaw(Y):1.846465

Roll(Z): -0.022388


X:-0.026542

Y:-0.000161

Z:0.003742

Pitch(X):0.022794

Yaw(Y): 2.416044

Roll(Z): -0.034487


X:-0.010614

Y:0.007571

Z:0.007740

Pitch(X):-0.364546

Yaw(Y): 2.641406

Roll(Z): -0.367083


X:-0.027216

Y:0.004899

Z:-0.000320

Pitch(X):-0.238927

Yaw(Y): 2.048639

Roll(Z): -0.120321


X:-0.021957

Y:0.002313

Z:0.002534

Pitch(X):-0.038846

Yaw(Y):2.287533

Roll(Z):-0.207727


X:-0.013860

Y:0.013607

Pitch(X):-0.583342

Yaw(Y): 1.941597

58

Z:0.008025 Roll(Z): -0.354822

De esta prueba debemos destacar el error que se produce en la tercera línea, donde nos

devuelve un giro sobre el eje X de más de 15 grados, cuando sabemos por la naturaleza

de nuestro robot que es imposible que gire en torno al eje X. Las fotografías en las que

se produce el error son las que podemos ver en las figuras 2 y 3 de la prueba actual.

Fig. 2 Fig. 3

El error se produce al tener 2 sillas iguales con un entorno prácticamente idéntico, por lo

que emparejar los puntos se transforma en una tarea realmente difícil y provoca el error

que se ha detectado.

Prueba 9

En este caso hacemos pruebas en otra zona del laboratorio, en la que tenemos más

espacio libre. Sin embargo el robot empieza desde una zona donde no puede seguir

recto, por lo que tendrá que ir girando hasta encontrar vía libre. La zona desde donde

empezamos la podemos ver en la figura 1 de esta prueba.

Fig. 1

59

Como podemos observar en la imagen anterior, si el robot siguiera recto colisionaría

contra las sillas, por lo que presumiblemente deberá esquivarlas para seguir recto más

tarde. En la secuencia de movimientos tenemos los movimientos necesarios para

esquivar las sillas, además de los de avance hasta el robot y los de esquivar el armario

que se ve al fondo de la fotografía.

Los movimientos que realiza el robot con su auto localización son:

Movimiento Translación Rotación


X:-0.008464

Y:-0.009837

Z:-0.013407

Pitch(X): 0.206373

Yaw(Y): -3.499528

Roll(Z): 0.135861


X:0.017624

Y:0.005676

Z:-0.003458

Pitch(X): -0.108570

Yaw(Y): -2.674218

Roll(Z): 0.035734


X:0.016179

Y:-0.001528

Z:0.000259

Pitch(X): -0.016018

Yaw(Y): -2.194870

Roll(Z): 0.120674


X:0.011527

Y:0.003214

Z:-0.000084

Pitch(X): -0.056364

Yaw(Y): -2.833463

Roll(Z): 0.142104

Hacia delante

X:0.032906

Y:-0.011344

Z:0.437085

Pitch(X): 0.149844

Yaw(Y): -2.824960

Roll(Z): -0.066952

Hacia delante

X: 0.006324

Y:0.001572

Z:0.518604

Pitch(X): 0.522445

Yaw(Y): -0.663863

Roll(Z): -0.313589

Hacia delante

X:-0.003225

Y:0.001382

Z:0.478284

Pitch(X): -0.111430

Yaw(Y): -0.345013

Roll(Z): -0.100012

Hacia delante

X:-0.000289

Y:-0.007702

Z:0.473848

Pitch(X): 0.881197

Yaw(Y): -0.366739

Roll(Z): 0.037479

Hacia delante

X: 0.014157

Y:0.009000

Pitch(X): 0.484769

Yaw(Y): -0.646705

60

Z:0.479544 Roll(Z): -0.198617


X:-0.020714

Y:0.010239

Z:0.047776

Pitch(X): -0.282561

Yaw(Y): 1.341480

Roll(Z): -0.248864


X:-0.017116

Y:0.000470

Z:0.005944

Pitch(X): -0.028086

Yaw(Y): 2.258349

Roll(Z): -0.118047


X:-0.029529

Y:0.004300

Z:0.005846

Pitch(X): -0.184490

Yaw(Y): 2.575656

Roll(Z): -0.384130


X:-0.019713

Y:-0.003640

Z:0.007850

Pitch(X): 0.039008

Yaw(Y): 2.589590

Roll(Z): 0.073263


X:-0.015931

Y:-0.001004

Z:0.004858

Pitch(X): -0.041041

Yaw(Y): 2.261453

Roll(Z): 0.026471


X:-0.018715

Y:0.004432

Z:0.004815

Pitch(X): -0.126588

Yaw(Y): 2.537314

Roll(Z): -0.130764

Hacia delante

X:-0.032430

Y:0.004710

Z:0.420759

Pitch(X): 0.085974

Yaw(Y): 1.400738

Roll(Z): 0.050912


X:-0.021249

Y:0.007920

Z:0.049472

Pitch(X): -0.014443

Yaw(Y): 2.257303

Roll(Z): -0.297495


X:-0.013605

Y:0.002592

Z:0.005734

Pitch(X): -0.077481

Yaw(Y): 1.953237

Roll(Z): -0.050468


X:-0.016127

Y:-0.000855

Z:0.004932

Pitch(X): -0.003222

Yaw(Y): 2.978340

Roll(Z): 0.012368


X:-0.011962

Y:-0.000514

Pitch(X): -0.040714

Yaw(Y): 2.624778

61

Z:0.005540 Roll(Z): -0.135780


X:-0.016622

Y:0.000605

Z:0.001454

Pitch(X): -0.015565

Yaw(Y): 2.285519

Roll(Z): -0.166826


X:-0.008364

Y:0.002874

Z:0.001760

Pitch(X): -0.070454

Yaw(Y): 2.373462

Roll(Z): -0.147454


X:-0.010063

Y:0.002426

Z:0.005477

Pitch(X): -0.075650

Yaw(Y): 2.443271

Roll(Z): -0.025933


X:-0.018750

Y:0.002391

Z:0.004095

Pitch(X): -0.114749

Yaw(Y): 2.813545

Roll(Z): 0.064643

Hacia delante

X:-0.002088

Y:-0.212291

Z:0.436182

Pitch(X): 8.767062

Yaw(Y): 0.534738

Roll(Z): 4.885701


X:-0.016291

Y:-0.004206

Z:0.039724

Pitch(X): 0.289157

Yaw(Y): 2.054444

Roll(Z): -0.140736


X:-0.018345

Y:0.013810

Z:0.004181

Pitch(X): -0.443216

Yaw(Y): 2.536514

Roll(Z): -0.203841


X:-0.018402

Y:-0.002540

Z:0.003280

Pitch(X): -0.032516

Yaw(Y): 2.018926

Roll(Z): -0.234619


X:-0.016293

Y:0.017532

Z:0.009547

Pitch(X): -0.649780

Yaw(Y): 2.706855

Roll(Z): -0.102938


X:-0.009688

Y:0.018160

Z:0.009766

Pitch(X): -0.532750

Yaw(Y): 2.695436

Roll(Z): -0.294966


X:-0.009544

Y:0.006807

Pitch(X): -0.258879

Yaw(Y): 2.811025

62

Z:0.010118 Roll(Z): -0.111452


X:0.013013

Y:0.025139

Z:0.013043

Pitch(X): -0.886490

Yaw(Y): 3.109017

Roll(Z): -0.314300


X:-0.050033

Y:-0.011109

Z:0.000015

Pitch(X): 0.373021

Yaw(Y): 1.870358

Roll(Z): -0.197722


X:-0.049667

Y:-0.015402

Z:-0.002665

Pitch(X): 0.539470

Yaw(Y): 1.639693

Roll(Z): -0.360696


X:-0.027077

Y:0.011118

Z:0.008479

Pitch(X): -0.298857

Yaw(Y): 2.113409

Roll(Z): 0.041772


X:0.027025

Y:-0.011215

Z:-0.002630

Pitch(X): 0.366275

Yaw(Y): -1.698186

Roll(Z): 0.274576


X:-0.028186

Y:0.001285

Z:0.001716

Pitch(X): -0.029412

Yaw(Y): 1.363482

Roll(Z): -0.253117


X:0.033481

Y:-0.016004

Z:-0.002355

Pitch(X): 0.559590

Yaw(Y): -1.849041

Roll(Z): 0.250714


X:-0.016087

Y:0.013350

Z:0.004345

Pitch(X): 0.055361

Yaw(Y): 1.756979

Roll(Z): -0.315693


X:-0.020593

Y:0.005908

Z:0.003727

Pitch(X): -0.462842

Yaw(Y): 2.417547

Roll(Z): -0.000572


X:-0.029408

Y:0.003423

Z:0.002278

Pitch(X): -0.184237

Yaw(Y): 2.068764

Roll(Z): -0.045393


X:-0.028596

Y:-0.006986

Pitch(X): -0.094882

Yaw(Y): 2.010413

63

Z:0.002976 Roll(Z): -0.097220


X:0.019792

Y:0.003300

Z:0.006545

Pitch(X): 0.367856

Yaw(Y): 2.684426

Roll(Z): 0.169335


X:-0.016093

Y:0.003561

Z:0.002147

Pitch(X): -0.049689

Yaw(Y): 2.611442

Roll(Z): -0.166791


X:-0.044757

Y:-0.001190

Z:0.002745

Pitch(X): -0.101333

Yaw(Y): 2.009313

Roll(Z): -0.191360


X:-0.020338

Y:-0.008697

Z:0.002248

Pitch(X): 0.055970

Yaw(Y): 2.090032

Roll(Z): -0.301255


X:-0.016959

Y:-0.003320

Z:0.009253

Pitch(X): -0.005345

Yaw(Y): 2.392916

Roll(Z): 0.282268

De esta prueba demos destacar los errores que se producen en el movimiento número

24, en el que tan sólo se avanza hacia delante y el programa de auto localización nos

devuelve que además de avanzar hacia delante se ha producido un giro de más de 8

grados en el eje X y de más de 4 en el eje Z. Las fotografías que recibe el programa de

auto localización para este movimiento se pueden ver en las figuras 2 y 3 de esta

prueba.

Fig. 2 Fig. 3

El emparejamiento de puntos se complica al distinguir pocos puntos, además de que de

una fotografía a la otra perdemos gran cantidad de éstos.

64

Prueba 10

Esta prueba es similar a la número 8, pero se ha realizado en otro de los pasillos creados

por el mobiliario del laboratorio. El punto de comienzo de la prueba se puede apreciar

en la figura 1 de esta prueba.

Fig. 1

Previsiblemente el robot debería girar hacia la derecha para esquivar la silla y después ir

hacia delante. Los resultados obtenidos son:


Giro hacia la derecha X:0.024296

Y:-0.002108

Z:-0.009255

Pitch(X): 0.016355

Yaw(Y): -2.214464

Roll(Z): -0.094992


Y:-0.000911

Z:-0.000886

Pitch(X): 0.023487

Yaw(Y): -2.621179

Roll(Z): 0.188704


Y:0.000117

Z:0.003572

Pitch(X): -0.017062

Yaw(Y): -2.918615

Roll(Z): 0.236591

Hacia delante X:0.018864

Y:0.005693

Z:0.457929

Pitch(X): 0.446560

Yaw(Y): -3.244366

Roll(Z): 0.054514

Hacia delante X:0.014985 Pitch(X): 4.266712

65

Y:-0.150680

Z:0.452636

Yaw(Y): -0.088075

Roll(Z): -0.338958

Giro hacia la izquierda X:-0.008750

Y:0.001055

Z:0.056689

Pitch(X): 0.128865

Yaw(Y): 1.045870

Roll(Z): 0.000553


Y:0.005258

Z:0.005539

Pitch(X): -0.181417

Yaw(Y): 1.925846

Roll(Z): 0.085577

Hacia delante X:-0.031805

Y:0.007668

Z:0.438251

Pitch(X): 0.333823

Yaw(Y): 1.529216

Roll(Z): -0.082681


Y:0.001413

Z:0.049728

Pitch(X): 0.041448

Yaw(Y): -2.135647

Roll(Z): 0.072588


Y:0.000293

Z:0.001972

Pitch(X): 0.009628

Yaw(Y): 0.852487

Roll(Z): -0.064549


Y:-0.003194

Z:0.000430

Pitch(X): 0.104028

Yaw(Y): -1.332013

Roll(Z): -0.016068

En este caso volvemos a tener errores en la auto localización, aunque son más pequeños.

En este caso en concreto, en los movimientos 4 y 5 vemos esas desviaciones. Para el

movimiento 4 miraremos las figuras 2 y 3 de esta prueba y para el movimiento 5 las

figuras 4 y 5.

Fig. 2 Fig. 3

66

Fig. 4 Fig. 5

Prueba 11

Se realiza una nueva prueba empezando desde otra zona del laboratorio, en concreto

desde la puerta de éste, como se puede observar en la figura 1 de esta prueba.

Fig. 1

Como podemos ver en la imagen el robot deberá esquivar el perchero y avanzar hacia la

izquierda. En este caso en concreto el robot avanzará y detectará que hay mayor espacio

libre hacia la derecha, por lo que girará a la derecha y caerá en un mínimo local, que

solucionará girando hacia la izquierda y posteriormente salvará el obstáculo.

Los movimientos, así como la auto localización son:

Movimiento Translación Slam3D Rotación Slam3D

Hacia delante

X:0.008790

Y:-0.012070

Z:0.422387

Pitch(X): 0.449356

Yaw(Y): -0.250765

Roll(Z): -0.782364

67

Giro a la derecha

X:0.017938

Y:-0.002266

Z:0.040288

Pitch(X): 0.192059

Yaw(Y): -2.586682

Roll(Z): -0.021082

Giro a la derecha

X:0.021149

Y:0.005315

Z:-0.010444

Pitch(X): -0.251528

Yaw(Y): -2.592897

Roll(Z): 0.705127

Giro a la derecha

X:0.015587

Y:-0.006918

Z:-0.006634

Pitch(X): 0.255954

Yaw(Y): -2.810922

Roll(Z): 0.220313

Giro a la derecha

X:0.040242

Y:0.003352

Z:-0.004482

Pitch(X): -0.086052

Yaw(Y): -1.515175

Roll(Z): 0.206257

Giro a la derecha

X:0.021375

Y:-0.008429

Z:-0.003832

Pitch(X): 0.349125

Yaw(Y): -3.409425

Roll(Z): -0.074440

Giro a la izquierda

X:-0.001255

Y:-0.002510

Z:0.002184

Pitch(X): 0.117872

Yaw(Y): 1.285112

Roll(Z): -0.105863

Giro a la derecha

X:0.007434

Y:-0.003784

Z:0.001655

Pitch(X): 0.138158

Yaw(Y): -1.315951

Roll(Z): -0.137169

Giro a la izquierda

X:-0.005083

Y:0.005501

Z:0.003466

Pitch(X): -0.179901

Yaw(Y): 0.949683

Roll(Z): 0.175105

Giro a la izquierda

X:-0.017796

Y:0.001233

Z:0.009295

Pitch(X): 0.017606

Yaw(Y): 1.525284

Roll(Z): -0.144648

Giro a la izquierda

X:-0.020315

Y:0.007096

Z:0.012905

Pitch(X): -0.232936

Yaw(Y): 3.176463

Roll(Z): -0.134274

Giro a la izquierda

X:-0.014188

Y:0.003784

Z:0.008763

Pitch(X): -0.096401

Yaw(Y): 1.725375

Roll(Z): -0.148562

68

Giro a la izquierda

X:-0.022328

Y:0.000188

Z:0.013493

Pitch(X): -0.034796

Yaw(Y): 3.205469

Roll(Z): -0.127265

Giro a la izquierda

X:-0.018254

Y:0.007782

Z:0.009899

Pitch(X): -0.217144

Yaw(Y): 2.450013

Roll(Z): 0.177632

Giro a la izquierda

X:-0.018184

Y:-0.003159

Z:0.005326

Pitch(X): 0.112559

Yaw(Y): 2.323020

Roll(Z): -0.401418

Giro a la izquierda

X:-0.013580

Y:-0.005373

Z:0.008248

Pitch(X): 0.156751

Yaw(Y): 2.624120

Roll(Z): -0.243435

Giro a la izquierda

X:-0.020341

Y:-0.000209

Z:0.008628

Pitch(X): -0.018165

Yaw(Y): 3.034866

Roll(Z): -0.165243

Giro a la izquierda

X:-0.004203

Y:-0.000405

Z:0.023536

Pitch(X): 0.009998

Yaw(Y): 2.294029

Roll(Z): -0.077538

Giro a la izquierda

X:-0.032601

Y:0.001893

Z:0.013788

Pitch(X): -0.015718

Yaw(Y): 2.649333

Roll(Z): -0.236554

Giro a la izquierda

X:-0.017784

Y:0.006430

Z:0.005875

Pitch(X): -0.089299

Yaw(Y): 2.719156

Roll(Z): -0.431660

Hacia delante

X:-0.034401

Y:-0.020239

Z:0.456717

Pitch(X): 0.576032

Yaw(Y): 2.112059

Roll(Z): 1.093568

Hacia delante

X:0.101660

Y:-0.078064

Z:0.472147

Pitch(X): 1.255960

Yaw(Y): 2.081599

Roll(Z): 2.449282

En esta prueba debemos destacar los 2 últimos movimientos hacia delante. En ellos

vemos giros en sus 3 ejes. Esto tiene una explicación, ya que el robot en estos

69

movimientos está encima de la base del colgador (Fig. 2 y 3), por lo que realmente hay

variaciones en los 3 ejes del robot.

Fig. 2 Fig. 3

Prueba 12

Por último realizaremos otra prueba en el exterior, comprobando que la auto

localización funciona de forma correcta en el caso en el que los puntos no sean

extremadamente difíciles de emparejar dado su entorno.

El lugar desde donde empezamos es:

Fig. 1

El robot deberá seguir recto y girar para esquivar la pared.



Hacia delante X:-0.014839

Y:-0.001578

Z:0.440937

Pitch(X): 0.363264

Yaw(Y) :-0.477288

Roll(Z): -0.021270

Hacia delante X:0.038989

Y:0.016720

Pitch(X): 0.661094

Yaw(Y): 0.655985

70

Z:0.363177 Roll(Z): 0.420870

Giro a la izquierda X:-0.007091

Y:0.006682

Z:0.054636

Pitch(X): -0.097834

Yaw(Y): 0.890368

Roll(Z): 0.029278

Giro a la izquierda X:-0.034172

Y:-0.010514

Z:-0.017462

Pitch(X): 0.121757

Yaw(Y): 5.355554

Roll(Z): -0.166978

Como podemos observar en este caso y, a diferencia del anterior hecho en exteriores,

vemos que funciona de forma correcta el algoritmo de auto localización. La diferencia

entre el anterior, que no funcionaba de forma correcta, y éste es que en este caso los

puntos y su entorno son diferentes, mientras que en el anterior no lo era, por lo que se

emparejaba de forma incorrecta.

71

6.8 Conclusiones

En este apartado hemos realizado la integración y la evaluación del algoritmo de auto

localización que se nos ha cedido.

Se ha realizado de forma correcta y satisfactoria la integración del programa de auto

localización slam3D, demostrando por medio de las pruebas que no hay ningún

problema en la integración de dicho programa.

Se han realizado todas las pruebas necesarias para poder afirmar que el programa de

auto localización funciona de forma correcta con pequeñas limitaciones. Estas

limitaciones son:

1. Fotografías en la que no somos capaces de identificar ningún punto.

2. Pequeños errores producidos por el algoritmo y por las distancias captadas por la

cámara.

3. Trabajar con puntos muy lejanos.

Se ha propuesto e implementado soluciones para la primera de las limitaciones, que

consiste en no pasar el fichero al programa, sino que realizaremos otro movimiento y

haremos la auto localización respecto a 2 movimientos.

Para la segunda de las limitaciones no podemos encontrar una solución, ya que depende

de la calibración de la cámara y de su resolución, así como de su SDK Triclops.

Con todo lo visto podemos afirmar que se han cumplido todos los objetivos marcados

en este apartado.

72

7. Desarrollo del proyecto

El proyecto se ha dividido en los siguientes apartados que se han ido realizando de

forma cronológica:

1. Estudio de las librerías del robot, así como las de la cámara.

2. Implementación del algoritmo que controla los movimientos del robot.

3. Pruebas y evaluación del programa que controla los movimientos del robot.

4. Soluciones a los problemas detectados en el programa controlador de los

movimientos del robot.

5. Estudio del algoritmo Slam3D para la auto localización del robot.

6. Integración del programa de auto localización con el programa controlador de los

movimientos del robot.

7. Pruebas y evaluación de los programas de control de movimientos y auto localización

integrados en el robot.

8. Soluciones a los problemas encontrados.

73

8 Conclusiones

Atendiendo a los objetivos que teníamos al empezar el proyecto podemos decir que se

han cumplido de forma satisfactoria, de manera que:

- Se ha realizado el programa para controlar los movimientos del robot realizando

antes un estudio exhaustivo de las librerías que controlan el movimiento del

robot, su estado y el funcionamiento de los sensores de ultrasonidos.

- Se han realizado las pruebas pertinentes para poder comprobar que funciona el

programa controlador de los movimientos del robot, incluyendo pruebas en

casos extremos como por ejemplo los mínimos locales.

- Se han buscado los errores del programa que controla los movimientos del robot

así como sus limitaciones, aplicado las soluciones pertinentes a los problemas

encontrados.

- Se ha realizado el estudio del programa de auto localización cedido para la

realización del proyecto, encontrando en él sus limitaciones.

- Se ha integrado de forma satisfactoria el programa de auto localización del robot

con el programa controlador de sus movimientos.

- Se ha comprobado, mediante diversas pruebas en diferentes entornos, el correcto

funcionamiento del programa de auto localización del robot.

74

9. Bibliografía y recursos utilizados

Programación en C, Luis Joyanes Aguilar e Ignacio Zahonero Martínez.

Editorial McGrawHill.

Manuales Aria-Reference y Saphira-Reference, que hacen referencia al uso del

robot Pioneer P2AT.

Páginas web consultadas:

Información sobre la medición de distancias mediante ultrasonidos:

http://picmania.garcia-cuervo.com/Proyectos_AUX_SRF04.htm

Información sobre el robot Pioneer P2AT:

http://www.activrobots.com/ROBOTS/p2at.html

Información sobre la cámara digiclops:

http://www.robosoft.fr/SHEET/06Vision/1004DIGICLOPS/digiclops.pdf

Programas auxiliares:

Programas de visión de imágenes:

1. Gimp

2. XinView

Programas para visualización de gráficas a partir de puntos:

1. Gnuplot.

75

10. Manual de uso

Para el uso del programa lo primero será encender el robot por el interruptor situado en

uno de sus laterales (fig. 7).

Fig. 7

Cuando el robot haya arrancado se deberá establecer una conexión por medio de bule-

tooth con el robot. La conexión no debe ser necesariamente por blue-tooth, pero no es

recomendable hacerla directamente por cable, ya que el robot se mueve, con lo que los

movimientos estarán limitados por la longitud del cable. Una vez realizada la conexión,

nos deberemos conectar por medio de una conexión ssh a la I.P. del robot, que por

defecto será 10.0.0.2, de manera que el comando será:

ssh [email protected]

Una vez hayamos establecido la conexión por medio del túnel ssh compilaremos el

programa en el caso en el que sea la primera vez que se ejecuta. Para compilarlo se ha

realizado un fichero de makefile, de manera que para compilarlo simplemente

tendremos que utilizar el comando make.

Una vez compilado el programa lo ejecutaremos:

./example7 > fichero_resultados

Redireccionamos la salida a un fichero para liberar a la conexión blue-tooth de la

transmisión de datos y que tenga que transmitir lo mínimo. esto no es estrictamente

necesario, y depende más de las necesidades del usuario para guardar los movimientos

que supuestamente ha realizado el robot.

Una vez ejecutado el programa se nos preguntará si queremos ejecutarlo en el modo

guiado o en el modo no guiado. En el caso del modo guiado podremos consultar para

cada movimiento todos los ficheros temporales. En el modo no guiado sólo podremos

consultar el Fichero_movimientos, que guardará todos los movimientos realizados por

el robot que se han detectado por la auto localización.

76

En el caso del modo guiado se consultará al usuario antes de realizar cada movimiento

si se puede realizar dicho movimiento, por lo que se podrán consultar los siguientes

ficheros:

Puntos_sensor fichero de puntos seleccionados por el robot para hacer el mapa para

la decisión del giro.

Out*.ppm imagen captada.

Out*.pts puntos de la imagen.

C0,c1 ficheros con las imágenes utilizadas para la auto localización con los puntos

identificados y emparejados.

d0,d1,f0,f1 los mismos ficheros que c0 y c1 pero filtrados los puntos erróneos y con

la ponderación para encontrar la distancia correcta que se ha movido el robot.

A parte de estos ficheros se podrá consultar el fichero de movimientos en cualquier

momento.

En el modo guiado para salir se deberá pulsar la tecla ‘n’ cuando se pida antes de

cualquier movimiento. En el modo no guiado para salir pulsar cualquier tecla en

cualquier momento.

Date post:	24-Sep-2018
Category:	Documents
Upload:	hoangngoc
View:	212 times
Download:	0 times

Proyecto final de carrera - Departament d'Enginyeria...

Documents