trabajo de fin de máster máster en ingeniería...

60
Nuevo título Autor: Alejandro Muñoz Cueva Tutores: José Antonio Cobano Suárez José Guillermo Heredia Benot Dep. Ingeniería de Sistemas y Automática Universidad Internacional de Andalucía Sevilla, 2016 Trabajo de Fin de Máster Máster en Ingeniería Industrial Método para identificación y seguimiento de objetos usando un sensor de rango

Upload: others

Post on 16-Aug-2020

49 views

Category:

Documents


0 download

TRANSCRIPT

Page 1: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

Nuevo título

Autor: Alejandro Muñoz Cueva

Tutores: José Antonio Cobano Suárez

José Guillermo Heredia Benot

Dep. Ingeniería de Sistemas y Automática

Universidad Internacional de Andalucía

Sevilla, 2016

Trabajo de Fin de Máster

Máster en Ingeniería Industrial

Método para identificación y seguimiento de

objetos usando un sensor de rango

Page 2: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

ii

Page 3: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

iii

Trabajo Fin de Máster

Máster en Ingeniería Industrial

Método para identificación y seguimiento de objetos

usando un sensor de rango

Autor:

Alejandro Muñoz Cueva

Tutores:

José Antonio Cobano Suárez

José Guillermo Heredia Benot

Dep. de Ingeniería de Sistemas y Automática

Universidad Internacional de Andalucía

Sevilla, 2016

Page 4: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método
Page 5: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

Trabajo Fin de Máster: Método para identificación y seguimiento de objetos usando un sensor de rango

Autor: Alejandro Muñoz Cueva

Tutores: José Antonio Cobano Suárez y

José Guillermo Heredia Benot

El tribunal nombrado para juzgar el Proyecto arriba indicado, compuesto por los siguientes miembros:

Presidente:

Vocales:

Secretario:

Acuerdan otorgarle la calificación de:

Sevilla, 2016

El Secretario del Tribunal

Page 6: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método
Page 7: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

i

Agradecimientos

Mi más sincero agradecimiento a mi tutor, José Antonio Cobano, cuya atención y orientación ha sido

fundamental para la consecución de este trabajo.

Page 8: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

ii

Resumen

Este trabajo presenta técnicas para el posicionamiento relativo haciendo uso de un sensor de profundidad, ya

sea un láser que realiza barridos en un plano u otros sensores de profundidad que proporcionen valores de

rango en tres dimensiones. En concreto, dentro de la navegación en interiores, se afronta con mayor

profundidad el problema de identificación y seguimiento de elementos móviles de referencia por parte de un

robot móvil. Así, el objetivo es dotar a un robot móvil de la información que le permita mantener una posición

fija relativa a un elemento de referencia en el entorno, de forma que se conozcan los movimientos que debería

realizar el robot móvil para recuperar dicha posición relativa, en caso de haberla perdido. Por tanto se ha

implementado un algoritmo de identificación y seguimiento del objeto de referencia, utilizando sólo la

información de un sensor de rango. Todos los métodos presentados en este trabajo están basados en el

algoritmo ICP (Iterative Closest Points), mostrándose desde un punto de vista práctico su aplicación a la

navegación utilizando sensores de profundidad en 2 y 3 dimensiones. Por otro lado, también se presentan

estrategias de aplicación del algoritmo ICP para afrontar un problema más general, como es el problema del

posicionamiento con respecto a un sistema de referencia fijo.

Page 9: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

iii

Índice

Agradecimientos i

Resumen ii

Índice iii

Índice de Figuras v

1 Introducción 1 1.1. Objetivo del trabajo 1 1.2. Estructura del trabajo 2

2 Formulación del problema 4 2.1. Situaciones adversas para los métodos tradicionales 4 2.2. Mantenimiento de una posición fija respecto a una referencia, en situaciones realistas 6

3 Algoritmo ICP y su aplicación a la navegación de robots móviles 9 3.1. Introducción 9 3.2. El algoritmo ICP 10

3.2.1. Aplicación del algoritmo ICP a la robótica móvil 10 3.2.2. Desglose del algoritmo ICP 10

3.3. Principales librerías utilizadas 12 3.3.1. Librería MRPT 13 3.3.2. Paquete mrpt bridge 14

3.4. Algoritmos implementados para el seguimiento de referencias 15 3.4.1. Algoritmo en 2 dimensiones 15 3.4.2. Algoritmo en 3 dimensiones 16

3.5. Aplicación del algoritmo ICP para la estimación de la posición respecto a un sistema de referencia fijo 18

3.5.1. Algoritmo en 2 dimensiones 18 3.5.2. Algoritmo en 3 dimensiones 19

4 Identificación y seguimiento de referencias 20 4.1. Problemática y necesidad de un filtrado 20 4.2. Implementación del filtro 23

4.2.1 Filtrado por distancia 23 4.2.2 Filtrado por dirección 26 4.2.3 Filtrado por pendiente 30 4.2.4 Algoritmo completo 32

4.3. Casuísticas menos habituales que pueden afectar a los resultados 33

5 Experimentos 35 5.1. Introducción 35 5.2. Seguimiento de elementos móviles 36

5.2.1 Aplicación del filtrado 39 5.3. Aplicación al posicionamiento respecto a un sistema de referencia fijo 43

6 Conclusiones y trabajo futuro 46

Page 10: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

iv

6.1. Conclusiones 46 6.2. Trabajo futuro 47

7 Referencias 48

Page 11: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

v

ÍNDICE DE FIGURAS

Figura 2.1: Esquema del control de navegación tomando señales GPS 5

Figura 2.2: Posición correspondiente a la referencia buscada, y lectura del sensor de rango. 6

Figura 2.3: Posición relativa distinta a la de referencia, y medida del sensor de rango. 7

Figura 2.4: Situación final en la que se ha recuperado la posición relativa buscada, y medida del sensor. 7

Figura 3.1 Esquema del algoritmo ICP 11

Figura 3.2. Matching entre dos figuras de 8 puntos 11

Figura 3.3: Ejemplo con 3 iteraciones del algoritmo ICP 12

Figura 3.4: Ejemplo de dos nubes de puntos a alinear 13

Figura 3.5: Papel del paquete mrpt_bridge. 14

Figura 3.6: Esquema de entrada de la información de un láser 2D. 15

Figura 3.7: Representación de dos nubes de puntos a alinear. 17

Figura 3.8: Cambio de orientación del robot 19

Figura 4.1: Representación de las medidas recogidas con el sensor de rango en la situación de referencia, y ejes

cuerpo del robot. Objeto a seguir resaltado por el círculo rojo. 21

Figura 4.2: Medidas tomadas por el sensor de rango en una situación en la que el cubo se ha desplazado. 22

Figura 4.3: Representación de las nubes de puntos correspondientes a patrón (izquierda) y medida a alinear

(derecha) que permitirían recuperar el patrón mediante el algoritmo ICP. 22

Figura 4.4: Representación del robot móvil (azul) y objeto a seguir (gris). El robot móvil está orientado

inicialmente mirando frontalmente hacia el objeto. 24

Figura 4.5: Representación de la distancia considerada para un punto 𝑷𝒊, así como el ángulo y la profundidad

correspondientes. 24

Figura 4.6: Representación de la estrategia seguida para medir la distancia media inicial. 25

Figura 4.7: Representación de la amplitud de un sensor láser situado solidario a un robot móvil. 26

Figura 4.8: Representación de la amplitud del filtro (zona coloreada) frente a la amplitud del sensor (zona

discontinua). 27

Figura 4.9: Representación de la zona de búsqueda (sombreado), objeto, robot, dirección principal del filtro

(línea gruesa discontinua) y magnitudes medidas, en el instante inicial. 28

Figura 4.10: Paso intermedio para la actualización del espacio de búsqueda del filtro, tras un desplazamiento

del objeto de referencia. 28

Figura 4.11: Representación del nuevo espacio de búsqueda del filtro, una vez recalculada la dirección

principal. 29

Figura 4.12: Representación de situación de referencia buscada (izquierda) y situación tras un movimiento

relativo en cierto instante (derecha). 30

Figura 4.13: Representación de la nube de puntos devuelta por el filtro, y posible movimiento erróneo del

robot móvil. 31

Figura 4.14: Ilustración del cálculo de la pendiente del punto i respecto al i-1. 32

Figura 4.15: Situación en la que se observan frontalmente 3 objetos distintos, siendo el central el que se desea

seguir. 33

Figura 4.16: Representación del cálculo para la corrección de la amplitud del patrón, tras la detección del

objeto individual. 34

Page 12: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

vi

Figura 5.1: Simulación de un robot Turtlebot en un entorno creado con Gazebo 36

Figura 5.2: Posición inicial y final tras movimiento de la caja, simulado en Gazebo. 37

Figura 5.3: Posición inicial del Turtlebot, correspondiente con la referencia buscada. 37

Figura 5.4: Posición a la que se lleva el robot de forma manual. 38

Figura 5.5: Situación final en la que el robot ha recuperado la posición relativa a la caja 38

Figura 5.6: Medidas del sensor láser sin filtrar, correspondientes con la situación de referencia. 39

Figura 5.7: Representación de las medidas resultantes del filtrado que identifican el objeto. 39

Figura 5.8: Representación de las medidas del sensor láser tras un desplazamiento del cajón, resaltado por el

círculo rojo. 40

Figura 5.9: Representación de la nube de puntos posterior al filtrado de las medidas de la Figura 5.8. 40

Figura 5.10: Nube de puntos posterior al filtrado de las medidas de la Figura 5.8, incluyendo el filtrado por

distancia. 41

Figura 5.11: Medidas recogidas por el sensor, tras un desplazamiento del cajón de referencia. 41

Figura 5.12: Nube de puntos resultante de filtrar la Figura 5.11, para el seguimiento de la cara frontal. 42

Figura 5.13: Posición inicial, en la que le robot móvil se encuentra frente al cajón que se pretende seguir, a una

distancia de referencia determinada. 42

Figura 5.14: Situación resultante del movimiento del cajón por la habitación. 43

Figura 5.15: Posición inicial y final del robot al comandarle un avance de 2.5 metros. 44

Figura 5.16: Posición inicial y final del robot al comandarle un avance de 2.5 metros, a velocidad muy baja.

44

Figura 5.17: Representación del movimiento del Hokuyo según el algoritmo ICP 45

Page 13: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

1

1 INTRODUCCIÓN

l uso de robots móviles para diversas aplicaciones es cada vez más común a medida que avanza la

tecnología. Conforme se avanza en la complejidad de las tareas desempeñadas por dichos robots

móviles, surge la necesidad de que éstos sean capaces de desenvolverse con destreza en entornos cada

vez más complejos y poblados de obstáculos, así como en escenarios donde quizás no sean adecuadas las

estrategias utilizadas tradicionalmente en este campo.

Un requisito fundamental para que un robot móvil sea capaz de realizar una misión autónomamente es que

éste tenga la capacidad de posicionarse en el entorno, es decir, de conocer con relativa precisión la posición

que ocupa en cada instante, ya sea respecto a un sistema de referencia fijo o respecto a algún punto de su

entorno. Si se consigue resolver este problema, la navegación se podrá llevar a cabo para conseguir el objetivo

final de que los robots móviles sean capaces de operar con la mayor autonomía posible.

1.1. Objetivo del trabajo

En el campo de la robótica existen gran cantidad de investigaciones que se centran en el estudio de sofisticadas

técnicas de control que permitan a un robot móvil seguir fielmente la referencia que se le indica, pero esto

carece de utilidad sin una correcta realimentación que aporte el nuevo estado del robot después de cada

movimiento.

Por ello, un factor limitante en el éxito de gran cantidad de misiones con robots móviles en la actualidad es la

limitada precisión con la que a menudo se conoce la posición del mismo, especialmente en entornos en los que

las técnicas más habituales de posicionamiento carecen de utilidad.

Existen gran cantidad de técnicas y sensores que tratan de resolver este problema, siendo cada uno de ellos

adecuado para su uso bajo determinadas condiciones, y no recomendable para otras. Un sensor muy habitual

en cualquier robot móvil es la IMU (Inertial Measurement Unit), que puede utilizarse para estimar

desplazamientos mediante la integración de los datos de velocidades y aceleraciones que arroja. Sin embargo,

el principal problema de esta técnica es la acumulación del error a lo largo del tiempo, lo cual supone una

calidad decreciente en la estimación.

Una estrategia muy habitual para complementar este sensor es el uso de sistemas de posicionamiento por

satélite, o GNSS (Global Navigation Satellite System). De entre los sistemas GNSS, el que sin duda es más

conocido y utilizado es el sistema GPS. Así, es tremendamente habitual encontrar robots móviles que realizan

misiones confiando en gran medida la estimación de su posición al sistema GPS. Sin embargo, esta tecnología,

E

Page 14: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

2

