Robótica Direccionada PDF

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
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
ii
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
Dep. de Ingeniería de Sistemas y Automática

Universidad Internacional de Andalucía
Sevilla, 2016
iii
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

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

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.
i
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.
ii
Í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.5. Aplicación del algoritmo ICP para la estimación de la posición respecto a un sistema de referencia fijo
18
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
iii
6.1. Conclusiones 46
6.2. Trabajo futuro 47
7 Referencias 48
iv
Í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
v
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
vi
1 INTRODUCCIÓN
l uso de robots móviles para diversas aplicaciones es cada vez más común a medida que avanza la
E 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,
1
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
2
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.
3
2 FORMULACIÓN DEL PROBLEMA
ste capítulo presenta la problemática que se pretende afrontar en este trabajo. Ante el espectacular
E 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.
4
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.
5
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.
6
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.
7
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.
8
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
E 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.
9
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.
10
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.
11
- 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.
12
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
13
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.
14
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.
15
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.
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.
16
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.
17
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.
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 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.
18
Figura 3.8: Cambio de orientación del robot
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 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.
19
4 IDENTIFICACIÓN Y SEGUIMIENTO DE
REFERENCIAS
Este capítulo explica el algoritmo implementado para la identificación y seguimiento de un objeto de
E 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.
20
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.
21
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.
22
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.
23
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.
24
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 𝑡𝑘+1 dicho 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.
25
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 ).
26
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 𝑁1 y 𝑁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 𝑁1 y 𝑁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.
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.
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 𝑁1 y 𝑁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.
28
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 𝑁1 y 𝑁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 𝑁1 y 𝑁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.
29
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.
30
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
31
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
32
 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.
33
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.
34
5 EXPERIMENTOS
ste capítulo muestra los resultados obtenidos aplicando las técnicas presentadas en los capítulos 3 y 4 en
E 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).
35
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.
36
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.
37
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.
38
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.
39
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.
40
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.
-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.
41
Figura 5.12: Nube de puntos resultante de filtrar la Figura 5.11, para el seguimiento de la cara frontal.
-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.
42
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.
43
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 X IMU(m) Y IMU(m) Yaw

ICP(rad) 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.
44
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.
45
6 CONCLUSIONES Y TRABAJO FUTURO
legados a este punto, ya se han expuesto por completo los métodos propuestos en este trabajo, y se
L 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.
46
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.
47
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]
48

Robótica Direccionada PDF

Caricato da

Informazioni sul documento

Titolo originale

Copyright

Formati disponibili

Condividi questo documento

Condividi o incorpora il documento

Opzioni di condivisione

Hai trovato utile questo documento?

Questo contenuto è inappropriato?

Copyright:

Formati disponibili

Robótica Direccionada PDF

Caricato da

Copyright:

Formati disponibili

Trabajo de Fin de Máster

Máster en Ingeniería Industrial

Método para identificación y seguimiento de

Autor: Alejandro Muñoz Cueva

Dep. Ingeniería de Sistemas y Automática

Método para identificación y seguimiento de objetos

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

Autor: Alejandro Muñoz Cueva

Tutores: José Antonio Cobano Suárez y

Acuerdan otorgarle la calificación de:

El Secretario del Tribunal

1.1. Objetivo del trabajo

1.2. Estructura del trabajo

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

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

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

2.1. Situaciones adversas para los métodos tradicionales

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

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

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

3.2. El algoritmo ICP

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

3.2.2. Desglose del algoritmo ICP

Figura 3.2. Matching entre dos figuras de 8 puntos

Figura 3.3: Ejemplo con 3 iteraciones del algoritmo ICP

3.3. Principales librerías utilizadas

Figura 3.4: Ejemplo de dos nubes de puntos a alinear

Figura 3.5: Papel del paquete mrpt_bridge.

- Transformación entre LaserScan y CObservation2DRangeScan.

3.4.1. Algoritmo en 2 dimensiones

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

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

3.4.2. Algoritmo en 3 dimensiones

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

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

3.5.1. Algoritmo en 2 dimensiones

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

3.5.2. Algoritmo en 3 dimensiones

El proceso a seguir en este caso es el siguiente:

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

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

4.1. Problemática y necesidad de un filtrado

4.2. Implementación del filtro

4.2.1 Filtrado por distancia

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

4.2.2 Filtrado por dirección

4.2.4 Algoritmo completo

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

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

5.2. Seguimiento de elementos móviles

-1.1012 0.5191 -49.54 0.000078

Tabla 5-1: Resultado del ICP y tiempo.

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.

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.

5.3. Aplicación al posicionamiento respecto a un sistema de referencia fijo

Método X ICP(m) Y ICP(m) Yaw X IMU(m) Y IMU(m) Yaw

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

A lo largo de este trabajo se ha afrontado un problema principalmente, que es el seguimiento de elementos