además de presentar una precisión limitada, resulta prácticamente inútil cuando las misiones se realizan en

interiores, o en cualquier situación en la que no se tenga visión directa de los satélites.

Dado el avance que el campo de la robótica, y especialmente el de los robots móviles, con la irrupción de los

UAVs en el panorama tecnológico, ha experimentado en los últimos tiempos, resulta inaceptable aceptar la

limitación de que un buen posicionamiento sólo pueda conseguirse cuando las misiones se realizan al aire

libre. Surge por tanto la necesidad de aprovechar el gran desarrollo tecnológico de este campo, así como la

extensa variedad de sensores disponibles en el mercado, y afrontar el problema del posicionamiento en

entornos sin GPS.

Esto es precisamente lo que se pretende en este trabajo. Concretamente, el objetivo es el de aportar técnicas de

estimación de posicionamiento relativo en entornos interiores, utilizando únicamente los datos proporcionados

por un sensor de rango, ya sea realizando barridos en un plano, como el caso de un LIDAR, u obteniendo

nubes de puntos en el espacio, como sería el caso de una Kinect. Esa estimación del desplazamiento producido

podría servir para cerrar el bucle de control tanto en posición como en orientación.

Además, el trabajo pretende ahondar en un problema más concreto, como es la identificación y seguimiento de

elementos móviles de referencia por parte de un robot móvil, utilizando para ello únicamente los datos

recogidos por un sensor de profundidad. Así, se presentan técnicas para conseguir que, una vez identificado el

elemento a seguir y la posición relativa a éste que se desea que mantenga el robot, éste sea capaz de realizar un

seguimiento del elemento de referencia, detectando para ello los desplazamientos relativos y actuando en

consecuencia. Se pretende que los algoritmos presentados tengan éxito en situaciones realistas, por lo que se

presentan técnicas para la identificación del elemento a seguir, de cara a ignorar los movimientos de otros

posibles elementos móviles que pudiera haber en el entorno, distintos al de referencia. Las soluciones

presentadas se basan en el algoritmo ICP (Iterative Closest Point). Además, este trabajo muestra la utilidad del

algoritmo ICP para la estimación de posición de un robot móvil respecto a un sistema de referencia fijo, de

cara a dejar la puerta abierta a realizar estudios basados en estas técnicas. Todos los desarrollos presentados se

han implementado en ROS (1), que es un framework para el desarrollo de software en robots, ya que esta

plataforma aporta una gran cantidad de herramientas y facilidades orientadas a su uso concreto en robótica.

Se han llevado a cabo simulaciones en el entorno Gazebo y experimentos reales con un robot donde

únicamente se ha utilizado un sensor de profundidad que proporciona valores de rango. Así, se pretende

presentar unas soluciones a los problemas mencionados que no entrañen demasiada complejidad

computacional, y que utilicen el menor número de sensores posible, buscando así simplicidad. Los métodos

presentados son fácilmente integrables con los datos de otros sensores, estrategia mediante la cual sólo se

conseguiría mejorar los resultados.

1.2. Estructura del trabajo

El capítulo 2, titulado “Formulación del problema”, presenta con mayor profundidad los problemas afrontados

en este trabajo. Este capítulo pretende ahondar en algunos conceptos mencionados en el capítulo de

introducción, como los problemas de algunos métodos de posicionamiento en determinadas situaciones, y la

formulación de los problemas a abordar.

El capítulo 3, llamado "Algoritmo ICP y su aplicación a la navegación en entornos interiores”, presenta el

algoritmo ICP, que es la herramienta en la que se basan los desarrollos propuestos. Proporciona una idea

conceptual del funcionamiento y utilidad de esta herramienta. Posteriormente, este capítulo presenta la

Page 15: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

3

estrategia seguida para el seguimiento de elementos móviles, centrándose en el papel del algoritmo ICP.

Además, este capítulo también muestra la posible aplicación del algoritmo ICP a otros problemas más

generales de la navegación en interiores, como es el posicionamiento respecto a un sistema de referencia fijo.

El capítulo 4, llamado “Identificación y seguimiento de referencias”, explica en profundidad el diseño de la

herramienta ideada para poder realizar la identificación y seguimiento de elementos móviles en entornos con

más elementos. Este capítulo es el que aporta la técnica central de este trabajo.

El capítulo 5, llamado “Simulaciones y experimentos”, trata de demostrar la validez de los métodos

presentados a lo largo del trabajo.

Finalmente, el capítulo 6, llamado “Conclusiones y trabajo futuro”, presenta las conclusiones analizando los

resultados obtenidos en el capítulo 5, y por otro lado pretende dejar la puerta abierta a posibles estudios

futuros.

Page 16: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

4

2 FORMULACIÓN DEL PROBLEMA

ste capítulo presenta la problemática que se pretende afrontar en este trabajo. Ante el espectacular

desarrollo experimentado en los últimos años por la robótica en general, y por los vehículos no

tripulados en particular, aparece la posibilidad de afrontar nuevos retos relacionados con este campo,

retos que hace tan sólo unos años eran impensables. Sin embargo, gracias al desarrollo tecnológico que rodea a

la robótica, surge la posibilidad de comenzar a plantear objetivos más ambiciosos que los afrontados

tradicionalmente. En el caso de este trabajo, se pretende aportar soluciones al problema de la estimación de la

posición relativa utilizando únicamente los datos de un sensor de rango. Este problema surge cuando sistemas

de posicionamiento, como el GPS, fallan o no se pueden utilizar.

En primer lugar se presentan algunas situaciones en las que puede ser necesario conocer la posición del robot

únicamente con un sensor de profundidad, y a continuación se formulan los problemas concretos que se

afrontan en este trabajo.

2.1. Situaciones adversas para los métodos tradicionales

En los últimos años se ha producido una revolución alrededor de los robots móviles, hasta el punto de que

poco tienen que ver actualmente sus prestaciones y capacidades con las que tenían hace algunos años. Esta

revolución no es fruto de la casualidad, sino que es producto de las grandes aplicaciones que puede tener un

robot móvil que se mueva autónomamente en gran cantidad de campos, como por ejemplo vigilancia, revisión

de entornos críticos, rescate, y un largo etcétera.

En la actualidad existen gran cantidad de estrategias de control, que permiten que un robot móvil siga con gran

fidelidad una trayectoria previamente planificada, e incluso existen técnicas que integran una planificación en

línea para ir decidiendo los movimientos a realizar. Aunque el abanico de estrategias de control es inmenso,

con técnicas a veces muy heterogéneas entre sí, finalmente todas ellas se reducen a intentar hacer coincidir una

referencia buscada con la posición en la que se encuentra el robot.

E

Page 17: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

5

Por ello, un aspecto fundamental para una navegación exitosa es disponer de una estimación de la posición

precisa. Resulta evidente que si la posición en la que el vehículo cree que se encuentra difiere en gran medida

de la real, la acción de control calculada y el posterior comportamiento del robot harán que éste se aleje de la

misión que pretende cumplir.

El método de posicionamiento más utilizado en robots móviles es el sistema GPS. A pesar de su extendido

uso, esta tecnología presenta algunos inconvenientes, como la baja frecuencia con la que se obtiene una

estimación de la posición, del orden de 1 Hz, o la precisión limitada en la estimación. Sin embargo, estos

problemas pueden mitigarse mediante la integración de los datos del sistema GPS con los de otros sensores,

como por ejemplo los de la IMU, como se muestra en el esquema de la Figura 2.1.

Figura 2.1: Esquema del control de navegación tomando señales GPS

A pesar de los inconvenientes mencionados, la mayor limitación del sistema GPS, y de cualquier sistema de

posicionamiento GNSS, es la imposibilidad de su uso en interiores. Esto se debe a que un receptor de GPS

calcula su posición mediante triangulación a partir de las señales recibidas de distintos satélites, y éstas son

incapaces de alcanzar al receptor cuando éste se encuentra, por ejemplo, dentro de una habitación.

Por ello resulta necesario el desarrollo de técnicas que permitan un posicionamiento adecuado en entornos

donde no se dispone de señales GNSS. En este trabajo se proponen estrategias de posicionamiento que utilizan

únicamente un sensor de profundidad, que arroja medidas de rango en distintas direcciones.

Este trabajo se centra principalmente en un problema concreto, como es el seguimiento de referencias móviles

por parte de un robot móvil, empleando para ello una herramienta conocida como “algoritmo ICP”. Se

pretende ahondar en este problema, mediante el diseño de técnicas que permitan conseguir este objetivo en

entornos realistas, donde el sensor de rango toma medidas correspondientes a otros elementos que no son el de

referencia. Como complemento a este objetivo, se pretende mostrar la utilidad del algoritmo ICP para otros

problemas más generales dentro del posicionamiento en interiores, como es el posicionamiento del robot

respecto a un sistema de referencia fijo.

Page 18: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

6

Como se ha comentado, la resolución que se propone utiliza únicamente los datos recogidos de un sensor de

rango, aunque en caso de disponer de más sensores podrían integrarse los datos y estimaciones de distintos

métodos para obtener unos resultados más precisos.

2.2. Mantenimiento de una posición fija respecto a una referencia, en situaciones realistas

El principal problema que se aborda en este trabajo consiste en mantener una posición constante respecto a un

objetivo cerrando el bucle de control a partir de la nube de puntos proporcionada por un sensor de rango como

única información. Esta situación, entendida desde el punto de vista de la información del sensor de la que se

dispone, se traduce en lo siguiente:

Cuando se alcanza la posición y orientación deseada respecto al objeto de referencia, el sensor devuelve una

medida correspondiente a una nube de puntos determinada. El objetivo por tanto es el de conseguir, dada una

medida del sensor distinta de la posición de referencia, ser capaz de conocer los movimientos que sería

necesario efectuar para volver a alcanzar la situación deseada, con el fin de que el sistema de control sea capaz

de recuperar dicha posición tras el movimiento del objeto de referencia.

A modo de ejemplo, la Figura 2.2 muestra un ejemplo sencillo con un robot terrestre llamado turtlebot y una

caja a un metro y medio de distancia. Dicho robot móvil está equipado con una cámara Kinect, que es el

sensor de rango utilizado en este caso. La parte izquierda de la Figura 2.2 se corresponde con la situación de

referencia en este ejemplo, por lo que la nube de puntos recogida por la Kinect y mostrada a la derecha de la

imagen se corresponde con el patrón a buscar.

Figura 2.2: Posición correspondiente a la referencia buscada, y lectura del sensor de rango.

Page 19: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

7

La Figura 2.3 muestra una situación en la que el robot no se ha movido, pero la caja ha sido desplazada hacia

atrás. El objetivo que se persigue es que el robot móvil sea capaz de detectar el desplazamiento relativo

analizando la nube de puntos obtenida de la Kinect, de tal forma que sea capaz de volver a alcanzar la

situación mostrada en la Figura 2.2.

Figura 2.3: Posición relativa distinta a la de referencia, y medida del sensor de rango.

La Figura 2.4 muestra el objetivo que se pretende conseguir, es decir, que el robot móvil se desplace para

volver a alcanzar la posición relativa al objeto marcada como referencia, o lo que es lo mismo, que la medida

del sensor de rango, una cámara Kinect en este caso, coincida con el patrón de referencia.

Figura 2.4: Situación final en la que se ha recuperado la posición relativa buscada, y medida del sensor.

Page 20: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

8

Esta técnica puede tener cabida en numerosas aplicaciones, como por ejemplo las de seguimiento, supervisión

o vigilancia. Sin embargo, para que el seguimiento sea exitoso en entornos reales, la estrategia comentada de

comparar las medidas del sensor en cada momento con las medidas en la situación de referencia no es

suficiente. Esta estrategia sólo sería válida en el caso en el que las medidas del sensor contuvieran únicamente

al elemento a seguir, cosa que no ocurrirá en la mayor parte de casos en la práctica.

Por lo tanto, hay que modificar esta estrategia de forma que sea únicamente una parte de las medidas, las

cuales definen el elemento a seguir, las que se comparen en cada momento con el patrón, que a su vez también

debe contener únicamente el objeto a seguir. Por tanto, el principal reto de este trabajo es el diseño del filtro

que decida qué medidas se corresponden con el objeto a seguir y cuáles no para llevar a cabo la identificación.

Una vez que se dispone de las medidas correspondientes al elemento a seguir tanto en la situación de

referencia como en otra situación distinta, en ese momento el objetivo sí es conseguir hacer coincidir ambas

nubes de puntos, momento en el cual se habrá vuelto a alcanzar la situación de referencia.

Page 21: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

9

3 ALGORITMO ICP Y SU APLICACIÓN A LA

NAVEGACIÓN DE ROBOTS MÓVILES

ste capítulo describe el método utilizado en el trabajo para la navegación, cuando la única información

de la que se dispone es la proporcionada por un sensor láser. Al hablar del amplio concepto de

navegación, este trabajo se centra con mayor profundidad en el problema del seguimiento de elementos

móviles por parte de un robot móvil. Para ello, es fundamental conocer el desplazamiento de dicho elemento

con respecto a la posición de referencia, para que el sistema de control posteriormente sea capaz de intentar

minimizarla, y esto es precisamente lo que persigue el método presentado.

Así, este capítulo presenta en primer lugar la herramienta en la que se basan los métodos presentados, el

algoritmo ICP (Iterative Closest Point), así como las principales librerías externas utilizadas. A continuación,

se presenta la aplicación del algoritmo ICP al problema concreto que se afronta, como es el seguimiento de

elementos móviles. Por último, con el fin de mostrar la posible aplicación de esta herramienta en un problema

de navegación más general, se presentan algoritmos que pueden permitir a un robot móvil conocer su posición

respecto a un sistema de referencia fijo.

3.1. Introducción

La estimación de la posición juega un papel fundamental en la navegación autónoma de un robot móvil. Ya

sea como posición respecto a un sistema de coordenadas fijo, como movimiento incremental con respecto a

una posición determinada o como movimiento relativo respecto a una referencia fija o móvil, es fundamental

disponer de una medida del desplazamiento del robot.

Quizás la técnica de posicionamiento más extendida, o al menos una de ellas, es el posicionamiento mediante

GPS. Es muy habitual observar UAVs realizando vuelos de forma autónoma y haciendo uso de un sistema

GPS para posicionarse. Sin embargo, esta herramienta, al igual que cualquier sistema de posicionamiento

basado en señales GNSS (Global Navigation Satellite System), es efectivo sólo en exteriores. Al resultar inútil

un receptor de GPS en interiores, se buscan métodos de posicionamiento alternativos, como pueden ser el uso

de la IMU o el posicionamiento mediante procesamiento de imágenes, balizas, etc.

E

Page 22: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

10

En este capítulo se presenta un método de posicionamiento en el que sólo se utiliza un sensor de rango, siendo

por lo tanto válido para situaciones en las que no se dispone de los datos de ningún otro sensor. Al tener los

sensores de profundidad comerciales un rango de alcance más o menos limitado, estos están pensados para ser

usados en interiores. Cabe destacar que, aunque en este trabajo todas las pruebas realizadas utilizan el sensor

de rango como única fuente de información, es posible también su integración con otros sensores, de forma

que sea posible conseguir una navegación satisfactoria en escenarios en los que no se disponga de GPS.

3.2. El algoritmo ICP

El algoritmo ICP es un algoritmo que tiene como objetivo minimizar las diferencias entre dos nubes de puntos.

Esto se traduce en la obtención de las transformaciones geométricas de rotación y traslación que deben

aplicarse a una nube de puntos para minimizar su diferencia con respecto a la nube original o referencia.

3.2.1. Aplicación del algoritmo ICP a la robótica móvil

El algoritmo ICP no está sujeto a un tipo de nube de puntos en concreto, ya que puede utilizarse con datos tan

dispares como los recogidos de una cámara, aplicando el algoritmo a puntos de interés que se encuentren en

las imágenes, o a datos de proximidad recogidos de un sensor de profundidad. La cualidad de ser aplicable a

distintos tipos de datos, como los mencionados, hacen de ésta una herramienta interesante para su aplicación a

la robótica móvil.

Como se ha mencionado, el algoritmo ICP trata de alinear dos nubes de puntos, calculando para ello la

transformación geométrica que mejor cumple este objetivo. Esta información puede ser de vital importancia en

el ámbito de la robótica móvil, ya que permitiría conocer el desplazamiento relativo realizado por el sensor

entre dos instantes con tan sólo comparar las nubes de puntos asociadas a los mismos.

Sin ir más lejos, ésta es la utilidad que se desea explotar en este trabajo. En todos los casos estudiados, la idea

fundamental es la de obtener medidas de un sensor de rango en dos instantes de tiempo distintos y,

comparando las nubes de puntos obtenidas mediante este algoritmo, estimar el movimiento relativo que ha

tenido lugar entre ambas imágenes.

3.2.2. Desglose del algoritmo ICP

Hasta ahora se ha hablado conceptualmente del algoritmo ICP, indicando únicamente que toma como entradas

dos nubes de puntos, representadas de una forma determinada, y devuelve una estimación de las

transformaciones geométricas que permitirían alinear dichas imágenes. Sin embargo, no se ha comentado el

funcionamiento del algoritmo en sí, y es este apartado el que presenta el desglose del mismo.

Para calcular la transformación geométrica que mejor alinea dos nubes de puntos tienen lugar una serie de

pasos, mostrados en la Figura 3.1.

Page 23: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

11

Figura 3.1 Esquema del algoritmo ICP

- El primer paso, representado en la Figura 3.1 como Matching, consiste en definir, para cada nube, la

pareja de cada uno de los puntos en la otra nube, con el fin de que sea la diferencia entre las parejas la

que se busque minimizar mediante la transformación geométrica a calcular. En el caso del algoritmo

ICP, la pareja de cada uno de los puntos de una imagen se escoge como el punto más cercano en la

otra imagen, suponiendo que ambas estuvieran superpuestas. Precisamente de esta técnica toma el

nombre este algoritmo, ya que busca para cada punto de la imagen A el punto más cercano en la

imagen B, y éstos son los que intenta hacer coincidir (ver Figura 3.2).

Figura 3.2. Matching entre dos figuras de 8 puntos

- Una vez que se dispone de las parejas cuya diferencia se pretende minimizar, el siguiente paso es

obtener la rotación y la traslación que efectivamente consiguen minimizarla. Para resolver este

problema existen una gran cantidad de alternativas, aunque actualmente se sigue optando por las

técnicas de optimización cuadrática, ya que su uso está ampliamente extendido en los problemas de

optimización y, por ello, existen herramientas que ofrecen grandes resultados.

- Tras resolver el problema de encontrar la transformación geométrica que minimiza la diferencia entre

las dos nubes de puntos, el siguiente paso es aplicar esta transformación a una de las nubes y calcular

la función de coste de esta situación, que dependerá del error entre las parejas de puntos definidas.

Page 24: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

12

- A continuación, simplemente se itera para repetir el proceso una y otra vez (ver Figura 3.3). Con el fin

de evitar que este algoritmo sea infinito, se define un umbral de forma que el algoritmo se detenga y

tome la solución obtenida como válida si la función de coste alcanza un valor por debajo del mismo.

Figura 3.3: Ejemplo con 3 iteraciones del algoritmo ICP

Cabe destacar que el desglose del algoritmo ICP mostrado en este apartado responde al objetivo de

proporcionar una visión general del mismo, a fin de entender su funcionamiento de forma cualitativa. En cada

una de las etapas mencionadas tienen cabida una gran cantidad de expresiones matemáticas y consideraciones

a tener en cuenta para obtener un resultado satisfactorio, como por ejemplo filtros para la eliminación de

outliers, métodos para optimizar el tiempo de cálculo, etc. No se ha entrado en esta memoria en detalle en

estos aspectos, al carecer de sentido en el contexto de este trabajo.

3.3. Principales librerías utilizadas

En la implementación en lenguaje C++ del método presentado, se ha hecho uso de una serie de librerías

externas que permiten facilitar la elaboración de los algoritmos. Así, se han integrado librerías que nos

proporcionan herramientas fundamentales para conseguir los objetivos marcados. Algunas de ellas, como la

librería MRPT que se expone a continuación, ha necesitado ser ligeramente tratada para poder ser integrada en

ROS, mientras que otras ya estaban preparadas para su uso en esta plataforma.

Como en cualquier proyecto realizado en ROS, se han utilizado una gran cantidad de librerías que abarcan

multitud de ámbitos. Por ello, a continuación se mencionan únicamente las librerías cuya aplicación es muy

concreta a los casos estudiados, y por lo tanto no son de uso habitual en cualquier proyecto.

Page 25: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

13

3.3.1. Librería MRPT

La principal herramienta que se ha utilizado en este trabajo es el proyecto MRPT (Mobile Robot Programming

Toolkit) (2). El proyecto MRPT consiste en una serie de librerías de uso libre, sujeto a la licencia BSD, que

han sido desarrolladas por diversos investigadores con el objetivo de facilitar el avance en distintas técnicas

relativas a la programación de robots móviles.

La librería MRPT abarca un amplio campo de aplicación dentro de la programación de robots móviles. Por

mencionar algunas técnicas más concretas en las que toma partido, la librería MRPT proporciona herramientas

utilizadas para diversas formas de SLAM (Simultaneous Location and Mapping), visión artificial, o

planificación de movimientos. Éstos son sólo tres de los campos de aplicación de esta potente librería, y

además dentro de cada uno de ellos se proporciona una gran cantidad de herramientas que facilitan la

investigación en nuevas técnicas, o la mejora de las ya existentes.

Si se busca una forma de combinar las herramientas que proporciona esta librería con las tremendas facilidades

que aporta un entorno completo orientado al campo de la robótica, como es ROS, resulta evidente que se

dispone de una potente herramienta. Esto es precisamente lo que se ha hecho en este trabajo, ya que se ha

integrado el conjunto de librerías de MRPT, de uso general, en el sistema operativo de ROS.

De las múltiples posibilidades que ofrece la librería MRPT, en este trabajo se han utilizado una serie de

funciones que permiten el uso del algoritmo ICP. Así, se proporciona la función mrpt::slam::cicp::Align, que

toma como entradas dos nubes de puntos obtenidas de dos barridos en dos dimensiones de un sensor láser, y

devuelve como salida la transformación geométrica que mejor hace encajar las dos nubes de puntos, como

desplazamiento en X e Y, y rotación respecto al eje vertical. De la misma forma, también se proporciona la

función mrpt::slam::cicp::Align3D, que toma como entradas dos nubes de puntos (ver Figura 3.4) y, al igual

que en el caso anterior, devuelve como salida la transformación geométrica que mejor hace coincidir ambas

nubes de puntos. Sin embargo, en este caso la transformación resultante se expresa como desplazamiento en

los 3 ejes del espacio y como rotación alrededor de cada uno de ellos.

En las dos funciones mencionadas las entradas son dos nubes de puntos, ya sean en 2D o en 3D, expresadas

como un objeto de la forma CSimplePointsMap, que es el que contiene la información de una nube de puntos

determinada. Ambas funciones también toman como entrada un parámetro de estimación inicial, orientado a

agilizar la búsqueda del resultado para casos en los que se conozca una estimación de cuál puede ser la

transformación geométrica que haga coincidir las dos nubes de puntos a priori. Además, las dos funcionen

permiten ajustar una gran cantidad de parámetros internos, lo cual también favorece a la investigación de

distintas formas de mejora del método.

Figura 3.4: Ejemplo de dos nubes de puntos a alinear

Page 26: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

14

3.3.2. Paquete mrpt bridge

Como se ha mencionado en el apartado anterior, la combinación de la librería mrpt con las herramientas de

ROS proporciona un amplio abanico de posibilidades. Por lo tanto, no resulta extraña la existencia de una serie

de paquetes de ROS que hagan uso de distintas maneras de la librería MRPT. Este es el caso del conjunto de

paquetes llamado mrpt_navigation (2). Este conjunto de paquetes es de uso público, al igual que la mayoría de

herramientas que se encuentran en el sitio Web de ROS.

El proyecto mrpt_navigation contiene varios paquetes, que pueden ser útiles para según qué aplicaciones. Así,

el paquete utilizado en este trabajo es el llamado mrpt_bridge. Este paquete, como su propio nombre indica,

pretende actuar como puente entre las variables con las que se trabaja habitualmente en ROS, y los tipos de

objetos con los que trabaja la librería MRPT (ver Figura 3.5).

En el apartado anterior se mencionó que la forma de representar las nubes de puntos en la librería MRPT es a

través de objetos del tipo CSimplePointsMap. En el caso de una nube de puntos correspondientes al barrido en

2D de un sensor láser de proximidad, este objeto CSimplePointsMap debe generarse a partir de un objeto de

tipo CObservation2DRangeScan, que es el que contiene la información del barrido del láser. Sin embargo, al

trabajar con sensores láser en ROS, habitualmente se obtiene un objeto de tipo LaserScan con dicha

información, surgiendo entonces la necesidad de transformar dicho objeto LaserScan en otro de tipo

CObservation2DRangeScan, para poder trabajar con dicha información utilizando las librerías MRPT.

Figura 3.5: Papel del paquete mrpt_bridge.

Ésta y otras transformaciones similares son de las que se encarga precisamente el paquete mrpt_bridge. Así,

hay una función que se encarga de realizar la mencionada transformación entre CObservation2DRangeScan y

LaserScan, en ambos sentidos, además de otras que permiten transformar entre los objetos más habituales en

las librerías MRPT y los más utilizados en el tratamiento de sensores de profundidad en ROS. Las funciones

utilizadas en este proyecto son las siguientes:

- Transformación entre LaserScan y CObservation2DRangeScan.

- Transformación de PointCloud a CSimplePointsMap.

- Transformación de PointCloud2 a CSimplePointsMap.

Page 27: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

15

3.4. Algoritmos implementados para el seguimiento de referencias

Este apartado expone el método utilizado para el mantenimiento de una determinada posición fija respecto a

un objeto de referencia. Esto, en el caso de que la referencia sea un elemento móvil, se traduce en la

persecución de dicho elemento, utilizando para ello únicamente los datos proporcionados por un sensor de

rango. Para ello, la estrategia seguida es la de comparar cada medida del láser con la medida correspondiente a

la disposición deseada, a la que denominamos patrón de referencia. De esta forma, el resultado del ICP dará

una estimación de la disposición del robot con respecto a la posición que se desea alcanzar, y el sistema de

control en posición podrá utilizar esta información para conseguir que el resultado del ICP sea cero, lo cual

significaría que la disposición actual es exactamente la deseada.

Como se ha comentado previamente, el algoritmo ICP permite trabajar tanto con nubes de puntos en 2D como

en 3D. Por ello, y a pesar de que el método en ambos casos es similar, se presentan en este apartado ambas

variantes por separado.

3.4.1. Algoritmo en 2 dimensiones

En esta sección se presenta la variante a utilizar cuando la entrada del algoritmo ICP son nubes de puntos en

dos dimensiones. Esta situación se corresponde con el caso de un robot que se mueve en el plano y toma datos

de un sensor láser que realiza un barrido en el plano horizontal. Como se ha mencionado en apartados

anteriores, la función que realiza el algoritmo ICP toma como entradas dos nubes de puntos codificadas en

objetos de tipo CSimplePointsMap. En el caso en el que se toman los datos de un barrido de un sensor láser en

2D, dicha información se almacena en objetos del tipo CObservation2DRangeScan, y a partir de éstos se crean

las nubes en CSimplePointsMap.

Por lo tanto, en todos los métodos en los que se utilizan datos recogidos de un barrido 2D, los primeros pasos

consistirán en obtener la información del láser como un objeto de tipo LaserScan, transformarla al tipo

CObservation2DRangeScan utilizando la librería mrpt_bridge, y generar el CSimplePointsMap. Este esquema

se muestra en la Figura 3.6.

Figura 3.6: Esquema de entrada de la información de un láser 2D.

Page 28: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

16

A continuación se muestra de forma esquemática el pseudocódigo implementado para intentar hacer coincidir

las medidas recogidas en cada instante con la correspondiente al patrón:

If llega una medida del láser (en forma LaserScan).

o Se realiza un filtrado a la medida del láser, y se guarda en scan_filtrado.

o Se transforma scan_filtrado a CObservation2DRangeScan mediante el paquete mrpt_bridge.

o If es la primera medida

Guardar la nueva medida, en forma CObservation2DRangeScan, en la variable

mapa_referencia.

o else

Borrar mapa_actual.

Insertar la nueva medida, en forma CObservation2DRangeScan, en mapa_actual.

Lanzar la función mrpt::slam::cicp::Align, pasándole como argumentos

mapa_actual, mapa_referencia, y una estimación inicial igual a cero, o igual al valor

del resultado anterior.

Utilizar el resultado del algoritmo ICP directamente para calcular la señal de control

a comandar.

- End.

El algoritmo presentado se corresponde fielmente con la estrategia comentada, excepto en lo que respecta a la

variable scan_filtrado. La necesidad de filtrar las medidas que entran al algoritmo, en lugar de pasárselas al

algoritmo ICP directamente, se explica en el capítulo 4. Además, este capítulo muestra en profundidad el

diseño del filtro mencionado.

3.4.2. Algoritmo en 3 dimensiones

A continuación se afrontan el mismo problema que para el caso anterior, pero en este caso las entradas

corresponden a nubes de puntos con coordenadas en el espacio. Esto quiere decir que en este caso no se parte

de las medidas proporcionadas por un sensor láser que realiza un barrido en el plano, sino de una matriz

completa de puntos en el espacio. Existen distintas estrategias en lo que a sensores se refiere para obtener la

información de esta forma, como son por ejemplo el uso de una Kinect, que directamente proporciona una

nube de puntos en el espacio, o el uso de un láser que realice un barrido en el plano combinado con un servo,

de forma que pueda cambiar el ángulo de inclinación para cubrir una superficie completa. La Figura 3.7

muestra un ejemplo de dos nubes de puntos que se quieren alinear para estimar la posición y orientación del

robot.

Page 29: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

17

Figura 3.7: Representación de dos nubes de puntos a alinear.

En el caso 3D existen algunas diferencias respecto al caso 2D, en lo que a tratamiento se refiere. La diferencia

fundamental en el proceso de tratamiento de la información es que en este caso no es necesario transformar la

información a un objeto de tipo CObservation2DRangeScan. En el trabajo con nubes de puntos en ROS, lo

más habitual es encontrarlas codificadas como objetos de tipo PointCloud, o bien como objetos de tipo

PointCloud2. Ambos tipos de objetos contienen la misma información expresada de distinta forma, y en el

paquete mrpt_bridge se proporcionan funciones para transformar ambas a objetos de tipo CSimplePointsMap.

Por ello, no es excesivamente relevante la forma en la que se proporcione la información de la nube de puntos,

puesto que se podrá transformar utilizando la función adecuada. Por lo tanto, en el caso 3D se obtiene del

sensor un objeto de tipo PointCloud o PointCloud2, y simplemente hay que incluir dicha información en un

objeto de tipo CsimplePointsMap para poder utilizar el algoritmo ICP implementado en la librería MRPT.

La otra diferencia fundamental en el caso 3D es que el algoritmo ICP devuelve en este caso 6 magnitudes, ya

que calcula desplazamiento según los tres ejes del espacio y rotación alrededor de los mismos.

En cuanto al objetivo marcado, en el caso en tres dimensiones se vuelve a utilizar la estrategia de comparar

cada medida del sensor con la medida deseada, que constituye el patrón buscado. Al ser la estrategia

conceptualmente análoga al caso en dos dimensiones, simplemente se muestra a continuación el proceso a

seguir:

- If llega una medida del sensor (en forma PointCloud o CpointCloud2).

o Se transforma a CSimplePointsMap mediante el paquete mrpt_bridge.

- If es la primera medida

o Realizar un filtrado de la nueva medida, y guardar el mapa resultante en mapa_referencia.

- else

o Borrar mapa_actual.

o Realizar un filtrado de la nueva medida, y guardar el mapa resultante en mapa_actual.

o Lanzar la función mrpt::slam::cicp::Align, pasándole como argumentos mapa_actual,

mapa_referencia, y una estimación inicial igual a cero, o igual al valor del resultado anterior.

o Utilizar el resultado del algoritmo ICP directamente para calcular la señal de control a

comandar.

- End.

Page 30: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

18

Al igual que en el caso en dos dimensiones, la necesidad de realizar el filtrado de las medidas de entrada se

explica en el capítulo 4.

3.5. Aplicación del algoritmo ICP para la estimación de la posición respecto a un sistema de referencia fijo

El objetivo concreto de este trabajo es presentar técnicas para el seguimiento de elementos móviles utilizando

únicamente un sensor de rango. Sin embargo, al utilizarse para ello el algoritmo ICP, resulta interesante

presentar también la utilidad que puede tener esta herramienta en la estimación de la posición de un robot

móvil respecto a un sistema de referencia fijo. Al igual que en el apartado anterior, se muestran algoritmos

tanto en dos como en tres dimensiones.

3.5.1. Algoritmo en 2 dimensiones

la estrategia propuesta consiste en recoger las medidas de un barrido del sensor láser con determinada

frecuencia, y utilizar el algoritmo ICP para estimar la distancia recorrida entre dos medidas consecutivas. Así,

integrando los resultados en el tiempo, se puede obtener una estimación del espacio recorrido. Además,

realizando una transformación que tenga en cuenta el ángulo de rotación se consigue situar en todo momento

al robot móvil en un sistema de coordenadas fijo, con origen coincidente con la posición inicial en la que

comenzó el algoritmo.

A continuación se muestran los pasos que realiza el algoritmo, en pseudocódigo:

- If llega una medida del láser (en forma LaserScan).

o Se transforma a CObservation2DRangeScan mediante el paquete mrpt_bridge.

o Guardar mapa_actual en mapa_anterior.

o Borrar mapa_actual.

o Insertar la nueva medida, en forma CObservation2DRangeScan, en mapa_actual.

o Lanzar la función mrpt::slam::cicp::Align, pasándole como argumentos mapa_actual,

mapa_anterior, y una estimación inicial igual a cero.

o Utilizar el resultado del algoritmo ICP para incrementar el valor de la posición respecto al

sistema de referencia fijo con origen en el punto inicial, teniendo en cuenta para ello la

transformación correspondiente a la posible rotación experimentada entre medidas.

- End.

Como se ha mencionado anteriormente, es necesario que las variables mapa_anterior y mapa_actual sean

objetos del tipo CSimplePointsMap. Además, es importante borrar el mapa_actual antes de añadir nuevas

medidas, ya que de lo contrario el mapa se haría más y más grande, dando lugar a resultados erróneos.

La transformación mencionada en el último punto es necesaria debido a que la función que realiza el algoritmo

ICP devuelve la transformación óptima entre los dos mapas, representado en ejes cuerpo, es decir, con el eje X

apuntando en la dirección en la que está mirando el robot móvil, y el eje Y hacia la izquierda del mismo. Por

ello, es necesario realizar una transformación que tenga en cuenta la variación del rumbo del robot (ver Figura

3.8), para representar la posición en un sistema de coordenadas fijo y coincidente con el sistema de referencia

en ejes cuerpo en la posición inicial del robot.

Page 31: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

19

Figura 3.8: Cambio de orientación del robot

3.5.2. Algoritmo en 3 dimensiones

La diferencia fundamental respecto al caso anterior es que en este caso se trabaja con nubes de puntos en tres

dimensiones. Además, como se ha mencionado previamente, el resultado proporcionado por el algoritmo ICP

en este caso es desplazamiento en X, Y y Z, y rotación alrededor de los tres ejes mencionados.

Para conseguir estimar el desplazamiento respecto a la posición inicial del robot, que se convierte desde el

mismo momento en origen del sistema de coordenadas fijo, se sigue la misma estrategia que en el caso 2D. Se

calcula mediante el algoritmo ICP el desplazamiento entre dos medidas consecutivas del sensor, en este caso

representadas por dos nubes de puntos en el espacio, y se integran dichos resultados a lo largo del tiempo para

obtener la posición respecto al sistema de referencia fijo antes mencionado.

El proceso a seguir en este caso es el siguiente:

- If llega una medida del sensor (en forma PointCloud/PointCloud2).

o Guardar mapa_actual en mapa_anterior.

o Borrar mapa_actual.

o Se transforma la nueva medida a CSimplePointsMap mediante el paquete mrpt_bridge, y se

guarda en mapa_actual.

o Lanzar la función mrpt::slam::cicp::Align3D, pasándole como argumentos mapa_actual,

mapa_anterior, y una estimación inicial igual a cero.

o Utilizar el resultado del algoritmo ICP para integrar el valor de la posición respecto al sistema

de referencia fijo con origen en el punto inicial, teniendo en cuenta para ello las rotaciones

experimentadas.

- End.

En este caso, como es evidente, la transformación para integrar los sucesivos resultados es más compleja, al

entrar en escena más rotaciones, pero se corresponde simplemente con la aplicación de una matriz de rotación

mayor, por lo que no se entrará en detalle en este trabajo, al ser un problema puramente matemático.

Page 32: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

20

4 IDENTIFICACIÓN Y SEGUIMIENTO DE

REFERENCIAS

Este capítulo explica el algoritmo implementado para la identificación y seguimiento de un objeto de

referencia en un entorno con más objetos, utilizando para ello únicamente los datos de profundidad

de un sensor de rango y el algoritmo ICP expuesto en el capítulo anterior. El algoritmo propuesto se

basa en un filtrado de las medidas del sensor de profundidad, de forma que se identifiquen las medidas que

definen el objeto a seguir, y se desechen el resto de medidas.

Se muestra en primer lugar la necesidad de realizar un filtrado de las medidas recogidas del sensor de

profundidad cuando se trabaja en entornos más realistas, con gran cantidad de medidas que no corresponden al

objeto de referencia que se desea seguir. A continuación, se explica el diseño del algoritmo propuesto para dar

solución a la problemática presentada, y por último se proponen estrategias para afrontar casuísticas menos

habituales.

4.1. Problemática y necesidad de un filtrado

En el capítulo anterior se presentó una estrategia que tenía como objetivo “perseguir” un determinado objeto

de referencia, manteniendo para ello una distancia constante respecto al mismo, empleando únicamente un

sensor de rango. La estrategia presentada se basada en tomar la medida del sensor de rango en la posición

deseada, y almacenar ésta como patrón a buscar, para posteriormente comparar cada una de las medidas con

dicho patrón mediante el algoritmo ICP, y utilizar la salida de este algoritmo para calcular las acciones de

control a realizar para minimizar la diferencia entre medida y patrón. Así, si en todo momento se busca hacer

coincidir la medida del sensor de rango con el patrón buscado, esto se traduce en que el robot móvil intentará

buscar la posición de referencia con la que se corresponde el patrón.

E

Page 33: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

21

Sin embargo, aunque esta estrategia a priori debería ser válida, en un entorno más realista en el que existan

otros objetos además del que tomamos como referencia este algoritmo puede no dar buenos resultados en

determinadas situaciones. A modo de ejemplo, se supone que se desea mantener una posición fija relativa a un

cubo que se encuentra a cierta distancia frente al robot móvil. En la Figura 4.1 se muestran las medidas de un

sensor láser de profundidad correspondientes a esta situación, y que según el algoritmo presentado en el

capítulo anterior se correspondería con el patrón a buscar. En dicha figura, las medidas correspondientes al

mencionado cubo se encuentran rodeadas por un círculo rojo.

Figura 4.1: Representación de las medidas recogidas con el sensor de rango en la situación de referencia, y ejes

cuerpo del robot. Objeto a seguir resaltado por el círculo rojo.

Si a continuación suponemos que se produce un desplazamiento del robot móvil debido a una perturbación

externa, la medida tomada se correspondería con unas medidas análogas a las del patrón de la Figura 4.1, pero

desplazadas, por lo que el algoritmo presentado permitiría volver a hacerlas coincidir con el patrón, lo que se

traduciría en que el robot volviera a la situación inicial.

Sin embargo, la Figura 4.2 muestra las medidas correspondientes a una situación en la que el robot móvil

continúa en el mismo punto, y es el cubo el que se ha desplazado. El algoritmo ICP busca el minimizar una

función de costes que mide el error entre todas las parejas de puntos correspondientes a las dos medidas, y

devuelve las transformaciones geométricas de rotación y traslación que dan lugar a dicho mínimo. Es decir, el

algoritmo ICP devuelve las transformaciones geométricas que mejor hacen coincidir ambas nubes de puntos.

En este caso, al no haberse desplazado la inmensa mayoría de los puntos, correspondientes a las paredes de la

habitación y a otros objetos, el algoritmo ICP concluirá que la transformación que mejor hace coincidir ambas

nubes de puntos se corresponde con una traslación y una rotación nulas.

Page 34: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

22

Figura 4.2: Medidas tomadas por el sensor de rango en una situación en la que el cubo se ha desplazado.

De esta problemática se deduce la necesidad de realizar un filtrado con cada una de las medidas, de forma que

tanto el patrón a seguir como las medidas con las que se comparará éste mediante el algoritmo ICP en los

distintos instantes contengan únicamente al objeto que se pretende tomar como referencia para poder

identificarlo. La Figura 4.3 muestra las dos nubes de puntos a las que se debería aplicar el algoritmo ICP para

poder recuperar la situación de referencia buscada. En ella se observa que ambas medidas contienen sólo los

puntos pertenecientes al objeto de referencia, en este caso el cubo, mientras que el resto de puntos observados

en cada medida no serían tenidos en cuenta a la hora de aplicar el algoritmo ICP. Así, al algoritmo ICP

entrarían dos nubes de puntos que contienen únicamente las medidas correspondientes al objeto a perseguir,

representadas en el sistema de referencia del robot en el instante en que fueron tomadas, de forma que la

solución del algoritmo permitirá conocer los movimientos a realizar por el robot para volver a la situación

buscada.

Figura 4.3: Representación de las nubes de puntos correspondientes a patrón (izquierda) y medida a alinear

(derecha) que permitirían recuperar el patrón mediante el algoritmo ICP.

Page 35: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

23

Una vez justificada la necesidad de realizar un filtrado de las medidas recogidas por el sensor de profundidad

para llevar a cabo la identificación de un objeto del entorno, en los siguientes apartados se explica el diseño del

filtro implementado.

4.2. Implementación del filtro

Este apartado presenta los pasos que se han seguido para realizar el diseño e implementación del filtro

mencionado en el apartado anterior. Este filtro debe ser capaz de seleccionar qué puntos de cada medida del

sensor de rango deben formar parte de la nube de puntos a alinear con el patrón mediante el algoritmo ICP,

para lo cual debe ser capaz de identificar qué puntos pertenecen al objeto que se pretende seguir, y desechar las

medidas que correspondan a otros elementos del entorno. De este modo, se considera el objeto a seguir como

identificado.

En la actualidad existen diversas técnicas que tienen como objetivo el reconocimiento e identificación de

patrones, apoyándose especialmente en técnicas como la visión artificial. Sin embargo, el objetivo de este

trabajo es presentar técnicas de navegación utilizando únicamente un sensor de rango, por lo que es necesario

idear una estrategia alternativa que sea capaz de reconocer el objeto a seguir a partir de un conjunto de

medidas de profundidad.

Esta labor de “reconocimiento” e identificación es la que realiza el filtro que se expone en este apartado. Para

ello, al disponer únicamente de un conjunto de medidas de profundidad en distintas direcciones, se emplean

distintas estrategias que se combinan. Así, en este apartado se exponen en primer lugar cada una de estas

estrategias por separado para, finalmente, mostrar un esquema global del funcionamiento del filtro

combinando todas ellas.

4.2.1 Filtrado por distancia

La cualidad que podemos extraer de forma más directa de un sensor de rango es la distancia. Por ello, resulta

evidente que la profundidad de cada uno de los puntos que conforman una determinada medida debe jugar un

papel fundamental en cualquier filtro que se base en información de rango. Como punto de partida, el

algoritmo presentado supone que, en el instante inicial, el objeto al que se desea realizar el seguimiento se

encuentra justo delante del robot móvil y, por extensión, del sensor de profundidad. De esta suposición se

desprende que, al comenzar el algoritmo, el haz central del sensor de rango incidirá frontalmente con el objeto

a seguir. Esta situación se ilustra en la Figura 4.4. El ella, el robot móvil aparece representado en azul, mientras

que el objeto a seguir está representado en gris. Asimismo, la línea de puntos representa lo que hemos llamado

como dirección principal del filtro, que en el instante inicial coincide con el haz central del sensor de

profundidad. El posterior apartado 4.2.2 explica con detalle qué representa esta dirección principal, así como el

cálculo de la misma para cada instante.

Page 36: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

24

Figura 4.4: Representación del robot móvil (azul) y objeto a seguir (gris). El robot móvil está orientado

inicialmente mirando frontalmente hacia el objeto.

El primer aspecto que se ha tenido en cuenta para el diseño del filtro es que, al hablar de la distancia a la que

se encuentra un punto respecto al robot, no se ha tenido en cuenta directamente el valor arrojado por el sensor

de profundidad en la dirección correspondiente a dicho punto, sino que se ha realizado una transformación a

dicho valor de la profundidad, de forma que la distancia considerada será la distancia de dicho punto al plano

perpendicular a la dirección principal del filtro. La Figura 4.5 ilustra esta situación, en la que se considera

como distancia 𝑑𝑖 la resultante de aplicar la expresión 𝑑𝑖 = 𝑙𝑖 ∗ 𝑐𝑜𝑠𝛼𝑖, donde 𝑙𝑖 y 𝛼𝑖 representan la medida de

profundidad del i-ésimo haz del sensor de rango, y el ángulo que forma este haz con la dirección principal del

filtro, respectivamente.

Figura 4.5: Representación de la distancia considerada para un punto 𝑷𝒊, así como el ángulo y la profundidad

correspondientes.

Page 37: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

25

Una vez definida la magnitud de distancia tal y como se considera en el diseño del filtro propuesto, el

siguiente paso es definir el criterio que se debe cumplir en este aspecto para que un punto sea candidato a ser

considerado como parte del objeto a seguir.

El algoritmo presentado se basa en todo momento en que si en un determinado instante 𝑡𝑘 el objeto se

encuentra a una determinada distancia media 𝑑𝑘, en el instante siguiente 𝑡𝑘+1dicho objeto no puede

encontrarse a una distancia ni muy superior ni muy inferior a 𝑑𝑘. Es decir, el criterio de distancia se basa en

que los cambios en la distancia media a la que se encuentran los puntos del objeto a seguir, tal y como se han

definido en este apartado, variará en todo momento de forma suave. Así, se define un valor de holgura en la

distancia, de forma que si determinada medida en el instante k+1 se corresponde con un valor de distancia

𝑑𝑖,𝑘+1 tal que si no se cumple la expresión 𝑑𝑘 − ℎ ≤ 𝑑𝑖,𝑘+1 ≤ 𝑑𝑘 + ℎ , se considera que dicho punto no

pertenece al objeto a seguir, y por tanto no será incluido en la nube de puntos a alinear con el patrón. En la

expresión anterior 𝑑𝑘 es la media de la distancia de los puntos que se han identificado como pertenecientes al

objeto a seguir en el instante k, h es la holgura definida, y 𝑑𝑖,𝑘+1 es la distancia calculada para un punto 𝑃𝑖 en

el instante k+1.

El motivo de que se seleccione la distancia de la forma presentada, en lugar de la profundidad arrojada por el

sensor directamente, es que durante el seguimiento del objeto se pretende conocer una medida única de

distancia media a la que se encuentra el objeto. De esta forma, si se considerara el valor de la profundidad

directamente, todas las medidas arrojarían una distancia mayor a la del haz central, incluso si el objeto a seguir

se encontrara paralelo al robot móvil. Esto se traduciría en una mayor dispersión de las distintas medidas

respecto al valor medio de todas ellas, lo cual haría necesario utilizar una holgura mayor, y por tanto el filtro

perdería eficacia.

La estrategia expuesta consistente en comparar cada valor de distancia calculada con la distancia media en el

instante anterior presenta la duda de cómo actuar en el instante inicial, cuando aún se desconoce la distancia a

la que se encuentra el objeto a seguir. La estrategia implementada en este trabajo consiste en definir un

pequeño conjunto de medidas en torno al haz central, y utilizar la media de la distancia calculada para estos

puntos como valor inicial de distancia media a la que se encuentra el obstáculo. Esta situación se ilustra en la

Figura 4.6.

Figura 4.6: Representación de la estrategia seguida para medir la distancia media inicial.

Page 38: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

26

Así, comparando la distancia calculada para cada punto con el valor medio de la medida anterior, y teniendo

en cuenta la holgura definida, es posible concluir directamente que todos los puntos que no se encuentran

dentro de esta holgura no pertenecen al objeto a seguir, por lo que con esta estrategia se consigue filtrar un

gran número de puntos. Sin embargo, encontrarse dentro de la holgura permitida no es la única condición que

debe cumplir un punto para que sea considerado como parte del objeto a seguir, sino tan solo una de ellas. El

resto de condiciones a cumplir, así como las distintas estrategias de búsqueda, se presentan en los apartados

posteriores.

4.2.2 Filtrado por dirección

Al utilizar un sensor de profundidad, concretamente un sensor láser que realiza barridos en un plano

horizontal, los datos que arroja de forma inmediata son aquellos correspondientes a la profundidad medida por

haces emitidos en determinadas direcciones dentro de un rango. Esto se traduce en dos magnitudes

fundamentales: profundidad y dirección. Por lo tanto, resulta conveniente utilizar ambas magnitudes en el

diseño del filtro a implementar.

Una vez presentada en el apartado anterior la estrategia para filtrar por distancia, este apartado afronta el

filtrado por dirección. En primer lugar, es necesario definir lo que se entiende como amplitud en este trabajo,

ya que es un concepto fundamental para el diseño del filtro. Dependiendo del modelo y del tipo de sensor de

rango, éste es capaz de realizar medidas de profundidad entre dos ángulos determinados (ver Figura 4.7). Así,

el sensor lanzará haces en las distintas direcciones dentro de la zona en cuestión, de forma que será capaz de

obtener valores de profundidad para cada una de estas direcciones, ignorando los objetos del entorno que se

sitúen fuera de este rango. Este rango de direcciones en los que el sensor es capaz de realizar medidas de

profundidad es conocido como amplitud del sensor.

Figura 4.7: Representación de la amplitud de un sensor láser situado solidario a un robot móvil.

Sin embargo, el filtro presentado en este trabajo no realiza la búsqueda del objeto a seguir en todo el rango

definido por la amplitud del sensor, sino en un rango menor de medidas, que definen la llamada amplitud del

filtro. Así, se pretende que la búsqueda de los puntos pertenecientes al objeto a seguir se realice entre unos

ángulos determinados, formando un espacio de búsqueda que puede representarse como un triángulo (ver

Figura 4.8 ).

Page 39: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

27

Figura 4.8: Representación de la amplitud del filtro (zona coloreada) frente a la amplitud del sensor (zona

discontinua).

El objetivo del filtro es realizar una búsqueda de los puntos pertenecientes al objeto a seguir únicamente dentro

del “triángulo” definido por la amplitud del filtro. Por ello, resulta necesario que el objeto a seguir se encuentre

íntegramente dentro del rango de direcciones definidas por la amplitud del filtro. De esta forma, la búsqueda

implementada en el filtro se basa en la suposición de que el objeto a seguir nunca realizará un movimiento con

tal brusquedad que se salga del triángulo de búsqueda entre un instante y el siguiente. Para que esta suposición

sea cierta, es necesario que el triángulo de búsqueda no tenga una amplitud igual a la del objeto a seguir, sino

que contenga además un margen adicional para asegurar que en siguiente instante el objeto sigue situándose

dentro de dicho triángulo.

Sin embargo, hasta ahora se han presentado los conceptos y los espacios de búsqueda de una forma estática.

Resulta evidente que, para que esta estrategia sea exitosa, el triángulo de búsqueda debe ir variando su

dirección conforme se mueva el objeto a seguir, así como su amplitud (amplitud del filtro), en función de la

distancia a la que se encuentre el objeto. Así, el triángulo de búsqueda se define de forma tal que la dirección

del haz central del mismo coincide con la dirección principal del filtro (definida en el apartado 4.2.1).

La Figura 4.9 muestra la situación inicial, en la que el robot móvil se encuentra frente al objeto a seguir,

situado a una distancia media 𝑑𝑚,0 (ver apartado 4.2.1). Como se ha comentado previamente, en el instante

inicial se considera que la dirección principal del filtro es la dirección frontal. Así, conocida la dirección

principal y la distancia media ̧ el algoritmo del filtro detectará los 𝑁1y 𝑁2 puntos que se encuentren a una

distancia calculada tal que la diferencia con la distancia media sea menor al valor de la holgura definida,

medidos 𝑁1y 𝑁2 a partir de la dirección principal del filtro en sentidos antihorario y horario, respectivamente.

En dicha figura aparecen representadas la holgura h, el incremento de amplitud del filtro con respecto a la

amplitud definida por el objeto a seguir, y las magnitudes mencionadas previamente

Siguiendo la estrategia expuesta previamente, en el instante siguiente sólo se analizarán las medidas que se

encuentren en el rango definido por la amplitud del filtro, y serán identificadas como pertenecientes al objeto

de referencia aquellas que se encuentren en la zona sombrada de la Figura 4.9, es decir, aquellas que cumplan

la condición de distancia expuesta en el apartado anterior. De esta forma se consigue reducir drásticamente el

espacio de búsqueda del objeto, por lo que el algoritmo gana en rapidez, además de no verse afectado por el

resto de elementos del entorno que se encuentren fuera del área de búsqueda definida.

Page 40: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

28

Figura 4.9: Representación de la zona de búsqueda (sombreado), objeto, robot, dirección principal del filtro

(línea gruesa discontinua) y magnitudes medidas, en el instante inicial.

Para ilustrar la forma en la que se recalcula la dirección principal del filtro tras un movimiento del objeto de

referencia, la Figura 4.10 ilustra esta situación. En ella se observa la disposición cuando el objeto de referencia

se ha desplazado respecto a la situación de la Figura 4.9, así como los nuevos valores de 𝑁1y 𝑁2 medidos

desde la dirección principal.

Figura 4.10: Paso intermedio para la actualización del espacio de búsqueda del filtro, tras un desplazamiento

del objeto de referencia.

Page 41: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

29

Observando la situación de la Figura 4.10, cabría pensar que el objeto puede acabar escapándose del espacio

de búsqueda si continúa moviéndose en esa dirección. Para evitar esto, los nuevos valores de 𝑁1y 𝑁2 permiten

recalcular el haz del sensor de rango que se corresponde con la nueva dirección principal del filtro. Así, si los

haces se enumeran en sentido horario y, en la situación anterior, la dirección principal se correspondía con el

haz número i, en el instante siguiente el haz que definirá la dirección principal será el número i - 𝑁1+ 𝑁2.

La Figura 4.11 representa la disposición del triángulo de búsqueda del filtro, así como la dirección principal,

en el siguiente instante, dado el movimiento presentado en la Figura 4.10. Como se observa en dicha Figura

4.11, el algoritmo presentado permite que el objeto a seguir vuelva a estar centrado en la región de búsqueda

para cuando lleguen las medidas correspondientes al siguiente instante.

Un aspecto a destacar de este algoritmo, y que quizás no ha quedado patente en el ejemplo mostrado, es que la

amplitud del filtro también depende de 𝑁1y 𝑁2, de forma que el triángulo se estrechará conforme el obstáculo

se aleje del robot, y se extenderá conforme se produzca un acercamiento entre ellos. Otro aspecto a tener en

cuenta es que, aunque en la Figura 4.11 el triángulo de búsqueda aparezca oblicuo al obstáculo, no hay que

olvidar que el objetivo de realizar este filtrado es que el robot móvil sea capaz de volver a la situación inicial,

por lo que el triángulo acabaría volviendo a la posición correspondiente. A pesar de esta aclaración, en el

siguiente apartado se incluye una funcionalidad más al filtro que pretende mitigar los posibles problemas que

pudieran aparecer por encontrarse el objeto de referencia en una posición oblicua respecto al sensor.

Figura 4.11: Representación del nuevo espacio de búsqueda del filtro, una vez recalculada la dirección

principal.

Como se ha observado, el filtrado por dirección expuesto en este apartado incluye a su vez las técnicas de

filtrado por distancia expuestas en el apartado anterior, de la misma manera que la siguiente sección añadirá

una nueva técnica a las presentadas hasta el momento.

Page 42: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

30

4.2.3 Filtrado por pendiente

La estrategia presentada en los apartados anteriores parece a priori adecuada para identificar las medidas

correspondientes al objeto a seguir, de forma que alineando esta nube de puntos filtrada con la correspondiente

al patrón de referencia mediante el algoritmo ICP se consiga alcanzar la situación deseada. Sin embargo,

aunque la estrategia presentada consiga identificar satisfactoriamente qué medidas pertenecen al objeto a

seguir y cuáles no, podría existir alguna situación en la que la nube de puntos filtrada fuera tal que, al alinear

ésta con la nube de puntos correspondiente al patrón mediante el algoritmo ICP, el resultado fuera erróneo y

no permitiera recuperar la situación de referencia.

La Figura 4.12 muestra una situación en la que podría aparecer el problema comentado. En ella, se muestra en

la parte izquierda la situación de referencia (que se corresponderá con el patrón a alinear con el resto de

medidas mediante el algoritmo ICP). En ella, el robot móvil se encuentra mirando frontalmente hacia el objeto

de referencia, en este caso un cubo. En la parte derecha de la imagen se muestra otra situación, en la que se ha

producido un desplazamiento relativo entre el robot y el cubo, de forma que el sensor láser detecta al cubo de

forma oblicua.

Figura 4.12: Representación de situación de referencia buscada (izquierda) y situación tras un movimiento

relativo en cierto instante (derecha).

En la situación de la figura anterior, el filtro diseñado tal y como se ha expuesto en los apartados anteriores

sería capaz de determinar cuáles de las medidas recogidas en ese instante pertenecen al cubo de referencia.

Así, la Figura 4.13 muestra la nube de puntos posterior al filtrado en esta situación. Como se observa, debido a

la perspectiva con la que el sensor está mirando hacia el cubo, se detectan un mayor número de puntos

pertenecientes a la cara lateral que a la frontal, siendo esta última la que se estaba afrontando en la situación

inicial. Esta situación puede provocar que, al intentar el algoritmo ICP alinear de la mejor manera la nube de

puntos representada con la correspondiente al patrón (que en este caso es simplemente una línea recta a

determinada distancia), el resultado de este algoritmo sea tal que alinee la cara lateral con el patrón, en lugar de

la cara frontal.

Esta situación se traduciría en que el robot móvil pasaría en ese momento a intentar seguir dicha cara lateral,

como se muestra en la Figura 4.13.

Page 43: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

31

Figura 4.13: Representación de la nube de puntos devuelta por el filtro, y posible movimiento erróneo del

robot móvil.

Así, queda de manifiesto que la perspectiva en la que se toman las medidas puede provocar un movimiento

erróneo del robot, especialmente en el caso en el que se estén siguiendo objetos cuya morfología frontal y

lateral son similares, como es el caso de un cubo. Esta problemática a priori no debería afectar a la distancia a

la que se situará el robot respecto al objeto, pero sí a la orientación, por lo que en definitiva puede imposibilitar

la búsqueda de una posición y orientación fijas respecto al mismo.

Para evitar el problema presentado, el filtro debería ser capaz de distinguir las distintas caras del cubo, en el

caso presentado. Para ello, el filtro debe añadir una condición más que deben cumplir los distintos puntos para

ser considerados como parte del objeto a seguir, y más concretamente de la parte del objeto que se pretende

seguir (la cara frontal en el caso del cubo). Esta condición es lo que llamamos filtrado por pendiente.

Al igual que en los apartados anteriores se partía de la base de que, entre un instante y el siguiente, el objeto a

seguir no puede realizar un movimiento tal que varíen de forma considerable su distancia media al sensor ni la

dirección principal del filtro, en este caso se considera que tampoco es posible un cambio brusco en la

orientación del objeto. Al disponerse únicametne de los datos proporcionados por un sensor de rango, la

magnitud que se ha escogido para asegurar esta condición es la pendiente.

El filtro considera como la pendiente de un punto, a la pendiente de la recta que une el punto anterior que

cumplió el resto de condiciones del filtro, con el punto que se está evaluando. Al igual que en el caso de la

distancia, en este punto se define un valor de pendiente media de los puntos que han sido seleccionados por el

filtro en el instante anterior. De esta forma, para que un punto sea seleccionado por el filtro, además de cumplir

el resto de condiciones expuestas en este capítulo, debe arrojar un valor de pendiente tal que la diferencia de

ésta con la pendiente media sea menor a un determinado valor de holgura. Al igual que en el caso de la

distancia, el valor de holgura definido dependerá de la brusquedad estimada en los movimientos entre dos

instantes, lo cual dependerá en gran medida de la frecuencia del sensor de profundidad. A mayor frecuencia

del sensor, menores valores de holgura serán necesarios y por lo tanto mejor actuará el filtro.

La Figura 4.14 muestra el cálculo de la pendiente para un punto 𝑃𝑖. Por no sobrecargar el esquema, no se han

representado las magnitudes 𝑙𝑖y 𝛼𝑖 que, como ya se expuso en este capítulo, se corresponden con la medida de

profundidad arrojada por el sensor y con el ángulo correspondiente con respecto a la dirección principal del

Page 44: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

32

filtro. La componente 𝑐𝑖que aparece en dicha figura se corresponde con la distancia medida.

De esta forma, teniendo en cuenta que la inclinación con respecto al punto anterior no puede ser distinta de la

inclinación media en una magnitud mayor que aquella definida por la holgura, el filtro es capaz de seleccionar

sólo una cara del cubo. Además, gracias precisamente a esta holgura, esta capacidad no imposibilita el

seguimiento de objetos que no sean lineales. Siguiendo esta estrategia, en la situación correspondiente a la

Figura 4.13 se detectaría un cambio brusco de aproximadamente 0 grados de pendiente, a unos 90 grados, por

lo que esta cara lateral no sería seleccionada por el filtro.

Figura 4.14: Ilustración del cálculo de la pendiente del punto i respecto al i-1.

4.2.4 Algoritmo completo

A lo largo de esta sección, los distintos apartados han presentado diversas técnicas que han sido tenidas en

cuenta para el diseño del filtro implementado. Aunque por simplicidad se han ido exponiendo de forma

individual, a la hora de decidir si un determinado punto debe ser considerado en la nube de puntos a alinear

con el algoritmo ICP o no el algoritmo de filtrado tiene en cuenta todas las consideraciones mostradas hasta

ahora.

Para aportar una visión general del funcionamiento global del filtro, este apartado presenta un esquema de los

pasos que se llevan a cabo desde que llega una medida, hasta que ésta es filtrada para que se toma como una de

las entradas del algoritmo ICP. El esquema simplificado del funcionamiento del filtro, que recibe las medidas

del sensor de rango en un determinado instante como entrada, es el siguiente:

- If es la primera ver que se llama al filtro.

o Se calculan la distancia media, la amplitud del filtro, la dirección principal del filtro y la

inclinación media iniciales.

- End

- Para cada dirección contenida entre dirección principal + amplitud del filtro/2 y dirección principal -

amplitud del filtro/2.

o Calcular distancia medida para esa dirección.

o If |distancia medida – distancia media| < holgura distancia

Calcular pendiente

Page 45: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

33

If |pendiente – pendiente media| < holgura pendiente

Añadir al mapa filtrado.

End

o End

- Recalcular distancia media, amplitud del filtro, dirección principal del filtro e inclinación media a

partir de los puntos que han sido filtrados.

- Devolver mapa filtrado.

4.3. Casuísticas menos habituales que pueden afectar a los resultados

El método presentado durante todo el capítulo tiene el objetivo de filtrar las medidas del sensor de profundidad

en los distintos instantes, de forma que se obtenga una nube de puntos que permita al robot móvil buscar la

situación correspondiente al patrón de referencia, comparando ambas mediante el algoritmo ICP. Así, el

objetivo en todo momento es que las medidas que entran al algoritmo ICP en cada instante contengan sólo la

información que necesita éste para conseguir tal objetivo.

Sin embargo, se presenta en este apartado una posible situación que no ha sido considerada hasta ahora, y que

podría darse en algún caso. La razón de que se considere en una sección aparte, exterior a la que explica el

diseño del filtro, es que se corresponde a una casuística poco habitual, aunque se propone igualmente una

solución al problema.

El problema aparece cuando es el patrón el que contiene más información de la necesaria, en lugar del resto de

medidas en instantes posteriores. La Figura 4.15 muestra una disposición en la que podría aparecer este

problema. En ella, se representan 3 objetos que en la situación inicial están situados de forma contigua. Al

estar situados de esta forma, el filtro es incapaz de reconocer que son 3 objetos distintos, partiendo únicamente

de la información proporcionada por el sensor de rango. Por ello, se consideraría como patrón las medidas

correspondientes a los 3 objetos, lo cual podría dar problemas cuando aparezca un movimiento relativo entre

ellos y el robot desee seguir sólo al central.

Figura 4.15: Situación en la que se observan frontalmente 3 objetos distintos, siendo el central el que se desea

seguir.

Page 46: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

34

La solución propuesta para las situaciones en las que aparezca este problema es, una vez detectado un cambio

de dimensiones abrupto las medidas (que ocurriría en el instante en el que el filtro pasara a detectar únicamente

el objeto central), se corrija el patrón almacenado para que el tamaño del mismo pase a ser similar al del recién

detectado objeto individual. La Figura 4.16 muestra un esquema de las variables que permitirían el cálculo de

la amplitud del objeto individual en el patrón corregido. Así, 𝛼0 sería el ángulo con respecto a la dirección

principal del filtro en el instante inicial que coincidiría con el límite del obstáculo central que ya ha sido

identificado. De esta forma, el patrón inicial debería corregirse para eliminar las medidas que se encuentren

por encima de este ángulo.

Figura 4.16: Representación del cálculo para la corrección de la amplitud del patrón, tras la detección del

objeto individual.

Como se ha comentado previamente, esta situación será poco habitual en la práctica, pero el objetivo de este

apartado es presentar una posible solución.

Page 47: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

35

5 EXPERIMENTOS

ste capítulo muestra los resultados obtenidos aplicando las técnicas presentadas en los capítulos 3 y 4 en

diferentes escenarios. En primer lugar se muestra la validez del algoritmo presentado en el capítulo 3

para el seguimiento de elementos móviles. Posteriormente se justifica la necesidad de utilizar el filtro

implementado para la identificación, y se muestran los resultados de aplicar dicha técnica en distintas

situaciones. Por último, se presentan algunos resultados de aplicar el algoritmo ICP a la estimación de la

posición de un robot móvil respecto a un sistema de referencia fijo, dejando así la puerta abierta a una posible

vía de investigación.

5.1. Introducción

Los experimentos mostrados en este capítulo muestran los resultados obtenidosutilizando las técnicas

presentadas. Como se ha mencionado en otros capítulos, los algoritmos se han integrado en ROS (Robotics

Operating System). ROS es una plataforma totalmente orientada a la robótica, con gran cantidad de funciones

y librerías que aportan utilidades tremendamente potentes para su aplicación en la robótica móvil, y para el

desarrollo de software en esta línea. Dentro de las utilidades que pone a disposición de los usuarios se

encuentran algunas herramientas gráficas de simulación, que permiten testear con gran fidelidad los algoritmos

desarrollados. Éste es el ejemplo de la herramienta Gazebo (ver Figura 5.1).

E

Page 48: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

36

Figura 5.1: Simulación de un robot Turtlebot en un entorno creado con Gazebo

Por otro lado, a lo largo de este capítulo se muestran numerosas imágenes que representan en RVIZ (3) los

datos reales recogidos por un sensor de profundidad, concretamente un sensor láser Hokuyo de 5 metros de

alcance. RVIZ es una potente herramienta de visualización de ROS, que en este caso nos permite observar y

reproducir las distintas medidas tomadas por el sensor de profundidad, o reproducir medidas reales

previamente almacenadas. En el caso de este trabajo, los datos representados en RVIZ corresponden a medidas

reales de un experimento real, por lo que es posible testear el comportamiento de los algoritmos con gran

fidelidad.

Además de todo lo anterior, este capítulo también recoge los resultados obtenidos directamente de aplicar los

algoritmos en distintos experimentos reales.

5.2. Seguimiento de elementos móviles

En este caso se presenta el resultado obtenido al utilizar el algoritmo presentado en el apartado 3.4.1 para

mantener una posición fija respecto a un elemento de referencia. Para ello, se muestra una simulación en la que

se pretende mantener una distancia fija respecto a una caja, utilizando para ello únicamente la información de

un barrido de lecturas 2D a partir de una cámara Kinect.

En la Figura 5.2, correspondiente a una simulación con Gazebo, se muestra, a la izquierda, la disposición

inicial del robot móvil con respecto a la caja, que se corresponde con el patrón que se desea seguir. En la parte

derecha se muestra la disposición final del mismo, después de haber movido la caja. Se observa perfectamente

que el robot ha sido capaz de recuperar la posición relativa a la misma.

Page 49: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

37

Figura 5.2: Posición inicial y final tras movimiento de la caja, simulado en Gazebo.

En cuanto a los tiempos de cálculo, se ha medido el tiempo consumido desde que llega la medida del barrido

láser, hasta que se dispone del resultado del algoritmo ICP del desplazamiento entre las dos medidas

correspondientes en cada caso. Aunque en este apartado sólo muestra algún ejemplo, en la práctica se han

realizado muchas más simulaciones, y el tiempo de cálculo en cada iteración siempre se encuentra por debajo

del milisegundo. Dado que la frecuencia con la que Gazebo proporciona un barrido es cada 10 Hz, se observa

que se dispone de un amplio margen en lo que al tiempo se refiere.

Por otro lado, las Figuras 5.3-5.5 muestran un robot real llamado Magabot (4), equipado con un sensor

Hokuyo, que está mirando hacia una caja. En la Figura 5.3 el Turtlebot está en la posición de referencia

deseada con respecto a la caja. En la Figura 5.4, aparece en una posición a la que se le ha desplazado

manualmente, y en la Figura 5.5 se observa cómo vuelve a la misma posición exactamente en la que se

encontraba inicialmente.

Figura 5.3: Posición inicial del Turtlebot, correspondiente con la referencia buscada.

Page 50: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

38

Figura 5.4: Posición a la que se lleva el robot de forma manual.

Figura 5.5: Situación final en la que el robot ha recuperado la posición relativa a la caja

La cruz azul en el suelo delata que efectivamente el robot recupera una posición prácticamente exacta a la

inicial. En los dos experimentos anteriores, no era necesario realizar un filtrado de los puntos medidos por en

sensor láser para que fueran exitosos, debido a que todas las medidas del entorno se movían de la misma

forma. En el experimento de la Figura 5.2 el entorno está vacío, a excepción únicamente del objeto que se

pretende seguir, por lo que todas las medidas se corresponden con éste. En cuanto al experimento de la Figura

5.3, se observa que ha sido el robot móvil el que se ha desplazado, con lo cual todas las medidas recogidas por

el sensor han sido desplazadas de la misma forma. Sin embargo, si en la misma situación se hubiera movido el

cajón en lugar del robot, éste último no habría realizado ningún movimiento. Esto se debe a que, al mantenerse

en la misma posición el resto de medidas del entorno (paredes, otros objetos, etc), las medidas

correspondientes a la situación de referencia y a la situación con el cajón desplazado coinciden en la gran

mayoría de puntos y por tanto se puede estimar con una buena precisión la posición y orientación del robot.

Page 51: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

39

5.2.1 Aplicación del filtrado

Con el ejemplo anterior ha quedado patente una vez más la necesidad de realizar un filtrado que seleccione

únicamente las medidas correspondientes al elemento de referencia para identificarlo y después realizar un

seguimiento exitoso. Esta sección muestra precisamente la aportación de este filtro, y la comparación entre las

medidas con y sin aplicarlo.

A continuación se muestran una serie de imágenes en las que se representan las distintas medidas recogidas

por un sensor láser durante un experimento real, mostradas con RVIZ. La Figura 5.6 muestra las medidas

recogidas por el sensor en la situación de referencia. En este experimento el robot móvil se mantiene estático,

mientras que las medidas que se encuentran justo delante del origen de coordenadas representado pertenecen a

un cajón, que se va moviendo a lo largo del experimento. El objetivo en este experimento es comprobar si el

filtro diseñado es capaz de identificar las medidas pertenecientes al cajón a pesar del movimiento del mismo.

Así, la Figura 5.7 muestra la nube de puntos resultantes del filtrado, es decir, las medidas que el filtro diseñado

en el capítulo 4 ha considerado que pertenecen al elemento de referencia en la situación buscada. Por lo tanto,

la nube de puntos representada en la Figura 5.7 es el patrón que se intentará buscar en las sucesivas lecturas

para después aplicar el algoritmo ICP.

Figura 5.6: Medidas del sensor láser sin filtrar, correspondientes con la situación de referencia.

Figura 5.7: Representación de las medidas resultantes del filtrado que identifican el objeto.

Page 52: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

40

La Figura 5.8 muestra las medidas del sensor en una situación en la que el cajón se ha desplazado hacia arriba

y hacia la derecha. Al verse en esta situación el cajón de forma oblicua al sensor de rango, se aprovecha este

ejemplo para ilustrar la necesidad del filtrado por pendiente, que se explica en el apartado 4.2.3 y que permite

discriminar las distintas caras del cajón.

Figura 5.8: Representación de las medidas del sensor láser tras un desplazamiento del cajón, resaltado por el

círculo rojo.

La Figura 5.9 muestra la identificación que haría el filtro si éste no incluyera el filtrado por pendiente. Los

resultados proporcionados por el algoritmo ICP, al comparar esta nube de puntos con la referencia (ver Figura

5.7), se muestran en la Tabla 5-1. Los resultados del algoritmo reflejan el conjunto de transformaciones que

habría que aplicar a la nube de puntos representada para obtener la correspondiente a la referencia, según los

ejes representados en la imagen. Se observa fácilmente que, a pesar de que la identificación del objeto de

referencia ha sido exitosa, la intención del algoritmo es alinear el patrón de la Figura 5.7 con la cara lateral del

cajón, que no es la cara que se pretendía seguir en un principio. Se observa así que en este caso no se

conseguiría realizar un seguimiento satisfactorio.

Figura 5.9: Representación de la nube de puntos posterior al filtrado de las medidas de la Figura 5.8.

Page 53: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

41

X(m) Y(m) Yaw(deg) tiempo(s)

-1.1012 0.5191 -49.54 0.000078

Tabla 5-1: Resultado del ICP y tiempo.

En cambio, la Figura 5.10 muestra la nube de puntos resultante cuando se incluye el filtrado por pendiente. Se

observa en dicha figura que se ha conseguido filtrar únicamente la parte del objeto que se pretendía seguir, es

decir, la cara frontal. En este caso, además, el resultado arrojado por el algoritmo ICP sí se corresponde con el

desplazamiento del cajón.

Figura 5.10: Nube de puntos posterior al filtrado de las medidas de la Figura 5.8, incluyendo el filtrado por

distancia.

X(m) Y(m) Yaw(deg) tiempo(s)

-0.6075 -0.8471 -0.43 0.000054

Tabla 5-2: Resultado del algoritmo ICP y tiempo empleado.

La Figura 5.11 muestra las medidas del sensor en otra situación distinta, en la que se ha producido un

desplazamiento del cajón, esta vez en la dirección contraria. La Figura 5.12 representa las medidas posteriores

al filtrado, y se observa que se corresponden perfectamente con la cara a seguir.

Figura 5.11: Medidas recogidas por el sensor, tras un desplazamiento del cajón de referencia.

Page 54: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

42

Figura 5.12: Nube de puntos resultante de filtrar la Figura 5.11, para el seguimiento de la cara frontal.

X(m) Y(m) Yaw(deg) tiempo(s)

-0.3873 0.6411 -3.004 0.000046

Tabla 5-3: Resultado del ICP y tiempo empleado.

Queda de esta forma demostrada la validez del método de filtrado propuesto para la identificación y

seguimiento de elementos móviles, discriminando el resto del entorno e incluso el resto de partes del elemento

que introducirían ruido en la solución del algoritmo ICP.

A continuación se muestra un experimento similar al de las Figuras 5.3-5.5, aunque en este caso es el cajón de

referencia el que realiza el movimiento, y el robot móvil debe intentar mantener una posición y orientación

constantes respecto a éste. La Figura 5.13 muestra la disposición inicial, que también se corresponde con la

situación de referencia que se pretende mantener respecto al cajón.

Figura 5.13: Posición inicial, en la que le robot móvil se encuentra frente al cajón que se pretende seguir, a una

distancia de referencia determinada.

Page 55: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

43

Así, la Figura 5.14 muestra una situación posterior, en la que el cajón se ha desplazado a lo largo de la

estancia. Se observa que el robot móvil se ha encargado de mantener una posición y orientación relativas al

cajón iguales a las correspondientes a la situación de referencia, utilizando únicamente los datos

proporcionados por el sensor láser Hokuyo. Con este experimento queda demostrado el éxito del método

presentado para el seguimiento de referencias, ya que para que el experimento haya tenido éxito el filtro

implementado ha debido seleccionar únicamente los puntos correspondientes a la cara a seguir, de forma que

el resto de medidas correspondientes al entorno no fueran tenidas en cuenta. Una muestra del éxito del filtro

diseñado es que este mismo experimento no provoca ningún movimiento del robot si se ignora el filtro

propuesto en este trabajo.

Figura 5.14: Situación resultante del movimiento del cajón por la habitación.

Es importante destacar que, al contrario de lo que pudiera parecer, el método presentado no sólo es válido para

el seguimiento de superficies planas, gracias a las holguras introducidas en el diseño del filtro (ver capítulo 4).

5.3. Aplicación al posicionamiento respecto a un sistema de referencia fijo

Como complemento a los algoritmos de seguimiento de elementos móviles, en este apartado se muestran los

resultados de aplicar los algoritmos presentados en el apartado 3.5 en algunas situaciones de ejemplo. Aunque

no es el objetivo principal del trabajo, se pretende así mostrar el posible campo de aplicación del algoritmo

ICP para el posicionamiento respecto a un sistema de referencia fijo, mediante la técnica de integración de los

desplazamientos calculados por dicho algoritmo.

La Figura 5.15 muestra el comportamiento del robot móvil al utilizar el algoritmo de posicionamiento

presentado, comandándole una posición objetivo de 2,5 metros hacia adelante. Cada baldosa del entorno tiene

un metro de lado, por lo que se observa una medida orientativa de la bondad del resultado.

Page 56: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

44

Figura 5.15: Posición inicial y final del robot al comandarle un avance de 2.5 metros.

La Figura 5.16 muestra el mismo ejemplo, pero realizando en este caso una acción de control menos agresiva,

es decir, a una velocidad inferior. Aunque de las imágenes se desprende que el comportamiento es bueno en

ambos casos y la diferencia es mínima, en la Tabla 5-4 se muestra una comparativa entre la posición calculada

en el punto final por el método basado en el algoritmo ICP, y la posición calculada a partir de los datos de la

odometría, proporcionados por el topic /odom. Es importante destacar que los valores de la odometría son

orientativos, ya que también presentan un error

Figura 5.16: Posición inicial y final del robot al comandarle un avance de 2.5 metros, a velocidad muy baja.

Método X ICP(m) Y ICP(m) Yaw

ICP(rad)

X IMU(m) Y IMU(m) Yaw

IMU(rad)

Caso 1 2.4052 -0.0238 0.2545 2.4982 0.0094 0.2358

Caso 2 2.4021 -0.0075 0.1576 2.4054 -0.0199 0.1434

Tabla 5-4: Comparación entre los resultados con el algoritmo ICP y la posición estimada a partir del nodo

/odom.

Page 57: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

45

Aunque el posicionamiento basado en la IMU incluye un error creciente a medida que avanza el tiempo, en

este caso se utiliza como una estimación con la que comparar los resultados obtenidos con el algoritmo

presentado. Se observa en la Tabla 4.1 que el caso 1, en el que la velocidad era mayor, arroja un resultado más

impreciso que en el caso 2, cuyo valor prácticamente coincide con el de /odom. Se desprende por tanto que el

método mejora a velocidades bajas.

Además de la simulación anterior,se realizó un experimento en el que un sensor láser Hokuyo se movía en

línea recta durante 69 centímetros, con el fin de comprobar la distancia recorrida estimada por el algoritmo de

integración de medidas a partir de barridos en dos dimensiones. Los resultados de este experimento se

muestran en la Figura 5.17. En ella se observa que el error en la estimación de la posición final en la X dista de

la real en poco más de un centímetro, y en la Y el error máximo es de unos 5 centímetros, aunque se observa

que se va compensando.

Figura 5.17: Representación del movimiento del Hokuyo según el algoritmo ICP

En los dos ejemplos mostrados queda de manifiesto que el algoritmo ICP permite realizar una estimación del

desplazamiento de forma satisfactoria, especialmente a velocidades bajas. Sin embargo, se ha comprobado que

el algoritmo pierde mucha precisión cuando aparecen giros en la trayectoria. Esto ocurre especialmente cuando

se parte de barridos en el plano, mientras que el comportamiento ante giros cuando se utilizan nubes de puntos

en el espacio es menos impreciso.

Como se ha comentado en otros apartados, estos resultados podrían mejorarse progresivamente conforme se le

fueran añadiendo los datos de otros sensores, ya que los métodos presentados utilizan información procedente

de un único sensor, razón por la cual existen situaciones en las que arrojan resultados que no son satisfactorios.

Otro aspecto muy a tener en cuenta es el bajo tiempo de cálculo del método, el cual también deja un margen

para la integración de más información procedente de otros sensores.

Page 58: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

46

6 CONCLUSIONES Y TRABAJO FUTURO

legados a este punto, ya se han expuesto por completo los métodos propuestos en este trabajo, y se

han presentado los resultados obtenidos en los distintos experimentos realizados, mostrados en el

capítulo 5. Por tanto, sólo queda comentar las conclusiones que se pueden extraer de los resultados

mostrados en el capítulo anterior, y proponer posibles ampliaciones que se puedan realizar siguiendo la línea

propuesta en este trabajo.

6.1. Conclusiones

A lo largo de este trabajo se ha afrontado un problema principalmente, que es el seguimiento de elementos

móviles, intentando mantener en todo momento una posición y orientación fijas respecto a éstos, y utilizando

únicamente los datos proporcionados por un sensor de profundidad. Para ello se han mostrados algoritmos,

tanto en 2 como en 3 dimensiones, basados en el algoritmo ICP, que es la técnica central utilizada en este

trabajo. Sin embargo, el principal avance que pretendía aportar este trabajo es el diseño de un filtro que

permitiera identificar únicamente los puntos correspondientes al elemento a seguir, de forma que se pudiera

realizar dicho seguimiento mediante el algoritmo ICP en situaciones realistas, en las que se toman más

medidas de otros elementos distintos al de referencia, ya sean móviles o no.

Los experimentos mostrados en el apartado 5.2 muestran que se ha conseguido el objetivo propuesto,

especialmente en lo que al filtro se refiere. Como se observa, el algoritmo es perfectamente capaz de

identificar no sólo al elemento que se desa tomar como referencia, sino también a la cara concreta que se

pretende seguir. Además, a pesar de que en los experimentos sólo se muestra el seguimiento de un cubo, el

filtro no está sujeto al seguimiento únicamente de superficies planas. De la misma forma, aunque el diseño del

filtro se ha particularizado para el uso de un sensor de profundidad que realiza barridos en el plano, la

extensión del mismo a un sensor que trabaja en 3 dimensiones es casi inmediata.

El objetivo principal de los experimentos era mostrar el éxito de los algoritmos diseñados, especialmente del

filtro. Sin embargo, para mostrar la aplicación a la práctica, se ha controlado un robot móvil en distintos

experimentos con los datos de salida del algoritmo ICP. En estos experimentos se ha implementado un simple

controlador proporcional, ya que el control no era el objetivo de este trabajo. Sin embargo, cabe mencionar que

con técnicas más sofisticadas se pueden obtener resultados muy satisfactorios de seguimiento de elementos

móviles, ya que ha quedado patente el satisfactorio resultado del algoritmo presentado.

L

Page 59: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

47

Otro aspecto a destacar es que sólo se ha utilizado la información de un sensor de profundidad. Esto deja la

puerta abierta a mejorar los algoritmos de identificación y seguimiento, combinando el método presentado con

la información que puedan proporcionar otros sensores, como por ejemplo una cámara que permita realizar

algoritmos con tratamiento de imágenes. Además, se ha mencionado que el algoritmo presentado consume un

tiempo de cálculo muy bajo, lo que deja un amplio magen de tiempo para diseñar algoritmos más complejos

computacionalmente, y que integren información de distintos sensores.

Por otro lado, se ha mostrado la aplicación del algoritmo ICP a la estimación de la posición de un robot móvil

respecto a un origen de coordenadas fijo. A pesar de que este no era el objetivo del trabajo, se ha considerado

conveniente mostrar la posible aplicación del algoritmo ICP a un problema más general de la navegación en

interiores. Así, se observa que el algoritmo de posicionamiento en un sistema fijo de coordenadas a través de la

integración de las distintas soluciones del ICP en dos dimensiones entre medidas consecutivas consigue

generalmente unos resultados aceptables para trayectorias simples, perdiendo precisión conforme más altas

son las velocidades y más bruscos los movimientos. Se ha comprobado que este método en dos dimensiones

habitualmente no responde bien ante giros bruscos, mejorando el comportamiento en este caso cuando se

dispone de una nube de puntos en el espacio. Esto es algo totalmente intuitivo, ya que, al disponer de una nube

de puntos en tres dimensiones, se dispone de más información para estimar la variación de posición del robot

tras cada movimiento que en el caso en el que sólo se disponga de la información de un barrido en un plano.

De la misma forma que el comportamiento mejora en el caso en tres dimensiones, por disponerse de más

información, también podría mejorarse el comportamiento mediante la integración de la información de más

sensores, como la IMU, cámaras, etc.

Por todo lo comentado anteriormente, se considera en este trabajo que se ha conseguido el objetivo de

proponer una técnica más o menos precisa para afrontar problemas de posicionamiento en interiores, siendo

ésta simple, rápida, y con posibilidad de ser mejorada mediante la integración con otras técnicas. Es decir, se

consigue el objetivo de presentar unas técnicas que puedan ser la base de otras técnicas más complejas, en

función de las necesidades de cada proyecto y del poder de adquisición de hardware en cada caso.

6.2. Trabajo futuro

Los métodos presentados dejan la puerta abierta a distintas ampliaciones y mejoras, algunas de las cuales ya

mencionadas previamente en esta memoria.

- La principal línea de ampliación de este trabajo se apoya en la fusión sensorial, para poder mejorar los

algoritmos de identificación y seguimiento en entornos cada vez más complejos, utilizando para ello

disitntas técnicas que se combinen con la presentada.

- Otra línea interesante sería un estudio más profundo del posicionamiento respecto a un sistema de

posicionamiento fijo basándose en el algoritmo ICP, continuando así la línea complementaria

mencionada en este trabajo. De igual forma, la fusión sensorial sería un reto importante en esta línea.

Page 60: Trabajo de Fin de Máster Máster en Ingeniería Industrialbibing.us.es/proyectos/abreproy/70790/fichero/TFM_AMC.pdf · Trabajo Fin de Máster Máster en Ingeniería Industrial Método

48

7 REFERENCIAS

1. www.ros.org. [En línea]

2. Jose-Luis Blanco-Claraco, Jesus Briales. http://www.mrpt.org/. [En línea] 2016.

3. foundation, Open source robotics. http://wiki.ros.org/rviz. Ros.org. [En línea]

4. http://artica.cc/projects/hardware/2013/12/02/magabot.html. [En línea]

6. Wilm, Hans Martin Kjer and Jakob. Evaluation of surface registration. [aut. libro] Technical

University of Denmark. 2010.

7. CCIA, Departamento. Alineamiento 3D a partir de nubes de puntos. 2012.

8. Ros.org. http://wiki.ros.org/mrpt_bridge. [En línea] 2014.

9. —. http://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html. [En línea]

10. —. http://docs.ros.org/kinetic/api/sensor_msgs/html/msg/PointCloud.html. [En línea]

11. http://reference.mrpt.org/devel/namespacemrpt_1_1slam.html. [En línea]

12. Ros.org. http://gazebosim.org/tutorials?tut=ros_overview. [En línea]